CN114815853B - A path planning method and system considering road obstacle characteristics - Google Patents

Abstract

The invention relates to a path planning method and a system considering road surface obstacle characteristics, and belongs to the technical field of path planning. According to the road surface obstacle characteristic-considered path planning method, whether the obstacle causes vehicle instability or not can be classified according to the acquired road information, the vehicle running information and the environment obstacle information and by combining the offline vehicle obstacle instability boundary, a grid map which can cross the obstacle and a grid map which cannot cross the obstacle are respectively constructed, a series of polynomial track groups generated by fitting are subjected to path collision screening based on the two grid maps, and further roll cost calculation caused by passing the obstacle can be considered, so that a feasible track which is smooth, free of collision instability and capable of meeting lateral and roll stability is finally generated, the problem of unnecessary detouring in the prior art is solved, the path distance can be further reduced, the economical efficiency and the real-time performance are improved, unnecessary steering operation is reduced, and the risk of vehicle instability is further reduced.

Description

A path planning method and system considering road obstacle characteristics

Technical Field

The present invention relates to the technical field of path planning, and in particular to a path planning method and system that takes road obstacle characteristics into consideration.

Background technique

The core of the development of intelligent transportation systems is unmanned vehicles, which use a variety of sensors to perceive the environment around the vehicle, and control the steering and speed of the vehicle based on the information obtained, ultimately achieving safe and reliable autonomous driving functions. The key technologies of unmanned vehicles include environmental perception, navigation positioning, path planning, and decision control. Among them, path planning is the bridge between information perception and intelligent control of unmanned vehicles, and is the basis for achieving autonomous driving. Its task is to search for an optimal path in an environment with obstacles according to certain evaluation criteria based on a certain path planning algorithm. The path planning result directly determines whether the smart car can complete various driving behaviors safely, efficiently, and smoothly and reach the target destination.

In recent years, the path planning technology of intelligent vehicles has developed rapidly and can be roughly divided into five categories: methods based on random sampling (such as fast search random tree RRT, probability graph PRM, etc.), methods based on graph search (such as A*, D* and its variants, etc.), methods based on geometric curves (such as B-spline, Bezier curve, quintic polynomial curve, etc.), methods based on optimization (such as artificial potential field method, model prediction method, etc.), and intelligent algorithms based on bionics (such as genetic algorithm, ant colony algorithm, fish school algorithm, etc.).

The above five commonly used path planning methods mostly perform obstacle avoidance processing, that is, bypass them during the path planning process, but do not take into account the impact of the vehicle's direct passage through small-sized road obstacles on the vehicle's dynamic response. The planned path will have unnecessary detours, which not only increases the path distance, reduces economy and real-time performance, but also increases unnecessary steering operations and increases the risk of vehicle instability.

Summary of the invention

In order to solve the above problems existing in the prior art, the present invention provides a path planning method and system taking into account the characteristics of road obstacles.

To achieve the above object, the present invention provides the following solutions:

A path planning method considering road obstacle characteristics, comprising:

Acquire current lane information, current vehicle position information, current vehicle posture information, current vehicle speed information, and obstacle position information and obstacle size information in the current lane; the lane information includes lane boundary information and lane line information;

The initial state is obtained by projecting and transforming the position information and the posture information of the current vehicle according to the local S-L coordinate system; the local S-L coordinate system is a coordinate system between the longitudinal distance and the lateral offset established based on the lane center reference line; the longitudinal distance is the longitudinal distance along the lane center reference line; the lateral offset is the lateral offset relative to the lane center reference line;

Determining a planning final state within a planning time domain according to the lane line information and the speed information of the current vehicle;

Generate a candidate trajectory group based on the initial state and the planned final state; the candidate trajectory group includes a plurality of smooth trajectories from the initial state to the planned final state;

Classifying obstacles in the smooth trajectory based on the instability boundary of the offline vehicle obstacle and the size information of the obstacle to obtain traversable obstacles and non-traversable obstacles;

Generate a surmountable obstacle grid map according to the position information of the surmountable obstacles and the size information of the surmountable obstacles, and generate an unsurmountable obstacle grid map according to the position information of the unsurmountable obstacles and the size information of the unsurmountable obstacles; the grid value of the surmountable obstacle grid map where there is a surmountable obstacle is set to the height value of the surmountable obstacle, and the grid value of the unsurmountable obstacle is set to 0; the grid value of the unsurmountable obstacle grid map where there is an unsurmountable obstacle is set to 1, and the grid value of the unsurmountable obstacle is set to 0;

determining a wheel track envelope domain within the smooth trajectory based on a tire width of the vehicle;

In the grid map of the insurmountable obstacle, the grid value in the wheel track envelope domain of the current smooth track is queried. When the grid values are not 0, the grid value in the wheel track envelope domain is jumped to the next smooth track to be queried.

When the grid values are all 0, the grid values in the wheel track envelope domain of the current smooth trajectory are queried in the obstacle-crossable grid map, and the relative roll angle of the current smooth trajectory is calculated to obtain the path cost of the current smooth trajectory;

