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