深蓝学院课程《机器人中的数值优化》（主讲：汪哲培博士）最后大作业。
已知一个空间的路径，需要控制机械臂、无人机等去沿着它运动（如下图所示），但如何才能在满足运动约束的条件下最快地完成它呢？这就是我们要研究的问题。
该问题的整体求解思路为：
设路径的表达方式为：随长度s变化的位置向量 q ( s ) q(s) q(s)。随路径变化的速率的平方为 b ( s ) b(s) b(s)，随路径参数变化的加速度大小为 a ( s ) a(s) a(s)。即：
a ( s ) = d 2 s d t 2 a(s)=\frac{\text{d}^2s}{\text{d}t^2} a(s)=dt2d2s， b ( s ) = ( d s d t ) 2 b(s)=(\frac{\text{d}s}{\text{d}t})^2 b(s)=(dtds)2
二者的关系为：
b
′
(
s
)
=
2
a
(
s
)
b'(s)=2a(s)
b′(s)=2a(s)，这是因为中学物理讲过的
v
s
2
−
v
0
2
=
2
a
s
v_s^2 - v_0^2=2as
vs2−v02=2as 。
我们最终的目标是得到时间最优的行进速率，即
b
(
s
)
\sqrt{b(s)}
b(s)
。
优化目标最短时间可写作：
T = ∫ 0 T 1 d t = ∫ 0 L 1 d s / d t d s = ∫ 0 L 1 b ( s ) d s T=\int_{0}^{T}1dt= \int_{0}^{L}\frac{1}{ds/dt} ds = \int_{0}^{L}\frac{1}{\sqrt{b(s)}} ds T=∫0T1dt=∫0Lds/dt1ds=∫0Lb(s) 1ds
实际速度（随时间变化）为：
d q d t = q ′ ( s ) b ( s ) \frac{\mathrm{d} q}{\mathrm{d} t} ={q}' (s)\sqrt{b(s)} dtdq=q′(s)b(s)
实际加速度（随时间变化）为：
d 2 q d t 2 = d ( q ′ ( s ) b ( s ) ) d s d s d t = q ′ ′ ( s ) b ( s ) + q ′ ( s ) a ( s ) \frac{\mathrm{d}^2 q}{\mathrm{d} t^2}=\frac{\mathrm{d} ({q}' (s)\sqrt{b(s)})}{\mathrm{d} s} \frac{\mathrm{d} s}{\mathrm{d} t} =q''(s)b(s)+q'(s)a(s) dt2d2q=dsd(q′(s)b(s) )dtds=q′′(s)b(s)+q′(s)a(s)
然后可以得到关于以函数
b
(
s
)
,
a
(
s
)
b(s),a(s)
b(s),a(s) 为优化变量的优化问题模型：
蓝色为已知量。这个优化问题是凸的。
【为什么式凸的？：凸优化问题即目标函数和约束条件都是凸的。首先说明优化目标函数是凸的：本问题自变量为函数，则优化目标函数是凸的就是在说，任意函数 b 1 ( s ) b_1(s) b1(s)和 b 2 ( s ) b_2(s) b2(s)以及 0 ≤ θ ≤ 1 0 \le \theta \le 1 0≤θ≤1满足:
∫ 0 L 1 θ b 1 ( s ) + ( 1 − θ ) b 2 ( s ) d s ≤ θ ∫ 0 L 1 b 1 ( s ) d s + ( 1 − θ ) ∫ 0 L 1 b 2 ( s ) d s \int_{0}^{L}\frac{1}{\sqrt{\theta b_1(s)+(1-\theta)b_2(s)}}\mathrm{d}s \le \theta \int_{0}^{L}\frac{1}{\sqrt{b_1(s)}}\mathrm{d}s +(1-\theta) \int_{0}^{L}\frac{1}{\sqrt{b_2(s)}}\mathrm{d}s ∫0Lθb1(s)+(1−θ)b2(s) 1ds≤θ∫0Lb1(s) 1ds+(1−θ)∫0Lb2(s) 1ds
由于被积函数大于0，积分可以看作加法，为凸函数，故可把积分去掉，得到其充分条件为：
1 θ x + ( 1 − θ ) y ≤ θ 1 x + ( 1 − θ ) 1 y \frac{1}{\sqrt{\theta x+(1-\theta)y}} \le \theta \frac{1}{\sqrt{x}}+(1-\theta) \frac{1}{\sqrt{y}} θx+(1−θ)y 1≤θx 1+(1−θ)y 1
亦即 1 x \frac{1}{\sqrt{x}} x 1为凸函数。该充分条件显然成立，所以目标函数为凸函数。
约束方面，导数是个线性操作，所以第二个约束是凸的。第四个约束不等号左边是无穷范数和放射变换，它们都是凸的，所以该约束是凸的。对于速度约束，该不等式等价于 ∣ ∣ ( q ′ ( s ) ) 2 b ( s ) ∣ ∣ ∞ ≤ v m a x 2 ||(q'(s))^2b(s)||_\infty \le v_{max}^2 ∣∣(q′(s))2b(s)∣∣∞≤vmax2，是线性的，所以是凸的。注：开根号是个非凸函数，与该约束是凸的并不矛盾，因为“约束是凸的”不是指左边函数是凸的，而是指解集是凸集，“左边函数是凸的”是“解集是凸的”的充分非必要条件。】
我们无法直接处理以连续函数为自变量的优化问题，所以对其进行离散：
进行如上图的离散，对于目标函数和约束分别进行离散可以得到：
【补充解释第一项：上图中离散后的线段 b k b k + 1 b^kb^{k+1} bkbk+1表达式设为： b ( s ) = α s + β , α = b k + 1 − b k s k + 1 − s k b(s) = \alpha s+\beta, \alpha = \frac{b^{k+1}-b^k}{s^{k+1}-s^k} b(s)=αs+β,α=sk+1−skbk+1−bk
则 s k s k + 1 s^ks^{k+1} sksk+1段的积分为：
∫
s
k
s
k
+
1
1
b
(
s
)
d
s
=
2
α
b
(
s
)
1
/
2
∣
s
k
s
k
+
1
=
2
α
(
b
k
+
1
−
b
k
)
=
2
s
k
+
1
−
s
k
b
k
+
1
−
b
k
(
b
k
+
1
−
b
k
)
=
2
s
k
+
1
−
s
k
b
k
+
1
+
b
k
\begin{aligned} \int_{s^k}^{s^{k+1}} \frac{1}{\sqrt{b(s)}} \mathrm{d}s &= \frac{2}{\alpha} b(s)^{1/2}|_{s^k}^{s^{k+1}} \\ &=\frac{2}{\alpha} \left ( \sqrt{b^{k+1}}-\sqrt{b^k} \right )\\ &= 2\frac{s^{k+1}-s^k}{b^{k+1}-b^k} \left ( \sqrt{b^{k+1}}-\sqrt{b^k} \right )\\ &=2\frac{s^{k+1}-s^k}{\sqrt{b^{k+1}}+\sqrt{b^k}} \end{aligned}
∫sksk+1b(s)
1ds=α2b(s)1/2∣sksk+1=α2(bk+1
−bk
)=2bk+1−bksk+1−sk(bk+1
−bk
)=2bk+1
+bk
sk+1−sk
然后求和即可得到】
由此得到优化问题：
思路大概是，通过引入一些变量进行一些紧的放缩，把表达形式凑成SOCP形式。
首先改造目标函数，增加隐变量 c k , d k c^k,d^k ck,dk，其满足：
c k ≤ b k c^k \le \sqrt{b^k} ck≤bk ， 1 c k + 1 + c k ≤ d k \frac{1}{c^{k+1}+c^k} \le d^k ck+1+ck1≤dk。
则可以得到
1
b
k
+
1
+
b
k
≤
d
k
\frac{1}{\sqrt{b^{k+1}}+\sqrt{b^k}} \le d^k
bk+1
+bk
1≤dk，以上每一步等号都可以取到，则
min
1
b
k
+
1
+
b
k
⟺
min
d
k
\min \frac{1}{\sqrt{b^{k+1}}+\sqrt{b^k}} \Longleftrightarrow \min d^k
minbk+1
+bk
1⟺mindk，同时
b
k
,
c
k
,
d
k
b^k,c^k,d^k
bk,ck,dk需要满足上面两个约束。该约束可写作锥的形式（乘开验证即可得到）：
经过转换可以得到该优化问题的SOCP表达形式：
min a k , b k , c k , d k ∑ k = 0 K − 1 2 ( s k + 1 − s k ) d k \min_{a^k,b^k,c^k,d^k} \sum_{k=0}^{K-1}2(s^{k+1}-s^k)d^k minak,bk,ck,dk∑k=0K−12(sk+1−sk)dk ,
∥ 2 c k + 1 + c k − d k ∥ 2 ≤ c k + 1 + c k + d k , ( 0 ≤ k ≤ K − 1 ) \left \| \begin{matrix} 2\\ c^{k+1}+c^k-d^k \end{matrix} \right \| _2 \le c^{k+1}+c^k+d^k, (0 \le k \le K-1) ∥ ∥2ck+1+ck−dk∥ ∥2≤ck+1+ck+dk,(0≤k≤K−1)
∥ 2 c k b k − 1 ∥ 2 ≤ b k + 1 , ( 0 ≤ k ≤ K ) \left \| \begin{matrix} 2c^k\\ b^k-1 \end{matrix} \right \| _2 \le b^k+1, (0 \le k \le K) ∥ ∥2ckbk−1∥ ∥2≤bk+1,(0≤k≤K)
2 ( s k + 1 − s k ) a k + b k − b k + 1 = 0 , ( 0 ≤ k ≤ K − 1 ) 2(s^{k+1}-s^k)a^k+b^k-b^{k+1}=0, (0 \le k \le K-1) 2(sk+1−sk)ak+bk−bk+1=0,(0≤k≤K−1)
∥ q ′ ( s 0 ) ∥ 2 2 b 0 = v s t a r t 2 \left \| q'(s^0) \right \|_2^2 b^0 =v^2_{start} ∥ ∥q′(s0)∥ ∥22b0=vstart2
∥ q ′ ( s k ) ∥ 2 2 b K = v e n d 2 \left \| q'(s^k) \right \|_2^2 b^K=v^2_{end} ∥ ∥q′(sk)∥ ∥22bK=vend2
− b k ≤ 0 , ( 0 ≤ k ≤ K ) -b_k \le 0, (0 \le k \le K) −bk≤0,(0≤k≤K)
( ( q y ′ ( s k ) ) 2 + ( q x ′ ( s k ) ) 2 ) b k − v m a x 2 ≤ 0 , ( 0 ≤ k ≤ K ) \left ( \left ( q'_y(s^k) \right )^2 +\left ( q'_x(s^k) \right )^2 \right ) b^k - v^2_{max} \le 0 , (0 \le k \le K) ((qy′(sk))2+(qx′(sk))2)bk−vmax2≤0,(0≤k≤K)
q x ′ ′ ( s k ) b k + q x ′ ( s k ) a k − a m a x ≤ 0 , ( 0 ≤ k ≤ K − 1 ) q''_x(s^k) b^k + q'_x(s^k) a^k - a_{max}\le 0, (0 \le k \le K-1) qx′′(sk)bk+qx′(sk)ak−amax≤0,(0≤k≤K−1)
q y ′ ′ ( s k ) b k + q y ′ ( s k ) a k − a m a x ≤ 0 , ( 0 ≤ k ≤ K − 1 ) q''_y(s^k) b^k + q'_y(s^k) a^k - a_{max} \le 0, (0 \le k \le K-1) qy′′(sk)bk+qy′(sk)ak−amax≤0,(0≤k≤K−1)
− q x ′ ′ ( s k ) b k − q x ′ ( s k ) a k − a m a x ≤ 0 , ( 0 ≤ k ≤ K − 1 ) -q''_x(s^k) b^k - q'_x(s^k) a^k -a_{max}\le 0, (0 \le k \le K-1) −qx′′(sk)bk−qx′(sk)ak−amax≤0,(0≤k≤K−1)
− q y ′ ′ ( s k ) b k − q y ′ ( s k ) a k − a m a x ≤ 0 , ( 0 ≤ k ≤ K − 1 ) -q''_y(s^k) b^k - q'_y(s^k) a^k -a_{max}\le 0, (0 \le k \le K-1) −qy′′(sk)bk−qy′(sk)ak−amax≤0,(0≤k≤K−1)
使用锥增广拉格朗日方法求解。
增广拉格朗日函数为：
L ( a , b , c , d , λ , μ , σ ) = ∑ k = 0 K − 1 2 ( s k + 1 − s k ) d k + ρ 2 { ∑ i = 1 2 K + 1 ∥ P k i ( μ i ρ − u i ) ∥ 2 + ∑ i = 1 K + 1 ∥ h i ( x ) + λ i ρ ∥ 2 + ∑ i = 1 6 K + 2 ∥ g i ( x ) + σ i ρ ∥ 2 } \mathcal{L}(a,b,c,d,\lambda,\mu,\sigma) = \sum_{k=0}^{K-1}2(s^{k+1}-s^k) d^k + \frac{\rho}{2}\left \{ \sum_{i=1}^{2K+1} \left \| \mathcal{P_{k_i}}\left (\frac{\mu_i}{\rho} -u_i \right ) \right \|^2+ \sum_{i=1}^{K+1} \left \| h_i(x)+\frac{\lambda_i}{\rho} \right \|^2+ \sum_{i=1}^{6K+2} \left \| g_i(x)+\frac{\sigma_i}{\rho} \right \|^2 \right \} L(a,b,c,d,λ,μ,σ)=∑k=0K−12(sk+1−sk)dk+2ρ{∑i=12K+1∥ ∥Pki(ρμi−ui)∥ ∥2+∑i=1K+1∥ ∥hi(x)+ρλi∥ ∥2+∑i=16K+2∥ ∥gi(x)+ρσi∥ ∥2}
其中，第一类锥约束中 u k = [ c k + 1 + c k + d k 2 c k + 1 + c k − d k ] , ( 0 ≤ k ≤ K − 1 ) u_k=\begin{bmatrix} c^{k+1}+c^k+d^k \\ 2 \\ c^{k+1}+c^k-d^k \end{bmatrix} , (0 \le k \le K-1) uk=⎣ ⎡ck+1+ck+dk2ck+1+ck−dk⎦ ⎤,(0≤k≤K−1) ；第二类锥约束中 u k = [ b k + 1 2 c k b k − 1 ] , ( 0 ≤ k ≤ K ) u_k=\begin{bmatrix} b^k+1 \\ 2c^k \\ b^k-1 \end{bmatrix} , (0 \le k \le K) uk=⎣ ⎡bk+12ckbk−1⎦ ⎤,(0≤k≤K)。
增广拉格朗日函数每项的导数分别为：
参数更新和终止条件如ppt所示：
从公式到代码会有很多细节不同，以下仅个人实现过程～
设待优化变量 x = ( b 0 , . . . , b K , a 0 , . . . , a K − 1 , c 1 , . . . , c K , d 0 , . . . , d K ) x=(b^0,...,b^K,a^0,...,a^{K-1}, c^1,...,c^K,d^0,...,d^K) x=(b0,...,bK,a0,...,aK−1,c1,...,cK,d0,...,dK)
根据之前的作业，我们已经得到分段三次多项式轨迹
p
(
t
)
p(t)
p(t)，用它作为本次task的输入路径。在求解该分段轨迹的时候，我们把参数t解释为时间，把导数解释为速度，在求解过程中控制相邻多项式轨迹段端点导数们相等，即速度、加速度相等。而本问题中，要把它视作路径而不是轨迹，所以t不是时间。为减少误解，下文把自变量写作u，即路径
p
(
u
)
p(u)
p(u)。
除此之外参数还已知最大速度、最大加速度，可以是随路径长度变化的曲线。本文为方便，取为恒值。
q ( s ) = p ( u ) q(s) = p(u) q(s)=p(u)，二者函数值相同，自变量不同。为了得到离散的s，首先均匀地离散u，设每个轨迹段被分成r小段，每小段端点是 u k u^k uk对应的点，则整条分段多项式轨迹的小段数 K = r * 多项式个数。然后按照 离散的u => p(u) => q(s) => s 的顺序得到离散的s。需要明确的是这种离散方式得到的s不是等距分布的。具体表达式为：
代码库地址：·gitee上的代码仓库
运行结果：
代码中尚存在的问题：
这项任务耗时远远超过预想，陷入一些思维误区，花了好长时间捋顺思路并调通代码，希望对大家有一些帮助。另外，本人代码水平有限，如果发现本文和代码中有错误，希望可以告知我～如果代码对你有帮助并且你有更好的实现，希望可以分享一下～