When all smooth trajectories in the candidate trajectory group have been queried, the smooth trajectory with the minimum path cost is used as the vehicle driving trajectory.

Preferably, the generating of a candidate trajectory group based on the initial state and the planned final state; the candidate trajectory group includes a plurality of smooth trajectories from the initial state to the planned final state, specifically including:

The initial state and the planned final state are described by using a five-term polynomial, and the described initial state and the planned final state are transformed into a global coordinate system to obtain a trajectory group to be selected; the global coordinate system is a coordinate system established based on the vehicle position and heading information.

Preferably, the process of determining the offline vehicle obstacle instability boundary is:

Based on the vehicle dynamics model, the simulation engine is used to simulate the vehicle's suspension shock absorber deformation and tire deformation response under different working conditions;

A vehicle vertical smoothness evaluation method based on prior knowledge, taking the suspension shock absorber deformation as a standard, dividing the tire deformation response to obtain a response boundary for characterizing the vehicle vertical obstacle instability;

Based on the response boundary of the vehicle's vertical obstacle instability, a mapping between the vehicle speed and obstacle size and whether the vehicle has vertical instability is constructed in a lookup table mode; when the height of the obstacle is greater than a preset threshold, it is determined that the vehicle has vertical obstacle instability.

Preferably, generating a surmountable obstacle grid map according to the position information of surmountable obstacles and the size information of surmountable obstacles, and generating an unsurmountable obstacle grid map according to the position information of unsurmountable obstacles and the size information of unsurmountable obstacles, specifically includes:

Taking the current vehicle position as the origin and the current heading of the vehicle as the positive direction of the x-axis, a grid map coordinate system is constructed based on the perception range of the vehicle's obstacles.

Mapping the position information and size information of the traversable obstacles to the grid map coordinate system to obtain a traversable obstacle grid map;

The position information of the insurmountable obstacle and the size information of the insurmountable obstacle are mapped to the grid map coordinate system to obtain the insurmountable obstacle grid map.

According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

The path planning method considering the characteristics of road obstacles provided by the present invention can classify whether obstacles will cause vehicle instability according to the acquired road information, vehicle driving information and environmental obstacle information, combined with the offline vehicle obstacle instability boundary, and respectively construct a grid map describing crossable obstacles and a grid map describing non-crossable obstacles, and perform path collision screening on a series of polynomial trajectory groups generated by fitting based on the two grid maps, and then consider the roll cost calculation caused by passing the crossable obstacles, and finally generate a smooth, collision-free and instability-free feasible trajectory that meets lateral and roll stability, so as to solve the problem of unnecessary detours in the prior art, thereby reducing the path distance, improving economy and real-time performance, and reducing unnecessary steering operations, thereby reducing the risk of vehicle instability.

Corresponding to the path planning method considering road obstacle characteristics provided above, the present invention also provides a path planning system considering road obstacle characteristics, the system comprising:

The vehicle perception module is used to obtain the current lane information, the current vehicle position information, the current vehicle posture information, the current vehicle speed information, and the position information and size information of obstacles in the current lane; the lane information includes: lane boundary information and lane line information;

A module for generating a group of selected trajectories, for projecting and transforming the position information of the current vehicle and the posture information of the current vehicle according to a local S-L coordinate system to obtain an initial state, for determining a final state of planning in a planning time domain according to the lane line information and the speed information of the current vehicle, and for generating a group of selected trajectories based on the initial state and the final state of planning; the local S-L coordinate system is a coordinate system between a longitudinal distance and a lateral offset established based on a lane center reference line; the longitudinal distance is a longitudinal distance along the lane center reference line; the lateral offset is a lateral offset relative to the lane center reference line; the group of selected trajectories includes a plurality of smooth trajectories from the initial state to the final state of planning;

An offline obstacle instability judgment module is used to classify obstacles in the smooth trajectory based on the offline vehicle obstacle instability boundary and the size information of the obstacle to obtain crossable obstacles and non-crossable obstacles;

A classification obstacle map generation module is used to generate a surmountable obstacle grid map according to the position information of surmountable obstacles and the size information of surmountable obstacles, and to generate an unsurmountable obstacle grid map according to the position information of unsurmountable obstacles and the size information of unsurmountable obstacles; the grid value of the surmountable obstacle grid map where there is a surmountable obstacle is set to the height value of the surmountable obstacle, and the grid value of the non-surmountable obstacle is set to 0; the grid value of the non-surmountable obstacle grid map where there is an unsurmountable obstacle is set to 1, and the grid value of the non-surmountable obstacle is set to 0;

A wheel track extension generation module, for determining a wheel track envelope domain within a smooth track based on a tire width of the vehicle;

The collision detection module is used to query the grid value in the wheel track envelope domain of the current smooth track in the grid map of the insurmountable obstacle. When the grid values are not 0, it jumps to the next smooth track to query the grid value in the wheel track envelope domain.

