A vehicle escape method in driving test scenarios based on grid maps

Abstract

The present invention relates to a vehicle escape method in a driving test scenario based on a grid map, comprising step S1: calculating the vehicle's posture, speed, angular velocity and driving trajectory; step S2: generating a grid map of the vehicle's body occupancy based on the vehicle's real-time dynamic data; step S3: performing collision detection on the grid map; judging whether there is a potential collision risk by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment; step S4: generating an escape grid map based on the result of the collision detection; step S5: projecting the vehicle's outline onto an arbitrary point posture; and step S6: controlling the vehicle's escape behavior through an escape state machine model and decision-making. 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 slopes.

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 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, thereby effectively avoiding collisions during driving and ensuring 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.), thereby 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] Currently, in the field of driving test robots, escape algorithms typically use a simple approach: based on the gear the vehicle was in when entering the electronic fence, they determine the reverse gear to use to exit the fence. While this method works in some cases, it doesn't take into account information about obstacles around the vehicle, nor does it account for the possibility of the vehicle rolling away when driving on a slope. Therefore, when faced with complex driving scenarios, this simple escape algorithm can easily cause the vehicle to collide with surrounding obstacles (whether static obstacles or other nearby vehicles), thereby increasing safety risks.

[0004] In order to improve the safety and reliability of driving test robots, it is particularly necessary to develop an intelligent escape algorithm that can comprehensively consider the vehicle's surrounding environment information and slope slippage conditions. Summary of the Invention

[0005] In view of the shortcomings of the current relevant existing technologies, the present invention uses the ROS (Robot Operating System) development platform to creatively propose a vehicle escape method for driving test scenarios based on grid maps. 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 slopes.

[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 comprises at least the following steps:

[0007] Step S1: Calculate the vehicle's position, velocity, angular velocity, and driving trajectory. This step is the foundation of the entire escape method. By acquiring the vehicle's real-time dynamic data, it provides necessary information support for subsequent processing.

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

[0009] Step S3: Performing collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determining 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 escape;

[0010] Step S4: Generate an escape grid map based on the collision detection results. 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 outline to any point; this step is to more accurately simulate the vehicle's motion 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 as follows: based on 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; 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 vehicle's posture information;

[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] Based on the vehicle's position, sampling is performed every 0.05s to calculate the vehicle's linear velocity (X-axis velocity in the base_link coordinate system) and angular velocity (Z-axis angular velocity in the base_link coordinate system);

[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 time 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); 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);

[0026] The vehicle angular velocity is calculated as:

[0027] The angular velocity is calculated by taking the difference between the two heading angles and sampling the heading angle. 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 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 vehicle's trajectory when the vehicle is engaged and out of trouble, so as to decide whether to suspend or resume the escape. The vehicle's trajectory is mainly calculated based on the bicycle model.

[0034] Vehicle turning radius calculation

[0035] The formula for calculating the turning radius at the center of the vehicle's rear axle is:

[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 at low speeds during driving. Therefore, the vehicle's direction should not be determined solely by the gear position, but by a comprehensive judgment based on 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 speeds, positioning has a certain amount of drift and is unreliable), it is considered to be in an absolutely stationary state, and the gear position is used to determine the direction:

[0040] When vx>0.1m / s, the moving direction is considered to be moveDirection=1, indicating forward movement;

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

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

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

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

[0045] Calculation of vehicle trajectory

[0046] The vehicle's trajectory is calculated based on the arc length pathLength and the interval between the paths. The number of points to be calculated is calculated, and the x, y, and yaw values ​​of each point are calculated one by one. The results are then converted 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 pose of the i-th point solved above, the heading angle is converted into a quaternion according to 1.1, and then converted into the message format of nav_msgs / Path as described 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 occupancy is generated by discretizing the electronic fence collected in advance and generating a global electronic fence point cloud, 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, this part of the point cloud is fused with the point cloud of the vehicle's lidar sensor to generate a real-time point cloud in the 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 the 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 (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 the number of points that 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 portion of the electronic fence point cloud around the vehicle, use the pcl::PassThrough tool, set xmax, xmin, ymax, ymin to perform pass-through filtering, and retain a portion of the electronic fence point cloud in front, behind, left, and right of the vehicle;

[0075] Point cloud fusion

[0076] After converting the point cloud published by the sensor to the base_link coordinate system, it is spliced ​​with the electronic fence point cloud after the above straight-through 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 map length and width, map resolution, 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 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) Assign a value of 100 to this index of the map, 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. Based on the value of the grid map index, it is determined whether there is an obstacle within the vehicle body contour;

[0087] Vehicle outline discretization

[0088] The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle's planar dimensions can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (from the rear axle to the rear of the vehicle). The discretization process is as follows:

[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 vehicle 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 the 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 the local map generation module to generate an escape map, which deducts the 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. If there are no obstacles, the escape will continue until the vehicle body contour no longer expands 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 point of the contour in the escape map, and assign 0 to the index value in the escape map.

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

[0100] Convert the vehicle's discrete contour 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;

[0101] For any point p(px,py) of the discrete contour, its new coordinates at any pose (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 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 pulling the handbrake and in neutral, it enters the ObstacleRelease state;

[0108] (3) When the state is ObstacleFaraway or ObstacleFarawayStop, the generated vehicle body contour is used to generate the vehicle's motion trajectory points, which are converted point by point and then collision detection is performed. 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, then 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 traditional related technologies do not take into account the information of obstacles around the vehicle, and do not consider the vehicle slipping scenario on the slope, which easily leads to the collision between the vehicle and the surrounding obstacles (static obstacles and surrounding vehicles) in complex scenarios. The technical solution provided by the present invention uses the ROS (Robot Operating System) development platform to creatively propose an escape algorithm based on a grid map. This algorithm aims to solve the two major problems faced by traditional escape algorithms in practical applications: one is that traditional algorithms lack the ability to effectively perceive 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 present invention's escape algorithm constructs a detailed grid map, enabling real-time acquisition and analysis of the robot's surrounding environment. This grid map divides the environment into multiple small cells, each representing a specific spatial region and recording information about the terrain and obstacles within that region. This allows the robot to more accurately identify and avoid obstacles, thereby improving its escape success rate.

[0114] 3. The present invention implements a grid map-based escape algorithm on the ROS development system, which not only solves the problems of traditional escape algorithms in environmental perception and slope misjudgment, 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 following briefly introduces the drawings required for use in the embodiments or the description of the prior art. 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 any creative work.

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

[0117] Figure 2 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 It is a schematic diagram of the 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 illustrated in the accompanying drawings. In the following description, when referring to the drawings, like numbers in different figures represent like or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all possible embodiments consistent with the present invention. Rather, they are merely examples of apparatus and methods consistent with certain 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 includes at least the following steps:

[0124] Step S1: Calculate the vehicle's position, velocity, angular velocity, and driving trajectory. This step is the foundation of the entire escape method. By acquiring the vehicle's real-time dynamic data, it provides necessary information support for subsequent processing.

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

[0126] Step S3: Performing collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determining 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 escape;

[0127] Step S4: Generate an escape grid map based on the collision detection results. 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 outline to any point; this step is to more accurately simulate the vehicle's motion 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, velocity, angular velocity, and driving trajectory are calculated as follows:

[0131] 1.1: Vehicle pose calculation

[0132] Based on the positioning and orientation device installed at the origin of the vehicle's base_link coordinate system, the output information of the point orientation device (generally including easting e, northing n, celestial u, heading yaw, pitch, and roll) is converted into the standard Ros message geometry_msgs / PoseStamped format. This message accurately represents the real-time position and attitude of the vehicle's 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 Euler angles (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 typically a fixed reference coordinate system used to describe the vehicle's position and posture in space. It serves as a benchmark for positioning other vehicle components and sensors, ensuring that the entire vehicle's positioning system accurately maintains spatial orientation. In the fields of robotics and autonomous driving, the base-link coordinate system is fundamental to 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 in 1.1, sampling is performed every 0.05s to calculate the vehicle's linear velocity (X-axis velocity in the base_link coordinate system) and angular velocity (Z-axis angular velocity in the base_link coordinate system).

[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, and 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).

[0147] 1.2.2 Calculation of angular velocity in this embodiment

[0148] The angular velocity is calculated by taking the difference between the two heading angles and sampling the heading angle. 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 trajectory calculation in this embodiment

[0154] This chapter primarily calculates the vehicle's trajectory in the vehicle coordinate system base_link based on the vehicle's front wheel angle, speed, and gear position. This is converted into the ROS nav_msgs / Path message format. This is used to determine whether there are obstacles along the vehicle's trajectory when the vehicle engages a gear to escape, thereby determining whether to pause or resume the escape. The vehicle's trajectory is primarily calculated based on a bicycle model.

[0155] 1.3.1 Calculation of vehicle turning radius

[0156] The formula for calculating the turning radius at the center of the vehicle's rear axle is:

[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 at low speeds during driving. Therefore, the vehicle's direction should not be determined solely by the gear position, but by a comprehensive judgment based on 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 speeds, positioning has a certain amount of drift and is unreliable), it is considered to be in an absolutely stationary state, and the gear position is used to determine the direction:

[0161] When vx>0.1m / s, the moving direction is considered to be moveDirection=1, indicating forward movement;

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

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

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

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

[0166] 1.3.3 Calculation of Vehicle Trajectory in This Example

[0167] The vehicle's trajectory is calculated based on the arc length pathLength and the interval distance interval required for fitting. The x, y, and yaw values ​​of each point are calculated one by one and converted 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 pose of the i-th point solved above, the heading angle is converted into a quaternion according to 1.1, and then converted into the message format of nav_msgs / Path as described below to generate the predicted trajectory at base_link.

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

[0187] The main function is to discretize the electronic fences 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, the point cloud of this part is fused with the point cloud of the vehicle lidar sensor to generate a real-time point cloud with the fused coordinate system of base_link, and the point cloud is projected onto a two-dimensional plane to generate a real-time raster semantic map.

[0188] 2.1 Electronic Fence Point Cloud Generation

[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, we use the pcl::PassThrough tool to set xmax, xmin, ymax, and ymin for pass-through filtering, and only retain a part of the electronic fence point cloud in front, behind, left, and right of the vehicle.

[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 above-mentioned straight-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 map length and width, map resolution, map origin position, and the value of each grid point. The grid point index 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 that grid point, and 100, which means there is an obstacle at that grid point.

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

[0209] Traverse all point cloud points and get 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 then determines whether there are obstacles within the vehicle body outline 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's planar dimensions can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (from the rear axle to the rear of the 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 vehicle 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's speed reaches zero and a collision is detected between the vehicle's outline and the grid map, a signal is sent to the local map generation module, which generates an escape map that excludes the vehicle body and obstacles within the expanded outline. This escape map is used to determine whether there are obstacles within the escape path. If no obstacles are present, the escape continues until the vehicle's outline no longer expands against the grid map, completing the escape. If an obstacle is present, the escape is paused until the obstacle disappears.

[0241] 4.1 Body extension profile generation in this embodiment

[0242] Generate the extended vehicle body outline according to 3.1 (the outline is expanded 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 Example

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

[0249] 5: Projection of the vehicle outline to any point in this embodiment

[0250] This function mainly converts the discrete outline 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 occupant grid within the trajectory range.

[0251] For any point p(px,py) of the discrete contour, its new coordinates at any pose (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 outline 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 2 seconds 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 2 seconds); 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 parking brake and neutral gear (the system defaults to release the brake by pulling the parking brake and shifting out of neutral gear), 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 into the vehicle motion trajectory points according to 1.3.3, and the points are converted point by point according to the method in 5. The collision detection is performed according to the method in 3.2 and the escape map. 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, then 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 understood that the above embodiments are exemplary and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify 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 by: It at least includes the following steps: Step S1: Calculate the vehicle's position, velocity, angular velocity, and driving trajectory. This step is the foundation of the entire escape method. By acquiring the vehicle's real-time dynamic data, it provides necessary information support for subsequent processing. Step S2: Generate a grid map of the vehicle's position based on the vehicle's real-time dynamic data. The grid map accurately reflects the vehicle's position and space in the current environment, providing basic data for subsequent collision detection and escape decisions. Step S3: Performing collision detection on the grid map; by analyzing the relationship between the vehicle's body occupancy grid map and the surrounding environment, determining 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 escape; Step S4: Generate an escape grid map based on the collision detection results. 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 outline to any point; this step is to more accurately simulate the vehicle's motion 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 the 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. In step S4, based on the result of the collision detection, an escape grid map is generated. The implementation method is as follows: when the vehicle speed is 0 and a collision between the vehicle outline and the grid map is detected, a signal needs to be sent to the local map generation module to generate an escape map. The map deducts the obstacles within the vehicle body and the extended outline; the escape map will be used to determine whether there are obstacles within the path range when the escape is started. If there are no obstacles, the escape will continue until the vehicle body outline no longer expands 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. Specifically, it includes the steps of generating the extended outline of the vehicle body: Generate extended body contours; Escape map generation; Assign a current grid map as an escape map to discretize the vehicle body extension contour, 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; Projecting the vehicle outline to any point can be achieved as follows: Convert the vehicle's discrete contour 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 pose (x,y,yaw) are: ； 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 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 2 seconds is extracted to determine the movement direction brakeTriggerDirection when triggering emergency braking; If the speed changes from positive to 0 within 2 seconds, brakeTriggerDirection=1; or from negative to 0, brakeTriggerDirection=-1; 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 pulling the handbrake and in neutral, it enters the ObstacleRelease state; (3) When the state is ObstacleFaraway or ObstacleFarawayStop, the generated vehicle body contour is used to generate the vehicle's motion trajectory points, which are converted point by point and then subjected to 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, then the system will re-enter Default, indicating that the escape is complete.

2. The method according to claim 1, wherein: In step S1, the vehicle posture is calculated as follows: based on 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, 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: ； The vehicle linear speed is calculated as: Based on 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: in and 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, and is determined by whether the vehicle's running direction is consistent with the vehicle's body direction; its calculation method is: ； Where yaw is the vehicle's heading angle between the x-axis of the base_link coordinate system and the x-axis of the map coordinate system, right-handed; The vehicle angular velocity is calculated as: The angular velocity is calculated by taking the difference between the two heading angles and sampling the heading angle. The calculation formula is: ； in and 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 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 vehicle's trajectory when the vehicle is engaged and out of trouble, so as to decide whether to suspend or resume the escape. The vehicle's trajectory is mainly calculated based on the bicycle model. Vehicle turning radius calculation The formula for calculating the turning radius at the center of the vehicle's rear axle is: , where wheelBase is the vehicle wheelbase and steerAngle is the vehicle front wheel steering angle; Calculation of vehicle travel direction The vehicle may slip while driving at low speeds. Therefore, the vehicle's direction should not be determined solely by the gear position, but rather by the vehicle's linear speed and gear position. The method for calculating the vehicle's direction 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, it is considered to be in an absolutely stationary state, and the gear position is used to determine the direction: When vx>0.1m / s, the moving direction is considered to be moveDirection=1, indicating forward movement; When vx<-0.1m / s, the driving direction moveDirection=-1, indicating backward movement; When abs(vx) < 0.1 m / s and the gear is in forward gear, the driving direction moveDirection = 1, indicating forward movement; When abs(vx)<0.1m / s and the gear is in reverse, the driving direction moveDirection=-1, indicating backward movement. When abs(vx)<0.1m / s and the gear is in neutral, the moving direction is considered to be 0, indicating stationary. Calculation of vehicle trajectory The vehicle's trajectory is calculated based on the arc length pathLength and the interval between the paths. The number of points to be calculated is calculated, and the x, y, and yaw values ​​of each point are calculated one by one. The results are then converted into nav_msgs / Path messages for publication. The number of points to be calculated is num=int(pathLength / interval), so for any , the x, y, yaw calculation method is: ； Based on the pose of the i-th point solved above, the heading angle is converted into a quaternion and converted according to the following nav_msgs / Path message format to generate the predicted trajectory at base_link.

3. The method according to claim 2, wherein: In step S2, a grid map of the vehicle's body occupancy is generated based on the vehicle's real-time dynamic data by discretizing the pre-collected electronic fences and generating a global electronic fence point cloud. The electronic fence point cloud is then projected from the map coordinate system to the vehicle's base_link coordinate system, and the electronic fence point cloud is filtered to retain point clouds within a certain range in front, behind, and on the left and right of the vehicle. Simultaneously, the point cloud is fused with the point cloud from the vehicle's lidar sensor to generate a real-time point cloud in the base_link coordinate system. The point cloud is then projected onto a two-dimensional plane to generate a real-time grid semantic map.

4. The method according to claim 3, wherein: 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 the point cloud format of sensor_msgs / PointCloud2 of ROS; The interpolation method is: (1) For each point on a line, in the order (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 the number of points that 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 portion of the electronic fence point cloud around the vehicle, use the pcl::PassThrough tool, set xmax, xmin, ymax, ymin to perform pass-through filtering, and retain a portion of the electronic fence point cloud in front, behind, left, and right of the vehicle; Point cloud fusion After converting the point cloud published by the sensor to the base_link coordinate system, it is spliced ​​with the electronic fence point cloud after the above straight-through 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 map length and width, map resolution, 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 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: is the origin of the grid map, (2) Calculate the index of the map Index=my*map_width+mx; where map_width is the width of the map; (3) Assign a value of 100 to this index of the map, 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. According to the value of the grid map index, it is determined whether there is an obstacle within the vehicle body contour; Vehicle outline discretization The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle's planar dimensions can be described by three dimensions: vehicleLength (length), vehicleWidth (width), and baseLink2Back (distance from the rear axle to the rear of the vehicle). The discretization process is as follows: (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 vehicle 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.

Images