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

相关推荐
郝同学今天有进步吗1 分钟前
构建 LangGraph Code Review Agent(四):文件过滤与 AnalysisPackage 分包
git·python·ai·code review
俊哥V5 分钟前
每日 AI 研究简报 · 2026-07-23
人工智能·ai
a1117767 分钟前
基于PyTorch的动物图像识别系统 开源
人工智能·pytorch·python
某林2127 分钟前
ROS2 + WebRTC + MQTT 异构系统架构
架构·系统架构·机器人·硬件架构·webrtc·ros2
qetfw13 分钟前
MWU:Vue 3 + FastAPI 的 MaaFramework 跨平台 WebUI 源码
前端·vue.js·python·fastapi·开源项目·效率工具
Xzaveir_77715 分钟前
企业号码负面标记治理:认证、申诉与合规事件的三轨模型
大数据·网络·人工智能·科技·产品经理
武子康15 分钟前
1.2GB 离线语音 Agent 真正值得复用的不是 908ms:四阶段职责 + 状态感知 Tool Schema + 可观测时间锚点
人工智能·后端·agent
a44931536218 分钟前
MacBook USB-C不充电故障诊断:从PD握手到主板供电链路
c语言·开发语言·macos·电脑
测试老哥20 分钟前
接口自动化测试分层设计与实践总结
自动化测试·软件测试·python·测试工具·职场和发展·测试用例·接口测试
水如烟22 分钟前
孤能子视角:中西医合璧系列·05 收束篇——同一关系场的四次显影:术法道伦理的完整循环
人工智能