基于回旋曲线的平行泊车路径规划

基于回旋曲线的平行泊车路径规划¹

基于圆弧-直线组合方式设计的平行泊车路径由两段相同的圆弧以及与圆弧相切的直线段组成。当两段圆弧的半径设置为车辆最小转弯半径时，基于圆弧-直线组合方式设计的平行泊车路径为最短泊车路径。

1、平行泊车可行泊车起始区域分析

（1）两段圆弧相切或与相同的直线段相切，因此，要求两段圆弧
圆心之间的最小距离等于圆弧直径，即
由几何图形关系，将式（1）的约束条件转化为平行泊车可行泊车起始点区域的圆弧边界AC。AC方程：
式中，W 为车宽；1S 为平行泊车最小泊车位长度与车辆后悬长度之差，可表示为

最小泊车车位长度：车辆在车位直接打满方向盘，以仅圆弧轨迹泊出时，所需的车位长度，将车身右前角的回转半径 与 圆心到车位的纵向距离，勾股定理即可得到上式。
AC，即为O2圆保持不动，将O1圆从O2圆正下方移动到O2圆正右方时（两圆保持相切 ），O1圆的最上点D的移动轨迹。
（2）平行泊车路径的直线段与泊车位前方障碍物的右后点O 无碰撞，保证泊车的安全性。因此，要求两段圆弧的最小角度为

图1 表示清晰，两个角度相减得到 α 角度。
由几何图形关系，将式（4）的约束条件转化为平行泊车可行泊车起始点区域的直线段边界 DB ，方程为

起始就是将D点 沿着直线段轨迹的方向进行延长；在DB线上直接进行圆弧轨迹行驶，都可行驶到直线段轨迹的延长线上
（3）平行泊车路径的圆心为O1 的圆弧段泊车路径中的与泊车位前方障碍物的右后点O 无碰撞，保证泊车的安全性。由此，得到平行泊车可行起始点区域的圆弧边界CD，方程为
将O1蓝色圆，保持与O点的接触，向下向左移动，D点的轨迹即为CD的方程轨迹，蓝色圆保持与O点的接触，简单推导即可得到。

（4）若不考虑泊车空间中侧向障碍物和泊车传感器探测距离的影响，以经过点 A 的直线段 AB 作为平行泊车可行泊车起始点区域的侧向边界，方程为

式（7）仅从泊车路径角度规定平行泊车可行泊车起始点区域的侧向边界，较为保守，泊车标准要求有效的侧向距离为 0.5m-1.5m。
综上所述，平行泊车可行泊车起始点区域是由方程式（2）、方程式（5）和方程式（7）来共同构成的封闭区域 ABDCA。

2、平行泊车的路径规划

采用回旋曲线设计的过渡路径以增加泊车空间需求为代价来完成圆弧与直线的平滑过渡，避免轮胎原地转向现象。现计算线段QI 和线段 I’Q’的长度，以量化采用回旋曲线设计的过渡路径增加的泊车空间需求。

如图 2 所示，建立坐标系 HXY ，由回旋曲线上任意点曲率随曲线长线性变化可得回旋曲线上点H 到点 K 的曲线长为

