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 参考文档。

相关推荐
冬奇Lab1 小时前
Workflow 系列(06):安全——跨步骤注入传播与四层防御
人工智能·工作流引擎
冬奇Lab1 小时前
每日一个开源项目(第149篇):RAG-Anything - 把图片、表格、公式当成一等公民的多模态 RAG 框架
人工智能·开源
米小虾2 小时前
AI Agent 安全实战指南:当智能体开始"不听话",开发者该如何应对?
人工智能·安全·agent
IT_陈寒3 小时前
Vite的热更新突然不香了,排查三小时差点砸键盘
前端·人工智能·后端
用户8356290780515 小时前
Python 实现 PDF 文件加密与解密方法
后端·python
用户8356290780515 小时前
使用 Python 冻结与拆分 Excel 窗格教程
后端·python
阿里云大数据AI技术5 小时前
构建高转化海外电商搜索:阿里云OpenSearch行业算法版的全链路智能优化策略实战
人工智能·搜索引擎
Awu12276 小时前
⚡从零开发 Agent CLI(五)实现一个可治理、可扩展的工具系统
前端·人工智能·claude
字节跳动视频云技术团队6 小时前
让 Agent 成为音视频工作台:AI MediaKit CLI + Skill 发布
人工智能·音视频开发