前序教程请参照专栏,如您是从0开始阅读,可以直接跳到Python运动规划库教程(Python Motion Planning)-0-简介与安装。
我来帮你把这份关于轨迹优化器的Markdown文档翻译成中文。
以下是翻译后的中文Markdown文档:
路径规划器返回一系列路径点后,路径通常是分段线性的,无法直接被真实机器人执行。轨迹优化器将这些路径点平滑或重塑为几何上连续的曲线,使路径跟踪控制器更容易跟随。在本库中,曲线生成器接收世界坐标系下的路径点,返回平滑后的世界坐标系路径,以及包含曲线信息(成功与否、长度)的字典。
曲线生成器根据输入路径点的格式分为两类:
- 基于点 的生成器(如
BSpline、CubicSpline)仅接收二维位置 ( x , y ) (x, y) (x,y)。它们通常用于平滑 Theta* 等基于栅格的路径规划器的输出,这些规划器产生的路径点没有方向信息。 - 基于位姿 的生成器(如
ReedsShepp、Dubins、Bezier、Polynomial)接收带有方向的二维位姿 ( 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_mode 和 spline_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 参考文档。