CN111089594A - 一种适用于多场景的自主泊车轨迹规划方法 - Google Patents

一种适用于多场景的自主泊车轨迹规划方法 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
English (en)
Other versions
CN111089594B (zh
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/zh
Publication of CN111089594A publication Critical patent/CN111089594A/zh
Priority to PCT/CN2020/125930 priority patent/WO2021135617A1/zh
Application granted granted Critical
Publication of CN111089594B publication Critical patent/CN111089594B/zh
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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种适用于多场景的自主泊车轨迹规划方法，包括：(1)构建泊车地图；(2)确定车辆当前位置和目标车位位置；(3)确定车辆物理参数，初始化车辆的虚拟保护框膨胀系数；(4)将车辆运动学微分方程表示为车辆运动状态约束；(5)使用虚拟保护框和虚拟保护框膨胀系数建立车辆与障碍物避免碰撞约束；(6)建立状态变量和输入变量约束；(7)确定优化目标，建立优化问题；(8)求解器求解优化问题，获得备选的自主泊车轨迹；(9)重新计算虚拟保护框膨胀系数，若满足停止条件，获得最终自主泊车轨迹，否则更新虚拟保护框膨胀系数，重复步骤(4)到步骤(8)。利用本发明，可以在多种场景下规划出无碰撞的泊车轨迹。

Description

一种适用于多场景的自主泊车轨迹规划方法
技术领域
本发明属于自动驾驶轨迹规划领域，尤其是涉及一种适用于多场景的自主泊车轨迹规划方法。
背景技术
自主泊车是自动驾驶中的一项重要功能，目前市面上已经出现了部分具备自主泊车的商用车，传统的自主泊车轨迹规划方法主要包括两个环节，首先将泊车地图划分为网格，在网格中使用诸如A*，状态晶格等搜索算法计算泊车路径，再根据泊车路径使用优化的方法计算路径速度，最终形成泊车轨迹，这种解决方案整体较复杂且不能保证最优，在控制器实施阶段容易发生偏差。
另一种计算泊车轨迹的方法是基于车辆运动学模型的最优化方法，这种方法使用最优控制将路径规划和配速两个步骤结合起来，直接计算泊车轨迹，但该方法一般需要人工选择优化点数，并且在将连续的优化问题离散后，往往存在约束违反问题，最终导致路径与障碍物存在碰撞，造成泊车安全隐患。
目前，存在一些使用对偶方法计算障碍物和车辆之间最短距离的方法，还有一些轨迹规划方法将车辆和障碍物看作多边形，使用面积法表示车辆和障碍物避免碰撞约束，这些方法在实施过程中仍然存在优化问题中约束违反现象，虽然有部分研究提出使用优化点重新提炼的方法，在发生约束违反的区域中增加优化点个数，然而这种方法往往导致问题变得更加复杂，求解时间变长，因此需要从原理上改进对于避碰约束和优化问题的表述。
针对上述自主泊车轨迹规划技术存在的问题和不足，找到一种能够保证与障碍物不碰撞，且生成的轨迹安全、可行，易于下层控制器实施的轨迹规划方法，将具有非常高的实用价值。
发明内容
本发明提供了一种适用于多场景的自主泊车轨迹规划方法，具有安全、可行、易于实施等特点，具有很高的实际应用价值。
一种适用于多场景的自主泊车轨迹规划方法，其特征在于，包括以下步骤：
(1)检测泊车环境，构建泊车地图；
(2)确定车辆当前位置和目标车位位置；
(3)确定车辆物理参数，初始化车辆的虚拟保护框膨胀系数；
(4)根据车辆运动学微分方程，建立车辆运动状态约束；
(5)使用虚拟保护框和虚拟保护框膨胀系数建立车辆与障碍物避免碰撞约束；
(6)根据车辆机械原理限制和舒适度要求，建立状态变量和输入变量约束；
(7)确定优化目标，建立优化问题；
(8)使用非线性规划求解器求解优化问题，获得备选的自主泊车轨迹；
(9)重新计算虚拟保护框膨胀系数，若虚拟保护框膨胀系数满足停止条件，获得最终自主泊车轨迹，否则更新虚拟保护框膨胀系数，重复步骤(4)到步骤(8)。
本发明可以在多场景下规划出一条安全可行的泊车轨迹，为自动驾驶和自主泊车提供轨迹规划解决方案。
步骤(1)中，构建泊车地图时，使用封闭或者半封闭的多边形表示障碍物。
步骤(2)中，所述车辆当前位置和目标车位位置分别表示为
Figure BDA0002345199920000021
和
Figure BDA0002345199920000022
具体包括车辆重心在全局坐标系下的x轴坐标和y轴坐标，以及车辆前进方向与全局坐标系x轴正方向的夹角。
步骤(3)中，需确定的车辆物理参数包括车辆宽度w和轴距L，虚拟保护框膨胀系数初始化取值为α0＝1。
步骤(4)的具体过程为：
(4-1)使用如下微分方程描述车辆的运动状态：
Figure BDA0002345199920000031
其中，X表示车辆重心在全局坐标系中的x轴坐标，Y表示车辆重心在全局坐标系中的y轴坐标，
Figure BDA0002345199920000032
表示车辆正前方向与x轴的夹角；
Figure BDA0002345199920000033
和
Figure BDA0002345199920000034
分别表示X、Y和
Figure BDA0002345199920000035
的微分；v表示车辆前进速度，δf表示前轮转向角度；将上述微分方程作为车辆的运动学模型，记为：
Figure BDA0002345199920000036
其中，
Figure BDA0002345199920000037
表示状态变量，u＝[v δf]T表示输入变量；
(4-2)假设ξ0,ξ1,...,ξN是N+1个需要规划的状态变量，根据车辆的运动学模型，在已知当前时刻k的状态变量ξk时，下一时刻的状态变量
Figure BDA0002345199920000038
表示为：
r1＝f(ξk,uk)
Figure BDA0002345199920000039
Figure BDA00023451999200000310
Figure BDA00023451999200000311
Figure BDA00023451999200000312
其中，r1,r2,r3,r4是中间状态值，uk表示k时刻的输入变量，T表示采样周期；
(4-3)建立车辆运动状态约束：
Figure BDA00023451999200000313
步骤(5)的具体过程如下：
(5-1)障碍物由多边形Oj表示，定义为
Figure BDA0002345199920000041
其中，下标j表示第j个障碍物，共有J个障碍物，
Figure BDA0002345199920000042
表示多边形Oj沿顺时针方向的第i个顶点，
Figure BDA0002345199920000043
表示第j个障碍物的Vj1,Vj2顶点的连线线段，以此类推；
(5-2)车辆由矩形E表示，定义为
Figure BDA0002345199920000044
其中，E1表示车辆所在矩形左前方顶点，E2表示车辆所在矩形右前方顶点，E3表示车辆所在矩形右后方顶点，E4表示车辆所在矩形左后方顶点，
Figure BDA0002345199920000045
表示车辆的E1,E2顶点的连线线段，以此类推；
(5-3)在车辆周围定义虚拟保护框Ef
Figure BDA0002345199920000046
E和Ef的关系为Ef沿平行于车辆方向的长度和E的对应长度相等，Ef沿垂直于车辆方向的长度等于E的对应长度乘以虚拟保护框膨胀系数α；
(5-4)使用线段集合表示相邻时刻间的车辆轨迹
Figure BDA0002345199920000047
其中，
Figure BDA0002345199920000048
表示虚拟保护框上
Figure BDA0002345199920000049
点在k时刻的位置，以此类推；
(5-5)建立车辆与障碍物避免碰撞约束：
Figure BDA00023451999200000410
其中，
Figure BDA00023451999200000411
Figure BDA00023451999200000412
表示相交，
Figure BDA00023451999200000413
表示车辆轨迹
Figure BDA00023451999200000414
和障碍物
Figure BDA00023451999200000415
非相交。
步骤(6)中，建立的状态变量和输入变量约束具体公式为：
Figure BDA00023451999200000416
其中，vmin表示车辆前进的最小速度，vmax表示车辆前进的最大速度，δfmin表示前轮转向的最小角度，δfmax表示前轮转向的最大角度。
步骤(7)中，优化目标及优化问题具体如下：
minT·N
s.t.
Figure BDA0002345199920000051
Figure BDA0002345199920000052
Figure BDA0002345199920000053
Figure BDA0002345199920000054
其中，minT·N为优化目标，表示总时间最小，T为采样周期，N为采样个数；s.t.表示服从下列约束；ξ0表示0时刻的状态变量，ξN表示N时刻的状态变量。
步骤(8)中，所述的自主泊车轨迹包含的信息有轨迹
Figure BDA0002345199920000055
和对应操作
Figure BDA0002345199920000056
其中，Traj(k)表示k时刻车辆在全局坐标系中的x轴坐标、y轴坐标、车辆前进方向与x轴的夹角，u(k)表示k时刻的车辆前进速度以及前轮转向角度。
步骤(9)中，重新计算虚拟保护框膨胀系数的步骤如下：
(9-1)计算每个时刻的虚拟保护框和车辆轮廓的曲率半径之差：
Figure BDA0002345199920000057
Figure BDA0002345199920000058
Figure BDA0002345199920000059
Figure BDA00023451999200000510
其中，δR1|k表示车辆左前方曲率半径之差，δR2|k表示车辆右前方曲率半径之差，δR3|k表示车辆右后方曲率半径之差，δR4|k表示车辆左后方曲率半径之差，
Figure BDA00023451999200000511
表示车辆所在矩形左前方顶点在k时刻的转弯半径，
Figure BDA00023451999200000512
表示车辆所在矩形右前方顶点在k时刻的转弯半径，
Figure BDA00023451999200000513
表示车辆所在矩形右后方顶点在k时刻的转弯半径，
Figure BDA00023451999200000514
表示车辆所在矩形左后方顶点在k时刻的转弯半径，
Figure BDA0002345199920000061
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure BDA0002345199920000062
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure BDA0002345199920000063
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure BDA0002345199920000064
表示虚拟保护框左前方顶点在k时刻的转弯半径，具体的如下
Figure BDA0002345199920000065
Figure BDA0002345199920000066
Figure BDA0002345199920000067
Figure BDA0002345199920000068
Figure BDA0002345199920000069
Figure BDA00023451999200000610
Figure BDA00023451999200000611
Figure BDA00023451999200000612
其中，
Figure BDA00023451999200000613
为车辆转弯曲率；w表示为车辆宽度，lf表示位车辆重心与车头之间的距离，lr表示为车辆重心与车尾之间的距离，α1|k表示为k时刻虚拟保护框左前方顶点的虚拟保护框膨胀系数，α2|k表示为k时刻虚拟保护框右前方顶点的虚拟保护框膨胀系数，α3|k表示为k时刻虚拟保护框右后方顶点的虚拟保护框膨胀系数，α4|k表示为k时刻虚拟保护框左后方顶点的虚拟保护框膨胀系数；
(9-2)计算转弯圆弧和转弯线段的最大误差：
Figure BDA0002345199920000071
Figure BDA0002345199920000072
Figure BDA0002345199920000073
Figure BDA0002345199920000074
其中，
Figure BDA0002345199920000075
表示k时刻虚拟保护框左前方顶点最大误差，
Figure BDA0002345199920000076
表示k时刻虚拟保护框右前方顶点最大误差，
Figure BDA0002345199920000077
表示为k时刻虚拟保护框右后方顶点最大误差，
Figure BDA0002345199920000078
表示为k时刻虚拟保护框左后方顶点最大误差；
(9-3)定义距离函数
Figure BDA0002345199920000079
计算最优虚拟保护框膨胀系数
Figure BDA00023451999200000710
其中，
Figure BDA00023451999200000711
表示距离函数distancei|k的下限，max表示取集合的最大值；
若α*-α的绝对值小于0.001，停止迭代，最终自主泊车轨迹，否则令α0＝α*，重复步骤(4)到步骤(8)。
与现有技术相比，本发明具有以下有益效果：
1、本发明生成具有时间信息的泊车轨迹，方便后续控制器设计。
2、本发明使用了虚拟保护框概念和虚拟保护框膨胀系数概念，保证生成的轨迹与障碍物不发生碰撞。
3、本发明的方法对于泊车场景没有具体要求，可广泛适用于倒车入库，侧方位停车，斜方停车等泊车场景。
附图说明
图1为本发明实施例中的泊车场景；
图2为本发明一种适用于多场景的自主泊车轨迹规划方法的流程示意图；
图3为本发明实施例中最终规划出的泊车轨迹。
具体实施方式
下面结合附图和实施例对本发明做进一步详细描述，需要指出的是，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。
本实施例以车辆倒车入库为例，对适用于多场景的自主泊车轨迹规划方法做详细描述。
车辆需要在图1所示的泊车场景中完成倒车入库，其中泊车位长5米，宽2.5米，操作路面宽度为6m。
如图2所示，一种适用于多场景的自主泊车轨迹规划方法，包括：
步骤1，检测泊车环境，构建泊车地图，其中障碍物使用封闭或者半封闭的多边形表示；
步骤2，确定车辆当前位置和目标车位位置，其中车辆当前位置在全局坐标系下的位置信息为
Figure BDA0002345199920000081
目标车位位置在全局坐标系下的位置信息为
Figure BDA0002345199920000082
步骤3，确定车辆物理参数，初始化虚拟保护框膨胀系数，所需确定的车辆物理参数为车辆宽度w＝1.9m，轴距L＝2.7m，虚拟保护框膨胀系数初始化取值为α0＝1。
步骤4，将车辆运动学微分方程表示为车辆运动状态约束，具体实施方式为：
步骤4-1，使用如下微分方程描述车辆的运动状态：
Figure BDA0002345199920000091
其中，X表示车辆重心在全局坐标系中的x轴坐标，Y表示车辆重心在全局坐标系中的y轴坐标，
Figure BDA0002345199920000092
表示车辆正前方向与x轴的夹角；v表示车辆前进速度，δf表示前轮转向角度。将上述微分方程记为：
Figure BDA0002345199920000093
其中，
Figure BDA0002345199920000094
表示状态变量，u＝[v δf]T表示输入变量。
步骤4-2，假设ξ0,ξ1,...,ξN是N+1个需要规划的状态变量，根据车辆的运动学模型，在已知当前状态ξk时，下一时刻的状态
Figure BDA0002345199920000095
可表示为：
r1＝f(ξk,uk)
Figure BDA0002345199920000096
Figure BDA0002345199920000097
Figure BDA0002345199920000098
Figure BDA0002345199920000099
其中，r1,r2,r3,r4是中间状态值，T表示采样周期，在本实施例中，N取50。
步骤4-3，建立车辆运动状态约束：
Figure BDA00023451999200000910
步骤5，使用虚拟保护框和虚拟保护框膨胀系数建立车辆与障碍物避免碰撞约束，具体实施方法为：
步骤5-1，障碍物由多边形Oj表示，定义为
Figure BDA00023451999200000911
其中，下标j表示第j个障碍物，共有J个障碍物，
Figure BDA00023451999200000912
表示多边形Oj沿顺时针方向的第i个顶点，
Figure BDA0002345199920000101
表示第j个障碍物的
Figure BDA0002345199920000102
顶点的连线线段，以此类推。
步骤5-2，车辆由矩形E表示，定义为
Figure BDA0002345199920000103
其中，E1表示车辆所在矩形左前方顶点，E2表示车辆所在矩形右前方顶点，E3表示车辆所在矩形右后方顶点，E4表示车辆所在矩形左后方顶点，
Figure BDA0002345199920000104
表示车辆的E1,E2顶点的连线线段，以此类推。
步骤5-3，在车辆周围定义虚拟保护框Ef
Figure BDA0002345199920000105
E和Ef的关系为Ef沿平行于车辆方向的长度和E的对应长度相等，Ef沿垂直于车辆方向的长度等于E的对应长度乘以虚拟保护框膨胀系数α。
步骤5-4，使用线段集合表示相邻时刻间的车辆轨迹
Figure BDA0002345199920000106
其中
Figure BDA0002345199920000107
表示虚拟保护框上
Figure BDA0002345199920000108
点在k时刻的位置，以此类推。
步骤5-5，建立车辆与障碍物避免碰撞约束：
Figure BDA0002345199920000109
其中
Figure BDA00023451999200001010
步骤6，根据车辆机械原理限制和舒适度要求，建立状态变量和输入变量约束，具体实施方法为：
Figure BDA00023451999200001011
步骤7，确定优化目标，建立优化问题，优化问题的具体形式为：
minT·N
s.t.
Figure BDA0002345199920000111
Figure BDA0002345199920000112
Figure BDA0002345199920000113
Figure BDA0002345199920000114
步骤8，使用非线性规划求解器求解优化问题，获得备选自主泊车轨迹，包含的信息有轨迹
Figure BDA0002345199920000115
和对应操作
Figure BDA0002345199920000116
步骤9，重新计算虚拟保护框膨胀系数，具体实施方法如下：
步骤9-1，计算每个时刻的虚拟保护框和车辆轮廓的曲率半径之差：
Figure BDA0002345199920000117
Figure BDA0002345199920000118
Figure BDA0002345199920000119
Figure BDA00023451999200001110
其中，
Figure BDA00023451999200001111
Figure BDA00023451999200001112
Figure BDA00023451999200001113
Figure BDA00023451999200001114
Figure BDA00023451999200001115
Figure BDA0002345199920000121
Figure BDA0002345199920000122
Figure BDA0002345199920000123
Figure BDA0002345199920000124
为车辆转弯曲率。
步骤9-2，计算转弯圆弧和转弯线段的最大误差：
Figure BDA0002345199920000125
Figure BDA0002345199920000126
Figure BDA0002345199920000127
Figure BDA0002345199920000128
步骤9-3，定义距离函数
distancei|k＝ErrorEi|k-δRi|k
计算最优虚拟保护框膨胀系数，
Figure BDA0002345199920000129
其中，
Figure BDA00023451999200001210
表示距离函数distancei|k的下限，max表示取集合的最大值。
若α*-α的绝对值小于0.001，停止迭代，最终自主泊车轨迹，否则令α0＝α*，重复步骤4到步骤8。
最终规划出的泊车轨迹如图3所示，车辆从全局坐标
Figure BDA0002345199920000131
出发，目标泊位为
Figure BDA0002345199920000132
使用本方法，规划出的轨迹能够保证和障碍物不发生碰撞，具有安全可行的优点。
以上所述的实施例对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的具体实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换，均应包含在本发明的保护范围之内。

Claims (10)

1.一种适用于多场景的自主泊车轨迹规划方法，其特征在于，包括以下步骤：
(1)检测泊车环境，构建泊车地图；
(2)确定车辆当前位置和目标车位位置；
(3)确定车辆物理参数，初始化车辆的虚拟保护框膨胀系数；
(4)根据车辆运动学微分方程，建立车辆运动状态约束；
(5)使用虚拟保护框和虚拟保护框膨胀系数建立车辆与障碍物避免碰撞约束；
(6)根据车辆机械原理限制和舒适度要求，建立状态变量和输入变量约束；
(7)确定优化目标，建立优化问题；
(8)使用非线性规划求解器求解优化问题，获得备选的自主泊车轨迹；
(9)重新计算虚拟保护框膨胀系数，若虚拟保护框膨胀系数满足停止条件，获得最终自主泊车轨迹，否则更新虚拟保护框膨胀系数，重复步骤(4)到步骤(8)。
2.根据权利要求1所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(1)中，构建泊车地图时，使用封闭或者半封闭的多边形表示障碍物。
3.根据权利要求1所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(2)中，所述车辆当前位置和目标车位位置分别表示为
Figure FDA0002345199910000011
和
Figure FDA0002345199910000012
具体包括车辆重心在全局坐标系下的x轴坐标和y轴坐标，以及车辆前进方向与全局坐标系x轴正方向的夹角。
4.根据权利要求3所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(3)中，需确定的车辆物理参数包括车辆宽度w和轴距L，虚拟保护框膨胀系数初始化取值为α0＝1。
5.根据权利要求4所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(4)的具体过程为：
(4-1)使用如下微分方程描述车辆的运动状态：
Figure FDA0002345199910000021
其中，X表示车辆重心在全局坐标系中的x轴坐标，Y表示车辆重心在全局坐标系中的y轴坐标，
Figure FDA0002345199910000022
表示车辆正前方向与x轴的夹角；
Figure FDA0002345199910000023
和
Figure FDA0002345199910000024
分别表示X、Y和
Figure FDA0002345199910000025
的微分；v表示车辆前进速度，δf表示前轮转向角度；将上述微分方程作为车辆的运动学模型，记为：
Figure FDA0002345199910000026
其中，
Figure FDA0002345199910000027
表示状态变量，u＝[v δf]T表示输入变量；
(4-2)假设ξ0,ξ1,...,ξN是N+1个需要规划的状态变量，根据车辆的运动学模型，在已知当前时刻k的状态变量ξk时，下一时刻的状态变量
Figure FDA0002345199910000028
表示为：
r1＝f(ξk,uk)
Figure FDA0002345199910000029
Figure FDA00023451999100000210
Figure FDA00023451999100000211
Figure FDA00023451999100000212
其中，r1,r2,r3,r4是中间状态值，uk表示k时刻的输入变量，T表示采样周期；
(4-3)建立车辆运动状态约束：
Figure FDA00023451999100000213
6.根据权利要求5所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(5)的具体过程如下：
(5-1)障碍物由多边形Oj表示，定义为
Figure FDA0002345199910000031
其中，下标j表示第j个障碍物，共有J个障碍物，
Figure FDA0002345199910000032
表示多边形Oj沿顺时针方向的第i个顶点，
Figure FDA0002345199910000033
表示第j个障碍物的
Figure FDA0002345199910000034
顶点的连线线段，以此类推；
(5-2)车辆由矩形E表示，定义为
Figure FDA0002345199910000035
其中，E1表示车辆所在矩形左前方顶点，E2表示车辆所在矩形右前方顶点，E3表示车辆所在矩形右后方顶点，E4表示车辆所在矩形左后方顶点，
Figure FDA0002345199910000036
表示车辆的E1,E2顶点的连线线段，以此类推；
(5-3)在车辆周围定义虚拟保护框Ef
Figure FDA0002345199910000037
E和Ef的关系为Ef沿平行于车辆方向的长度和E的对应长度相等，Ef沿垂直于车辆方向的长度等于E的对应长度乘以虚拟保护框膨胀系数α；
(5-4)使用线段集合表示相邻时刻间的车辆轨迹
Figure FDA0002345199910000038
其中，
Figure FDA0002345199910000039
表示虚拟保护框上
Figure FDA00023451999100000310
点在k时刻的位置，以此类推；
(5-5)建立车辆与障碍物避免碰撞约束：
Figure FDA00023451999100000311
其中，
Figure FDA00023451999100000312
表示相交，
Figure FDA00023451999100000313
表示车辆轨迹
Figure FDA00023451999100000314
和障碍物
Figure FDA00023451999100000315
非相交。
7.根据权利要求6所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(6)中，建立的状态变量和输入变量约束具体公式为：
Figure FDA0002345199910000041
其中，vmin表示车辆前进的最小速度，vmax表示车辆前进的最大速度，δfmin表示前轮转向的最小角度，δfmax表示前轮转向的最大角度。
8.根据权利要求6所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(7)中，优化目标及优化问题具体如下：
minT·N
s.t.
Figure FDA0002345199910000042
Figure FDA0002345199910000043
Figure FDA0002345199910000044
Figure FDA0002345199910000045
其中，minT·N为优化目标，表示总时间最小，T为采样周期，N为采样个数；s.t.表示服从下列约束；ξ0表示0时刻的状态变量，ξN表示N时刻的状态变量。
9.根据权利要求8所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(8)中，所述的自主泊车轨迹包含的信息有轨迹
Figure FDA0002345199910000046
和对应操作
Figure FDA0002345199910000047
其中，Traj(k)表示k时刻车辆在全局坐标系中的x轴坐标、y轴坐标、车辆前进方向与x轴的夹角，u(k)表示k时刻的车辆前进速度以及前轮转向角度。
10.根据权利要求9所述的适用于多场景的自主泊车轨迹规划方法，其特征在于，步骤(9)中，重新计算虚拟保护框膨胀系数的步骤如下：
(9-1)计算每个时刻的虚拟保护框和车辆轮廓的曲率半径之差：
Figure FDA0002345199910000048
Figure FDA0002345199910000049
Figure FDA00023451999100000410
Figure FDA00023451999100000411
其中，δR1|k表示车辆左前方曲率半径之差，δR2|k表示车辆右前方曲率半径之差，δR3|k表示车辆右后方曲率半径之差，δR4|k表示车辆左后方曲率半径之差，
Figure FDA0002345199910000051
表示车辆所在矩形左前方顶点在k时刻的转弯半径，
Figure FDA0002345199910000052
表示车辆所在矩形右前方顶点在k时刻的转弯半径，
Figure FDA0002345199910000053
表示车辆所在矩形右后方顶点在k时刻的转弯半径，
Figure FDA0002345199910000054
表示车辆所在矩形左后方顶点在k时刻的转弯半径，
Figure FDA0002345199910000055
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure FDA0002345199910000056
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure FDA0002345199910000057
表示虚拟保护框左前方顶点在k时刻的转弯半径，
Figure FDA0002345199910000058
表示虚拟保护框左前方顶点在k时刻的转弯半径，具体的如下
Figure FDA0002345199910000059
Figure FDA00023451999100000510
Figure FDA00023451999100000511
Figure FDA00023451999100000512
Figure FDA00023451999100000513
Figure FDA00023451999100000514
Figure FDA00023451999100000515
Figure FDA0002345199910000061
其中，
Figure FDA0002345199910000062
为车辆转弯曲率；w表示为车辆宽度，lf表示位车辆重心与车头之间的距离，lr表示为车辆重心与车尾之间的距离，α1|k表示为k时刻虚拟保护框左前方顶点的虚拟保护框膨胀系数，α2|k表示为k时刻虚拟保护框右前方顶点的虚拟保护框膨胀系数，α3|k表示为k时刻虚拟保护框右后方顶点的虚拟保护框膨胀系数，α4|k表示为k时刻虚拟保护框左后方顶点的虚拟保护框膨胀系数；
(9-2)计算转弯圆弧和转弯线段的最大误差：
Figure FDA0002345199910000063
Figure FDA0002345199910000064
Figure FDA0002345199910000065
Figure FDA0002345199910000066
其中，
Figure FDA0002345199910000067
表示k时刻虚拟保护框左前方顶点最大误差，
Figure FDA0002345199910000068
表示k时刻虚拟保护框右前方顶点最大误差，
Figure FDA0002345199910000069
表示为k时刻虚拟保护框右后方顶点最大误差，
Figure FDA00023451999100000610
表示为k时刻虚拟保护框左后方顶点最大误差；
(9-3)定义距离函数
Figure FDA00023451999100000611
计算最优虚拟保护框膨胀系数
Figure FDA00023451999100000612
其中，
Figure FDA00023451999100000613
表示距离函数distancei|k的下限，max表示取集合的最大值；
若α*-α的绝对值小于0.001，停止迭代，最终自主泊车轨迹，否则令α0＝α*，重复步骤(4)到步骤(8)。
CN201911391911.0A 2019-12-30 2019-12-30 一种适用于多场景的自主泊车轨迹规划方法 Active CN111089594B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201911391911.0A CN111089594B (zh) 2019-12-30 2019-12-30 一种适用于多场景的自主泊车轨迹规划方法
PCT/CN2020/125930 WO2021135617A1 (zh) 2019-12-30 2020-11-02 一种适用于多场景的自主泊车轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911391911.0A CN111089594B (zh) 2019-12-30 2019-12-30 一种适用于多场景的自主泊车轨迹规划方法

Publications (2)

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

Family

ID=70397573

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911391911.0A Active CN111089594B (zh) 2019-12-30 2019-12-30 一种适用于多场景的自主泊车轨迹规划方法

Country Status (2)

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

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111783595A (zh) * 2020-06-24 2020-10-16 中国第一汽车股份有限公司 一种车辆泊车方法、装置、车载设备及存储介质
CN112799393A (zh) * 2020-12-04 2021-05-14 湖南大学 一种面向泊车场景的地图简化***
CN112937555A (zh) * 2021-03-08 2021-06-11 华南理工大学 一种基于车辆运动学模型的平行泊车轨迹规划方法
WO2021135617A1 (zh) * 2019-12-30 2021-07-08 浙江大学 一种适用于多场景的自主泊车轨迹规划方法
CN113096209A (zh) * 2021-03-29 2021-07-09 惠州华阳通用电子有限公司 一种车载影像轨迹线的显示方法
WO2021227304A1 (zh) * 2020-05-09 2021-11-18 惠州市德赛西威汽车电子股份有限公司 一种自动泊车路径规划的避障方法及泊车路径规划***
CN114347982A (zh) * 2021-12-24 2022-04-15 北京经纬恒润科技股份有限公司 用于自动泊车的路径规划方法以及***
CN114435345A (zh) * 2022-01-25 2022-05-06 岚图汽车科技有限公司 一种自动泊车方柱防碰撞方法及***
CN114475780A (zh) * 2021-12-21 2022-05-13 大连中科创达软件有限公司 一种自动泊车方法、装置、设备及存储介质
WO2023123704A1 (zh) * 2021-12-28 2023-07-06 魔门塔(苏州)科技有限公司 一种自动泊车路径规划方法、装置、介质及设备
CN117901850A (zh) * 2024-03-20 2024-04-19 北京理工大学 一种非结构化场景自主泊车轨迹规划方法及相关装置

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 (zh) * 2012-11-01 2013-03-13 同济大学 一种智能辅助泊车方法及其实现***
CN105335797A (zh) * 2015-11-03 2016-02-17 浙江大学 一种基于全联立动态优化框架的自主泊车轨迹优化方法
CN105857306A (zh) * 2016-04-14 2016-08-17 中国科学院合肥物质科学研究院 一种用于多种泊车场景的车辆自主泊车路径规划方法
CN108121205A (zh) * 2017-12-13 2018-06-05 深圳市航盛电子股份有限公司 一种用于多种泊车场景的路径规划方法、***及介质
CN109910878A (zh) * 2019-03-21 2019-06-21 山东交通学院 基于轨迹规划的自动驾驶车辆避障控制方法及***
CN110352391A (zh) * 2017-01-20 2019-10-18 株式会社久保田 作业车

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 (ko) * 2014-02-04 2015-06-04 계명대학교 산학협력단 버스의 오토도킹 제어방법 및 이 방법을 사용하는 제어 시스템
KR102463453B1 (ko) * 2016-12-14 2022-11-04 현대자동차주식회사 오토 파킹 제어방법 및 시프트 바이 와이어 시스템
CN107672585B (zh) * 2017-08-29 2019-09-17 广州小鹏汽车科技有限公司 一种自动泊车路径规划方法及***
CN111089594B (zh) * 2019-12-30 2021-09-07 浙江大学 一种适用于多场景的自主泊车轨迹规划方法

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 (zh) * 2012-11-01 2013-03-13 同济大学 一种智能辅助泊车方法及其实现***
CN105335797A (zh) * 2015-11-03 2016-02-17 浙江大学 一种基于全联立动态优化框架的自主泊车轨迹优化方法
CN105857306A (zh) * 2016-04-14 2016-08-17 中国科学院合肥物质科学研究院 一种用于多种泊车场景的车辆自主泊车路径规划方法
CN110352391A (zh) * 2017-01-20 2019-10-18 株式会社久保田 作业车
CN108121205A (zh) * 2017-12-13 2018-06-05 深圳市航盛电子股份有限公司 一种用于多种泊车场景的路径规划方法、***及介质
CN109910878A (zh) * 2019-03-21 2019-06-21 山东交通学院 基于轨迹规划的自动驾驶车辆避障控制方法及***

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 (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021135617A1 (zh) * 2019-12-30 2021-07-08 浙江大学 一种适用于多场景的自主泊车轨迹规划方法
WO2021227304A1 (zh) * 2020-05-09 2021-11-18 惠州市德赛西威汽车电子股份有限公司 一种自动泊车路径规划的避障方法及泊车路径规划***
CN111783595A (zh) * 2020-06-24 2020-10-16 中国第一汽车股份有限公司 一种车辆泊车方法、装置、车载设备及存储介质
CN112799393A (zh) * 2020-12-04 2021-05-14 湖南大学 一种面向泊车场景的地图简化***
CN112799393B (zh) * 2020-12-04 2022-04-19 湖南大学 一种面向泊车场景的地图简化***
CN112937555A (zh) * 2021-03-08 2021-06-11 华南理工大学 一种基于车辆运动学模型的平行泊车轨迹规划方法
CN113096209B (zh) * 2021-03-29 2024-06-04 惠州华阳通用电子有限公司 一种车载影像轨迹线的显示方法
CN113096209A (zh) * 2021-03-29 2021-07-09 惠州华阳通用电子有限公司 一种车载影像轨迹线的显示方法
CN114475780A (zh) * 2021-12-21 2022-05-13 大连中科创达软件有限公司 一种自动泊车方法、装置、设备及存储介质
CN114475780B (zh) * 2021-12-21 2023-09-15 大连中科创达软件有限公司 一种自动泊车方法、装置、设备及存储介质
CN114347982A (zh) * 2021-12-24 2022-04-15 北京经纬恒润科技股份有限公司 用于自动泊车的路径规划方法以及***
WO2023123704A1 (zh) * 2021-12-28 2023-07-06 魔门塔(苏州)科技有限公司 一种自动泊车路径规划方法、装置、介质及设备
CN114435345A (zh) * 2022-01-25 2022-05-06 岚图汽车科技有限公司 一种自动泊车方柱防碰撞方法及***
CN114435345B (zh) * 2022-01-25 2023-10-24 岚图汽车科技有限公司 一种自动泊车方柱防碰撞方法及***
CN117901850A (zh) * 2024-03-20 2024-04-19 北京理工大学 一种非结构化场景自主泊车轨迹规划方法及相关装置
CN117901850B (zh) * 2024-03-20 2024-05-14 北京理工大学 一种非结构化场景自主泊车轨迹规划方法及相关装置

Also Published As

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

Similar Documents

Publication Publication Date Title
CN111089594B (zh) 一种适用于多场景的自主泊车轨迹规划方法
CN107702716B (zh) 一种无人驾驶路径规划方法、***和装置
CN108519773B (zh) 一种结构化环境下无人驾驶车辆的路径规划方法
CN110298122B (zh) 基于冲突消解的无人驾驶车辆城市交叉口左转决策方法
CN107264531B (zh) 一种半结构化环境中智能车辆自主换道超车运动规划方法
US20200406969A1 (en) Lane Change Maneuvers With Minimized Tire Slip
CN105857306A (zh) 一种用于多种泊车场景的车辆自主泊车路径规划方法
CN114610016B (zh) 基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法
CN112810630A (zh) 一种自动驾驶车辆轨迹规划方法及***
CN109941275A (zh) 车道变换方法、装置、电子设备及存储介质
JP7315039B2 (ja) 自動車の回避経路を求める方法
CN113895463B (zh) 一种适用于自动驾驶车辆掉头的路径规划方法
CN111896004A (zh) 一种狭窄通道车辆轨迹规划方法及***
CN116499486B (zh) 一种复杂越野环境路径规划方法、***及电子设备
CN112238856B (zh) 一种基于混合粒子群算法的智能车超车轨迹优化方法
CN116118780A (zh) 一种车辆避障轨迹规划方法、***、车辆及存储介质
CN115285145A (zh) 一种无人驾驶弯道避撞轨迹规划与跟踪控制方法
CN114637292A (zh) 一种兼顾避障的车辆轨迹跟踪鲁棒控制方法和***
CN113635898A (zh) 车辆限速控制的方法、设备、存储介质及程序产品
Li et al. Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving
Wurts et al. Minimum slip collision imminent steering in curved roads using nonlinear model predictive control
CN116520855A (zh) 一种履带式移动工程机械及其移动控制方法、装置和介质
Hu et al. Decision-making system based on finite state machine for low-speed autonomous vehicles in the park
CN114537435B (zh) 一种自动驾驶中的实时整车轨迹规划方法
CN114889589A (zh) 一种智能汽车转向与制动协同避撞控制***及方法

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