APA(自动泊车辅助系统):路径规划算法(Dubins算法)
文章目录:
1.车辆运动学模型
2.Dubins算法概述
3.Dubins算法计算推导
1.车辆运动学模型
综合前面两节对APA泊车过程的描述,车辆在整个泊车过程中,其运动学方程可参考如下公式:
转弯半径:转弯半径在整个泊车过程中,是一个非常重要的参数。车辆左泊入与右泊入不与车库发生碰撞,首先应考虑的约束条件即是在最小转弯半径的前提下对车辆进行几何约束。故,转弯半径可表示为:
式1 车辆转弯半径表达式
由以前的章节可知,实际泊车过程中,车辆转向系统的阿克曼角在转弯过程中产生的轴向阿克曼偏移量对车辆转弯半径有着实际工程影响,因此在实际工程应用时,应考虑阿克曼偏移量对车辆转弯半径的影响。故,式1可转变为如下表述方式:
式2 考虑阿克曼偏移量的车辆转弯半径
而简化的车辆运动学模型方程可以表示为:
式3 车辆在全局坐标下的运动方程表达式
根据APA以前发布的文章对车辆单位时间间隔运动方程的推导可知,将式3离散化,同时取时间间隔为t,则离散方程可表达为:
式 4 车辆位姿坐标
以上4个公式为车辆在泊车过程中的运动学方程推导,也是理解后续路径规划的前提,其中转弯半径的概念对后续路径规划算法的理解至关重要。
2.Dubins算法概述
APA泊车过程中车辆运动可分为三类(车辆任何形式下的运动均如此):即直线运动(S)、左转(L)、右转(R),Dubins 曲线不考虑车辆后退(汽车只能朝前开)。Dubins曲线是在满足曲率约束和规定的始端和末端的切线方向的条件下,连接两个二维平面(即X-Y平面)的最短路径。
即Dubins路径规划的原理:在运动方向已经知道且最小半径已经确定的前提下,通过车辆的三种运动形式组合成车辆起点至终点的路径。
3.Dubins算法计算推导
对于APA整个泊车过程的分析可知,均是采用圆弧与直线运动的方式进行泊车,因此与Dubins原理下对车辆运动的描述非常吻合。因此,参考Dubis原理,在泊车过程中应首先计算出泊车关键点,即初始点与终点(每一段的初始与终点,因为整个泊车过程可分为很多段来完成)。
Dubins路径类型用直线(S)、左转(L)和右转(R)组成,一共分为6种类型:{RSR,RSL,LSR,LSL,RLR,LRL},以其中一种情况进行推导,其余同理,推导过程如下:
图 1 Dubins RSR规划原理图
将次反代入运动学方程即可求解出初始点与终点的切点坐标。
其余Dubins规划算法情况类似于RSR,其余情况如下所示:
Dubins 路径规划算法最佳六种类型