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 transformation
  gx -= 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 motion
  DubinsMode 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 transformation
  Eigen::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_pose
	gx, gy, gyaw = goal_pose
	
	# coordinate transformation
	gx, gy = gx - sx, gy - sy
	theta = self.mod2pi(math.atan2(gy, gx))
	dist = math.hypot(gx, gy) * self.max_curv
	alpha = self.mod2pi(syaw - theta)
	beta = self.mod2pi(gyaw - theta)
	
	# select the best motion
	planners = [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:
			continue
		cost = (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 transformation
	rot = 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, :] + sx
	y_list = converted_xy[1, :] + sy
	yaw_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 transformation
    gx = 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 motion
    planners = ["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)
            continue
        end
        cost = (abs(segs(1)) + abs(segs(2)) + abs(segs(3)));
        if best_cost > cost
            best_segs = segs;
            best_mode = mode;
            best_cost = cost;
        end
    end
            
    % interpolation
    ...

    % coordinate transformation
    rot = [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深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

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