0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 什么是Dubins曲线？

Dubins曲线是指由美国数学家 Lester Dubins 在20世纪50年代提出的一种特殊类型的最短路径曲线。这种曲线通常用于描述在给定转弯半径下的无人机、汽车或船只等载具的最短路径，其特点是起始点和终点处的切线方向和曲率都是已知的。

Dubins曲线包括直线段和最大转弯半径下的圆弧组成，通过合适的组合可以实现从一个姿态到另一个姿态的最短路径规划。这种曲线在航空、航海、自动驾驶等领域具有广泛的应用，能够有效地规划航行路径，减少能量消耗并提高效率。

2 Dubins曲线原理

2.1 坐标变换

如图所示，在全局坐标系$xOy$中，设机器人起始位姿、终止位姿、最小转弯半径分别为$\left(x_s,y_s,\alpha \right)$、$\left(x_g,y_g,\beta \right)$与$R$，则以${\mathbf{p}}_s=\left(x_s,y_s\right)$为新坐标系原点，${\mathbf{p}}_s$指向${\mathbf{p}}_g=\left(x_g,y_g\right)$方向为$x^{\prime }$轴，垂直方向为$y^{\prime }$轴建立新坐标系$x^{\prime }{O}^{\prime }{y}^{\prime }$。

根据比例关系$d/D=r/R$，其中$D={\Vert {\mathbf{p}}_s-{\mathbf{p}}_g\Vert }_2$。为了便于后续推导，不妨归一化最小转弯半径，即令$r=1$。所以在坐标系$x^{\prime }{O}^{\prime }{y}^{\prime }$中，通常取起点、终点间距为$d=D/R$，从而起始位姿、终止位姿、最小转弯半径分别转换为

$$\mathrm{start}={\left[\begin{array}{ccc}0& 0& \alpha -\theta \end{array}\right]}^T,\mathrm{goal}={\left[\begin{array}{ccc}d& 0& \beta -\theta \end{array}\right]}^T,r=1$$

其中$\theta =\mathrm{arc}\tan \left(\left(y_g-y_s\right)/\left(x_g-x_s\right)\right)$，接下来的推导均基于转换坐标系$x^{\prime }{O}^{\prime }{y}^{\prime }$。

2.2 单步运动公式

对于直行运动，设沿直线行进距离为$l$，则

$${\left[\begin{array}{ccc}{x}^{\ast }& y^{\ast }& {\varphi }^{\ast }\end{array}\right]}^T={\left[\begin{array}{ccc}x+l\cos \varphi & y+l\sin \varphi & \varphi \end{array}\right]}^T$$

对于转弯运动，假设转向角为$\psi$，则由弧长公式可得

$$l=\psi r\stackrel{r=1}{=}\psi$$

因此设沿圆弧行进距离为$l$，以左转为例，由几何关系易得

$${\left[\begin{array}{ccc}{x}^{\ast }& y^{\ast }& {\varphi }^{\ast }\end{array}\right]}^T={\left[\begin{array}{ccc}x+r\sin \left(\varphi +\psi \right)-r\sin \left(\varphi \right)& y+r\cos \left(\varphi +\psi \right)+r\cos \left(\varphi \right)& \varphi +\psi \end{array}\right]}^T$$

代入$r=1$、$\psi =l$可得

$${\left[\begin{array}{ccc}{x}^{\ast }& y^{\ast }& {\varphi }^{\ast }\end{array}\right]}^T={\left[\begin{array}{ccc}x+\sin \left(\varphi +l\right)-\sin \left(\varphi \right)& y+\cos \left(\varphi +l\right)+\cos \left(\varphi \right)& \varphi +l\end{array}\right]}^T$$

同理，对于右转而言，有

$${\left[\begin{array}{ccc}{x}^{\ast }& y^{\ast }& {\varphi }^{\ast }\end{array}\right]}^T={\left[\begin{array}{ccc}x-\sin \left(\varphi -l\right)+\sin \left(\varphi \right)& y+\cos \left(\varphi +l\right)-\cos \left(\varphi \right)& \varphi -l\end{array}\right]}^T$$

综上所述，可得单步运动映射

$$\{\begin{array}{l}{L}_l^{+}\left(x,y,\varphi \right)={\left[\begin{array}{ccc}x+\sin \left(\varphi +l\right)-\sin \left(\varphi \right)& y-\cos \left(\varphi +l\right)+\cos \left(\varphi \right)& \varphi +l\end{array}\right]}^T\\ R_l^{+}\left(x,y,\varphi \right)={\left[\begin{array}{ccc}x-\sin \left(\varphi -l\right)+\sin \left(\varphi \right)& y+\cos \left(\varphi -l\right)-\cos \left(\varphi \right)& \varphi -l\end{array}\right]}^T\\ S_l^{+}\left(x,y,\varphi \right)={\left[\begin{array}{ccc}x+l\cos \varphi & y+l\sin \varphi & \varphi \end{array}\right]}^T\end{array}$$

2.3 曲线模式

Dubins曲线假设物体只能向前，通过组合左转、右转、直行可得六种运动模式

{ L S L , R S R , R S L , L S R , R L R , L R L } \left\{ LSL, RSR, RSL, LSR, RLR, LRL \right\} {LSL,RSR,RSL,LSR,RLR,LRL}

可以总结这六种运动模式的解析解为

3 Dubins曲线生成算法

Dubins曲线路径生成算法流程如表所示

4 仿真实现

4.1 ROS C++实现

核心代码如下所示

Points2d Dubins::generation(Pose2d start, Pose2d goal)
{Points2d path;double sx, sy, syaw;double gx, gy, gyaw;std::tie(sx, sy, syaw) = start;std::tie(gx, gy, gyaw) = goal;// coordinate transformationgx -= sx;gy -= sy;double theta = helper::mod2pi(atan2(gy, gx));double dist = hypot(gx, gy) * max_curv_;double alpha = helper::mod2pi(syaw - theta);double beta = helper::mod2pi(gyaw - theta);// select the best motionDubinsMode best_mode;double best_cost = DUBINS_MAX;DubinsLength length;DubinsLength best_length = { DUBINS_NONE, DUBINS_NONE, DUBINS_NONE };DubinsMode mode;for (const auto solver : dubins_solvers){(this->*solver)(alpha, beta, dist, length, mode);_update(length, mode, best_length, best_mode, best_cost);}if (best_cost == DUBINS_MAX)return path;// interpolation...// coordinate transformationEigen::AngleAxisd r_vec(theta, Eigen::Vector3d(0, 0, 1));Eigen::Matrix3d R = r_vec.toRotationMatrix();Eigen::MatrixXd P = Eigen::MatrixXd::Ones(3, path_x.size());for (size_t i = 0; i < path_x.size(); i++){P(0, i) = path_x[i];P(1, i) = path_y[i];}P = R * P;for (size_t i = 0; i < path_x.size(); i++)path.push_back({ P(0, i) + sx, P(1, i) + sy });return path;
}

4.2 Python实现

核心代码如下所示

def generation(self, start_pose: tuple, goal_pose: tuple):sx, sy, syaw = start_posegx, gy, gyaw = goal_pose# coordinate transformationgx, gy = gx - sx, gy - sytheta = self.mod2pi(math.atan2(gy, gx))dist = math.hypot(gx, gy) * self.max_curvalpha = self.mod2pi(syaw - theta)beta = self.mod2pi(gyaw - theta)# select the best motionplanners = [self.LSL, self.RSR, self.LSR, self.RSL, self.RLR, self.LRL]best_t, best_p, best_q, best_mode, best_cost = None, None, None, None, float("inf")for planner in planners:t, p, q, mode = planner(alpha, beta, dist)if t is None:continuecost = (abs(t) + abs(p) + abs(q))if best_cost > cost:best_t, best_p, best_q, best_mode, best_cost = t, p, q, mode, cost# interpolation...# coordinate transformationrot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2]converted_xy = rot @ np.stack([x_list, y_list])x_list = converted_xy[0, :] + sxy_list = converted_xy[1, :] + syyaw_list = [self.pi2pi(i_yaw + theta) for i_yaw in yaw_list]return best_cost, best_mode, x_list, y_list, yaw_list

4.3 Matlab实现

核心代码如下所示

function [x_list, y_list, yaw_list] = generation(start_pose, goal_pose, param)sx = start_pose(1); sy = start_pose(2); syaw = start_pose(3);gx = goal_pose(1); gy = goal_pose(2); gyaw = goal_pose(3);% coordinate transformationgx = gx - sx; gy = gy - sy;theta = mod2pi(atan2(gy, gx));dist = hypot(gx, gy) * param.max_curv;alpha = mod2pi(syaw - theta);beta = mod2pi(gyaw - theta);% select the best motionplanners = ["LSL", "RSR", "LSR", "RSL", "RLR", "LRL"];best_cost = inf;best_segs = [];best_mode = [];for i=1:length(planners)planner = str2func(planners(i));[segs, mode] = planner(alpha, beta, dist);if isempty(segs)continueendcost = (abs(segs(1)) + abs(segs(2)) + abs(segs(3)));if best_cost > costbest_segs = segs;best_mode = mode;best_cost = cost;endend% interpolation...% coordinate transformationrot = [cos(theta), -sin(theta); sin(theta), cos(theta)];converted_xy = rot * [x_list; y_list];x_list = converted_xy(1, :) + sx;y_list = converted_xy(2, :) + sy;for j=1:length(yaw_list)yaw_list(j) = pi2pi(yaw_list(j) + theta);end
end

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