式中， c 为曲率变化率。（注意：曲率随线长的变化率）
曲率变化率与车辆转角关系的公式推导 待补充
由回旋曲线上任意点坐标计算公式和切线角计
算公式可得点 K 的坐标为(xk，yk）和圆弧IKK’I ’ 在点 K 的切线与直线HO2 的夹角为α2 为

(xk，yk）是sk的函数，sk与曲率变化率相关。即以及sk求(xk，yk）以及αk。
将sk离散处理，即可求得回旋曲线的全部轨迹点。 详细计算代码可见缓和曲线——回旋线的计算
由式（9）和式（11）可得点 H 与点 I 的横向偏移量为

由式（10）和式（11）可得点 H 与点 I 的纵向偏移量为

由式（12）和式（13）可得线段QI 和线段 I’Q’的长度，即平滑偏移量为

基于回旋曲线的平行泊车路径平滑原理，对图3 所示的由圆弧 直线 圆弧的路径曲线进行修正。
n0已知，G1点为起点，后面所有点的修正都是基于G1点与n0，进行计算得到。
将点G1修正为点G1’ ，点G2 修正为点G2’ ，点G3 修正为点G3*，点G4 修正为点G4*。修正后的点G1’ 的坐标变为
修正后的点G2’的坐标为：

修正后的点G3的坐标为：

修正后的点G4的坐标为：

    若原始的平行泊车路径曲线中的点2G 和点3G 之间的距离小于2n0，则向量 G2 G2’与向量 G3 G3，产生不合理的部分重叠现象。为了避免此部分重合现象，按照图 4 所示的方式，将点G1‘’ 进一步的修正为点G1*，点G2’ 进一步的修正为点G2*。以1O 为圆心的圆弧G1G2 修正为以O1为圆心的圆弧G1G2**。
将 ‘O1**’ 点沿轨迹直线段方向 延长了两个n0长度。

该修正后的平行泊车路径曲线由平滑曲线段G1G2、直线段G2G3和平滑曲线段G3G4组成。
由式（14）可知，平滑偏移量n0是圆弧角度的函数。
由于平滑偏移量n0未知，需要在上述分析得到平行泊车路径平滑曲线G1G2G3G4的基础上增加一个平行泊车起始点G0*，已知其坐标为（xG0*，yG0*）。为何要增加一个G0？开始没想明白，后来发现是因为点G4*在G4点的基础上向左产生了滑移，导致车辆后悬碰撞车位左边界，故取一个“平滑偏移量最大值为nmax = n1 + n2”，消除滑移影响 。
平滑偏移量最大值为nmax = n1 + n2 ，平行泊车路径平滑曲线G0G1G2G3G4*具体规划过程为：
此部分都是几何推导，只需耐心计算即可。
式28、式33有错误，应为（1-cos），可以计算验证一下。

此部分都是几何推导，只需耐心计算即可。

3、平行泊车可行泊车起始区域修正

如图 4 所示，将原始的起始点G1沿着圆弧G1G2和圆弧G3G4 的公共切线方向平移2 * n0，再沿着 X 轴方向平移nmax得到新的起始点G0 * 。基于此准则对图5 所示的原始的可行泊车起始点区域 ABDCA 进行修正：

（1）若原始的起始点1G 位于图 5 所示的可行泊车起始点区域的圆弧边界AC上，则圆弧G1G2和圆弧G3G4的公共切线方向与圆弧边界 AC 上的原始起始点G1的切线方向相同。 因此，图 5 所示的原始可行泊车起始点区域的圆弧边界AC可以修正为圆弧边界A * C * ，相应的描述圆弧边界AC的方程式（2）修正为


标黄部分久思不得其解，终于看明白，圆弧G1G2和圆弧G3G4的公共切线方向与G1在圆弧 AC 上的切线方向相同，当两圆接触相切时，从圆O2与弧AC的圆心来分析，圆O2的弧G3G4和圆AC的弧G1G4的圆心角都是α，故切线方向相同。
进而，半径也很好得出来了，勾股定理：修正半径 = (2R)² + (2n0)²。

由于平滑偏移量0n 未知，式（34）进一步保守的修正为：

（2）若原始的起始点G1位于图 5 所示的可行泊车起始点区域的直线段边界 DB 上，则圆弧G1G2和圆弧G3G4的公共切线方向即为直线段边界 DB的方向。因此，图 5 所示的原始可行泊车起始点区域的直线段边界 DB 修正为直线段边界D * B * ，相应的描述直线段边界 DB 的方程式（5）修正为


简单几何计算
（3）若原始的起始点G1位于图 5 所示的可行泊车起始点区域的圆弧边界CD上，则圆弧G1G2和圆弧G3G4 的公共切线方向随着原始起始点G1在圆弧边界CD上的位置变化而变化。假设图 5 所示的原始可行泊车起始点区域的圆弧边界CD上任意点的坐标为（xCD，yCD），修正后的圆弧边界C * D * 相应点的坐标为（xC * D * ，yC * D * ），则有
式中，θ 为基于圆弧-直线组合方式规划的起始点为（xCD，yCD），目标点为 ( −S1, −W/2) 的单步平行泊车路径曲线中的圆弧的角度。
由于平滑偏移量n0未知，式（37）进一步保守的修正为

这里貌似也需要采用作图法来确定，个人认为可以保守估计，将原圆弧进行足量的平移，得到具体的表达式。
(4）依然采用经过点 A 的直线段 AB 作为修正后单步平行泊车可行泊车起始点区域的侧向边界，方程为

综上所述，修正后的单步平行泊车可行泊车起始点区域是由方程式（35）、方程式（36）、式（38）和方程式（39）共同构成的封闭区域 A * B * D* C* A* 。

4、计算结果

红色线条为圆弧、直线；绿色线条为回旋线。

---

1. 张家旭, 赵健, 施正堂,杨雄. 基于回旋曲线的平行泊车轨迹规划与跟踪控制[J]. 吉林大学学报(工学版).
   https://doi.org/10.13229/j.cnki.jdxbgxb20190632 ↩︎