曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真)

这篇具有很好参考价值的文章主要介绍了曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

0 专栏介绍

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

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


1 什么是Reeds-Shepp曲线?

Reeds-Shepp曲线是一种用于描述在平面上从一个点到另一个点最优路径的数学模型。这种曲线是由美国数学家 J. A. Reeds 和 L. A. Shepp 在1990年提出的,它被广泛应用于路径规划和运动规划问题中。Reeds-Shepp曲线的很多原理和Dubins曲线类似,可以先学习曲线生成 | 图解Dubins曲线生成原理(附ROS C++/Python/Matlab仿真)

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

Reeds-Shepp曲线具有以下特点:

  • 最优性:Reeds-Shepp曲线是连接两个点的最短路径之一,通常是沿着曲线长度最短的路径。相比于Dubins曲线只允许车辆向前运动,RS曲线同时允许车辆前向、后向运动,使得在某些情况下可以得出比 Dubins 曲线更优的解
  • 约束性:曲线遵循机器人或车辆的运动学约束,例如最大转角、最大速度等。
  • 多样性:存在不同类型的Reeds-Shepp曲线,例如直线-圆弧-直线(L-S-L)、直线-圆弧-反向圆弧-直线(L-S-R-S)等,以适应不同场景下的路径规划需求。

通过计算和生成Reeds-Shepp曲线,可以帮助机器人或车辆高效地规划路径并完成复杂的运动任务。

2 Reeds-Shepp曲线的运动模式

经过证明,RS曲线从起点到终点的最短路径一定是下面的组合之一

{ C ∣ C ∣ C , C C ∣ C , C ∣ C C , C S C , C C β ∣ C β C , C ∣ C β C β ∣ C , C ∣ C π / 2 S C , C S C π / 2 ∣ C , C ∣ C π / 2 S C π / 2 ∣ C } \left\{ \begin{array}{c} C|C|C, CC|C, C|CC, CSC, CC_{\beta}|C_{\beta}C, C|C_{\beta}C_{\beta}|C,\\ C|C_{{{\pi}/{2}}}SC, CSC_{{{\pi}/{2}}}|C, C|C_{{{\pi}/{2}}}SC_{{{\pi}/{2}}}|C\\\end{array} \right\} {CCC,CCC,CCC,CSC,CCβCβC,CCβCβC,CCπ/2SC,CSCπ/2C,CCπ/2SCπ/2C}

其中 C C C表示圆弧运动, S S S表示直线运动,|表示车辆运动朝向发生改变。带 π / 2 \pi/2 π/2下标表示该段轨迹弧长对应的角度为 π / 2 \pi/2 π/2,带 β \beta β下标表示相邻两段轨迹弧长对应的角度相等。将上述组合完整展开后对应如表所示的48种运动模式,其中+代表前行,-代表倒车。后续经过证明, ( L − R + L − ) \left( L^-R^+L^- \right) (LR+L) ( R − L + R − ) \left( R^-L^+R^- \right) (RL+R)两种序列是多余的。

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

RS曲线在实现上的复杂度远远高于只有6种组合的Dubins曲线,考虑到序列间的对称关系,引入下面的变换简化曲线求解过程。

3 Reeds-Shepp曲线算法原理

3.1 坐标变换

类似Dubins曲线的思想进行坐标变换。在全局坐标系 x O y xOy xOy中,设机器人起始位姿 p s \boldsymbol{p}_s ps、终止位姿 p g \boldsymbol{p}_g pg、最小转弯半径分别为 ( x s , y s , α ) \left( x_s,y_s,\alpha \right) (xs,ys,α) ( x g , y g , β ) \left( x_g,y_g,\beta \right) (xg,yg,β) R R R。以 p s \boldsymbol{p}_s ps为新坐标系原点,位姿角 α \alpha α方向为 x ′ x' x轴,垂直方向为 y ′ y' y轴建立新坐标系 ,同样考虑归一化最小转弯半径

p s ′ = [ 0 0 0 ] , p g ′ = [ ( x g cos ⁡ β + y g sin ⁡ β ) R ( − x g sin ⁡ β + y g cos ⁡ β ) R β − α ] \boldsymbol{p}_{s}^{'}=\left[ \begin{array}{c} 0\\ 0\\ 0\\\end{array} \right] , \boldsymbol{p}_{g}^{'}=\left[ \begin{array}{c} \left( x_g\cos \beta +y_g\sin \beta \right) R\\ \left( -x_g\sin \beta +y_g\cos \beta \right) R\\ \beta -\alpha\\\end{array} \right] ps= 000 ,pg= (xgcosβ+ygsinβ)R(xgsinβ+ygcosβ)Rβα

