CN111089594A - Autonomous parking trajectory planning method suitable for multiple scenes - Google Patents

Autonomous parking trajectory planning method suitable for multiple scenes Download PDF

Info

Publication number
CN111089594A
CN111089594A CN201911391911.0A CN201911391911A CN111089594A CN 111089594 A CN111089594 A CN 111089594A CN 201911391911 A CN201911391911 A CN 201911391911A CN 111089594 A CN111089594 A CN 111089594A
Authority
CN
China
Prior art keywords
vehicle
virtual
vertex
expansion coefficient
time
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.)
Granted
Application number
CN201911391911.0A
Other languages
Chinese (zh)
Other versions
CN111089594B (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN201911391911.0A priority Critical patent/CN111089594B/en
Publication of CN111089594A publication Critical patent/CN111089594A/en
Priority to PCT/CN2020/125930 priority patent/WO2021135617A1/en
Application granted granted Critical
Publication of CN111089594B publication Critical patent/CN111089594B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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/0002—Automatic control, details of type of controller or control system architecture
    • 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/0021—Differentiating means
    • 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
    • B60W2050/0034—Multiple-track, 2D vehicle model, e.g. four-wheel model
    • 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/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation

Abstract

The invention discloses an autonomous parking trajectory planning method suitable for multiple scenes, which comprises the following steps: (1) constructing a parking map; (2) determining the current position of the vehicle and the position of a target parking space; (3) determining physical parameters of the vehicle, and initializing the expansion coefficient of a virtual protection frame of the vehicle; (4) expressing a vehicle kinematic differential equation as a vehicle kinematic state constraint; (5) establishing collision avoidance restraint between the vehicle and the barrier by using the virtual protective frame and the expansion coefficient of the virtual protective frame; (6) establishing state variable and input variable constraints; (7) determining an optimization target and establishing an optimization problem; (8) solving an optimization problem by a solver to obtain an alternative autonomous parking track; (9) and (4) recalculating the expansion coefficient of the virtual guard frame, obtaining the final autonomous parking track if the stop condition is met, otherwise, updating the expansion coefficient of the virtual guard frame, and repeating the steps (4) to (8). By using the invention, the collision-free parking track can be planned in various scenes.

Description

Autonomous parking trajectory planning method suitable for multiple scenes
Technical Field
The invention belongs to the field of automatic driving track planning, and particularly relates to an autonomous parking track planning method suitable for multiple scenes.
Background
The automatic parking is an important function in automatic driving, partial commercial vehicles with automatic parking are already available on the market at present, the traditional automatic parking trajectory planning method mainly comprises two links, a parking map is divided into grids, a parking path is calculated in the grids by using search algorithms such as A, state lattices and the like, the path speed is calculated by using an optimization method according to the parking path, and the parking trajectory is finally formed.
The other method for calculating the parking track is an optimization method based on a vehicle kinematic model, the method combines two steps of path planning and speed matching by using optimal control to directly calculate the parking track, but the method generally needs to manually select an optimization point number, and after a continuous optimization problem is dispersed, a constraint violation problem often exists, and finally the path and an obstacle collide to cause the parking safety hidden trouble.
At present, there are some methods for calculating the shortest distance between an obstacle and a vehicle by using a dual method, and some trajectory planning methods regard the vehicle and the obstacle as polygons, and use an area method to represent collision avoidance constraints of the vehicle and the obstacle, and these methods still have constraint violation phenomena in optimization problems in the implementation process.
Aiming at the problems and the defects of the autonomous parking trajectory planning technology, the trajectory planning method which can ensure that the autonomous parking trajectory does not collide with the obstacle, and the generated trajectory is safe and feasible and is easy to implement by a lower-layer controller is found, and the method has very high practical value.
Disclosure of Invention
The invention provides the autonomous parking trajectory planning method suitable for multiple scenes, which has the characteristics of safety, feasibility, easiness in implementation and the like and has high practical application value.
An autonomous parking trajectory planning method suitable for multiple scenes is characterized by comprising the following steps:
(1) detecting a parking environment and constructing a parking map;
(2) determining the current position of the vehicle and the position of a target parking space;
(3) determining physical parameters of the vehicle, and initializing the expansion coefficient of a virtual protection frame of the vehicle;
(4) according to a vehicle kinematic differential equation, establishing vehicle motion state constraint;
(5) establishing collision avoidance restraint between the vehicle and the barrier by using the virtual protective frame and the expansion coefficient of the virtual protective frame;
(6) establishing state variable and input variable constraints according to vehicle mechanical principle limitations and comfort requirements;
(7) determining an optimization target and establishing an optimization problem;
(8) solving an optimization problem by using a nonlinear programming solver to obtain an alternative autonomous parking track;
(9) and (4) recalculating the expansion coefficient of the virtual protection frame, obtaining the final autonomous parking track if the expansion coefficient of the virtual protection frame meets the stop condition, otherwise, updating the expansion coefficient of the virtual protection frame, and repeating the steps (4) to (8).
The invention can plan a safe and feasible parking track in multiple scenes, and provides a track planning solution for automatic driving and autonomous parking.
In the step (1), when the parking map is constructed, a closed or semi-closed polygon is used for representing the obstacle.
In the step (2), the current position and the target parking space position of the vehicle are respectively expressed as
Figure BDA0002345199920000021
And
Figure BDA0002345199920000022
the vehicle center of gravity detection method specifically comprises an x-axis coordinate and a y-axis coordinate of the vehicle center of gravity under a global coordinate system, and an included angle between the vehicle advancing direction and the positive direction of the x axis of the global coordinate system.
In the step (3), the physical parameters of the vehicle to be determined comprise the width w and the wheel base L of the vehicle, and the initial value of the expansion coefficient of the virtual protection frame is α0＝1。
The specific process of the step (4) is as follows:
(4-1) describing a motion state of the vehicle using a differential equation as follows:
Figure BDA0002345199920000031
wherein X represents the X-axis coordinate of the center of gravity of the vehicle in the global coordinate system, Y represents the Y-axis coordinate of the center of gravity of the vehicle in the global coordinate system,
Figure BDA0002345199920000032
representing the included angle between the right front direction of the vehicle and the x axis;
Figure BDA0002345199920000033
and
Figure BDA0002345199920000034
respectively X, Y and
Figure BDA0002345199920000035
differentiation of (1); v represents the vehicle forward speed, δfRepresenting a front wheel steering angle; taking the differential equation as a kinematic model of the vehicle, and recording as:
Figure BDA0002345199920000036
wherein the content of the first and second substances,
Figure BDA0002345199920000037
denotes the state variable, u ═ v δf]TRepresenting an input variable;
(4-2) hypothesis ξ0,ξ1,...,ξNIs N +1 state variables to be planned, the state variable ξ at the known current time k according to the kinematic model of the vehiclekState variables of time, next time
Figure BDA0002345199920000038
Expressed as:
r1＝f(ξk,uk)
Figure BDA0002345199920000039
Figure BDA00023451999200000310
Figure BDA00023451999200000311
Figure BDA00023451999200000312
wherein r is1,r2,r3,r4Is an intermediate state value, ukAn input variable representing time k, and T represents a sampling period;
(4-3) establishing vehicle motion state constraint:
Figure BDA00023451999200000313
the specific process of the step (5) is as follows:
(5-1) obstacle composed of polygon OjIs represented by, defined as
Figure BDA0002345199920000041
Wherein the subscript J indicates the jth obstacle, for a total of J obstacles,
Figure BDA0002345199920000042
represents a polygon OjThe ith vertex in the clockwise direction,
Figure BDA0002345199920000043
v representing jth obstaclej1,Vj2The line segment of the vertex is analogized in the same way;
(5-2) the vehicle is represented by a rectangle E, defined as
Figure BDA0002345199920000044
Wherein E is1Indicates the left front vertex of the rectangle in which the vehicle is located, E2Indicating the right front vertex of the rectangle in which the vehicle is located, E3Indicating the right rear vertex of the rectangle in which the vehicle is located, E4The left rear vertex of the rectangle in which the vehicle is located is shown,
Figure BDA0002345199920000045
indicating the vehicle E1,E2The line segment of the vertex is analogized in the same way;
(5-3) defining a virtual protective frame E around the vehiclef
Figure BDA0002345199920000046
E and EfThe relationship of (A) is EfThe length in the direction parallel to the vehicle is equal to the corresponding length of E, EfA corresponding length in the direction perpendicular to the vehicle equal to E multiplied by the virtual bezel expansion coefficient α;
(5-4) representing the vehicle trajectory between adjacent time instants using a set of line segments
Figure BDA0002345199920000047
Wherein the content of the first and second substances,
Figure BDA0002345199920000048
on the virtual protective frame
Figure BDA0002345199920000049
The position of the point at the moment k, and so on;
(5-5) establishing a vehicle and obstacle avoidance collision restraint:
Figure BDA00023451999200000410
wherein the content of the first and second substances,
Figure BDA00023451999200000411
Figure BDA00023451999200000412
it is shown that the intersection is by-passed,
Figure BDA00023451999200000413
indicating vehicle trajectory
Figure BDA00023451999200000414
And obstacles
Figure BDA00023451999200000415
Non-intersecting.
In the step (6), the established state variable and input variable constraint concrete formula is as follows:
Figure BDA00023451999200000416
wherein v isminIndicating the minimum speed, v, at which the vehicle is moving forwardmaxRepresenting the maximum speed, δ, at which the vehicle is travellingfminIndicating the minimum angle, δ, of steering of the front wheelsfmaxIndicating the maximum angle of steering of the front wheels.
In the step (7), the optimization objective and the optimization problem are specifically as follows:
minT·N
s.t.
Figure BDA0002345199920000051
Figure BDA0002345199920000052
Figure BDA0002345199920000053
Figure BDA0002345199920000054
wherein minT.N is an optimization target and represents the minimum total time, T is a sampling period, N is the number of samples, s.t. represents obedience to the following constraints, ξ0State variable representing time 0, ξNRepresenting the state variable at time N.
In step (8), the information track included in the autonomous parking track is track
Figure BDA0002345199920000055
And corresponding operations
Figure BDA0002345199920000056
Wherein traj (k) represents an x-axis coordinate, a y-axis coordinate, and an included angle between a vehicle advancing direction and an x-axis of the vehicle in the global coordinate system at the time k, and u (k) represents a vehicle advancing speed and a front wheel steering angle at the time k.
In the step (9), the step of recalculating the expansion coefficient of the virtual protection frame is as follows:
(9-1) calculating a difference between the virtual fender frame and the curvature radius of the vehicle contour at each time:
Figure BDA0002345199920000057
Figure BDA0002345199920000058
Figure BDA0002345199920000059
Figure BDA00023451999200000510
wherein, δ R1|kIndicating the difference of the radius of curvature of the front left of the vehicle, δ R2|kIndicating the difference, δ R, in the radius of curvature of the front right of the vehicle3|kIndicating vehicleDifference between rear right-hand radius of curvature, δ R4|kIndicating the difference in the left rear radius of curvature of the vehicle,
Figure BDA00023451999200000511
represents the turning radius of the vertex of the left front of the rectangle where the vehicle is positioned at the moment k,
Figure BDA00023451999200000512
represents the turning radius of the right front vertex of the rectangle where the vehicle is located at the moment k,
Figure BDA00023451999200000513
indicating the turning radius of the right rear vertex of the rectangle in which the vehicle is located at the time point k,
Figure BDA00023451999200000514
represents the turning radius of the left rear vertex of the rectangle where the vehicle is located at the moment k,
Figure BDA0002345199920000061
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure BDA0002345199920000062
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure BDA0002345199920000063
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure BDA0002345199920000064
the turning radius at the time k of the left front vertex of the virtual fender frame is shown as follows
Figure BDA0002345199920000065
Figure BDA0002345199920000066
Figure BDA0002345199920000067
Figure BDA0002345199920000068
Figure BDA0002345199920000069
Figure BDA00023451999200000610
Figure BDA00023451999200000611
Figure BDA00023451999200000612
Wherein the content of the first and second substances,
Figure BDA00023451999200000613
is the vehicle turning curvature; w is expressed as vehicle width, lfIndicating the distance between the center of gravity of the vehicle and the head, lrExpressed as the distance between the center of gravity of the vehicle and the rear of the vehicle, α1|kThe virtual mask expansion coefficient represented as the vertex at the front left of the virtual mask at time k, α2|kThe virtual reticle expansion coefficient represented as the right front vertex of the virtual reticle at time k, α3|kThe virtual mask expansion coefficient represented as the right rear vertex of the virtual mask at time k, α4|kThe virtual protection frame expansion coefficient is expressed as the vertex at the left back of the virtual protection frame at the moment k;
(9-2) calculating the maximum error of the turning arc and the turning line segment:
Figure BDA0002345199920000071
Figure BDA0002345199920000072
Figure BDA0002345199920000073
Figure BDA0002345199920000074
wherein the content of the first and second substances,
Figure BDA0002345199920000075
represents the maximum error of the vertex at the front left of the virtual protective frame at the moment k,
Figure BDA0002345199920000076
represents the maximum error of the right front vertex of the virtual protective frame at the moment k,
Figure BDA0002345199920000077
expressed as the maximum error of the rear right vertex of the virtual protection frame at the time k,
Figure BDA0002345199920000078
the maximum error of the left rear vertex of the virtual protection frame at the moment k is represented;
(9-3) defining a distance function
Figure BDA0002345199920000079
Calculating the optimal virtual protection frame expansion coefficient
Figure BDA00023451999200000710
Wherein the content of the first and second substances,
Figure BDA00023451999200000711
representing distance function distancei|kMax represents the maximum value of the set;
if α*Absolute value of- α being less than 0.001, stopStopping iteration, and finally stopping the autonomous parking trajectory, otherwise, leading α0＝α*And (5) repeating the steps (4) to (8).
Compared with the prior art, the invention has the following beneficial effects:
1. the invention generates the parking track with time information, thereby facilitating the subsequent controller design.
2. The invention uses the concept of the virtual protection frame and the concept of the expansion coefficient of the virtual protection frame to ensure that the generated track does not collide with the barrier.
3. The method has no specific requirements on parking scenes, and can be widely applied to parking scenes such as backing up and warehousing, parking at a side position, parking at an oblique direction and the like.
Drawings
FIG. 1 is a parking scenario in an embodiment of the present invention;
FIG. 2 is a flow chart of an autonomous parking trajectory planning method suitable for multiple scenes according to the present invention;
fig. 3 is a final planned parking trajectory in the embodiment of the present invention.
Detailed Description
The invention will be described in further detail below with reference to the drawings and examples, which are intended to facilitate the understanding of the invention without limiting it in any way.
In the embodiment, a vehicle backing and warehousing is taken as an example, and the autonomous parking trajectory planning method suitable for multiple scenes is described in detail.
The vehicle needs to complete backing and parking in the parking scene shown in fig. 1, wherein the parking space is 5 meters long, the width is 2.5 meters, and the width of the operation road surface is 6 m.
As shown in fig. 2, an autonomous parking trajectory planning method suitable for multiple scenes includes:
step 1, detecting a parking environment and constructing a parking map, wherein obstacles are represented by closed or semi-closed polygons;
step 2, determining the current position of the vehicle and the position of the target parking space, wherein the position information of the current position of the vehicle in the global coordinate system is
Figure BDA0002345199920000081
The position information of the target parking space position in the global coordinate system is
Figure BDA0002345199920000082
And 3, determining vehicle physical parameters and initializing an expansion coefficient of the virtual protection frame, wherein the vehicle physical parameters to be determined are that the vehicle width w is 1.9m, the wheel base L is 2.7m, and the initialized value of the expansion coefficient of the virtual protection frame is α0＝1。
And 4, expressing a vehicle kinematic differential equation as vehicle motion state constraint, wherein the specific implementation mode is as follows:
step 4-1, the motion state of the vehicle is described using the following differential equation:
Figure BDA0002345199920000091
wherein X represents the X-axis coordinate of the center of gravity of the vehicle in the global coordinate system, Y represents the Y-axis coordinate of the center of gravity of the vehicle in the global coordinate system,
Figure BDA0002345199920000092
representing the included angle between the right front direction of the vehicle and the x axis; v represents the vehicle forward speed, δfIndicating the front wheel steering angle. The above differential equation is written as:
Figure BDA0002345199920000093
wherein the content of the first and second substances,
Figure BDA0002345199920000094
denotes the state variable, u ═ v δf]TRepresenting the input variables.
Step 4-2, assume ξ0,ξ1,...,ξNIs N +1 state variables to be planned, according to the kinematic model of the vehicle, at the known current state ξkThen, next oneState of the moment
Figure BDA0002345199920000095
Can be expressed as:
r1＝f(ξk,uk)
Figure BDA0002345199920000096
Figure BDA0002345199920000097
Figure BDA0002345199920000098
Figure BDA0002345199920000099
wherein r is1,r2,r3,r4Is the intermediate state value and T represents the sampling period, in this embodiment, N is taken to be 50.
Step 4-3, establishing vehicle motion state constraint:
Figure BDA00023451999200000910
step 5, establishing collision avoidance restraint between the vehicle and the barrier by using the virtual protective frame and the expansion coefficient of the virtual protective frame, wherein the specific implementation method comprises the following steps:
step 5-1, the obstacle is composed of a polygon OjIs represented by, defined as
Figure BDA00023451999200000911
Wherein the subscript J indicates the jth obstacle, for a total of J obstacles,
Figure BDA00023451999200000912
represents a polygon OjIn the clockwise directionThe (i) th vertex is (is) the vertex,
Figure BDA0002345199920000101
representing the jth obstacle
Figure BDA0002345199920000102
And the line segment of the vertex is connected, and the like.
Step 5-2, the vehicle is represented by rectangle E, defined as
Figure BDA0002345199920000103
Wherein E is1Indicates the left front vertex of the rectangle in which the vehicle is located, E2Indicating the right front vertex of the rectangle in which the vehicle is located, E3Indicating the right rear vertex of the rectangle in which the vehicle is located, E4The left rear vertex of the rectangle in which the vehicle is located is shown,
Figure BDA0002345199920000104
indicating the vehicle E1,E2And the line segment of the vertex is connected, and the like.
Step 5-3, defining a virtual protective frame E around the vehiclef
Figure BDA0002345199920000105
E and EfThe relationship of (A) is EfThe length in the direction parallel to the vehicle is equal to the corresponding length of E, EfThe length in the direction perpendicular to the vehicle is equal to the corresponding length of E multiplied by the virtual bezel expansion coefficient α.
Step 5-4, representing the vehicle track between adjacent moments by using a line segment set
Figure BDA0002345199920000106
Wherein
Figure BDA0002345199920000107
On the virtual protective frame
Figure BDA0002345199920000108
The position of the point at time k, and so on.
Step 5-5, establishing collision avoidance restraint of the vehicle and the barrier:
Figure BDA0002345199920000109
wherein
Figure BDA00023451999200001010
Step 6, establishing state variable and input variable constraints according to the mechanical principle limit and comfort requirement of the vehicle, wherein the specific implementation method comprises the following steps:
Figure BDA00023451999200001011
and 7, determining an optimization target, and establishing an optimization problem, wherein the specific form of the optimization problem is as follows:
minT·N
s.t.
Figure BDA0002345199920000111
Figure BDA0002345199920000112
Figure BDA0002345199920000113
Figure BDA0002345199920000114
and 8, solving the optimization problem by using a nonlinear programming solver to obtain an alternative autonomous parking track containing information with tracks
Figure BDA0002345199920000115
And corresponding operations
Figure BDA0002345199920000116
Step 9, recalculating the expansion coefficient of the virtual protection frame, wherein the specific implementation method comprises the following steps:
step 9-1, calculating the difference between the curvature radii of the virtual protective frame and the vehicle outline at each moment:
Figure BDA0002345199920000117
Figure BDA0002345199920000118
Figure BDA0002345199920000119
Figure BDA00023451999200001110
wherein the content of the first and second substances,
Figure BDA00023451999200001111
Figure BDA00023451999200001112
Figure BDA00023451999200001113
Figure BDA00023451999200001114
Figure BDA00023451999200001115
Figure BDA0002345199920000121
Figure BDA0002345199920000122
Figure BDA0002345199920000123
Figure BDA0002345199920000124
is the vehicle turning curvature.
Step 9-2, calculating the maximum error of the turning arc and the turning line segment:
Figure BDA0002345199920000125
Figure BDA0002345199920000126
Figure BDA0002345199920000127
Figure BDA0002345199920000128
step 9-3, defining a distance function
distancei|k＝ErrorEi|k-δRi|k
Calculating the optimal expansion coefficient of the virtual protective frame,
Figure BDA0002345199920000129
wherein the content of the first and second substances,
Figure BDA00023451999200001210
representing distance function distancei|kLower limit of (1), max tableThe maximum value of the set is shown.
If α*- α absolute value less than 0.001, stop iteration, final autonomous parking trajectory, otherwise order α0＝α*And repeating the steps 4 to 8.
The finally planned parking trajectory is shown in fig. 3, the vehicle from global coordinates
Figure BDA0002345199920000131
Starting from a target berth of
Figure BDA0002345199920000132
By using the method, the planned track can be ensured not to collide with the barrier, and the method has the advantages of safety and feasibility.
The embodiments described above are intended to illustrate the technical solutions and advantages of the present invention, and it should be understood that the above-mentioned embodiments are only specific embodiments of the present invention, and are not intended to limit the present invention, and any modifications, additions and equivalents made within the scope of the principles of the present invention should be included in the scope of the present invention.

Claims (10)

1. An autonomous parking trajectory planning method suitable for multiple scenes is characterized by comprising the following steps:
(1) detecting a parking environment and constructing a parking map;
(2) determining the current position of the vehicle and the position of a target parking space;
(3) determining physical parameters of the vehicle, and initializing the expansion coefficient of a virtual protection frame of the vehicle;
(4) according to a vehicle kinematic differential equation, establishing vehicle motion state constraint;
(5) establishing collision avoidance restraint between the vehicle and the barrier by using the virtual protective frame and the expansion coefficient of the virtual protective frame;
(6) establishing state variable and input variable constraints according to vehicle mechanical principle limitations and comfort requirements;
(7) determining an optimization target and establishing an optimization problem;
(8) solving an optimization problem by using a nonlinear programming solver to obtain an alternative autonomous parking track;
(9) and (4) recalculating the expansion coefficient of the virtual protection frame, obtaining the final autonomous parking track if the expansion coefficient of the virtual protection frame meets the stop condition, otherwise, updating the expansion coefficient of the virtual protection frame, and repeating the steps (4) to (8).
2. The method for planning the autonomous parking trajectory for multiple scenes according to claim 1, wherein in the step (1), the parking map is constructed by using closed or semi-closed polygons to represent the obstacles.
3. The method for planning the autonomous parking trajectory for multiple scenes according to claim 1, wherein in step (2), the current position and the target parking space position of the vehicle are respectively represented as
Figure FDA0002345199910000011
And
Figure FDA0002345199910000012
the vehicle center of gravity detection method specifically comprises an x-axis coordinate and a y-axis coordinate of the vehicle center of gravity under a global coordinate system, and an included angle between the vehicle advancing direction and the positive direction of the x axis of the global coordinate system.
4. The method for planning the autonomous parking trajectory in multiple scenes according to claim 3, wherein in the step (3), the physical parameters of the vehicle to be determined include a vehicle width w and a wheel base L, and the initial value of the expansion coefficient of the virtual fender box is α0＝1。
5. The method for planning the autonomous parking trajectory in multiple scenes according to claim 4, wherein the specific process of the step (4) is as follows:
(4-1) describing a motion state of the vehicle using a differential equation as follows:
Figure FDA0002345199910000021
wherein X represents the X-axis coordinate of the center of gravity of the vehicle in the global coordinate system, Y represents the Y-axis coordinate of the center of gravity of the vehicle in the global coordinate system,
Figure FDA0002345199910000022
representing the included angle between the right front direction of the vehicle and the x axis;
Figure FDA0002345199910000023
and
Figure FDA0002345199910000024
respectively X, Y and
Figure FDA0002345199910000025
differentiation of (1); v represents the vehicle forward speed, δfRepresenting a front wheel steering angle; taking the differential equation as a kinematic model of the vehicle, and recording as:
Figure FDA0002345199910000026
wherein the content of the first and second substances,
Figure FDA0002345199910000027
denotes the state variable, u ═ v δf]TRepresenting an input variable;
(4-2) hypothesis ξ0,ξ1,...,ξNIs N +1 state variables to be planned, the state variable ξ at the known current time k according to the kinematic model of the vehiclekState variables of time, next time
Figure FDA0002345199910000028
Expressed as:
r1＝f(ξk,uk)
Figure FDA0002345199910000029
Figure FDA00023451999100000210
Figure FDA00023451999100000211
Figure FDA00023451999100000212
wherein r is1,r2,r3,r4Is an intermediate state value, ukAn input variable representing time k, and T represents a sampling period;
(4-3) establishing vehicle motion state constraint:
Figure FDA00023451999100000213
6. the method for planning the autonomous parking trajectory for multiple scenes according to claim 5, wherein the specific process of the step (5) is as follows:
(5-1) obstacle composed of polygon OjIs represented by, defined as
Figure FDA0002345199910000031
Wherein the subscript J indicates the jth obstacle, for a total of J obstacles,
Figure FDA0002345199910000032
represents a polygon OjThe ith vertex in the clockwise direction,
Figure FDA0002345199910000033
representing the jth obstacle
Figure FDA0002345199910000034
The line segment of the vertex is analogized in the same way;
(5-2) the vehicle is represented by a rectangle E, defined as
Figure FDA0002345199910000035
Wherein E is1Indicates the left front vertex of the rectangle in which the vehicle is located, E2Indicating the right front vertex of the rectangle in which the vehicle is located, E3Indicating the right rear vertex of the rectangle in which the vehicle is located, E4The left rear vertex of the rectangle in which the vehicle is located is shown,
Figure FDA0002345199910000036
indicating the vehicle E1,E2The line segment of the vertex is analogized in the same way;
(5-3) defining a virtual protective frame E around the vehiclef
Figure FDA0002345199910000037
E and EfThe relationship of (A) is EfThe length in the direction parallel to the vehicle is equal to the corresponding length of E, EfA corresponding length in the direction perpendicular to the vehicle equal to E multiplied by the virtual bezel expansion coefficient α;
(5-4) representing the vehicle trajectory between adjacent time instants using a set of line segments
Figure FDA0002345199910000038
Wherein the content of the first and second substances,
Figure FDA0002345199910000039
on the virtual protective frame
Figure FDA00023451999100000310
The position of the point at the moment k, and so on;
(5-5) establishing a vehicle and obstacle avoidance collision restraint:
Figure FDA00023451999100000311
wherein the content of the first and second substances,
Figure FDA00023451999100000312
it is shown that the intersection is by-passed,
Figure FDA00023451999100000313
indicating vehicle trajectory
Figure FDA00023451999100000314
And obstacles
Figure FDA00023451999100000315
Non-intersecting.
7. The method for planning the autonomous parking trajectory in multiple scenes according to claim 6, wherein in the step (6), the specific formula of the established state variable and input variable constraints is as follows:
Figure FDA0002345199910000041
wherein v isminIndicating the minimum speed, v, at which the vehicle is moving forwardmaxRepresenting the maximum speed, δ, at which the vehicle is travellingfminIndicating the minimum angle, δ, of steering of the front wheelsfmaxIndicating the maximum angle of steering of the front wheels.
8. The method for planning the autonomous parking trajectory in multiple scenes according to claim 6, wherein in the step (7), the optimization objective and the optimization problem are specifically as follows:
minT·N
s.t.
Figure FDA0002345199910000042
Figure FDA0002345199910000043
Figure FDA0002345199910000044
Figure FDA0002345199910000045
wherein minT.N is an optimization target and represents the minimum total time, T is a sampling period, N is the number of samples, s.t. represents obedience to the following constraints, ξ0State variable representing time 0, ξNRepresenting the state variable at time N.
9. The method for planning an autonomous parking trajectory for multiple scenes according to claim 8, wherein in step (8), the autonomous parking trajectory includes information including a trajectory
Figure FDA0002345199910000046
And corresponding operations
Figure FDA0002345199910000047
Wherein traj (k) represents an x-axis coordinate, a y-axis coordinate, and an included angle between a vehicle advancing direction and an x-axis of the vehicle in the global coordinate system at the time k, and u (k) represents a vehicle advancing speed and a front wheel steering angle at the time k.
10. The method for planning the autonomous parking trajectory for multiple scenes according to claim 9, wherein in step (9), the step of recalculating the expansion coefficient of the virtual guard frame is as follows:
(9-1) calculating a difference between the virtual fender frame and the curvature radius of the vehicle contour at each time:
Figure FDA0002345199910000048
Figure FDA0002345199910000049
Figure FDA00023451999100000410
Figure FDA00023451999100000411
wherein, δ R1|kIndicating the difference of the radius of curvature of the front left of the vehicle, δ R2|kIndicating the difference, δ R, in the radius of curvature of the front right of the vehicle3|kIndicating the difference between the right rear radii of curvature of the vehicle, δ R4|kIndicating the difference in the left rear radius of curvature of the vehicle,
Figure FDA0002345199910000051
represents the turning radius of the vertex of the left front of the rectangle where the vehicle is positioned at the moment k,
Figure FDA0002345199910000052
represents the turning radius of the right front vertex of the rectangle where the vehicle is located at the moment k,
Figure FDA0002345199910000053
indicating the turning radius of the right rear vertex of the rectangle in which the vehicle is located at the time point k,
Figure FDA0002345199910000054
represents the turning radius of the left rear vertex of the rectangle where the vehicle is located at the moment k,
Figure FDA0002345199910000055
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure FDA0002345199910000056
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure FDA0002345199910000057
represents the turning radius of the left front vertex of the virtual protective frame at the time point k,
Figure FDA0002345199910000058
the turning radius at the time k of the left front vertex of the virtual fender frame is shown as follows
Figure FDA0002345199910000059
Figure FDA00023451999100000510
Figure FDA00023451999100000511
Figure FDA00023451999100000512
Figure FDA00023451999100000513
Figure FDA00023451999100000514
Figure FDA00023451999100000515
Figure FDA0002345199910000061
Wherein the content of the first and second substances,
Figure FDA0002345199910000062
is the vehicle turning curvature; w is expressed as vehicle width, lfIndicating the distance between the center of gravity of the vehicle and the head, lrExpressed as the distance between the center of gravity of the vehicle and the rear of the vehicle, α1|kThe virtual mask expansion coefficient represented as the vertex at the front left of the virtual mask at time k, α2|kThe virtual reticle expansion coefficient represented as the right front vertex of the virtual reticle at time k, α3|kThe virtual mask expansion coefficient represented as the right rear vertex of the virtual mask at time k, α4|kThe virtual protection frame expansion coefficient is expressed as the vertex at the left back of the virtual protection frame at the moment k;
(9-2) calculating the maximum error of the turning arc and the turning line segment:
Figure FDA0002345199910000063
Figure FDA0002345199910000064
Figure FDA0002345199910000065
Figure FDA0002345199910000066
wherein the content of the first and second substances,
Figure FDA0002345199910000067
represents the maximum error of the vertex at the front left of the virtual protective frame at the moment k,
Figure FDA0002345199910000068
represents the maximum error of the right front vertex of the virtual protective frame at the moment k,
Figure FDA0002345199910000069
expressed as the maximum error of the rear right vertex of the virtual protection frame at the time k,
Figure FDA00023451999100000610
the maximum error of the left rear vertex of the virtual protection frame at the moment k is represented;
(9-3) defining a distance function
Figure FDA00023451999100000611
Calculating the optimal virtual protection frame expansion coefficient
Figure FDA00023451999100000612
Wherein the content of the first and second substances,
Figure FDA00023451999100000613
representing distance function distancei|kMax represents the maximum value of the set;
if α*- α absolute value less than 0.001, stop iteration, final autonomous parking trajectory, otherwise order α0＝α*And (5) repeating the steps (4) to (8).
CN201911391911.0A 2019-12-30 2019-12-30 Autonomous parking trajectory planning method suitable for multiple scenes Active CN111089594B (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201911391911.0A CN111089594B (en) 2019-12-30 2019-12-30 Autonomous parking trajectory planning method suitable for multiple scenes
PCT/CN2020/125930 WO2021135617A1 (en) 2019-12-30 2020-11-02 Autonomous parking trajectory planning method suitable for multiple scenes

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911391911.0A CN111089594B (en) 2019-12-30 2019-12-30 Autonomous parking trajectory planning method suitable for multiple scenes

Publications (2)

Publication Number Publication Date
CN111089594A true CN111089594A (en) 2020-05-01
CN111089594B CN111089594B (en) 2021-09-07

Family

ID=70397573

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911391911.0A Active CN111089594B (en) 2019-12-30 2019-12-30 Autonomous parking trajectory planning method suitable for multiple scenes

Country Status (2)

Country Link
CN (1) CN111089594B (en)
WO (1) WO2021135617A1 (en)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111783595A (en) * 2020-06-24 2020-10-16 中国第一汽车股份有限公司 Vehicle parking method and device, vehicle-mounted equipment and storage medium
CN112799393A (en) * 2020-12-04 2021-05-14 湖南大学 Parking scene oriented map simplifying system
CN112937555A (en) * 2021-03-08 2021-06-11 华南理工大学 Parallel parking trajectory planning method based on vehicle kinematics model
WO2021135617A1 (en) * 2019-12-30 2021-07-08 浙江大学 Autonomous parking trajectory planning method suitable for multiple scenes
CN113096209A (en) * 2021-03-29 2021-07-09 惠州华阳通用电子有限公司 Display method of vehicle-mounted image trajectory line
WO2021227304A1 (en) * 2020-05-09 2021-11-18 惠州市德赛西威汽车电子股份有限公司 Obstacle avoidance method in automatic parking path planning and parking path planning system
CN114347982A (en) * 2021-12-24 2022-04-15 北京经纬恒润科技股份有限公司 Path planning method and system for automatic parking
CN114435345A (en) * 2022-01-25 2022-05-06 岚图汽车科技有限公司 Automatic parking square column anti-collision method and system
CN114475780A (en) * 2021-12-21 2022-05-13 大连中科创达软件有限公司 Automatic parking method, device, equipment and storage medium
WO2023123704A1 (en) * 2021-12-28 2023-07-06 魔门塔(苏州)科技有限公司 Automatic parking path planning method and apparatus, medium, and device

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100076640A1 (en) * 2008-09-22 2010-03-25 Komatsu Ltd. Travel route generating method for unmanned vehicle
CN102963355A (en) * 2012-11-01 2013-03-13 同济大学 Intelligent auxiliary parking method and implementation system thereof
CN105335797A (en) * 2015-11-03 2016-02-17 浙江大学 Automatic parking locus optimization method based on full-simultaneous dynamic optimization framework
CN105857306A (en) * 2016-04-14 2016-08-17 中国科学院合肥物质科学研究院 Vehicle autonomous parking path programming method used for multiple parking scenes
CN108121205A (en) * 2017-12-13 2018-06-05 深圳市航盛电子股份有限公司 A kind of paths planning method, system and medium for a variety of scenes of parking
CN109910878A (en) * 2019-03-21 2019-06-21 山东交通学院 Automatic driving vehicle avoidance obstacle method and system based on trajectory planning
CN110352391A (en) * 2017-01-20 2019-10-18 株式会社久保田 Operation Van

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB201100840D0 (en) * 2011-01-18 2011-08-17 Bae Systems Plc Trajectory planning
KR101525897B1 (en) * 2014-02-04 2015-06-04 계명대학교 산학협력단 auto-docking control method of bus, and thereof control system
KR102463453B1 (en) * 2016-12-14 2022-11-04 현대자동차주식회사 Method of Auto Parking Control and Shift By Wire System thereof
CN107672585B (en) * 2017-08-29 2019-09-17 广州小鹏汽车科技有限公司 A kind of automatic parking paths planning method and system
CN111089594B (en) * 2019-12-30 2021-09-07 浙江大学 Autonomous parking trajectory planning method suitable for multiple scenes

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100076640A1 (en) * 2008-09-22 2010-03-25 Komatsu Ltd. Travel route generating method for unmanned vehicle
CN102963355A (en) * 2012-11-01 2013-03-13 同济大学 Intelligent auxiliary parking method and implementation system thereof
CN105335797A (en) * 2015-11-03 2016-02-17 浙江大学 Automatic parking locus optimization method based on full-simultaneous dynamic optimization framework
CN105857306A (en) * 2016-04-14 2016-08-17 中国科学院合肥物质科学研究院 Vehicle autonomous parking path programming method used for multiple parking scenes
CN110352391A (en) * 2017-01-20 2019-10-18 株式会社久保田 Operation Van
CN108121205A (en) * 2017-12-13 2018-06-05 深圳市航盛电子股份有限公司 A kind of paths planning method, system and medium for a variety of scenes of parking
CN109910878A (en) * 2019-03-21 2019-06-21 山东交通学院 Automatic driving vehicle avoidance obstacle method and system based on trajectory planning

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
XIAOJING ZHANG 等: "Autonomous Parking using Optimization-Based Collision Avoidance", 《2018 IEEE CONFERENCE ON DECISION AND CONTROL (CDC)》 *
张仁永: "平行泊车轨迹规划及控制算法研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021135617A1 (en) * 2019-12-30 2021-07-08 浙江大学 Autonomous parking trajectory planning method suitable for multiple scenes
WO2021227304A1 (en) * 2020-05-09 2021-11-18 惠州市德赛西威汽车电子股份有限公司 Obstacle avoidance method in automatic parking path planning and parking path planning system
CN111783595A (en) * 2020-06-24 2020-10-16 中国第一汽车股份有限公司 Vehicle parking method and device, vehicle-mounted equipment and storage medium
CN112799393B (en) * 2020-12-04 2022-04-19 湖南大学 Parking scene oriented map simplifying system
CN112799393A (en) * 2020-12-04 2021-05-14 湖南大学 Parking scene oriented map simplifying system
CN112937555A (en) * 2021-03-08 2021-06-11 华南理工大学 Parallel parking trajectory planning method based on vehicle kinematics model
CN113096209A (en) * 2021-03-29 2021-07-09 惠州华阳通用电子有限公司 Display method of vehicle-mounted image trajectory line
CN114475780A (en) * 2021-12-21 2022-05-13 大连中科创达软件有限公司 Automatic parking method, device, equipment and storage medium
CN114475780B (en) * 2021-12-21 2023-09-15 大连中科创达软件有限公司 Automatic parking method, device, equipment and storage medium
CN114347982A (en) * 2021-12-24 2022-04-15 北京经纬恒润科技股份有限公司 Path planning method and system for automatic parking
WO2023123704A1 (en) * 2021-12-28 2023-07-06 魔门塔(苏州)科技有限公司 Automatic parking path planning method and apparatus, medium, and device
CN114435345A (en) * 2022-01-25 2022-05-06 岚图汽车科技有限公司 Automatic parking square column anti-collision method and system
CN114435345B (en) * 2022-01-25 2023-10-24 岚图汽车科技有限公司 Automatic parking square column anti-collision method and system

Also Published As

Publication number Publication date
WO2021135617A1 (en) 2021-07-08
CN111089594B (en) 2021-09-07

Similar Documents

Publication Publication Date Title
CN111089594B (en) Autonomous parking trajectory planning method suitable for multiple scenes
CN110298122B (en) Unmanned vehicle urban intersection left-turn decision-making method based on conflict resolution
CN107264531B (en) The autonomous lane-change of intelligent vehicle is overtaken other vehicles motion planning method in a kind of semi-structure environment
CN110018689B (en) Dynamic window-based multi-virtual target point global dynamic path planning algorithm
US20200406969A1 (en) Lane Change Maneuvers With Minimized Tire Slip
CN110361013B (en) Path planning system and method for vehicle model
CN105857306A (en) Vehicle autonomous parking path programming method used for multiple parking scenes
CN111806467A (en) Variable speed dynamic track changing planning method based on vehicle driving rule
CN109941275A (en) Lane changing method, apparatus, electronic equipment and storage medium
CN112810630A (en) Method and system for planning track of automatic driving vehicle
JP7315039B2 (en) How to find an avoidance route for a car
CN114610016B (en) Intelligent vehicle collision avoidance path planning method based on obstacle dynamic virtual expansion
CN111896004A (en) Narrow passage vehicle track planning method and system
CN113895463B (en) Path planning method suitable for turning around of automatic driving vehicle
CN116499486B (en) Complex off-road environment path planning method and system and electronic equipment
CN114637292A (en) Vehicle track tracking robust control method and system considering obstacle avoidance
CN113635898A (en) Method, apparatus, storage medium, and program product for vehicle speed limit control
Li et al. Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving
Osman et al. Vision based lane reference detection and tracking control of an automated guided vehicle
CN112238856B (en) Intelligent vehicle overtaking track optimization method based on hybrid particle swarm optimization
Wurts et al. Minimum slip collision imminent steering in curved roads using nonlinear model predictive control
Hu et al. Decision-making System based on Finite State Machine for Low-speed Autonomous Vehicles in the Park
CN114537435A (en) Real-time whole vehicle track planning method in automatic driving
CN114889589A (en) Intelligent automobile steering and braking cooperative collision avoidance control system and method
CN111857112B (en) Automobile local path planning method and electronic equipment

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