CN108891412B - 一种平行泊车的路径规划方法 - Google Patents
一种平行泊车的路径规划方法 Download PDFInfo
- Publication number
- CN108891412B CN108891412B CN201810794684.5A CN201810794684A CN108891412B CN 108891412 B CN108891412 B CN 108891412B CN 201810794684 A CN201810794684 A CN 201810794684A CN 108891412 B CN108891412 B CN 108891412B
- Authority
- CN
- China
- Prior art keywords
- parking
- path
- starting point
- coordinate
- vehicle
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000000977 initiatory effect Effects 0.000 claims abstract description 34
- 238000001514 detection method Methods 0.000 claims description 18
- 230000004888 barrier function Effects 0.000 claims description 13
- 230000002068 genetic effect Effects 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000006870 function Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 5
- 238000003860 storage Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000002950 deficient Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000002028 premature Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
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/06—Automatic manoeuvring for parking
-
- 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
- B60W30/09—Taking automatic action to avoid collision, e.g. braking and steering
-
- 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
- B60W2520/00—Input parameters relating to overall vehicle dynamics
-
- 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
- B60W2540/00—Input parameters relating to occupants
- B60W2540/18—Steering angle
-
- 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
- B60W2554/00—Input parameters relating to objects
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明公开一种平行泊车的路径规划方法，包括步骤1：确定泊车起始区域，采用B样条曲线获取泊车路径，存储每条B样条曲线的泊车路径参数；步骤2：确定当前车辆泊车起始区域，获取距离当前泊车位置较近的起始点，并提取所述泊车起始点对应的且与当前车身倾斜角度接近的泊车路径参数，计算新的泊车路径的前三个控制点步骤3：将计算的三个控制点代替距离当前泊车位置较近的泊车起始点对应的泊车路径的前三个控制点，并与所述泊车路径之后的控制点一起生成以当前泊车位置为起始点的新的B样条曲线；步骤4：将获得的新的B样条曲线离散为路径点，验证每一个路径点是否满足约束条件，若满足约束，则输出路径，若不满足，则返回步骤2直至满足约束。
Description
技术领域
本发明涉及自动泊车技术领域，更具体的是，本发明涉及一种平行泊车的路径规划方法。
背景技术
随着车辆数量的增加使城市有限的空间变得逐渐拥挤，泊车空间的缩小增加了泊车困难，因此人们对辅助泊车系统的需求日益增加。
目前对自动泊车的研究很多，整体上说，这些研究基本上分为两个方向:一个方向是基于经验的算法，另一个方向是路径规划。其中基于经验的算法很难准确地获取经验知识以及如何将这些经验转化到控制器。而路径规划方法是根据车位的空间分布，建立起车辆的运动学模型，加上一些碰撞约束条件，规划出一条泊车的路径，然后设计路径跟踪控制器来控制车辆，使车辆沿着规划的路径倒入泊车位。
路径规划方法主要采用圆弧一直线的泊车路径，但由于曲线曲率不连续，车辆到达曲率间断点处时需停车转向，否则因方向盘转速和车速的影响，车辆将偏离目标路径。基于渐开线、三角函数曲线、多项式曲线、Ferguson曲线等曲线设计的行驶路径，因曲线变化欠灵活性限制了这些方法在狭小空间泊车路径规划的应用。
而B样条曲线变化灵活且易实现多阶可导等特点，路径规划可基于B样条理论，并结合泊车避障约束、车辆最小转弯半径约束、方向盘转向角速度约束等多个非线性约束设计泊车路径。
在中国发明专利CN105197010A公开的“辅助泊车系统以及辅助泊车控制方法”中，采用B样条理论设计出满足车辆结构参数约束和碰撞约束的泊车路径，但由B样条曲线设计泊车路径时泊车约束过多，当车辆在泊车起始点区域内任意位置时无法以前车辆位置为泊车路径起始点实时生成泊车路径，因此只能将设计的路径曲线以数据形式固化于泊车路径存储器。随后在泊车路径追踪时读取储存好的泊车路径。但在实际泊车过程中，很难将车停到规划好的泊车路径上，从而在进行路径追踪时增加了泊车控制难度以及泊车失败率。
在中国发明专利CN103373349A公开的“自动泊车辅助系统的障碍物避碰装置及方法”中，所发明的方法主要作用是检测泊车过程中的障碍物检测，即通过多个传感器，扩大了障碍物移动检测区域，而只对有亮度变化的图像检测障碍物区域和障碍物移动。
发明内容
本发明设计开发了一种平行泊车的路径规划方法，在当前车辆泊车起始点区域内任意位置均可实时生成满足车辆参数和碰撞约束的平行泊车的路径，泊车过程流畅。
本发明提供的技术方案为：
一种平行泊车的路径规划方法，包括如下步骤：
步骤1：确定泊车起始区域，在所述泊车起始区域内均匀生成多个泊车起始点分别作为泊车路径的起始点，并基于不同车身初始航向角度，采用B样条曲线获取所述泊车路径，存储每条B样条曲线的泊车路径参数；
步骤2：确定当前车辆泊车起始区域，并将车辆停至当前车辆泊车起始区域内任意位置，获取距离当前泊车位置较近的起始点，并提取所述泊车起始点对应的且与当前车身倾斜角度接近的泊车路径参数，计算新的泊车路径的前三个控制点：
式中，M1x为第一个控制点沿第一方向的坐标，M1y为第一个控制点沿第二方向的坐标，M2x为第二个控制点沿第一方向的坐标，M2y为第二个控制点沿第二方向的坐标，M3x为第三个控制点沿第一方向的坐标，M3y为第三个控制点沿第二方向的坐标，M4x为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第一方向的坐标，M4y为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第二方向的坐标，xqs为当前车辆起始点沿第一方向的坐标，yqs为当前车辆起始点沿第二方向的坐标，xqs’为当前车辆起始点沿第一方向的坐标的一阶导数值，yqs’为当前车辆起始点沿第二方向的坐标的一阶导数值，xqs”为当前车辆起始点沿第一方向的坐标的二阶导数值，yqs”为当前车辆起始点沿第二方向的坐标的二阶导数值，所述第一方向与第二方向垂直；
步骤3：将计算的三个控制点代替距离当前泊车位置较近的泊车起始点对应的泊车路径的前三个控制点，并与所述泊车路径之后的控制点一起生成以当前泊车位置为起始点的新的B样条曲线：
式中，x为新的B样条曲线沿第一方向的坐标，y为新的B样条曲线沿第二方向的坐标，t为新的B样条曲线的无量纲变量且t∈[0,1]，M为新的B样条曲线的控制点，P为调节矩阵；
步骤4：将获得的新的B样条曲线离散为路径点，验证每一个路径点是否满足约束条件；
若满足所述约束条件，则输出路径；
若不满足所述约束条件，则重新进行所述步骤2直至满足所述约束条件。
优选的是，在所述步骤2中，所述当前车辆泊车起始区域的确定包括：
获取当前车辆在所有泊车路径下的泊车面积；
对当前车辆待泊车位进行障碍物检测，包括：
当检测存在障碍物时，获取障碍物位置，并筛选出与所述障碍物无重叠的泊车面积及其对应的泊车路径起始点，以所述对应的泊车路径起始点为圆心做出圆形区域，其为当前车辆泊车起始区域，其中，所述圆形区域的半径为r，取值为0.15～0.2m；
当检测不存在障碍物时，以所述步骤1确定的泊车起始区域为当前车辆泊车起始区域。
优选的是，在所述步骤2中，距离当前泊车位置较近的泊车起始点的获取包括：
以当前车辆停至当前车辆泊车起始区域内的位置为圆心，以r为半径做圆，获取所有沿第一方向的坐标和沿第二方向的坐标均在圆内的泊车起始点。
优选的是，在所述步骤4中，所述约束条件为：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R；其中，R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
优选的是，在所述步骤1中，所述泊车路径的获取包括：
基于不同车身初始航向角度的泊车路径的起始点，基于B样条曲线随机生成多个控制点，以B样条曲线最后一点切线最小为目标，采用多种群遗传算法对所述B样条曲线的控制点进行优化，获得所述多个控制点的近似解；
通过非线性等式和不等式约束优化函数求解所述多个近似解的精确坐标，所述非线性等式和不等式约束满足：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R；其中，R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
优选的是，所述泊车路径包括9个控制点，每相邻5个控制点生成一段B样条曲线，所述9个控制点生成5段B样条曲线，所述泊车路径由所述5段B样条曲线平滑连接构成。
优选的是，在所述步骤3中，所述调节矩阵P为：
优选的是，所述泊车路径参数包括：泊车起始区域内每条B样条曲线的泊车路径起始点坐标，车身初始航向角度，泊车路径的控制点坐标，泊车路径起始点横坐标的一阶导数、二阶导数，以及泊车路径起始点纵坐标的一阶导数、二阶导数。
优选的是，所述泊车起始区域根据最小泊车位确定。
优选的是，所述车身初始航向角度包括0度，正负1度，正负2度，正负4度，正负6度，正负8度和正负10度。
本发明所述的有益效果：
本发明提供的平行泊车的路径规划方法，其采用基于B样条曲线的平行泊车路径规划算法来解决在泊车起始点区域内任意位置不能实时生成满足车辆参数和碰撞约束的B样条路径的问题，在泊车起始点区域内得到车辆的位置和车身倾斜角后，计算得到当前起始状态到车位的泊车路径。规划的路径光滑连续可导、转弯半径约束等，满足车辆自身结构参数约束和碰撞约束，该算法能快速实现机器人路径规划，方法简单、可靠，易于实现，计算量小，实时性较好，能较好的满足平行泊车路径规划要求。能够使得泊车轨迹生成简便、计算资源占用少且泊车过程流畅。
附图说明
图1为本发明所述的平行泊车路径规划方法流程图。
图2为本发明所述的泊车起始点区域内起始点分布图。
图3为本发明所述的单条路径的泊车面积示意图。
图4为本发明实施例获得的新的泊车路径图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明采用的装置由路径参数储存单元、控制点参数计算单元、路径生成单元、约束检测单元、障碍物检测单元、路径输出单元组成。路径参数储存单元储存泊车起始点区域内13*N条泊车路径参数，该参数包括泊车路径起始点坐标，泊车路径的控制点坐标、泊车路径起始点横坐标的一阶导数、二阶导数、以及泊车路径起始点纵坐标的一阶导数、二阶导数；控制点参数计算单元根据实际泊车初始位置计算出以当前位置为起始点B样条泊车路径的控制点；路径生成单元计算由控制点参数计算单元计算出的控制点，并生成泊车路径，并离散该泊车路径；约束检测单元将离散后的泊车路径点代入约束验证单元，验证路径上每一点是否满足约束。若满足约束则输出路径。若不满足约束，则重新计算控制点；障碍物检测单元在检测车位时同时检测车位内部障碍物，并根据障碍物坐标在泊车起始点区域选择适当泊车区间，然后生成物碰撞障碍物的泊车路径；路径输出单元输出离散的泊车路径，并将路径点的控制信息如车身倾斜角、以及与此对应的方向盘转角输出至控制器。
如图1所示，本发明提供一种平行泊车的路径规划方法，包括如下步骤：
步骤1：路径参数的计算与存储
根据最小泊车位确定泊车起始区域G，在所述泊车起始区域内均匀生成N个泊车起始点分别作为泊车路径的起始点Q，其泊车起始区域内起始点设计如图2所示，每个起始点，包括起始点横坐标，纵坐标，车身初始航向角度信息。每个起始点距离为H，为提高泊车路径计算精度，H一般取0.2M至0.3M，并基于不同车身初始航向角度(所述车身初始航向角度包括0度，正负1度，正负2度，正负4度，正负6度，正负8度和正负10度)，采用B样条曲线获取所述泊车路径，可得到13*N条泊车路径，存储泊车起始区域内所有起始点对应的B样条曲线的泊车路径参数；
所述泊车路径的获取包括：
以车位外侧起始点为原点建立全局坐标系，采用四次五阶B样条控制点的样条曲线计算泊车路径；
基于不同车身初始航向角度的泊车路径的起始点，基于B样条曲线随机生成9个控制点，以B样条曲线最后一点切线最小为目标，采用多种群遗传算法对所述B样条曲线的控制点进行优化，获得所述9个控制点的近似解；其中多种群遗传算法将泊车中约束条件转化为罚函数，并设计自适应罚函数，避免遗传算法早熟问题。
通过非线性等式和不等式约束(fmincon)优化函数求解所述9个近似解的精确坐标，所述非线性等式和不等式约束满足：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R，其中R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
所述泊车路径包括9个控制点，每相邻5个控制点生成一段B样条曲线，所述9个控制点生成5段B样条曲线，所述泊车路径由所述5段B样条曲线平滑连接构成。
需要说明的是，控制点的选取并不限于为9个控制点，控制点越多，计算完控制点后约束验证成功的概率越高。但是控制点过多会对B样条函数优化求解的成功率带来挑战。故控制点个数应该控制在9-19之间。
得到的控制点记为:M1，M2，…，M9，由9个控制点M1-M9的坐标可得到泊车路径曲线。
其中，x为泊车路径曲线在X轴的坐标，y为泊车路径曲线在Y轴的坐标，t为泊车路径曲线的无量纲变量且t∈[0,1]，M为B样条曲线的控制点，P为调节矩阵，
所述泊车路径曲线的一阶导数为：
x'为路径X轴坐标对t的一阶导数，y'为路径Y轴坐标对t的一阶导数，t为B样条曲线变量，M为B样条曲线控制点。
所述泊车路径曲线的二阶导数为：
x”为为路径X轴坐标对t的二阶导数，y”为路径Y轴坐标对t的二阶导数，t为B样条曲线变量，M为B样条曲线控制点。
所述泊车路径参数包括：泊车起始区域内每条B样条曲线的泊车路径起始点坐标，车身初始航向角度，泊车路径的控制点坐标，泊车路径起始点横坐标的一阶导数、二阶导数，以及泊车路径起始点纵坐标的一阶导数、二阶导数。
步骤2：确定当前车辆泊车起始区域，其包括：
获取当前车辆在所有泊车路径L(步骤1中得到的13*N条泊车路径)下的泊车面积O，单条泊车路径的泊车面积如图3所示；
对当前车辆待泊车位C进行障碍物检测，
当检测存在障碍物时，获取障碍物位置，并筛选出与所述障碍物无重叠的泊车面积及其对应的泊车路径起始点，以所述对应的泊车路径起始点为圆心，以r为半径做圆，所得到的区域为当前车辆泊车起始区域，其中，r为经验值，一般取值范围为0.15～0.2m；
当检测不存在障碍物时，以步骤1确定的泊车起始区域为当前车辆泊车起始区域。
将车辆停至当前车辆泊车起始区域内任意位置，使用传感器获得当前车辆横纵坐标以及车身倾斜角，并获取距离当前泊车位置较近的起始点，其包括：
以当前车辆停至当前车辆泊车起始区域内的位置为圆心，以r为半径做圆，获取所有X轴的坐标和Y轴的坐标均在所述圆内的泊车起始点。
提取所有X轴的坐标和Y轴的坐标均在所述圆内的泊车起始点对应的且与当前车身倾斜角度接近的泊车路径参数，计算新的泊车路径的前三个控制点：
式中，M1x为第一个控制点沿第一方向(X轴)的坐标，M1y为第一个控制点沿第二方向(Y轴)的坐标，M2x为第二个控制点沿第一方向的坐标，M2y为第二个控制点沿第二方向的坐标，M3x为第三个控制点沿第一方向的坐标，M3y为第三个控制点沿第二方向的坐标，M4x为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第一方向的坐标，M4y为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第二方向的坐标，xqs为当前车辆起始点沿第一方向的坐标，yqs为当前车辆起始点沿第二方向的坐标，xqs’为当前车辆起始点沿第一方向的坐标的一阶导数值，yqs’为当前车辆起始点沿第二方向的坐标的一阶导数值，xqs”为当前车辆起始点沿第一方向的坐标的二阶导数值，yqs”为当前车辆起始点沿第二方向的坐标的二阶导数值，所述第一方向与第二方向垂直；
将已知量M4x，M4y，xqs，yqs，xqs’，yqs’，xqs”，yqs”代如上述方程组，并联立前三个方程并解方程组可得M1x，M2x，M3x的值；联立后三个方程并解方程组可得M1y，M2y，M3y。即得到新的第一、二、三控制点坐标。
步骤:3：将计算的三个控制点代替距离当前泊车位置较近的泊车起始点对应的泊车路径的前三个控制点，并与所述距离当前泊车位置较近的泊车起始点对应的泊车路径之后的控制点一起生成以当前泊车位置为起始点的新的B样条曲线：
式中，x1为新的B样条曲线沿第一方向的坐标，y1为新的B样条曲线沿第二方向的坐标，t为新的B样条曲线的无量纲变量且t∈[0,1]，M为新的B样条曲线的控制点，P为调节矩阵；
步骤4：将获得的新的B样条曲线离散为路径点，将新的B样条曲线等间距离散为路径点；
验证每一个路径点是否满足约束条件，所述约束条件为：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R，其中R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
若满足约束，则输出路径，
若不满足，则返回步骤2，选取另一个距离当前泊车位置较近的泊车起始点，并计算新的控制点。
满足约束条件的路径点对应的B样条曲线即为泊车路径。
实施例
选取的车辆和车库参数为：
| 车长 | 4.2m |
| 车宽 | 1.6m |
| 轴距 | 2.4m |
| 轮距 | 1.3m |
| 车库长 | 6.4m |
| 车库宽 | 2m |
检测车位内无障碍物存在；
现车辆停的位置为(8.90，1.65)，且此时车辆航向角为0度；
获取距离当前车辆停车位置较近的起始点位置为(8.708，1.510)，该起始点横坐标一阶导数为-1.4843，该起始点横坐标二阶导数为1.5722，该起始点纵坐标一阶导数为-0.00148，该起始点纵坐标二阶导数为0.00375，该起始点车辆的车辆航向角为0度。该起始点的控制点参数为：
经上述公式，计算新的泊车路径的前三个控制点为：
M＝[14.9847 1.7384
8.7055 1.6079
8.8325 1.6915]；
与所述距离当前泊车位置较近的泊车起始点(8.708，1.510)对应的泊车路径之后的控制点一起生成以当前泊车位置(8.90，1.65)为起始点的新的B样条曲线，其控制点为：
将获得的新的B样条曲线离散为路径点，得到新的泊车路径L，如图4所示，从图中可看出，该车辆可以安全、准确且平稳停入泊车位内。
本发明提供的平行泊车的路径规划方法，其采用基于B样条曲线的平行泊车路径规划算法来解决在泊车起始点区域内任意位置不能实时生成满足车辆参数和碰撞约束的B样条路径的问题，在泊车起始点区域内得到车辆的位置和车身倾斜角后，计算得到当前起始状态到车位的泊车路径。规划的路径光滑连续可导、转弯半径约束等，满足车辆自身结构参数约束和碰撞约束，该算法能快速实现机器人路径规划，方法简单、可靠，易于实现，计算量小，实时性较好，能较好的满足平行泊车路径规划要求。能够使得泊车轨迹生成简便、计算资源占用少且泊车过程流畅。
尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。
Claims (10)
1.一种平行泊车的路径规划方法，其特征在于，包括如下步骤：
步骤1：确定泊车起始区域，在所述泊车起始区域内均匀生成多个泊车起始点分别作为泊车路径的起始点，并基于不同车身初始航向角度，采用B样条曲线获取所述泊车路径，存储每条B样条曲线的泊车路径参数；
步骤2：确定当前车辆泊车起始区域，并将车辆停至当前车辆泊车起始区域内任意位置，获取距离当前泊车位置较近的起始点，并提取所述距离当前泊车位置较近的起始点对应的且与当前车身倾斜角度接近的泊车路径参数，计算新的泊车路径的前三个控制点：
式中，M1x为第一个控制点沿第一方向的坐标，M1y为第一个控制点沿第二方向的坐标，M2x为第二个控制点沿第一方向的坐标，M2y为第二个控制点沿第二方向的坐标，M3x为第三个控制点沿第一方向的坐标，M3y为第三个控制点沿第二方向的坐标，M4x为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第一方向的坐标，M4y为距离当前泊车位置较近的泊车起始点对应的泊车路径的第四控制点沿第二方向的坐标，xqs为当前车辆起始点沿第一方向的坐标，yqs为当前车辆起始点沿第二方向的坐标，xqs’为当前车辆起始点沿第一方向的坐标的一阶导数值，yqs’为当前车辆起始点沿第二方向的坐标的一阶导数值，xqs”为当前车辆起始点沿第一方向的坐标的二阶导数值，yqs”为当前车辆起始点沿第二方向的坐标的二阶导数值，所述第一方向与第二方向垂直；
步骤3：将计算的三个控制点代替距离当前泊车位置较近的泊车起始点对应的泊车路径的前三个控制点，并与所述泊车路径之后的控制点一起生成以当前泊车位置为起始点的新的B样条曲线：
式中，x为新的B样条曲线沿第一方向的坐标，y为新的B样条曲线沿第二方向的坐标，t为新的B样条曲线的无量纲变量且t∈[0,1]，M为新的B样条曲线的控制点，P为调节矩阵；
步骤4：将获得的新的B样条曲线离散为路径点，验证每一个路径点是否满足约束条件；
若满足所述约束条件，则输出路径；
若不满足所述约束条件，则重新进行所述步骤2直至满足所述约束条件。
2.如权利要求1所述的平行泊车的路径规划方法，其特征在于，在所述步骤2中，所述当前车辆泊车起始区域的确定包括：
获取当前车辆在所有泊车路径下的泊车面积；
对当前车辆待泊车位进行障碍物检测，包括：
当检测存在障碍物时，获取障碍物位置，并筛选出与所述障碍物无重叠的泊车面积及其对应的泊车路径起始点，以所述对应的泊车路径起始点为圆心做出圆形区域，其为当前车辆泊车起始区域，其中，所述圆形区域的半径为r，取值为0.15～0.2m；
当检测不存在障碍物时，以所述步骤1确定的泊车起始区域为当前车辆泊车起始区域。
3.如权利要求2所述的平行泊车的路径规划方法，其特征在于，在所述步骤2中，距离当前泊车位置较近的泊车起始点的获取包括：
以当前车辆停至当前车辆泊车起始区域内的位置为圆心，以r为半径做圆，获取所有沿第一方向的坐标和沿第二方向的坐标均在圆内的泊车起始点。
4.如权利要求3所述的平行泊车的路径规划方法，其特征在于，在所述步骤4中，所述约束条件为：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R；其中，R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
5.如权利要求1所述的平行泊车的路径规划方法，其特征在于，在所述步骤1中，所述泊车路径的获取包括：
基于不同车身初始航向角度的泊车路径的起始点，基于B样条曲线随机生成多个控制点，以B样条曲线最后一点切线最小为目标，采用多种群遗传算法对所述B样条曲线的控制点进行优化，获得所述多个控制点的近似解；
通过非线性等式和不等式约束优化函数求解所述多个近似解的精确坐标，所述非线性等式和不等式约束满足：
车辆沿路径行驶与周围物体不发生碰撞；
车辆到达泊车路径终点时完全进入车位且与车位平行；
新的泊车路径曲线的最大曲率不大于1/R；其中，R为车辆的最小转向角；
车辆以规定车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速。
6.如权利要求5所述的平行泊车的路径规划方法，其特征在于，所述泊车路径包括9个控制点，每相邻5个控制点生成一段B样条曲线，所述9个控制点生成5段B样条曲线，所述泊车路径由所述5段B样条曲线平滑连接构成。
7.如权利要求1所述的平行泊车的路径规划方法，其特征在于，在所述步骤3中，所述调节矩阵P为：
8.如权利要求1所述的平行泊车的路径规划方法，其特征在于，所述泊车路径参数包括：泊车起始区域内每条B样条曲线的泊车路径起始点坐标，车身初始航向角度，泊车路径的控制点坐标，泊车路径起始点横坐标的一阶导数、二阶导数，以及泊车路径起始点纵坐标的一阶导数、二阶导数。
9.如权利要求1所述的平行泊车的路径规划方法，其特征在于，所述泊车起始区域根据最小泊车位确定。
10.如权利要求1所述的平行泊车的路径规划方法，其特征在于，所述车身初始航向角度包括0度，正负1度，正负2度，正负4度，正负6度，正负8度和正负10度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810794684.5A CN108891412B (zh) | 2018-07-19 | 2018-07-19 | 一种平行泊车的路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810794684.5A CN108891412B (zh) | 2018-07-19 | 2018-07-19 | 一种平行泊车的路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108891412A CN108891412A (zh) | 2018-11-27 |
| CN108891412B true CN108891412B (zh) | 2019-08-23 |
Family
ID=64350961
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810794684.5A Expired - Fee Related CN108891412B (zh) | 2018-07-19 | 2018-07-19 | 一种平行泊车的路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108891412B (zh) |
Families Citing this family (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20200084464A (ko) * | 2018-12-27 | 2020-07-13 | 현대자동차주식회사 | 자율 발렛 주차를 지원하는 시스템 및 방법, 그리고 이를 위한 인프라 및 차량 |
| CN109814115B (zh) * | 2019-01-16 | 2021-01-26 | 杭州湘滨电子科技有限公司 | 一种垂直泊车的角度识别校正方法 |
| CN109866761B (zh) * | 2019-03-05 | 2020-07-10 | 重庆长安汽车股份有限公司 | 自动泊车路径规划方法、系统及计算机可读存储介质 |
| CN109866762B (zh) * | 2019-03-11 | 2021-06-29 | 武汉环宇智行科技有限公司 | 基于高精度地图的自主平行泊车方法 |
| CN111731269B (zh) * | 2019-03-22 | 2022-07-12 | 广州汽车集团股份有限公司 | 一种自动泊车路径规划方法及其系统 |
| CN112068537B (zh) * | 2019-05-21 | 2023-07-07 | 宇通客车股份有限公司 | 基于区域划分的自动驾驶公交车进/出站控制方法与装置 |
| CN110239526B (zh) * | 2019-06-05 | 2020-08-04 | 北京理工大学 | 一种基于准均匀b样条曲线的泊车路径规划方法 |
| CN110262506A (zh) * | 2019-07-02 | 2019-09-20 | 华人运通(上海)自动驾驶科技有限公司 | 预瞄点的确认方法、车辆行驶控制方法、装置及设备 |
| CN110843767A (zh) * | 2019-10-25 | 2020-02-28 | 池州学院 | 一种基于样条理论的自动入库泊车方法及系统 |
| CN110775053B (zh) * | 2019-10-28 | 2021-04-20 | 湖南大学 | 一种基于碰撞情况分析的自动泊车起始区域确定方法 |
| CN111016886B (zh) * | 2019-12-19 | 2021-07-30 | 合达信科技集团有限公司 | 一种基于b样条理论的自动泊车路径规划方法 |
| CN111650930B (zh) * | 2020-04-09 | 2023-04-14 | 吉利汽车研究院（宁波）有限公司 | 基于遗传算法的自动泊车路径规划方法及装置 |
| CN111547049A (zh) * | 2020-05-22 | 2020-08-18 | 北京罗克维尔斯科技有限公司 | 车辆泊车入位的控制方法、装置及车辆 |
| FR3111314B1 (fr) | 2020-06-11 | 2022-07-29 | Renault Sas | Procédé et système d'aide au stationnement d'un véhicule automobile |
| CN111830979B (zh) * | 2020-07-13 | 2024-06-07 | 广州小鹏自动驾驶科技有限公司 | 一种轨迹优化方法和装置 |
| CN112277932A (zh) * | 2020-10-21 | 2021-01-29 | 深圳市德航智能技术有限公司 | 基于北斗定位的农机自动驾驶系统关键技术研发 |
| CN113002530B (zh) * | 2021-04-12 | 2022-05-20 | 东风柳州汽车有限公司 | 基于不平路面的自动泊车方法、装置、设备及存储介质 |
| CN113460040B (zh) * | 2021-07-23 | 2023-02-03 | 广州小鹏自动驾驶科技有限公司 | 泊车路径确定方法、装置、车辆及存储介质 |
| CN113799766B (zh) * | 2021-09-17 | 2023-04-18 | 东风汽车集团股份有限公司 | 带约束的平行泊车路径规划方法、电子设备及存储介质 |
| CN114013426A (zh) * | 2021-11-11 | 2022-02-08 | 上海商泰汽车信息系统有限公司 | 水平泊车路径规划方法及装置、可读存储介质、终端 |
| CN114162113B (zh) * | 2021-12-28 | 2024-04-09 | 武汉理工大学 | 一种结合多段式曲线的平行泊车路径规划方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016120914A (ja) * | 2016-03-07 | 2016-07-07 | 株式会社デンソー | 車両制御装置 |
| DE102015220643A1 (de) * | 2015-10-22 | 2017-04-27 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Verringern eines Kollisionsrisikos einer Kollision eines Kraftfahrzeugs mit einem Objekt |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102012010130A1 (de) * | 2012-05-23 | 2012-12-13 | Daimler Ag | Verfahren zum Ermitteln von kollisionsgefährdenden Lenkwinkelwerteneines Kraftfahrzeuges unter Berücksichtigung eines dynamischen Abstandsgrenzwerts |
| FR2994550B1 (fr) * | 2012-08-14 | 2015-07-17 | Renault Sa | Strategie de parking automatique en creneau |
| JP5566445B2 (ja) * | 2012-12-13 | 2014-08-06 | ダイハツ工業株式会社 | 運転支援装置 |
| DE102013019371A1 (de) * | 2013-11-19 | 2015-05-21 | Audi Ag | Verfahren zum Betrieb eines Fahrerassistenzsystems zum Schutz eines Kraftfahrzeugs vor Beschädigungen bei einem Rangiervorgang und Kraftfahrzeug |
| CN104442810A (zh) * | 2014-10-22 | 2015-03-25 | 合肥星服信息科技有限责任公司 | 一种汽车泊车辅助仪 |
| CN107672585B (zh) * | 2017-08-29 | 2019-09-17 | 广州小鹏汽车科技有限公司 | 一种自动泊车路径规划方法及系统 |
-
2018
- 2018-07-19 CN CN201810794684.5A patent/CN108891412B/zh not_active Expired - Fee Related
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102015220643A1 (de) * | 2015-10-22 | 2017-04-27 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Verringern eines Kollisionsrisikos einer Kollision eines Kraftfahrzeugs mit einem Objekt |
| JP2016120914A (ja) * | 2016-03-07 | 2016-07-07 | 株式会社デンソー | 車両制御装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108891412A (zh) | 2018-11-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108891412B (zh) | 一种平行泊车的路径规划方法 | |
| CN107672585B (zh) | 一种自动泊车路径规划方法及系统 | |
| CN110466516B (zh) | 一种基于非线性规划的曲线道路自动车换道轨迹规划方法 | |
| EP3829958B1 (en) | Method for steering a vehicle and apparatus therefor | |
| CN113306549B (zh) | 一种自动泊车轨迹规划算法 | |
| CN105197010B (zh) | 辅助泊车系统以及辅助泊车控制方法 | |
| CN109131317A (zh) | 基于多段式规划与机器学习的自动垂直泊车系统及方法 | |
| CN110189547B (zh) | 一种障碍物检测方法、装置及车辆 | |
| CN111016898B (zh) | 一种智能车轨迹换道规划方法 | |
| JP2021500259A (ja) | 駐車プロセスを制御するための方法 | |
| CN110307850A (zh) | 航迹推算定位方法及自动泊车系统 | |
| CN108423067A (zh) | 水平泊车控制方法及控制系统 | |
| CN105593776B (zh) | 车辆位置姿势角推定装置及车辆位置姿势角推定方法 | |
| CN109143255B (zh) | 一种铰接轮式装载机姿态预测方法 | |
| CN110271539A (zh) | 一种自动垂直泊车控制系统 | |
| CN113589685B (zh) | 一种基于深度神经网络的挪车机器人控制系统及其方法 | |
| CN113581181B (zh) | 智能车辆超车轨迹的规划方法 | |
| CN115489548A (zh) | 智能汽车园区道路路径规划方法 | |
| CN109814115A (zh) | 一种垂直泊车的角度识别校正方法 | |
| CN111930126A (zh) | 一种基于差速轮组agv的导航纠偏方法 | |
| CN114132305B (zh) | 用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆 | |
| CN114889643A (zh) | 一种运动障碍物的三元素自主避障方法 | |
| CN116576875B (zh) | 四轮转向车辆外轮廓无碰撞局部路径实时规划方法及系统 | |
| CN116300948A (zh) | 一种园区快递配送车定位与最优路径的规划控制方法 | |
| CN116080678A (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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190823 Termination date: 20200719 |