CN111452786B - Obstacle avoidance method and system for unmanned vehicle - Google Patents

Obstacle avoidance method and system for unmanned vehicle Download PDF

Info

Publication number
CN111452786B
CN111452786B CN202010288571.5A CN202010288571A CN111452786B CN 111452786 B CN111452786 B CN 111452786B CN 202010288571 A CN202010288571 A CN 202010288571A CN 111452786 B CN111452786 B CN 111452786B
Authority
CN
China
Prior art keywords
vehicle
obstacle avoidance
point
constructing
constraint condition
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010288571.5A
Other languages
Chinese (zh)
Other versions
CN111452786A (en
Inventor
邹渊
张旭东
孙逢春
杜广泽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202010288571.5A priority Critical patent/CN111452786B/en
Publication of CN111452786A publication Critical patent/CN111452786A/en
Application granted granted Critical
Publication of CN111452786B publication Critical patent/CN111452786B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

The invention provides an obstacle avoidance method and system for an unmanned vehicle, wherein the method comprises the following steps: determining a vehicle motion area; constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area; adopting an hp self-adaptive pseudo-spectrum method to convert the vehicle obstacle avoidance segmental optimal control model of the continuous state space into a vehicle obstacle avoidance segmental optimal control model containing an optimized variable; solving a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an interior point method, and obtaining expected longitudinal acceleration and front wheel corners of a vehicle in a prediction time domain; calculating the throttle opening according to the expected longitudinal acceleration; calculating a steering wheel angle according to the front wheel steering angle; and realizing vehicle control according to the accelerator opening and the steering wheel angle. The unmanned vehicle obstacle avoidance method disclosed by the invention can realize unmanned driving under the condition that an off-line map and a reference track are unknown, can realize unmanned driving on an unstructured road, and expands application places.

Description

Obstacle avoidance method and system for unmanned vehicle
Technical Field
The invention relates to the technical field of unmanned vehicle obstacle avoidance, in particular to an unmanned vehicle obstacle avoidance method and system.
Background
As one of unmanned platforms, ground unmanned vehicles are taking on more and more important functions and tasks, both in the civilian and military fields. Obstacle avoidance of an unmanned vehicle system is an important functional requirement, and the function relates to a vehicle environment perception technology, a planning technology and a control technology. The main idea of unmanned driving is to realize trajectory planning based on the sensing result and further perform dynamics control.
At present, a vehicle obstacle avoidance method based on an offline map and under the condition that a reference track is unknown is not mature, and obstacle avoidance can be realized only by specifying a vehicle with the reference track. The unstructured road comprises more elements such as obstacles and trees with complex shapes, the sizes and the shapes of the obstacles are not easy to describe, and the size and the shapes of the obstacles are limited by the characteristics of grid shapes and low resolution of laser radars at a distance, so that distortion is easily generated in the process of reconstructing a map, and the obstacle avoidance effect of the unmanned vehicle is influenced.
Disclosure of Invention
Based on the above, the invention aims to provide an unmanned vehicle obstacle avoidance method and system, which can realize unmanned driving under the condition that an off-line map and a reference track are unknown, and can also realize unmanned driving on an unstructured road.
In order to achieve the purpose, the invention provides an obstacle avoidance method for an unmanned vehicle, which comprises the following steps:
step S1: determining a vehicle motion area;
step S2: constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area;
step S3: adopting an hp self-adaptive pseudo-spectrum method to convert the vehicle obstacle avoidance segmental optimal control model of the continuous state space into a vehicle obstacle avoidance segmental optimal control model containing an optimized variable;
step S4: solving a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an interior point method, and obtaining expected longitudinal acceleration and front wheel corners of a vehicle in a prediction time domain;
step S5: calculating the throttle opening according to the expected longitudinal acceleration;
step S6: calculating a steering wheel angle according to the front wheel steering angle;
step S7: and realizing vehicle control according to the accelerator opening and the steering wheel angle.
Optionally, the determining the vehicle motion area specifically includes:
step S11: acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information;
step S12: judging whether the point cloud data in a certain angle direction reaches a set farthest boundary or not in a polar coordinate system of the vehicle; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and an upper polar diameter and a lower polar diameter which correspond to the arc form a sector area in the local map; if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; fitting all continuous barrier points into a straight line, and combining upper and lower pole diameters corresponding to the straight line to form a triangular area in a local map;
step S13: judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; selecting a fan-shaped area closest to the target point as an area where a planning end point is located according to a principle of the closest distance;
step S14: and constructing a vehicle motion area based on the triangular area corresponding to the vehicle and the area where the planned end point is located.
Optionally, before step S12, the method further includes:
and removing invalid points and performing down-sampling treatment on the point cloud data.
Optionally, a vehicle obstacle avoidance segmented optimal control model is constructed based on the vehicle motion area, and the method specifically includes:
step S21: constructing an objective function;
step S22: constructing vehicle dynamics constraint conditions;
step S23: constructing a vehicle state quantity constraint condition;
step S24: constructing an optimized control quantity constraint condition;
step S25: constructing a vehicle safety index constraint condition;
step S26: constructing a vehicle position constraint condition;
step S27: constructing a state quantity continuous constraint condition;
step S28: constructing constraint conditions of the vehicle at a planning starting point;
step S29: and constructing the constraint condition of the vehicle at the planning end point.
Optionally, the throttle opening is calculated according to the expected longitudinal acceleration, and the specific formula is as follows:
Figure BDA0002449500490000031
wherein pad (t) is the throttle opening, e (t) is the desired longitudinal acceleration ax(t) error from the actual longitudinal acceleration a (t), KP，KiAnd KdProportional, integral and differential coefficients, T, of the controllerCTo control the time domain.
Optionally, the steering wheel angle is calculated according to a front wheel steering angle, and the specific formula is as follows:
β(t)＝γ·δ(t),t＜TC；
wherein beta (T) is the steering wheel angle, TCTo control the time domain, γ is the steering gear ratio and δ (t) is the front wheel steering angle.
The invention also provides an obstacle avoidance system for the unmanned vehicle, which comprises:
the vehicle motion region determining module is used for determining a vehicle motion region;
the vehicle obstacle avoidance segmented optimal control model determining module is used for constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area;
the conversion module is used for converting the vehicle obstacle avoidance segmented optimal control model of the continuous state space into a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an hp self-adaptive pseudo-spectral method;
the solving module is used for solving the vehicle obstacle avoidance segmented optimal control model containing the optimized variables by adopting an interior point method to obtain the expected longitudinal acceleration and the front wheel corner of the vehicle in the prediction time domain;
the accelerator opening determining module is used for calculating the accelerator opening according to the expected longitudinal acceleration;
the steering wheel rotating angle determining module is used for calculating the steering wheel rotating angle according to the front wheel rotating angle;
and the control module is used for realizing vehicle control according to the accelerator opening and the steering wheel turning angle.
Optionally, the vehicle motion region determining module specifically includes:
an acquisition unit for acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information;
the region determining unit is used for judging whether the point cloud data in a certain angle direction reaches a set farthest boundary in a vehicle polar coordinate system; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and an upper polar diameter and a lower polar diameter which correspond to the arc form a sector area in the local map; if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; fitting all continuous barrier points into a straight line, and combining upper and lower pole diameters corresponding to the straight line to form a triangular area in a local map;
the judging unit is used for judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; selecting a fan-shaped area closest to the target point as an area where a planning end point is located according to a principle of the closest distance;
and the vehicle motion area determining unit is used for constructing a vehicle motion area based on the triangular area corresponding to the vehicle and the area where the planning terminal point is located.
Optionally, the vehicle motion region determining module further includes:
and the preprocessing unit is used for removing invalid points and performing down-sampling processing on the point cloud data.
Optionally, the module for determining the optimal control model of the vehicle obstacle avoidance segment specifically includes:
the target function constructing unit is used for constructing a target function;
the first constraint condition construction unit is used for constructing vehicle dynamic constraint conditions;
a second constraint condition construction unit for constructing a vehicle state quantity constraint condition;
the third constraint condition construction unit is used for constructing an optimized control quantity constraint condition;
the fourth constraint condition construction unit is used for constructing a vehicle safety index constraint condition;
a fifth constraint condition construction unit for constructing a vehicle position constraint condition;
a sixth constraint condition construction unit for constructing a state quantity continuous constraint condition;
a seventh constraint condition construction unit, configured to construct a constraint condition of the vehicle at the planned starting point;
and the eighth constraint condition construction unit is used for constructing the constraint condition of the vehicle at the planning end point.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention provides an obstacle avoidance method and system for an unmanned vehicle, wherein the method comprises the following steps: determining a vehicle motion area; constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area; adopting an hp self-adaptive pseudo-spectrum method to convert the vehicle obstacle avoidance segmental optimal control model of the continuous state space into a vehicle obstacle avoidance segmental optimal control model containing an optimized variable; solving a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an interior point method, and obtaining expected longitudinal acceleration and front wheel corners of a vehicle in a prediction time domain; calculating the throttle opening according to the expected longitudinal acceleration; calculating a steering wheel angle according to the front wheel steering angle; and realizing vehicle control according to the accelerator opening and the steering wheel angle. The unmanned vehicle obstacle avoidance method disclosed by the invention can realize unmanned driving on a structured road, can realize unmanned driving under the condition that an off-line map and a reference track are unknown, and can even realize unmanned driving on an unstructured road, thereby expanding the application field.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a flow chart of an obstacle avoidance method for an unmanned vehicle according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of cost function parameters according to an embodiment of the present invention;
FIG. 3 is a schematic diagram illustrating a local map analysis according to an embodiment of the present invention;
fig. 4 is a structural diagram of an obstacle avoidance system of an unmanned vehicle according to an embodiment of the invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide an unmanned vehicle obstacle avoidance method and system, which can realize unmanned driving under the condition that an off-line map and a reference track are unknown and can also realize unmanned driving on an unstructured road.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Fig. 1 is a flowchart of an obstacle avoidance method for an unmanned vehicle according to an embodiment of the present invention, and as shown in fig. 1, the present invention provides an obstacle avoidance method for an unmanned vehicle, where the method includes:
step S1: a vehicle motion zone is determined.
Step S2: and constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area.
Step S3: and converting the vehicle obstacle avoidance segmented optimal control model of the continuous state space into a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an hp self-adaptive pseudo-spectral method.
Step S4: and solving the vehicle obstacle avoidance segmented optimal control model containing the optimized variables by adopting an inner point method to obtain the expected longitudinal acceleration and the front wheel corner of the vehicle in the prediction time domain.
Step S5: and calculating the throttle opening according to the expected longitudinal acceleration.
Step S6: the steering wheel angle is calculated from the front wheel angle.
Step S7: and realizing vehicle control according to the accelerator opening and the steering wheel angle.
The individual steps are discussed in detail below:
step S1: the determining the vehicle motion area specifically comprises:
step S11: acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information. The point cloud data in the invention can be directly obtained through a vehicle-mounted sensor, wherein the vehicle-mounted sensor is a laser radar, a point cloud millimeter wave radar or a vehicle-mounted camera; the point cloud data of part of working conditions around the vehicle can be acquired by receiving the point cloud data captured by the road base sensor in the vehicle-road cooperative system based on the distributed vehicle-mounted networking equipment in communication modes such as LTE-V or DSRC.
Step S12: judging whether the point cloud data in a certain angle direction reaches a set farthest boundary or not in a polar coordinate system of the vehicle; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and the upper and lower pole diameters corresponding to the arc form a sector area in the local map shown in fig. 2- (a); if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; and fitting all continuous obstacle points into a straight line, and combining the upper and lower pole diameters corresponding to the straight line to form a triangular area in the local map shown in the part (a) of FIG. 2.
Dividing a vehicle passable area into a plurality of areas according to the shape, wherein the sector area indicates that the boundary of the area reaches the maximum range which can be detected by a sensor, namely no obstacle exists in the sector area; the triangular region indicates that the boundary of the region does not reach the set farthest boundary value, that is, an obstacle exists at the end of the region. Fig. 2- (b) shows the result obtained after the area division, in which all areas can be used as the vehicle driving area, the gray background represents the triangular area, and the white background represents the sector area.
Step S13: judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; and according to the principle of the closest distance, selecting the fan-shaped area closest to the target point as the area where the planning end point is located.
Specifically, in the obstacle avoidance problem, the vehicle should travel to reach a target point specified in advance under the collision-free condition, that is, a point G shown in fig. 2- (b), and the vehicle should approach the target point as fast as possible, so that a planning end point is selected on the boundary of the sector area closest to the point G, and thus, the sector area closest to the target point is selected as the range where the planning end point is located according to the principle of closest distance. As shown in fig. 2- (b), the point G is located in the polar axis range shown by the sector area 4 in the polar coordinate system, so the planned end point of the vehicle should be in the area 4, and the vehicle is expected to pass through two areas 3 and 4 under the current local map, and the planned end point is represented as a point M in the area 4.
Step S14: and constructing a vehicle motion area based on the triangular area corresponding to the vehicle and the area where the planned end point is located.
Specifically, after the planned terminal is selected, the positions where the vehicles pass through can be represented by triangular areas or fan-shaped areas. As shown in fig. 2- (c), the boundaries of the 3, 4 two vehicle motion areas are replaced with secant lines. The boundary of the vehicle motion region is described using a mathematical method, and thus the vehicle motion region may be represented by a set of inequalities in a local map coordinate system.
Before step S12, the method further includes:
step S15: and removing invalid points and performing down-sampling treatment on the point cloud data.
After the area division is finished, because the vehicles in each vehicle motion area have different constraint conditions, the vehicle obstacle avoidance problem is constructed into a segmented optimal control problem according to the vehicle motion area divided in the step S1. In the state shown in fig. 2, the obstacle avoidance problem is divided into two sections, the first section corresponds to the area No. 3, and the second section corresponds to the area No. 4. And constructing an optimal control problem according to an objective function, vehicle dynamic constraints, position constraints, safety constraints, continuity constraints, constraints at the planning starting point and constraints at the planning ending point, wherein in the example shown in the figure 2, the number of the vehicle segments N is 2.
Step S2: based on vehicle motion region constructs for vehicle obstacle avoidance segmentation optimal control model, specifically includes:
step S21: constructing an objective function; the initial formula of the objective function is as follows:
Figure BDA0002449500490000071
after the above formula is transformed, the specific formula of the objective function is obtained as follows:
Figure BDA0002449500490000081
wherein J is a cost function,
Figure BDA0002449500490000082
w1，w2，w3，w4，w5are all weight coefficients, xgAnd ygRespectively represent the horizontal and vertical coordinates of the target point under the local map coordinate system,
Figure BDA0002449500490000083
is the azimuth angle of the target point in the local map coordinate system,
Figure BDA0002449500490000084
planning a terminal point for a vehicleCourse u in local map coordinate systemtargetDesired speed for vehicle travel, yrRepresenting the longitudinal axial coordinate, x (T), of the reference pointp) And y (T)p) Planning the horizontal and vertical coordinates of the terminal point of the vehicle in the local map coordinate system, wherein x (0) and y (0) are the horizontal and vertical coordinates of the starting point of the vehicle in the local map coordinate system, and s1For the distance, s, from the target point to the planned end point0Distance of target point to starting point, TPFor predicting the time domain, u (t) is the longitudinal quota of the vehicle, y (t) is the y-axis coordinate of the vehicle at the time t, and ω (t) is the yaw rate of the vehicle, as shown in fig. 3.
Step S22: constructing vehicle dynamics constraint conditions; the vehicle dynamics constraint condition is specifically defined as:
Figure BDA0002449500490000085
wherein,
Figure BDA0002449500490000086
quantity of state
Figure BDA0002449500490000087
Respectively representing the x-axis coordinate, the y-axis coordinate, the vehicle heading, the longitudinal vehicle speed, the transverse vehicle speed and the transverse standard angular speed of the vehicle, and the control quantity mu is [ a ═x,δ]T，axDelta is the longitudinal acceleration of the vehicle and the front wheel slip angle, FyfAnd FyrRespectively front and rear axial side forces, kfFor wheel cornering stiffness, LfAnd LrRespectively the horizontal distance from the front and rear axle to the mass center of the vehicle, m is the mass of the vehicle, IzIs the moment of inertia of the vehicle.
In order to make the whole formula clearer, the time t is omitted from all the parameters.
Step S23: constructing a vehicle state quantity constraint condition; the vehicle state quantity constraint condition is specifically defined as:
Figure BDA0002449500490000091
wherein, X(i)(t) is the state quantity of the vehicle at the ith stage at the time t,
Figure BDA0002449500490000092
umaxfor maximum longitudinal speed constraint of the vehicle, vmaxFor maximum lateral velocity constraint of the vehicle, ωmaxThe highest yaw rate of the vehicle.
Step S24: constructing an optimized control quantity constraint condition, wherein the specific formula of the optimized control quantity constraint condition is as follows:
Figure BDA0002449500490000093
wherein, mu(i)(t) control quantity at the i-th stage at time t for optimal control problem,
Figure BDA0002449500490000094
Figure BDA0002449500490000095
axmaxfor maximum longitudinal acceleration of the vehicle, deltamaxThe maximum turning angle of the front wheels of the vehicle is determined by the mechanical structure of the vehicle.
Step S25: constructing a vehicle safety index constraint condition, wherein the specific formula of the vehicle safety index constraint condition is as follows:
Figure BDA0002449500490000096
wherein,
Figure BDA0002449500490000097
beta is the vehicle mass center slip angle, betamaxIs the maximum centroid slip angle, u is the longitudinal vehicle speed, v is the transverse vehicle speed, eta is the road surface adhesion coefficient, g is the gravity acceleration constant, h is the ground clearance of the centroid of the vehicle, omega is the yaw velocity of the vehicle, m is the vehicle mass,
Figure BDA0002449500490000098
FNRand FLRVertical ground forces, F, respectively, for the left and right side wheelsNminAnd B is the wheel track of the vehicle and theta is the vehicle roll angle.
Step S26: constructing a vehicle position constraint condition, wherein the specific formula of the vehicle position constraint condition is as follows:
P(i)x(i)(t)+Q(i)y(i)(t)＜R(i),t∈[Ti-1,Ti]；
wherein, P(i)，Q(i)，R(i)Are respectively triangular region constraint coefficients, which are all vectors with the length of 3, x(i)(t) and y(i)(T) controlling the abscissa and ordinate of the ith segment for optimal segmentation, TiThe end time of the ith section of the problem is optimally controlled.
Step S27: constructing a state quantity continuous constraint condition, wherein the state quantity continuous constraint condition has a specific formula as follows:
X(i)(Ti)＝X(i+1)(Ti)；
wherein, X(i)(Ti) Is TiThe state quantity, X, of the vehicle in the i-th segment at that moment(i+1)(Ti) Is TiThe state quantity of the vehicle in the (i + 1) th section at the moment.
This constraint is expressed as: for a certain segment of the segmentation problem, the state quantity of the last moment of the previous segment and the control quantity of the initial moment of the segment should be kept consistent in a continuous state space.
Step S28: constructing a constraint condition of a vehicle at a planning starting point, wherein an initial formula of the constraint condition of the vehicle at the planning starting point is as follows:
G[X(N)(TN)]≥0。
step S29: constructing a constraint condition of a vehicle at a planning end point, wherein an initial formula of the constraint condition of the vehicle at the planning end point is as follows:
T0＝0,X(1)(T0)＝X(T0),Tp≤Tpmax；
wherein, T0For the initial moment of the optimal control problem, X(1)(T0) For the initial moment, the state quantity of the vehicle in the 1 st segment, TpmaxFor a set temporal upper limit of prediction, TPIs the prediction time domain.
When the optimal control problem is solved each time, the local map coordinate system is superposed with the vehicle coordinate system of the planning starting point, namely at the initial moment:
X(T0)＝[0,0,0,u(T0),v(T0),ω(T0)]T(ii) a Wherein, X (T)0) As an initial time vehicle state quantity, u (T)0) Controlling quantity, v (T), for optimal control problem at initial time0) The vehicle lateral velocity at the initial moment, ω (T)0) The vehicle yaw rate at the initial time.
When the target point is outside the local map range, the coordinates of the planning end point are selected as being close to the local map boundary, namely:
Figure BDA0002449500490000101
wherein G [ X ](N)(TN)]As a function of the end time, X(N)(TN) The state quantity of the vehicle in the Nth stage at the end time, x (T)N) For the end time vehicle x-axis coordinate, y (T)N) Is the y-axis coordinate of the vehicle at the end time, epsilon is a radius scaling factor, RLThe maximum boundary distance of the local map.
When the target point is within the range of the local map, the target point is selected as the coordinate of the planning terminal point, namely:
Figure BDA0002449500490000111
wherein λ is an endpoint distance threshold.
Step S3: and converting the vehicle obstacle avoidance segmented optimal control model of the continuous state space into a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an hp self-adaptive pseudo-spectral method.
Specifically, for the piecewise optimization problem of the continuous state space, the existing hp adaptive pseudo-spectrum method is used to convert the optimization problem of the continuous state space into a nonlinear problem containing optimization variables, so that the piecewise optimization problem in the obstacle avoidance process is converted into the nonlinear optimization problem containing vehicle state quantities. The method uses a Legendre-Gauss-Radau point matching method, integrates the advantages of a finite element method and a global pseudo-spectral method, and can adaptively adjust the unit length and the order of a Lagrangian basis function and quickly converge to given precision.
Step S4: and solving the vehicle obstacle avoidance segmented optimal control model containing the optimized variables by adopting an inner point method to obtain the expected longitudinal acceleration and the front wheel corner of the vehicle in the prediction time domain.
Specifically, the existing nonlinear problem solver IPOPT is used for solving the nonlinear problem, and the IPOPT is used for solving the local optimal solution of the nonlinear problem by using an interior point method. The method decomposes equality and inequality constraints into a series of equality constraints and then solves a nonlinear problem only containing equality constraints by using an iterative method. The method can use the accurate Jacobian matrix and Hessian matrix so as to ensure the accuracy of the solution result. Obtaining an optimization result of the vehicle obstacle avoidance problem after the solution is completed, wherein the optimization result comprises a prediction time domain TPInternal state quantity
Figure BDA0002449500490000113
Sequence, and control quantity mu (t) ═ ax(t),δ(t)]TSequence, ax(t) is a predicted time-domain expected longitudinal acceleration of the vehicle, and δ (t) is a front wheel steering angle.
Before step S5, the method further includes: judging whether the expected acceleration value exceeds a vehicle acceleration set value or not; if the desired acceleration value does not exceed the vehicle acceleration set point, step S5 is executed.
Step S5: the throttle opening is calculated according to the expected longitudinal acceleration, and the specific formula is as follows:
Figure BDA0002449500490000112
wherein e (t) is the desired longitudinal acceleration ax(t) error from the actual longitudinal acceleration a (t), KP，KiAnd KdProportional, integral and differential coefficients, T, of the controllerCTo control the time domain.
Before step S6, the method further includes: it is determined whether the front wheel steering angle exceeds the vehicle front wheel steering angle set value, and if the front wheel steering angle does not exceed the vehicle front wheel steering angle set value, step S6 is executed.
Step S6: the steering wheel angle is calculated according to the front wheel steering angle, and the specific formula is as follows:
β(t)＝γ·δ(t),t＜TC；
wherein beta (T) is the steering wheel angle, TCTo control the time domain, γ is the steering gear ratio and δ (t) is the front wheel steering angle.
Fig. 4 is a structural diagram of an obstacle avoidance system of an unmanned vehicle according to an embodiment of the present invention, and as shown in fig. 4, the present invention further provides an obstacle avoidance system of an unmanned vehicle, where the system includes:
the vehicle motion region determining module 1 is used for determining a vehicle motion region;
the vehicle obstacle avoidance segmental optimal control model determining module 2 is used for constructing a vehicle obstacle avoidance segmental optimal control model of a continuous state space based on the vehicle motion area;
the conversion module 3 is used for converting the vehicle obstacle avoidance segmental optimal control model of the continuous state space into a vehicle obstacle avoidance segmental optimal control model containing optimized variables by adopting an hp self-adaptive pseudo-spectral method;
the solving module 4 is used for solving the vehicle obstacle avoidance segmented optimal control model containing the optimized variables by adopting an interior point method to obtain the expected longitudinal acceleration and the front wheel corner of the vehicle in the prediction time domain;
the accelerator opening determining module 5 is used for calculating the accelerator opening according to the expected longitudinal acceleration;
a steering wheel angle determination module 6 for calculating a steering wheel angle from the front wheel angle;
and the control module 7 is used for realizing vehicle control according to the accelerator opening and the steering wheel turning angle.
As an optional implementation manner, the vehicle motion region determining module 1 of the present invention specifically includes:
an acquisition unit for acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information;
the region determining unit is used for judging whether the point cloud data in a certain angle direction reaches a set farthest boundary in a vehicle polar coordinate system; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and an upper polar diameter and a lower polar diameter which correspond to the arc form a sector area in the local map; if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; fitting all continuous barrier points into a straight line, and combining upper and lower pole diameters corresponding to the straight line to form a triangular area in a local map;
the judging unit is used for judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; selecting a fan-shaped area closest to the target point as an area where a planning end point is located according to a principle of the closest distance;
and the vehicle motion area determining unit is used for constructing a vehicle motion area based on the triangular area corresponding to the vehicle and the area where the planning terminal point is located.
As an alternative embodiment, the vehicle motion region determination module 1 of the present invention further includes:
and the preprocessing unit is used for removing invalid points and performing down-sampling processing on the point cloud data.
As an optional implementation manner, the vehicle obstacle avoidance segmented optimal control model determining module 2 specifically includes:
the target function constructing unit is used for constructing a target function;
the first constraint condition construction unit is used for constructing vehicle dynamic constraint conditions;
a second constraint condition construction unit for constructing a vehicle state quantity constraint condition;
the third constraint condition construction unit is used for constructing an optimized control quantity constraint condition;
the fourth constraint condition construction unit is used for constructing a vehicle safety index constraint condition;
a fifth constraint condition construction unit for constructing a vehicle position constraint condition;
a sixth constraint condition construction unit for constructing a state quantity continuous constraint condition;
a seventh constraint condition construction unit, configured to construct a constraint condition of the vehicle at the planned starting point;
and the eighth constraint condition construction unit is used for constructing the constraint condition of the vehicle at the planning end point.
As an optional implementation, the system of the present invention further includes:
the first judgment module is used for judging whether the expected acceleration value exceeds a vehicle acceleration set value or not; if the desired acceleration value does not exceed the vehicle acceleration set point, the "accelerator opening determination module 5" is executed.
And a second judgment module for judging whether the front wheel steering angle exceeds the vehicle front wheel steering angle set value, and executing a steering wheel steering angle determination module 6 if the front wheel steering angle does not exceed the vehicle front wheel steering angle set value.
The method disclosed by the invention can divide the regional boundary according to the point cloud data under the conditions of no reference track and no off-line map, convert the problem into the segmented optimal control problem, solve and issue the control quantity, and realize vehicle obstacle avoidance.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.

