Trajectory planning method for reconstructing double-balance vehicle into four-wheeler

Abstract

The invention discloses a trajectory planning method for reconstructing double balance vehicles into a four-wheeler, which comprises the following steps of: establishing wireless communication between the two balance vehicles, calculating an optimal docking preparation position according to state information of a front vehicle, generating a path of a rear vehicle from a current position to the optimal docking preparation position by using a quadratic programming method, and determining the path of the rear vehicle from the current position to the optimal docking preparation position. Planning the speed of the rear vehicle based on dynamic planning and secondary optimization; determining that the longitudinal axes of the front vehicle and the rear vehicle are parallel, calculating a predetermined docking position according to the state information of the front vehicle and the structural parameters of the mechanical reconstruction mechanism, and generating a track from the optimal docking preparation position to the predetermined docking position by adopting a track planning method; a sensor is used for monitoring the position and speed information of the two vehicles in real time, and a control method in which double PID controllers are combined with a speed form is adopted to drive a butt joint connector of the rear vehicle to move towards a butt joint port of the front vehicle along a linear connecting line along a generated track until the butt joint connector is in contact with the butt joint port to complete butt joint. According to the invention, the efficiency and accuracy of path planning are improved.

Description

Technical Field

[0001] The present 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-wheel vehicle. Background Art

[0002] Two-Wheeled Self-Balancing Scooter, also known as a self-balancing scooter or electric scooter, is an electric vehicle. It uses advanced gyroscopes and accelerometers to monitor the vehicle's tilt angle and motion state in real time, and quickly adjusts the speed and direction of the wheels to maintain balance. Users only need to lean forward or backward to control the speed of the vehicle, and lean left or right to control the direction. The operation is simple and intuitive. Two-wheeled self-balancing scooters are usually small in size and can flexibly turn and move in a small space.

[0003] Although two-wheeled balancing cars have many advantages, they also have some significant disadvantages and limitations. It may take time for first-time users to adapt to their operation, especially when driving on uneven roads, where they are prone to losing balance. When turning, accelerating or decelerating, improper operation may cause the vehicle body to tip over, increasing the risk of use. Two-wheeled balancing cars have high requirements for the flatness of the road surface and are difficult to drive on potholes, gravel, slippery or muddy roads. On steep slopes, two-wheeled balancing cars may not provide sufficient power or stability and are prone to slipping or overturning. Due to the limitations of the structural design, two-wheeled balancing cars have a small load capacity and are usually suitable for single-person riding. They cannot carry much luggage, and their pedals and space are small, so standing and driving for a long time may cause fatigue.

[0004] Traditional trajectory planning methods usually require a lot of computing resources and time, especially in complex environments. This leads to inefficient path planning and cannot meet real-time requirements. In addition, the generated path may not be optimal, and there is a problem of path deviation. This deviation may cause the dual-balance vehicle to be unable to dock accurately, increasing the control difficulty and the risk of docking failure. The existing methods do not accurately control the vehicle speed and acceleration, which can easily cause the vehicle to shake or be unstable during the docking movement. This not only affects the docking accuracy, but may also cause damage to the mechanical structure. Summary of the invention

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

[0006] To achieve the above object, the present invention provides a trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle, comprising:

[0007] Establish wireless communication between the two balancing vehicles, calculate the optimal docking preparation position according to the state information of the front vehicle, use the quadratic programming method to generate the path of the rear vehicle from the current position to the optimal docking preparation position, and plan the speed of the rear vehicle based on dynamic programming and quadratic optimization;

[0008] Determining that the longitudinal axes of the leading vehicle and the trailing vehicle are parallel, calculating a predetermined docking position according to the state information of the leading vehicle 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;

[0009] Sensors are used to monitor the position and speed information of the two vehicles in real time. A dual PID controller combined with a speed control method is used to drive the docking connector of the rear vehicle along a straight connecting line toward the docking interface of the front vehicle along the generated trajectory until the docking connector contacts the docking interface to complete the docking.

[0010] Preferably, calculating the optimal docking preparation position according to the status information of the preceding vehicle includes:

[0011] Determine the safe distance based on the speed of the vehicle ahead;

[0012] Based on the safety distance, generating a plurality of candidate positions by lateral and longitudinal offsets;

[0013] The comprehensive cost function of each candidate position is calculated, and the candidate position with the smallest comprehensive cost is selected as the optimal docking preparation position.

[0014] Preferably, the method for determining the safety distance is:

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

[0016] Where D safe is the safety distance after comprehensive consideration of speed and minimum safety distance, k is the safety factor, v1 is the speed of the first car, and D min is the minimum safe distance;

[0017] The method for generating several candidate locations is:

[0018]

[0019] Where (x1, y1, θ1) is the current position and direction angle of the first vehicle, i and j represent the index of the longitudinal offset and the index of the lateral offset respectively, and the range is arrive arrive n is the number of vertical candidate positions, m is the number of horizontal candidate positions; x center is the horizontal coordinate of the center position, y centeris the vertical coordinate of the center position, P center is the center position, p ij is the candidate position, Δx is the longitudinal offset, and Δy is the lateral offset.

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

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

[0022] Where P prep To prepare the position for optimal docking, P best is the best position, C total () is the total cost function, P i,j for candidate locations.

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

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

[0025] |v1-v2|≤∈ v

[0026] Among them, θ1 is the heading angle of the front vehicle, θ2 is the heading angle of the rear vehicle, ∈ θ is the maximum allowable deviation between the heading angles of the two vehicles; v1 is the speed of the front vehicle, v2 is the speed of the rear vehicle, ∈ v is the maximum permissible deviation between the speeds of the two vehicles.

[0027] Preferably, the constraints defined for the predetermined docking position are:

[0028] Mechanical reconstruction structure adaptive depth matching constraints:

[0029] |hH|≤ε h

[0030] Automatic centering constraints of mechanical reconstruction structure:

[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] Among them, t0 is the start time of the algorithm, and t1 is the end time of the scheduled docking trajectory planning step. (x prep ,y prep ) is the initial state coordinate, i.e., the docking preparation position, (x dock ,y dock ) is the final state coordinate, i.e., the predetermined docking position, (x f ,y f ) is the tip position of the V-shaped docking connector in the mechanical reconstruction structure of the front vehicle, H is the groove depth, D is the radius distance of the bottom of the docking interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance of the two longitudinal axes, ε h is the maximum allowable depth error, ε d is the maximum permissible lateral offset error, v prep and a prep are the speed and acceleration of the following vehicle at time t0, i.e. the beginning of planning, and v dock and a dock are the expected speed and expected acceleration at the end of planning, respectively.

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

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

[0038] generating the trajectory from the optimal docking preparation position to the predetermined docking position according to the coefficients of the quintic polynomial;

[0039] Wherein, the fifth-order polynomial equation is:

[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] Where x(t) is the horizontal coordinate of the trajectory, y(t) is the vertical coordinate of the trajectory, a0, a1, a2, a3, a4, and a5 are the coefficients of the horizontal quintic polynomial, and t, t 2 ,t 3 ,t 4 ,t 5 are the different powers of time t, b0, b1, b2, b3, b4, and b5 are the coefficients of the longitudinal quintic polynomial.

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

[0044] Design a position error PID controller, calculate a target speed, and convert 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] According to the acceleration curve generated by the speed error PID controller, the rear vehicle docking connector is driven to perform instantaneous acceleration linear motion along the acceleration curve toward 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) is the longitudinal offset error of the tip, v1(t) is the actual speed of the preceding vehicle at time t, h(t) is the longitudinal offset distance of the tip at time t, h target (t) is the target longitudinal offset distance, v target (t) is the target speed, K p1 is the proportional coefficient of the position error PID controller, K i1 is the integral coefficient of the position error PID controller, K d1 is the differential coefficient of the position error PID controller, derr pos (t) is the rate of change of position error;

[0051] The first discrete model is:

[0052]

[0053] Among them, v target(k) is the target speed at discrete time k, err pos (k) is the position error at discrete time k, err pos (i) is the position error at discrete time i, Δt is the discrete time step, err pos (k-1) is the position error at discrete time k-1, v1(k) is the actual speed of the preceding vehicle 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) is the error between the target speed and the actual speed of the following vehicle, v2(t) is the actual speed of the following vehicle at time t, and a vel (t) is the target acceleration, K p2 is the proportional coefficient of the speed error PID controller, K i2 is the integral coefficient of the speed error PID controller, K d2 is the differential coefficient of the speed error PID controller, derr vel (t) is the rate of change of velocity error;

