An automatic driving vehicle platoon method based on a point cloud map

Abstract

The application relates to a kind of automatic driving vehicle platoon methods based on point cloud map, belong to the field of automatic driving;Including the following steps: for the tested automatic driving vehicle, fusion GNSS, IMU and LIDAR measurement data real-time construction scene point cloud map;Then, obtain the motion state information of each vehicle in the automatic driving vehicle platoon from the point cloud map, establish the planar platoon kinematic model including the pilot vehicle and the following vehicle;According to the target point following strategy, design the lateral and longitudinal controller of the following vehicle, establish the dynamics model of vehicle longitudinal platoon on the road with slope change under discrete time, design MPC for the pilot vehicle, design DMPC for the following vehicle, simulate the platoon driving results of automatic driving vehicle on uphill and downhill road section.The application considers the ground slope information provided by point cloud map to improve the modeling accuracy, help the vehicle to accelerate or decelerate in advance before entering uphill and downhill road section, thereby reducing the speed fluctuation of the platoon in uphill and downhill, and promoting the platoon to converge to the equilibrium state at a faster speed.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving, and specifically relates to an autonomous driving vehicle platooning method based on point cloud maps. Background Art

[0002] The rapid development of disciplines such as computing, automation, and communications has laid the foundation for the creation of intelligent, networked urban transportation systems. Intelligent Transportation Systems (ITS) effectively integrate advanced information and communications technologies into transportation management systems. By coordinating the various traffic participants, they aim to improve transportation efficiency, alleviate traffic congestion, reduce traffic accidents, lower energy consumption, and mitigate environmental pollution.

[0003] The main components of ITS are intelligent connected vehicles (ICVs) and intelligent road infrastructure. ICVs typically consist of autonomous vehicles and communication infrastructure, combining single-vehicle intelligence with multi-vehicle collaboration. Information collected by sensors installed on autonomous vehicles and infrastructure is exchanged through vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communications, helping vehicles understand current traffic conditions at a macro level.

[0004] The main method to solve the problem of traffic congestion in urban road environments is to control the formation of vehicles traveling in lanes. Vehicle cooperative formation control can enable the fleet to maintain a small longitudinal spacing, form a stable queue, and greatly improve road traffic capacity.

[0005] Point cloud maps can provide vehicles with richer environmental information and more accurate positioning information. Introducing point cloud maps into vehicle formation control can improve the formation control effect. Summary of the Invention

[0006] The present invention aims to design a method for autonomous driving vehicle platooning based on point cloud maps, which includes fusing multi-sensor data to establish a scene map and realizing platoon control based on point cloud map information, thereby achieving the purpose of high-precision positioning and platoon driving in urban road environments.

[0007] The specific steps of the autonomous driving vehicle platooning method based on point cloud maps are as follows:

[0008] Step 1: For the autonomous driving vehicle under test, the measurement data from the three sensors, LIDAR, IMU, and GNSS, are integrated to obtain a point cloud map of the scene;

[0009] The fusion process is:

[0010] First, the IMU sensor's measurement data is optimized using the vehicle's front-end odometer.

[0011] The optimization result is x:

[0012]

[0013] p wb is the IMU position vector in the world coordinate system, 3×1 dimension; v wb is the IMU velocity vector in the world coordinate system, 3×1 dimension; q wb is the IMU attitude quaternion in the world coordinate system, a 4×1 dimensional vector; b a is the zero bias of the accelerometer in the IMU, a 3×1 dimensional vector; b w is the zero bias of the gyroscope in the IMU, a 3×1 dimensional vector.

[0014] Then, use the backend to align the pose matrix Optimize.

[0015] Then, the point cloud collected by the LIDAR sensor at each moment is transformed into the edge feature point cloud set M by the optimized pose matrix. e and surface feature point cloud set M s ;

[0016]

[0017]

[0018] Represents the point cloud collected by the LIDAR sensor in the i-1th frame, is to use the optimized pose matrix to Transform the edge feature point cloud to the world coordinates, It will Transform the surface feature point cloud into world coordinates;

[0019] Finally, the set M e and set M s The feature point clouds in the scene are stitched together to form a point cloud map M of the scene:

[0020] M={M e ,M s}

[0021] Step 2: Obtain the motion status information of each vehicle in the autonomous driving fleet from the point cloud map and establish a planar formation kinematic model including the lead vehicle and the following vehicles;

[0022] The status information of the leading vehicle is transmitted to the following vehicle through inter-vehicle communication. The control input of the following vehicle is the vehicle speed and the front wheel angle.

[0023] The lead car follows the manually recorded path at a fixed speed. The longitudinal distance between the follower car and the lead car at the starting moment is greater than the expected distance, and the initial speed is 0.

[0024] The specific process is:

[0025] Step 201: For the current following vehicle A, a sliding window is used to save the discrete point sequence of the driving path of the pilot vehicle in the following vehicle A. When receiving the latest pilot car data When , the data is pressed into the sliding window in chronological order, and the discrete point sequence in the window is regarded as the tracking path of the lateral tracking controller of the following vehicle A;

[0026] Step 202: Calculate the distance between the following vehicle A and each discrete point in the sliding window one by one, and find the discrete point with the smallest distance to the current position of the following vehicle A. Point As a benchmark, the point The previous point is removed from the sliding window, and the cumulative calculation is performed from the discrete points To discrete points The distance between the following vehicle A and the leading vehicle is used as the expected longitudinal spacing between the following vehicle A and the leading vehicle.