Claims (8)

1. An obstacle avoidance method for an unmanned vehicle, the method comprising:
step S1: determining a vehicle motion area, specifically comprising:
step S11: acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information;
step S12: judging whether the point cloud data in a certain angle direction reaches a set farthest boundary or not in a polar coordinate system of the vehicle; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and an upper polar diameter and a lower polar diameter which correspond to the arc form a sector area in the local map; if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; fitting all continuous barrier points into a straight line, and combining upper and lower pole diameters corresponding to the straight line to form a triangular area in a local map;
step S13: judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; selecting a fan-shaped area closest to the target point as an area where a planning end point is located according to a principle of the closest distance;
step S14: constructing a vehicle motion area based on a triangular area corresponding to the vehicle and an area where a planning terminal point is located;
step S2: constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area;
step S3: adopting an hp self-adaptive pseudo-spectrum method to convert the vehicle obstacle avoidance segmental optimal control model of the continuous state space into a vehicle obstacle avoidance segmental optimal control model containing an optimized variable;
step S4: solving a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an interior point method, and obtaining expected longitudinal acceleration and front wheel corners of a vehicle in a prediction time domain;
step S5: calculating the throttle opening according to the expected longitudinal acceleration;
step S6: calculating a steering wheel angle according to the front wheel steering angle;
step S7: and realizing vehicle control according to the accelerator opening and the steering wheel angle.
2. The obstacle avoidance method for the unmanned vehicle according to claim 1, further comprising, before step S12:
and removing invalid points and performing down-sampling treatment on the point cloud data.
3. The unmanned vehicle obstacle avoidance method according to claim 1, wherein a vehicle obstacle avoidance segmented optimal control model is constructed based on the vehicle motion area, and specifically comprises:
step S21: constructing an objective function;
step S22: constructing vehicle dynamics constraint conditions;
step S23: constructing a vehicle state quantity constraint condition;
step S24: constructing an optimized control quantity constraint condition;
step S25: constructing a vehicle safety index constraint condition;
step S26: constructing a vehicle position constraint condition;
step S27: constructing a state quantity continuous constraint condition;
step S28: constructing constraint conditions of the vehicle at a planning starting point;
step S29: and constructing the constraint condition of the vehicle at the planning end point.
4. The unmanned vehicle obstacle avoidance method of claim 1, wherein the throttle opening is calculated according to a desired longitudinal acceleration, and the specific formula is as follows:
Figure FDA0002909943890000021
wherein pad (t) is the throttle opening at time t, and e (t) is the desired longitudinal acceleration a at time txError of the actual longitudinal acceleration a (t) at time (t) and t, KP，KiAnd KdProportional, integral and differential coefficients, T, of the controllerCTo control the time domain.
5. The unmanned vehicle obstacle avoidance method of claim 1, wherein the steering wheel angle is calculated according to a front wheel steering angle, and the specific formula is as follows:
β(t)＝γ·δ(t),t＜TC；
wherein beta (T) is the steering wheel angle at time T, TCTo control the time domain, γ is the steering gear ratio and δ (t) is the front wheel steering angle at time t.
6. An unmanned vehicle obstacle avoidance system, the system comprising:
the vehicle motion region determining module is used for determining a vehicle motion region; the vehicle motion region determination module specifically includes:
an acquisition unit for acquiring point cloud data; the point cloud data is a set formed by reflection points of all laser beams; the reflection point contains position information;
the region determining unit is used for judging whether the point cloud data in a certain angle direction reaches a set farthest boundary in a vehicle polar coordinate system; if the point cloud data in a certain angle direction reaches a set farthest boundary value, the angle direction is considered to have no barrier point, all the continuous point cloud data which can reach the set farthest boundary form an arc, and an upper polar diameter and a lower polar diameter which correspond to the arc form a sector area in the local map; if the point cloud data in a certain angle direction does not reach the set farthest boundary, the angle direction is considered to have obstacles, and obstacle points are recorded; fitting all continuous barrier points into a straight line, and combining upper and lower pole diameters corresponding to the straight line to form a triangular area in a local map;
the judging unit is used for judging whether the target point falls within the range of the local map; if the target point is within the local map range, taking the target point as a planning terminal point; if the target point does not fall within the local map range, selecting a planning terminal point on the boundary of the fan-shaped area closest to the target point; selecting a fan-shaped area closest to the target point as an area where a planning end point is located according to a principle of the closest distance;
the vehicle motion area determining unit is used for constructing a vehicle motion area based on a triangular area corresponding to the vehicle and an area where the planning terminal point is located;
the vehicle obstacle avoidance segmented optimal control model determining module is used for constructing a vehicle obstacle avoidance segmented optimal control model of a continuous state space based on the vehicle motion area;
the conversion module is used for converting the vehicle obstacle avoidance segmented optimal control model of the continuous state space into a vehicle obstacle avoidance segmented optimal control model containing optimized variables by adopting an hp self-adaptive pseudo-spectral method;
the solving module is used for solving the vehicle obstacle avoidance segmented optimal control model containing the optimized variables by adopting an interior point method to obtain the expected longitudinal acceleration and the front wheel corner of the vehicle in the prediction time domain;
the accelerator opening determining module is used for calculating the accelerator opening according to the expected longitudinal acceleration;
the steering wheel rotating angle determining module is used for calculating the steering wheel rotating angle according to the front wheel rotating angle;
and the control module is used for realizing vehicle control according to the accelerator opening and the steering wheel turning angle.
7. The unmanned vehicle obstacle avoidance system of claim 6, wherein the vehicle motion zone determination module further comprises:
and the preprocessing unit is used for removing invalid points and performing down-sampling processing on the point cloud data.
8. The unmanned vehicle obstacle avoidance system of claim 6, wherein the vehicle obstacle avoidance segment optimal control model determining module specifically comprises:
the target function constructing unit is used for constructing a target function;
the first constraint condition construction unit is used for constructing vehicle dynamic constraint conditions;
a second constraint condition construction unit for constructing a vehicle state quantity constraint condition;
the third constraint condition construction unit is used for constructing an optimized control quantity constraint condition;
the fourth constraint condition construction unit is used for constructing a vehicle safety index constraint condition;
a fifth constraint condition construction unit for constructing a vehicle position constraint condition;
a sixth constraint condition construction unit for constructing a state quantity continuous constraint condition;
a seventh constraint condition construction unit, configured to construct a constraint condition of the vehicle at the planned starting point;
and the eighth constraint condition construction unit is used for constructing the constraint condition of the vehicle at the planning end point.
CN202010288571.5A 2020-04-14 2020-04-14 Obstacle avoidance method and system for unmanned vehicle Active CN111452786B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010288571.5A CN111452786B (en) 2020-04-14 2020-04-14 Obstacle avoidance method and system for unmanned vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010288571.5A CN111452786B (en) 2020-04-14 2020-04-14 Obstacle avoidance method and system for unmanned vehicle

