A trajectory planning method for reconstructing a double-balanced car into a four-wheel car

Abstract

This invention discloses a trajectory planning method for reconstructing a four-wheeled vehicle from two self-balancing scooters. The method includes: establishing wireless communication between the two scooters; calculating the optimal docking preparation position based on the state information of the leading scooter; generating a path for the trailing scooter from its current position to the optimal docking preparation position using a quadratic programming method; and planning the trailing scooter's speed based on dynamic programming and quadratic optimization. The method also involves determining that the longitudinal axes of the leading and trailing scooters are parallel; calculating a predetermined docking position based on the state information of the leading scooter and the structural parameters of the mechanical reconstruction mechanism; and generating a trajectory from the optimal docking preparation position to the predetermined docking position using a trajectory planning method. Finally, the method uses sensors to monitor the position and speed information of both scooters in real time; employs a dual PID controller combined with a speed-based control method; and drives the trailing scooter's docking connector along a straight connection line to move towards the leading scooter's docking interface until the docking connector contacts the docking interface to complete the docking. This invention improves the efficiency and accuracy of path planning.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a trajectory planning method for reconstructing a dual-balance vehicle into a four-wheeled vehicle. Background Technology

[0002] A two-wheeled self-balancing scooter, also known as a balance scooter or electric scooter, is an electric mode of transportation. It utilizes advanced gyroscopes and accelerometers to monitor the vehicle's tilt angle and motion in real time, maintaining balance by rapidly adjusting wheel speed and direction. Users simply control speed by leaning forward or backward, and direction by tilting left or right; the operation is simple and intuitive. Two-wheeled self-balancing scooters are typically small in size, allowing for agile maneuvering and movement in confined spaces.

[0003] While self-balancing scooters offer many advantages, they also have some significant drawbacks and limitations. First-time users may need time to adapt to their operation, especially on uneven surfaces, where they are prone to losing balance. Improper operation during turns, acceleration, or deceleration can cause the scooter to tip over, increasing the risk of tipping. Self-balancing scooters require relatively smooth surfaces and are difficult to navigate on potholes, gravel, wet, or muddy terrain. On steep inclines, they may not provide sufficient power or stability, making them prone to slipping or tipping over. Due to structural design limitations, self-balancing scooters have a limited load capacity, typically suitable for single-person riding, and cannot carry much luggage. Furthermore, their limited footboard and interior space can lead to fatigue from prolonged standing.

[0004] Traditional trajectory planning methods typically require significant computational resources and time, especially in complex environments. This leads to inefficient path planning, failing to meet real-time requirements. Furthermore, the generated paths may not be optimal, exhibiting path deviations. These deviations can prevent accurate docking of the two self-balancing vehicles, increasing control complexity and the risk of docking failure. Existing methods lack precise control over vehicle speed and acceleration, easily causing swaying or instability during docking. This not only affects docking accuracy but may also damage the mechanical structure. Summary of the Invention

[0005] This invention aims to overcome the shortcomings of existing two-wheeled self-balancing vehicles in terms of stability, road condition adaptability, and safety, and provides a trajectory planning method for reconstructing a two-wheeled self-balancing vehicle into a four-wheeled vehicle.

[0006] To achieve the above objectives, the present invention provides a trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter, comprising:

[0007] Establish wireless communication between two self-balancing vehicles, calculate the optimal docking preparation position based on the status information of the leading vehicle, generate the path of the trailing vehicle from its current position to the optimal docking preparation position using a quadratic programming method, and plan the speed of the trailing vehicle based on dynamic programming and quadratic optimization.

[0008] The longitudinal axes of the preceding and following vehicles are determined to be parallel. Based on the status information of the preceding vehicle and the structural parameters of the mechanical reconfiguration mechanism, the predetermined docking position is calculated, and a trajectory planning method is used to generate a trajectory from the optimal docking preparation position to the predetermined docking position.

[0009] Using sensors to monitor the position and speed information of the two vehicles in real time, a dual PID controller combined with speed control method is adopted to drive the rear vehicle docking connector to move along the straight connection line to the front vehicle docking interface along the generated trajectory until the docking connector contacts the docking interface to complete the docking.

[0010] Preferably, the optimal docking preparation position is calculated based on the status information of the preceding vehicle, including:

[0011] Determine a safe distance based on the speed of the vehicle in front;

[0012] Based on the safety distance, several candidate positions are generated by lateral and longitudinal offsets;

[0013] Calculate the comprehensive cost function for each candidate position, and select the candidate position with the minimum comprehensive cost as the optimal docking preparation position.

[0014] Preferably, the method for determining the safe distance is as follows:

[0015] D safe =max(k·v1,D min )

[0016] In the formula, D safe To determine the safe distance after comprehensively considering speed and minimum safe distance, k is the safety factor, v1 is the speed of the first vehicle, and D... min Minimum safe distance;

[0017] The method for generating several candidate positions is as follows:

[0018]

[0019] In the formula, (x1, y1, θ1) represents the current position and orientation angle of the first vehicle, and i and j represent the indices of the longitudinal and lateral offsets, respectively, with a range of... arrive arrive n represents the number of candidate positions in the vertical direction, and m represents the number of candidate positions in the horizontal direction; x center Let y be the x-coordinate of the center position. centerP is the ordinate of the center position. center p is the central position ij Δx represents the candidate position, Δy represents the vertical offset, and Δy represents the horizontal offset.

[0020] Preferably, the method for calculating the optimal docking preparation position is as follows:

[0021] P prep =P best =argmin i,j C total (P i,j )

