Driving test scene vehicle escape method based on grid map

Abstract

The invention relates to a driving test scene vehicle escape method based on a grid map. The driving test scene vehicle escape method comprises the steps that S1, the posture, the speed, the angular speed and the driving track of a vehicle are calculated; s2, according to the real-time dynamic data of the vehicle, generating a grid map of vehicle follow-up occupation; s3, carrying out collision detection on the grid map; whether a potential collision risk exists or not is judged by analyzing the mutual relation between the vehicle following body occupation grid map and the surrounding environment; s4, based on a collision detection result, generating a detrapping grid map; S5, projecting the outline of the vehicle to the pose of any point; and step S6, controlling the escape behavior of the vehicle through the escape state machine model and the decision. The objective of the invention is to solve the two major problems in the practical application of the traditional escape algorithm: the traditional algorithm lacks the effective sensing ability to the surrounding environment in the escape process, and the misjudgment is easy to generate under the specific terrain conditions such as ramps and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of decision-making and planning, and in particular to a method for vehicle escape from distress in a driving test scenario based on a grid map. Background Art

[0002] In the scenario of driving training, driving schools usually set up an electronic fence area so that they can better monitor and manage the vehicle's driving range when students are practicing driving. At the same time, driving test robots are usually equipped with high-precision positioning equipment and a variety of radar sensors. The purpose of these devices is to monitor the environment around the vehicle in real time, so as to effectively avoid collisions during driving and ensure the safety of the vehicle. However, when the vehicle enters the electronic fence area, if the student's operation is improper, it may cause the vehicle to further collide with surrounding obstacles (such as curbs, telephone poles, etc.), causing traffic accidents. Therefore, after the vehicle enters the electronic fence area, how to effectively guide the students to safely drive away from the potential danger area has become one of the important measures to ensure the safe operation of the driving test robot.

[0003] At present, in the field of driving test robots, the escape algorithm usually adopts a simple method, that is, according to the gear used by the vehicle when entering the electronic fence, it decides to use the reverse gear to exit the fence. Although this method can work in some cases, it does not take into account the obstacle information around the vehicle, nor does it take into account the possible slipping of the vehicle when driving on a slope. Therefore, when faced with complex driving scenarios, this simple escape algorithm is prone to cause the vehicle to collide with surrounding obstacles (whether static obstacles or other vehicles driving around), thereby increasing safety risks.

[0004] In order to improve the safety and reliability of the driving test robot, it is particularly necessary to develop an intelligent escape algorithm that can comprehensively consider the vehicle's surrounding environment information and the situation of sliding on a slope. Summary of the invention

[0005] In view of the shortcomings of the current relevant existing technologies, the present invention utilizes the ROS (Robot Operating System) development platform and creatively proposes a vehicle escape method for a driving test scenario based on a grid map. The present invention aims to solve two major problems faced by traditional escape algorithms in practical applications: one is that traditional algorithms lack effective perception of the surrounding environment during the escape process, and the other is that they are prone to misjudgment under specific terrain conditions such as ramps.

[0006] To achieve the above object, the present invention provides a method for escaping a vehicle in a driving test scenario based on a grid map, which at least comprises the following steps:

[0007] Step S1: Calculate the vehicle's position, speed, angular velocity and driving trajectory; this step is the basis of the entire escape method, and provides necessary information support for subsequent processing by obtaining real-time dynamic data of the vehicle;

[0008] Step S2: Generate a grid map of the vehicle's position based on the vehicle's real-time dynamic data; the grid map can accurately reflect the vehicle's position and space in the current environment, providing basic data for subsequent collision detection and escape decisions;

[0009] Step S3: perform collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determine whether there is a potential collision risk; this step can effectively identify obstacles that the vehicle may encounter during driving, providing an important reference for escaping from difficulties;

[0010] Step S4: Generate an escape grid map based on the result of collision detection; the purpose of this step is to find a safe escape path to ensure that the vehicle can successfully avoid obstacles and complete the escape task in complex driving test scenarios;

[0011] Step S5: Project the vehicle contour to any point; this step is to more accurately simulate the motion state of the vehicle during the escape process to ensure the feasibility and safety of the escape path.

[0012] Step S6: Control the vehicle's escape behavior through the escape state machine model and decision-making; this step comprehensively considers the vehicle's dynamic data, collision detection results and escape grid map, and generates the optimal escape strategy through intelligent algorithms to ensure that the vehicle can complete the escape task safely and efficiently.

[0013] Furthermore, in step S1, the vehicle posture is solved in the following way: according to the positioning and orientation device installed at the origin of the vehicle base_link coordinate system, the output information of the point orientation device, including east e, north n, celestial u, heading yaw, pitch, and roll roll, is converted into the geometry_msgs / PoseStamped type in the ros standard message format, which can accurately represent the real-time position and posture of the vehicle base_link relative to the map coordinate system map; wherein the position information corresponds to the northeast celestial (e, n, u), and the orientation is a quaternion converted from the Euler angle (roll, pitch, yaw), which is used to represent the posture information of the vehicle;

[0014] The conversion formula is:

[0015] q x=cos(roll / 2)cos(pitch / 2)cos(yaw / 2)+sin(roll / 2)sin(pitch / 2)sin(yaw / 2)

[0016] q y =sin(roll / 2)cos(pitch / 2)cos(yaw / 2)-cos(roll / 2)sin(pitch / 2)sin(yaw / 2)

[0017] q z =cos(roll / 2)sin(pitch / 2)cos(yaw / 2)+sin(roll / 2)cos(pitch / 2)sin(yaw / 2)

[0018] q w =cos(roll / 2)cos(pitch / 2)sin(yaw / 2)-sin(roll / 2)sin(pitch / 2)cos(yaw / 2)

[0019] The vehicle linear speed is calculated as:

[0020] According to the vehicle's posture, sampling is performed every 0.05s to calculate the vehicle's linear velocity (base_link coordinate system X-axis velocity) and angular velocity (base_link coordinate system Z-axis angular velocity);

[0021] The linear velocity calculation method is:

[0022]