The cost calculation module is used to query the grid values in the wheel track envelope domain of the current smooth track in the obstacle-crossable grid map when the grid values are all 0, and calculate the relative roll angle of the current smooth track to obtain the path cost of the current smooth track;

The vehicle driving trajectory output module is used to take the smooth trajectory with the minimum path cost as the vehicle driving trajectory after all smooth trajectories in the candidate trajectory group have been queried.

Preferably, the candidate trajectory group generation module includes:

The candidate trajectory group generating unit is used to describe the initial state and the planned final state by using a five-term polynomial, and transform the described initial state and the planned final state into a global coordinate system to obtain a candidate trajectory group; the global coordinate system is a coordinate system established based on the vehicle position and heading information.

Preferably, the offline obstacle instability judgment module includes:

A simulation unit is used to simulate the deformation of the suspension shock absorber and tire deformation of the vehicle under different working conditions using a simulation engine based on a vehicle dynamics model;

A division unit is used for a method for evaluating the vertical ride comfort of a vehicle based on prior knowledge, taking the deformation of the suspension shock absorber as a standard, dividing the tire deformation response to obtain a response boundary for characterizing the vertical obstacle instability of the vehicle;

The first mapping unit is used to construct a mapping between the vehicle speed and the obstacle size and whether the vehicle has vertical instability based on the response boundary of the vehicle's vertical obstacle instability in a lookup table mode; when the height of the obstacle is greater than a preset threshold, it is determined that the vehicle has vertical obstacle instability.

Preferably, the classification obstacle map generation module includes:

A coordinate system construction unit, used to construct a grid map coordinate system based on the perception range of the vehicle obstacles, taking the current vehicle position as the origin and the current heading of the vehicle as the positive direction of the x-axis;

A second mapping unit, used for mapping the position information of the traversable obstacle and the size information of the traversable obstacle to the grid map coordinate system to obtain a traversable obstacle grid map;

The third mapping unit is used to map the position information of the insurmountable obstacle and the size information of the insurmountable obstacle to the grid map coordinate system to obtain the insurmountable obstacle grid map.

Since the technical effect achieved by the path planning system considering road obstacle characteristics provided by the present invention is the same as the technical effect achieved by the path planning method considering road obstacle characteristics provided above, it will not be described in detail here.

BRIEF DESCRIPTION OF THE DRAWINGS

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 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.

FIG1 is a flow chart of a path planning method considering road obstacle characteristics provided by the present invention;

FIG2 is a diagram showing an implementation framework of a path planning system taking into account road obstacle characteristics provided by an embodiment of the present invention;

FIG3 is a schematic diagram of an offline obstacle instability boundary judgment process according to an embodiment of the present invention;

FIG4 is a schematic diagram of a global coordinate system state provided by an embodiment of the present invention;

FIG5 is a schematic diagram of a local coordinate system state provided by an embodiment of the present invention;

FIG6 is a crossable obstacle grid map provided by an embodiment of the present invention;

FIG7 is a grid map of insurmountable obstacles provided by an embodiment of the present invention;

FIG8 is a schematic diagram of wheel track expansion generation according to an embodiment of the present invention;

FIG9 is a schematic diagram of calculating the relative roll angle of a road surface provided by an embodiment of the present invention;

Detailed ways

The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

The purpose of the present invention is to provide a path planning method and system that takes into account the characteristics of road obstacles, so as to solve the problem of unnecessary detours in the prior art, thereby reducing the path distance, improving economy and real-time performance, and reducing unnecessary steering operations, thereby reducing the risk of vehicle instability.

Terminology explanation:

Vehicle non-obstacle instability: The vehicle may skid or roll over due to the influence of input factors such as the driver's steering and external lateral wind and ramps while driving.

Vehicle obstacle instability: When a vehicle is driving, it is stimulated by road obstacles (pots, bumps, etc.), causing the wheels to leave the ground or the vehicle to trip and overturn.

Obstacle instability boundary determination: A mapping between road obstacle size, vehicle longitudinal speed and vehicle vertical dynamic response established based on a data-driven approach; this mapping can use road obstacle size and vehicle longitudinal speed as input to determine whether the vehicle will experience obstacle instability under the current environmental conditions.

Crossable and non-crossable obstacles: obstacles are classified into crossable obstacles and non-crossable obstacles based on whether the obstacles will cause the current vehicle to become unstable.

In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

As shown in FIG1 , the path planning method considering road obstacle characteristics provided by the present invention includes:

Step 100: Obtain current lane information, current vehicle position information, current vehicle posture information, current vehicle speed information, and obstacle position information and obstacle size information in the current lane. Lane information includes lane boundary information and lane line information.

Step 101: Project the current vehicle position information and the current vehicle posture information according to the local S-L coordinate system to obtain the initial state. The local S-L coordinate system is a coordinate system between the longitudinal distance and the lateral offset based on the lane center reference line, as shown in Figure 5. The longitudinal distance is the longitudinal distance along the lane center reference line. The lateral offset is the lateral offset relative to the lane center reference line.

Step 102: Determine the final planning state in the planning time domain according to the lane line information and the current vehicle speed information. For example, based on the current vehicle speed v _x and the lane line information L _left and L _right , determine a series of possible final planning states in the planning time domain t _p , where L _left and L _right are the maximum left/right lateral offsets allowed by the current lane. The end point SL coordinate system state is shown in Table 1.

Table 1 Preset planning final state table

Step 103: Generate a candidate trajectory group based on the initial state and the planned final state. The candidate trajectory group includes multiple smooth trajectories from the initial state to the planned final state.

Based on the above description, it is known that the initial state and the final state of the planning in the local coordinate system With/> That is, two sets of equations with 6 known unknowns can be used to describe the movement of the vehicle in the s and L directions in the local coordinate system using fifth-order polynomials, and then transformed into the global coordinate system (as shown in Figure 4), to obtain a set of smooth trajectories from the initial state to the final state of the planning. The states at each moment include: the longitudinal and lateral positions x, y in the global coordinate system, the heading angle θ in the global coordinate system, the trajectory curvature κ, the longitudinal distance s along the reference line, and the lateral offset L relative to the reference line.

Step 104: Classify obstacles in the smooth trajectory based on the offline vehicle obstacle instability boundary and obstacle size information to obtain crossable obstacles and non-crossable obstacles.

Among them, this embodiment can determine the offline vehicle obstacle instability boundary in the following way:

1) Establish a dynamic model of the planned vehicle, and simulate its driving conditions at different speeds through road obstacles of different sizes (length, width, height) in the simulation environment of a high-precision physics engine to obtain the vehicle's suspension shock absorber deformation and tire deformation response.

2) Through the vehicle vertical smoothness evaluation method based on prior knowledge, a response boundary representing the vertical obstacle instability of the vehicle is divided with the deformation of the suspension shock absorber as the standard. That is, under this working condition, if the vehicle response represents that the vehicle is off the ground, it is considered that this working condition will cause the vehicle obstacle instability.

3) Based on the lookup-table mode, a mapping between vehicle speed, obstacle size and whether the vehicle is vertically unstable is constructed. The input should be the longitudinal speed of the vehicle and the obstacle size, and the output is whether the obstacle will cause the vehicle obstacle instability at the current speed (as shown in Figure 3). Here, the obstacle height h _threshold corresponding to the vehicle's passing angle is used as an additional threshold. If the obstacle height is higher than the threshold, it is unconditionally considered that the obstacle will cause instability.

Step 105: Generate a surmountable obstacle grid map based on the location information of the surmountable obstacles and the size information of the surmountable obstacles, and generate an unsurmountable obstacle grid map based on the location information of the unsurmountable obstacles and the size information of the unsurmountable obstacles. The grid value of the surmountable obstacle in the surmountable obstacle grid map is set to the height value of the surmountable obstacle, and the grid value of the unsurmountable obstacle is set to 0. The grid value of the unsurmountable obstacle in the unsurmountable obstacle grid map is set to 1, and the grid value of the unsurmountable obstacle is set to 0. The darker the color of the grid in Figures 6 and 7, the larger the grid value. The implementation process of this step can be as follows:

Step 1050: With the current vehicle position as the origin and the current heading as the positive direction of the x-axis, a grid map coordinate system is established, and a rectangular grid map of a certain length, width and resolution is constructed based on the perception range. Each grid point can store binary (0 and 1) information or obstacle height (scalar) information.

Step 1051: Based on the uncertainty of the obstacle information obtained, the obstacle size information is expanded by a certain proportion.

Step 1052: For the crossable obstacle grid map, the position and size information of the crossable obstacle is projected into the grid map coordinate system, and the grid value is its height size. The grid value of the non-crossable obstacle is set to 0, as shown in FIG6 .

Step 1053: For the grid map of insurmountable obstacles, the location and size information of the insurmountable obstacles are projected into the grid map coordinate system, and the grid values where the insurmountable obstacles exist are set to 1, and the rest are set to 0, as shown in FIG. 7 .

Step 106: Determine the wheel track envelope domain within the smooth trajectory based on the tire width of the vehicle. For example, for the trajectory within the current cycle, the tire width position of each point is calculated based on formula (1), thereby obtaining the wheel track of the vehicle on the trajectory, where X, _Xleft and _Xright are the horizontal axis positions of the trajectory, the left wheel on the trajectory and the right wheel on the trajectory, respectively, and Y, _Yleft and _Yright are the vertical axis positions of the trajectory, the left wheel on the trajectory and the right wheel on the trajectory, respectively, as shown in FIG8 .

Step 107: query the grid value in the wheel track envelope domain of the current smooth track in the grid map of the insurmountable obstacle. When all the grid values are not 0, jump to the next smooth track to query the grid value in the wheel track envelope domain.

Step 108: When the grid values are all 0, query the grid values in the wheel track envelope domain of the current smooth trajectory in the obstacle-crossable grid map, and calculate the relative roll angle of the current smooth trajectory to obtain the path cost of the current smooth trajectory.

The path cost is the weighted sum of the lateral response cost, lateral offset error cost and roll risk cost. The lateral response cost is the sum of the curvatures of the sampling points on each candidate trajectory, which represents the severity of the vehicle's steering when following the trajectory. The lateral offset error is the sum of the lateral displacements in the s-l coordinate state of the sampling points on each candidate trajectory, which represents the degree of deviation of the vehicle trajectory relative to the reference lane line. The roll risk cost is the sum of the relative roll angles of the sampling points calculated based on the height information of the sampling points on the left and right wheel tracks of each candidate trajectory in the crossable obstacle grid map, which represents the rollover risk caused by contact with crossable obstacles on the vehicle trajectory.

The calculation process of the path cost is:

Step 1080: Based on the lateral offset and curvature in the current trajectory information, calculate the cumulative weighted lateral offset cost and the cumulative weighted curvature cost of the trajectory.

Step 1081: Based on the wheel track information of the trajectory, query the obstacle height corresponding to each point of the wheel track in the surmountable obstacle grid map, and calculate the relative roll angle of the trajectory (as shown in FIG. 9 ), thereby obtaining the cumulative weighted roll risk cost of the trajectory.

state _trajectory,i = [xy θ κ s L] (2)

Where x, y, θ, κ, s, and L are the global horizontal and vertical coordinates of each point on the trajectory, the global heading angle, the curvature, the arc length in the local coordinate system, and the lateral offset in the local coordinate system. is the relative roll angle of the vehicle at each point on the track, which is calculated from the road surface height at each point on the wheel track and the vehicle wheelbase, /> is the relative roll angle of the vehicle at the i-th point on the trajectory, Q ₁ , Q ₂ and Q ₃ are the weight coefficients of lateral offset, relative roll and curvature in the cost function, respectively, h _left is the height of the left wheel from the ground, h _right is the height of the right wheel from the ground, wheeltraj _i is the wheel track of the i-th sampling point on the trajectory, T is the vehicle wheel width, state _trajectory,i is the state contained in the i-th sampling point on the trajectory, cost is the cost function of the current trajectory, _Li is the lateral offset of the i-th sampling point on the trajectory in the local coordinate system, N is the total number of sampling points on the trajectory, and κ _i is the curvature of the i-th sampling point on the trajectory.

Step 109: When all smooth trajectories in the candidate trajectory group have been queried, the smooth trajectory with the minimum path cost is used as the vehicle driving trajectory. The obtained vehicle driving trajectory can meet the requirements of obstacle avoidance and vehicle stability, and the path is smooth and differentiable, which is convenient for tracking the control part.

This embodiment takes into account the degree of influence of road obstacle characteristics on vehicle stability, and adopts a data-driven obstacle instability boundary model to predict the dynamic response of obstacles on the vehicle's path, and classifies the perceived obstacles as a criterion for obstacle instability, respectively constructing grid maps of crossable and non-crossable obstacles, and performing collision detection and cost calculation of trajectories based on the two maps, and finally planning a feasible path that meets the obstacle avoidance and stability requirements. The system for implementing the above-mentioned path planning method mainly consists of six parts: an offline obstacle instability judgment module, a classified obstacle map generation module, a candidate trajectory group generation module, a wheel track expansion generation module, a collision detection module and a cost calculation module. The following is a detailed description of the system based on this architecture.

Specifically, as shown in Figure 2, the path planning system considering road obstacle characteristics provided by the present invention includes: a vehicle perception module, a selected trajectory group generation module, an offline obstacle instability judgment module, a classified obstacle map generation module, a wheel track expansion generation module, a collision detection module, a cost calculation module and a vehicle driving trajectory output module.

The vehicle perception module is used to obtain the current lane information, the current vehicle position information, the current vehicle posture information, the current vehicle speed information, and the position information and size information of obstacles in the current lane. The lane information includes: lane boundary information and lane line information.

The module for generating the candidate trajectory group is used to project and transform the position information and posture information of the current vehicle according to the local S-L coordinate system to obtain the initial state, to determine the final state of planning in the planning time domain according to the lane line information and the speed information of the current vehicle, and to generate the candidate trajectory group based on the initial state and the final state of planning. The local S-L coordinate system is a coordinate system between the longitudinal distance and the lateral offset established based on the lane center reference line. The longitudinal distance is the longitudinal distance along the lane center reference line. The lateral offset is the lateral offset relative to the lane center reference line. The candidate trajectory group includes multiple smooth trajectories from the initial state to the final state of planning.

Specifically, the module for generating the selected trajectory group includes global-local coordinate conversion, planning final state definition and polynomial fitting trajectory generation. The module input is the vehicle speed information and reference lane line information obtained by the perception layer. The global-local coordinate conversion converts the vehicle's current position and heading information in the global x-y coordinate system to the local s-l coordinate system generated based on the reference lane centerline. The planning final state is based on the trajectory prediction duration, the current vehicle state and the road information to define a series of possible s-l coordinate system states of the vehicle at the end of the planning, and based on 4/5-order polynomial fitting, a polynomial trajectory group that meets the planning initial/final state s-l coordinate system state constraints is generated.

