An automatic driving decision control method based on roadside fusion perception

Abstract

This invention relates to an autonomous driving decision-making and control method based on roadside fusion perception, comprising: Step S1: acquiring road condition information and environmental information surrounding the vehicle and the vehicle's own state information; Step S2: determining the vehicle's driving behavior based on the road condition information, environmental information surrounding the vehicle and the vehicle's own state information; Step S3: planning the optimal driving path for the vehicle based on the determined driving behavior; Step S4: controlling the longitudinal trajectory and lateral trajectory of the vehicle's movement to make the vehicle travel along the optimal driving path. This invention can control the vehicle's driving behavior based on the current scenario, plan the optimal path for the vehicle based on the driving behavior, and control the vehicle to travel along the optimal path.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an autonomous driving decision-making and control method based on roadside fusion perception. Background Technology

[0002] The traditional three elements of transportation—"people, vehicles, and roads"—only include traffic participants and some transportation infrastructure. Clearly, considering only these elements is insufficient to support the development of intelligent and smart transportation in my country. In the construction of intelligent transportation, we must fully consider various factors. Therefore, the improvement of vehicle-road cooperative technology based on "people, vehicles, roads, and environment" has greatly contributed to the development of my country's intelligent transportation system, while the development of roadside fusion sensing systems plays a crucial role in the implementation of vehicle-road cooperation. With the rapid development of sensor and communication technologies in recent years, vehicle-road cooperation based on V2V and V2X has gradually been promoted and realized, organically combining traffic participants, transportation infrastructure, and the traffic environment, achieving information exchange. Breakthroughs in roadside fusion sensing equipment have made the collection of basic traffic information richer and more comprehensive, greatly improving traffic safety and road network efficiency.

[0003] Existing roadside fusion sensing technologies have the following main drawbacks:

[0004] 1. Traditional roadside solutions require multiple sensors to be mounted on the same pole for comprehensive sensing, while the computing platform may need to be housed in a roadside electrical box, increasing deployment difficulty and construction costs. Secondly, installing multiple sensors separately not only requires multiple pole mountings but also necessitates independent calibration for each sensor. In practical highway applications, this would require mounting a new set of equipment approximately every 200 meters, significantly increasing workload.

[0005] 2. Existing roadside fusion sensing devices are mostly limited to applications on highways, closed industrial parks, mining areas, and between buses and traffic lights or stops, and have not yet been widely adopted. Furthermore, the information transmitted between them is limited and simple, mainly consisting of basic elements such as vehicle speed and traffic light timings. In the future, building a smart city where everything is interconnected and everything is interoperable requires more than just acquiring and transmitting these basic elements.

[0006] 3. The driving environment surrounding the intelligent connected vehicle was not fully considered; only one or two variables of lane-changing and following behavior were considered, without fully taking into account the influencing factors of lane-changing and following. Secondly, the anthropomorphic characteristics of the decision-making and planning model were not taken into account.

[0007] 4. Research on motion planning algorithms is quite disorganized, and the established models are complex and diverse. Graph search-based methods consider the influence of the surrounding environment, but their computational efficiency is affected by graph resolution and the computational load is large; RRT sampling-based methods achieve high-dimensional path planning, but the continuity of the planned paths is too poor; interpolation-based methods can quickly plan continuous paths based on known path points, but they need to comprehensively consider vehicle dynamics and kinematic constraints.

[0008] 5. Regarding lateral control of the vehicle, the accuracy and robustness of the controller cannot be guaranteed under extreme road conditions and uncertain conditions.

[0009] In summary, existing autonomous driving technologies consider fewer factors in control decisions, resulting in imperfect driving control schemes that need improvement. Summary of the Invention

[0010] Therefore, the technical problem to be solved by the present invention is to overcome the shortcomings of existing autonomous driving decision control methods, which consider fewer factors and result in imperfect driving control schemes.

[0011] To address the aforementioned technical problems, this invention provides an autonomous driving decision-making and control method based on roadside fusion perception, comprising:

[0012] Step S1: Obtain road condition information and environmental information around the vehicle, as well as the vehicle's own status information;

[0013] Step S2: Determine the vehicle's driving behavior based on the road conditions and environment around the vehicle and the vehicle's own status information;

[0014] Step S3: Plan the optimal driving path for the vehicle based on the determined driving behavior;

[0015] Step S4: By controlling the longitudinal trajectory and lateral trajectory of the vehicle's movement, the vehicle is made to travel along the optimal driving path.

[0016] In one embodiment of the present invention, step S1 further includes fusing the acquired road condition information around the vehicle, specifically:

[0017] Target information is obtained through LiDAR and visual sensors. The point cloud data obtained by LiDAR is converted to the coordinate system of the visual sensor, and then the point cloud data in the coordinate system of the visual sensor is converted to the pixel coordinate system corresponding to the image data obtained by the visual sensor.

[0018] Obtain the 3D bounding box C of the point cloud data in pixel coordinate system respectively j ={A,B',C',D} and the 3D bounding box C of the image data i={a,b',c',d};

[0019] The three-dimensional border C j ={A,B',C',D} and 3D border C i The target is matched against {a, b', c', d}. If the matching result is less than the fixed parameter σ, it indicates that the target detected by the lidar and the vision sensor are the same; otherwise, the detected targets are not the same. The formula is: in, ΔL represents the difference between the size of the bounding box after the LiDAR point cloud coordinate system transformation and the pixel size of the visual sensor. ΔL represents the difference between the bounding box lengths obtained by the LiDAR and the visual sensor, and l represents the bounding box length obtained by the visual sensor.

[0020] In one embodiment of the present invention, the method for determining driving behavior in step S2 based on road condition information and environmental information surrounding the vehicle and the vehicle's own state information includes:

[0021] When the vehicle is located at a pedestrian crossing in the middle of the road, the driving behavior is determined based on whether there are pedestrians crossing the crossing. Specifically:

[0022] If there are no vehicles ahead in the lane in which the vehicle is traveling and no pedestrians are crossing the crosswalk, then the vehicle can be controlled to move freely.

[0023] If there are no vehicles ahead in the lane the vehicle is traveling in and pedestrians are crossing the crosswalk, then determine the time T taken for the pedestrian to cross the crosswalk. p Time T for the vehicle to reach the yielding line v The size relationship between them, if T p <T v If T is selected, the vehicle is controlled to move freely; otherwise, the vehicle is controlled to decelerate and stop. v =T v-c +T v-l T v-c T represents the time taken for a vehicle to choose a lane change. v-l This indicates the time it takes for the vehicle to travel straight on the road after changing lanes;

[0024] If there are vehicles ahead in the lane where the vehicle is traveling and pedestrians are crossing the crosswalk, then, while maintaining a safe distance between the vehicles in front and behind, determine the time T taken for the pedestrian to cross the crosswalk. p Time T for the vehicle to reach the yielding line v The size relationship between them, if T p <T v If it is set, the vehicle will be allowed to move freely; otherwise, the vehicle will be slowed down and brought to a stop.

[0025] If there are vehicles ahead in the lane the vehicle is traveling in and no pedestrians are crossing the crosswalk, then the desired vehicle speed V is determined. v Speed ​​V of the vehicle in front l The size relationship between them, if V v >V l If the conditions for lane changing are met, then control the vehicle to change lanes; if V v <V l Then the vehicle can be controlled to move freely;

[0026] When the vehicle is in a roadside blind spot, determine whether pedestrians in the blind spot pose a potential danger to the driver. If they do not pose a potential danger to the vehicle, control the vehicle to drive freely. If they do pose a potential danger to the vehicle, continue to determine whether there are other vehicles in front of the vehicle. If there are, control the vehicle to follow. If not, control the vehicle to slow down and stop.

[0027] In one embodiment of the present invention, the vehicle is controlled to change lanes under a first constraint condition, the formula for which the first constraint condition is:

[0028]

[0029] Among them, V v V represents the vehicle's expected speed, T represents the vehicle's actual speed, and Z represents the sampling period. tsv S represents the set threshold. v D0 represents the distance traveled by the vehicle, and S represents the distance between the front of the vehicle and the front of the vehicle in front. l L represents the distance traveled by the vehicle in front. l This indicates the length of the vehicle in front.

[0030] In one embodiment of the present invention, the vehicle is controlled to follow the car while satisfying a second constraint condition, the formula for which the second constraint condition is:

[0031]

[0032] Among them, a v a l Let a represent the deceleration of the preceding vehicle and the currently controlled vehicle, respectively. f a represents the desired acceleration. min a max To satisfy the minimum and maximum acceleration under the maximum speed limit conditions in the scenario, D0 represents the distance between the front of the vehicle and the front of the vehicle in front, and L... l C represents the length of the vehicle in front, C represents the response time of the vehicle components, and v represents the length of the vehicle in front. v This indicates the current speed of the vehicle.

[0033] In one embodiment of the present invention, step S3 specifically involves: based on the determined driving behavior, using quintic polynomial trajectory planning theory to plan the lateral and longitudinal trajectories of the vehicle movement to obtain the optimal driving path, including:

[0034] Obtain the vehicle's current state and the state at the target point, and construct the vehicle's lateral and longitudinal position, velocity, and acceleration state formulas based on the obtained state data using fifth-order polynomial trajectory planning theory:

[0035]

[0036] in, This represents the lateral trajectory of the vehicle's movement. Represents the longitudinal trajectory of the vehicle's motion, (x x1 ,v x1 ,a x1 ),(x x2 ,v x2 ,a x2 (y) represents the vehicle's horizontal coordinate, velocity, and acceleration state before and after lateral movement, respectively. y1 ,v y1 ,a y1 ),(y y2 ,v y2 ,a y2 A represents the vehicle's vertical coordinate, speed, and acceleration before and after longitudinal movement, respectively. T =(a5,a4,a3,a2,a1,a0), B T = (b5, b4, b3, b2, b1, b0), a i b i All are coefficients of a polynomial. t tra Indicates the trajectory prediction time;

[0037] Based on the vehicle's lateral and longitudinal positions, velocity, and acceleration state, design the first objective function for the vehicle's lateral trajectory.

[0038] Based on the vehicle's lateral and longitudinal positions, velocity, and acceleration state, design a second objective function for the vehicle's longitudinal trajectory.

[0039] The lateral trajectory under the first objective function and the longitudinal trajectory under the second objective function constitute the optimal driving path of the vehicle.

[0040] In one embodiment of the present invention, the formula for the first objective function is:

[0041]

[0042] Where, kh_t The first coefficient represents the trajectory prediction time T. k is the time parameter for trajectory planning. h_i It is the endpoint of the lateral displacement, k h_i The coefficient k represents the endpoint of the lateral displacement. hv_t The coefficient represents the integral of the rate of change of lateral acceleration over time, and x(t) represents the position trajectory in the lateral direction.

[0043] In one embodiment of the present invention, the formula for the second objective function is:

[0044]

[0045] Where, k v_t The second coefficient represents the trajectory prediction time T; k v_s The coefficient representing the squared distance between the predicted trajectory's longitudinal displacement endpoint and the longitudinal target point; k vv_t The coefficient represents the integral of the square of the longitudinal acceleration rate of change with respect to time; y(t) represents the longitudinal position trajectory, s pf The formula represents the target point location for maintaining a safe distance when following another vehicle.

[0046] s pf =s v_l (t)-2L l -H t ·v l (t)

[0047] Among them, s v_l (t) represents the position of the vehicle ahead at time t, L l H represents the length of the vehicle in front. t Indicates a fixed time interval, v l (t) represents the speed of the vehicle in front at time t.

[0048] In one embodiment of the present invention, step S4 involves controlling the longitudinal and lateral trajectories of the vehicle's movement to ensure the vehicle travels along the optimal driving path. The method includes:

[0049] The PID control algorithm is used to improve the aiming-ahead algorithm, and the improved algorithm is used to control the longitudinal trajectory of the vehicle. The improved aiming-ahead algorithm is used to compensate for the lateral deviation at the aiming point when the vehicle is turning. The formula of the improved aiming-ahead algorithm is as follows:

[0050]

[0051] Where L1 is the length of the axle, L2 is the distance between the center point of the rear axle and the aiming point, and θ is the distance between the current vehicle heading and the distance between the center point of the rear axle and the aiming point (p). x ,py The angle between the lines is Δl1, which represents the deviation between the expected lateral distance and the current actual distance at the aiming point; Δl′1 represents the deviation between the expected lateral movement rate and the actual lateral movement rate at the aiming point; and Δl2 represents the deviation between the lateral distance between the vehicle and the nearest point on the reference trajectory.

[0052] A model predictive control algorithm is used to control the lateral trajectory of the vehicle. The model predictive control algorithm has an objective function, the formula of which is:

[0053]

[0054] Where Z represents the vehicle's state vector. Q is the first cost matrix of the prediction model and the desired trajectory, and R is the first cost matrix of the optimization control variables. f Z represents the second cost matrix between the prediction model and the desired trajectory. T_r Z represents the state control variable for predicting the expected value in the time domain. T Z represents the current state control variable in the prediction time domain. t_r Z represents the desired state control variable during the sampling time. t u represents the state control variable during the sampling time. t R represents the control variable at time t. d Let R(t) represent the second cost matrix for optimizing the control variables. Q(t) is used to eliminate deviation values, and R(t) is used to ensure the stability of vehicle driving.

[0055] In one embodiment of the present invention, the constraint condition of f(Δl1,Δl1',Δl2) in the improved pre-aiming algorithm formula is:

[0056]

[0057] Among them, K P Represents the proportionality coefficient, K I Indicates the integral coefficient, K D denoted by the differential coefficient, and t represents time.

[0058] The technical solution of the present invention has the following advantages compared with the prior art:

[0059] This invention addresses the traffic conflicts caused by intelligent connected vehicles encountering pedestrians crossing the road or by blind spots due to obstacles on the roadside. It differentiates decision-making schemes for different scenarios, ultimately improving traffic flow smoothness, reducing the probability of traffic accidents, and contributing to the future development and implementation of vehicle-road cooperation.

[0060] This invention formulates specific driving behaviors for intelligent connected vehicles in different scenarios, and creatively constructs a first constraint condition for lane changing and a second constraint condition for following the vehicle, so that the specific driving behaviors of the vehicle can be safe and effective.

[0061] This invention is based on an improved PID pre-aiming algorithm, using a positional PID algorithm in the trajectory tracking controller. This allows the vehicle to quickly return to the desired trajectory (preventing deviation during lane change tracking), improving system stability and dynamic response speed. It effectively eliminates oscillations in the proportional element, reduces static deviation, and improves system control accuracy. Furthermore, the hybrid optimization stage of model predictive control is optimized by designing an objective function that enables the vehicle to achieve optimal control with relatively few operations, reducing the error in tracking the desired trajectory and ensuring smooth, oscillation-free tracking. Attached Figure Description

[0062] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings.

[0063] Figure 1 This is a flowchart of the method in an embodiment of the present invention;

[0064] Figure 2 This is a schematic diagram of pedestrian detection using the fusion of lidar and visual sensors in an embodiment of the present invention;

[0065] Figure 3 This is a schematic diagram of a pedestrian crossing in the center of a road in an embodiment of the present invention;

[0066] Figure 4 This is a schematic diagram of the blind spot of roadside obstacles in an embodiment of the present invention;

[0067] Figure 5 This is a schematic diagram of pure aiming considering lateral deviation feedback in an embodiment of the present invention. Detailed Implementation

[0068] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0069] Reference Figure 1 As shown, this invention relates to an autonomous driving decision-making and control method based on roadside fusion perception, comprising:

[0070] Step S1: Obtain road condition information and environmental information around the vehicle, as well as the vehicle's own status information;

[0071] Step S2: Determine the vehicle's driving behavior based on the road conditions and environment around the vehicle and the vehicle's own status information;

[0072] Step S3: Plan the optimal driving path for the vehicle based on the determined driving behavior;

[0073] Step S4: By tracking and controlling the vehicle, the vehicle is made to travel along the optimal driving path.

[0074] This invention formulates specific driving behaviors for intelligent connected vehicles in different scenarios, and creatively constructs a first constraint condition for lane changing and a second constraint condition for following the vehicle, so that the specific driving behaviors of the vehicle can be safe and effective.

[0075] The present invention will be further described in detail below, and the specific steps are as follows:

[0076] Step 1: When multiple types of vehicles enter the detection area, the roadside fusion sensing equipment identifies the vehicle type and its lane position. It then determines the application scenario of the intelligent connected vehicle, whether it's a pedestrian crossing in the center of the road or a blind spot due to roadside obstacles. Next, it integrates sensing equipment, including LiDAR and visual sensors, to acquire comprehensive traffic information, including road condition information, vehicle driving information, pedestrian crossing information, roadside blind spot information, and other relevant information.

[0077] Step 11: The main principle of pedestrian and vehicle location perception is through, for example... Figure 2 As shown, S1 represents the LiDAR and S2 represents the visual sensor. The roadside fusion sensing device detects pedestrians on the crosswalk, primarily detecting their position and speed. The LiDAR mainly calculates distance, while the visual sensor identifies individual pedestrians. The pedestrian's position and speed information are calculated as follows:

[0078]

[0079] In the three dimensions of the pedestrian, the boundary points of the distance fusion detection device are selected as the boundary points of the 3D bounding box, thus forming a 3D pedestrian bounding box image. R in the formula... i This indicates the location information of the pedestrian detected by the lidar, x i The coordinates in the x-direction of the point cloud data formed by the lidar are represented by the coordinates in the y-direction. i β represents the coordinates in the y-direction of the point cloud data formed by the lidar. i ΔV represents the angle between the laser beam emitted by the lidar and the horizontal direction, ΔR represents the pedestrian's speed, and ΔR represents the distance traveled in time ΔT. When calculating pedestrian speed, the average speed of the pedestrian is used.

[0080] Step 12: As Figure 2 As shown, during fusion perception, the data acquired by LiDAR and visual sensors need to undergo coordinate unification processing, mapping the point cloud data acquired by LiDAR to the pixel points obtained by the visual sensor. This facilitates subsequent data processing and analysis. Secondly, using data from both sensors for unified analysis significantly improves recognition accuracy. First, the point cloud coordinate system is converted to the visual sensor coordinate system. The point cloud data acquired by LiDAR is represented as [x...]. i ,y i ,z i The distance between the data point and the visual sensor is represented as [x]. j ,y j ,z j Then use R (3×3) The transformation matrix, denoted by r, represents the transformation between two coordinate systems. (3×1) The translation matrix between corresponding points in the coordinate system is expressed as follows:

[0081]

[0082] Switching to the image and pixel coordinate system of the visual sensor, the transformation expression is as follows:

[0083]

[0084] In the above formula, z c is the scaling factor, (μ,v) are the image pixel coordinates, dx and dy represent the physical size of the pixel, i.e., the actual physical position, (μ0,v0) are the coordinates of the image origin, s' is the tilt factor, and f represents the focal length in the vision sensor.

[0085] Step 13: After unifying the coordinate system in Step 12, the information acquired by the LiDAR and visual sensors is fused, and then processed using a decision-level fusion method. The information acquired by the visual detector is represented on the image by a 3D bounding box, and then processed by C... i ={a,b',c',d} represents the point cloud data acquired by the lidar. After transformation in step 12, C is used to represent the point cloud data. j ={A,B',C',D} represents the boundary information of the two sets of data.

[0086]

[0087] It is important to note that Figure 2 There's no problem with the figure having 8 points. The meaning expressed by using four points in the above formula is that the 3D frame of the person can be represented by the opposite vertices of the 3D shape. In the above formula, This represents the difference between the size of the bounding box after the LiDAR point cloud coordinate system transformation and the pixel size of the visual sensor. ΔL represents the difference between the bounding box lengths obtained by the LiDAR and the visual sensor, l represents the bounding box length obtained by the visual sensor, and σ is a fixed parameter. If the above formula is satisfied, it means that the obstacles detected by both are the same; otherwise, they are different. Step 2: If the scenario where the intelligent connected vehicle is in is a pedestrian crossing in the middle of the road, and there is a pedestrian P crossing the road ahead, or there is no pedestrian P crossing the road, then proceed to step 21. If the scenario where the intelligent connected vehicle is in is a roadside blind spot, and there are large vehicles, buses, etc. on the right side of the road ahead that will cause a blind spot, the appearance of pedestrian P is likely to cause a traffic accident, then proceed to step 22.

[0088] Step 211: When there are no vehicles or people ahead, the intelligent connected vehicle can drive freely.

[0089] Step 212: When there are no other vehicles ahead, but pedestrian P is crossing the zebra crossing, determine the time T taken for pedestrian P to cross the road. p The time T taken for a connected vehicle to reach the stop line v The size relationship between them, if T p <T v Then the vehicle can move freely. If T p >T v If this happens, the vehicle needs to slow down and stop.

[0090] Among them, T v =T v-c +T v-l T v-c T represents the time taken for a vehicle to choose a lane change. v-l This indicates the time it takes for a vehicle to travel straight on the road after changing lanes.

[0091] Step 213: When there are other vehicles ahead and pedestrian P is crossing the zebra crossing, while maintaining a safe distance between the vehicles in front and behind, determine the time T taken for pedestrian P to cross the road. p The time T taken for a connected vehicle to reach the stop line v The size relationship between them, if T p <T v Then the vehicle can move freely. If T p >T v If this happens, the vehicle needs to slow down and stop.

[0092] Step 214: When there are vehicles ahead on the road and no one is at the crosswalk, it is necessary to analyze the expected vehicle speed V. v Speed ​​V of the vehicle in front l Judge the relationship between V and V. v >Vl If the conditions for lane changing are met, then lane changing is permitted. If V v <V l Then you can drive freely.

[0093] Step 22: Based on the information of the scene ahead identified by the roadside fusion device, if there is a blind spot S on the roadside ahead, such as... Figure 4 As shown. The roadside fusion perception module is needed to analyze obstacles in the blind spot to determine whether obstacle P within the blind spot poses a potential hazard to driving. If it has no impact, the connected vehicle can drive freely; if it does affect the connected vehicle, the system continues to analyze whether there are other vehicles ahead. If there are, it follows; if not, it prepares to slow down and stop.

[0094] The determination of whether there is a potential risk is mainly based on the roadside sensing equipment to perceive the behavioral intentions of pedestrian P in the blind spot. The general standards are as follows:

[0095]

[0096] Among them, t 制动+反应 The time t represents the vehicle's braking and reaction time (i.e., the reaction time is the response time of the vehicle's components). p This indicates the time it takes for a pedestrian to reach the stop line boundary.

[0097] It is worth mentioning that the feasibility analysis of lane changing in the above steps mainly considers two aspects: (1) the generation of the intention to change lanes; and (2) the minimum safe distance for lane changing. The following conditions must be met simultaneously.

[0098]

[0099] In the above formula, V v V represents the vehicle's expected speed, T represents the vehicle's actual speed, and Z represents the sampling period. tsv S represents the set threshold. v The distance traveled by the intelligent connected vehicle is represented by D0, where D0 represents the distance between the front and rear of the vehicle, and S represents the distance between the front and rear of the vehicle. l L represents the distance traveled by the vehicle in front. l This indicates the length of the vehicle in front.

[0100] It is worth mentioning that, for the following driving in steps 21 and 22 above, it is necessary to ensure that the control of the desired speed of the intelligent connected vehicle can meet the requirement that the vehicle in front can stop in time to avoid a rear-end collision when braking at maximum deceleration. Therefore, the key to the following driving of the intelligent connected vehicle is also equivalent to the decision on the desired acceleration of the vehicle itself, which needs to meet the following constraints:

[0101]

[0102] In the formula, av a l Let a represent the deceleration of the front and rear vehicles respectively, C represent the response time of the vehicle components, and a represent the deceleration of the front and rear vehicles respectively. f The desired acceleration satisfies [a] min ,a max Between ] . a min a max It is the minimum and maximum acceleration under conditions such as the maximum speed limit of the scenario.

[0103] Step 3: Based on the decisions made in Step 2, plan the driving path for the intelligent connected vehicle. Using the fifth-order polynomial trajectory planning theory, plan and design the lateral and longitudinal trajectories of the intelligent connected vehicle. When changing lanes, the following factors need to be considered: the curvature and speed of the planned trajectory must be continuous in the time domain and cannot change abruptly; and the algorithm must be able to generate the optimal trajectory even when the speed changes. This invention assumes that, given a known global path, the vehicle's motion on a structured road is divided into lateral and longitudinal motions by establishing a coordinate system.

[0104] like Figure 3 As shown, based on the state of the intelligent connected vehicle at time t0 and the state of the target point at time t1 obtained by the roadside fusion perception module, the expressions for the vehicle's lateral and longitudinal positions, velocity, and acceleration are obtained:

[0105]

[0106] in, This represents the lateral trajectory of the vehicle's movement. Represents the longitudinal trajectory of the vehicle's motion, (x x1 ,v x1 ,a x1 ),(x x2 ,v x2 ,a x2 (y) represents the vehicle's horizontal coordinate, velocity, and acceleration state before and after lateral movement, respectively. y1 ,v y1 ,a y1 ),(y y2 ,v y2 ,a y2 A represents the vehicle's vertical coordinate, speed, and acceleration before and after longitudinal movement, respectively. T =(a5,a4,a3,a2,a1,a0), B T = (b5, b4, b3, b2, b1, b0), where a and b are the coefficients of the polynomial.

[0107]

[0108] Among them, t tra Indicates the trajectory prediction time.

[0109] Step 31: Based on the state expression described in Step 3, and building upon the fifth-order polynomial trajectory planning, design the first objective function for lateral motion planning:

[0110]

[0111] Where, k h_t A coefficient representing the trajectory prediction time T. k is the time parameter for trajectory planning. h_i It is the endpoint of the lateral displacement, k h_i It is a coefficient representing the endpoint of the lateral displacement, k hv_t It represents the coefficient of the lateral acceleration rate of change integral over time, and x(t) represents the position trajectory in the lateral direction.

[0112] Step 32: Determine the trajectory using the position, velocity, and acceleration of the starting and ending points of the driving trajectory based on the fifth-order polynomial. For lane-changing scenarios, characteristics such as low acceleration and rapid trajectory convergence are required; therefore, the second objective function is:

[0113]

[0114] In the formula, k v_t The coefficient k represents the trajectory prediction time T. v_s The coefficient representing the squared distance between the predicted trajectory's longitudinal displacement endpoint and the longitudinal target point; k vv_t The coefficient represents the integral of the square of the longitudinal acceleration rate of change with respect to time; y(t) represents the longitudinal position trajectory; s pf The formula for determining the target point location to maintain a safe following distance in a following vehicle scenario is expressed as follows:

[0115] s pf =s v_l (t)-2L l -H t ·v l (t)

[0116] In the above formula, s v_l (t) represents the position of the vehicle ahead at time t, L l H represents the length of the vehicle in front. t Indicates a fixed time interval, v l (t) represents the speed of the vehicle in front at time t.

[0117] Step 4: Based on the vehicle's motion model, perform tracking control on the target trajectory function expression obtained in Step 3. The improved pre-aiming algorithm controls the longitudinal trajectory of the vehicle's motion, proceeding to Step 41. The model predictive control algorithm is then used to control the lateral trajectory of the vehicle's motion, proceeding to Step 42.

[0118] Step 41: As Figure 5 As shown, this embodiment uses PID to improve the pre-aiming algorithm and uses the improved pre-aiming algorithm to control the longitudinal trajectory of the vehicle. The improved pre-aiming algorithm compensates for the problem of lateral deviation feedback at the pre-aiming point when turning. The formula of the improved pre-aiming algorithm is:

[0119]

[0120] In the above formula, L1 is the length of the axle, L2 is the distance between the center point of the rear axle and the aiming point, and θ is the distance between the current vehicle heading and the distance between the center point of the rear axle and the aiming point (p). x ,p y Let f(Δl1, Δl′1, Δl2) be the angle between the lines connecting the two points, where Δl1 represents the deviation between the expected lateral distance and the current actual distance at the aiming point, Δl′1 represents the deviation between the expected lateral movement rate and the actual lateral movement rate at the aiming point, and Δl2 represents the deviation between the lateral distance between the vehicle and the nearest point on the reference trajectory. The constraints for f(Δl1, Δl′1, Δl2) are:

[0121]

[0122] Among them, K P Represents the proportionality coefficient, K I Indicates the integral coefficient, K D denoted by the differential coefficient, and t represents time.

[0123] This embodiment improves the preview algorithm using PID, which can reduce the deviation between the controlled system and the desired value and improve the accuracy of control.

[0124] Step 42: The lateral trajectory of the vehicle is controlled using a model predictive control algorithm. The model predictive control algorithm has an objective function (this part is prior art and will not be described in detail here). The formula for the objective function is:

[0125]

[0126] Where Z represents the vehicle's state vector. Q is the first cost matrix of the prediction model and the desired trajectory, and R is the first cost matrix of the optimization control variables. f Z represents the second cost matrix between the prediction model and the desired trajectory. T_r Z represents the state control variable for predicting the expected value in the time domain. T Z represents the current state control variable in the prediction time domain. t_r Z represents the desired state control variable during the sampling time. t u represents the state control variable during the sampling time.t R represents the control variable at time t. d Let R(t) represent the second cost matrix for optimizing the control variables. Q(t) is used to eliminate deviation values, and R(t) is used to ensure the stability of vehicle driving.

[0127] The ultimate implementation of trajectory tracking involves transmitting control signals to relevant controllers such as the steering wheel, accelerator, and brakes to achieve overall vehicle control. The modified vehicle then acts as a dynamic participant in the scene, continuously driving within the scene area.

[0128] Although preferred embodiments of this application have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of this application.

[0129] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. An autonomous driving decision-making and control method based on roadside fusion perception, characterized in that, include: Step S1: Obtain road condition information and environmental information around the vehicle, as well as the vehicle's own status information; Step S2: Determine the vehicle's driving intention based on the road conditions and environment around the vehicle and the vehicle's own status information; In step S2, the driving intention is determined based on the road conditions and environmental information surrounding the vehicle and the vehicle's own status information. The method includes: When the vehicle is in a pedestrian crossing in the middle of the road, the driving intention is determined based on whether there are pedestrians crossing. Specifically: If there are no vehicles ahead in the lane in which the vehicle is traveling and no pedestrians are crossing the crosswalk, then the vehicle can be controlled to move freely. If there are no vehicles ahead in the lane the vehicle is traveling in and pedestrians are crossing the crosswalk, then determine the time taken for the pedestrians to cross the crosswalk. Time taken for vehicles to reach the yielding line The size relationship between them, if If it is controlled, the vehicle will move freely; otherwise, the vehicle will decelerate and stop. , This indicates the time taken for a vehicle to choose a lane change. This indicates the time it takes for the vehicle to travel straight on the road after changing lanes; If there are vehicles ahead in the lane where the vehicle is traveling and pedestrians are crossing the crosswalk, then, while maintaining a safe distance between the vehicles in front and behind, determine the time it takes for the pedestrians to cross the crosswalk. Time taken for vehicles to reach the yielding line The size relationship between them, if If it is set, the vehicle will be allowed to move freely; otherwise, the vehicle will be slowed down and brought to a stop. If there are vehicles ahead in the lane the vehicle is traveling in and no pedestrians are crossing the crosswalk, then determine the desired speed. Speed ​​of the vehicle in front The size relationship between them, if If the conditions for lane changing are met, then control the vehicle to change lanes; if Then the vehicle can be controlled to move freely; When the vehicle is in a roadside blind spot, determine whether pedestrians in the blind spot pose a potential danger to the driver. If they do not pose a potential danger to the vehicle, control the vehicle to drive freely. If they do pose a potential danger to the vehicle, continue to determine whether there are other vehicles in front of the vehicle. If there are, control the vehicle to follow. If not, control the vehicle to slow down and stop. To control the vehicle's following motion under the second constraint condition, the formula for the second constraint condition is: ； in, , These represent the deceleration rates of the preceding vehicle and the currently controlled vehicle, respectively. Indicates the expected acceleration. , To satisfy the minimum and maximum acceleration under the maximum speed limit condition in the given scenario, Indicates the distance between the front of one vehicle and the front of the vehicle in front. Indicates the length of the vehicle in front. Indicates the response time of vehicle components. This indicates the current speed of the vehicle; Step S3: Plan the optimal driving path for the vehicle based on the determined driving intention; Step S4: By controlling the longitudinal trajectory and lateral trajectory of the vehicle's movement, the vehicle is made to travel along the optimal driving path.

2. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 1, characterized in that: Step S1 further includes fusing the acquired road condition information around the vehicle, specifically: Target information is obtained through LiDAR and visual sensors. The point cloud data obtained by LiDAR is converted to the coordinate system of the visual sensor, and then the point cloud data in the coordinate system of the visual sensor is converted to the pixel coordinate system corresponding to the image data obtained by the visual sensor. Obtain the 3D bounding box of the point cloud data in pixel coordinate system respectively 3D borders for image data ; The three-dimensional border and 3D border Perform a match; if the match result is less than a fixed parameter... This indicates that the target detected by the lidar and the visual sensor is the same; Otherwise, the detected targets are inconsistent, and the formula is: ,in, This represents the difference between the size of the bounding box of the LiDAR point cloud after coordinate transformation and the pixel size of the visual sensor. This represents the difference between the bounding box lengths obtained by the LiDAR and the visual sensor. This indicates the border length acquired by the vision sensor.

3. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 1, characterized in that: To control the vehicle's lane-changing behavior under the first constraint condition, the formula for the first constraint condition is: ； in, Indicates the vehicle's desired speed. Indicates the vehicle's actual speed. The sampling period is This indicates the set threshold. Indicates the distance traveled by the vehicle. Indicates the distance between the front of one vehicle and the front of the vehicle in front. Indicates the distance traveled by the vehicle in front. This indicates the length of the vehicle in front.

4. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 1, characterized in that: Step S3 specifically involves: based on the determined driving intention, using fifth-order polynomial trajectory planning theory to plan the lateral and longitudinal trajectories of the vehicle's motion, obtaining the optimal driving path, including: Obtain the vehicle's current state and the state at the target point, and construct the vehicle's lateral and longitudinal position, velocity, and acceleration state formulas based on the obtained state data using fifth-order polynomial trajectory planning theory: ； in, This represents the lateral trajectory of the vehicle's movement. This represents the longitudinal trajectory of the vehicle's movement. These represent the vehicle's horizontal coordinates, velocity, and acceleration states before and after lateral movement, respectively. These represent the vehicle's vertical coordinate, speed, and acceleration before and after longitudinal movement, respectively. , , , All are coefficients of a polynomial. , Indicates the trajectory prediction time; Based on the vehicle's lateral and longitudinal positions, velocity, and acceleration state, design the first objective function for the vehicle's lateral trajectory. Based on the vehicle's lateral and longitudinal positions, velocity, and acceleration state, design a second objective function for the vehicle's longitudinal trajectory. The lateral trajectory under the first objective function and the longitudinal trajectory under the second objective function constitute the optimal driving path of the vehicle.

5. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 4, characterized in that: The formula for the first objective function is: ； in, Indicates trajectory prediction time The first coefficient, These are the time parameters for trajectory planning. It is the endpoint of the lateral displacement. A coefficient representing the endpoint of the lateral displacement. The coefficient representing the integral of the rate of change of lateral acceleration over time. It represents the position trajectory in the horizontal direction.

6. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 4, characterized in that: The formula for the second objective function is: ； in, Indicates trajectory prediction time The second coefficient; A coefficient representing the squared distance between the endpoint of the longitudinal displacement of the predicted trajectory and the longitudinal target point; The coefficient representing the integral of the square of the rate of change of longitudinal acceleration over time; It represents the vertical position trajectory. The formula represents the target point location for maintaining a safe distance when following another vehicle. ； in, express The position of the car in front at all times. Indicates the length of the vehicle in front. Indicates a fixed time interval. express The speed of the car in front at any given time.

7. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 1, characterized in that: In step S4, the vehicle is controlled to travel along the optimal driving path by controlling the longitudinal and lateral trajectories of its movement. The method includes: The PID control algorithm is used to improve the vehicle's aiming ability, and the improved algorithm controls the longitudinal trajectory of the vehicle. The improved aiming algorithm is used to compensate for lateral deviation at the aiming point when the vehicle is turning. The formula for the improved aiming algorithm is as follows: ； in, The length of the axle. It is the distance between the center point of the rear axle of the vehicle and the aiming point. The current vehicle heading and the distance from the center point of the rear axle to the aiming point. The angle between the lines, This represents the deviation between the expected lateral distance and the current actual distance at the pre-aiming point. This represents the deviation between the expected lateral movement rate and the actual lateral movement rate at the aiming point. This indicates the deviation of the vehicle from the nearest point on the reference trajectory in terms of lateral distance; A model predictive control algorithm is used to control the lateral trajectory of the vehicle. The model predictive control algorithm has an objective function, the formula of which is: ； in, Represents the state vector of the vehicle , It is the first cost matrix between the prediction model and the expected trajectory. It is the first cost matrix for optimizing the control variables. This represents the second cost matrix between the prediction model and the desired trajectory. This represents the state control variable for predicting the expected value in the time domain. This represents the current state control variable in the prediction time domain. This represents the desired state control variable during the sampling time. This represents the state control variable within the sampling time. express Control variables at time, This represents the second cost matrix for optimizing the control variables. Used to eliminate deviation values Used to ensure the stability of vehicle operation.

8. The autonomous driving decision-making and control method based on roadside fusion perception according to claim 7, characterized in that: In the improved pre-aiming algorithm formula The constraints are: ； in, Indicates the proportionality coefficient, Indicates the integral coefficient, Represents the differential coefficient. Indicates the time.

Images