[0058] The second discrete model is:

[0059]

[0060] Among them, a vel (k) is the acceleration at discrete time k, err vel (k) is the velocity error at discrete time k, err vel (i) is the velocity error at discrete time i, err vel (k-1) is 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 | is less than the preset threshold, the mechanical reconstruction structure is started to complete the docking, where h(t) is the longitudinal offset distance of the tip at time t, h target (t) is the target longitudinal offset distance.

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

[0063] (1) The present invention adopts a method of independently planning the path and speed, and independently optimizes the path shape and speed curve, thereby ensuring that the following vehicle can adjust and adapt to complex environments in real time. 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 safety of the overall operation.

[0064] (2) In order to meet the adaptive depth matching constraints and automatic centering constraints of the mechanical reconstruction structure, the present invention adopts a quintic polynomial trajectory planning method to accurately control the states (including position, velocity and acceleration) of the starting and ending points of the trajectory. It ensures that the longitudinal axes of the two vehicles are parallel in the subsequent splicing process, and meets the adaptive splicing constraints of the mechanical reconstruction structure, realizes high-precision docking control, avoids deviations in position and attitude in the subsequent docking process, and reduces the risk of mechanical errors and docking failures that may occur in the docking process.

[0065] (3) In order to ensure the stability of the docking connector of the rear vehicle when docking with the docking interface of the front vehicle and reduce the impact force, the present invention adopts a dual PID algorithm for control under the premise of satisfying the automatic centering constraints of the mechanical reconstruction structure. The dual PID algorithm is used to accurately adjust the speed and acceleration to ensure the stability of the acceleration process, avoid shock or vibration caused by sudden changes, and finally complete the splicing through mechanical locking. Ensure the stability and safety of the docking process and reduce the risk of docking failure. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0067] Figure 1 A flow chart of a trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to an embodiment of the present invention;

[0068] Figure 2 This is a flow chart of the docking preparation phase of an embodiment of the present invention;

[0069] Figure 3 This is a flow chart of the scheduled docking phase of an embodiment of the present invention;

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

[0071] Figure 5 This is a V-shaped docking connector in the mechanical reconstruction structure of an embodiment of the present invention. DETAILED DESCRIPTION

[0072] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

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

[0074] The present invention proposes a trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle, such as Figure 1 ,include:

[0075] Establish wireless communication between the two balancing vehicles, calculate the optimal docking preparation position according to the state information of the front vehicle, use the quadratic programming method to generate the path of the rear vehicle from the current position to the optimal docking preparation position, and plan the speed of the rear vehicle based on dynamic programming and quadratic optimization;

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

[0077] Determine the safe distance based on the speed of the vehicle ahead;

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

[0079] The comprehensive cost function of each candidate position is calculated, and the candidate position with the smallest comprehensive cost is selected as the optimal docking preparation position.

[0080] The method for determining the safety distance is:

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

[0082] Where D safe is the safety distance after comprehensive consideration of speed and minimum safety distance, k is the safety factor, v1 is the speed of the first car, and D min is the minimum safe distance;

[0083] The method for generating several candidate locations is:

[0084]

[0085] Where (x1, y1, θ1) is the current position and direction angle of the first vehicle, i and j represent the index of the longitudinal offset and the index of the lateral offset respectively, and the range is arrive arrive n is the number of vertical candidate positions, m is the number of horizontal candidate positions; x center is the horizontal coordinate of the center position, y center is the vertical coordinate of the center position, P center is the center position, p ij is the candidate position, Δx is the longitudinal offset, and Δy is the lateral offset.

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

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

[0088] Where P prep To prepare the position for optimal docking, P best is the best position, C total () is the total cost function, P i,j for candidate locations.

[0089] After planning the speed of the following vehicle, the heading angle and speed of the leading and following vehicles should satisfy:

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

[0091] |v1-v2|≤∈ v

[0092] Among them, θ1 is the heading angle of the front vehicle, θ2 is the heading angle of the rear vehicle, ∈ θ is the maximum allowable deviation between the heading angles of the two vehicles; v1 is the speed of the front vehicle, v2 is the speed of the rear vehicle, ∈ v is 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] In the docking preparation phase, in order to cope with the dynamic environment and diverse constraints, the path and speed are planned independently, and the path shape and speed curve are optimized independently, so as to ensure that the following vehicle can adjust and adapt to the complex environment in real time. By adjusting the speed and direction in advance, 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 safety of the overall operation.

[0095] Determining that the longitudinal axes of the leading vehicle and the trailing vehicle are parallel, calculating a predetermined docking position according to the state information of the leading vehicle and the structural parameters of the mechanical reconstruction mechanism, and using a trajectory planning method 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:

[0097] Mechanical reconstruction structure adaptive depth matching constraints:

[0098] |hH|≤ε h

[0099] Automatic centering constraints of mechanical reconstruction structure:

[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] Among them, t0 is the start time of the algorithm, and t1 is the end time of the scheduled docking trajectory planning step. (x prep ,y prep ) is the initial state coordinate, i.e., the docking preparation position, (x dock ,y dock ) is the final state coordinate, i.e., the predetermined docking position, (x f ,y f ) is the tip position of the V-shaped docking connector in the mechanical reconstruction structure of the front vehicle, H is the groove depth, D is the radius distance of the bottom of the docking interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance of the two longitudinal axes, ε h is the maximum allowable depth error, ε d is the maximum permissible lateral offset error, v prep and a prep are the speed and acceleration of the following vehicle at time t0, i.e. the beginning of planning, and v dock and a dock are the expected speed and expected acceleration at the end of planning, respectively.

[0105] Generate a trajectory from the optimal docking preparation position to the predetermined docking position based on the trajectory planning method, including:

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

[0107] generating the trajectory from the optimal docking preparation position to the predetermined docking position according to the coefficients of the quintic polynomial;

[0108] Wherein, the fifth-order polynomial equation is:

[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] Where x(t) is the horizontal coordinate of the trajectory, y(t) is the vertical coordinate of the trajectory, a0, a1, a2, a3, a4, and a5 are the coefficients of the horizontal quintic polynomial, and t, t 2 ,t 3 ,t 4 ,t 5 are the different powers of time t, b0, b1, b2, b3, b4, and b5 are the coefficients of the longitudinal quintic polynomial.

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

[0113] In the scheduled docking phase, in order to meet the adaptive depth matching constraints and automatic centering constraints of the mechanical reconstruction structure, the quintic polynomial trajectory planning method is used to accurately control the states (including position, speed and acceleration) of the start and end points of the trajectory. This method ensures that the longitudinal axes of the two vehicles are parallel in the subsequent splicing process, and meets the adaptive splicing constraints of the mechanical reconstruction structure, achieving high-precision docking control, avoiding deviations in position and attitude in the subsequent docking process, and reducing the risk of mechanical errors and docking failures that may occur during the docking process.

[0114] Sensors are used to monitor the position and speed information of the two vehicles in real time. A dual PID controller combined with a speed control method is used to drive the docking connector of the rear vehicle along a straight connecting line toward the docking interface of the front vehicle 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 the straight connection line toward the front vehicle docking interface includes:

[0116] Design a position error PID controller, calculate a target speed, and convert 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] Generate an acceleration curve, and the rear vehicle performs instantaneous acceleration and linear motion relative to the front 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) is the longitudinal offset error of the tip, v1(t) is the actual speed of the preceding vehicle at time t, h(t) is the longitudinal offset distance of the tip at time t, h target (t) is the target longitudinal offset distance, v target (t) is the target speed, K p1 is the proportional coefficient of the position error PID controller, K i1 is the integral coefficient of the position error PID controller, K d1 is the differential coefficient of the position error PID controller, derr pos (t) is the rate of change of position error;

[0123] The first discrete model is:

[0124]

[0125] Among them, v target (k) is the target speed at discrete time k, err pos (k) is the position error at discrete time k, err pos (i) is the position error at discrete time i, Δt is the discrete time step, err pos(k-1) is the position error at discrete time k-1, v1(k) is the actual speed of the preceding vehicle 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) is the error between the target speed and the actual speed of the following vehicle, v2(t) is the actual speed of the following vehicle at time t, and a vel (t) is the target acceleration, K p2 is the proportional coefficient of the speed error PID controller, K i2 is the integral coefficient of the speed error PID controller, K d2 is the differential coefficient of the speed error PID controller, derr vel (t) is the rate of change of velocity error;

[0130] The second discrete model is:

[0131]

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

[0133] Use sensors to monitor the position and speed of the two vehicles in real time and make dynamic adjustments; when |h(t)-h target |When it is less than the preset threshold, the mechanical reconstruction structure is started to complete the docking.

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

[0135] After docking, reduce 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] In order to more clearly express the technical solution of the present invention, the following specific embodiments are provided to introduce the solution:

[0137] A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle specifically includes:

[0138] S1. Ensure that the hardware configuration of the two two-wheeled balancing vehicles is consistent, including sensors (such as gyroscopes, accelerometers, GPS), motors, and control systems. Prepare splicing devices, such as mechanical connectors or magnetic connectors, for physical connection.

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

[0140] S3, the system receives the splicing instruction and starts to prepare 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 best docking preparation position, generate a smooth path from the current position to the docking preparation position, and ensure that the heading angle and speed of the two vehicles are consistent. In order to cope with dynamic environments and diverse constraints, the path and speed planning method is adopted to independently optimize the path shape and speed curve, thereby ensuring real-time adjustment and adaptation to complex environments. Figure 2 .

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

[0143] S4.1.1. Consider the speed of the first vehicle and determine the safe distance:

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

[0145] Where D safe is the safety distance after comprehensive consideration of speed and minimum safety distance, k is the safety factor, v1 is the speed of the first car, and D min The minimum safe distance.

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

[0147]

[0148] Where (x1, y1, θ1) is the current position and direction angle of the first vehicle, i and j represent the index of the longitudinal and lateral offsets respectively, and the range is arrive arrive n is the number of vertical candidate positions, m is the number of horizontal candidate positions; x centeris the horizontal coordinate of the center position, y center is the vertical coordinate of the center position, P center is the center position, p ij is the candidate position, Δx is the longitudinal offset, and Δy is the lateral offset.

[0149] S4.1.3. Design a comprehensive cost function to 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 are the weight coefficients of path length, path accessibility, direction consistency, environmental complexity and risk assessment value, respectively, total is the total value of the comprehensive cost function, C length is the path length cost, C reachability is the path accessibility cost, C direction is the direction consistency cost, C env is the environmental complexity cost, C risk is the risk assessment cost, L length is the path length, θ goal is the target direction angle, θ1 is the current direction angle of the first vehicle, E actual is the actual environment complexity value, R actual is the actual risk assessment value.

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

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

[0154] Among them, P prep To prepare the position for optimal docking, P best is the best position, C total () is the total cost function, P i,j for candidate locations.

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

[0156] S4.2.1. Comprehensively consider the vehicle's dynamic performance and ride comfort, and 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-order derivative of the lateral displacement and the second-order derivative of the lateral displacement in the Frenet coordinate system;

[0159] The design objective function is:

[0160]

[0161] k in the formula d , k d1 , k d2 , k d3 is the weight of the optimization variable and the weight k for the docking preparation position prep-d , k prep-d1 , k prep-d2 .

[0162] The solution for the third-order derivative is: but

[0163]

[0164] S4.2.2. Design constraints 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] Among them, bound low is the lower boundary value, bound up is the upper boundary value, d, d′, d″, d″′ are the lateral displacement, the first-order derivative of the lateral displacement, the second-order derivative of the lateral displacement, and the third-order derivative of the lateral displacement in the Frenet coordinate system respectively; Δs is the longitudinal increment, x init are the initial lateral displacement, the first-order derivative of the lateral displacement, and the second-order 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 the current position to the docking preparation position.

[0171] S4.3, speed planning using dynamic programming and secondary optimization:

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

[0173] S4.3.2. Generate multiple candidate speed curves based on speed limit, acceleration limit, and obstacle limit. S4.3.3. Design state transfer equation and cost function:

[0174]

[0175] Among them, C(s i-1 ,t j-1 ,s i ,t j ) indicates that from the state (s i-1 ,t j-1 )Transfer to (s i ,t j ) costs.

[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 obstacle cost, C prep (s i ,t j ) represents the cost of the distance to 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 ) are velocity cost, acceleration cost and jerk cost respectively, C t (s i-1 ,t j-1 ,s i ,t j ) is the time cost.

[0179] S4.3.4. Dynamic programming traversal starts from the end state, follows the state transfer record, and traces back to the starting point to obtain the optimal speed curve.