[0022] In the formula, P prep P prepares the position for optimal docking. best For the optimal position, C total () is the total cost function, P i,j These are candidate positions.

[0023] Preferably, after the speed of the following vehicle is planned, the heading angle and speed of the preceding and following vehicles should respectively satisfy:

[0024] |θ1-θ2|≤∈ θ

[0025] |v1-v2|≤∈ v

[0026] Where θ1 is the heading angle of the leading vehicle, θ2 is the heading angle of the trailing vehicle, ∈ θ v1 is the maximum permissible deviation between the heading angles of the two vehicles; v2 is the speed of the preceding vehicle, v1 is the speed of the following vehicle, ∈ v This represents the maximum permissible deviation between the speeds of the two vehicles.

[0027] Preferably, the constraint conditions defined for the predetermined docking position are as follows:

[0028] Adaptive depth matching constraints for mechanical reconfiguration structures:

[0029] |hH|≤ε h

[0030] Automatic centering constraints for mechanically reconfigurable structures:

[0031] |dD|≤ε d

[0032] Position constraint: x(t0) = x prep y(t0)=y prep , x(t1)=x dock =x f +d, y(t1)=y dock =y f -h;

[0033] Speed ​​constraints:

[0034] Acceleration constraints:

[0035] Where t0 is the algorithm start time, the predetermined docking trajectory planning step ends (i.e., the start time), and t1 is the predetermined docking trajectory planning step end time. (x prep y prep (x) represents the initial state coordinates, i.e., the docking preparation position. dock y dock (x) represents the final state coordinates, i.e., the predetermined docking position. f y f ) represents the tip position of the V-shaped connector in the mechanical reconfiguration structure of the front vehicle, H is the groove depth, D is the radius distance from the bottom of the interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance between the two longitudinal axes, and ε h ε is the maximum allowable depth error. d For the maximum permissible lateral offset error, v prep and a prep These represent the velocity and acceleration of the following vehicle at time t0, i.e., the start of the planning process, respectively. dock and a dock These are the expected velocity and expected acceleration at the end of the planning process, respectively.

[0036] Preferably, generating a trajectory from the optimal docking preparation position to the predetermined docking position based on a trajectory planning method includes:

[0037] Construct a fifth-degree polynomial equation and solve it to obtain the coefficients of the fifth-degree polynomial;

[0038] Based on the coefficients of the fifth-degree polynomial, the trajectory from the optimal docking preparation position to the predetermined docking position is generated;

[0039] The fifth-degree polynomial equation is as follows:

[0040] x(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 +a5t 5

[0041] y(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0042] In the formula, x(t) is the abscissa of the trajectory, y(t) is the ordinate of the trajectory, a0, a1, a2, a3, a4, and a5 are the coefficients of the fifth-degree polynomial in the horizontal direction, and t, t 2 t 3 t 4 t 5 These are different powers of time t, and b0, b1, b2, b3, b4, and b5 are the coefficients of the vertical fifth-degree polynomial.

[0043] Preferably, driving the rear vehicle docking connector to move along a straight connection line towards the front vehicle docking interface includes:

[0044] Design a position error PID controller, calculate the target velocity, and transform the position error PID controller into a first discrete model;

[0045] Design a speed error PID controller to control the actual acceleration of the following vehicle, and convert the speed error PID controller into a second discrete model;

[0046] Based on the acceleration curve generated by the speed error PID controller, the rear vehicle docking connector is driven to move instantaneously along the acceleration curve to the front vehicle docking interface.

[0047] Preferably, the position error PID controller is:

[0048] err pos (t)=h(t)-h target (t)

[0049]

[0050] In the formula, err pos (t) represents the longitudinal offset error of the tip, v1(t) is the actual speed of the preceding vehicle at time t, and h(t) is the longitudinal offset distance of the tip at time t. target (t) represents the longitudinal offset distance of the target, v target (t) represents the target velocity, K p1 K is the proportional coefficient of the position error PID controller. i1 K represents the integral coefficient of the position error PID controller. d1 The derivative coefficients of the position error PID controller are derr pos (t) represents the rate of change of the position error;

[0051] The first discrete model is:

[0052]

[0053] Among them, v target(k) represents the target velocity at discrete time k, err pos (k) represents the position error at discrete time k, err pos (i) represents the position error at discrete time i, Δt represents the discrete time step, and err pos (k-1) represents the position error at discrete time k-1, and v1(k) represents the actual speed of the vehicle ahead at discrete time k.

[0054] The speed error PID controller is:

[0055] err vel (t)=v tar get (t)-v2(t)

[0056]

[0057] In the formula, err vel (t) represents the error between the target vehicle speed and the actual speed of the following vehicle, v2(t) represents the actual speed of the following vehicle at time t, and a vel (t) represents the target acceleration, K p2 K is the proportional coefficient of the speed error PID controller. i2 K represents the integral coefficient of the speed error PID controller. d2 The derivative coefficients of the speed error PID controller are derr vel (t) represents the rate of change of the velocity error;

[0058] The second discrete model is:

[0059]

[0060] Among them, a vel (k) represents the acceleration at discrete time k, err vel (k) represents the velocity error at discrete time k, err vel (i) represents the velocity error at discrete time i, err vel (k-1) represents the velocity error at discrete time k-1.

[0061] Preferably, sensors are used to monitor the position and speed information of the two vehicles in real time and make dynamic adjustments; when the position error |h(t)-h target When the distance is less than a preset threshold, the mechanical reconstruction structure is initiated to complete the docking, where h(t) is the longitudinal offset distance of the tip at time t. target (t) represents the longitudinal offset distance of the target.

[0062] Compared with the prior art, the present invention has the following advantages and technical effects:

[0063] (1) This invention adopts a method of independent planning of path and speed, and independently optimizes the path shape and speed curve, thereby ensuring that the following vehicle can adjust in real time and adapt to complex environments. By adjusting the speed and direction in advance, it ensures that the heading angle and speed of the two vehicles are basically consistent during the subsequent splicing process, avoiding large speed or direction changes during the subsequent splicing process, reducing the risk of collision or loss of control during the splicing process, and significantly improving the overall operational safety.

[0064] (2) To meet the adaptive depth matching constraints and automatic centering constraints of the mechanical reconfiguration structure, this invention employs a fifth-order polynomial trajectory planning method to precisely control the state (including position, velocity, and acceleration) of the starting and ending points of the trajectory. This ensures that the longitudinal axes of the two vehicles are parallel during subsequent splicing and satisfies the adaptive splicing constraints of the mechanical reconfiguration structure, achieving high-precision docking control, avoiding positional and attitude deviations during subsequent docking, and reducing the risk of mechanical errors and docking failures that may occur during docking.

[0065] (3) To ensure the smoothness of the docking connector of the rear vehicle when docking with the interface of the front vehicle and to reduce impact, this invention employs a dual PID algorithm for control, while meeting the automatic alignment constraints of the mechanical reconfiguration structure. The dual PID algorithm precisely adjusts speed and acceleration to ensure the smoothness of the acceleration process, avoiding impacts or vibrations caused by sudden changes, and finally completes the splicing through mechanical locking. This ensures the smoothness and safety of the docking process and reduces the risk of docking failure. Attached Figure Description

[0066] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0067] Figure 1 This is a flowchart of a trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to an embodiment of the present invention;

