问题提出
存在的问题
基于minimum snap生成的轨迹，我们控制的是waypoint，但是在轨迹生成的时候可能出现“overshoot”的现象，产生碰撞。
解决思路
通常将第一种情况变成【软约束】的优化问题，将第二种情况变成【硬约束】的优化问题
硬/软约束的概念
在整个x的定义域内，都要【严格满足等式约束和不等式约束】
\[\begin{matrix}\min&\quad f\left(x\right)\\ s.f.&\quad g_i\left(x\right)=c_i,\quad i=1,\cdots,n\quad\text{Equality constants}\\ &\quad h_j\left(x\right)\ge d_j,\quad j=1,\cdots,n\quad\text{ bluequality constraints}\end{matrix}
\]
将等式约束和不等式约束融入了目标函数中
\[\text{min}\quad f(x)+\lambda_1\cdot g(x)+\lambda_2\cdot h(x)
\]
使结果倾向于满足约束条件，而不是严格被满足，\(\lambda_1\cdot g(x)+\lambda_2\cdot h(x)\)被称为惩罚项/损失函数(loss function)，有很多种常见的loss function，比如:
硬约束
Corridor based Trajectory Optimization
框架
- 将环境信息载入八叉树地图
- 用path-finding的方法，在地图上搜索出一条安全的路径（安全走廊）
- 将安全走廊在地图中进行膨胀
- 在膨胀后的安全走廊中进行轨迹生成
轨迹生成
- 分段多项式轨迹函数与minimum snap和minimum jerk是一样的：
\[f_\mu(t)=\begin{cases} \sum_{j=0}^N p_{1J}(t-T_0)^J&T_0\leq t\leq T_1\\ \sum_{J=0}^N p_{2J}(t-T_1)^J&T_0\leq t\leq T_1\\ \vdots&\vdots&\vdots\\ \sum_{J=0}^N p_{MJ}(t-T_{M-1})^J&T_0\leq t\leq T_1\end{cases}
\]
\[J=\sum_{\mu\in\{x,y,z\}}\int_{0}^{T}\left(\frac{d^{k}f_{\mu}(t)}{d t^{k}}\right)^{2}d t
\]
由于我们是在安全走廊里进行优化的，并没有指定waypoint，因此waypoint的导数约束是没有的，取而代之的是安全性约束，具体的约束有：
- Instant linear constraints::
起点和终点的状态约束(\(Ap=b\))
Transition point 的约束(\(Ap=b,Ap≤b\))
我们希望在每个凸包内生成的轨迹都是被bound在凸包内的，但是这是一件比较困难的事情，可以先这样做，将分段的轨迹的连接点(Transition point)，bound在两个凸包的公共区域内。
但是这样做也是不够的，轨迹也可能落在凸包外面，安全性不完备
连续性约束(\(Ap_i=b_i\))
- Interval linear constraints:
边界约束(\(\textbf{A}(t)\textbf{p}\leq\textbf{b},\forall t\in[t_l,t_r]\))，整条轨迹的边界约束是很难施加的，除非将T离散化非常细，然后进行约束，但是这样会大大增加凸优化问题的约束数量，导致求解非常困难，通常采取后验的方式
动力学约束(\(\textbf{A}(t)\textbf{p}\leq\textbf{b},\forall t\in[t_l,t_r]\))，和边界约束同样的问题，难以施加，通常采取后验的方式
- 优点：效率非常高，质量非常高，由于没有了waypoint这种fixed的点，优化的自由度被大大增加了，能够自动找到最优的Transition point，而且可以隐式的调节时间分配的问题，在fixed的waypoint的时候如果T分配不好，得到的轨迹也不是很好，而现在point是可以在凸包交集里动的，因此可以隐式的调节时间分配带来的影响
- 问题，全局边界和动力学的interval的约束是很难施加的
- 解决方法：
在轨迹生成之后做检查，不需要将t离散化逐个检查，检查极值即可，如果超过边界，继续施加新的约束，将极值往下压，可以收紧一些约束，然后迭代检查，约束，求解，需要多次求解QP，对在线求解速度施加了压力，而且有没有解，都需要进行多次迭代才能看出。
Bezier Curve Optimization
从普通多项式曲线转到贝塞尔曲线
用Bernstein polynomial代替monomial polynomial，贝塞尔曲线
\[\begin{array}{c}{{B_{j}(t)=c_{j}^{0}b_{n}^{0}(t)+c_{j}^{1}b_{n}^{1}(t)+\cdots+c_{j}^{n}b_{n}^{n}(t)=\sum_{i=0}^{n}c_{j}^{i}b_{n}^{i}(t)}}\\ {{b_{n}^{i}(t)={\binom{n}{i}}\cdot t^{i}\cdot(1-t)^{n-i}}}\end{array}
\]
monomial polynomial各阶的系数和Bernstein polynomial各阶系数是一一对应的，\(p = M c\)
\[For 6 order:
\mathbf{M}=\left[\begin{array}{ccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 \\-6 & 6 & 0 & 0 & 0 & 0 & 0 \\15 & -30 & 15 & 0 & 0 & 0 & 0 \\-20 & 60 & -60 & 20 & 0 & 0 & 0 \\15 & -60 & 90 & -60 & 15 & 0 & 0 \\-6 & 30 & -60 & 60 & -30 & 6 & 0 \\1 & -6 & 15 & -20 & 15 & -6 & 1\end{array}\right]
\]
贝塞尔曲线性质
- Endpoint interpolation：贝塞尔曲线总是从第一个控制点开始，在最后一个控制点结束，永远不经过任何其他控制点。
- Convex hull：贝塞尔曲线 𝐵(𝑡) 完全被限制在一组控制点 ci 组成的凸包内，这就解决了之前轨迹不能完备的被凸包bound住的问题！
- Hodograph：贝塞尔曲线的导数仍然是贝塞尔曲线，而且导数的控制点，可以被原函数的控制点线性表示，而且就等于\(𝑛\times(𝑐_{i+1} − 𝑐_{𝑖})\)，这样对导数的控制点的约束，就可以加在原函数上。
- Fixed time interval：每一段曲线的时间间隔一定是在\([0,1]\)之间的，这是他的一个缺点，需要用时间映射来，将0-1映射到T。
利用贝塞尔曲线的凸包性质解决之前的问题
- 把贝塞尔曲线的控制点放在之前的矩形凸包内，这样控制点生成的凸包一定在矩形凸包里，进而曲线被矩形凸包bound住了！不需要将T离散化非常细，然后进行约束，只需要将控制点约束在凸包内，增加约束的个数等于贝塞尔曲线阶数+1，大大减少约束的个数。
- 由于贝塞尔曲线的导数也是贝塞尔曲线，导数也可以被导数的控制点bound在凸包内，而且还可以转化成对曲线的控制点的约束，这样动力学约束也被满足了。
基于贝塞尔曲线的优化
l阶控制点的形式：\(a_{\mu j}^{l,i}\)表示\(\mu\)方向上的第\(j\)条轨迹的\(l\)阶导数的第\(i\)个控制点
\[a_{\mu j}^{0,i}=c_{\mu j}^{i},a_{\mu j}^{l,i}=\frac{n!}{(n-l)!}\cdot(a_{\mu j}^{l-1,i+1}-a_{\mu j}^{l-1,i}),l\geq1
\]
\[a^{l,0}_{\mu j}\cdot s^{(1-l)}_{j}=d^{(l)}_{\mu j}
\]
之前对起点和终点的约束都是将时间t = 0，T带入轨迹使表达式的值等于期望的起点和终点的各阶导数值，由于贝塞尔曲线具有一定经过第一个控制点和最后一个控制点的性质，那么我们就可以将第一个控制点直接配置在起点，将最后一个控制点直接配置在终点，对于速度和加速度的贝塞尔曲线也是一样，直接配置在速度的起点和终点，加速度等也是同理。
\[a_{\mu j}^{\phi,n}\cdot s_j^{(1-\phi)}=a_{\mu,j+1}^{\phi,0}\cdot s_{j+1}^{(1-\phi)},a_{\mu j}^{0,i}=c_{\mu j}^i
\]
同样道理，对于连续性约束，可以直接将前一段曲线的最后一个控制点，和下一段轨迹的第一个控制点配置在一起
\[\beta_{\mu j}^{-}\leq c_{\mu j}^{i}\leq\beta_{\mu j}^{+},\mu\in\{x,y,z\},i=0,1,2,...,n
\]
利于贝塞尔曲线的凸包性质，将控制点配置在安全走廊的矩形凸包内，进而曲线也被完全限制在凸包内
\[\begin{array}{l}{{v_{m}^{-}\leq n\cdot(c_{\mu j}^{i}-c_{\mu j}^{i-1})\leq v_{m}^{+},}}\\ {{a_{m}^{-}\leq n\cdot(n-1)\cdot(c_{\mu j}^{i}-2c_{\mu j}^{i-1}+c_{\mu j}^{i-2})/s_{j}\leq a_{m}}}\end{array}
\]
同理利用速度曲线的控制点将速度曲线控制在凸包内，加速度也一样，这样动力学约束也被满足了
至此，优化问题转化成了如下的形式：彻底避免了多次迭代求解的问题！
软约束
在障碍物上加”推力“，使机器人远离障碍物
软约束的动机
在硬约束中，将整个环境分为了有障碍物和没有障碍物两种，在安全区域内，硬约束倾向于将所有的点都看成是等价的，最后生成的轨迹有些部分可能会离障碍物很近，由于控制等会出现误差，导致机器人可能会碰到障碍物，此外硬约束是依靠安全走廊进行的轨迹生成，由于感知等误差，可能会出现安全走廊质量较差的情况。
基于距离的软约束轨迹优化
\[f_{\mu}(t)=\left\{\begin{array}{l l}{{\sum_{j=0}^{N}p_{1j}(t-T_{0})^{j}}}&{{T_{0}\leq t\leq T_{1}}}\\ {{\sum_{j=0}^{N}p_{2j}(t-T_{1})^{j}}}&{{T_{0}\leq t\leq T_{1}}}\\ {{\vdots}}&{{\vdots}}\\ {{\sum_{j=0}^{N}p_{Mj}(t-T_{M-1})^{j}}}&{{T_{0}\leq t\leq T_{1}}}\end{array}\right.
\]
\[\begin{split}J& =J_s+J_c+J_d \\& =\lambda _1J_1(smoothness\ cost)+\lambda _2J_2(collision\ cost)+\lambda _3J_3(Dynamical\ cost)\end{split}
\]
\(\lambda_1,\lambda_2,\lambda_3\)分别是各个惩罚项的权重系数
Smoothness Cost
\[\begin{aligned}J_{s} & =\sum_{\mu \in\{x, y, z\}} \int_{0}^{T}\left(\frac{d^{k} f_{\mu}(t)}{d t^{k}}\right)^{2} d t \\& =\left[\begin{array}{l}\boldsymbol{d}_{F} \\\boldsymbol{d}_{P}\end{array}\right]^{T} \boldsymbol{C}^{T} \boldsymbol{M}^{-T} \boldsymbol{Q} \boldsymbol{M}^{-1} \boldsymbol{C}\left[\begin{array}{l}\boldsymbol{d}_{F} \\\boldsymbol{d}_{P}\end{array}\right]=\left[\begin{array}{l}\boldsymbol{d}_{F} \\\boldsymbol{d}_{P}\end{array}\right]^{T}\left[\begin{array}{ll}\boldsymbol{R}_{F F} & \boldsymbol{R}_{F P} \\\boldsymbol{R}_{P F} & \boldsymbol{R}_{P P}\end{array}\right]\left[\begin{array}{l}\boldsymbol{d}_{F} \\\boldsymbol{d}_{P}\end{array}\right]\end{aligned}
\]
Collision Cost
距离场(ESDF):
沿着轨迹将各个点在距离场(ESDF)中的数值(离最近的障碍物的距离)进行积分，这里的积分是对曲线积分，\(ds\)用\(\left | \left | v(t) \right | \right |\delta (t)\) 近似替代
\[\begin{aligned}J_{c} & =\int_{T_{0}}^{T_{M}} c(p(t)) d s \\& =\sum_{k=0}^{T / \delta t} c\left(p\left(T_{k}\right)\right)\|v(t)\| \delta t, T_{k}=T_{0}+k \delta t\end{aligned}
\]
各个点在距离场中的数值是没法写成关于轨迹的函数的形式，只能将轨迹离散化求出各个点在距离场中的数值接着算出惩罚的大小，然后进行求和， c是设计的惩罚函数，里障碍物越近惩罚越大，图示是一种指数的惩罚函数
Dynamical Cost: 对速度和加速度的惩罚，和碰撞惩罚类似，也是需要离散化再求和
这样的优化问题不是凸优化问题，因为障碍物形状各异，距离场不是一个凸空间，因此只能按照非线性优化的方式，通过目标函数对优化变量\(d_{p\mu}\)求导，再进行数值求解，各项Cost导数如下：
Smoothness Cost
\[\frac{\alpha J_s}{\alpha\boldsymbol d_{p\mu}}=2\boldsymbol d_F^T\boldsymbol R_{FP}+2\boldsymbol d_P^T\boldsymbol R_{PP}
\]
Collision Cost
\[\frac{\alpha J_{c}}{\alpha d_{p,u}}=\sum_{k=0}^{T/\delta t}\left\{\forall_{\mu}c\big(p(T_{k})\big)\|v\|F+c\big(p(T_{k})\big)\frac{v_{\mu}}{\|v\|}{G}\right\}\delta t,\mu\in\{x,y,z\}
\]
尝试推导一下，\(c\left(p\left(T_{k}\right)\right)\|v(t)\| \delta t\)对\(d_{p\mu}\)求导，由于\(\delta t\)是离散步长，为常数，因此没有影响。
\[\frac{\mathrm{d} [c\left(p\left(T_{k}\right)\right)\|v(t)\|]}{\mathrm{d} d_p\mu } = \frac{\mathrm{d} [c\left(p\left(T_{k}\right)\right] }{\mathrm{d} d_p\mu }\|v(t)\|+c\left(p\left(T_{k}\right)\right)\frac{\mathrm{d} \|v(t)\| }{\mathrm{d} d_p\mu }
\]
根据复合函数求导链式法则：
\[\frac{\mathrm{d} [c\left(p\left(T_{k}\right)\right)]}{\mathrm{d} d_p\mu }\|v(t)\| = \frac{\mathrm{d} [c(p(T_k))]}{\mathrm{d} p(T_k)}\times \frac{\mathrm{d} p(T_k)}{\mathrm{d} d_{p\mu }}\times \|v(t)\|
\]
\(\frac{\mathrm{d} c(p(T_k))}{\mathrm{d} p(T_k)}\)是碰撞惩罚函数\(c\)在\(\mu\)方向上的梯度，记为\(\forall _\mu c(\cdot)\)
\(p(T_k)\)是\(T_k\)时刻的位置，下式中的\(p_i\)是轨迹参数
\[\begin{split}
p(T_k)
& = \left [T_k^0,T_k^1,\dots ,T_k^n \right ] \left [p_0,p_1,\dots,p_n \right ] ^ T \\
& = \left [T_k^0,T_k^1,\dots ,T_k^n \right ] M^{-1}C\begin{bmatrix}
d_F\\
d_P
\end{bmatrix}
\end{split}
\]
记，\(T= \left [T_k^0,T_k^1,\dots ,T_k^n \right ]\) ，为常量，把 \(M^{-1}C\)写成分块矩阵\(\left [L_{df},L_{dp} \right ]\) ，则
\[\begin{split}p(T_k)& = T \left [ L_{df},L_{dp} \right ] \begin{bmatrix}d_F\\d_P\end{bmatrix}=T(L_{df}d_F+L_{dp}d_P)\end{split}
\]
因为\(TL_{dp}d_P\)是常量，因此
\[\frac{\mathrm{d} p(T_k)}{\mathrm{d} d_{p\mu }} = TL_{dp}=F
\]
所以第一项等于
\[\forall_{\mu}c\big(p(T_{k})\big)\|v\|F
\]
根据复合函数求导链式法则：
\[\begin{split}
c\left(p\left(T_{k}\right)\right)\frac{\mathrm{d} \|v(t)\| }{\mathrm{d} d_p\mu } = c\left(p\left(T_{k}\right)\right) \times \frac{\mathrm{d} \|v\|}{\mathrm{d} v_\mu} \times \frac{\mathrm{d} v_\mu}{\mathrm{d} d_{p\mu}}
\end{split}
\]
\(\|v\| = (v_x^2+v_y^2+v_z^2)^{\frac{1}{2}}\)，\(\mu \in \left \{ x,y,z \right \}\) ,以\(\|v\|\)对\(v_x\)求导为例
\[\frac{\mathrm{d} \|v\|}{\mathrm{d} v_x} = \frac{v_x}{\|v\|}
\]
因此
\[\frac{\mathrm{d} \|v\|}{\mathrm{d} v_\mu} = \frac{v_\mu}{\|v\|}
\]
而
\[\begin{split}
v_\mu
& = \left [T_k^0,T_k^1,\dots ,T_k^n \right ] V_M\left [p_0,p_1,\dots,p_n \right ] ^ T \\
& = \left [T_k^0,T_k^1,\dots ,T_k^n \right ] V_M M^{-1}C\begin{bmatrix}
d_F\\
d_P
\end{bmatrix} \\ &= TV_ML_{dp}d_P \end{split}
\]
因此
\[\frac{\mathrm{d} v_\mu}{\mathrm{d} d_{p\mu }} = TV_ML_{dp}=G
\]
所以第二项等于
\[c\big(p(T_{k})\big)\frac{v_{\mu}}{\|v\|}{G}
\]
综合来看：
\[\frac{\alpha J_{c}}{\alpha d_{p,u}}=\sum_{k=0}^{T/\delta t}\left\{\forall_{\mu}c\big(p(T_{k})\big)\|v\|F+c\big(p(T_{k})\big)\frac{v_{\mu}}{\|v\|}{G}\right\}\delta t,\mu\in\{x,y,z\}
\]
数值优化
优化无约束目标函数
\[\begin{array}{l}\text{minimize}f(x)\end{array}
\]
寻找一个\(x\)序列使其逼近全局最优解
\[f\bigl(x^{(k)}\bigr)\to p^\star
\]
或者使其导数等于0
\[\nabla f(x^{\star})=0
\]
浙公网安备 33010602011771号