[0023] Among them (e t+1 , n t+1 ) and (e t , n t ) are the east and north directions at time t+1 and t respectively, dt is the sampling time, sign is the sign function (-1 / 1, determined by whether the vehicle running direction is consistent with the vehicle body direction); its calculation method is:

[0024]

[0025] Where yaw is the heading angle of the vehicle (the angle between the x-axis of the base_link coordinate system and the x-axis of the map coordinate system, right-handed system);

[0026] The vehicle angular velocity is calculated as:

[0027] The angular velocity is calculated by sampling the heading angle and calculating the difference between the two heading angles. The calculation formula is:

[0028]

[0029] Among them yaw t and yaw t+1 are the heading angles at time t and t+1 respectively, dt is the sampling time, sign is the sign function, and its expression is as follows:

[0030]

[0031] Convert the solved linear velocity and angular velocity into the geometry_msgs / TwistSatmped message format of ROS, which mainly includes the coordinate system, the linear velocity of the X axis, and the angular velocity of the Z axis;

[0032] The vehicle running trajectory is calculated as follows:

[0033] The vehicle trajectory in the vehicle body coordinate system base_link is calculated based on the vehicle's front wheel angle, speed, and gear position, and converted into the ros nav_msgs / Path message format to determine whether there are obstacles on the driving trajectory when the vehicle is in gear to escape, so as to decide whether to suspend or resume the escape; the vehicle's driving trajectory is mainly calculated based on the bicycle model;

[0034] Vehicle turning radius calculation

[0035] The turning radius at the center of the vehicle's rear axle is calculated as:

[0036] R=wheelBase / tan(abs(steerAngle))

[0037] Among them, wheelBase is the vehicle wheelbase, steerAngle is the vehicle front wheel steering angle;

[0038] Calculation of vehicle travel direction

[0039] The driving test vehicle may slip while driving at low speed. Therefore, the vehicle's direction should not be determined entirely by the gear position, but by the vehicle's linear speed and gear position. The method for calculating the vehicle's driving direction in this patent is to determine the direction based on the speed when the absolute linear speed is greater than 0.1m / s; when the absolute speed is less than 0.1m / s (at low speed, the positioning has a certain drift and is unreliable), it is considered to be in an absolute stationary state, and the gear position is used to determine:

[0040] When vx>0.1m / s, the driving direction moveDirection=1, indicating forward movement;

[0041] When vx<-0.1m / s, the driving direction moveDirection=0, which means moving backward;

[0042] When abs(vx)<0.1m / s and the gear is in the forward gear, the driving direction moveDirection=1, indicating forward movement;

[0043] When abs(vx)<0.1m / s and the gear is in reverse gear, the driving direction moveDirection=-1, indicating forward movement;

[0044] When abs(vx)<0.1m / s and the gear is in neutral, the driving direction moveDirection=0, indicating stationary;

[0045] Calculation of vehicle trajectory

[0046] The vehicle driving trajectory calculates the number of points to be calculated based on the arc length pathLength and the track interval interval to be fitted, and calculates the x, y, yaw of each point one by one, and converts them into nav_msgs / Path messages for publication;

[0047] The number of points to be calculated is num = int(pathLength / interval), so for any i∈[0,num], the calculation method of x, y, yaw is:

[0048] When steerAngle>=0.0 and moveDirection=1:

[0049] yaw i =pathInterval*i / R

[0050] x i =Rsin(yaw i )

[0051] y i =R(1-cos(yaw i ))

[0052] When steerAngle>=0.0 and moveDirection=-1:

[0053] yaw i =-pathInterval*i / R

[0054] x i =Rsin(yaw i )

[0055] y i =R(1-cos(yaw i ))

[0056] When steerAngle<=0.0 and moveDirection=1:

[0057] yaw i =-pathInterval*i / R

[0058] x i =-Rsin(yaw i )

[0059] y i =R(cos(yaw i )-1)

[0060] When steerAngle<=0.0 and moveDirection=-1:

[0061] yaw i =pathInterval*i / R

[0062] x i =-Rsin(yaw i )

[0063] y i =R(cos(yaw i )-1)

[0064] Based on the above solution of the position and posture of the i-th point, the heading angle is converted into a quaternion according to 1.1, and converted according to the message format of nav_msgs / Path below to generate the predicted trajectory at base_link.

[0065] Furthermore, in step S2, based on the real-time dynamic data of the vehicle, a grid map of the vehicle's body position is generated by discretizing the electronic fence collected in advance and generating a global electronic fence point cloud, and projecting the electronic fence point cloud from the map coordinate system to the vehicle base_link coordinate system, and filtering the electronic fence point cloud to retain the point cloud within a certain range in front, behind, left and right of the vehicle; at the same time, the point cloud of this part is fused with the point cloud of the vehicle's lidar sensor to generate a real-time point cloud with a fused coordinate system of base_link, and the point cloud is projected onto a two-dimensional plane to generate a real-time grid semantic map.

[0066] Furthermore, the implementation of generating point cloud by electronic fence is as follows:

[0067] Electronic fence analysis and point cloud generation

[0068] Each electronic fence is generated by a number of points to generate a polyline, and each electronic fence is interpolated at a certain interval to generate a point cloud format of sensor_msgs / PointCloud2 of ROS;

[0069] The interpolation method is:

[0070] (1) For each point on a line, in the order of (xn,yn), (xn+1,yn+1), a series of points are linearly inserted in the middle according to a fixed length; (2) Calculate the distance between two points; (3) Calculate how many points need to be generated; (4) Linear interpolation;

[0071] Geo-fence point cloud transformation

[0072] The electronic fence point cloud is in the map coordinate system and needs to be converted to the vehicle base_link coordinate system. Use the pcl_ros::transformPointCloud tool of ros to convert the electronic fence point cloud from the map coordinate system to the base_link coordinate system.

[0073] Point cloud pass-through filtering

[0074] Focus on a part of the electronic fence point cloud around the vehicle, use the pcl::PassThrough tool, set xmax, xmin, ymax, ymin for pass-through filtering, and retain a part of the electronic fence point cloud in front, behind, left and right of the vehicle;

[0075] Point cloud fusion

[0076] After the point cloud published by the sensor is also converted to the base_link coordinate system, it is spliced ​​with the electronic fence point cloud after the direct filtering to generate an electronic fence point cloud for generating a local grid map;

[0077] Real-time placeholder grid map generation

[0078] The grid map is in the message format of nav_mgs / OccupancgGrid of ros, which mainly includes the length and width of the map, the map resolution, the position of the map origin, and the value of each grid point; the index of the grid points is numbered from left to right and from bottom to top. The value of each index point is 0, which means that there is no obstacle at the grid point, and 100, which means that there is an obstacle at the grid point;

[0079] The process of generating raster maps from point clouds includes:

[0080] (1) Traverse all point cloud points and obtain the row index and column index of each point in the grid map:

[0081] mx=int((x-map_orin_x) / map_resolution);

[0082] my=int((y-map_orin_y) / map_resolution); where (map_orin_x,map_orin_y) is the origin of the grid map,

[0083] (2) Calculate the map index

[0084] Index = my*map_width+mx; where map_width is the width of the map;

[0085] (3) The index of the map is assigned a value of 100, indicating that there is an obstacle.

[0086] Furthermore, the specific implementation of the grid map collision detection in step S3 is as follows: this part discretizes the vehicle, and calculates the index of each discrete point in the grid, and determines whether there is an obstacle within the vehicle body contour according to the value of the index in the grid map;

[0087] Vehicle outline discretization

[0088] The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle plane size can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (rear axle to rear of vehicle); the discretization process is:

[0089] (1) Define the left, right, top, and bottom boundaries of the vehicle:

[0090] (2) Discretize the horizontal and vertical directions:

[0091] Collision detection between vehicle outline and grid map

[0092] For the body contour point, calculate the index value of the point in the grid map and obtain the value of the index value. If the index value is 100, it means that a collision has occurred; otherwise, no collision has occurred.

[0093] Furthermore, in step S4, based on the result of collision detection, an escape grid map is generated, which is implemented as follows: when the vehicle speed is 0 and a collision between the contour and the grid map is detected, a signal needs to be sent to a local map generation module to generate an escape map, which removes obstacles within the vehicle body and the extended contour; the escape map will be used to determine whether there are obstacles within the path range when starting the escape, and if there are no obstacles, the escape will continue until the vehicle body contour does not expand with its own grid map, thereby completing the escape; if there is an obstacle, the escape will be suspended and wait for the obstacle to disappear.

[0094] Further, specifically including body extension contour generation

[0095] Steps:

[0096] Generate extended body contours;

[0097] Escape map generation;

[0098] Assign a current grid map as an escape map to discretize the extended contour of the vehicle body, calculate the index value of the discrete points of the contour in the escape map, and assign 0 to the index value in the escape map.

[0099] Furthermore, the vehicle contour is projected to any point, which can be achieved as follows:

[0100] Convert the discrete contour of the vehicle at base_link to the trajectory point at base_link to determine whether the vehicle will collide with the placeholder grid within the trajectory range;

[0101] For any point p(px,py) of the discrete contour, its new coordinates at any position (x,y,yaw) are:

[0102] new_px=x+px*cos(yaw)-py*sin(yaw)

[0103] new_py=y+px*cos(yaw)-py*sin(yaw)

[0104] Furthermore, in step S6, the vehicle's escape behavior is controlled through the escape state machine model and decision making, which includes the following:

[0105] Define the escape state machine model: the escape state is Default, indicating a normal driving state;

[0106] (1) In the Default state, when the method detects that the vehicle outline collides with the vehicle body occupancy grid map and the speed is 0, the state is converted to ObstacleBrake, and based on the speed information, the speed information within 2s is extracted to determine the movement direction brakeTriggerDirection when the emergency brake is triggered (whether the speed changes from positive to 0 (brakeTriggerDirection=1) or from negative to 0 (brakeTriggerDirection=-1) within 2s); and the vehicle's position and posture at this time are obtained, and the signal is sent to the escape map module to generate an escape map;

[0107] (2) When the state is ObstacleBrake, if it is detected that the vehicle is in neutral with the handbrake pulled, it enters the ObstacleRelease state;

[0108] (3) When the state is ObstacleFaraway or ObstacleFarawayStop, the generated vehicle body contour generates the vehicle's motion trajectory points, converts them point by point, and performs collision detection. If there is no obstacle within the 2.0-meter predicted trajectory range, the state is ObstacleFaraway, otherwise the state is

[0109] ObstacleFarawayStop;

[0110] (4) If the state is ObstacleFaraway and it is detected that there is no collision between the vehicle outline and the real-time occupancy grid map, the system will re-enter Default, indicating that the escape is complete.

[0111] The present invention adopts the above technical solution, which has at least the following beneficial effects:

[0112] 1. The method provided by the present invention solves the problem that the information of obstacles around the vehicle is not considered in the traditional related technologies, and the vehicle slipping scene on the ramp is considered, which easily leads to the collision between the vehicle and the surrounding obstacles (static obstacles and surrounding vehicles) in complex scenes. The technical solution provided by the present invention is used to develop the ROS (Robot Operating System) platform, and a grid map-based escape algorithm is creatively proposed. The algorithm aims to solve the two major problems faced by traditional escape algorithms in practical applications: one is that traditional algorithms lack effective perception of the surrounding environment during the escape process, and the other is that they are prone to misjudgment under specific terrain conditions such as ramps.

[0113] 2. The escape algorithm of the present invention can obtain and analyze the environmental information around the robot in real time by constructing a detailed grid map. This grid map divides the environment into multiple small cells, each cell represents a certain spatial area, and records the terrain features and obstacle information in the area. In this way, the robot can identify and avoid obstacles more accurately, thereby improving the success rate of escaping.

[0114] 3. The present invention not only solves the problems of traditional escape algorithms in environmental perception and slope misjudgment through the grid map-based escape algorithm implemented on the ROS development system, but also provides more reliable and efficient technical support for the robot's autonomous navigation and escape in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0115] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0116] Figure 1 This is a flow chart of a method for escaping a vehicle in a driving test scenario according to the present invention;

