移动机器人路径规划（六）--- 软约束、硬约束下的轨迹优化/生成SOFT AND HARD CONSTRAINED TRAJECTORY OPTIMIZATION_硬约束和软约束

目录

1 介绍

1.1 引子

1.2 什么叫做具有软约束、硬约束的轨迹优化问题呢

2 硬约束的轨迹优化  Hard-constrained Optimization

3 软约束的轨迹约束 Soft-constrained Optimization

1 介绍

1.1 引子

        上次我们介绍了Minimum Snap，它有很多优点：

        1.可以让机器人在某个时刻达到某个航路点

        2.在任何期望的时间内我们可以通过数学求解出机器人的速度加速度及各阶导数使得问题层次简单

        3.非常有效率

        但是也有缺点：

        1.可能在两端轨迹间发生碰撞（如上图）

        因此它会生成很平滑的曲线，但是不适合去做避障。

        因此这里我们就优化它让他可以去避障。

        我们可以从两个方面出发：

        从障碍物角度出发：让所有障碍物产生一个推力，让所有minimum snap的轨迹推向远离障碍物的区域。

        从可通行角度出发：将轨迹吸引到安全可通行区域里面。加入硬约束，让他们必须通过bounding box。

1.2 什么叫做具有软约束、硬约束的轨迹优化问题呢

        对于一个优化问题，我们有优化目标以及等式、不等式的约束。

        对于硬约束优化问题来说，我们要求等式、不等式的约束在全局条件下都要满足的。

        对于软约束优化问题来说把约束加在了目标函数中，将约束作为惩罚函数，约束并不是完全满足的而是倾向于使得约束成立。并不能满足约束...

2 硬约束的轨迹优化  Hard-constrained Optimization

        在环境中可通行区域里给机器人施加一个一定要从这里走的一个约束。

        目前比较流行的基于飞行走廊的轨迹优化（Corridor-based Trajectory Optimization）。

        用八叉树表示环境中的障碍物信息把障碍物装入八叉树。第一步就是把环境里的障碍物塞入八叉树地图。

        第二步：用A*等算法计算出一点到另一点的路径（不含优化）这时已经构成了一个飞行走廊。

        第三步：搜索的Path只是一个空余的小区域，我现在要把八叉树的飞行走廊做一个膨胀。

        第四步：扩展到障碍物边缘，减少栅格块的个数，让其顶到边缘。再次规划优化轨迹。（和minimum snap基本一样，但是我们现在已经没有路标点了，我们只需要确保在安全飞行走廊就可以了）

        看一下问题的表达形式：

        我们仍然有起点、终点的约束、两端轨迹的连接点在两个方块的交集、连续性约束。

        保证整段轨迹不能在边界值外面（把时间离散化然后每个点施加边界点约束？？NO），我们利用启发式的思想，我们规划完轨迹后我们加上一个后验的检查，检查这段轨迹是否超过边界，并且无人机也不能有一个特别夸张的加速度速度.....同样将时间离散化也是不聪明的，我们采取迭代的后验检测的方式。

        我们看看以下的方法有哪些好处？前端的路径搜索和后端的轨迹优化非常的有效率，飞行走廊有一个非常大的解的空间。

        因此我们先检查给定约束下轨迹的极值看看极值有无超过约束，若超过约束则施加额外的约束进行迭代。

        同样的，我们可以将这种策略用在速度、加速度上。我们求多项式的极值是否超过我们预期值，若超过，我们将约束收紧再次解这个优化问题。

        低于4阶的话有求值求根公式，大于四阶可以用伴随矩阵的方法：求解问题转化为求特征值的方法。

        问题：在线的时候可能求解很多次；如果无解的条件下还要求解很多次才能知道无解，这在无人机的避障上是毁灭性的。

        如何解决：使用Bernstein polynomial这种特殊的多项式替换常见的多项式

        就是我们的，n表示多项式的阶数。这里的系数c被称为控制点。

        BC曲线和我们原来的多项式monomial存在一一映射关系。6阶的话是上面的形式。因此minimum snap的所有monomial能用的BC都能用。

        那么它有什么特点呢？

        1.它会起始于第一个控制点，终止于最后一个控制点，不会通过中间控制点

        2.凸包性质：所有控制点会组成凸包，会把BC曲线全部包围

        3.求导后曲线仍然是BC曲线，它的控制点可以用本身控制点写成一个线性的表达式

        4.每一段轨迹时间定义在[0,1]上的

        那么如何利用凸包性质呢？

        飞行走廊中每一个子单元都是一个凸的几何体。如果我们把某一段BCR曲线的控制点约束在多面体里。（飞行走廊中每一个凸多面体对应一段曲线，我们把曲线的所有控制点强行约束到多面体中）同样的，速度、加速度也可以施加这种约束。并且我们这里无需进行迭代求解，我们加入约束的个数 = BCR曲线的阶数。

        具体的形式呢？

        边界条件：第一段轨迹的第一个点等于我们的起始状态，最后一段轨迹的最后一个点等于终止状态。

        连续性条件：控制点和控制点建立等式约束（比如两个轨迹的交界点是一个控制点，位置速度加速度都要相等），我们只需要将最后一个控制点和下一段轨迹的第一个控制点的位置、速度、加速度相等即可。

        安全性约束：每一个控制点约束在安全走廊的凸包内。

        动力学可行性：约束轨迹速度、加速度

        最后我们发现：目标函数没有变（还是二次型），还有一些等式、不等式约束。

