机器人中的数值优化（十六）—— 约束优化的应用：控制分配问题、碰撞距离计算、非线性MPC

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

---

---

  

   二十五、控制分配问题

   控制分配对于冗余驱动的系统的含义可以通过以下例子进行理解，一般来说，四旋翼就可以使得飞机产生一个总的推力和三轴力矩，但是有时候为了安全，会采用六旋翼或八旋翼，那如何使用这六个或八个旋翼产生一个总的推力和三轴力矩就是控制分配问题。

   对于上图中四个旋翼的情况，当其转速等比例增大时，向上的推力就会变大，四个旋翼转速等比例减小时，推力就会减小。再比如，我们可以把后两个旋翼的转速增大，前两个旋翼的转速减小，维持总推力不变，使其绕y轴旋转，进行姿态变换，如上面第一个四旋翼示意图所示，同理，我们可以实现绕x轴进行旋转，如上面第二个四旋翼示意图所示。我们可以把某对角线上的两个转向相同的旋翼的转速增大，而另一个对角线上的两个转向相同的旋翼的转速减小，来实现绕z轴旋转，如上面的第三个四旋翼示意图所示。

   当我们有更多个旋翼时，如何实现以上的姿态变换，也就是控制分配问题，即如何计算出每个旋翼的转速，来得到期望的总推力和三轴力矩，我们可以采用优化的方法来进行计算。

   假设第n个电机在水平面上与x轴的夹角（逆时针）为${\varphi }_{\mathbf{N}}$，与中心原点的距离为$r_{\mathbf{N}}$，我们用${\eta }_i$表示该旋翼是否可正常使用，若可正常使用，则其值为1，若发生故障，则其值为0。电机的推力和转速的平方成正比，$\mu$是跟空气动力相关的常数项，所以某个旋翼的推力$F_i$可表示为下式

   $$F_i={\eta }_i\cdot \mu \cdot {\omega }_i^2$$

   某个旋翼沿着z轴产生的力矩也与转速的平方成比例关系，$\kappa$是与产生力矩相关的空气动力学常数，力矩与转速反向，用$\mathrm{sign}({\omega }_i)$表示他的转向，所以某个旋翼的力矩$T_i$可表示为下式

   $$T_i=-{\eta }_i\cdot \mathrm{sign}({\omega }_i)\cdot \kappa \cdot {\omega }_i^2$$

   因此，实际总推力和横摇/俯仰/偏航力矩计算如下：

   [ f t o t a l τ r o l l τ p i t c h τ y a w ] = [ t 1 ⋯ t N l 1 ⋯ l N m 1 ⋯ m N n 1 ⋯ n N ] [ ω 1 2 ⋮ ω N 2 ] \begin{bmatrix}f_\mathrm{total}\\\tau_\mathrm{roll}\\\tau_\mathrm{pitch}\\\tau_\mathrm{yaw}\end{bmatrix}=\begin{bmatrix}t_1&\cdots&t_N\\l_1&\cdots&l_N\\m_1&\cdots&m_N\\n_1&\cdots&n_N\end{bmatrix}\color{red}{\begin{bmatrix}\omega_1^2\\\vdots\\\omega_N^2\end{bmatrix}} ​ftotal​τroll​τpitch​τyaw​​ ​= ​t1​l1​m1​n1​​⋯⋯⋯⋯​tN​lN​mN​nN​​ ​ ​ω12​⋮ωN2​​ ​

   其中，上式中的常数矩阵中的元素计算方式如下：

   $$\begin{array}{rlrl}& t_i& & ={\eta }_i\cdot \mu \\ & {\mathbf{l}}_i& & ={\eta }_i\cdot \mu \cdot r_i\cdot \sin ({\phi }_i)\\ & m_i& & ={\eta }_i\cdot \mu \cdot r_i\cdot \cos ({\phi }_i)\\ & n_i& & =-{\eta }_i\cdot \kappa \cdot \mathrm{sign}({\omega }_i)\end{array}$$

   所以，该优化问题的目标是使得推力、力矩的实际值与期望值的偏差尽可能的小，即使得下式的值尽可能的小

   ∥ [ f ˉ t o t a l τ ˉ r o l l τ ˉ p i t c h τ ˉ y a w ] − [ t 1 ⋯ t N l 1 ⋯ l N m 1 ⋯ m N n 1 ⋯ n N ] [ ω 1 2 ⋮ ω N 2 ] ∥ W 2 \left\|\begin{bmatrix}\color{red}{\bar{f}_\mathrm{total}}\\\color{red}{\bar{\tau}_\mathrm{roll}}\\\color{red}{\bar{\tau}_\mathrm{pitch}}\\\color{red}{\bar{\tau}_\mathrm{yaw}}\end{bmatrix}-\begin{bmatrix}t_1&\cdots&t_N\\l_1&\cdots&l_N\\m_1&\cdots&m_N\\n_1&\cdots&n_N\end{bmatrix}\color{blue}{\begin{bmatrix}\omega_1^2\\\vdots\\\omega_N^2\end{bmatrix}}\right\|_W^2 ​ ​fˉ​total​τˉroll​τˉpitch​τˉyaw​​ ​− ​t1​l1​m1​n1​​⋯⋯⋯⋯​tN​lN​mN​nN​​ ​ ​ω12​⋮ωN2​​ ​ ​W2​

   我们优化的变量是转速的平方，电机出厂时即确定了其上下限，即其需要满足下式

   $${\omega }_{\min }^2\le {\omega }_i^2\le {\omega }_{\max }^2,\forall i\in \{1,\dots ,N\}$$

   同时，我们要减少旋翼速度的不连续跳变，即使得上一下转速与下一次转速之间的跳变尽可能的小，即使得下式的值尽可能的小

   ∥ [ ω ˉ 1 2 ⋮ ω ˉ N 2 ] − [ ω 1 2 ⋮ ω N 2 ] ∥ 2 \left\|\color{red}{\begin{bmatrix}\bar{\omega}_1^2\\\vdots\\\bar{\omega}_N^2\end{bmatrix}}-\color{blue}{\begin{bmatrix}\omega_1^2\\\vdots\\\omega_N^2\end{bmatrix}}\right\|^2 ​ ​ωˉ12​⋮ωˉN2​​ ​− ​ω12​⋮ωN2​​ ​ ​2

   此外，推力和力矩还需要满足一些安全约束，如下式所示：

   A [ t 1 ⋯ t N l 1 ⋯ l N m 1 ⋯ m N n 1 ⋯ n N ] [ ω 1 2 ⋮ ω N 2 ] ≤ b A\begin{bmatrix}t_1&\cdots&t_N\\l_1&\cdots&l_N\\m_1&\cdots&m_N\\n_1&\cdots&n_N\end{bmatrix}\color{blue}{{\begin{bmatrix}\omega_1^2\\\vdots\\\omega_N^2\end{bmatrix}}} \leq b A ​t1​l1​m1​n1​​⋯⋯⋯⋯​tN​lN​mN​nN​​ ​ ​ω12​⋮ωN2​​ ​≤b

   设当前某个旋翼的转速平方为${\bar{\omega }}_i^2$，则其迭代式可表示成下式，其中${\nu }_i$是增量，也是经过下式转换后的新的优化变量

   $${\omega }_i^2={\bar{\omega }}_i^2+{\nu }_i$$

---

   因此，该问题最终变为求取${\nu }_i$使得下式最小，即使得推力、力矩的实际值与期望值之间的偏差尽可能小，同时使得两次迭代间转速的变化尽可能得小

   min ⁡ ν 1 , … , ν N ∥ [ f ˉ t o t a l τ ˉ r o l l τ ˉ p i t c h τ ˉ y a w ] − [ t 1 ⋯ t N l 1 ⋯ l N m 1 ⋯ m N n 1 ⋯ n N ] [ ω ˉ 1 2 + ν 1 ⋮ ω ˉ N 2 + ν N ] ∥ W 2 + ρ ∥ [ ν 1 ⋮ ν N ] ∥ 2 \min_{\nu_1,\ldots,\nu_N}\left\|\begin{bmatrix}\bar{f}_\mathrm{total}\\\bar{\tau}_\mathrm{roll}\\\bar{\tau}_\mathrm{pitch}\\\bar{\tau}_\mathrm{yaw}\end{bmatrix}-\begin{bmatrix}t_1&\cdots&t_N\\l_1&\cdots&l_N\\m_1&\cdots&m_N\\n_1&\cdots&n_N\end{bmatrix}\begin{bmatrix}\bar{\omega}_1^2+\nu_1\\\vdots\\\bar{\omega}_N^2+\nu_N\end{bmatrix}\right\|_W^2+\rho\left\|\begin{bmatrix}\color{green}\nu_1\\\color{green}\vdots\\\color{green}\nu_N\end{bmatrix}\right\|^2 ν1​,…,νN​min​ ​ ​fˉ​total​τˉroll​τˉpitch​τˉyaw​​ ​− ​t1​l1​m1​n1​​⋯⋯⋯⋯​tN​lN​mN​nN​​ ​ ​ωˉ12​+ν1​⋮ωˉN2​+νN​​ ​ ​W2​+ρ ​ ​ν1​⋮νN​​ ​ ​2

   所求的${\nu }_i$还需要满足以下不等式约束

   [ ω m i n 2 − ω ˉ 1 2 ⋮ ω m i n 2 − ω ˉ N 2 ] ≤ [ ν 1 ⋮ ν N ] ≤ [ ω m a x 2 − ω ˉ 1 2 ⋮ ω m a x 2 − ω ˉ N 2 ] A [ t 1 ⋯ t N l 1 ⋯ l N m 1 ⋯ m N n 1 ⋯ n N ] [ ω ˉ 1 2 + ν 1 ⋮ ω ˉ N 2 + ν N ] ≤ b \begin{aligned}&\begin{bmatrix}\omega_{\mathrm{min}}^2-\bar{\omega}_1^2\\\vdots\\\omega_{\mathrm{min}}^2-\bar{\omega}_N^2\end{bmatrix}\leq\begin{bmatrix}\color{green}\nu_1\\\color{green}\vdots\\\color{green}\nu_N\end{bmatrix}\leq\begin{bmatrix}\omega_{\mathrm{max}}^2-\bar{\omega}_1^2\\\vdots\\\omega_{\mathrm{max}}^2-\bar{\omega}_N^2\end{bmatrix}\\&A\begin{bmatrix}t_1&\cdots&t_N\\l_1&\cdots&l_N\\m_1&\cdots&m_N\\n_1&\cdots&n_N\end{bmatrix}\begin{bmatrix}\bar{\omega}_1^2+\color{green}\nu_1\\\vdots\\\bar{\omega}_N^2+\color{green}\nu_N\end{bmatrix}\leq b\end{aligned} ​ ​ωmin2​−ωˉ12​⋮ωmin2​−ωˉN2​​ ​≤ ​ν1​⋮νN​​ ​≤ ​ωmax2​−ωˉ12​⋮ωmax2​−ωˉN2​​ ​A ​t1​l1​m1​n1​​⋯⋯⋯⋯​tN​lN​mN​nN​​ ​ ​ωˉ12​+ν1​⋮ωˉN2​+νN​​ ​≤b​

   对于以上严格凸的带不等式约束的QP问题，由于优化变量的维度较低（N<10），且严格凸的，使用QP方法求解时，它的精确解可以在线性时间内得到。

---

   二十六、碰撞距离计算问题

   在下图所示的例子中，障碍物由一系列的不规则多边形组成，有效计算碰撞距离是机器人导航的关键功能，那么如何计算机器人与这些障碍物的最近距离呢（也即下图中的蓝色向量）？

   假设如下图所示的障碍物用一系列半空间(不等式)的交去表示（H-representation），这一系列的半空间有可能是冗余的，即存在一些不等式不在障碍物的边界上，如$\begin{array}{c}{x}_1+x_2+x_3\le 1\\ x_1+x_2+x_3\le 2\end{array}$中的第二个式子显然是冗余的。

   我们只需要找到一个与机器人所在位置$x_{robot}$最近的x，其在构成障碍物的这一系列不等式的内部，即满足不等式$A_{\mathcal{P}}x\le b_{\mathcal{P}}$，即该问题可转换为以下QP问题，可以使用之前介绍的方法求解，把每个障碍物都过一遍，既可以找到当前机器人与环境中障碍物的最近距离。

   $$\begin{array}{rl}& \underset{x\in {\mathbb{R}}^d}{\min }\Vert x-x_{\mathrm{robot}}{\Vert }^2\\ & s.t.A_{\mathcal{P}}x\le b_{\mathcal{P}}\end{array}$$

  

。

   假设障碍物不是由H-representation表示的，而是由V-representation表示的，即障碍物是由包含一堆点的最小凸包表示的，这些点中同样存在冗余的点

。

   第一种处理思路是，一个点处于由点集构成的凸包的内部或者边界上时，它一定可以表示成v1~vm的一个凸组合，即

   $$x\in \mathcal{P}:=\mathrm{conv}\{v_1,\dots ,v_m\}$$
   if and only if there are ${\lambda }_i$ and ${\Sigma }_{i=1}^m{\lambda }_i=1$    such that $x={\Sigma }_{i=1}^m{\lambda }_i{v}_i$

   我们可以以${\lambda }_i$为优化变量，来最小化由这一系列${\lambda }_i$构成的x与机器人所在位置$x_{robot}$之间的距离，则该问题转换为以下带等式和不等式约束的优化问题：

   $$\underset{\lambda \in {\mathbb{R}}^m}{\min }\Vert (v_1,\dots ,v_m)\mathbf{\lambda }-x_{\mathrm{robot}}{\Vert }^2$$

   $$s.t.\lambda \ge 0,1^{\mathrm{T}}\lambda =1$$

   这是一个QP问题，它有有m维变量和(m+1)约束。当存在大量冗余顶点时，该QP具有较高的维度，可以采用PHR增广拉格朗日算法进行迭代求解。通过解的稀疏性来精确求解仍然是可能的，但比较复杂。

---

   第二种更高效的处理思路是，找一个支撑平面，分割开机器人与障碍物，因此，我们可以以最大化机器人与该支撑平面的距离为优化目标，来寻求这样一个支撑平面，该支撑平面可用下式表示：

   $$\left\{x\in {\mathbb{R}}^d\mid y^{\mathrm{T}}(x-x_{\mathrm{robot}})\le y^{\mathrm{T}}y\right\}$$

   该优化问题可表示为（为非凸问题）：

   $$\begin{array}{rl}& \underset{y\in {\mathbb{R}}^d}{\max }{y}^{\mathrm{T}}y,\\ & s.t.(v_i-x_{\mathrm{robot}}{)}^{\mathrm{T}}y\ge y^{\mathrm{T}}y,\forall i\in \{1,\dots ,m\}\end{array}$$

   $\text{Notice that if we use }z=y/\left(y^{\text{T}}y\right)\text{or equivalently }y=z/\left(z^{\text{T}}z\right)$，因此，该QP优化问题又可以表示为（为凸问题）：

   $$\begin{array}{rl}& \underset{z\in {\mathbb{R}}^d}{\min }{z}^{\mathrm{T}}z,\\ & s.t.(v_i-x_{\mathrm{robot}}{)}^{\mathrm{T}}z\ge 1,\forall i\in \{1,\dots ,m\}\end{array}$$

   不可行性意味着发生碰撞，否则最优解$x=y+x_{\mathrm{robot}}=z/(z^{\mathrm{T}}z)+x_{\mathrm{robot}}$

---

---

   二十七、非线性模型预测控制

---

   比如自动驾驶中跟踪参考轨迹时，需要同时控制车辆纵向加速度和转向角，也即输入量，状态量选为后轮位置坐标x、y，航向角$\varphi$和速度v

   $$s_{k+1}=F(s_k,u_k):=f(s_k,u_k)\tau +s_k$$

   其中：

   s k = [ x k y k ϕ k v k ] , u k = [ a k δ k ] , τ is a constant duration s_k=\begin{bmatrix}x_k\\y_k\\\phi_k\\v_k\end{bmatrix},u_k=\begin{bmatrix}a_k\\\delta_k\end{bmatrix},\tau\text{is a constant duration} sk​= ​xk​yk​ϕk​vk​​ ​,uk​=[ak​δk​​],τis a constant duration

   在MPC模型预测控制中，给定了每个$x_k$和$y_k$对应的期望值$x_k^{ref}$和$y_k^{ref}$，并希望加速度的变化尽可能的小，转向角的变化尽可能得小，因此可得到以下目标函数：

   $$J(s_1,\dots ,s_N,u_0,\dots ,u_N):=\sum _{k=1}^N[(x_k-x_k^{\mathrm{ref}}{)}^2+(y_k-y_k^{\mathrm{ref}}{)}^2+w_v(a_k-a_{k-1}{)}^2+w_{\delta }({\delta }_k-{\delta }_{k-1}{)}^2]$$
  

   其还需满足下面的不等式约束：

   $$\begin{array}{rlr}& \forall k\in \{0,\dots ,N\}& \\ & a_{\min }\le a_k\le a_{\max }& \\ & {\delta }_{\min }\le {\delta }_k\le {\delta }_{\max }& G(s_k,u_k)\le 0\\ & v_{\min }\le v_k\le v_{\max }& \end{array}$$

   将上面的问题写成带约束优化的形式，如下所示：

   $$\underset{s_1,\dots ,s_N,u_0,\dots ,u_N}{\min }J(s_1,\dots ,s_N,u_0,\dots ,u_N)$$

   $$\begin{array}{rl}s.t.& F(s_k,u_k)=s_{k+1},\forall i\in \{0,\dots ,N\}\\ & G(s_k,u_k)\le 0,\forall i\in \{0,\dots ,N\}\end{array}$$

   路径点较多时，N较大，该优化问题的维度可能比较高，可以用前面介绍的PHR增广拉格朗日方法求解。另一种思路是省去上面等式约束中不必要的部分，即每个$s_k$都可以用前面的s和u表示，$s_k$是$u_0$到$u_k$的函数，所以它必然可以写成$u_0$到$u_N$的函数，代入到目标函数，就可以把优化变量$s_0$到$s_N$消去，如下所示：

   $$\begin{array}{r}\underset{u_{0:N}}{\min }J(s_1(u_{0:N}),\dots ,s_N(u_{0:N}),u_{0:N})\\ s.t.G(s_k(u_{0:N}),u_k)\le 0,\forall i\in \{0,\dots ,N\}\end{array}$$

   上面的优化问题同样可以通过PHR增广拉格朗日方法求取

   效果演示：

---

---

   参考资料：

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

   2、机器人中的数值优化

---