[0027] Step 203: Perform longitudinal PID control based on the deviation between the actual longitudinal spacing of the vehicles and the desired longitudinal spacing to obtain the desired speed of the following vehicle A.

[0028] Step 204: Similarly, using discrete points As a benchmark, the point The previous point is removed from the sliding window, and the discrete point sequence Find the target point x of the lateral tracking controller within G , and calculate the front wheel turning angle expected by the following vehicle A;

[0029] Step 205: Integrate the desired vehicle speed and the desired front wheel steering angle of the following vehicle A and apply the resultant integration to the following vehicle A.

[0030] Step 206: Repeat all the above steps to complete the plane formation task in a loop.

[0031] Step 3: Establish a dynamic model of the vehicle longitudinal formation in discrete time and rewrite it into the form of state space equations;

[0032] Specifically:

[0033] The longitudinal dynamic model of vehicle number i is:

[0034] p i (t+1)=p i (t)+v i (t)Δt

[0035]

[0036]

[0037] p i (t) represents the displacement of vehicle number i at time t, v i (t) represents the speed of vehicle number i at time t, Δt represents the discrete time interval; m i represents the mass of vehicle number i, η i represents the transmission efficiency of vehicle number i, R i represents the rolling radius of the tire of vehicle number i, T i (t) represents the actual driving / braking torque of vehicle numbered i at time t, τ i Indicates the number i The inertia lag time of the vehicle's transmission system, u i (t) represents the expected driving / braking torque of vehicle numbered i at time t, F i (v i (t)) represents the running resistance of vehicle numbered i;

[0038] Let the state variable x i (t)=[p i (t),v i (t),T i (t)] T , taking displacement and velocity as output variables y i (t)=[p i (t), v i (t)] T , the longitudinal dynamic model of vehicle number i in discrete time is written in the form of state space equation:

[0039] x i (t+1)=φ i (x i (t))+ψ i u i (t)

[0040] y i (t) = γx i (t)

[0041]

[0042]

[0043]

[0044] Step 4: Design a model predictive controller (MPC) for the pilot vehicle and a distributed model predictive controller (DMPC) for the follower vehicle. Combined with state-space equations, simulate the platooning of autonomous vehicles on downhill sections of urban roads.

[0045] Specifically:

[0046] The MPC controller is designed for the pilot car. The state space equation is used to construct the cost function of the pilot car based on the difference between the output variable in the prediction time domain and the expected output variable:

[0047]

[0048] N p is the total time; Q0 and R0 are the weight matrices of the output and input of the pilot vehicle respectively. represents the predicted input of the pilot car at time k, represents the predicted output of the pilot car at time k, y des,0 (k|t) represents the expected output of the pilot vehicle at time k, represents the predicted speed of the pilot vehicle at time k, Indicates offset pilot car The driving torque of the uniform speed resistance.

[0049] The following car uses the DMPC controller. The objective function includes not only the state information of each car itself, but also the state information of each vehicle in the neighborhood. The cost function of the following car numbered i at time k is:

[0050]

[0051] Q i ,R i ,F i ,G i are all positive semidefinite weight matrices, is the predicted output of the following vehicle numbered i at time k; y des,i (k|t) is the expected equilibrium state output; is the expected input torque of the following vehicle numbered i at time k, is the hypothetical output of the following vehicle numbered i in the prediction time domain at time t; To offset the following car number i The resistance torque generated by the resistance of uniform speed movement, is the neighborhood set of the following car numbered i The hypothetical output of is the expected value of the difference between the predicted output and the assumed output.

[0052] Finally, through Lyapunov stability analysis, the weight matrix F is set i and G i , making the autonomous vehicle platoon asymptotically stable.

[0053] The advantages of the present invention are:

[0054] (1) A method for autonomous vehicle platooning based on point cloud maps. The positioning algorithm results are derived from the fusion of measurement data from three sensors: LIDAR, IMU, and GNSS. When GNSS positioning jumps due to signal occlusion, LIDAR-based feature matching can still provide reliable result output, improving the positioning accuracy of fixed scenes. In addition, the algorithm can run in real time on the inexpensive Jeston TX2.

[0055] (2) A method for autonomous vehicle platooning based on point cloud maps. Without relying on GNSS measurements, the prior point cloud map can still provide reliable positioning information for the vehicle platoon. By designing a simple and effective planar formation control strategy, the lateral following deviation and longitudinal vehicle distance deviation of the planar formation are both within the range of ±0.3m.

[0056] (3) A method for autonomous driving vehicle platooning based on point cloud maps. In the model predictive control of longitudinal platooning, considering the ground slope information provided by the point cloud map will improve the modeling accuracy and help vehicles accelerate and decelerate in advance before entering uphill and downhill sections, thereby reducing the speed fluctuation of the platoon during the uphill and downhill process, and prompting the platoon to converge to a balanced state at a faster speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 This is a flow chart of a method for autonomous driving vehicle platooning based on point cloud maps according to the present invention;

[0058] Figure 2 Schematic diagram of the multi-sensor fusion mapping front-end odometer of the present invention.

[0059] Figure 3 This is a schematic diagram of the multi-sensor fusion mapping backend optimization of the present invention.

[0060] Figure 4 The point cloud map of the test scene established for this invention.

[0061] Figure 5 This is a flow chart for executing the plane formation controller of the present invention.

