移动机器人运动规划(六)--软约束和硬约束下的轨迹优化

课程的难度是越来越大的，本节课介绍软约束和硬约束下的轨迹优化。

Introduction

加“推力”（软约束）；在可通行区域内加“约束 ”（硬约束）；

硬约束：在全局条件下满足s.t约束；
软约束：倾向于实现所加的约束；

Hard-constrained Optimization

Corridor-based Trajectory Optimization(基于走廊的轨迹优化)

首先将环境表达成八叉树的栅格地图；然后搜寻轨迹；之后将可通行栅格进行膨胀(用方块进行膨胀的原因一是八叉树本来就是用方块表达；二是为了保证能够将其转换为凸优化问题)；最后再重新生成路径。

将轨迹约束在相应格点间的方法；

硬约束的优点：效率很高，能够在有限资源内快速求解；提供了一个很大的且质量很高的自由解空间，能够优化到一个非常好的状态。

关于多项式函数的求解：直到四次函数都是可以直接用求根公式进行求解；更高次的则可以用“伴随矩阵”的方法(具体参考matlab中的roots函数方法，主要思想就是将多项式求根问题转换为求矩阵特征值问题)。

Bezier Curve Optimization

基于走廊的优化方法存在的问题是：施加全局的安全和动力学可行的约束不是很方便；
利用贝塞尔曲线则能够解决以上的不足之处。贝塞尔曲线总是可以找到对应的多项式表达，映射关系可以用一个矩阵来表示。

贝塞尔曲线一定会通过起止控制点而不通过中间的控制点；
凸包性质(很重要的性质)；
求导后的依然是一个贝塞尔曲线；
每一段曲线均是定义在[0,1]。所以需要一个时间的映射。

可以利用凸包性质将速度和加速度做一个很好的限制。

Other Options

加很多很多的方块约束；

混合整数优化；

Soft-constrained Optimization

Distance-based Trajectory Optimization

硬约束方法存在的问题：首先是将所有的安全区域都认为是等价的，因此可能会导致轨迹的某些地方距离障碍物太近，如果控制部分不能完全按照轨迹走，则依然会导致碰撞，这是硬约束的不足之处；另一个潜在问题在于如果大量的使用基于视觉的方法，则会产生较多的噪点，那么就会给规划问题带来麻烦(对噪声敏感)。软约束的方法则是施加一个“推力”，将轨迹往远离障碍物的方向推。核心问题在于设计好的目标函数，因此当目标函数设定不合理时会导致路径可能会碰到障碍物，这是软约束的不足。

以下都是数值分析里面的相关知识，关于各种方法求解数值解。或者直接去看凸优化的相关方法。

整个规划的完整流程。

基于梯度的优化选取初值时很重要的；

分两步走的优化方式比直接加在一起优化的方式要好很多。

Case Study

Fast Planner

贝塞尔曲线可以看成是一种特殊的B样条曲线。

Homework