Dubins曲线原理与实现

Dubins曲线通常用于机器人和控制理论领域，作为规划轮式机器人、飞机和水下车辆路径的一种方式。前人已经证明连接两点的最短路径可以通过最大曲率的圆弧和直线段的构成，并且必然存在，最佳路径类型可以用与右转（R），左转（L）或直线行驶（S）三种汽车行为来描述，分为六大类，RSR，RSL，LSR，LSL，RLR，LRL。

二、车辆简单模型

如下图所示，车辆简单模型是一个表达车辆运动的简易模型。该模型将车辆看做平面上的刚体运动，刚体的原点位于车辆后轮的中心；x轴沿着车辆主轴方向，与车辆运动方向相同；车辆在任意一个时刻的姿态可以表述为(x, y, $\theta$ )。车辆的运动速度为s；方向盘的转角为 $\phi$ ，它与前轮的转角相同；前轮和后轮中心的距离为L；如果方向角的转角固定，车辆会在原地转圈，转圈的半径为 $\rho$ 。

在一个很短的时间 $\Delta t$ 内，可以认为车辆沿着后轮指向的方向前进，

$\tan (\theta )=dy/dx$

$dy/dx=dy/dt * dt/dx=\dot{y}/\dot{x}=v_{y}/v_{x}$

即，运动车辆需满足如下条件

$\dot{y}*\cos (\theta ) = \dot{x}*\sin(\theta)$

$\dot{\theta }=\frac{S}{L}*\tan (\phi )$

三、Dubins曲线计算

1.坐标变换

设起点为 $s(x_{i}, y_{i}, \alpha _{i})$ ，终点为 $g(x_{g}, y_{g}, \beta _{g})$ ，通过坐标变换将起点和终点放置于x轴，则起点终点连线逆时针旋转 $\theta$ 角，此时起点坐标为 $s(0,0,\alpha)$ ，终点坐标为 $g(d, 0, \beta )$ ，则

$\theta =\arctan (\tfrac{y_g-y_i}{x_g-x_i})$

$D=\sqrt{(x_i-x_g)^{2}+(y_i-y_g)^{2}}$

$d=\frac{D}{R}$

$\alpha =mod(\alpha_i-\theta,2\pi)$

$\beta=mod(\beta_g-\theta,2\pi)$

$\theta$ 为起点和终点的航向差，以上角度的取值均在 $[0,2\pi]$ 之间。上述第三式是为了将半径归一化，在后续的计算中将转弯半径都作为1，便于从角度计算弧长，简化计算。

2.LSL

根据几何关系（公式来源论文：Classiﬁcation of the Dubins set），即d与p在x轴和y轴上的关系，以及车辆偏航角变化与 $\beta$ 角的关系：

$p\cos(\alpha+t)-\sin(\alpha)+\sin(\beta)=d$

$p\sin(\alpha+t)+\cos(\alpha)-\cos(\beta)=0$

$\alpha+t+q=\beta[mod2\pi]$

角度运算关系：

$\angle \alpha=\angle1+\pi$

$\angle1=\angle2+\angle3$

$\angle t+\angle2=\pi$

$\cos(\angle\alpha+\angle t)=\cos(\angle3)$

$\sin(\angle \alpha+\angle t)=-\sin(\angle3)$

$\sin(\angle \alpha)=-\sin(\angle1), \cos(\angle \alpha)=\cos(\angle1)$

几何关系：

另外车辆从s走过s+t+p角度，到g点角度为 $\beta$ ，即

$\alpha+t+q=\beta[mod2\pi]$

解方程得

$t_{lsl}=-\alpha+\arctan(\frac{\cos\beta-\cos\alpha}{d+\sin\alpha-\sin\beta})[mod2\pi]$

$p_{lsl}=\sqrt{2+d^2-2\cos(\alpha-\beta)+2d(\sin\alpha-\sin\beta)}$

$q_{lsl}=\beta-\arctan{\frac{\cos\beta-\cos\alpha}{d+\sin\alpha-\sin\beta}}[mod2\pi]$

总长度等于：

$L_{lsl}=t_{lsl}+p_{lsl}+q_{lsl}=-\alpha+\beta+p_{lsl}$

若 $\angle3=0$ ，上式依然成立。

3.LRL

对于LRL和RLR的情况，一定是 $D<4R$ 的情况下才会出现，即使 $D=4R$ 的情况，前面的LSL 等4种情况中的某种情况也会比这两个情况要更优，因此，LRL 情况时构成的三个圆，都是等半径的，也即等于 1。

跟前面 LSL 的方法一样，在x和y向构建方程组：

$-2\sin(\alpha+t-p)+2\sin(\alpha+t)=d+\sin\alpha-\sin\beta$

$2\cos(\alpha+t-p)-2\cos(\alpha+t)=-\cos\alpha+\cos\beta$

$\alpha+t-p+q=\beta[mod2\pi]$

角度运算关系：

$\angle \alpha=\pi+\angle1$

$\angle t=\pi+\angle2$

$\angle p=\angle HO_1K+\angle3=\angle1+\angle2+\angle3$

$\angle \alpha+\angle t=2\pi+\angle1+\angle2=\angle1+\angle2$

$\angle \alpha+\angle t-\angle p=-\angle3$

$\angle q-\angle3=\angle \beta$

正余弦关系：

$\sin(\alpha)=-\sin(\angle1)$

$\cos(\alpha)=-\cos(\angle1)$

$\sin(\angle \alpha+\angle t)=\sin(\angle1+\angle2)$

$\cos(\angle \alpha+\angle t)=\cos(\angle1+\angle2)$

$\sin(\angle \alpha+\angle t-\angle p)=\sin(-\angle3)$

$\cos(\angle \alpha+\angle t-\angle p)=\cos(-\angle3)$

几何关系：

解方程:

$t_{lrl}=(-\alpha+\arctan(\frac{\cos\beta-\cos\alpha}{d+\sin\alpha-\sin\beta})+\frac{p_{lrl}}{2})[mod2\pi]$

$p_{lrl}=\arccos\frac{1}{8}(6-d^2+2\cos(\alpha-\beta)+2d(\sin\alpha-\sin\beta))[mod 2\pi]$

$q_{lrl}=\beta[mod 2\pi]-\alpha+2p_{lrl}[mod 2\pi]$

总长度：

$L_{lrl}=t_{lrl}+p_{lrl}+q_{lrl}=-\alpha+\beta+2p_{lrl}$

4.RSR

$p\cos(\alpha-t)+\sin\alpha-\sin\beta=d$

$p\sin(\alpha-t)-\cos\alpha+\cos\beta=0$

$\alpha-t-q=\beta[mod 2\pi]$

求解

$t_{rsr}=\alpha-\arctan\frac{\cos\alpha-\cos\beta}{d-\sin\alpha+\sin\beta}[mod 2\pi]$

$p_{rsr}=\sqrt{2+d^2-2\cos(\alpha-\beta)+2d(\sin\beta-\sin\alpha)}$

$q_{rsr}=-\beta[mod 2\pi]+\arctan(\frac{\cos\alpha-\cos\beta}{d-\sin\alpha+\sin\beta})[mod 2\pi]$

$L_{rsr}=t_{rsr}+p_{rsr}+q_{rsr}=\alpha-\beta+p_{rsr}$

5.LSR

$p\cos(\alpha+t)+2\sin(\alpha+t)-\sin\alpha-\sin\beta=d$

$p\sin(\alpha+t)-2\cos(\alpha+t)+\cos\alpha+\cos\beta=0$

$\alpha+t-q=\beta[mod 2\pi]$

求解

$t_{lsr}=(-\alpha+\arctan(\frac{-\cos\alpha-\cos\beta}{d+\sin\alpha+\sin\beta})-\arctan(\frac{-2}{p_{lsr}}))[mod 2\pi]$

$p_{lsr}=\sqrt{-2+d^2+2\cos(\alpha-\beta)+2d(\sin\alpha+\sin\beta)}$

$q_{lsr}=-\beta[mod 2\pi]+\arctan(\frac{-\cos\alpha-\cos\beta}{d+\sin\alpha+\sin\beta})-\arctan(\frac{-2}{p_{lsr}})[mod 2\pi]$

$L_{lsr}=t_{lsr}+p_{lsr}+q_{lsr}=\alpha-\beta+2t_{lsr}+p_{lsr}$

6.RSL

$p\cos(\alpha-t)-2\sin(\alpha-t)+\sin\alpha+\sin\beta=d$

$p\sin(\alpha-t)+2\cos(\alpha-t)-\cos\alpha-\cos\beta=0$

$\alpha-t+q=\beta[mod 2\pi]$

求解

$t_{rsl}=\alpha-\arctan(\frac{\cos\alpha+\cos\beta}{d-\sin\alpha-\sin\beta})+\arctan(\frac{2}{p_{rsl}})[mod 2\pi]$

$p_{rsl}=\sqrt{d^2-2+2\cos(\alpha-\beta)-2d(\sin\alpha+\sin\beta)}$

$q_{rsl}=\beta[mod 2\pi]-\arctan(\frac{\cos\alpha+\cos\beta}{d-\sin\alpha-\sin\beta})+\arctan(\frac{2}{p_{rsl}})[mod2\pi]$

$L_{rsl}=t_{rsl}+p_{rsl}+q_{rsl}=-\alpha+\beta+2t_{rsl}+p_{rsl}$

7.RLR

$2\sin(\alpha-t+q)-2\sin(\alpha-t)=d-\sin\alpha+\sin\beta$

$-2\cos(\alpha-t+p)+2\cos(\alpha-t)=\cos\alpha-\cos\beta$

$\alpha-t+p-q=\beta[mod2\pi]$

求解

$t_{rlr}=\alpha-\arctan(\frac{\cos\alpha-\cos\beta}{d-\sin\alpha+\sin\beta})+\frac{p_{rlr}}{2}[mod2\pi]$

$p_{rlr}=\arccos\frac{1}{8}(6-d^2+2\cos(\alpha-\beta)+2d(\sin\alpha-\sin\beta))$

$q_{rlr}=\alpha-\beta-t_{rlr}+p_{rlr}[mod2\pi]$

$L_{rlr}=t_{rlr}+p_{rlr}+q_{rlr}=\alpha-\beta+2p_{rlr}$

8.计算每一段终点

左转

以 $s(x_0,y_0,\angle1)$ 为起点，左转 $\angle t$ 的终点计算

$\angle t+\angle1=\pi-\angle BCD$

$x=x_0+\left | OD \right |-\left | OA \right |=x_0+\sin(\angle t+\angle1)-\sin\angle1$

$y=y_0+\left | CD \right |+\left | As \right |=y_0-\cos(\angle t+\angle1)+\cos\angle1$

$\theta=\angle1+\angle t$

右转

以 $s(x_0,y_0,\angle1)$ 为起点，右转 $\angle t$ 的终点计算

$\angle1-\angle t=\angle OCB-\pi=\angle OCB+\pi$

$x=x_0+\left | AO \right |+\left | OB \right |=x_0+\sin\angle1-\sin(\angle1-\angle t)$

$y=y_0-\left | As \right |-\left | BC \right |=y_0+\cos(\angle1-\angle t)+\cos\angle1$

$\theta=\angle1-\angle t$

直行

以 $s(x_0,y_0,\angle1)$ 为起点，直行p的终点计算

$x=\left | As \right |=x_0+p\cos\angle1$

$y=\left | AC \right |=y_0+p\sin\angle1$

$\theta=\angle1$

每一段间的点，通过和计算终点的方式去计算，例如左转圆弧上的点，则可以左转 $\gamma$ 角去获得下一个，点的疏密可以通过 $\gamma$ 的大小去控制，越小则越密，起到每一步的 $\gamma$ 角的总和大于等于 $\angle t$ ，则这一段圆弧完成了点的生成。

至此，Dubins 曲线计算完毕。

四、C++与ROS实现

#include "r_s_planner/dubins.h"
 
//设置起点、终点坐标和航向角
double start_state[3] = {0, 0, M_PI/4};
double final_state[3] = {3, 4, 0};
 
//生成Dubins曲线
double steer_radius = 5.8;
DubinsStateSpace db_planner(steer_radius);
std::vector<std::vector<double> > db_path;
db_planner.sample(start_state, final_state, step_size, length, db_path);
 
//可以使用nav_msgs::Path等ROS消息在RVIZ可视化
//...

源代码在($ car_geometric_planner)/src/dubins.cpp，作者已经较好封装，可以在ROS中使用，详见参考文献3。

五、参考文献

1.自动驾驶运动规划-Dubins曲线 - 半杯茶的小酒杯的文章 - 知乎 https://zhuanlan.zhihu.com/p/120272035

2.Dubins曲线详细笔记 - 烟酒僧的文章 - 知乎 https://zhuanlan.zhihu.com/p/414753861

3.
https://github.com/zwkcoding/car_geometric_planner

ROS 路径规划 C++学习笔记 Dubins

打赏 0

下一篇：Dijkstra算法原理与实现

Dubins曲线原理与实现

wangpenghnu

前言

一、Dubins

二、车辆简单模型

三、Dubins曲线计算

1.坐标变换

2.LSL

3.LRL

4.RSR

5.LSR

6.RSL

7.RLR

8.计算每一段终点

左转

右转

直行

四、C++与ROS实现

五、参考文献

为你推荐

精选《类车机器人的动力学轨迹优化与控制》论文解读一

自动驾驶路径规划——轨迹规划（详解插值法）

AprilTags二维码的检测与应用

Apollo planning之RuleBasedStopDecider

史上最简SLAM零基础解读(2) - 对极约束→Essential矩阵、Fundamental矩阵推导

图像处理基础篇-形态学处理-边缘检测(matlab仿真与图像处理系列第4期）

评论（0）

关于作者

wangpenghnu

10

0

1

2

二叉树的遍历

ROS进阶级教程

HD-Map lanelet2(2)

相关推荐

【自动驾驶产业流程调研】（更新中）从招聘需求，观察自动驾驶领域行业概貌，及自动驾驶公司运作流程。

经典文献阅读之--Bidirectional Camera-LiDAR Fusion（Camera-LiDAR双向融合新范式）

车辆运动学建模(二)

语义分割算法——应用空洞卷积和CRF的DeepLabv1

精选thrust工程化学习(三)----点云裁剪部分操作

精选【自动驾驶】路径规划算法Dijkstra与A*

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板