Publications (2)

Publication Number Publication Date
CN111452786A CN111452786A (en) 2020-07-28
CN111452786B true CN111452786B (en) 2021-03-19

Family

ID=71675502

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010288571.5A Active CN111452786B (en) 2020-04-14 2020-04-14 Obstacle avoidance method and system for unmanned vehicle

Country Status (1)

Country Link
CN (1) CN111452786B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113238563B (en) * 2021-06-04 2022-07-22 重庆大学 High-real-time automatic driving motion planning method
CN117111479B (en) * 2023-10-23 2024-01-30 深圳市智绘科技有限公司 Robot control method, apparatus, electronic device and readable storage medium
CN118114791B (en) * 2024-01-18 2024-10-15 青岛睿哲信息技术有限公司 Order data management method and system
CN117666594B (en) * 2024-02-02 2024-06-07 上海友道智途科技有限公司 Motion planning method, system, equipment and medium based on blocking Gaussian elimination

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106021784B (en) * 2016-05-31 2019-01-11 北京航空航天大学 A kind of full track mark optimum design method based on bilevel optimization strategy
US10837773B2 (en) * 2016-12-30 2020-11-17 DeepMap Inc. Detection of vertical structures based on LiDAR scanner data for high-definition maps for autonomous vehicles
US10754341B2 (en) * 2018-02-07 2020-08-25 Baidu Usa Llc Systems and methods for accelerated curve projection
CN112203918B (en) * 2018-05-31 2023-11-21 北美日产公司 Trajectory planning
US10823575B2 (en) * 2018-06-27 2020-11-03 Baidu Usa Llc Reference line smoothing method using piecewise spiral curves with weighted geometry costs
CN110377039B (en) * 2019-07-26 2021-07-30 吉林大学 Vehicle obstacle avoidance track planning and tracking control method