[0117] Figure 2 It is a schematic diagram of the straight-through filtering point cloud of the present invention;

[0118] Figure 3 is a schematic diagram of the present invention containing an electronic fence point cloud;

[0119] Figure 4 is a schematic diagram of a grid map of the present invention;

[0120] Figure 5 It is a schematic diagram of a local grid map of the present invention;

[0121] Figure 6 It is a schematic diagram of the obstacle grid map of the present invention. DETAILED DESCRIPTION

[0122] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Instead, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0123] like Figure 1 As shown, this embodiment provides a method for escaping a vehicle in a driving test scenario based on a grid map, which at least includes the following steps:

[0124] Step S1: Calculate the vehicle's position, speed, angular velocity and driving trajectory; this step is the basis of the entire escape method, and provides necessary information support for subsequent processing by obtaining real-time dynamic data of the vehicle;

[0125] Step S2: Generate a grid map of the vehicle's position based on the vehicle's real-time dynamic data; the grid map can accurately reflect the vehicle's position and space in the current environment, providing basic data for subsequent collision detection and escape decisions;

[0126] Step S3: perform collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determine whether there is a potential collision risk; this step can effectively identify obstacles that the vehicle may encounter during driving, providing an important reference for escaping from difficulties;

[0127] Step S4: Generate an escape grid map based on the result of collision detection; the purpose of this step is to find a safe escape path to ensure that the vehicle can successfully avoid obstacles and complete the escape task in complex driving test scenarios;

[0128] Step S5: Project the vehicle contour to any point; this step is to more accurately simulate the motion state of the vehicle during the escape process to ensure the feasibility and safety of the escape path.

[0129] Step S6: Control the vehicle's escape behavior through the escape state machine model and decision-making; this step comprehensively considers the vehicle's dynamic data, collision detection results and escape grid map, and generates the optimal escape strategy through intelligent algorithms to ensure that the vehicle can complete the escape task safely and efficiently.

[0130] 1: In this embodiment, the vehicle position, speed, angular velocity, and driving trajectory are calculated as follows:

[0131] 1.1: Vehicle pose calculation

[0132] According to the positioning and orientation device installed at the origin of the vehicle base_link coordinate system, the output information of the point orientation device (generally including east e, north n, celestial u, heading yaw, pitch, roll) is converted into the ros standard message geometry_msgs / PoseStamped format, which can accurately represent the real-time position and posture of the vehicle base_link relative to the map coordinate system map. The position information corresponds to the northeast celestial (e, n, u), and the orientation is the quaternion converted from the Euler angle (roll, pitch, yaw).

[0133] The conversion formula is:

[0134] q x =cos(roll / 2)cos(pitch / 2)cos(yaw / 2)+sin(roll / 2)sin(pitch / 2)sin(yaw / 2)

[0135] q y=sin(roll / 2)cos(pitch / 2)cos(yaw / 2)-cos(roll / 2)sin(pitch / 2)sin(yaw / 2)

[0136] q z =cos(roll / 2)sin(pitch / 2)cos(yaw / 2)+sin(roll / 2)cos(pitch / 2)sin(yaw / 2)

[0137] q w =cos(roll / 2)cos(pitch / 2)sin(yaw / 2)-sin(roll / 2)sin(pitch / 2)cos(yaw / 2)

[0138] The base_link coordinate system of an AI vehicle is usually a fixed reference coordinate system used to describe the position and posture of the vehicle in space. It serves as a reference for positioning other vehicle components or sensors, ensuring that the positioning system of the entire vehicle can accurately perform spatial positioning. In the fields of robotics and autonomous driving, the base_link coordinate system is the basis for building vehicle models and performing navigation and control.

[0139] 1.2: Calculation of vehicle linear velocity and angular velocity in this embodiment

[0140] 1.2.1: Calculation of vehicle linear speed

[0141] According to the vehicle's position and posture in 1.1, sampling is performed every 0.05s to calculate the vehicle's linear velocity (base_link coordinate system X-axis velocity) and angular velocity (base_link coordinate system Z-axis angular velocity).

[0142] The linear velocity calculation method is:

[0143]

[0144] Among them (e t+1 , n t+1 ) and (e t , n t ) are the east and north directions at time t+1 and t respectively, dt is the sampling time, sign is the sign function (-1 / 1, determined by whether the vehicle's running direction is consistent with the vehicle's body direction). The calculation method is:

[0145]

[0146] Where yaw is the heading angle of the vehicle (the angle between the x-axis of the base_link coordinate system and the x-axis of the map coordinate system, right-handed system).

[0147] 1.2.2 Calculation of angular velocity in this embodiment

[0148] The angular velocity is calculated by sampling the heading angle and calculating the difference between the two heading angles. The calculation formula is:

[0149]

[0150] Among them yaw t and yaw t+1 are the heading angles at time t and t+1 respectively, dt is the sampling time, sign is the sign function, and its expression is as follows:

[0151]

[0152] Convert the solved linear velocity and angular velocity into the geometry_msgs / TwistSatmped message format of ROS, which mainly includes the coordinate system, the linear velocity of the X axis, and the angular velocity of the Z axis.

[0153] 1.3 Vehicle running trajectory calculation in this embodiment

[0154] This chapter mainly calculates the vehicle trajectory in the vehicle body coordinate system base_link based on the vehicle's front wheel angle, speed, and gear position, and converts it into the ros nav_msgs / Path message format to determine whether there are obstacles on the driving trajectory when the vehicle is in gear to escape, so as to decide whether to suspend or resume the escape. The vehicle's driving trajectory is mainly calculated based on the bicycle model.

[0155] 1.3.1 Calculation of vehicle turning radius

[0156] The turning radius at the center of the vehicle's rear axle is calculated as:

[0157] R=wheelBase / tan(abs(steerAngle))

[0158] Among them, wheelBase is the vehicle wheelbase, and steerAngle is the vehicle's front wheel steering angle.

[0159] 1.3.2 Calculation of vehicle travel direction

[0160] The driving test vehicle may slip while driving at low speed. Therefore, the vehicle's direction should not be determined entirely by the gear position, but by the vehicle's linear speed and gear position. The method for calculating the vehicle's driving direction in this patent is to determine the direction based on the speed when the absolute linear speed is greater than 0.1m / s; when the absolute speed is less than 0.1m / s (at low speed, the positioning has a certain drift and is unreliable), it is considered to be in an absolute stationary state, and the gear position is used to determine:

[0161] When vx>0.1m / s, the driving direction moveDirection=1, indicating forward movement;

[0162] When vx<-0.1m / s, the driving direction moveDirection=0, which means moving backward;

[0163] When abs(vx)<0.1m / s and the gear is in the forward gear, the driving direction moveDirection=1, indicating forward movement;

[0164] When abs(vx)<0.1m / s and the gear is in reverse gear, the driving direction moveDirection=-1, indicating forward movement;

[0165] When abs(vx)<0.1m / s and the gear is in neutral, the driving direction moveDirection=0, indicating stationary;

[0166] 1.3.3 Calculation of vehicle driving trajectory in this embodiment

[0167] The vehicle driving trajectory calculates the number of points that need to be calculated based on the arc length pathLength and the trajectory interval distance interval that need to be fitted, and calculates the x, y, yaw of each point one by one, and converts them into nav_msgs / Path messages for publication.

[0168] The number of points to be calculated is num = int(pathLength / interval), so for any i∈[0,num], the calculation method of x, y, yaw is:

[0169] When steerAngle>=0.0 and moveDirection=1:

[0170] yaw i =pathInterval*i / R

[0171] x i =Rsin(yaw i )

[0172] y i =R(1-cos(yaw i ))

[0173] When steerAngle>=0.0 and moveDirection=-1:

[0174] yaw i =-pathInterval*i / R

[0175] x i =Rsin(yaw i )

[0176] y i =R(1-cos(yaw i ))

[0177] When steerAngle<=0.0 and moveDirection=1:

[0178] yaw i =-pathInterval*i / R

[0179] x i =-Rsin(yaw i )

[0180] y i =R(cos(yaw i )-1)

[0181] When steerAngle<=0.0 and moveDirection=-1:

[0182] yaw i =pathInterval*i / R

[0183] x i =-Rsin(yaw i )

[0184] y i =R(cos(yaw i )-1)

[0185] Based on the above solution of the position and posture of the i-th point, the heading angle is converted into a quaternion according to 1.1, and converted according to the message format of nav_msgs / Path below to generate the predicted trajectory at base_link.

[0186] 2. Vehicle-related occupancy grid map generation in this embodiment

[0187] The main function is to discretize the electronic fence collected in advance and generate a global electronic fence point cloud, project the electronic fence point cloud from the map coordinate system to the vehicle base_link coordinate system, and filter the electronic fence point cloud to retain the point cloud within a certain range in front, behind, left and right of the vehicle; at the same time, fuse this part of the point cloud with the point cloud of the vehicle lidar sensor to generate a fused real-time point cloud under the base_link coordinate system, and project the point cloud to a two-dimensional plane to generate a real-time raster semantic map.

[0188] 2.1 Electronic fence generates point cloud

[0189] 2.1.1 Electronic fence analysis and point cloud generation

[0190] Each electronic fence is a polyline generated by several points.

[0191] Therefore, each electronic fence is interpolated at a certain interval and the point cloud format of sensor_msgs / PointCloud2 of ROS is generated.

[0192] The interpolation method is:

[0193] For each point on a line, in the order of (xn,yn), (xn+1,yn+1), a series of points are linearly inserted in the middle according to a fixed length (such as 0.1).

[0194] (2) Calculate the distance between two points dis = sqrt((xn+1-xn)**2+(yn+1-yn)**2)

[0195] (3) The calculation needs to generate several points n = 1 + dis / resolution (for example 0.1)

[0196] (4) Linear interpolation for (i = 0; i <n;i++){

[0197] x=xn+(xn+1-xn)*i / n-1;

[0198] y=yn+(yn+1-yn)*i / n-1;

[0199] }

[0200] 2.1.2 Electronic Fence Point Cloud Transformation

[0201] Since the electronic fence point cloud is in the map coordinate system, it needs to be converted to the vehicle base_link coordinate system. Therefore, the pcl_ros::transformPointCloud tool of ros is used to convert the electronic fence point cloud from the map coordinate system to the base_link coordinate system.

[0202] 2.1.3 Point cloud direct filtering

[0203] like Figure 2 As shown in the figure, since we only need to focus on a part of the electronic fence point cloud around the vehicle, the pcl::PassThrough tool is used to set xmax, xmin, ymax, and ymin for pass-through filtering, and only a part of the electronic fence point cloud in front, behind, left, and right of the vehicle is retained.

[0204] 2.2 As Figure 3 As shown, in this embodiment, point cloud fusion

[0205] In this embodiment, the point cloud published by the sensor is also converted into the base_link coordinate system, and then spliced ​​with the electronic fence point cloud after the through filtering to generate an electronic fence point cloud for generating a local grid map.

[0206] 2.3 In this embodiment, the real-time placeholder grid map is generated as follows:

[0207] like Figure 4 As shown in the figure, the grid map is in the message format of nav_mgs / OccupancgGrid of ros, which mainly includes the length and width of the map, the map resolution, the map origin position and the value of each grid point. The index of the grid points is numbered from left to right and from bottom to top. The value of each index point is 0, which means there is no obstacle at the grid point, and 100, which means there is an obstacle at the grid point.

[0208] like Figure 5 As shown, the process of generating a grid map from a point cloud in this embodiment includes:

[0209] Traverse all point cloud points and obtain the row index and column index of each point in the grid map:

[0210] mx=int((x-map_orin_x) / map_resolution);

[0211] my=int((y-map_orin_y) / map_resolution);

[0212] Where (map_orin_x,map_orin_y) is the origin of the grid map.

[0213] Calculate the index of the map

[0214] Index = my * map_width + mx;

[0215] Where map_width is the width of the map.

[0216] The index of the map is assigned a value of 100, indicating that there is an obstacle.

[0217] 3 In this embodiment, the grid map collision detection is as follows:

[0218] This part mainly discretizes the vehicle and calculates the index of each discrete point in the grid. It determines whether there are obstacles within the vehicle body contour based on the value of the index in the grid map.

[0219] 3.1 Vehicle outline discretization

[0220] The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle plane size can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (from rear axle to rear of vehicle). Therefore, the discretization process is:

[0221] Define the left, right, top and bottom boundaries of the vehicle:

[0222] Left = -1.0*baseLink2Back;

[0223] Right=vehicleLength-baseLink2Back;

[0224] Top = vehicleWidth / 2.0;

[0225] Bottom=-1.0*vehicleWidth / 2.0;

[0226] Discretize the horizontal and vertical directions:

[0227] The pseudo code is:

[0228] PointVector = []

[0229] pointX = left;

[0230] While(pointX <right+resolution):

[0231] pointY=bottom

[0232] While(pointY <top+resolution)

[0233] PointVector.pushback(pointX,pointY)

[0234] pointY=pointY+resolution

[0235] pointX=pointX+resolution

[0236] The resolution is generally slightly smaller than the map resolution.

[0237] 3.2 Collision detection between vehicle outline and grid map in this embodiment

[0238] Calculate the index value of the body contour point in the grid map according to 2.3, and obtain the value of the index value. If the index value is 100, it means that a collision has occurred; otherwise, no collision has occurred.

[0239] 4. Escape Grid Map

[0240] When the vehicle speed is 0 and the contour is detected to collide with the grid map, a signal needs to be sent to the local map generation module to generate an escape map, which removes the obstacles within the vehicle body and the extended contour. The escape map will be used to start the escape path to see if there are obstacles. If there are no obstacles, the escape will continue until the vehicle body contour does not expand with its own grid map to complete the escape; if there are obstacles, the escape will be suspended and wait for the obstacle to disappear.

[0241] 4.1 Body extension profile generation in this embodiment

[0242] Generate an extended vehicle body contour according to 3.1 (the contour is extended to deduct more grids with obstacles around the vehicle to facilitate escape). The only difference is that the values ​​of Left, Right, Bottom and Top need to be modified.

[0243] Left=-1.0*baseLink2Back-Extend

[0244] Right=vehicleLength-baseLink2Back+Extend

[0245] Top=vehicleWidth / 2.0+Extend

[0246] Bottom=-1.0*vehicleWidth / 2.0-Extend

[0247] 4.2 Generation of escape map in this embodiment

[0248] like Figure 6 As shown, a current grid map is assigned as an escape map, the vehicle body extension contour is discretized according to 3.1, and the index value of the discrete points of the contour in the escape map is calculated according to 2.3, and the index value in the escape map is assigned 0. The escape grid map clears obstacles within a certain range around the vehicle

[0249] 5: The projection of the vehicle contour to any point in this embodiment

[0250] This function mainly converts the discrete contour of the vehicle at base_link to the trajectory point at base_link (generated according to 1.3.3) to determine whether the vehicle will collide with the placeholder grid within the trajectory range.

[0251] For any point p(px,py) of the discrete contour, its new coordinates at any position (x,y,yaw) are:

[0252] new_px=x+px*cos(yaw)-py*sin(yaw)

[0253] new_py=y+px*cos(yaw)-py*sin(yaw)

[0254] 6: Escape state machine model and decision making

[0255] Define the escape state machine model:

[0256] / *State machine model* /

[0257] enum ExtricateState{

[0258] / *Default state* /

[0259] Default_=0,

[0260] / *Obstacle brake* /

[0261] ObstacleBrake_=1,

[0262] / *Release the brake in neutral position* /

[0263] ObstacleRelease_=2,

[0264] / *Escape interrupt mode* /

[0265] ObstacleFarawayStop_=3,

[0266] / *Escape mode* /

[0267] ObstacleFaraway_=4,

[0268] };

[0269] And by default, the escape status is Default, indicating normal driving status.

[0270] (1) In the Default state, when the vehicle contour is detected to collide with the vehicle body occupancy grid map according to the method in 3.2 and the speed is 0 (the longitudinal control module will issue an emergency brake when it detects danger), the state is converted to ObstacleBrake, and according to the speed information in 1.2.1, the speed information within 2s is extracted to determine the movement direction brakeTriggerDirection when the emergency brake is triggered (whether the speed changes from positive to 0 (brakeTriggerDirection=1) or from negative to 0 (brakeTriggerDirection=-1) within 2s); and the vehicle's position and posture at this time are obtained, and the signal is sent to the escape map module to generate an escape map;

[0271] (2) When the state is ObstacleBrake, if it is detected that the vehicle is in the handbrake + neutral gear (the system defaults to release the brakes when the handbrake is pulled + neutral gear is shifted), it will enter the ObstacleRelease state;

[0272] (3) When the state is ObstacleFaraway or ObstacleFarawayStop, the vehicle body contour generated in 3.1 is converted according to the method in 1.3.3, and the points are converted point by point according to the method in 5. The collision detection is performed with the escape map according to the method in 3.2. If there is no obstacle within the 2.0-meter predicted trajectory range, the state is ObstacleFaraway, otherwise the state is ObstacleFarawayStop;

[0273] (4) If the state is ObstacleFaraway and it is detected that there is no collision between the vehicle outline and the real-time occupancy grid map, the system will re-enter Default, indicating that the escape is complete.

[0274] Although the embodiments of the present invention have been shown and described above, it will be appreciated that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, substitute and vary the above embodiments within the scope of the present invention.

Claims

