课程视频：运动规划B站
课程资料：运动规划git
机器人具备：
状态估计
感知
规划
控制
motion planning ？
安全：无碰撞
柔顺：节能，舒适，某种最优
动力学可行：控制闭环可执行
怎么做研究：
overall knowledge：
不同场景选择合适的方法；设计customized strategy
dirty work :
不要等，动手自己做，实验验证
熟悉整个系统
机器人的所有component都考虑到
基于搜索
图搜索、dijkstra, A*、跳点
基于采样
PRM, RRT, RRT*, 改进版
考虑动力学的
两点边界值最优控制问题、state lattice， kinodynamic rrt * , hybrid A*
back-end :trajectory optimization
minimum snap
栅格地图grid map
八叉树地图 octomap , sparse , structural, 非直接索引递归查询
voxel hashing,
point cloud map
TSDF, truncated signed distance functions
ESDF, euclidean signed distance functions
在工作空间中，机器人有形状和大小，在配置空间（configuration space）是一个点，易于规划运动。
障碍物膨胀处理，将机器人看为质点。
图：节点和边；有向图和无向图；边的权重/代价；
搜索总是从起始状态点开始，向目标节点形成连接，形成树状结构。
维护一个container容器，存储所有有效节点
循环:
访问容器内的节点，延伸向其邻节点加入容器，
广度优先搜索BFS
队列，先进先出
弹出最浅的节点进行延伸，一层一层frontier推进
深度优先搜索DFS
堆栈，后进先出
先弹出（选择并延伸）最深的节点（一条路上最后的节点继续延伸）
先顺时针后逆时针（自己定义一个循环选择策略）
贪心算法greedy beset first search
根据人为定义的启发式规则（对目标距离的猜测）指引方向
优先向距离更近的邻节点延伸
在障碍物环境中陷入局部最优
策略：累计代价最低的节点先被选择向外延伸
通用循环：弹出-》拓展-》压入堆栈
优先级队列数据结构：自动排序，自动弹出代价最小的节点。
从初始容器弹出节点，延伸新节点加入队列，弹出节点加入另一个容器（被延伸过的）
新节点加入时赋予其由总代价= 父节点代价+父子边代价，如新节点在队列中，比较新旧代价，使队列中节点代价变小。
dijkstra结合贪心：
累积代价gn:当前节点到起点的代价
启发函数hn:当前节点到目标节点的估计代价。
fn = gn + hn
根据fn队列排序，弹出fn最小的节点
新节点的gn是累加的=从起点到该节点的边代价之和，fn是独立的
问题：估计代价并不是实际代价，会影响路径选择。估计代价要小于实际代价，保持最终路径的最优性。
估计代价：欧氏距离，曼哈顿距离
栅格地图结构化highly structural
理想的启发式函数规则
运动规划：前段路径搜索+后端轨迹生成
跳点算法
找到搜索的对称性并打破它，找特殊点
inferior neighbor
natural neighbor
forced neighbor
straight pruning
diagonal pruning
A* 几何邻居加入openlist
JPS 将特殊点加入openlist
建图阶段：距离准则和障碍物准则
寻路阶段：搜索图中的路径连线
随机采样Xrand
找树中最近点Xnear
steer延伸一个步长到Xnew
连接Xnear–> Xnew
碰撞检测边的有效性
有则加入树无则（略，减距离加）
优势：快，可行路径
kdtree:
改进Xnear 找最近点的过程
narrow passage:
rrtconnect ，双向搜索
改进采样过程
pipeline ： mission - > path finding -> trajectory optimization -> execution
前端考虑动力学，后端轨迹优化难度降低，轨迹优化是局部的
topology，轨迹优化不能改变路径的拓扑结构
考虑动力学和不考虑动力学的路径在地图中是属于不同的拓扑结构，后端的轨迹优化只能在同拓扑结构内优化平滑？不能跳跃到其余拓扑结构中。
图搜索是已解决的问题，不再考虑。
不满足于机器人是一个质点。
新的需求：路线图中的边是feasible motion connection， 可行的运动过程。
如何产生executable edge：
控制空间的离散sample，a* 等grid search based， control， action， 在某个状态时采取不同的control量， 从而，到达新的状态量。
状态空间离散，sample based， prm , rrt, 先离散采样状态，再产生控制过程去连接两个状态。
从一个状态选取不同的控制量（油门大小，方向盘方向，给系统的输入）车的位置和朝向是状态。
fixed u，T， 一个时间步内控制量u恒定， T之后积分到下一个状态。简单易于实现，没有任务 导向，规划效率低。
从状态S0 到 S1， 分配时间T， 计算过程控制量u, 有任务导向，难implement，效率高。
状态转移方程，从s0转移到s1 的控制过程。
现代控制理论！
幂零矩阵？
state lattice 第一层需要考虑初始状态和速度，第一层的过程不一样，之后的状态转移控制过程都一致，外层间的过程一样。
状态空间采样后，计算可行的连接控制过程。
庞特里亚金极小值原理？
costate 协态变量， 哈密尔顿函数。
final state 的惩罚（误差函数）
+
transition cost 过程代价函数
启发函数:
无碰撞，考虑动力学可行，两个代价函数结合。
dense lattice costs too much time .
prune nodes using grid map.
栅格法剪枝，每格留一个最优（代价最低，control cost, 不只是欧氏距离代价）节点。
控制空间采样离散低效率，加入one shot 状态空间采样（终点）直接求解某已有状态到终点的最优控制问题。
状态空间采样，解（最优）两点边界值问题（O）BVP，形成状态之间的连接，构成搜索树。
无人机的运动规划12个状态量，微分平坦为4个量的规划过程。对姿态只有偏航角需要规划，
线速度角速度都用【x,y,z,theta】的微分表示。
参考论文2011 ICRA
无人机的规划过程：
boundary condition，初始终止 位置，速度，加速度确定
六变量用五次多项式六参数。
多段轨迹连续：
带速度通过via point
速度不能指定，而是用优化方法确定一个最优的速度加速度值。
段数越多，多项式次数可以降低：
代价函数J：
矩阵形式的约束
首位的位置速度加速度加加速度等式约束。
中间点的位置约束
相邻两段轨迹的速度加速度加加速度的连续约束。
fk’(tf) = fk+1 ’ (t0)
典型凸优化问题,二次规划问题
hessian 矩阵对角矩阵
disciplined convex optimization programs
tools :
linear programming;
quadratic programming;
quadratically constrained qp;
second order cone programming;
semidefinite programming;
formulation .
decision variable mapping.
将多项式系数映射到端点物理参数。
重要
solver :
cvx
mosek
ooqp
glpk
numerical stability
全局时间轴太长造成数值计算不稳定，高阶多项式。
方案：每段轨迹归一化到时间0-1， 求出多项式后再进行放缩。
minimum snap 只约束了way point上的约束，中间过程没有约束（障碍物信息不考虑，只优化指标）
硬约束，满足约束subject to 。
软约束，最优化指标函数，倾向于满足约束范围。不一定严格满足。
corridor-based trajectory optimization
栅格化自由空间，栅格膨胀简化。
轨迹连接点处于两个栅格的重叠区域，满足走廊约束。
instant linear constraint：
首位状态约束；中间点约束；连续性约束。
interval linear constraint：
边界约束boundary constraint；
动力学约束 速度加速度连续可达。
problem：
way point 上面满足约束，如何保证全局满足约束？
iteratitvely check 检查轨迹极值是否超出约束，超出则添加中间点使轨迹被压入约束内。
检查极值点是否超出约束？
多项式求极值问题，伴随矩阵的秩 求多项式根。
eigenvalue.
贝塞尔曲线，way point 作为控制点，全局满足约束 bernstein.
一定起始于起点，终止于最后一个点，中间控制点不通过。
贝塞尔曲线的凸包性质，贝塞尔曲线各段，都在控制点构成的凸包内，因此不会发生碰撞。
位置贝塞尔，速度贝塞尔曲线。
边界约束；
连续性约束，前后段的尾首控制点相同，则连续。
稠密约束
硬约束：障碍物和自由空间两种平均分布的空间，自由空间内没有障碍物距离概念，对感知反馈的噪音敏感。
软约束：考虑到障碍物的距离
vision based drone：
感知范围有限，有噪音。
多路径，最优路径的选择
markov decision process 马尔科夫决策过程
目前的假设：
执行是完美无误差的，没有扰动
完美的反馈，状态估计准确
实际上：存在uncertainty
cost function is a metrics that evaluate all possible plans(or paths)
S / X state space
A / U action space
P state transition function under probabilistic model
R / L immediate reward / negative one-step cost
theta: nature 的action, 隐式函数或者显式定义值。
l：cost function， 简单定义为 欧式距离
worst case analysis for nondeterministic model
最小化最差情况代价
expected case analysis for probabilistic model
最小化期望代价
计算代价（cost to go ）需要小于等于实际代价
decision making, 和强化学习。
不确定性的规划中，在机器人在某确定状态下实施动作，由于nature 的动作影响，其输出状态不确定。
minimize (max (cost to go)) of all states.
状态维度爆炸，难直接求解
在Uk集合中选择最好的动作，使得Xi 在最坏的θ 环境动作下，最小化cost到达Xf.
robust to uncertainty
overly pessimistic
harder to compute than normal paths
probabilistic model
图搜索
随机搜索
凸优化：二次规划
非凸优化：连续二次规划
粒子群优化（非凸，非线性，不连续的）
设定初末状态
设置测量反馈模块
解优化问题求控制量
输入控制量控制系统
控制量输入nominal system（优化的系统模型），产生了参考理想轨迹（没有外界扰动），真实系统通过一个associate controller 来处理反馈量和外界扰动，跟随理想轨迹。
