[运动规划算法]基于硬约束和软约束的轨迹规划

---

前言

1.基于Minimum snap的轨迹优化

基于Minimum snap框架有利于光滑轨迹的生成但是不利于作避障。

2.带安全约束的Minimum snap

一般是添施加推力将轨迹往里推（软约束），或者轨迹上添加bounding box（硬约束）。

3.硬约束和软约束

硬约束（下图上方）要求等式约束和不等式约束都必须在全局的条件下满足，生成的轨迹才能是我们可以接受的解。

软约束（下图下方）将上方的等式约束和不等式约束都加到目标函数中，它是没法保证约束一定是成立的，但它是使约束是倾向性成立。常见的惩罚函数L2 loss、L1 loss、Huber Loss和Barrier function。

---

一、硬约束优化

1.基于走廊的光滑轨迹生成

实现步骤：构建八叉树地图 -> 在八叉树地图中搜索可行的路径并构建飞行走廊 -> 飞行走廊进行膨胀 -> 生成飞行走廊里的轨迹。

如何保证整段轨迹在飞行走廊中（安全区域内）？

解法：迭代检查最大值并添加额外约束，当轨迹中存在极值点查过边界限制yb时，在每个极值点出添加额外约束。

新点约束比原来约束yb的限制要更加紧迫，在有限次的迭代后，会把多项式压在约束边界内。

• 迭代求解过程耗费大量时间
• 实际中有些情况是不存在满足所有约束的可行解，但是需要通过多次迭代才能知道解是否存在。

如何能够尽快地判断是否存在可行解？

解法：将基于monomial polynomial轨迹变为基于Bernstein polynomial轨迹。

---

2.基于贝塞尔曲线的轨迹优化

贝塞尔曲线的特点

1. 轨迹一定经过起点和终点，并且轨迹一定不经过中间控制点；
2. 避免迭代求值，整段贝塞尔曲线一定完全被控制点构成的凸包所包围；
3. 贝塞尔曲线的导数还是贝塞尔曲线，并且有闭式解，如果定义了轨迹的贝塞尔曲线，那么高阶导数的曲线可以直接得到；
4. 每段曲线时间间隔都定义在（0，1）之间。
   
   
   waypoints constraints：轨迹经过起点和终点。
   continuity constraints：多段轨迹必须连续。
   safety constraints：各段的control points必须落在飞行走廊中。
   dynamical feasibility constraints：整段的速度和加速度不能超过物理极限。
   
   综合所有约束项之后得到的公式化描述，包括等式约束、不等式约束、动力学约束。回归到一个QP问题，使用Mosek convex solver求解。此时的QP问题只需要求解一次，不需要迭代过程。（之前求解次数较多是因为安全性约束无法保证，并且每次更改边界后也没办法保证可行解的存在性和可行性）。

---

3.其他解决方法

---

二、软约束优化

1.基于距离的轨迹优化

硬约束方法存在的问题：将所有的安全区域都认为是等价的，很可能会导致轨迹的某些地方距离障碍物太近，如果控制部分不能完全按照轨迹走，则依然会导致碰撞；另一个潜在问题在于如果大量的使用基于视觉的方法，则会产生较多的噪点，那么就会给规划问题带来麻烦(对噪声敏感)。

软约束的方法则是施加一个“推力”，将轨迹往远离障碍物的方向推。核心问题在于设计好的目标函数，因此当目标函数设定不合理时会导致路径可能会碰到障碍物，这是软约束的不足。

2.目标函数的设计

目标函数 = 光滑代价函数 + 碰撞代价函数 + 动力学代价函数。
光滑代价函数： 使用minimum snap来实现。
碰撞代价函数： 对太靠近障碍物进行惩罚。
动力学代价函数：对超出动力学约束进行惩罚。

3.欧式距离场

使用指数函数作为代价函数是为了惩罚太靠近障碍物的轨迹。

4.数值优化

5.规划策略

整个规划流程：

1. 导航开始前先获取一条全局的路径；
2. 使用前端获取局部路径和局部的目标点；
3. 使用后端优化生成局部轨迹；
4. 跟踪生成的轨迹

扩展策略

1. 建图模块出现异常值阻塞轨迹时进行激活；
2. 在附近区域生成短而安全可行的轨迹；
3. 悬停进行观察。
   

• 使用minimum snap初始化轨迹

  • 有利于光滑的轨迹生成；
  • 可能会因为过冲而造成碰撞损失，初始值不安全。

• 使用沿着路径的直线轨迹来初始化轨迹

  • 生成的轨迹不够光滑；
  • 初始值安全且不会发生碰撞；
  • 通过使用指数函数来作为惩罚函数可以给出一条无碰撞安全可行的初始轨迹。
    

• 优化策略

  • 仅以碰撞代价来优化轨迹；
  • 重新分配时间和重新调整参数来优化轨迹；
  • 通过添加平滑项和动态惩罚项来优化目标。
    

6.实验结果

https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization

---

参考资料

【1】深蓝学院运动规划课程。
【2】【泡泡机器人公开课】第九十三课：Online quadrotor trajectory generation-高飞
https://www.bilibili.com/video/BV1W4411E7jq