[0062] Figure 6 Comparison of the driving paths of the lead vehicle and the following vehicle in the plane formation experiment of the present invention.

[0063] Figure 7 Schematic diagram of the communication topology of the queue in the longitudinal formation task of the present invention.

[0064] Figure 8 This is the effect of the slope information on the longitudinal speed of the pilot vehicle according to the present invention.

[0065] Figure 9 This is the distributed model predictive control idea of ​​the present invention. DETAILED DESCRIPTION

[0066] The present invention will be further described in detail below with reference to the accompanying drawings and examples;

[0067] The present invention discloses a method for controlling a platoon of autonomous vehicles in a closed environment. By fusing lidar and inertial navigation data, the method estimates the motion state of the autonomous vehicles and optimizes the distance between the platoons. To achieve the above objectives, the method specifically employs the following methods:

[0068] (1) Design a real-time mapping algorithm that integrates GNSS, IMU, and LIDAR measurement data, and run the algorithm on the Jeston TX2 mounted on the experimental vehicle to verify the mapping and positioning accuracy. The point cloud map of the experimental scene is obtained by splicing the feature point cloud with the estimated pose.

[0069] (2) Design the lateral and longitudinal controllers of the following vehicle in the plane formation task based on the target point following strategy, and conduct a real vehicle test of the plane formation based on the positioning information provided by the point cloud map.

[0070] (3) A dynamic model of the longitudinal formation of vehicles on a road with varying slopes was established. A model predictive controller was designed for the lead vehicle and a distributed model predictive controller was designed for the following vehicle. The changes in the distance between adjacent vehicles in the formation when going up and downhill were simulated through Matlab simulation.

[0071] The autonomous driving vehicle platooning method based on point cloud map, such as Figure 1 The specific steps are as follows:

[0072] Step 1: For the autonomous driving vehicle under test, the measurement data from the three sensors, LIDAR, IMU, and GNSS, are integrated to obtain a point cloud map of the scene;

[0073] The fusion process is:

[0074] First, the state variables of the IMU sensor’s measurement data between two consecutive LIDAR frames are optimized using the vehicle’s front-end odometer.

[0075] The optimization result is x:

[0076]

[0077] pwb is the IMU position vector in the world coordinate system, 3×1 dimension; v wb is the IMU velocity vector in the world coordinate system, 3×1 dimension; q wb is the IMU attitude quaternion in the world coordinate system, a 4×1 dimensional vector; b a is the zero bias of the accelerometer in the IMU, a 3×1 dimensional vector; b w is the zero bias of the gyroscope in the IMU, a 3×1 dimensional vector.

[0078] Then, the LIDAR odometer factor, GNSS measurement factor and loop detection factor of the frame image matching are optimized in the backend, and the optimized variables are the pose corresponding to the key frame.

[0079] The data contained in the key frame includes the measurement timestamp, the point cloud collected by the LIDAR sensor at that moment, and the pose The point cloud is measured by the LIDAR sensor, and the pose is solved according to the optimization process.

[0080] Then, the point cloud collected by the LIDAR sensor at each moment is transformed into the edge feature point cloud set M by the optimized pose matrix. e and surface feature point cloud set M s ;

[0081]

[0082]

[0083] Represents the point cloud collected by the LIDAR sensor in the i-1th frame, is to use the optimized pose matrix to Transform the edge feature point cloud to the world coordinates, It will Transform the surface feature point cloud into world coordinates;

[0084] The feature point cloud at the current moment The result of registration with feature points in the nearby local map M is used as the measurement of the LIDAR odometry.

[0085] Finally, the set M e and set M s The feature point clouds in the scene are stitched together to form a point cloud map M of the scene:

[0086] M={M e ,M s}

[0087] Step 2: Obtain the motion status information of each vehicle in the autonomous driving fleet from the point cloud map, establish a planar formation kinematic model including the leader vehicle and follower vehicles, and complete the planar formation task.

[0088] The status information of the leading vehicle is transmitted to the following vehicle through inter-vehicle communication. The control input of the following vehicle is the vehicle speed and the front wheel angle.

[0089] The lead car follows the manually recorded path at a fixed speed. The longitudinal distance between the follower car and the lead car at the starting moment is greater than the expected distance, and the initial speed is 0.

[0090] The specific process is:

[0091] Step 201: For the current following vehicle A, a sliding window data structure is used in the following vehicle A to store the discrete point sequence of the driving path of the pilot vehicle. When receiving the latest pilot car data When , the data is pressed into the sliding window in chronological order, and the discrete point sequence in the window is regarded as the tracking path of the lateral tracking controller of the following vehicle A;

[0092] By using a sliding window to save the data sent by the pilot car, this method can save the space required to store the involved data, and the amount of data required to be sent by the pilot car is small. In engineering practice, it can effectively reduce the communication burden due to the front and rear vehicles.

[0093] Step 202: Calculate the distance between the following vehicle A and each discrete point in the sliding window one by one, and find the discrete point with the smallest distance to the current position of the following vehicle A. Point As a benchmark, the point The previous point is removed from the sliding window, and the cumulative calculation is performed from the discrete points To discrete points The distance between two points in the sequence is used as the expected longitudinal spacing from the follower vehicle A to the leader vehicle;

[0094] Step 203: Perform longitudinal PID control based on the deviation between the actual longitudinal spacing of the vehicles and the expected longitudinal spacing, obtain the expected speed of the following vehicle A, and calculate the forward sight distance l at the current speed. d ;

