1.4 - 曲线生成轨迹优化算法(以BSpline和ReedsShepp为例) - Python运动规划库教程(Python Motion Planning)

前序教程请参照专栏,如您是从0开始阅读,可以直接跳到Python运动规划库教程(Python Motion Planning)-0-简介与安装

我来帮你把这份关于轨迹优化器的Markdown文档翻译成中文。

以下是翻译后的中文Markdown文档:


路径规划器返回一系列路径点后,路径通常是分段线性的,无法直接被真实机器人执行。轨迹优化器将这些路径点平滑或重塑为几何上连续的曲线,使路径跟踪控制器更容易跟随。在本库中,曲线生成器接收世界坐标系下的路径点,返回平滑后的世界坐标系路径,以及包含曲线信息(成功与否、长度)的字典。

曲线生成器根据输入路径点的格式分为两类:

  • 基于点 的生成器(如 BSplineCubicSpline)仅接收二维位置 ( x , y ) (x, y) (x,y)。它们通常用于平滑 Theta* 等基于栅格的路径规划器的输出,这些规划器产生的路径点没有方向信息。
  • 基于位姿 的生成器(如 ReedsSheppDubinsBezierPolynomial)接收带有方向的二维位姿 ( x , y , θ ) (x, y, \theta) (x,y,θ)。它们通常用于为具有曲率约束的类车机器人连接关键位姿。

与其他模块一起导入轨迹优化器模块。

python 复制代码
from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *

基于点的曲线生成器(B-Spline)

对于基于点的生成器,我们首先使用任意全局路径规划器规划路径,然后将路径点从地图坐标系转换到世界坐标系,就像传递给控制器之前所做的那样。

python 复制代码
planner = ThetaStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])  # 用于可视化扩展节点

path_world = map_.path_map_to_world(path)

创建曲线生成器并生成平滑曲线。这里以 B-Spline 曲线为例。你可以设置 step 参数来控制曲线上的采样密度,k 参数来控制 B-Spline 的阶数。param_modespline_mode 参数分别控制参数选择和 B-Spline 构造模式。更多详情请参考 API 参考部分。

python 复制代码
generator = BSpline(step=0.01, k=3, param_mode="centripetal", spline_mode="interpolation")
smooth_path, curve_info = generator.generate(path_world)
print(curve_info)

打印结果:

复制代码
{'success': True, 'length': 59.68077764623435}

可视化。原始 Theta* 路径用虚线绘制以作对比,平滑后的 B-Spline 曲线用实线绘制。注意 map_frame 设置为 False,因为曲线生成器返回的是世界坐标系下的路径点。

python 复制代码
vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()

可运行的完整代码:

python 复制代码
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = ThetaStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])

path_world = map_.path_map_to_world(path)

generator = BSpline(step=0.01, k=3, param_mode="centripetal", spline_mode="interpolation")
smooth_path, curve_info = generator.generate(path_world)
print(curve_info)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()

基于位姿的曲线生成器(Reeds-Shepp)

基于位姿的生成器接收世界坐标系下的位姿列表 ( x , y , θ ) (x, y, \theta) (x,y,θ),而不是简单的二维位置,因为曲线的形状取决于每个路径点的方向。这适用于用户给定关键位姿或由位姿感知规划器产生位姿的场景。

如果你只有二维位置,可以使用 Geometry.add_orient_to_2d_path 自动为每个路径点分配指向下一个路径点的方向,无需手动构建位姿列表。

python 复制代码
path_with_orient = Geometry.add_orient_to_2d_path(path_world)
print(path_with_orient[:3])

打印结果:

复制代码
[(5.5, 5.5, 1.4711276743037347), (6.5, 15.5, 0.982793723247329), (8.5, 18.5, 0.3217505543966422)]

创建 Reeds-Shepp 曲线生成器并生成曲线。max_curv 参数设置类车机器人可执行的最大曲率,一般为最小转弯半径的倒数 1 r min \frac{1}{r_\text{min}} rmin1。生成器自动搜索所有 Reeds-Shepp 运动模式(前进和后退),并为每个路段选择最短的可行组合。更多详情请参考 API 参考部分。

python 复制代码
generator = ReedsShepp(step=0.01, max_curv=1.0)
smooth_path, curve_info = generator.generate(path_with_orient)
print(curve_info)

打印结果:

复制代码
{'success': True, 'length': 59.75446987444724}

可视化。原始 Theta* 路径用虚线绘制以作对比,生成的 Reeds-Shepp 曲线用实线绘制。同样,map_frame 设置为 False,因为曲线处于世界坐标系。

python 复制代码
vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()

可运行的完整代码:

python 复制代码
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = ThetaStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])

path_world = map_.path_map_to_world(path)
path_with_orient = Geometry.add_orient_to_2d_path(path_world)

generator = ReedsShepp(step=0.01, max_curv=1.0)
smooth_path, curve_info = generator.generate(path_with_orient)
print(curve_info)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()

更多曲线生成器及其参数,请参考 API 参考文档。

相关推荐
阿里云大数据AI技术21 小时前
官宣|Apache Fluss 毕业成为顶级项目,湖流一体开启 Agentic Lake 全面实时化时代
人工智能·flink
敲代码的玉米C1 天前
测试一直在写你的真实数据根
前端·人工智能·架构
Mr.huang1 天前
自注意力机制(Self‑Attention)
人工智能·深度学习
Wzx1980121 天前
python沙箱和docker沙箱你选对了吗?
开发语言·python·docker
Python私教1 天前
签名 URL 刷新导致审批失效?别急着删掉所有 query
python·安全
牧羊人.3331 天前
Python 办公自动化从入门到入土|09 数据容器之字典
开发语言·python
品牌测评1 天前
大模型推理算力平台推荐分享|六家平台计费与架构拆解
大数据·人工智能·架构
武汉万象奥科1 天前
8路AHD摄像头同时输出演示,万象奥科RK3576 AHD硬件方案
人工智能·计算机视觉
LedgerNinja1 天前
WEEX 真实情况如何?交易平台如何判断是否可靠
大数据·人工智能·区块链
重庆传粉科技1 天前
GEO深度解读:AI时代品牌营销的核心,从内容优化转向事实治理
人工智能