[0180] S4.3.5. Set optimization variables and optimize the 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 displacement, the first-order derivative of the longitudinal displacement and the second-order derivative of the longitudinal displacement in the Frenet coordinate system.

[0183] The design objective function is:

[0184]

[0185] k in the formula s , k s1 , k s2 , k s3 is the weight of the optimization variable and the weight k for the docking preparation position prep-s , k prep-s1 , k prep-s2 .

[0186] The solution for the third-order derivative is:

[0187] S4.3.6. Design constraints 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 is the lower boundary value, bound up is the upper boundary value, Δt is the time increment, s0, s′0, s″0 are the longitudinal displacement, the first-order derivative of the longitudinal displacement and the second-order derivative of the longitudinal displacement in the Frenet coordinate system, respectively, and x init is 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 following vehicle from the current position to the docking preparation position.

[0194] S4.4. The path curve and the speed 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. At this time, the following conditions are met:

[0195] Heading angle consistency constraint:

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

[0197] Among them, θ1 is the heading angle of the front vehicle, θ2 is the heading angle of the rear vehicle, ∈ θ is the maximum allowable deviation between the heading angles of the two vehicles;

[0198] Speed ​​matching constraints:

[0199] |v1-v2|≤∈ v

[0200] Among them, v1 is the speed of the front car, v2 is the speed of the rear car, ∈ v is the maximum permissible deviation between the speeds of the two vehicles.

[0201] S5, scheduled docking stage: The core task of this stage is to ensure that the longitudinal axes of the two vehicles are parallel and meet the mechanical reconstruction structure adaptive depth matching constraints and mechanical reconstruction structure adaptive automatic centering constraints. In order to ensure the smoothness and continuity of the trajectory and accurately control the starting and ending states of the trajectory, and considering that this stage requires high-precision trajectory tracking and control to achieve high consistency and synchronization of the two vehicles in space, the fifth-order polynomial trajectory generation method is adopted. Figure 3 .

[0202] S5.1. The scheduled docking point shall satisfy the following constraints:

[0203] Mechanical reconstruction structure adaptive depth matching constraints:

[0204] |hH|≤ε h

[0205] Automatic centering constraints of mechanical reconstruction structure:

[0206] |dD|≤ε d

[0207] like Figure 5 The V-shaped butt connector in the mechanical reconstruction structure shown in the figure, where H is the groove depth, D is the radius distance of the bottom of the butt interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance of the two longitudinal axes, ε h is the maximum allowable depth error, ε d is the maximum allowable lateral offset error. The predetermined docking position P can be calculated based on the position of the front vehicle. dock .

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

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

[0210]

[0211] Where x(t0)=x prep , y(t0)=y prep , After T time, it reaches the end point P dock , where T = t1-t0, then the target state is where x(t1) = x dock , y(t1)=y dock ,

[0212] Then the quintic polynomial of x, y with respect to time t is:

[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, b0-b5 are unknown coefficients. If t0=0, then t1=T. The equation group can be obtained from the initial state and the target state:

[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 quintic polynomial.

[0223] S5.3, based on the obtained polynomial coefficients, a trajectory from the docking preparation position to the predetermined docking position is generated. 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, avoid shock or vibration caused by sudden changes, and finally achieve a stable and reliable physical connection through mechanical locking or magnetic adsorption. In order to achieve this goal, a dual PID controller combined with a speed control method is used to achieve a smoother and more stable docking process. Figure 4 .

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

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

[0227]

[0228] where err pos (t) is the longitudinal offset error of the tip, and v1(t) is the actual speed of the leading vehicle at time t.

[0229] Transformed into the first discrete model:

[0230]

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

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

[0233]

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

[0235] Transformed into the second discrete model:

[0236]

[0237] Generate an acceleration curve, and the rear vehicle performs instantaneous acceleration and linear motion relative to the front 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 When |h(t)-h target When (t)| is less than a preset threshold, the mechanical reconstruction structure docking is started.

[0240] S6.4. After docking, 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.

[0241] The present invention uses a combination of quadratic programming and dynamic programming to quickly generate the path of the following vehicle and improve the efficiency of path planning. It uses the state information of the leading vehicle and the structural parameters of the mechanical reconstruction mechanism to accurately calculate the optimal docking preparation position and the predetermined docking position to generate a high-precision trajectory. It uses a dual PID controller to accurately plan and control the speed and acceleration of the vehicle to ensure the stability of the motion docking process and the success rate of docking.

[0242] The method of the present invention increases the number of wheels in contact with the ground by splicing two two-wheeled balancing vehicles into a four-wheeled vehicle, improves the overall stability of the vehicle, reduces the risk of tipping, and is more stable, especially when turning, accelerating or decelerating. The four-wheel design enables the vehicle to better adapt to complex road conditions, including uneven roads, ramps and slippery roads. By optimizing trajectory planning, the vehicle can smoothly pass through various road surfaces and improve applicability. Through precise trajectory planning and control algorithms, it is ensured that the two-wheeled balancing vehicle can achieve smooth acceleration and direction adjustment during the reconstruction of the four-wheeled vehicle, reduce operational risks, and ensure safety. The four-wheeled vehicle design provides a larger load capacity and a more spacious driving space, and can carry more luggage or passengers to meet diverse usage needs. Through the trajectory planning method for reconstructing a four-wheeled vehicle with a two-wheeled balancing vehicle provided by the present invention, the two-wheeled balancing vehicle is spliced ​​into a four-wheeled vehicle, which not only inherits the flexibility of the two-wheeled vehicle, but also combines the stability and load-bearing capacity of the four-wheeled vehicle, providing a new direction for the development of intelligent transportation tools.

[0243] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle, characterized in that: include: Establish wireless communication between the two balancing vehicles, calculate the optimal docking preparation position according to the state information of the front vehicle, use the quadratic programming method to generate the path of the rear vehicle from the current position to the optimal docking preparation position, and plan the speed of the rear vehicle based on dynamic programming and quadratic optimization; Determining that the longitudinal axes of the leading vehicle and the trailing vehicle are parallel, calculating a predetermined docking position according to the state information of the leading vehicle 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; Sensors are used to monitor the position and speed information of the two vehicles in real time. A dual PID controller combined with a speed control method is used to drive the docking connector of the rear vehicle along a straight connecting line toward the docking interface of the front vehicle along the generated trajectory until the docking connector contacts the docking interface to complete the docking.

2. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 1, characterized in that: Calculate the best docking preparation position based on the status information of the preceding vehicle, including: Determine the safe distance based on the speed of the vehicle ahead; Based on the safety distance, generating a plurality of candidate positions by lateral and longitudinal offsets; The comprehensive cost function of each candidate position is calculated, and the candidate position with the smallest comprehensive cost is selected as the optimal docking preparation position.

3. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 2, characterized in that: The method for determining the safety distance is: D safe =max(k·v1,D min ) Where D safe is the safety distance after comprehensive consideration of speed and minimum safety distance, k is the safety factor, v1 is the speed of the first car, and D min is the minimum safe distance; The method for generating several candidate locations is: Where (x1, y1, θ1) is the current position and direction angle of the first vehicle, i and j represent the index of the longitudinal offset and the index of the lateral offset respectively, and the range is arrive arrive n is the number of vertical candidate positions, m is the number of horizontal candidate positions; x center is the horizontal coordinate of the center position, y center is the vertical coordinate of the center position, P center is the center position, p ij is the candidate position, Δx is the longitudinal offset, and Δy is the lateral offset.

4. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 2, characterized in that: The method for calculating the optimal docking preparation position is: P.S prep JP best 1argmin i,j C total (P i,j ) Where P prep To prepare the best docking position, P best is the best position, C total ( ) is the total cost function, P i,j for candidate locations.

5. The trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 1, characterized in that: After the speed of the following vehicle is planned, the heading angle and speed of the leading vehicle and the following vehicle should satisfy: |θ1-θ2|≤∈ θ <h2 style=";text-align:left;direction:ltr">|v1-v2|≤∈<h2 style=";text-align:left;direction:ltr"> v Among them, θ1 is the heading angle of the front vehicle, θ2 is the heading angle of the rear vehicle, ∈ θ is the maximum allowable deviation between the heading angles of the two vehicles; v1 is the speed of the front vehicle, v2 is the speed of the rear vehicle, ∈ v is the maximum permissible deviation between the speeds of the two vehicles.

6. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 1, characterized in that: The constraints defined for the predetermined docking position are: Mechanical reconstruction structure adaptive depth matching constraints: |h-H|≤ε h Automatic centering constraints of mechanical reconstruction structure: |d-D|≤ε d Position constraint: x(t0)=x prep , y(t0)=y prep , x(t1)=x dock =x f +d,y(t1)=y dock =y f -h; Speed ​​Constraints: Acceleration constraints: Among them, t0 is the start time of the algorithm, and t1 is the end time of the scheduled docking trajectory planning step. (x prep ,y prep ) is the initial state coordinate, i.e., the docking preparation position, (x dock ,y dock ) is the final state coordinate, i.e., the predetermined docking position, (x f ,y f ) is the tip position of the V-shaped docking connector in the mechanical reconstruction structure of the front vehicle, H is the groove depth, D is the radius distance of the bottom of the docking interface, h is the longitudinal offset distance of the tip, d is the lateral offset distance of the two longitudinal axes, ε h is the maximum allowable depth error, ε d is the maximum permissible lateral offset error, v prep and a prep are the speed and acceleration of the following vehicle at time t0, i.e. the beginning of planning, and v dock and a dock are the expected speed and expected acceleration at the end of planning, respectively.

7. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 6, characterized in that: Generating a trajectory from the optimal docking preparation position to the predetermined docking position based on a trajectory planning method includes: Construct a quintic polynomial equation and solve it to obtain the coefficients of the quintic polynomial; generating the trajectory from the optimal docking preparation position to the predetermined docking position according to the coefficients of the quintic polynomial; Wherein, the fifth-order polynomial equation is: x(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 <h2 style=";text-align:left;direction:ltr">y(t) = b0 + b1t + b2t<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +b3t<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +b4t<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> +b5t<h2 style=";text-align:left;direction:ltr"> 5 Where x(t) is the horizontal coordinate of the trajectory, y(t) is the vertical coordinate of the trajectory, a0, a1, a2, a3, a4, and a5 are the coefficients of the horizontal quintic polynomial, and t, t 2 ,t 3 ,t 4 ,t 5 are the different powers of time t, b0, b1, b2, b3, b4, and b5 are the coefficients of the longitudinal quintic polynomial.

8. The trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 1, characterized in that: Driving the rear vehicle docking connector to move along a straight connecting line toward the front vehicle docking interface includes: Design a position error PID controller, calculate a target speed, and convert 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; According to the acceleration curve generated by the speed error PID controller, the rear vehicle docking connector is driven to perform instantaneous acceleration linear motion along the acceleration curve toward the front vehicle docking interface.

9. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 8, characterized in that: The position error PID controller is: err pos (t)=h(t)-t target (t) In the formula, err pos (t) is the longitudinal offset error of the tip, v1(t) is the actual speed of the preceding vehicle at time t, h(t) is the longitudinal offset distance of the tip at time t, h target (t) is the target longitudinal offset distance, v target (t) is the target speed, K p1 is the proportional coefficient of the position error PID controller, K i1 is the integral coefficient of the position error PID controller, K d1 is the differential coefficient of the position error PID controller, derr pos (t) is the rate of change of position error; The first discrete model is: Among them, v target (k) is the target speed at discrete time k, err pos (k) is the position error at discrete time k, err pos (i) is the position error at discrete time i, Δt is the discrete time step, err pos (k-1) is the position error at discrete time k-1, v1(k) is the actual speed of the preceding vehicle at discrete time k; The speed error PID controller is: err vel (t)=v target (t)-v2(t) In the formula, err vel (t) is the error between the target speed and the actual speed of the following vehicle, v2(t) is the actual speed of the following vehicle at time t, and a vel (t) is the target acceleration, K p2 is the proportional coefficient of the speed error PID controller, K i2 is the integral coefficient of the speed error PID controller, K d2 is the differential coefficient of the speed error PID controller, derr vel (t) is the rate of change of velocity error; The second discrete model is: Among them, a vel (k) is the acceleration at discrete time k, err vel (k) is the velocity error at discrete time k, err vel (i) is the velocity error at discrete time i, err vel (k-1) is the velocity error at discrete time k-1.

10. A trajectory planning method for reconstructing a dual-balance vehicle into a four-wheel vehicle according to claim 9, characterized in that: Use sensors 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 | is less than the preset threshold, the mechanical reconstruction structure is started to complete the docking, where h(t) is the longitudinal offset distance of the tip at time t, h target (t) is the target longitudinal offset distance.

Images