Dubins曲线（2D）

定义

dubins曲线可以表示成3个运动基本动作的组合（即左转L、右转R、直行S）。

符号	含义	绕单位圆
L	左转	逆时针
R	右转	顺时针
S	直走	直行

dubins曲线给出了充分的路经集合，该集合包含的曲线叫做最佳路径。由Dubins证得最短路径只从6条曲线中选取：{LRL LSL LSR RLR RSR RSL}。
进一步可简化为：

符号	曲线
CCC	{LRL RLR}
CSC	{LSL RSR LSR RSL}

其中C表示单位圆弧，S表示直线段。

过程推导

Dubins曲线详细笔记

1）坐标变换

设起点为 $s\left(x_i,y_i,{\alpha }_i\right)$ ， 终点为 $g\left(x_g,y_g,{\beta }_g\right)$ ，先坐标变换+将起点平移支原点，并放置 $\theta$角，则终点也落在 $x$ 轴上，起点和终点的坐标为 $s(0,0,\alpha ),g(d,0,\beta )$ ，其中:

$$\begin{array}{rl}& \theta =\mathrm{atan}2\left(\frac{y_g-y_i}{x_g-x_i}\right)\\ & D=\mathrm{sqrt}\left({\left(x_i-x_g\right)}^2+{\left(y_i-y_g\right)}^2\right)\\ & d=D/R\\ & \alpha =\mathrm{mod}\left({\alpha }_i-\theta ,2\pi \right)\\ & \beta =\mathrm{mod}\left({\beta }_g-\theta ,2\pi \right)\end{array}$$

其中 $\theta$ 为起点和终点航向角度差，上面的角度都在 $[0,2\pi ]$ 之间。上面将 $D$ 除上 $R$ 是为了正则化 $R$ ，这样每个最小转弯半径都为 1 ，由角度计算弧长时更方便，弧长即等于角度的弧度，因此在后面看到的 $\cos (\alpha )$ ，其实是前面省略了 $\mathbf{R}$ ，也就是 $1$ 。

2）Dubins 路径长度计算

整个路径长度$L$f分为三段：
$L=t+p+q$
(1) LSL

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

p cos ⁡ ( α + t ) − sin ⁡ α + sin ⁡ β = d p sin ⁡ ( α + t ) + cos ⁡ α − cos ⁡ β = 0 α + t + q = β {   m o d   2 π } \begin{aligned} & p \cos (\alpha+t)-\sin \alpha+\sin \beta=d \\ & p \sin (\alpha+t)+\cos \alpha-\cos \beta=0 \\ & \alpha+t+q=\beta\{\bmod 2 \pi\} \end{aligned} ​pcos(α+t)−sinα+sinβ=dpsin(α+t)+cosα−cosβ=0α+t+q=β{mod2π}​

上面公式的过程：

$$\begin{array}{rl}& \angle \alpha =\angle 1+\pi \\ & \angle 1=\angle 2+\angle 3\\ & \angle t+\angle 2=\pi \\ & -->\angle \alpha +\angle t=2\pi +\angle 3\\ & --<\cos (\angle \alpha +\angle t)=\cos (\angle 3)\\ & -->\sin (\angle \alpha +\angle t)=-\sin (\angle 3)\\ & -->\sin (\angle \alpha )=-\sin (\angle 1);\cos (\angle \alpha )=\cos (\angle 1)\end{array}$$
在 $\Delta s{O}_{1}B$ 中， $|sB|=\left|s{O}_1\right|\sin (\angle 1)=-\sin (\alpha )$ ，
在 $\Delta g{O}_{2}C$ 中， $|gC|=\left|g{O}_2\right|\sin (\beta )，$
在 $\Delta O_1{O}_{2}A$ 中， $\left|A{O}_2\right|=p\cos (\angle 3)=\cos (\angle \alpha +\angle t)$ ，
由于 $\mathrm{R}=1$,则 $\angle t=t,q=\beta -\angle 3$ ，
另外，车辆从 s 走过 $s+t+p$ 角度，到达 g 点时的角度为 $\beta$ ，即 $\alpha +t+q=\beta \mathrm{mod}2\pi$.
解上面的方程组，可解得:

t l s l = − α + arctan ⁡ cos ⁡ β − cos ⁡ α d + sin ⁡ α − sin ⁡ β {   m o d   2 π } p l s l = 2 + d 2 − 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α − sin ⁡ β ) q l s l = β − arctan ⁡ cos ⁡ β − cos ⁡ α d + sin ⁡ α − sin ⁡ β {   m o d   2 π } \begin{aligned} & t_{l s l}=-\alpha+\arctan \frac{\cos \beta-\cos \alpha}{d+\sin \alpha-\sin \beta}\{\bmod 2 \pi\} \\ & p_{l s l}=\sqrt{2+d^2-2 \cos (\alpha-\beta)+2 d(\sin \alpha-\sin \beta)} \\ & q_{l s l}=\beta-\arctan \frac{\cos \beta-\cos \alpha}{d+\sin \alpha-\sin \beta}\{\bmod 2 \pi\} \end{aligned} ​tlsl​=−α+arctand+sinα−sinβcosβ−cosα​{mod2π}plsl​=2+d2−2cos(α−β)+2d(sinα−sinβ) ​qlsl​=β−arctand+sinα−sinβcosβ−cosα​{mod2π}​

则总长度等于:

$${\mathcal{L}}_{lsl}=t_{lsl}+p_{lsl}+q_{lsl}=-\alpha +\beta +p_{lsl}$$

对于其中一种特殊情况:

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

跟前面 LSL 的方法一样，在x和y向构建方程组(未加 $\angle$ 符号，例如 $\alpha$ 即 $\angle \alpha$ ):

− 2 sin ⁡ ( α + t − p ) + 2 sin ⁡ ( α + t ) = d + sin ⁡ α − sin ⁡ β 2 cos ⁡ ( α + t − p ) − 2 cos ⁡ ( α + t ) = − cos ⁡ α + cos ⁡ β α + t − p + q = β {   m o d   2 π } \begin{aligned} & -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\{\bmod 2 \pi\} \end{aligned} ​−2sin(α+t−p)+2sin(α+t)=d+sinα−sinβ2cos(α+t−p)−2cos(α+t)=−cosα+cosβα+t−p+q=β{mod2π}​

首先来看角度关系：

$$\begin{array}{rl}& \angle \alpha =\pi +\angle 1\\ & \angle t=\pi +\angle 2\\ & \angle p=\angle H{O}_{1}K+\angle 3=\angle 1+\angle 2+\angle 3\\ & \angle \alpha +\angle t=2\pi +\angle 1+\angle 2=\angle 1+\angle 2\\ & \angle \alpha +\angle t-\angle p=-\angle 3\\ & \angle q-\angle 3=\angle \beta \end{array}$$

正余弦与长度关系：

$$\begin{array}{rl}& \sin (\angle \alpha )=-\sin (\angle 1)=-|sB|\\ & \cos (\angle \alpha )=-\cos (\angle 1)=-\left|B{O}_1\right|\\ & \sin (\angle \beta )=|Cg|\\ & \cos (\angle \beta )=\left|C{O}_2\right|\\ & \sin (\angle \alpha +\angle t)=\sin (\angle 1+\angle 2)=|HK|\\ & \cos (\angle \alpha +\angle t)=\cos (\angle 1+\angle 2)=\left|O_{1}H\right|\\ & \sin (\angle \alpha +\angle t-\angle p)=\sin (-\angle 3)=-|FG|\\ & \cos (\angle \alpha +\angle t-\angle p)=\cos (-\angle 3)=\left|F{O}_3\right|\end{array}$$
$x$ 方向:
对于 $|sB|$ 和 $|Cg|$ ，与LSL的情况一样，分别等于 $-\sin (\alpha )$ 和 $\sin (\beta )$ ，因此第一个等式的右边即对应 $|BC|$ 。

$$\begin{array}{rl}& |BE|=2|BD|=2\sin (\angle 1+\angle 2)=2\sin (\angle \alpha +\angle t)\\ & |EC|=2|FG|=2\sin (\angle 3)=-2\sin (-\angle 3)=-2\sin (\angle \alpha +\angle t-\angle p)\end{array}$$

y 方向:

$$\left|C{O}_2\right|+|BH|+2\left|H{O}_1\right|=2\left|F{O}_3\right|$$

根据正余弦值与长度的关系：

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

解方程组 ${}^{+}$得：

t l r l = ( − α + arctan ⁡ ( − cos ⁡ α + cos ⁡ α d + sin ⁡ α − sin ⁡ β ) + p l r l 2 ) {   m o d   2 π } p l r l = arccos ⁡ 1 8 ( 6 − d 2 + 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α − sin ⁡ β ) ) {   m o d   2 π } q l r l = β (   m o d   2 π ) − α + 2 p l r l {   m o d   2 π } \begin{aligned} t_{l r l} & =\left(-\alpha+\arctan \left(\frac{-\cos \alpha+\cos \alpha}{d+\sin \alpha-\sin \beta}\right)+\frac{p_{l r l}}{2}\right)\{\bmod 2 \pi\} \\ p_{l r l} & =\arccos \frac{1}{8}\left(6-d^2+2 \cos (\alpha-\beta)+2 d(\sin \alpha-\sin \beta)\right)\{\bmod 2 \pi\} \\ q_{l r l} & =\beta(\bmod 2 \pi)-\alpha+2 p_{l r l}\{\bmod 2 \pi\} \end{aligned} tlrl​plrl​qlrl​​=(−α+arctan(d+sinα−sinβ−cosα+cosα​)+2plrl​​){mod2π}=arccos81​(6−d2+2cos(α−β)+2d(sinα−sinβ)){mod2π}=β(mod2π)−α+2plrl​{mod2π}​

总长度为:

$${\mathcal{L}}_{lrl}=t_{lrl}+p_{lrl}+q_{lrl}=-\alpha +\beta +2p_{lrl}$$
上面只是介绍了CSC和CCC中的一种情况
其他情况的解如下图所示

3）位置坐标计算

Matlab 程序

引自
main

clear all;  % 清除工作空间中的所有变量
close all;  % 关闭所有图形窗口
clc;        % 清空命令行窗口

r = 5;  % 设定曲率半径

% % 以下几行分别代表不同的Dubins路径类型，取消注释来选择路径类型

% % LSL: 左-直-左的路径
p1 = [10 10 0*pi/180];  % 起点坐标及初始朝向角（弧度制）
p2 = [15 15 0*pi/180];  % 终点坐标及终点朝向角（弧度制）

% LSR: 左-直-右的路径
% p1 = [10 10 0*pi/180];
% p2 = [25 25 0*pi/180];

% RSL: 右-直-左的路径
% p1 = [10 10 0*pi/180];  % 设置起点
% p2 = [25 -25 0*pi/180];  % 设置终点

% % RSR: 右-直-右的路径
% p1 = [0 0 90*pi/180];
% p2 = [15 15 0*pi/180];

% % RLR: 右-左-右的路径
% p1 = [10 10 0*pi/180];
% p2 = [15 15 180*pi/180];

% % LRL: 左-右-左的路径
% p1 = [10 10 180*pi/180];
% p2 = [15 15 0*pi/180];

% 计算起点和终点之间的坐标差值
dx = p2(1) - p1(1);  % x轴方向差
dy = p2(2) - p1(2);  % y轴方向差

% 计算两点之间的距离，并标准化为以半径r为单位的距离
d = sqrt(dx^2 + dy^2) / r;

% 计算起点与终点连线相对于x轴的角度
theta = mod(atan2(dy, dx), 2*pi);

% 计算alpha和beta，分别为起点和终点的方向角相对于theta的角度
alpha = mod((p1(3) - theta), 2*pi);
beta  = mod((p2(3) - theta), 2*pi);

% 初始化存储路径长度的矩阵L，每行对应一种路径类型
L = zeros(6, 4);  
L(1,:) = LSL(alpha, beta, d);  % 计算LSL路径的长度
L(2,:) = LSR(alpha, beta, d);  % 计算LSR路径的长度
L(3,:) = RSL(alpha, beta, d);  % 计算RSL路径的长度
L(4,:) = RSR(alpha, beta, d);  % 计算RSR路径的长度
L(5,:) = RLR(alpha, beta, d);  % 计算RLR路径的长度
L(6,:) = LRL(alpha, beta, d);  % 计算LRL路径的长度