Also Published As

Publication number Publication date
CN111452786A (en) 2020-07-28

Similar Documents

Publication Publication Date Title
CN111452786B (en) Obstacle avoidance method and system for unmanned vehicle
CN112733270B (en) System and method for predicting vehicle running track and evaluating risk degree of track deviation
CN109669461B (en) Decision-making system for automatically driving vehicle under complex working condition and track planning method thereof
CN106080598B (en) Real-time goal pace control
CN106681327B (en) A kind of the intelligent driving transverse and longitudinal decoupling control method and system of big inertial electrodynamic car
US9244462B2 (en) Vehicle trajectory planning for autonomous vehicles
CN111142091B (en) Automatic driving system laser radar online calibration method fusing vehicle-mounted information
KR100939039B1 (en) Vehicle control device
CN113246974B (en) Risk avoidance/loss reduction control method in unmanned emergency scene, storage medium and electronic device
CN109799828B (en) Transverse and longitudinal coordination control method for automatic driving vehicle
CN107085424A (en) Pre- for automatic Pilot takes aim at crosswise joint
CN113819914A (en) Map construction method and device
CN110525436A (en) Vehicle lane-changing control method, device, vehicle and storage medium
JP2007022117A (en) Vehicle stabilization control system
CN114942642A (en) Unmanned automobile track planning method
EP4049916A1 (en) Vehicle control method, vehicle control system, and vehicle
CN113819905A (en) Multi-sensor fusion-based odometer method and device
US20210139007A1 (en) Enhanced vehicle operation
CN116080754B (en) Transverse control method for autonomous driving of vehicle
JP7185511B2 (en) Vehicle travel control device
CN115042816B (en) Path tracking method, device, apparatus, medium, and program
CN115303265A (en) Vehicle obstacle avoidance control method and device and vehicle
CN115214616A (en) Travel control system and travel control method
CN116300971A (en) Traction sliding control method and device for civil aircraft, tractor and storage medium
JP6898636B2 (en) Vehicle travel control device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant