机器人中的数值优化（十九）—— SOCP锥规划应用：时间最优路径参数化(TOPP)

   本系列文章主要是我在学习《数值优化》过程中的一些笔记和相关思考，主要的学习资料是深蓝学院的课程《机器人中的数值优化》和高立编著的《数值最优化方法》等，本系列文章篇数较多，不定期更新，上半部分介绍无约束优化，下半部分介绍带约束的优化，中间会穿插一些路径规划方面的应用实例

---

---

  

   三十一、时间最优路径参数化(TOPP)

   如果我们有一条二阶连续可微的路径q，现在我们想要机器人去跟踪这个路径，需要给这条路径加入时间属性，同时使其满足机器人的动力学约束，路径可由弧长s变量参数化，将路径分割成若干段小路径，我们可以知道每段小路径对应的空间位置，但是缺乏时间属性，TOPP就是在给定的平滑路径$q(s)$上生成时间信息，并使得在满足机器人动力学约束的前提下，路径花费的时间应该尽可能短。

   使用弧长参数s来对路径进行参数化，s的值表征的是距离路径起点所走过的路径距离，在下图的例子中，左下角路径的起点处s=0，右上角路径的终点处s=L，L即该路径的总长度，对于该路径，我们可以用$\frac{\mathrm{d}s}{\mathrm{d}t}$来表征路径上某个位置处的速度，它是连续的，如下图中右下角的曲线所示，同理，可以用$\frac{{\mathrm{d}}^{2}s}{\mathrm{d}{t}^2}$来表征路径上某个位置处的加速度，它可以是不连续的，如下图中左上角的曲线所示。