% 选择最短的路径类型
[~, ind] = min(L(:,1));  % 找出最短路径的索引

% 定义路径类型的字符串
types = ['LSL'; 'LSR'; 'RSL'; 'RSR'; 'RLR'; 'LRL'];

% 定义起始点，包含坐标和方向角
p_start = [0 0 p1(3)];

% 计算Dubins路径的每一段
mid1 = dubins_segment(L(ind, 2), p_start, types(ind, 1));  % 第1段路径
mid2 = dubins_segment(L(ind, 3), mid1, types(ind, 2));     % 第2段路径

% 初始化路径数组
path = [];

% 逐步计算路径上的点
for step = 0:0.05:L(ind,1)*r
    t = step / r;  % 当前步长归一化
    
    % 根据当前步长判断处于哪一段路径并计算相应的点
    if (t < L(ind, 2))
        end_pt = dubins_segment(t, p_start, types(ind, 1));  % 第1段路径
    elseif (t < L(ind, 2) + L(ind, 3))
        end_pt = dubins_segment(t - L(ind, 2), mid1, types(ind, 2));  % 第2段路径
    else
        end_pt = dubins_segment(t - L(ind, 2) - L(ind, 3), mid2, types(ind, 3));  % 第3段路径
    end
    
    % 将路径点坐标还原回实际坐标，并计算方向角
    end_pt(1) = end_pt(1) * r + p1(1);  % x坐标转换
    end_pt(2) = end_pt(2) * r + p1(2);  % y坐标转换
    end_pt(3) = mod(end_pt(3), 2*pi);   % 角度转换
    
    % 将当前点添加到路径中
    path = [path; end_pt];
end

% 画出起点和终点
plot(p1(1), p1(2), 'ro');  % 画出起点
hold on;
quiver(p1(1), p1(2), 2*cos(p1(3)), 2*sin(p1(3)));  % 画出起点的方向

plot(p2(1), p2(2), 'r*');  % 画出终点
quiver(p2(1), p2(2), 2*cos(p2(3)), 2*sin(p2(3)));  % 画出终点的方向

% 画出路径
plot(path(:,1), path(:,2), 'b');  % 画出Dubins路径
axis equal;  % 设置坐标轴比例相等

dubins_segment

function seg_end = dubins_segment(seg_param, seg_init, seg_type)
    % 该函数计算Dubins路径中某一段的终点位置
    % 输入:
    %   seg_param: 当前段的参数（长度或角度）
    %   seg_init:  当前段的起始点坐标及角度 [x, y, theta]
    %   seg_type:  当前段的类型 ('L'表示左转, 'R'表示右转, 'S'表示直线)
    % 输出:
    %   seg_end:  该段的终点坐标及角度 [x, y, theta]

    % 如果当前段是左转 ('L')，使用相应的公式计算终点
    if( seg_type == 'L' ) 
        seg_end(1) = seg_init(1) + sin(seg_init(3) + seg_param) - sin(seg_init(3));  % 计算x坐标
        seg_end(2) = seg_init(2) - cos(seg_init(3) + seg_param) + cos(seg_init(3));  % 计算y坐标
        seg_end(3) = seg_init(3) + seg_param;  % 更新角度（朝向）
        
    % 如果当前段是右转 ('R')，使用相应的公式计算终点
    elseif( seg_type == 'R' )
        seg_end(1) = seg_init(1) - sin(seg_init(3) - seg_param) + sin(seg_init(3));  % 计算x坐标
        seg_end(2) = seg_init(2) + cos(seg_init(3) - seg_param) - cos(seg_init(3));  % 计算y坐标
        seg_end(3) = seg_init(3) - seg_param;  % 更新角度（朝向）

    % 如果当前段是直线 ('S')，使用相应的公式计算终点
    elseif( seg_type == 'S' ) 
        seg_end(1) = seg_init(1) + cos(seg_init(3)) * seg_param;  % 计算x坐标
        seg_end(2) = seg_init(2) + sin(seg_init(3)) * seg_param;  % 计算y坐标
        seg_end(3) = seg_init(3);  % 角度保持不变
    end
end

LSL

function L = LSL(alpha,beta,d)
tmp0 = d + sin(alpha) - sin(beta);
p_squared = 2 + (d*d) -(2*cos(alpha - beta)) + (2*d*(sin(alpha) - sin(beta))); % plsl的平方
if( p_squared < 0 )
    L = [inf inf inf inf];
else
    tmp1 = atan2( (cos(beta)-cos(alpha)), tmp0 );
    t = mod((-alpha + tmp1 ), 2*pi);
    p = sqrt( p_squared );
    q = mod((beta - tmp1 ), 2*pi);
    L=[t+p+q t p q];
end

end

RSL

function L = RSL(alpha,beta,d)
p_squared = (d*d) -2 + (2*cos(alpha - beta)) - (2*d*(sin(alpha)+sin(beta)));
if( p_squared< 0 )
    L = [inf inf inf inf];
else
    p    = sqrt( p_squared );
    tmp2 = atan2( (cos(alpha)+cos(beta)), (d-sin(alpha)-sin(beta)) ) - atan2(2.0, p);
    t    = mod((alpha - tmp2), 2*pi);
    q    = mod((beta - tmp2), 2*pi);
    L=[t+p+q t p q];
end
end

LSR

function L = LSR(alpha,beta,d)
p_squared = -2 + (d*d) + (2*cos(alpha - beta)) + (2*d*(sin(alpha)+sin(beta)));
if( p_squared < 0 )
    L = [inf inf inf inf];
else
    p    = sqrt( p_squared );
    tmp2 = atan2( (-cos(alpha)-cos(beta)), (d+sin(alpha)+sin(beta)) ) - atan2(-2.0, p);
    t    = mod((-alpha + tmp2), 2*pi);
    q    = mod(( -mod((beta), 2*pi) + tmp2 ), 2*pi);
    L=[t+p+q t p q];
end
end

RSR

function L = RSR(alpha,beta,d)
tmp0 = d-sin(alpha)+sin(beta);
p_squared = 2 + (d*d) -(2*cos(alpha - beta)) + (2*d*(sin(beta)-sin(alpha)));
if( p_squared < 0 )
    L = [inf inf inf inf];
else
    tmp1 = atan2( (cos(alpha)-cos(beta)), tmp0 );
    t = mod(( alpha - tmp1 ), 2*pi);
    p = sqrt( p_squared );
    q = mod(( -beta + tmp1 ), 2*pi);
    L=[t+p+q t p q];
end
end

RLR

function L = RLR(alpha,beta,d)
tmp_rlr = (6. - d*d + 2*cos(alpha - beta) + 2*d*(sin(alpha)-sin(beta))) / 8.;
if( abs(tmp_rlr) > 1)
    L = [inf inf inf inf];
else
    p = mod(( 2*pi - acos( tmp_rlr ) ), 2*pi);
    t = mod((alpha - atan2( cos(alpha)-cos(beta), d-sin(alpha)+sin(beta) ) + mod(p/2, 2*pi)), 2*pi);
    q = mod((alpha - beta - t + mod(p, 2*pi)), 2*pi);
    L=[t+p+q t p q];
end
end

LRL

function L = LRL(alpha,beta,d)
tmp_lrl = (6. - d*d + 2*cos(alpha - beta) + 2*d*(- sin(alpha) + sin(beta))) / 8.;
if( abs(tmp_lrl) > 1)
    L = [inf inf inf inf];
else
    p = mod(( 2*pi - acos( tmp_lrl ) ), 2*pi);
    t = mod((-alpha - atan2( cos(alpha)-cos(beta), d+sin(alpha)-sin(beta) ) + p/2), 2*pi);
    q = mod((mod(beta, 2*pi) - alpha -t + mod(p, 2*pi)), 2*pi);
    L=[t+p+q t p q];
end
end

参考内容
[1] Dubins曲线详细笔记
[2]Dubins曲线学习笔记
[3 matlab练习程序（dubins曲线）