The offline obstacle instability judgment module is used to classify obstacles in the smooth trajectory based on the offline vehicle obstacle instability boundary and the obstacle size information to obtain crossable obstacles and non-crossable obstacles.

Specifically, the offline obstacle instability judgment module includes a data-driven obstacle size feature (including but not limited to length, width, height, obstacle curvature, slope features, etc.)-vehicle dynamic response mapping model and a vehicle dynamic response-vehicle obstacle instability boundary mapping model, which can use the dynamic response of the vehicle passing through different obstacles simulated under a high-precision physics engine, combined with the vehicle instability boundary designed by experts or experience, to express the mapping relationship between the perceived obstacle size information and the degree of vehicle instability, thereby classifying the perceived obstacle information into insurmountable obstacles and surmountable obstacles, and outputting them to the classified obstacle map generation module.

The classification obstacle map generation module is used to generate a surmountable obstacle grid map according to the location information of surmountable obstacles and the size information of surmountable obstacles, and to generate an insurmountable obstacle grid map according to the location information of insurmountable obstacles and the size information of insurmountable obstacles. The grid value of the surmountable obstacle in the surmountable obstacle grid map is set to the height value of the surmountable obstacle, and the grid value of the non-surmountable obstacle is set to 0. The grid value of the non-surmountable obstacle in the insurmountable obstacle grid map is set to 1, and the grid value of the non-surmountable obstacle is set to 0.

Specifically, the classification obstacle map generation module includes the generation of the grid map of non-crossable obstacles and the generation of the grid map of crossable obstacles. Based on the obstacle classification results obtained in the previous module, combined with the perspective, viewing distance and uncertainty information of the perception module, rectangular grid maps of the same length and width are generated respectively (see step 5 below). The former only contains the position and size (excluding height) information of the non-crossable obstacles, which is used for collision detection in the subsequent process. The latter only contains the position and size (including height) information of the crossable obstacles, which is used for trajectory cost calculation in the subsequent process.

The wheel track extension generation module is used to determine the wheel track envelope domain within the smooth track based on the tire width of the vehicle.

Specifically, the wheel track extension generation module performs cyclic sampling on the input polynomial trajectory group, obtains the position and heading information of each candidate trajectory, and expands it based on the vehicle wheelbase size (see step six below), thereby generating the wheel track envelope domain on the candidate trajectory. The envelope domain will be used for the next step of collision detection, and the envelope domain boundary will be used for the roll risk cost calculation in the trajectory cost calculation.

The collision detection module is used to query the grid value in the wheel track envelope domain of the current smooth track in the grid map of the insurmountable obstacle. When the grid values are not 0, it jumps to the next smooth track to query the grid value in the wheel track envelope domain.

Specifically, the collision detection module queries whether there is an uncrossable obstacle in the wheel track envelope corresponding to each selected track based on the uncrossable obstacle grid map information. If there is, the cost calculation of the track is skipped and the collision detection of the next selected track is entered. If not, the cost calculation module is transferred to perform the cost calculation of the track. The main function of the collision detection module is to filter out the tracks of barrier-free instability and collision.

The cost calculation module is used to query the grid values within the wheel track envelope domain of the current smooth trajectory in the obstacle-crossable grid map when the grid values are all 0, and calculate the relative roll angle of the current smooth trajectory to obtain the path cost of the current smooth trajectory.

Specifically, the path cost calculation module includes lateral response cost, lateral offset error cost and roll risk cost. The lateral response cost is the sum of the curvatures of each sampling point on each selected trajectory, which characterizes the severity of the steering when the vehicle follows the trajectory. The lateral offset error is the sum of the lateral displacements in the s-l coordinate state of each sampling point on each selected trajectory, which characterizes the degree of deviation of the vehicle trajectory relative to the reference lane line. The roll risk cost is the sum of the relative roll angles of each sampling point calculated based on the height information of each sampling point on the left and right wheel tracks of each selected trajectory in the crossable obstacle grid map, which characterizes the rollover risk caused by contact with crossable obstacles on the vehicle trajectory. The path cost is the sum of the weighted calculation of the three costs, and finally outputs a path with the lowest cost, which can meet the requirements of obstacle avoidance and vehicle stability, and the path is smooth and differentiable, which is convenient for tracking the control part.

The vehicle driving trajectory output module is used to output the smooth trajectory with the minimum path cost as the current optimal trajectory after all smooth trajectories in the candidate trajectory group have been queried, and transmit it to the tracking control layer.

Based on the system structure provided above, after the driver starts the vehicle, the vehicle perception module obtains the road environment information and sends it to the planning layer (i.e., the candidate trajectory group generation module, the offline obstacle instability judgment module, the classified obstacle map generation module, the wheel track expansion generation module, the collision detection module and the cost calculation module) after processing.

The IMU (Inertial Measurement Unit) obtains the current longitudinal vehicle speed v _x and sends it to the offline obstacle instability judgment module and the candidate trajectory group generation module respectively.

The GPS module obtains the current vehicle position and posture information (X, Y, θ) and sends it to the trajectory group generation module.

The V2X/Internet of Vehicles module and the camera/lidar module obtain lane boundary information, including: the number of left and right lanes nLane _left , nLane _right , lane width (assuming all lanes are of equal width) W _lane , reference lane centerline position, heading and curvature information (X _ref , Y _ref , θ _ref , κ _ref ), and send it to the candidate trajectory group generation module.

The camera/lidar module obtains the position and size information (X _obs , Y _obs , length _obs , width _obs , height _obs ) of the current obstacle and sends it to the offline obstacle instability judgment module.

In the process of classifying perceived obstacles using the offline obstacle instability judgment module, obstacle size information is input into the offline obstacle instability boundary, and the obstacles are classified according to whether the output is unstable or not.

Among them, for surmountable obstacles, all their position and size information are retained and sent to the classification obstacle map generation module. For insurmountable obstacles, only their position and length and width size information are retained and sent to the classification obstacle map generation module.

In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

This article uses specific examples to illustrate the principles and implementation methods of the present invention. The above examples are only used to help understand the method and core ideas of the present invention. At the same time, for those skilled in the art, according to the ideas of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims (8)

1. A path planning method considering road surface obstacle characteristics, comprising:

acquiring current lane information, current vehicle position information, current vehicle pose information, current vehicle speed information, and position information and size information of an obstacle in a current lane; the lane information includes: boundary information and lane line information of a lane;

Performing projection transformation on the position information of the current vehicle and the pose information of the current vehicle according to a local S-L coordinate system to obtain an initial state; the local S-L coordinate system is a coordinate system between a longitudinal distance and a lateral offset which are established based on a lane center reference line; the longitudinal distance is a longitudinal distance advancing along the lane center reference line; the lateral offset is relative to the lane center reference line;

Determining a planning final state in a planning time domain according to the lane line information and the speed information of the current vehicle;

generating a track group to be selected based on the initial state and the planning final state; the track group to be selected comprises a plurality of smooth tracks from the initial state to the planning final state;

classifying the obstacles in the smooth track based on the off-line vehicle obstacle instability boundary and the size information of the obstacles to obtain a spanable obstacle and a non-spanable obstacle;

Generating a spanned obstacle grid map according to the position information of the spanned obstacle and the size information of the spanned obstacle, and generating a non-spanned obstacle grid map according to the position information of the non-spanned obstacle and the size information of the non-spanned obstacle; the grid value of the crossing barrier which exists in the crossing barrier grid map is set to be the height value of the crossing barrier, and the grid value of the crossing barrier which does not exist is set to be 0; the grid value of the non-crossing barrier which exists in the non-crossing barrier grid map is set to be 1, and the grid value of the non-crossing barrier which does not exist is set to be 0;

Determining a track envelope within the smooth track based on the tire width of the vehicle;

Inquiring the grid value in the track envelope domain of the current smooth track in the non-crossing barrier grid map, and jumping to the next smooth track to inquire the grid value in the track envelope domain when the grid value is not 0;

When the grid values are 0, inquiring the grid value in the track envelope domain of the current smooth track in the crossing barrier grid map, and calculating the relative roll angle of the current smooth track to obtain the path cost of the current smooth track;

And when all the smooth tracks in the track group to be selected are completely inquired, taking the smooth track with the minimum path cost as a vehicle running track.

2. The method of claim 1, wherein the generating a set of candidate trajectories based on the initial state and the final planned state; the track group to be selected comprises a plurality of smooth tracks from the initial state to the planning final state, and specifically comprises:

Describing the initial state and the planning final state by adopting a five-term polynomial, and converting the described initial state and the described planning final state into a global coordinate system to obtain a track group to be selected; the global coordinate system is a coordinate system established based on vehicle position and heading information.

3. The method for planning a path taking into account characteristics of a road surface obstacle according to claim 1, wherein the determining process of the off-line vehicle obstacle destabilization boundary is:

Based on a vehicle dynamics model, adopting a simulation engine to simulate and obtain the deformation of a suspension damper and the tire deformation response of the vehicle under different working conditions;

According to the vehicle vertical smoothness evaluation method based on priori knowledge, the deformation of the suspension damper is used as a standard, and the tire deformation response is divided to obtain a response boundary for representing the instability of the vehicle vertical obstacle;

based on the response boundary of the vertical obstacle instability of the vehicle, constructing a mapping between the vehicle speed and the obstacle size and whether the vertical instability exists in the vehicle or not in a lookup table mode; and when the height of the obstacle is larger than a preset threshold value, determining that the vehicle is unstable due to the vertical obstacle.

4. The method for planning a path taking into account characteristics of a road surface obstacle according to claim 1, wherein the generating a traversable obstacle grid map from position information of a traversable obstacle and size information of a traversable obstacle, specifically comprises:

Constructing a grid map coordinate system based on the perception range of the vehicle obstacle by taking the current position of the vehicle as an origin and the current heading of the vehicle as the positive direction of the x-axis;

Mapping position information of the straddlable obstacle and size information of the straddlable obstacle to the grid map coordinate system to obtain a straddlable obstacle grid map;

Mapping the location information of the non-traversable obstacle and the size information of the non-traversable obstacle to the grid map coordinate system to obtain a non-traversable obstacle grid map.

5. A path planning system that accounts for road obstacle characteristics, comprising:

The vehicle sensing module is used for acquiring current lane information, current vehicle position information, current vehicle pose information, current vehicle speed information, current lane obstacle position information and obstacle size information; the lane information includes: boundary information and lane line information of a lane;

The system comprises a track group to be selected generation module, a track group selection module and a track group selection module, wherein the track group to be selected generation module is used for carrying out projection conversion on the position information of the current vehicle and the pose information of the current vehicle according to a local S-L coordinate system to obtain an initial state, determining a planning final state in a planning time domain according to the lane line information and the speed information of the current vehicle, and generating the track group to be selected based on the initial state and the planning final state; the local S-L coordinate system is a coordinate system between a longitudinal distance and a lateral offset which are established based on a lane center reference line; the longitudinal distance is a longitudinal distance advancing along the lane center reference line; the lateral offset is relative to the lane center reference line; the track group to be selected comprises a plurality of smooth tracks from the initial state to the planning final state;

the off-line obstacle instability judging module is used for classifying the obstacles in the smooth track based on the off-line vehicle obstacle instability boundary and the size information of the obstacles to obtain the spanable obstacles and the non-spanable obstacles;

A classification obstacle map generation module for generating a spanable obstacle grid map according to the position information of the spanable obstacle and the size information of the spanable obstacle, and generating a non-spanable obstacle grid map according to the position information of the non-spanable obstacle and the size information of the non-spanable obstacle; the grid value of the crossing barrier which exists in the crossing barrier grid map is set to be the height value of the crossing barrier, and the grid value of the crossing barrier which does not exist is set to be 0; the grid value of the non-crossing barrier which exists in the non-crossing barrier grid map is set to be 1, and the grid value of the non-crossing barrier which does not exist is set to be 0;

the track expansion generation module is used for determining a track envelope domain in the smooth track based on the width of the tire of the vehicle;

The collision detection module is used for inquiring the grid value in the track envelope domain of the current smooth track in the non-crossing obstacle grid map, and jumping to the next smooth track to inquire the grid value in the track envelope domain when the grid value is not 0;

The cost calculation module is used for inquiring the grid value in the track envelope domain of the current smooth track in the crossing barrier grid map when the grid values are 0, and calculating the relative roll angle of the current smooth track to obtain the path cost of the current smooth track;

and the vehicle running track output module is used for taking the smooth track with the minimum path cost as the vehicle running track after all the smooth tracks in the track group to be selected are queried.

6. The path planning system for considering road obstacle characteristics as set forth in claim 5, wherein the candidate trajectory set generation module comprises:

The track group to be selected generating unit is used for describing the initial state and the planning final state by adopting a five-term polynomial, and converting the described initial state and the described planning final state into a global coordinate system to obtain a track group to be selected; the global coordinate system is a coordinate system established based on vehicle position and heading information.

7. The path planning system in consideration of road surface obstacle characteristics as set forth in claim 5, wherein the off-line obstacle instability determination module comprises:

The simulation unit is used for obtaining the response of the deformation of the suspension damper and the deformation of the tire of the vehicle under different working conditions by adopting a simulation engine based on the vehicle dynamics model;

The dividing unit is used for dividing the tire deformation response to obtain a response boundary for representing the instability of the vertical obstacle of the vehicle by taking the deformation of the suspension damper as a standard according to the prior knowledge-based vehicle vertical smoothness evaluation method;

The first mapping unit is used for constructing a mapping between the vehicle speed and the size of the obstacle and whether the vehicle has vertical instability or not in a lookup table mode based on the response boundary of the vertical obstacle instability of the vehicle; and when the height of the obstacle is larger than a preset threshold value, determining that the vehicle is unstable due to the vertical obstacle.

8. The path planning system in consideration of road obstacle characteristics as set forth in claim 5, wherein the classification obstacle map generation module comprises:

The coordinate system construction unit is used for constructing a grid map coordinate system based on the perception range of the vehicle obstacle by taking the position of the current vehicle as an origin and the current heading of the vehicle as the positive direction of the x axis;

a second mapping unit, configured to map position information of the spanned obstacle and size information of the spanned obstacle to the grid map coordinate system to obtain a spanned obstacle grid map;

and a third mapping unit, configured to map the location information of the non-traversable obstacle and the size information of the non-traversable obstacle to the grid map coordinate system to obtain a non-traversable obstacle grid map.