Dubins曲线（3D）

Dubins曲线通常只能用于二维空间（2D空间的Dubins曲线实现点这里），对于三维空间可以通过增加 z 轴的插值来近似 3D Dubins 曲线。

主要步骤说明：
generate_dubins_3d 函数：
输入起点和终点的 (x, y, z, theta, phi)。其中，theta表示航向角 θ、phi表示俯仰角
调用 2D Dubins 路径生成函数 generate_dubins_2d，在平面上生成 (x, y) 的路径。
对 z 轴采用线性插值，从起点 z 到终点 z，生成 3D 路径。
generate_dubins_2d 函数：

这里为简化说明，示例中只用了简单的线性插值，实际中你可以实现或者调用 Dubins 曲线生成器来生成精确的 2D 路径。下面两篇文章就提到采用Dubins曲线在Z轴上的连续Bezier曲线内插来实现
[1] Cai W, Zhang M, Zheng Y R. Task assignment and path planning for multiple autonomous underwater vehicles using 3D dubins curves[J]. Sensors, 2017, 17(7): 1607.
[2] Cai W, Zhang M. Smooth 3D dubins curves based mobile data gathering in sparse underwater sensor networks[J]. Sensors, 2018, 18(7): 2105.

function dubins3D_path = generate_dubins_3d(start_pos, goal_pos, turning_radius)
    % start_pos 和 goal_pos 分别是起点和终点，包含 (x, y, z, theta, phi)
    % turning_radius 是最小转弯半径

    
    % 提取起点和终点的 (x, y, z, theta, phi)
    start_xy = [start_pos(1), start_pos(2), start_pos(4)]; % (x, y, theta)
    goal_xy = [goal_pos(1), goal_pos(2), goal_pos(4)]; % (x, y, theta)
    
    % 计算 2D Dubins 曲线
    path2D = generate_dubins_2d(start_xy, goal_xy, turning_radius);
    
    % 使用线性插值 z 坐标，确保曲线在 z 轴方向上平滑变化
    start_z = start_pos(3);
    goal_z = goal_pos(3);
    
    num_points = size(path2D, 1);
    z_interp = linspace(start_z, goal_z, num_points);
    
    % 构造 3D 路径
    dubins3D_path = [path2D(:,1), path2D(:,2), z_interp'];
end

```matlab
function path = generate_dubins_2d(p1, p2, r)


% 计算起点和终点之间的坐标差值
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段路径的终点（x,y,theta)
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;  % 设置坐标轴比例相等
end