state lattice planner 调试效果和思路分析_state lattice planner的km kf sf分别是什么

简介

State lattice planner 功能包，局部状态节点规划器。

通过生成基于 base_link 坐标下的到各个状态点的曲线的参数。在实际运行机器过程中，根据机器当前状态，目标 goal 位姿，查表找到接近的参数。再调整参数值来获取终点靠近目标 goal 的轨迹，选择曲线上的控制节点作为控制输出。

紫色三角是机器 base_link，蓝色圆点是状态表中的目标点状态，红色菱形是实际目标点。通过查表得到 D 最接近 G，用 D 的状态可以生成一条从 base_link 到 D 曲线 c。调整 D 状态参数可以在附近生成多条曲线，用最接近 G 的曲线参数，生成控制节点输出。

调试效果

视频演示的是更改了源码的目标采样，轨迹碰撞检测，轨迹评价的效果。

轨迹效果受限于采样点与状态表匹配程度，由于状态表基于 base_link，在机器运行过程中采用点与状态表的匹配不稳定，生成的轨迹会有抖动。

绿色多边形是机器碰撞检测框，红色点的方阵是状态表采样的位置，黄色箭头平行于目标点采样，蓝色曲线是规划出来到采样点的轨迹，机器跟随的红色轨迹就是经过评价函数筛选后的轨迹。

思路

1. 状态表

通过目标位姿 [x, y, yaw] 和机器当前状态线速度和角速度 [v0, k0] ，在加速度，角速度，角加速度限制下采样曲线，不需要机器的位姿是因为该方法是基于 base_link 的。

如何生成曲线下面再讲。

最终的状态表包含内容是：

2. 目标采样

源码是以机器为圆心，与目标 goal 距离为半径的圆上，在 goal 附近采样。可以根据自己需要修改采样规则，我就将其改为与 goal 平行的采样。

下图中紫色三角为机器，蓝色原点为状态表中的采样状态，红色菱形 G 是目标，橙色菱形是 G 的采样点。

3. 查表

根据当前速度 v0，角速度 k0，与采样点位姿 [x, y, yaw] 查表

得到曲线参数插值点角速度 km，终点角速度 kf，曲线长度 sf。

在一开始生成状态表的时候，km，kf 参数为 0，sf 是原点到采样点的欧氏距离。之后优化曲线后，会把 km，kf，sf参数更新到状态表中。

4. 曲线优化

• 从 v0 提升速度达到 vt 目标速度，并且走完 sf 距离作为走完轨迹的时间预估 t。

• 根据控制频率可以得到每个时间控制节点间隔 dt；

• 由 dt 和 v0，vt，加速度 a 生成每个控制节点 ct 的速度 vt 和累积的距离 st。

• 以曲线长度为 x 轴，角速度为 y 轴，用 ax^3+bx^2+cx+d 来作 b 样条插值角速度与曲线长度的函数，在初始点 [s0, k0]，插值点[sm, km]，终点[sf, kf] 来拟合两条曲线。[s0, k0] 到 [sm, km] 是一条角速度曲线，[sm, km] 到 [sf, kf] 是另一条角速度曲线。

• 有了“曲线长度-角速度”函数，用控制节点的 st 便可以获得该节点的角速度 kt。

• 在加速度，最高速度，角加速度，最高角速度等约束下，从初始节点 c0 开始，向下一个控制节点计算 dt 时间后的机器状态，连续迭代，得到一系列状态和控制，进而得到轨迹。

• 轨迹优化

不同参数 km，kf，sf 计算后的轨迹终点也不同，我们需要在优化中让轨迹终点向目标点靠近。

源码以 [km, kf, sf] 为自变量，轨迹为应变量，计算雅克比矩阵。

让 h = [km, kf, sf] = [0.05, 0.05, 0.1]，计算 2 个 h 前后的轨迹点 c(t-1) 和 c(t+1) 的变化梯度。

把 x 轴看做曲线参数 h，也就是 [km, kf, sf] ，把 y 轴看作曲线终点，绿线看作目标 goal，那么交点 A B C D E F 对应的 x 坐标就是曲线终点在目标点的曲线参数 h 值。

用轨迹终点与目标 goal 作差得到 cost，用雅克比矩阵解算 cost。

这里是把 goal 当做轨迹上的点，并且在轨迹终点前后 2 个 h 的状态之间。

形如 j * dh = cost，求解 dh 得到曲线终点接近 goal 的曲线参数 h 的变化量。

用 dh 更新状态表原来的曲线参数，生成更加接近 goal 的曲线。对所有 goal 的采样点生成轨迹。

5. 轨迹评价

源码以曲线终点与 goal 的 [x y yaw] 的模作为评价标准，这个地方也可以根据自己需求加入障碍物距离评价，目标航向角评价，轨迹曲率评价，轨迹长度评价等。

优缺点

优点：

具备了规划和控制功能；

采样量相对较小，因为是局部的状态晶格采样；

缺点：

是基于 base_link 的状态晶格规划，当机器动起来时候，状态晶格的位置相对静态地图就发生变化，会重新生成不一样的轨迹；

生成的轨迹是线速度提升到目标速度的轨迹，只能做简单的绕障。复杂的绕障可能需要加速-减速-加速的轨迹规划；

规划效果依赖状态表的密度，并且对与机器平行的目标点，机器身后的目标点难以规划；