–

   现在，我们假设把s拉成直线，对于匀加速运动，起点s=0处的速度为$v_0$，终点s=L处的速度为$v_L$，由基础的物理学公式可知 $v_L^2-v_0^2=2aL$，进一步可知:

   $$\underset{L\to 0}{\lim }\frac{V_L^2-V_0^2}{L}=2a$$

   也即$\frac{d{v}^2}{ds}=2a$，我们得到了一个不依赖于时间属性的微分关系，我们以弧长s为参量的话，$v^2$与2a是线性的微分关系，$v^2$可以用${\left(\frac{\mathrm{d}s}{\mathrm{d}t}\right)}^2$表示

   通过类比以上速度和加速度的概念，我们可以得到

   $$\begin{array}{c}a(s)=\frac{{\mathrm{d}}^{2}s}{\mathrm{d}{t}^2},b(s)=(\frac{\mathrm{d}s}{\mathrm{d}t}{)}^2\\ \\ v_s^2-v_0^2=2as\\ \\ b^{\prime }(s)=2a(s)\end{array}$$

   其实，我们想要在一系列的约束下，找一个s与s’的时间最优的轨迹

   我们想要整个轨迹在动力学约束下时间最短，也就是最小化总的时间T，由于我们缺乏时间信息，所以，需要进行换元，将对时间的积分转换成对弧长s的积分，如下式所示：

   $$T={\int }_0^{T}1\mathrm{d}t={\int }_{s(0)}^{s(T)}\frac{1}{\mathrm{d}s/\mathrm{d}t}\mathrm{d}s={\int }_0^L\frac{1}{\mathrm{d}s/\mathrm{d}t}\mathrm{d}s={\int }_0^L\frac{1}{\sqrt{b(s)}}\mathrm{d}s$$

   利用链式求导法则可以得到真实的速度$\frac{\mathrm{d}q}{\mathrm{d}t}$和加速度$\frac{{\mathrm{d}}^{2}q}{\mathrm{d}{t}^2}$，分别与$\frac{\mathrm{d}s}{\mathrm{d}t}$、$\frac{{\mathrm{d}}^{2}s}{\mathrm{d}{t}^2}$的关系，如下所示：

   $$\frac{\mathrm{d}q}{\mathrm{d}t}=q^{\prime }(s)\frac{\mathrm{d}s}{\mathrm{d}t}=q^{\prime }(s)\sqrt{b(s)}$$

   $$\frac{{\mathrm{d}}^{2}q}{\mathrm{d}{t}^2}=q^{\prime \prime }(s){\left(\frac{\mathrm{d}s}{\mathrm{d}t}\right)}^2+q^{\prime }(s)\frac{{\mathrm{d}}^{2}s}{\mathrm{d}{t}^2}=q^{\prime \prime }(s)b(s)+q^{\prime }(s)a(s)$$

   若我们只考虑向前运动的情况，则b(s)≥0应该严格成立，并仅在某个或某几个瞬间为0.

   因此，我们可以将这个简单的TOPP问题表述为：

   $$\begin{array}{rl}\underset{a(s),b(s)}{\min }& {\int }_0^L\frac{1}{\sqrt{b(s)}}\mathrm{d}s\\ s.t.& b(s)\ge 0,\forall s\in [0,L],\\ & b^{\prime }(s)=2a(s),\forall s\in [0,L],\\ & {\Vert q^{\prime }(s)\sqrt{b(s)}\Vert }_{\infty }\le v_{max},\forall s\in [0,L],\\ & {\Vert q^{\prime \prime }(s)b(s)+q^{\prime }(s)a(s)\Vert }_{\infty }\le a_{max},\forall s\in [0,L],\\ & b(0)=b_0,b(L)=b_L.\end{array}$$

   其中${\min }_{a(s),b(s)}{\int }_0^L\frac{1}{\sqrt{b(s)}}\mathrm{d}s$是我们优化的目标函数，即时间最短，$b(s)\ge 0$用于确保s关于t的变化率的平方是非负的，$b^{\prime }(s)=2a(s)$是前面推导出其满足的物理关系式，${\Vert q^{\prime }(s)\sqrt{b(s)}\Vert }_{\infty }\le v_{max}$即任意弧长在s处的速度要满足动力学约束，同理${\Vert q^{\prime \prime }(s)b(s)+q^{\prime }(s)a(s)\Vert }_{\infty }\le a_{max}$任意弧长在s处的加速度要满足动力学约束，$b(0)=b_0,b(L)=b_L$是让s对t变化率的平方在开始和结束时的值是我们人为给定的。

   此外，上式中的蓝色部分都是已知的常数，我们要求的优化变量是a(s)和b(s)这两个凸函数，也即以函数为优化变量的凸问题。

   以函数为优化变量的凸问题，在计算机里面不能处理连续时间的问题，我们需要对其进行离散化，即把路径分割成若干段小路径，在每段小路径上，$a^i$是一个常数，$b^i$是一个线性的函数，如下图所示

   离散化后的数学表达式如下所示：

   $$\begin{array}{rl}{\int }_0^L\frac{1}{\sqrt{b(s)}}\mathrm{d}s& \iff \sum _{k=0}^{K-1}\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}\\ b(s)\ge 0,\forall s\in [0,L]& \iff b^k\ge 0,0\le k\le K\\ b^{\prime }(s)=2a(s),\forall s\in [0,L]& \iff \frac{b^{k+1}-b^k}{s^{k+1}-s^k}=2a^k,0\le k\le K\\ {|q^{\prime }(s)\sqrt{b(s)}\Vert }_{\infty }\le v_{max},\forall s\in [0,L]& \iff {\Vert q^{\prime }(s^k)\sqrt{b^k}\Vert }_{\infty }\le v_{max},0\le k\le K\\ {\Vert q^{\prime \prime }(s)b(s)+q^{\prime }(s)a(s)\Vert }_{\infty }\le a_{max},\forall s\in [0,L]& \iff {\Vert q^{\prime \prime }(s^k)b^k+q^{\prime }(s^k)a^k\Vert }_{\infty }\le a_{max},0\le k\le K\end{array}$$

   我们现在已经把以函数为优化变量的凸优化转换成了以离散序列$a^0$ ~ $a^{k-1}$、$b^0$ ~ $b^{k-1}$为优化变量的优化问题，其约束都是线性的等式或不等式。

   $$\underset{a^k,b^k}{\min }\boxed{\sum _{k=0}^{K-1}\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}}$$

   $$\begin{array}{rlr}& b^k\ge 0,& 0\le k\le K\\ & b^{k+1}-b^k=2(s^{k+1}-s^k)a^k,& 0\le k\le K\\ & {\Vert q^{\prime }(s^k)\sqrt{b^k}\Vert }_{\infty }\le v_{max},& 0\le k\le K\\ & {\Vert q^{\prime \prime }(s^k)b^k+q^{\prime }(s^k)a^k\Vert }_{\infty }\le a_{max},& 0\le k\le K\end{array}$$

   $$b(0)=b_0,b(L)=b_L.$$

   那么。我们如何将以上问题转化为锥规划问题呢？可以对优化的目标函数进行如下的转换：

   至此，我们得到了该问题的SOCP形式如下：

   $$\begin{array}{rlr}& \underset{a^k,b^k,c^k,d^k}{\min }\sum _{k=0}^{K-1}2(s^{k+1}-s^k)d^k& \\ & {\Vert \begin{array}{c}2\\ c^{k+1}+c^k-d^k\end{array}\Vert }_2\le c^{k+1}+c^k+d^k,0\le k\le K-1& \\ & & \\ & {\Vert \begin{array}{c}2c^k\\ b^k-1\end{array}\Vert }_2\le b^k+1,& 0\le k\le K.\end{array}$$

   $$\begin{array}{rlr}& b^k\ge 0,& 0\le k\le K\\ & b^{k+1}-b^k=2(s^{k+1}-s^k)a^k,& 0\le k\le K\\ & {\Vert q^{\prime }(s^k)\sqrt{b^k}\Vert }_{\infty }\le v_{max},& 0\le k\le K\\ & {\Vert q^{\prime \prime }(s^k)b^k+q^{\prime }(s^k)a^k\Vert }_{\infty }\le a_{max},& 0\le k\le K\end{array}$$

   $$b(0)=b_0,b(L)=b_L.$$

–

–

–

---

   参考资料：

   1、数值最优化方法（高立 编著）

   2、机器人中的数值优化

---