GitHub - HKUST-Aerial-Robotics/Btraj: Bezier Trajectory Generation for Autonomous Quadrotor, ICRA 2018Bezier Trajectory Generation for Autonomous Quadrotor, ICRA 2018 - GitHub - HKUST-Aerial-Robotics/Btraj: Bezier Trajectory Generation for Autonomous Quadrotor, ICRA 2018https://github.com/HKUST-Aerial-Robotics/Btraj        其他的方法也可以加硬约束：

        1.前端找PATH（膨胀后的）

        2.把PATH离散均匀分成很多点

3 软约束的轨迹约束 Soft-constrained Optimization

        动机：硬约束可能留给障碍物的空间太小了、把所有的安全地域都认为是等价的，解空间对噪声非常敏感。

        软约束是向障碍物施加一个推动力。

        怎么实施呢？

        这里和硬约束不同的是障碍物的惩罚。

        解问题差不多要这样的：

        怎么把轨迹优化算法部署到无人机呢？？

        1.前端：任务开始时获得一个全局的路径，但由于无人机在线的感知范围是非常的有限的，因此我只会在很短的距离做一个planning。

        2.在每次局部规划的时候会在前端上找一个局部的path后端进行局部轨迹的生成（蓝色的轨迹），灰色的是全局的轨迹。如果灰色可用那么就用，如果由障碍物的话就重新搜索一个路径。我会让无人机把这一段执行完再进行下一步的优化。

        3.探索策略：视觉的话如果没路了让机器人做一段探索再寻路。

        初值选择：前端的直线路径？？？（collision free（不会发生碰撞）但光滑性很差），先用minimun snap求解一下但是会发生碰撞。

        在机器人领域内，我们希望安全第第一，我们可以接收最后优化过的轨迹不是那么光滑但是我们一定要安全的。我们使用一定安全的path做初始值，我们可以用一个指数爆炸上升的惩罚函数。

        优化策略：两步优化

        先只优化jc，先不管光滑和动力学可行。再考虑光滑和动力学可行。

        GitHub - HKUST-Aerial-Robotics/grad_traj_optimization: Gradient-Based Online Safe Trajectory GeneratorGradient-Based Online Safe Trajectory Generator . Contribute to HKUST-Aerial-Robotics/grad_traj_optimization development by creating an account on GitHub.https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization