一种基于车辆运动学模型的自动泊车轨迹规划方法与流程

1.本发明涉及智能汽车领域，尤其是涉及一种基于车辆运动学模型的自动泊车轨迹规划方法。


背景技术：

2.当前中国泊车市场存在巨大的未满足需求，在泊车上存在“停车难、取车难”等强烈痛点，另外根据交通事故数据库统计资料和保险公司事故统计资料，泊车导致的事故占到各类事故的44％，其中大约1/2到3/4的泊车碰撞是倒车造成的。自主泊车系统不仅能够提高停车的安全率，同时还可以提高停车场停车位的使用率。目前几乎所有商业化的泊车辅助系统都基于各种传感器和基础设施，并且具有被动安全辅助功能，如在停车时警告驾驶员障碍物的存在，即使采用智能泊车辅助系统等主动泊车辅助系统，泊车位也需由驾驶员确定，为了提高驾驶员的便利性和停车场的安全性，自主代客泊车系统已经得到了广泛的研究。
3.轨迹规划作为无人驾驶系统中重要的一环，在自动泊车领域也发挥了重要的作用。基于搜索的a*算法非常适用于非结构化道路的无人车路径规划，在泊车工况下具有其独特的算法优势，然而由于泊车环境较狭窄，路径精度要求高，基于搜索的传统无人车路径规划方法搜索失败可能性较大，并且不利于控制模块进行轨迹跟踪，从而达不到预想的泊车精度，此外，搜索算法需要对周围环境地图进行高精度离散，极大影响算法实时性，在泊车环境并不适用。
4.因此，如何解决泊车轨迹搜索算法所带来的问题，是本领域技术人员亟待解决的问题。


技术实现要素：

5.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于车辆运动学模型的自动泊车轨迹规划方法。
6.本发明的目的可以通过以下技术方案来实现：
7.一种基于车辆运动学模型的自动泊车轨迹规划方法，包括以下步骤：
8.s1:车辆行驶至预先采集的停车场路径地图中的参考路径附近，启动车辆自动代客泊车功能，进入acc模式沿前方参考路径行驶，根据前方参考路径长度，规划相应路径控制点的速度和加速度，发送给控制模块进行轨迹跟踪；
9.s2:车辆在acc模式下的行驶过程中，通过前向视觉摄像头和毫米波雷达检测前方行进轨迹上是否存在障碍物，如有，则对前方参考路径控制点规划速度和减速度，进行停车避撞，等待障碍物通行；
10.s3:车辆行驶至目标泊车库位附近区域，通过环视摄像头检测是否存在有效库位，如不存在有效库位，车辆将继续沿参考路径行驶并检测有效库位，如存在，则根据库位类别、库位信息和车道线信息规划泊车路径，进入泊车模式；
11.s301:在目标库位附近，环视摄像头没有检测到尺寸满足要求且库位内部没有车辆的有效库位时，车辆将继续沿参考路径行驶，并持续检测搜索有效库位，当车辆行驶至参考路径终点时，将减速停车，退出自动代客泊车功能；
12.s302:一旦检测到有效库位，车辆将停车减速，在完全停止后，将根据最终静止状态下检测到的库位信息进行泊车路径规划，进入泊车模式；
13.s4:根据检测到的有效库位角点信息，计算得出库位类型、库位尺寸以及泊车目标终点信息，同时考虑考虑车道线宽度约束，以靠近车辆一侧位于前方的库位角点为第0点，靠近车辆一侧位于后方的库位角点为第3点，库位在右侧时顺时针(在左侧时逆时针)规定四个库位角点编号为0123，以当前车辆后轴中心为坐标原点，车辆前方为x正方向，车辆左方为y正方向，逆时针为航向角正方向，进行泊车路径规划；
14.s401:库位类型为垂直库位，计算向前行驶段路径终点a的坐标，以及在a点开始一次泊入情况下后轴中心轨迹对应的圆心坐标o1，则一次泊入所需车道宽度为：其中，l
o1_0
表示第0个库位角点到泊车轨迹圆心o1之间的距离，w为库位宽度，r为车辆泊车转弯半径，θ为库位边界相对当前车辆的航向角，当θ大于等于0时，前进段路径终点a的航向角为0，即与库位边界平行，当θ小于0时，前进段路径终点a的航向角等于θ，更为方便一次入库，点α为直线o1_0与库位角点0和库位角点3连成的直线的夹角。若disa小于当前车道宽度约束，则满足一次泊入条件，以o1为圆心规划泊车入库的圆弧路径，当路径向角与库位终点航向角一致时，规划直线倒车路径，到达泊车目标终点位置；
15.s402:库位类型为垂直库位且当disa大于等于当前车道宽度约束时，车辆无法一次泊车入库，需要规划三段式泊车路径；令disa等于当前车道宽度约束长度，计算第一段倒车路径的圆心坐标o1，从a点开始往后泊车，并在车辆与库位角点2、3的连线放生碰撞时停止，其对应的圆心角为β1，根据泊车目标终点坐标和航线信息，以及三段泊车圆弧路径的圆心角之和为90
°
的约束，求得第二段和第三段圆弧泊车路径的圆心o2和o3及其路径对应的圆心角β2和β3，最后规划直线倒车路径到达泊车目标终点位置。
16.s403:库位类型为平行库位，规划由当前车道中心横向距离到泊车目标终点的泊车路径所需的两端圆弧泊车路径，计算其圆心o1和o2的坐标及其圆弧路径对应的圆心角β，最后根据泊车目标终点坐标规划一段前进或倒车的直线调整路径，到达泊车目标终点；
17.s5:根据泊车路径控制点上的车辆行驶方向，给定对应的速度值，并将轨迹发送给控制模块，开始泊车；
18.s6:控制模块开始控制车辆入库后，根据毫米波雷达和超声波雷达检测行进轨迹上是否存在障碍物，若是，则发送停车减速指令，并等待若干时间，等待障碍物移走后继续沿规划轨迹进行泊车，若超过预期等待时间则关闭原路返回，并将当前库位标记为无效库位，返回步骤s3；
19.s7:车辆到达泊车目标终点位置，完成所规划的泊车路径，退出自动代客泊车功能，泊车结束。
20.与现有技术相比，本发明具有以下优点：
21.本发明解决了部分基于搜索的泊车轨迹规划方法遇到的精度不高，可能搜索失败
等问题，考虑人类驾驶员泊车时的车辆运动学模型，在多种不同工况下进行泊车轨迹规划，符合人类驾驶员的泊车行为习惯，可以满足绝大部分自动代客泊车需求；同时，本发明能够很好的处理泊车过程中遇到障碍物的问题，若检测到行进轨迹上存在障碍物较长时间且不再移动，则车辆原路返回，将当前库位标记无效并搜索下一个有效库位，有效地保证了自动代客泊车系统的安全性和过程连续性。
附图说明
22.图1为本发明自动泊车轨迹规划方法的过程流程图。
23.图2为本发明垂直库位泊车一次入库轨迹示意图。
24.图3为本发明垂直库位泊车三段式入库轨迹示意图。
25.图4为本发明平行库位泊车轨迹示意图。
26.图5为本发明车辆运动模型示意图。
具体实施方式
27.下面结合附图和具体实施例对本发明进行详细说明。
28.本发明涉及一种基于车辆运动学模型的自动泊车轨迹规划方法，如图1所示，该方法包括下列步骤：
29.s1：车辆行驶至预先采集过参考路径的停车场中，启动车辆自动代客泊车功能，进入acc模式沿，开始跟随前方最近的参考路径行驶，根据前方参考路径长度，设置速度约束和加速约束，通过优化求解的方法规划相应路径控制点的速度和加速度，发送给控制模块进行轨迹跟踪；
30.s2：车辆在acc模式下的行驶过程中，通过前向视觉摄像头和毫米波雷达检测障碍物，并将其投影至车辆坐标系，随后进行如下判断：
31.a)判断1：前方行进轨迹上是否存在障碍物，若有，则对前方参考路径控制点规划速度和减速度，进行停车避撞，等待障碍物通行；若无，进入s3。
32.s3：车辆行驶至目标泊车库位附近区域，通过环视摄像头检测库位信息，判断是否检测到有效库位，如不存在有效库位，车辆将继续沿参考路径行驶并检测有效库位，如存在，则根据库位类别、库位信息和车道线信息规划泊车路径，进入泊车模式。
33.s4：根据检测到的有效库位角点信息，计算得出库位类型、库位尺寸以及泊车目标终点信息，同时考虑考虑车道线宽度约束，以靠近车辆一侧位于前方的库位角点为第0点，靠近车辆一侧位于后方的库位角点为第3点，库位在右侧时顺时针(在左侧时逆时针)规定四个库位角点编号为0123，以当前车辆后轴中心为坐标原点，车辆前方为x正方向，车辆左方为y正方向，逆时针为航向角正方向，如图2所示，进行泊车路径规划，进行如下判断：
34.a)判断1：根据检测到的每个库位的第0和第3个库位角点，判断库位尺寸和库位类型，若没有检测到第1和第2个库位角点，则根据0、3角点推算出1、2角点的坐标信息，若库位类型为垂直库位，则进入b)判断2，若库位类型为平行库位，进入s43。
35.b)判断2：进行一次泊车横向最小距离判断，计算向前行驶段路径终点a的坐标，以及在a点开始一次泊入情况下后轴中心轨迹对应的圆心坐标o1，则一次泊入所需横向最小
宽度disa为：宽度disa为：
36.其中，l
o1_0
表示第0个库位角点到泊车轨迹圆心o1之间的距离，lw为车辆宽度，r为车辆泊车转弯半径，θ为库位边界相对当前车辆的航向角，当θ小于0时，前进段路径终点a的航向角等于θ，更为方便一次入库，α为直线o1_0与库位角点0和库位角点3连成的直线的夹角。其中w为库位宽度。若disa小于当前车道宽度约束，则进入s41，否则，进入s42；
37.s41：满足一次泊入条件，以o1为圆心规划泊车入库的圆弧路径，当路径向角与库位终点航向角一致时，规划直线倒车路径，到达泊车目标终点位置，如图3所示，其中具体步骤包括：
38.s411：计算泊车圆弧路径的圆心o1坐标：
[0039][0040]
s412：计算前进段路径终点a点坐标：
[0041][0042]
s413：计算第一段圆弧泊车路径对应的圆心角：
[0043][0044]
s414：根据s512的a点坐标，以车辆当前后轴中心为起点，采用4次贝塞尔曲线规划从o到a的第一段前进路径，其中5个控制点的坐标为：
[0045][0046][0047]
s415：根据s513计算的圆心角β，以a点为起点，o1为圆心，画出圆弧ab作为第二段圆弧泊车路径。
[0048]
s416：以b为起点向泊车目标终点g，采用贝塞尔曲线规划第三段泊车路径，到达泊
车目标终点g。
[0049]
s42：不满足一次泊入条件，需要规划三段式泊车路径，如图4所示，其中具体s包括：
[0050]
s421：令disa等于当前车道宽度约束长度，计算第一段倒车路径的圆心坐标o1，
[0051][0052]
s422：计算第一段前进路径终点a的坐标，以车辆当前后轴中心o为起点，使用4次贝塞尔曲线规划第一段前进路径oa：
[0053][0054]
s423：计算第二段泊车圆弧路径ab对应的圆心角，规划第二段圆弧泊车路径：
[0055]
β1＝γ-∠bjo1[0056]
其中，l
o1_23
是o1到库位边23的垂直距离，r
r_out
是车辆转弯中心到车辆后侧最远角点的距离，l
safe2
是车辆停在b点时，与库位2、3角点连线的最小安全垂直距离。
[0057]
s424：计算第三段和第四段圆弧泊车路径对应的圆心角β2和β3，规划第三段和第四段泊车圆弧路径bc和cd，以保证经过三段圆弧泊车路径之后车辆能到达库位中细线上。
[0058][0059]
s425：以d点为起点，使用4次贝塞尔曲线规划第五段倒车路径dg，到达泊车目标终点g。
[0060]
s43：库位类型为平行库位，规划由当前车道中心横向距离到泊车目标终点的泊车路径所需的两端圆弧泊车路径，如图5所示，具体s包括：
[0061]
s431：计算两段圆弧路径对应的圆心角β，使车辆经过2段圆弧泊车路径后，能够到达库位中心线位置上，
[0062][0063]
s432：计算第一段前进路径终点a的坐标：
[0064][0065]
s433：以a为起点，根据相应的圆心角和转弯半径，规划两段圆弧泊车路径ab和bc，使车辆到达库位中心线上。
[0066]
s434：最后根据泊车目标终点坐标规划一段前进或倒车的直线调整路径cg，到达泊车目标终点；
[0067]
s5：根据泊车路径控制点上的车辆行驶方向，给定对应的速度值。将轨迹发送给控
制模块，开始泊车。
[0068]
s6：控制模块开始控制车辆泊车后，根据毫米波雷达和超声波雷达检测行进轨迹上是否存在障碍物，若否，进入s7，若是，则发送停车减速指令，并等待若干时间，等待障碍物移走后继续沿规划轨迹进行泊车，进入s7，若超过预期等待时间则关闭原路返回，并将当前库位标记为无效库位，返回s3；
[0069]
s7:车辆到达泊车目标终点位置，完成所规划的泊车路径，退出自动代客泊车功能，泊车结束。
[0070]
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

技术特征：
1.一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，包括以下步骤：s1：车辆行驶至预先采集的停车场路径地图中的参考路附近，启动车辆自动代客泊车功能，进入acc模式沿前方参考路径行驶；s2：车辆在acc模式下的行驶过程中，通过前向视觉摄像头和毫米波雷达检测前方行进轨迹进行障碍物碰撞检测；s3：车辆行驶至目标泊车库位附近，当靠近目标泊车库位时，通过环视摄像头检测库位角点，收到库位信息，判断所收到的所有库位是否有效，若存在有效库位，则进入泊车模式；若不存在有效库位，则车辆继续行驶，通过环视摄像头搜索有效库位；若车辆到达参考路终点时仍然没有检测到有效库位，则减速停车，退出自动泊车模式；s4：进入泊车模式后，根据环视摄像头检测到的库位角点坐标信息，判断库位类型、库位尺寸大小，其中库位尺寸大小根据环视图像投影至鸟瞰平面后的几何关系确认，根据库位信息确定库位类型、4个角点坐标以及泊车终点坐标与航向角；根据自车当前姿态以及车辆运动学模型，规划从自车后轴中心开始至泊车终点结束的泊车路径；s5：路径规划根据前进和倒车的模式对规划出的泊车路径点赋予速度值，其中速度为正表示前进，速度为负表示倒车，并将最终轨迹发送给控制模块；s6：控制模块根据获取的轨迹信息控制车辆泊车入库，在此过程中，车辆通过前向单目摄像头、毫米波雷达和超声波雷达实时检测车辆行进轨迹上是否存在障碍物，若没有障碍物，则进入步骤s7，若存在障碍物，减速并原地停车等待障碍离去，若等待时间过长，则驾驶员可以选择沿规划路径返回，并重新选择新的有效库位，返回步骤s3；或者驾驶员选择手动接管，退出自动泊车模式；s7：控制模块完成跟踪所有规划处的轨迹，车辆到达泊车终点位置，退出自动泊车模式，泊车结束。2.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s1中，acc模式下，将根据参考路控制点的速度值以及当前车辆状态，规划出前方10m长度控制点的速度和加速度。3.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s2中，车辆在acc模式下的行驶过程中，通过环视模块图像拼接所得的鸟瞰平面图检测库位角点及其内部是否有障碍物，并推导出库位信息，当车辆与目标库位终点靠近至一定距离内时，开始库位检测；当鸟瞰图中存在有效库位时，减速停车，在静止状态下再次检测库位，获得精确的库位坐标信息。4.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s3中，若环视拼接的鸟瞰平面图中的一个库位的角点只检测出2个，则根据其相对距离判断库位类型，并推导另外两个角点坐标。5.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s4中，根据库位类型、库位信息和车道宽度信息确认的泊车模式，包括垂直一次入库泊车、垂直三段式泊车和平行泊车。6.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s5中，规划模块最终输出的轨迹控制点信息包含车辆后轴轨迹中心的坐标x，y、航向角θ，速度v、加速度a、距离s。
7.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s5中，规划模块最终输出的轨迹控制点信息包含车辆后轴轨迹中心的坐标x，y、航向角θ，速度v、加速度a、距离s，以当前车辆后轴中心为坐标原点，车辆前方为x正方向，车辆左方为y正方向，逆时针为航向角正方向。8.根据权利要求1所述的一种基于车辆运动学模型的自动泊车轨迹规划方法，其特征在于，所述的步骤s6中，四个环视摄像头分别安装与车辆左、右后视镜下方、车尾牌照下方和车前牌照上方，毫米雷达安装于车前牌照下方。

技术总结
本发明涉及智能汽车领域，尤其是涉及一种基于车辆运动学模型的自动泊车轨迹规划方法，本发明通过组合定位装置接收车辆定位信息以及目标库位位置信息，通过前向摄像头、毫米波雷达以及超声波雷达获取周围障碍物信息，通过环视感知模块获取库位角点信息。车辆将根据当前姿态、库位信息以及车道宽度信息进行泊车路径规划，根据不同的库位类型、库位信息约束、道路宽度约束以及车辆运动学模型，设计了一次垂直泊车规划、多段式垂直泊车规划以及倒入式平行泊车规划方法，规划成功后，将轨迹控制点发送给车载控制器，车载控制器控制车辆泊入目标库位。与现有技术相比，本发明具有参数调整能力强、算法实时性好等优点。算法实时性好等优点。算法实时性好等优点。


技术研发人员：李拙人 熊璐 冷搏 方培元 邓振文 曾德全 胡一明
受保护的技术使用者：南昌智能新能源汽车研究院
技术研发日：2022.04.08
技术公布日：2022/5/25