[0068] Figure 2 This is a flowchart of the docking preparation stage according to an embodiment of the present invention;

[0069] Figure 3 This is a flowchart of the predetermined docking stage in an embodiment of the present invention;

[0070] Figure 4 This is a flowchart of the final docking stage in an embodiment of the present invention;

[0071] Figure 5 This refers to the V-shaped mating connector in the mechanical reconfiguration structure of this invention. Detailed Implementation

[0072] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0073] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0074] This invention proposes a trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter, such as... Figure 1 ,include:

[0075] Establish wireless communication between two self-balancing vehicles, calculate the optimal docking preparation position based on the status information of the leading vehicle, generate the path of the trailing vehicle from its current position to the optimal docking preparation position using a quadratic programming method, and plan the speed of the trailing vehicle based on dynamic programming and quadratic optimization.

[0076] Specifically, the optimal docking preparation position is calculated based on the status information of the preceding vehicle, including:

[0077] Determine a safe distance based on the speed of the vehicle in front;

[0078] Based on the safe distance, several candidate locations are generated by horizontal and vertical offsets;

[0079] Calculate the comprehensive cost function for each candidate position, and select the candidate position with the minimum comprehensive cost as the optimal docking preparation position.

[0080] The method for determining the safe distance is as follows:

[0081] D safe =max(k·v1,D min )

[0082] In the formula, D safe To determine the safe distance after comprehensively considering speed and minimum safe distance, k is the safety factor, v1 is the speed of the first vehicle, and D... min Minimum safe distance;

[0083] The method for generating several candidate positions is as follows:

[0084]

[0085] In the formula, (x1, y1, θ1) represents the current position and orientation angle of the first vehicle, and i and j represent the indices of the longitudinal and lateral offsets, respectively, with a range of... arrive arrive n represents the number of candidate positions in the vertical direction, and m represents the number of candidate positions in the horizontal direction; x center Let y be the x-coordinate of the center position. center P is the ordinate of the center position. center p is the central position ij Δx represents the candidate position, Δy represents the vertical offset, and Δy represents the horizontal offset.

[0086] The method for calculating the optimal docking preparation position is as follows:

[0087] P prep =P best =argmin i,j C total (P i,j )

[0088] In the formula, P prep P prepares the position for optimal docking. best For the optimal position, C total () is the total cost function, P i,j These are candidate positions.

[0089] After the speed of the following vehicle is planned, the heading angles and speeds of the preceding and following vehicles should respectively satisfy:

[0090] |θ1-θ2|≤∈ θ

[0091] |v1-v2|≤∈ v

[0092] Where θ1 is the heading angle of the leading vehicle, θ2 is the heading angle of the trailing vehicle, ∈ θ v1 is the maximum permissible deviation between the heading angles of the two vehicles; v2 is the speed of the preceding vehicle, v1 is the speed of the following vehicle, ∈ v This represents the maximum permissible deviation between the speeds of the two vehicles.

[0093] The core task of this phase (docking preparation phase) is to evaluate and select the best docking preparation position, generate a smooth trajectory from the current position to the docking preparation position, and ensure that the heading angle and speed of the two vehicles are consistent.

[0094] During the docking preparation phase, to cope with the dynamic environment and diverse constraints, a method of independently planning the path and speed was adopted, and the path shape and speed curve were independently optimized to ensure that the following vehicle could adjust in real time and adapt to the complex environment. By adjusting the speed and direction in advance, the heading angle and speed of the two vehicles were ensured to be basically consistent during the subsequent assembly process, avoiding large-scale speed or direction changes during the assembly process, reducing the risk of collision or loss of control during the assembly process, and significantly improving the overall operational safety.

[0095] The longitudinal axes of the preceding and following vehicles are determined to be parallel. Based on the status information of the preceding vehicle and the structural parameters of the mechanical reconfiguration mechanism, the predetermined docking position is calculated, and a trajectory planning method is used to generate a trajectory from the optimal docking preparation position to the predetermined docking position.

[0096] Specifically, the constraints defined for the predetermined docking position are as follows:

[0097] Adaptive depth matching constraints for mechanical reconfiguration structures:

[0098] |hH|≤ε h

[0099] Automatic centering constraints for mechanically reconfigurable structures:

[0100] |dD|≤ε d

[0101] Position constraint: x(t0) = x prep y(t0)=y prep , x(t1)=x dock =x f +d, y(t1)=y dock =y f -h;

[0102] Speed ​​constraints:

[0103] Acceleration constraints:

[0104] Where t0 is the algorithm start time, the predetermined docking trajectory planning step ends (i.e., the start time), and t1 is the predetermined docking trajectory planning step end time. (x prep y prep (x) represents the initial state coordinates, i.e., the docking preparation position. dock y dock (x) represents the final state coordinates, i.e., the predetermined docking position. f y f ) represents the tip position of the V-shaped connector in the mechanical reconfiguration structure of the front vehicle, H is the groove depth, D is the radius distance from the bottom of the interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance between the two longitudinal axes, and ε h ε is the maximum allowable depth error. d For the maximum permissible lateral offset error, v prep and a prep These represent the velocity and acceleration of the following vehicle at time t0, i.e., the start of the planning process, respectively. dock and a dock These are the expected velocity and expected acceleration at the end of the planning process, respectively.

[0105] The trajectory is generated based on the trajectory planning method from the optimal docking preparation position to the predetermined docking position, including:

[0106] Construct a fifth-degree polynomial equation and solve it to obtain the coefficients of the fifth-degree polynomial;

[0107] Based on the coefficients of the fifth-degree polynomial, the trajectory from the optimal docking preparation position to the predetermined docking position is generated;

[0108] The fifth-degree polynomial equation is as follows:

