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

相关推荐
2501_941875281 小时前
从配置中心到动态管理的互联网工程语法演进与多语言实践分享
开发语言·python
辰烨chenye1 小时前
LeetCode Hot 100 题解 · 普通数组篇
算法·leetcode·职场和发展
hfywmsj1 小时前
广州餐饮铺位招租决策模型:多因子选址系统设计
开发语言·人工智能·python·广州餐饮铺位招租
努力的lpp3 小时前
php反序列化一(大白话版)
开发语言·web安全·php·ctf·反序列化
菜鸟~noob2333 小时前
【电子战】第03篇:CFAR(恒虚警)处理【含matlab代码】
开发语言·matlab
沧沧凉凉4 小时前
同一个 Blender 建模,Claude 两个模型都翻车,GPT-6 一次过
人工智能·游戏·ai编程
X54先生(人文科技)4 小时前
ELR-SELLM Edge 神经元网络架构评估报告
人工智能·深度学习·架构·开源
今年下半年4 小时前
从零开始搭建一套大语言模型 + LangGraph 多智能体编排 + RAG 知识库检索** 的智能问答平台
人工智能·语言模型·自然语言处理
泡海椒4 小时前
内置SPI函数库详解:JQuick-Java Builtin工具类实战用法
java·开发语言·python