3.2 时间翻转(time-flip)

将计算曲线的运动方向全部取反,得到的新曲线与原曲线具有时间翻转关系。如图所示,以 L − R + S + L + ↔ L + R − S − L − L^-R^+S^+L^+\leftrightarrow L^+R^-S^-L^- LR+S+L+L+RSL为例解释时间翻转:设实现了对 L − R + S + L + L^-R^+S^+L^+ LR+S+L+的计算 f ( x , y , ϕ ) f\left( x,y,\phi \right) f(x,y,ϕ),若用同样的函数计算 f ( − x , y , − ϕ ) f\left( -x,y,-\phi \right) f(x,y,ϕ),并将各段路径取反,则等价于以轨迹 L + R − S − L − L^+R^-S^-L^- L+RSL到达 ( x , y , ϕ ) \left( x,y,\phi \right) (x,y,ϕ)

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

3.3 反射变换(reflect)

将计算曲线的圆周运动类型全部取反,得到的新曲线与原曲线具有反射变换关系。如图所示,以 L − R + S + L + ↔ R − L + S + R + L^-R^+S^+L^+\leftrightarrow R^-L^+S^+R^+ LR+S+L+RL+S+R+为例解释仿射变换:设实现了对 L − R + S + L + L^-R^+S^+L^+ LR+S+L+的计算 f ( x , y , ϕ ) f\left( x,y,\phi \right) f(x,y,ϕ),若用同样的函数计算 f ( x , − y , − ϕ ) f\left( x,-y,-\phi \right) f(x,y,ϕ),并将圆弧段类型取反,则等价于以轨迹 R − L + S + R + R^-L^+S^+R^+ RL+S+R+到达 ( x , y , ϕ ) \left( x,y,\phi \right) (x,y,ϕ)

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

3.4 后向变换(backwards)

将计算曲线的轨迹段逆序,得到的新曲线与原曲线具有后向变换关系。如图所示,以 L − R + S + L + ↔ L + S + R + L − L^-R^+S^+L^+\leftrightarrow L^+S^+R^+L^- LR+S+L+L+S+R+L为例解释后向变换:设实现了对 L − R + S + L + L^-R^+S^+L^+ LR+S+L+的计算 f ( x , y , ϕ ) f\left( x,y,\phi \right) f(x,y,ϕ),若用同样的函数计算 f ( x cos ⁡ ϕ + y sin ⁡ ϕ , x sin ⁡ ϕ − y cos ⁡ ϕ , ϕ ) f\left( x\cos \phi +y\sin \phi ,x\sin \phi -y\cos \phi ,\phi \right) f(xcosϕ+ysinϕ,xsinϕycosϕ,ϕ),并将计算曲线逆序,则等价于以轨迹 L + S + R + L − L^+S^+R^+L^- L+S+R+L到达 ( x , y , ϕ ) \left( x,y,\phi \right) (x,y,ϕ)

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

4 仿真实现

4.1 ROS C++实现

核心代码如下所示

