??????? 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。