1. A method for escaping a vehicle in a driving test scenario based on a grid map, characterized in that: It at least includes the following steps: Step S1: Calculate the vehicle's position, speed, angular velocity and driving trajectory; this step is the basis of the entire escape method, and provides necessary information support for subsequent processing by obtaining real-time dynamic data of the vehicle; Step S2: Generate a grid map of the vehicle's position based on the vehicle's real-time dynamic data; the grid map can accurately reflect the vehicle's position and space in the current environment, providing basic data for subsequent collision detection and escape decisions; Step S3: perform collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determine whether there is a potential collision risk; this step can effectively identify obstacles that the vehicle may encounter during driving, providing an important reference for escaping from difficulties; Step S4: Generate an escape grid map based on the result of collision detection; the purpose of this step is to find a safe escape path to ensure that the vehicle can successfully avoid obstacles and complete the escape task in complex driving test scenarios; Step S5: Project the vehicle contour to any point; this step is to more accurately simulate the motion state of the vehicle during the escape process to ensure the feasibility and safety of the escape path. Step S6: Control the vehicle's escape behavior through the escape state machine model and decision-making; this step comprehensively considers the vehicle's dynamic data, collision detection results and escape grid map, and generates the optimal escape strategy through intelligent algorithms to ensure that the vehicle can complete the escape task safely and efficiently.

2. The method according to claim 1, characterized in that: In step S1, the vehicle posture is solved as follows: according to the positioning and orientation device installed at the origin of the vehicle base_link coordinate system, the output information of the point orientation device, including east e, north n, celestial u, heading yaw, pitch, and roll roll, is converted into the geometry_msgs / PoseStamped type in the ros standard message format. This message can accurately represent the real-time position and posture of the vehicle base_link relative to the map coordinate system map; the position information corresponds to the northeast celestial (e, n, u), and the orientation is a quaternion converted from the Euler angle (roll, pitch, yaw), which is used to represent the vehicle's posture information; The conversion formula is: q x =cos(roll / 2)cos(pitch / 2)cos(yaw / 2)+sin(roll / 2)sin(pitch / 2)sin(yaw / 2) q y =sin(roll / 2)cos(pitch / 2)cos(yaw / 2)-cos(roll / 2)sin(pitch / 2)sin(yaw / 2) q z =cos(roll / 2)sin(pitch / 2)cos(yaw / 2)+sin(roll / 2)cos(pitch / 2)sin(yaw / 2) q w =cos(roll / 2)cos(pitch / 2)sin(yaw / 2)-sin(roll / 2)sin(pitch / 2)cos(yaw / 2) The vehicle linear speed is calculated as: According to the vehicle's posture, sampling is performed every 0.05s to calculate the vehicle's linear velocity (base_link coordinate system X-axis velocity) and angular velocity (base_link coordinate system Z-axis angular velocity); The linear velocity calculation method is: Among them (e t+1 , n t+1 ) and (e t , n t ) are the east and north directions at time t+1 and t respectively, dt is the sampling time, sign is the sign function (-1 / 1, determined by whether the vehicle running direction is consistent with the vehicle body direction); its calculation method is: Where yaw is the heading angle of the vehicle (the angle between the x-axis of the base_link coordinate system and the x-axis of the map coordinate system, right-handed system); The vehicle angular velocity is calculated as: The angular velocity is calculated by sampling the heading angle and calculating the difference between the two heading angles. The calculation formula is: Among them yaw t and yaw t+1 are the heading angles at time t and t+1 respectively, dt is the sampling time, sign is the sign function, and its expression is as follows: Convert the solved linear velocity and angular velocity into the geometry_msgs / TwistSatmped message format of ROS, which mainly includes the coordinate system, the linear velocity of the X axis, and the angular velocity of the Z axis; The vehicle running trajectory is calculated as follows: The vehicle trajectory in the vehicle body coordinate system base_link is calculated based on the vehicle's front wheel angle, speed, and gear position, and converted into the ros nav_msgs / Path message format to determine whether there are obstacles on the driving trajectory when the vehicle is in gear to escape, so as to decide whether to suspend or resume the escape; the vehicle's driving trajectory is mainly calculated based on the bicycle model; Vehicle turning radius calculation The turning radius at the center of the vehicle's rear axle is calculated as: R=wheelBase / tan(abs(steerAngle)) Among them, wheelBase is the vehicle wheelbase, steerAngle is the vehicle front wheel steering angle; Calculation of vehicle travel direction The driving test vehicle may slip while driving at low speed. Therefore, the vehicle's direction should not be determined entirely by the gear position, but by the vehicle's linear speed and gear position. The method for calculating the vehicle's driving direction in this patent is to determine the direction based on the speed when the absolute linear speed is greater than 0.1m / s; when the absolute speed is less than 0.1m / s (at low speed, the positioning has a certain drift and is unreliable), it is considered to be in an absolute stationary state, and the gear position is used to determine: When vx>0.1m / s, the driving direction moveDirection=1, indicating forward movement; When vx<-0.1m / s, the driving direction moveDirection=0, which means moving backward; When abs(vx)<0.1m / s and the gear is in the forward gear, the driving direction moveDirection=1, indicating forward movement; When abs(vx)<0.1m / s and the gear is in reverse gear, the driving direction moveDirection=-1, indicating forward movement; When abs(vx)<0.1m / s and the gear is in neutral, the driving direction moveDirection=0, indicating stationary; Calculation of vehicle trajectory The vehicle driving trajectory calculates the number of points to be calculated based on the arc length pathLength and the track interval interval to be fitted, and calculates the x, y, yaw of each point one by one, and converts them into nav_msgs / Path messages for publication; The number of points to be calculated is num = int(pathLength / interval), so for any i∈[0,num], the calculation method of x, y, yaw is: (1) When steerAngle>=0.0 and moveDirection=1: yaw i =pathInterval*i / R x i =Rsin(yaw i ) y i =R(1-cos(yaw i )) (2) When steerAngle>=0.0 and moveDirection=-1: yaw i =-pathInterval*i / R x i =Rsin(yaw i ) y i =R(1-cos(yaw i )) (3) When steerAngle<=0.0 and moveDirection=1: yaw i =-pathInterval*i / R x i =-Rsin(yaw i ) y i =R(cos(yaw i )-1) (4) When steerAngle<=0.0 and moveDirection=-1: yaw i =pathInterval*i / R x i =-Rsin(yaw i ) y i =R(cos(yaw i )-1) Based on the above solution of the position and posture of the i-th point, the heading angle is converted into a quaternion according to 1.1, and converted according to the message format of nav_msgs / Path below to generate the predicted trajectory at base_link.

3. The method according to claim 2, characterized in that: In step S2, based on the real-time dynamic data of the vehicle, a grid map of the vehicle's body position is generated by discretizing the electronic fence collected in advance and generating a global electronic fence point cloud, and projecting the electronic fence point cloud from the map coordinate system to the vehicle base_link coordinate system, and filtering the electronic fence point cloud to retain the point cloud within a certain range in front, behind, left and right of the vehicle; at the same time, the point cloud of this part is fused with the point cloud of the vehicle's lidar sensor to generate a fused real-time point cloud in the base_link coordinate system, and the point cloud is projected onto a two-dimensional plane to generate a real-time grid semantic map.

4. The method according to claim 3, characterized in that: The implementation of generating point cloud by electronic fence is as follows: Electronic fence analysis and point cloud generation Each electronic fence is generated by a number of points to generate a polyline, and each electronic fence is interpolated at a certain interval to generate a point cloud format of sensor_msgs / PointCloud2 of ROS; The interpolation method is: (1) For each point on a line, in the order of (xn,yn), (xn+1,yn+1), a series of points are linearly inserted in the middle according to a fixed length; (2) Calculate the distance between two points; (3) Calculate how many points need to be generated; (4) Linear interpolation; Geo-fence point cloud transformation The electronic fence point cloud is in the map coordinate system and needs to be converted to the vehicle base_link coordinate system. Use the pcl_ros::transformPointCloud tool of ros to convert the electronic fence point cloud from the map coordinate system to the base_link coordinate system. Point cloud pass-through filtering Focus on a part of the electronic fence point cloud around the vehicle, use the pcl::PassThrough tool, set xmax, xmin, ymax, ymin for pass-through filtering, and retain a part of the electronic fence point cloud in front, behind, left and right of the vehicle; Point cloud fusion After the point cloud published by the sensor is also converted to the base_link coordinate system, it is spliced ​​with the electronic fence point cloud after the direct filtering to generate an electronic fence point cloud for generating a local grid map; Real-time placeholder grid map generation The grid map is in the message format of nav_mgs / OccupancgGrid of ros, which mainly includes the length and width of the map, the map resolution, the position of the map origin, and the value of each grid point; the index of the grid points is numbered from left to right and from bottom to top. The value of each index point is 0, which means that there is no obstacle at the grid point, and 100, which means that there is an obstacle at the grid point; The process of generating raster maps from point clouds includes: (1) Traverse all point cloud points and obtain the row index and column index of each point in the grid map: mx=int((x-map_orin_x) / map_resolution); my=int((y-map_orin_y) / map_resolution); where (map_orin_x,map_orin_y) is the origin of the grid map, (2) Calculate the map index Index = my*map_width+mx; where map_width is the width of the map; (3) The index of the map is assigned a value of 100, indicating that there is an obstacle.

5. The method according to claim 4, characterized in that: The specific implementation of the grid map collision detection in step S3 is as follows: This part discretizes the vehicle and calculates the index of each discrete point in the grid, and determines whether there is an obstacle within the vehicle body contour according to the value of the index in the grid map; Vehicle outline discretization The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle plane size can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (rear axle to rear of vehicle); the discretization process is: (1) Define the left, right, top, and bottom boundaries of the vehicle: (2) Discretize the horizontal and vertical directions: Collision detection between vehicle outline and grid map For the body contour point, the index value of the point in the grid map is calculated, and the value of the index value is obtained. If the index value is 100, it means that a collision has occurred; otherwise, no collision has occurred.

6. The method according to claim 5, characterized in that: In step S4, based on the result of collision detection, an escape grid map is generated, which is implemented as follows: when the vehicle speed is 0 and a collision between the contour and the grid map is detected, a signal needs to be sent to a local map generation module to generate an escape map, and the map removes obstacles within the vehicle body and the extended contour; the escape map will be used to determine whether there are obstacles within the path range when starting the escape, and if there are no obstacles, the escape will continue until the vehicle body contour does not expand with its own grid map, thereby completing the escape; if there is an obstacle, the escape will be suspended and wait for the obstacle to disappear.

7. The method according to claim 6, characterized in that: Specifically include the steps of body extension contour generation: Generate extended body contours; Escape map generation; Assign a current grid map as an escape map to discretize the extended contour of the vehicle body, calculate the index value of the discrete points of the contour in the escape map, and assign 0 to the index value in the escape map.

8. The method according to claim 7, characterized in that: Projecting the vehicle contour to any point is achieved as follows: Convert the discrete contour of the vehicle at base_link to the trajectory point at base_link to determine whether the vehicle will collide with the occupant grid within the trajectory range; For any point p(px,py) of the discrete contour, its new coordinates at any position (x,y,yaw) are: new_px=x+px*cos(yaw)-py*sin(yaw) new_py=y+px*cos(yaw)-py*sin(yaw).

9. The method according to claim 8, characterized in that: In step S6, the vehicle's escape behavior is controlled through the escape state machine model and decision making, which includes the following: Define the escape state machine model: the escape state is Default, indicating a normal driving state; (1) In the Default state, when the method detects that the vehicle outline collides with the vehicle body occupancy grid map and the speed is 0, the state is converted to ObstacleBrake, and based on the speed information, the speed information within 2s is extracted to determine the movement direction brakeTriggerDirection when the emergency brake is triggered (whether the speed changes from positive to 0 (brakeTriggerDirection=1) or from negative to 0 (brakeTriggerDirection=-1) within 2s); and the vehicle's position and posture at this time are obtained, and the signal is sent to the escape map module to generate an escape map; (2) When the state is ObstacleBrake, if it is detected that the vehicle is in neutral with the handbrake pulled, it enters the ObstacleRelease state; (3) When the state is ObstacleFaraway or ObstacleFarawayStop, the generated vehicle body contour generates the vehicle's motion trajectory points, converts them point by point, and performs collision detection. If there is no obstacle within the 2.0-meter predicted trajectory range, the state is ObstacleFaraway, otherwise the state is ObstacleFarawayStop; (4) If the state is ObstacleFaraway and it is detected that there is no collision between the vehicle outline and the real-time occupancy grid map, the system will re-enter Default, indicating that the escape is complete.

Images