[0109] x(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 +a5t 5

[0110] y(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0111] In the formula, x(t) is the abscissa of the trajectory, y(t) is the ordinate of the trajectory, a0, a1, a2, a3, a4, and a5 are the coefficients of the fifth-degree polynomial in the horizontal direction, and t, t 2 t 3 t 4 t 5 These are different powers of time t, and b0, b1, b2, b3, b4, and b5 are the coefficients of the vertical fifth-degree polynomial.

[0112] The core task of this stage (the pre-arranged docking stage) is to ensure that the longitudinal axes of the two vehicles are parallel and to meet the adaptive depth matching constraints and automatic centering constraints of the mechanical reconstruction structure, so as to prepare for the final splicing.

[0113] During the planned docking phase, to meet the adaptive depth matching constraints and automatic centering constraints of the mechanical reconfiguration structure, a fifth-order polynomial trajectory planning method is employed to precisely control the state (including position, velocity, and acceleration) of the trajectory's start and end points. This method ensures that the longitudinal axes of the two vehicles are parallel during subsequent assembly and satisfies the adaptive assembly constraints of the mechanical reconfiguration structure, achieving high-precision docking control and avoiding positional and attitude deviations during subsequent docking, thus reducing the risk of mechanical errors and docking failure.

[0114] Using sensors to monitor the position and speed information of the two vehicles in real time, a dual PID controller combined with speed control method is adopted to drive the rear vehicle docking connector to move along the straight connection line to the front vehicle docking interface along the generated trajectory until the docking connector contacts the docking interface to complete the docking.

[0115] Specifically, driving the rear vehicle docking connector to move along a straight connection line towards the front vehicle docking interface includes:

[0116] Design a position error PID controller, calculate the target velocity, and transform the position error PID controller into a first discrete model;

[0117] Design a speed error PID controller to control the actual acceleration of the following vehicle, and convert the speed error PID controller into a second discrete model;

[0118] An acceleration curve is generated, showing that the following vehicle accelerates instantaneously relative to the preceding vehicle.

[0119] The position error PID controller is:

[0120] err pos (t)=h(t)-h target (t)

[0121]

[0122] In the formula, err pos (t) represents the longitudinal offset error of the tip, v1(t) is the actual speed of the preceding vehicle at time t, and h(t) is the longitudinal offset distance of the tip at time t. target (t) represents the longitudinal offset distance of the target, v target (t) represents the target velocity, K p1 K is the proportional coefficient of the position error PID controller. i1 K represents the integral coefficient of the position error PID controller. d1 The derivative coefficients of the position error PID controller are derr pos (t) represents the rate of change of the position error;

[0123] The first discrete model is:

[0124]

[0125] Among them, v target (k) represents the target velocity at discrete time k, err pos (k) represents the position error at discrete time k, err pos (i) represents the position error at discrete time i, Δt represents the discrete time step, and err pos(k-1) represents the position error at discrete time k-1, and v1(k) represents the actual speed of the vehicle ahead at discrete time k.

[0126] The speed error PID controller is:

[0127] err vel (t)=v tar get (t)-v2(t)

[0128]

[0129] In the formula, err vel (t) represents the error between the target vehicle speed and the actual speed of the following vehicle, v2(t) represents the actual speed of the following vehicle at time t, and a vel (t) represents the target acceleration, K p2 K is the proportional coefficient of the speed error PID controller. i2 K represents the integral coefficient of the speed error PID controller. d2 The derivative coefficients of the speed error PID controller are derr vel (t) represents the rate of change of the velocity error;

[0130] The second discrete model is:

[0131]

[0132] Among them, a vel (k) represents the acceleration at discrete time k, err vel (k) represents the velocity error at discrete time k, err vel (i) represents the velocity error at discrete time i, err vel (k-1) represents the velocity error at discrete time k-1.

[0133] Sensors are used to monitor the position and speed information of the two vehicles in real time, and dynamic adjustments are made; when |h(t)-h target When the value is less than the preset threshold, the mechanical reconfiguration structure is initiated to complete the docking.

[0134] The focus of this stage (final docking stage) is to ensure the smoothness of the acceleration process, avoid shocks or vibrations caused by sudden changes, and ultimately achieve a stable and reliable physical connection through mechanical locking or magnetic adsorption.

[0135] After docking is complete, reduce the acceleration to zero to ensure that the two vehicles remain stable in the docking position, and confirm the mechanical locking or magnetic adsorption status to ensure that the physical connection is stable and reliable.

[0136] To more clearly illustrate the technical solution of the present invention, specific embodiments are provided below for description:

[0137] A trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter specifically includes:

[0138] S1. Ensure that the hardware configurations of the two self-balancing scooters are identical, including sensors (such as gyroscopes, accelerometers, and GPS), motors, and control systems. Prepare splicing devices, such as mechanical or magnetic connectors, for physical connection.

[0139] S2. Calibrate the sensors of both vehicles to ensure data consistency for synchronized control and trajectory planning. Establish a wireless communication system between the two vehicles to enable real-time transmission of data and control commands. Bluetooth, Wi-Fi, or other wireless communication methods can be used.

[0140] S3. The system receives the splicing command and begins preparing for the splicing operation. The status of the preceding vehicle is detected through the communication system.

[0141] S4. Docking Preparation Phase: Evaluate and select the optimal docking preparation position, generate a smooth path from the current position to the preparation position, and ensure that the heading angles and speeds of both vehicles are consistent. To cope with dynamic environments and diverse constraints, a method of separate path and speed planning is adopted to independently optimize the path shape and speed curve, thereby ensuring real-time adjustments and adaptation to complex environments. Figure 2 .

[0142] S4.1. Evaluate and select the best docking preparation position;

[0143] S4.1.1. Considering the speed of the first vehicle, determine the safe distance:

[0144] D safe =max(k·v1,D min )

[0145] In the formula, D safe To determine the safe distance after comprehensively considering speed and minimum safe distance, k is the safety factor, v1 is the speed of the first vehicle, and D... min This is the minimum safe distance.

[0146] S4.1.2, in D safe Based on this, multiple candidate locations are generated through horizontal and vertical offsets:

[0147]

[0148] In the formula, (x1, y1, θ1) represents the current position and orientation angle of the first vehicle, and i and j represent the indices of the longitudinal and lateral offsets, respectively, with a range of... arrive arrive n represents the number of candidate positions in the vertical direction, and m represents the number of candidate positions in the horizontal direction; x centerLet y be the x-coordinate of the center position. center P is the ordinate of the center position. center p is the central position ij Δx represents the candidate position, Δy represents the vertical offset, and Δy represents the horizontal offset.

[0149] S4.1.3 Design the comprehensive cost function and calculate the optimal docking preparation position:

[0150] C total =w l ·C length +w r ·C reachability +w d ·C direction +w e ·C env +w k ·C risk =w l ·L length +w r ·C reachability +w d ·|θ goal -θ1|+w e ·E actual +w k ·R actual

[0151] In the formula, w l w r w d w e w k These are the weighting coefficients for path length, path reachability, directional consistency, environmental complexity, and risk assessment value, respectively. total C represents the total value of the comprehensive cost function. length C is the path length cost. reachability C is the cost of path reachability. direction For the cost of directional consistency, C env For the cost of environmental complexity, C risk For the cost of risk assessment, L length θ is the path length. goal Let θ1 be the target direction angle, and E be the current direction angle of the first vehicle. actual R represents the actual environmental complexity value. actual This is the actual risk assessment value.

[0152] S4.1.4 Select the candidate location with the lowest overall cost as the final docking preparation location:

[0153] P prep =P best =argmin i,jC total (P i,j )

[0154] Among them, P prep P prepares the position for optimal docking. best For the optimal position, C total () is the total cost function, P i,j These are candidate positions.

[0155] S4.2 Path Planning: Based on the global path curve, a quadratic planning method is used to generate a smooth path from the current position of the following vehicle to the docking preparation position behind the preceding vehicle.

[0156] S4.2.1. Taking into account both the vehicle's dynamic performance and passenger comfort, design the optimization variable U:

[0157] U = {d0, d1, ..., d} n-2 ,d n-1 ,d′0,d′1,…,d′ n-2 ,d′ n-1 ,d″0,d″1,…,d″ n-2 ,d″ n-1}

[0158] Where: d, d′, d″ are the lateral displacement, the first derivative of the lateral displacement, and the second derivative of the lateral displacement in the Frenet coordinate system;

[0159] The objective function is designed as follows:

[0160]

[0161] k in the formula d k d1 k d2 k d3 To optimize the weights of variables, and the weight k for the docking preparation position. prep-d k prep-d1 k prep-d2 .

[0162] The solution for the third derivative is as follows: but

[0163]

[0164] S4.2.2 Design constraint conditions based on dynamic constraints:

[0165] bound low ≤d,d′,d″,d″′≤bound up

[0166] d″ i+1 =d″i +d″′ i→i+1 *Δs

[0167]

[0168] d0,d′0,d″0=x init

[0169] Where, bound low The lower boundary value, bound up Let d, d′, d″, d″′ be the upper boundary values, and let d, d′, d″′ be the lateral displacement, first derivative of the lateral displacement, second derivative of the lateral displacement, and third derivative of the lateral displacement in the Frenet coordinate system, respectively; Δs is the longitudinal increment, x init Let be the initial lateral displacement, the first derivative of the lateral displacement, and the second derivative of the lateral displacement.

[0170] S4.2.3 Use a quadratic programming solver (such as QuadProg or CVXOPT) to solve the above optimization problem and generate a smooth path for the following vehicle from its current position to the docking preparation position.

[0171] S4.3. Speed ​​planning is performed using dynamic programming and quadratic optimization:

[0172] S4.3.1. Perform fixed time interval sampling in the horizontal t direction of the ST diagram, for example (1s, 2s, 3s, 4s, 5s), and perform fixed distance interval sampling in the vertical s direction.

[0173] S4.3.2 Generate multiple candidate velocity curves based on speed limits, acceleration limits, and obstacle limitations. S4.3.3 Design the state transition equations and cost function:

[0174]

[0175] Wherein, C(s) i-1 ,t j-1 ,s i ,t j ) indicates from state (s) i-1 ,t j-1 ) transferred to (s i ,t j The cost of ).

[0176] C(s i-1 ,t j-1 ,s i ,t j ) = C obs (s i ,t j )+C prep (s i ,tj )+C speed (s i-1 ,t j-1 ,s i ,t j )

[0177] +C acc (s i-1 ,t j-1 ,s i ,t j )+C jerk (s i-1 ,t j-1 ,s i ,t j )+C t (s i-1 ,t j-1 ,s i ,t j )

[0178] In the formula, C obs (s i ,t j ) represents the cost of the obstacle, C prep (s i ,t j ) represents the cost of moving from the docking preparation position, C speed (s i-1 ,t j-1 ,s i ,t j ), C acc (s i-1 ,t j-1 ,s i ,t j ), C jerk (s i-1 ,t j-1 ,s i ,t j The costs are velocity cost, acceleration cost, and jerk cost, respectively. t (s i-1 ,t j-1 ,s i ,t j (This is a time cost.)

[0179] S4.3.4. Use dynamic programming to traverse the system and start from the endpoint state. Follow the state transition records and backtrack to the starting point to obtain the optimal speed curve.

[0180] S4.3.5. Set optimization variables and optimization objective function:

[0181] X = {s0, s1, ..., s} n-2 ,s n-1,s′0,s′1,…,s′ n-2 ,s′ n-1 ,s″0,s″1,…,s″ n-2 ,s″ n-1}

[0182] Where: s, s′, s″ are the longitudinal displacements in the Frenet coordinate system, the first derivative of the longitudinal displacement, and the second derivative of the longitudinal displacement.

[0183] The objective function is designed as follows:

[0184]

[0185] k in the formula s k s1 k s2 k s3 To optimize the weights of variables, and the weight k for the docking preparation position. prep-s k prep-s1 k prep-s2 .

[0186] The solution for the third derivative is as follows:

[0187] S4.3.6 Design constraint conditions based on dynamic constraints:

[0188] bound low ≤s,s′,s″≤bound up

[0189] s″ i+1 =s″ i +s″ i→i+1 *Δt

[0190]

[0191] s0,s′0,s″0=x init

[0192] In the formula, bound low The lower boundary value, bound up Let x be the upper boundary value, Δt be the time increment, s0, s′0, s″0 be the longitudinal displacement, the first derivative of the longitudinal displacement, and the second derivative of the longitudinal displacement in the Frenet coordinate system, respectively. init Let be the initial longitudinal displacement and its first and second derivatives.

[0193] S4.3.7 Use a quadratic programming solver (such as QuadProg or CVXOPT) to solve the above optimization problem and generate a smooth speed curve of the rear vehicle from its current position to the docking preparation position.

[0194] S4.4. The path curve and velocity curve are fitted to generate a complete docking preparation trajectory. When the following vehicle reaches the predetermined docking position along this trajectory, the first stage of trajectory planning is completed, and the following conditions are met:

[0195] Heading angle consistency constraint:

[0196] |θ1-θ2|≤∈ θ

[0197] Where θ1 is the heading angle of the leading vehicle, θ2 is the heading angle of the trailing vehicle, ∈ θ This represents the maximum permissible deviation between the heading angles of the two vehicles.

[0198] Speed ​​matching constraints:

[0199] |v1-v2|≤∈ v

[0200] Where v1 is the speed of the vehicle in front, v2 is the speed of the vehicle behind, ∈ v This represents the maximum permissible deviation between the speeds of the two vehicles.

[0201] S5. Pre-Dock Stage: The core task of this stage is to ensure that the longitudinal axes of the two vehicles are parallel and to satisfy the adaptive depth matching constraint and the adaptive automatic centering constraint of the mechanical reconfiguration structure. To ensure the smoothness and continuity of the trajectory, and to precisely control the starting and ending states of the trajectory, while also considering the need for high-precision trajectory tracking and control in this stage to achieve a high degree of consistency and synchronization between the two vehicles in space, a fifth-order polynomial trajectory generation method is adopted. For example... Figure 3 .

[0202] S5.1 The predetermined docking point shall meet the following constraints:

[0203] Adaptive depth matching constraints for mechanical reconfiguration structures:

[0204] |hH|≤ε h

[0205] Automatic centering constraints for mechanically reconfigurable structures:

[0206] |dD|≤ε d

[0207] like Figure 5 The V-shaped mating connector in the mechanical reconfiguration structure shown is defined as follows: H is the groove depth, D is the radius distance from the bottom of the mating interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance between the two longitudinal axes, and ε... h ε is the maximum allowable depth error. d The maximum permissible lateral offset error. The predetermined docking position P can be calculated from the position of the preceding vehicle. dock .

[0208] S5.2 Establishing a quintic polynomial equation:

[0209] The initial state of the second vehicle is:

[0210]

[0211] Where, x(t0)=x prep , y(t0)=y prep , After time T, the destination P is reached. dock Where T = t1 - t0, then the target state is Where x(t1) = x dock , y(t1)=y dock ,

[0212] Then the fifth-degree polynomials of x and y with respect to time t are:

[0213] x(t) = a0 + a1t + a2t 2 +a3t 3 +a4t 4 +a5t 5

[0214] y(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0215] In the formula, a0-a5 and b0-b5 are unknown coefficients. Let t0 = 0, then t1 = T. From the initial state and the target state, the system of equations can be obtained:

[0216] x(t0)=x(0)=a0=x prep

[0217] y(t0)=y(0)=b0=y prep

[0218]

[0219] x(t1)=x(T)=a0+a1T+a2T 2 +a3T 3 +a4T 4 +a5T 5 =x dock

[0220] y(t1)=y(T)=b0+b1T+b2T2 +b3T 3 +b4T 4 +b5T 5 =y dock

[0221]

[0222] Solve the system of linear equations to obtain the coefficients of the fifth-degree polynomial.

[0223] S5.3. Based on the obtained polynomial coefficients, generate a trajectory from the docking preparation position to the predetermined docking position. When the following vehicle reaches the predetermined docking position along this trajectory, the second stage of trajectory planning is completed.

[0224] S6. Final Docking Stage: The focus of this stage is to ensure the smoothness of the accelerated docking process, avoiding shocks or vibrations caused by sudden changes, and ultimately achieving a stable and reliable physical connection through methods such as mechanical locking or magnetic adsorption. To achieve this goal, a dual PID controller combined with a speed-based control method is used to achieve a smoother and more stable docking process. For example... Figure 4 .

[0225] S6.1 Design a position error PID controller: used to calculate the target speed so that the following vehicle can successfully dock and reach the expected position, and gradually adjust the speed to match the speed of the preceding vehicle.

[0226] err pos (t)=h(t)-h target (t)

[0227]

[0228] Where err pos v1(t) represents the offset error of the tip in the longitudinal direction, and v1(t) represents the actual speed of the preceding vehicle at time t.

[0229] Transformed into the first discrete model:

[0230]

[0231] S6.2 Design a speed error PID controller: used to control the actual acceleration of the following vehicle to make it as close as possible to the speed of the preceding vehicle.

[0232] err vel (t)=v target (t)-v2(t)

[0233]

[0234] Among them, err vel(t) represents the error between the target vehicle speed and the actual speed of the following vehicle, and v2(t) represents the actual speed of the following vehicle at time t.

[0235] Transformed into the second discrete model:

[0236]

[0237] An acceleration curve is generated, showing that the following vehicle accelerates instantaneously relative to the preceding vehicle.

[0238] S6.3 Use sensors to monitor the position and speed information of the two vehicles in real time and make dynamic adjustments to ensure a smooth docking process.

[0239] When h(t) approaches h target At that time, and |h(t)-h target When (t) is less than the preset threshold, mechanical reconfiguration of the structure is initiated.

[0240] S6.4 After docking is completed, reduce the acceleration to zero to ensure that the two vehicles remain stable in the docking position, and confirm the mechanical locking or magnetic adsorption state to ensure that the physical connection is stable and reliable.

[0241] This invention employs a combination of quadratic programming and dynamic programming to rapidly generate the path of the following vehicle, improving path planning efficiency. Utilizing the state information of the preceding vehicle and the structural parameters of the mechanical reconfiguration mechanism, it accurately calculates the optimal docking preparation position and the predetermined docking position, generating a high-precision trajectory. A dual PID controller is used to precisely plan and control the vehicle's speed and acceleration, ensuring a smooth docking process and a high success rate.

[0242] This invention combines two two-wheeled self-balancing scooters into a four-wheeled vehicle, increasing the number of wheels in contact with the ground, improving overall vehicle stability, and reducing the risk of tipping over, especially during turning, acceleration, or deceleration. The four-wheel design allows the vehicle to better adapt to complex road conditions, including uneven roads, slopes, and slippery surfaces. Optimized trajectory planning enables the vehicle to smoothly traverse various road surfaces, improving its applicability. Precise trajectory planning and control algorithms ensure smooth acceleration and directional adjustments during the conversion from two-wheeled to four-wheeled scooters, reducing operational risks and ensuring safety. The four-wheeled design provides greater load capacity and a more spacious driving area, allowing for the carrying of more luggage or passengers, meeting diverse usage needs. The trajectory planning method for converting two-wheeled self-balancing scooters into four-wheeled vehicles provided by this invention not only inherits the flexibility of two-wheeled scooters but also combines the stability and load-bearing capacity of four-wheeled vehicles, providing a new direction for the development of intelligent transportation tools.

[0243] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A trajectory planning method for reconstructing a four-wheeled vehicle from a double-balanced two-wheeled vehicle, characterized by, include: Establish wireless communication between two self-balancing vehicles, calculate the optimal docking preparation position based on the status information of the leading vehicle, generate the path of the trailing vehicle from its current position to the optimal docking preparation position using a quadratic programming method, and plan the speed of the trailing vehicle based on dynamic programming and quadratic optimization. The longitudinal axes of the preceding and following vehicles are determined to be parallel. Based on the status information of the preceding vehicle and the structural parameters of the mechanical reconfiguration mechanism, the predetermined docking position is calculated, and a trajectory planning method is used to generate a trajectory from the optimal docking preparation position to the predetermined docking position. Using sensors to monitor the position and speed information of the two vehicles in real time, a dual PID controller combined with speed control method is adopted to drive the rear vehicle docking connector to move along the straight connection line to the front vehicle docking interface along the generated trajectory until the docking connector contacts the docking interface to complete the docking. Calculate the optimal docking preparation position based on the status information of the preceding vehicle, including: Determine a safe distance based on the speed of the vehicle in front; Based on the safety distance, several candidate positions are generated by lateral and longitudinal offsets; Calculate the comprehensive cost function for each candidate position, and select the candidate position with the minimum comprehensive cost as the optimal docking preparation position; The method for determining the safe distance is as follows: In the formula, To determine the safe distance after comprehensively considering speed and minimum safe distance, For safety reasons, The speed of the first car Minimum safe distance; The method for generating several candidate positions is as follows: In the formula, This refers to the current position and heading angle of the first vehicle. i and j These represent the indices for the vertical offset and the horizontal offset, respectively, with a range of... , n This represents the number of candidate positions in the vertical direction. m This represents the number of candidate positions in the horizontal direction. The x-coordinate of the center position, The ordinate of the center position, As the central location, As candidate positions, This is the vertical offset. This represents the lateral offset.

2. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 1, characterized in that, The method for calculating the optimal docking preparation position is as follows: In the formula, Prepare the optimal docking position. For the best position, Let the total cost function be... These are candidate positions.

3. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 1, characterized in that, After the speed of the following vehicle is planned, the heading angle and speed of the preceding and following vehicles should respectively satisfy: in, The heading angle of the vehicle in front. For the heading angle of the following vehicle, This represents the maximum permissible deviation between the heading angles of the two vehicles. The speed of the vehicle in front. The speed of the vehicle behind. This represents the maximum permissible deviation between the speeds of the two vehicles.

4. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 1, characterized in that, The constraints defined for the predetermined docking position are as follows: Adaptive depth matching constraints for mechanical reconfiguration structures: Automatic centering constraints for mechanically reconfigurable structures: Position constraints: , , , ; Speed ​​constraints: , , , ; Acceleration constraints: , , , ; in, The start time of the algorithm is predetermined when the docking trajectory planning step ends. The end time of the planned docking trajectory steps. The initial state coordinates are the docking preparation positions. The coordinates represent the final state, i.e., the predetermined docking position. The position of the V-shaped connector tip in the mechanical reconfiguration structure of the preceding vehicle is shown, where H is the groove depth and D is the radius distance from the bottom of the interface. h This represents the longitudinal offset distance of the tip. d This represents the lateral offset distance between the two longitudinal axes. For the maximum allowable depth error, The maximum permissible lateral offset error, and They are respectively The moment refers to the speed and acceleration of the following vehicle at the start of the plan. and These are the expected velocity and expected acceleration at the end of the planning process, respectively.

5. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 4, characterized in that, Generate a trajectory from the optimal docking preparation position to the predetermined docking position based on a trajectory planning method, including: Construct a fifth-degree polynomial equation and solve it to obtain the coefficients of the fifth-degree polynomial; Based on the coefficients of the fifth-degree polynomial, the trajectory from the optimal docking preparation position to the predetermined docking position is generated; The fifth-degree polynomial equation is as follows: In the formula, Let x be the x-coordinate of the trajectory. Let be the ordinate of the trajectory. These are the coefficients of the horizontal fifth-degree polynomial. They are time Different powers of terms, These are the coefficients of the vertical fifth-degree polynomial.

6. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 1, characterized in that, Driving the rear vehicle docking connector to move along a straight connection line towards the front vehicle docking interface includes: Design a position error PID controller, calculate the target velocity, and transform the position error PID controller into a first discrete model; Design a speed error PID controller to control the actual acceleration of the following vehicle, and convert the speed error PID controller into a second discrete model; Based on the acceleration curve generated by the speed error PID controller, the rear vehicle docking connector is driven to move instantaneously along the acceleration curve to the front vehicle docking interface.

7. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 6, characterized in that, The position error PID controller is: In the formula, This refers to the longitudinal offset error of the tip. Let t be the actual speed of the vehicle in front at time t. h(t) for t The vertical offset distance of the tip of time. The longitudinal offset distance of the target. For the target speed, This is the proportional coefficient of the position error PID controller. The integral coefficients of the position error PID controller are... These are the differential coefficients of the position error PID controller. This represents the rate of change of the position error; The first discrete model is: in, Discrete time k Target speed, Discrete time k Positional error, Discrete time i Positional error, For discrete time steps, Discrete time k -1 position error, Discrete time k The actual speed of the vehicle in front; The speed error PID controller is: In the formula, The difference between the target vehicle speed and the actual speed of the following vehicle. Let t be the actual speed of the following vehicle at time t. Accelerate towards the target The proportional coefficient of the speed error PID controller. The integral coefficients of the speed error PID controller are... The differential coefficients of the speed error PID controller are... The rate of change of the velocity error; The second discrete model is: in, Discrete time k acceleration, Discrete time k Speed ​​error, Discrete time i Speed ​​error, Discrete time k -1 speed error.

8. The trajectory planning method for reconstructing a four-wheeled vehicle from a dual-balancing scooter according to claim 7, characterized in that, Sensors are used to monitor the position and speed information of the two vehicles in real time, and dynamic adjustments are made; when position error When the value is less than a preset threshold, the mechanical reconfiguration structure is activated to complete the docking. h(t) for t The vertical offset distance of the tip of time. The longitudinal offset distance of the target.

Images