Points2d ReedsShepp::generation(Pose2d start, Pose2d goal)
{
  ...

  // coordinate transformation
  ...

  // select the best motion
  RSPath best_path({ REEDS_SHEPP_MAX }, { REEDS_SHEPP_NONE });

  _update(SCS(x, y, dyaw), best_path);
  _update(CCC(x, y, dyaw), best_path);
  _update(CSC(x, y, dyaw), best_path);
  _update(CCCC(x, y, dyaw), best_path);
  _update(CCSC(x, y, dyaw), best_path);
  _update(CCSCC(x, y, dyaw), best_path);

  if (best_path.len() == REEDS_SHEPP_MAX)
    return path;

  // interpolation
  int points_num = int(best_path.len() / step_) + 6;

  int i = 0;
  for (size_t j = 0; j < best_path.size(); j++)
  {
    int m;
    double seg_length;
    best_path.get(j, seg_length, m);

    // path increment
    double d_l = seg_length > 0.0 ? step_ : -step_;
    double x = path_x[i];
    double y = path_y[i];
    double yaw = path_yaw[i];

    // current path length
    double l = d_l;
    while (fabs(l) <= fabs(seg_length))
    {
      i += 1;
      std::tie(path_x[i], path_y[i], path_yaw[i]) = interpolate(m, l, { x, y, yaw });
      l += d_l;
    }
    i += 1;
    std::tie(path_x[i], path_y[i], path_yaw[i]) = interpolate(m, seg_length, { x, y, yaw });
  }

  // remove unused data
  ...

  // coordinate transformation
  ...

  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
	...

	# select the best motion
	planners = [self.SCS, self.CCC, self.CSC, self.CCCC, self.CCSC, self.CCSCC]
	best_path, best_cost = None, float("inf")

	for planner in planners:
		paths = planner(x, y, dyaw)
		for path in paths:
			if path.path_length < best_cost:
				best_path, best_cost = path, path.path_length

	# interpolation
	points_num = int(best_cost / self.step) + len(best_path.lengths) + 3
	x_list = [0.0 for _ in range(points_num)]
	y_list = [0.0 for _ in range(points_num)]
	yaw_list = [0.0 for _ in range(points_num)]

	i = 0
	for mode_, seg_length in zip(best_path.ctypes, best_path.lengths):
		# path increment
		d_length = self.step if seg_length > 0.0 else -self.step
		x, y, yaw = x_list[i], y_list[i], yaw_list[i]
		# current path length
		length = d_length
		while abs(length) <= abs(seg_length):
			i += 1
			x_list[i], y_list[i], yaw_list[i] = self.interpolate(mode_, length, (x, y, yaw))
			length += d_length
		i += 1
		x_list[i], y_list[i], yaw_list[i] = self.interpolate(mode_, seg_length, (x, y, yaw))
	
	# failed
	...

	# remove unused data
	...

	# coordinate transformation
	...

	return best_cost / self.max_curv, best_path.ctypes, x_list_, y_list_, yaw_list_

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

4.3 Matlab实现

核心代码如下所示

function [x_list, y_list, yaw_list] = generation(start_pose, goal_pose, param)  
    % coordinate transformation
    ...

    % select the best motion
    planners = ["SCS", "CCC", "CSC", "CCCC", "CCSC", "CCSCC"];
    best_cost = inf;
    best_path = [];

    for i=1:length(planners)
        planner = str2func(planners(i));
        paths = planner(x, y, dyaw);
        for j=1:length(paths)
            if paths(j).len < best_cost
                best_path = paths(j);
                best_cost = paths(j).len;
            end
        end
    end
    
    % interpolation
    points_num = floor(best_cost / param.step) + length(best_path.segs) + 3;
    x_list_ = zeros(points_num);
    y_list_ = zeros(points_num);
    yaw_list_ = zeros(points_num);

    i = 1;
    for j = 1:length(best_path.segs)
        m = best_path.ctypes(j);
        seg_length = best_path.segs(j);
        
        % path increment
         if seg_length > 0.0
            d_length = param.step;
         else
            d_length = -param.step;
         end
        x = x_list_(i); y = y_list_(i); yaw = yaw_list_(i);
        
        % current path length
        l = d_length;
        while abs(l) <= abs(seg_length)
            i = i + 1;
            new_pt = interpolate(m, l, [x, y, yaw], param);
            x_list_(i) = new_pt(1); y_list_(i) = new_pt(2); yaw_list_(i) = new_pt(3);
            l = l + d_length;
        end
        i = i + 1;
        new_pt = interpolate(m, seg_length, [x, y, yaw], param);
        x_list_(i) = new_pt(1); y_list_(i) = new_pt(2); yaw_list_(i) = new_pt(3);
    end
    
    % remove unused data
    ...
    
    % coordinate transformation
    ...
end

曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真),机器人,人工智能,自动驾驶,ROS,ROS2,智能硬件

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


🔥 更多精彩专栏文章来源地址https://www.toymoban.com/news/detail-843401.html

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

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

到了这里,关于曲线生成 | 图解Reeds-Shepp曲线生成原理(附ROS C++/Python/Matlab仿真)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

原文地址:https://blog.csdn.net/FRIGIDWINTER/article/details/136994820

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 路径规划 | 图解LPA*算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法

    2024年01月16日
    浏览(9)
  • 路径规划 | 图解Theta*算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法

    2024年02月11日
    浏览(7)
  • 路径规划 | 图解RRT*算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 RRT*算法 针对传统RRT算

    2024年02月06日
    浏览(15)
  • 路径规划 | 图解Lazy Theta*算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法

    2024年02月10日
    浏览(10)
  • 轨迹规划 | 图解路径跟踪PID算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 PID控制 是一种常用的

    2024年02月07日
    浏览(11)
  • 路径规划 | 图解D* Lite算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 上节我们介绍了LPA*算

    2023年04月08日
    浏览(6)
  • 路径规划 | 图解Informed RRT*算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 传统的RRT算法存在一些

    2024年02月08日
    浏览(16)
  • 路径规划 | 蚁群算法图解与分析(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法

    2024年02月16日
    浏览(8)
  • 轨迹规划 | 图解动态窗口算法DWA(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Moti

    2024年02月08日
    浏览(16)
  • 路径规划 | 图解动态A*(D*)算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法

    2024年02月03日
    浏览(14)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包