[0095] Step 204: Similarly, using discrete points As a benchmark, the point The previous point is removed from the sliding window, and the discrete point sequence Find the target point x of the lateral tracking controller within G , and calculate the front wheel turning angle expected by the following vehicle A;

[0096] Step 205: Integrate the desired vehicle speed and the desired front wheel steering angle of the following vehicle A and apply the resultant integration to the following vehicle A.

[0097] Step 206: Repeat all the above steps to complete the plane formation task in a loop.

[0098] Step 3: Establish the dynamic models of two types of vehicle longitudinal formations with and without considering slope resistance in discrete time and rewrite them into the form of state space equations;

[0099] Specifically:

[0100] The longitudinal dynamic model of vehicle number i in the platoon at discrete time is:

[0101] p i (t+1)=p i (t)+v i (t)Δt

[0102]

[0103]

[0104] p i (t) represents the displacement of vehicle number i at time t, v i (t) indicates the number i The speed of the vehicle at time t, Δt represents the discrete time interval; m i represents the mass of vehicle number i, η i represents the transmission efficiency of vehicle number i, R i represents the rolling radius of the tire of vehicle number i, T i (t) represents the actual driving / braking torque of vehicle numbered i at time t, τ i Indicates the inertia hysteresis time of the transmission system of the vehicle numbered i, u i (t) represents the expected driving / braking torque of vehicle numbered i at time t, F i (v i (t)) represents the running resistance of vehicle numbered i;

[0105] Let the state variable x i (t)=[p i (t),v i (t),T i (t)] T , taking displacement and velocity as output variables y i (t)=[p i (t), v i (t)] T, the longitudinal dynamic model of vehicle number i in discrete time is written in the form of state space equation:

[0106] x i (t+1)=φi i (x i (t))+ψ i u i (t)

[0107] y i (t) = γx i (t)

[0108]

[0109]

[0110]

[0111] Step 4: Design a model predictive controller (MPC) for the pilot vehicle and a distributed model predictive controller (DMPC) for the follower vehicle. Combined with state-space equations, simulate the platooning of autonomous vehicles on downhill sections of urban roads.

[0112] Specifically:

[0113] The MPC controller is designed for the pilot car. The state space equation is used to construct the cost function of the pilot car based on the difference between the output variable in the prediction time domain and the expected output variable:

[0114]

[0115] N p is the total time; Q0 and R0 are the weight matrices of the output and input of the pilot vehicle respectively. represents the predicted input of the pilot car at time k, represents the predicted output of the pilot car at time k, y des,0 (k|t) represents the expected output of the pilot vehicle at time k, represents the predicted speed of the pilot vehicle at time k, Indicates offset pilot car The driving torque of the uniform speed resistance.

[0116] The following vehicle uses a DMPC controller. Each vehicle in the platoon is considered as a separate subsystem, and a separate model predictive controller is designed for each subsystem. The objective function includes not only the state information of each vehicle itself, but also the state information of all vehicles in the neighborhood.

[0117] It consists of the following four parts:

[0118] (1) Measure the deviation between the predicted output of node i and the equilibrium state output

[0119] The following vehicle numbered i is regarded as node i. When all vehicles in the queue keep moving at the desired speed v0, the queue is said to have reached the desired equilibrium state. The equilibrium state of node i corresponds to its desired state x des,i (k|t), which can be output by predicting the prediction in the time domain and the expected equilibrium state output y des,i The (k|t) deviation term is recorded as

[0120]

[0121] (2) Measure the deviation between the predicted input of node i and the equilibrium state input

[0122] When the queue reaches equilibrium, the expected input torque of node i is Should be equal to the resistance torque generated by the driving resistance In the non-equilibrium state, the deviation between the predicted input and the equilibrium input is

[0123]

[0124] (3) Measure the deviation between the predicted output of node i and the assumed output of node i

[0125] Node i predicts the hypothetical output in the time domain at time t The optimized output in the time domain is predicted by time t-1 Transfer and predict the hypothetical output in the time domain and the hypothetical input within the prediction time domain The relationship is:

[0126]

[0127]

[0128] Assume input The first N p -1 element, which is the optimal control quantity sequence solved at time t Middle and back N p -1 element is shifted one unit to the left along the time axis, Nth p Elements Set as terminal constraint:

[0129]

[0130] The predicted output from node i in the prediction time domain Should be close to the assumed output To ensure that the system gradually stabilizes, remember the deviation term between the predicted output and the assumed output

[0131]

[0132] (4) Measure the predicted output of node i and the neighborhood set Bias term of the internal hypothesis output

[0133] Node i receives node The state information of node i is the predicted output and its neighborhood set The hypothetical output of It should also be as close as possible to:

[0134]

[0135] Among the above four items, Q i ,R i ,F i ,G i All are semi-positive definite weight matrices. Combining the above four items, the cost function of node i at time k is:

[0136]

[0137] is the predicted output of the following vehicle numbered i at time k; y des,i (k|t) is the expected equilibrium state output; is the expected input torque of the following vehicle numbered i at time k, is the hypothetical output of the following vehicle numbered i in the prediction time domain at time t; To offset the following car number i The resistance torque generated by the resistance of uniform speed movement, is the neighborhood set of the following car numbered i The hypothetical output of is the expected value of the difference between the predicted output and the assumed output.

[0138] Finally, through Lyapunov stability analysis, the weight matrix F is set i and G i , making the autonomous vehicle platoon asymptotically stable.

[0139] Example:

[0140] The platform of this invention is built based on ROS melodic under Ubuntu 18.04. In the experiment, the written function is encapsulated into ROS nodes, and the data transmission between nodes is realized through the message communication mechanism. The main nodes in this invention are the vehicle positioning node and the pilot-follower controller node;

[0141] The specific algorithm is:

[0142] In the first step, a multi-sensor fusion mapping algorithm based on a factor graph optimization model was designed, including a front-end odometry and a back-end optimization module, and the positioning and mapping effects of the algorithm were tested on campus.

[0143] 0) LIDAR point cloud preprocessing

[0144] The method of extracting feature points based on curvature proposed by Zhang J in reference 1 is used to extract edge feature points p from the original point cloud. e and surface feature points.

[0145] For a 16-line laser radar, the curvature of all points on each line is calculated one by one. The nth point on the mth line in the kth frame point cloud is recorded as Point The curvature at is defined as:

[0146]

[0147] Where, Yes The set of neighborhood points in the horizontal direction, set point Take 5 points clockwise and counterclockwise along the mth line express The number of internal points. Calculated will be smaller; and for discontinuous features, In the implementation, in order to avoid the edge features from being too concentrated, when the point When it is determined to be an edge feature point, the neighborhood The points inside are no longer considered as edge feature points, and the same is true for face features. point The set of edge feature points P e , The points constitute the surface feature point set P s .

[0148] 1) Front-end odometer

[0149] The role of the front-end odometry is to estimate the relative motion of the vehicle between consecutive LIDAR frames at high frequency and low precision, provide initial optimization values ​​for the low-frequency and high-precision back-end optimization, and accept the results of the back-end optimization to interpolate its own low-precision results and correct the accumulated deviation of the odometry.

[0150] The front-end mileage calculation method designed in this embodiment is as follows: Figure 2 As shown in Figure 2, the factor graph contains the LIDAR odometry factor and IMU pre-integration factor between consecutive frames. The optimized state variable of the graph model is x:

[0151]

[0152] The LIDAR odometry factor between consecutive frames is calculated by aligning the edge feature points and surface feature points between two consecutive frames, calculating the transformation matrix between consecutive frames, and implementing the feature extraction and matching calculation process in reference [2]. Since the IMU data frequency is higher than the LIDAR data frequency, there are multiple frames of IMU data between two LIDAR frames. In order to use the IMU measurement data to constrain the graph model, the IMU measurement data needs to be integrated first to obtain the IMU pre-integration factor. The detailed principle and derivation of the pre-integration factor can be found in reference [3]. With the introduction of each frame of LIDAR data, the optimization variables of the front-end odometry will increase over time. In order to reduce the time complexity, the front-end odometry performs nonlinear optimization based on the sliding window concept.

[0153] 2) Backend optimization

[0154] The front-end odometer can obtain the transformation matrix between consecutive frames, and then recursively obtain the optimization variables corresponding to each frame. However, the use of the sliding window algorithm in the optimization makes the iterative solution of the optimization variables only within the window range of a fixed length, and the optimization variables outside the window remain the original value. In order to eliminate the cumulative error of the front-end odometer, the absolute position measured by GNSS and closed-loop detection are used in the back-end optimization for correction. [4] At the same time, a key frame mechanism is used: when the distance between the current frame and the next latest frame in the factor graph is greater than the set distance threshold of 10m, the current frame is added to the factor graph for optimization, and all radar frames between the next latest frame and the current frame are discarded. Figure 3 The backend optimization model shown includes the LIDAR odometer factor, GNSS measurement factor and loop detection factor for frame matching. The optimization variable is the pose corresponding to the key frame.

[0155] exist Figure 2 In the calculation of LIDAR odometry factor, the feature points of the k+1th frame are matched with the feature points of the kth frame. Figure 3 In the middle, the feature points of the current frame are The result of the registration with the feature points of the nearby local map M is used as the measurement of the LIDAR odometry. M is composed of the edge feature map M e and surface feature map M s Composition, specifically expressed as:

[0156]

[0157] The first step of mapping results is as follows Figure 4 As shown in the figure, it can be seen that the features of the established point cloud map are clear and closed, without overlap or obvious distortion. In this step, the present invention proposes a novel multi-sensor data fusion solution. Experimental results show that this method can be applied in closed environments such as campuses.

[0158] The second step is to establish a kinematic model of the plane formation, and design the lateral and longitudinal control strategies and control targets based on the actual controllable quantities of the experimental vehicle. The pure tracking control is used in the lateral direction. [5] PID control is used in the longitudinal direction, and the self-motion state information is obtained by combining the point cloud map to complete the plane formation task. The execution flow chart of the following vehicle controller is as follows: Figure 5 shown.

[0159] (1) Using a sliding window data structure in the following car to save the discrete point sequence of the driving path of the pilot car When receiving the latest pilot car data When , the time sequence is pressed into the sliding window, so that the discrete point sequence in the window can be used as the tracking path of the lateral tracking controller of the following vehicle;

[0160] (2) Calculate the distance between the following vehicle and the path points in the window one by one, and find the path points from the current position of the following vehicle Point As a benchmark, remove the previous points from the window, reduce the window length, and accumulate the calculation from arrive The longitudinal distance between the following vehicle and the leading vehicle is obtained by the distance between two points in the discrete point sequence of ;

[0161] (3) Based on the deviation between the actual longitudinal spacing and the expected longitudinal spacing, longitudinal PID control is performed to obtain the expected speed of the following vehicle and calculate the forward sight distance l at the current speed. d ;

[0162] (4) As a benchmark, Find the target point x of the lateral tracking controller within the sequence G , and calculates the desired front wheel angle, which is then integrated with the desired vehicle speed information and applied to the following vehicle. Finally, return to step 1) to complete the planar formation task in a loop.

[0163] In the plane formation experiment, the lead vehicle follows the manually recorded path at a fixed speed. At the starting moment, the longitudinal distance between the follower vehicle and the lead vehicle is greater than the expected distance, and the initial speed is 0.

[0164] Figure 6 The following diagram shows a comparison of the driving paths of the lead and follower vehicles during a planar formation, demonstrating that the follower vehicle is able to follow the lead vehicle's path on both straight and curved sections. This section uses a sliding window data structure to store the lead vehicle's path, reducing the communication burden between the two vehicles and simplifying the computational complexity of the follower vehicle's control variables. This allows the follower vehicle to follow the lead vehicle's path and maintain longitudinal distance in a simple and efficient manner.

[0165] In the third step, starting from the vehicle longitudinal dynamics model, two platoon models were established, one considering slope resistance and the other not. A model predictive control (MPC) was designed for the lead vehicle in the platoon, and a distributed model predictive control (DMPC) was designed for the following vehicle. The driving results of the platoon on uphill and downhill sections were simulated using Matlab.

[0166] 1) Longitudinal platooning vehicle dynamics model

[0167] According to the literature [5], the third-order nonlinear longitudinal dynamics model of a car is widely used in longitudinal platoon control. The third-order nonlinear longitudinal dynamics model assumes that: (1) the left and right sides of the car body are symmetrical, and the vehicle is simplified as a rigid body; (2) the platoon is driving on a dry road, and the longitudinal side slip of the tires can be ignored; (3) the car's transmission system is a first-order inertial delay link. Based on the assumptions and Newton's laws of dynamics, the longitudinal dynamics model of the car numbered i in the platoon in discrete time is:

[0168]

[0169] F i (v i (t)) represents the driving resistance of the car, which is composed of rolling resistance, air resistance and slope resistance:

[0170] F i (v i (t))=m i gfcosα+C A,i v i (t) 2 +m i gsinα (4)

[0171] f represents the road resistance coefficient, C A,irepresents the air resistance coefficient, α represents the road slope angle, and g represents the gravity constant.

[0172] Let the state variable x i (t)=[p i (t),v i (t),T i (t)] T , taking displacement and velocity as output variables y i (t)=[p i (t), v i (t)] T , we get the form of the state space equation:

[0173]

[0174] 2) Queue communication topology

[0175] The communication in the present invention is realized by V2V. The vehicles in the queue transmit their motion status through the wireless network. The communication topology adopts the pilot-follower method. Figure 7 As shown in Figure 1, each following car in the queue can receive information from the lead car and calculate the expected state of the following car based on the state of the lead car. The set of directed edges in Represents the message sending and receiving path of the following vehicle in the queue, the node set V N Represents the following car in the queue. Define the adjacency matrix Used to describe the communication relationship between following vehicles. in:

[0176]

[0177] when a ij = 1, there is a directed edge from node j to node i. Define the input neighborhood set of the following vehicle of node i Node i only receives The nodes in receive information, that is:

[0178]

[0179] Similarly, define the output neighborhood set of node i Node i The nodes in send information, including:

[0180]

[0181] 3) Queue control target

[0182] The present invention focuses on the ability to maintain the platoon formation based on map information: as time goes by, the longitudinal spacing between adjacent vehicles gradually converges to a desired value, and the relative longitudinal speed approaches 0.

[0183]

[0184] In formula (9), d i-1,i represents the desired longitudinal spacing between adjacent vehicles, and v0(t) represents the desired platoon travel speed.

[0185] 4) Pilot vehicle controller design

[0186] If the platoon is traveling on a road with a varying slope, the speed of the lead vehicle changes, and the expected speed of the following vehicles also changes. In this case, when there are multiple following vehicles, the following vehicles frequently accelerate and decelerate, which is not conducive to the stability of the platoon. Here, an MPC is designed for the lead vehicle. The prediction model uses Equation (5). The cost function of the lead vehicle is constructed based on the difference between the output variables in the prediction time domain and the expected output variables solved by the model prediction:

[0187]

[0188] The designed controller is simulated in MATLAB. The simulation conditions are that at the initial time t=0s, the pilot car travels at a constant speed of 10m / s on a flat road. After 10s, it reaches an uphill section with a slope angle of 10 degrees. Figure 8 The speed control performance of the pilot car during an uphill climb was compared between models that incorporate and exclude slope information. Around t = 10 seconds, the longitudinal speed fluctuated due to the presence of an uphill section in both models. The difference was that the model with known slope predicted the change in the road ahead by predicting displacement within the time domain, increasing the driving torque in advance to overcome the slope resistance. As a result, the pilot car's speed quickly returned to 10 m / s after ascending the slope.

[0189] 5) Follower car controller design

[0190] Distributed model predictive control has the advantages of MPC and can explicitly express the relative distance and relative speed constraints of adjacent vehicles in the queue, thereby adding constraints to the queue optimization control process. The control of the following vehicle in the queue adopts DMPC. Each vehicle in the queue is regarded as a separate subsystem, and a separate model predictive controller is designed for each separate subsystem. The objective function of the model control not only includes its own state information, but also includes the state information of the neighborhood. The idea of ​​distributed model predictive control is as follows: Figure 9 As shown in Figure 2. The designed optimization objective function consists of the following four parts:

[0191] (1) Measure the deviation between the predicted output of node i and the equilibrium state output

[0192] When all vehicles in the queue maintain a uniform motion at the desired speed v0, the queue is said to have reached the desired equilibrium state. The equilibrium state of node i corresponds to its desired state x des,i (k|t), which can be output by predicting the prediction in the time domain and the expected equilibrium state output y des,i The (k|t) deviation term is recorded as

[0193]

[0194] (2) Measure the deviation between the predicted input of node i and the equilibrium state input

[0195] When the queue reaches equilibrium, the expected input torque of node i is Should be equal to the resistance torque generated by the driving resistance In the non-equilibrium state, the deviation between the predicted input and the equilibrium input is

[0196]

[0197] (3) Measure the deviation between the predicted output of node i and the assumed output of node i

[0198] Node i predicts the hypothetical output in the time domain at time t The optimized output in the time domain is predicted by time t-1 Transfer and predict the hypothetical output in the time domain and the hypothetical input within the prediction time domain The relationship is:

[0199]

[0200]

[0201] Assume input The first N p -1 element is the optimal control quantity sequence solved at time t Middle and back N p -1 element is shifted one unit to the left along the time axis, Nth p Elements Set as terminal constraint:

[0202]

[0203] The predicted output from node i in the prediction time domain Should be close to the assumed output To ensure that the system gradually stabilizes, remember the deviation term between the predicted output and the assumed output

[0204]

[0205] (4) Measure the predicted output of node i and the neighborhood set Bias term of the internal hypothesis output

[0206] Node i receives node The state information of node i is the predicted output and its neighborhood set The hypothetical output of It should also be as close as possible to:

[0207]

[0208] Among the above four items, Q i ,R i ,F i ,G i All are semi-positive definite weight matrices. Combining the above four items, the cost function of node i at time k is:

[0209]

[0210] Through Lyapunov stability analysis, the weight matrix F i and G i The setting can make the queue asymptotically stable. [5] .

[0211] The simulation results for a platoon consisting of one lead vehicle and four followers under varying slope conditions are presented. Number 0 represents the lead vehicle, and numbers 1 to 4 represent the followers. Initially, at t = 0s, the longitudinal spacing between adjacent vehicles in the platoon is 20 meters. They travel at a desired speed of 10 m / s on a straight road. After 10 seconds, they reach a road section with a 10-degree slope. The lead vehicle in the platoon begins ascending, followed by the followers one by one. Around t = 18s (the specific time depends on the control effect), the last follower vehicle ascends, and the entire platoon continues to travel on the 10-degree slope until the end of the simulation. Compared to the case where the minimum speed of the vehicles in the platoon is reduced to 9.5 m / s without considering the slope, the speed curve of the platoon with slope considerations clearly shows less fluctuation.

[0212] In summary, when multi-sensor fusion is used to build a map, the slope information of the road surface within the map is obtained, and a mapping relationship table between the position within the map and the slope is established. Based on the positioning provided by the map, real-time slope information can be obtained by looking up the table, and the known slope information can be added to the prediction model, thereby improving the effectiveness of formation control and improving road traffic capacity.

[0213] References

[0214] [1]Zhang J.,Singh S.Low-drift and real-time lidar odometry andmapping[J].Autonomous Robots,2017,41(2):401-416.

[0215] [2]Qin T.,Li P.,Shen S.Vins-mono:A robust and versatile monocularvisual-inertial state estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.

[0216] [3]Kim G.,Kim A.Scan context:Egocentric spatial descriptor for placerecognition within 3d point cloud map[C] / / 2018IEEE / RSJ InternationalConference on Intelligent Robots and Systems(IROS).IEEE,2018:4802-4809.

[0217] [4]Samuel M.,Hussein M.,Mohamad M.B.A review of some pure-pursuitbased path tracking techniques for control of autonomous vehicle[J].International Journal of Computer Applications,2016,135(1):35-38.

[0218] [5]Zheng Y.,Li S.E.,Li K.,et al.Platooning of connected vehicles withundirected topologies:Robustness analysis and distributed H-infinitycontroller synthesis[J].IEEE Transactions on Intelligent TransportationSystems,2017,19(5):1353-1364.。

Claims

1. A method for autonomous driving vehicle platooning based on point cloud maps, characterized in that: The specific steps are as follows: First, for the autonomous driving vehicle under test, the measurement data from three sensors, LIDAR, IMU, and GNSS, are integrated to construct a point cloud map of the scene in real time by stitching feature point clouds using the estimated pose. The fusion process is: First, the IMU sensor's measurement data is optimized using the vehicle's front-end odometer. The optimization result is : is the IMU position vector in the world coordinate system, 3 1D; is the IMU velocity vector in the world coordinate system, 3 1D; is the IMU attitude quaternion in the world coordinate system, 4 1-dimensional vector; is the zero bias of the accelerometer in the IMU, 3 1-dimensional vector; is the zero bias of the gyroscope in the IMU, 3 1-dimensional vector; Then, the LIDAR odometer factor, GNSS measurement factor and loop detection factor of the frame image matching are optimized in the backend, and the optimized variables are the pose corresponding to the key frame. ; Then, the point cloud collected by the LIDAR sensor at each moment is transformed into a set of edge feature point clouds through the optimized pose matrix. and surface feature point cloud collection ; Indicates the Edge feature point cloud collected by frame LIDAR sensor, The edge feature point cloud is transformed using the optimized pose matrix Transform the edge feature point cloud to the world coordinates, The surface feature point cloud Transform the surface feature point cloud into world coordinates; Finally, the collection and collection The feature point clouds in the scene are stitched together to form a point cloud map of the scene. : Then, the motion status information of each vehicle in the autonomous driving platoon is obtained from the point cloud map, and a planar formation kinematic model consisting of the lead vehicle and the following vehicle is established. The lateral and longitudinal controllers of the following vehicle in the planar formation task are designed based on the target point following strategy. The status information of the lead vehicle is transmitted to the following vehicle via inter-vehicle communication. The control inputs of the following vehicle are the vehicle speed and the front wheel angle. The specific process of establishing the planar formation kinematic model is as follows: Step 201: For the current following vehicle A, a sliding window is used to save the discrete point sequence of the driving path of the pilot vehicle in the following vehicle A. , when receiving the latest pilot car data When , the data is pressed into the sliding window in chronological order, and the discrete point sequence in the window is regarded as the tracking path of the lateral tracking controller of the following vehicle A; Step 202: Calculate the distance between the following vehicle A and each discrete point in the sliding window one by one, and find the discrete point with the smallest distance to the current position of the following vehicle A. , with a point As a benchmark, the point The previous point is removed from the sliding window, and the cumulative calculation is performed from the discrete points To discrete points The distance between the following vehicle A and the leading vehicle is used as the expected longitudinal spacing between the following vehicle A and the leading vehicle. Step 203: Perform longitudinal PID control based on the deviation between the actual longitudinal spacing of the vehicles and the desired longitudinal spacing to obtain the desired speed of the following vehicle A. Step 204: Similarly, using discrete points As a benchmark, the point The previous point is removed from the sliding window, and the discrete point sequence Find the target point of the lateral tracking controller within , and calculate the front wheel turning angle expected by the following vehicle A; Step 205: Integrate the desired speed and the desired front wheel steering angle of the following vehicle A and apply the result to the following vehicle A. Step 206: Repeat all the above steps to complete the plane formation task in a loop; Next, a dynamic model of the longitudinal vehicle formation on a road with varying slope is established in discrete time and rewritten into the form of a state-space equation. Finally, a model predictive controller (MPC) is designed for the lead vehicle and a distributed model predictive controller (DMPC) is designed for the follower vehicle. Combined with the state-space equations, the results of platooning of autonomous driving vehicles on downhill sections of urban roads are simulated.

2. The method for autonomous driving vehicle platooning based on point cloud maps according to claim 1, characterized in that: The pilot vehicle follows the manually recorded path at a fixed speed. The longitudinal distance between the following vehicle and the pilot vehicle at the starting moment is greater than the expected distance, and the initial speed is 0.

3. The method for autonomous driving vehicle platooning based on point cloud maps according to claim 1, characterized in that: The dynamic model of the longitudinal vehicle formation is rewritten into the form of a state space equation, specifically: Number The longitudinal dynamic model of the vehicle is: Indicates the number of vehicles in The displacement of time, Indicates the number of vehicles in The speed of time, represents a discrete time interval; Indicates the number The vehicle quality, Indicates the number The transmission efficiency of the vehicle, Indicates the number The rolling radius of the vehicle's tires, Indicates the number of vehicles in The actual driving / braking torque at the moment, Indicates the number The inertia hysteresis time of the vehicle's transmission system, Indicates the number of vehicles in Expected driving / braking torque at time, Indicates the number Vehicle running resistance; Let the state variable , Take displacement and velocity as output variables , numbered The longitudinal dynamics model of the vehicle in discrete time is written in the form of state space equations: 。 4. The method for autonomous driving vehicle platooning based on point cloud maps according to claim 1, wherein: The process of designing MPC for the lead vehicle, designing DMPC for the follower vehicle, and simulating the platoon driving results is as follows: The MPC controller is designed for the pilot car. The state space equation is used to construct the cost function of the pilot car based on the difference between the output variable in the prediction time domain and the expected output variable: is the total time; and are the weight matrices of the pilot vehicle’s output and input, Indicates pilot car The predicted input at the moment, Indicates pilot car The predicted output at time Indicates pilot car The expected output at time, Indicates pilot car The predicted speed of the moment, Indicates offset pilot car Driving torque of uniform speed resistance; The following car uses the controller DMPC. The objective function includes not only the state information of each car itself, but also the state information of each vehicle in the neighborhood, numbered as The following car is The cost function at the moment is: , , , are all positive semidefinite weight matrices, For the number Following car Prediction output at the moment; Output for the desired equilibrium state; For the number Following car The expected input torque at time t, For the number The following car is Predict hypothetical outputs within the time domain at every moment; To offset the number Following car The resistance torque generated by the resistance of uniform speed movement, For the number The neighborhood set of following vehicles The hypothetical output of is the expected value of the difference between the predicted output and the assumed output; Finally, through Lyapunov stability analysis, the weight matrix is ​​set and , making the autonomous vehicle platoon asymptotically stable.

Images