CoNi-MPC: Cooperative Non-inertial Frame Based Model Predictive Control
提出了多机器人协同的系统，可用于在移动的平台上降落、和目标物保持特定的相对运动等任务 直接在目标的坐标系中控制UAV，不对目标的运动做假设 在非惯性系（non-inertial frame）中使用NMPC 需要目标的相对姿态和速度、角速度以及加速度 该框架不需要准确的状态估计、对目标运动模型的先验知识、频繁的重规划
目标的相对姿态和速度可由相对定位方法获得 目标的角速度和加速度可由MEMS IMU传感器获得 主要贡献：
提出了在非惯性系中使用MPC的无人机-目标（drone-target）相对运动框架。不需要目标在世界坐标系中的姿态和运动。 通过相对运动模型，将和目标相关的元素组合在一起并使用目标坐标系中的IMU信息替换。避免了使用世界坐标系中的数据。 MPC控制器作为一个统一的框架支持多种无人机-目标交互的任务。无需频繁的重规划。 其他工作需要全局状态估计和频繁的全局路径重规划，本文的方法可避免这两项
B B B N N N W W W
左侧上标：以该坐标系为参考；右侧下标：该坐标系的物理量
标量 ：小写字母；向量 ：加粗小写字母；矩阵 ：加粗大写字母
⊙ \odot ⊙ [ t ] × [\boldsymbol{t}]_{\times} [ t ] × t \boldsymbol{t} t
四元数的Hamilton乘积，也称为四元数的乘法，是定义在四元数集合上的一种二元运算。
向量的反对称矩阵通常用于表示三维向量的叉积。给定一个三维向量v = ( v 1 , v 2 , v 3 ) \mathbf{v} = (v_1, v_2, v_3) v = ( v 1 , v 2 , v 3 ) A A A A = ( 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ) A = \begin{pmatrix}
0 & -v_3 & v_2 \\
v_3 & 0 & -v_1 \\
-v_2 & v_1 & 0
\end{pmatrix} A = 0 v 3 − v 2 − v 3 0 v 1 v 2 − v 1 0
两个向量u \mathbf{u} u v \mathbf{v} v u × v = A u \mathbf{u} \times \mathbf{v} = A\mathbf{u} u × v = A u A A A v \mathbf{v} v
相对位置 为
N p B = N R W t N B →
{ }^N \boldsymbol{p}_B={ }^N \boldsymbol{R}_W \boldsymbol{t}_{\overrightarrow{N B}}
N p B = N R W t NB 其中t N B → = t B − t N \boldsymbol{t}_{\overrightarrow{N B}}=\boldsymbol{t}_B-\boldsymbol{t}_N t NB = t B − t N N N N B B B
相对速度 为
N p ˙ B = N v B = d d t ( N R W ) t N B → + N R W t ˙ N B → = − [ N Ω N ] × N R W t N B → + N R W t ˙ N B → = − [ N Ω N ] N p B + N R W t ˙ N B →
\begin{aligned}{ }^N \dot{\boldsymbol{p}}_B={ }^N \boldsymbol{v}_B & =\frac{d}{d t}\left({ }^N \boldsymbol{R}_W\right) \boldsymbol{t}_{\overrightarrow{N B}}+{ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_{\overrightarrow{N B}} \\& =-\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{R}_W \boldsymbol{t}_{\overrightarrow{N B}}+{ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_{\overrightarrow{N B}} \\& =-\left[{ }^N \boldsymbol{\Omega}_N\right]{ }^N \boldsymbol{p}_B+{ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_{\overrightarrow{N B}}\end{aligned}
N p ˙ B = N v B = d t d ( N R W ) t NB + N R W t ˙ NB = − [ N Ω N ] × N R W t NB + N R W t ˙ NB = − [ N Ω N ] N p B + N R W t ˙ NB 相对加速度 为
N v ˙ B = − d d t ( [ N Ω N ] × ) N p B − [ N Ω N ] × N p ˙ B − [ N Ω N ] × N R W t ˙ N B → + N R W t ¨ N B → = − d d t ( [ N Ω N ] × ) N p B − [ N Ω N ] × N v B − [ N Ω N ] × ( N v B + [ N Ω N ] × N p B ) + N R W ( t ¨ B − t ¨ N ) = − [ N β N ] × N p B − 2 [ N Ω N ] × N v B − [ N Ω N ] × 2 N p B + N R B B T B + N R W g − N R W a N ⏟ values relying on estimations in W
\begin{aligned}{ }^N \dot{\boldsymbol{v}}_B= & -\frac{d}{d t}\left(\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}\right)^N \boldsymbol{p}_B-\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \dot{\boldsymbol{p}}_B \\& -\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_{\overrightarrow{N B}}+{ }^N \boldsymbol{R}_W \ddot{\boldsymbol{t}}_{\overrightarrow{\mathrm{NB}}} \\= & -\frac{d}{d t}\left(\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}\right)^N \boldsymbol{p}_B-\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{v}_B \\& -\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}\left({ }^N \boldsymbol{v}_B+\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{p}_B\right)+{ }^N \boldsymbol{R}_W\left(\ddot{\boldsymbol{t}}_B-\ddot{\boldsymbol{t}}_N\right) \\= & -\left[{ }^N \boldsymbol{\beta}_N\right]_{\times}{ }^N \boldsymbol{p}_B-2\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{v}_B-\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}^2 { }^N \boldsymbol{p}_B \\& +{ }^N \boldsymbol{R}_B{ }^B \boldsymbol{T}_B+\underbrace{{ }^N \boldsymbol{R}_W \boldsymbol{g}-{ }^N \boldsymbol{R}_W \boldsymbol{a}_N}_{\text {values relying on estimations in } W}\end{aligned}
N v ˙ B = = = − d t d ( [ N Ω N ] × ) N p B − [ N Ω N ] × N p ˙ B − [ N Ω N ] × N R W t ˙ NB + N R W t ¨ NB − d t d ( [ N Ω N ] × ) N p B − [ N Ω N ] × N v B − [ N Ω N ] × ( N v B + [ N Ω N ] × N p B ) + N R W ( t ¨ B − t ¨ N ) − [ N β N ] × N p B − 2 [ N Ω N ] × N v B − [ N Ω N ] × 2 N p B + N R B B T B + values relying on estimations in W N R W g − N R W a N 其中t ¨ B \ddot{t}_B t ¨ B
t ¨ B = W R B B T B + g
\ddot{\boldsymbol{t}}_B={ }^W \boldsymbol{R}_B{ }^B \boldsymbol{T}_B+\boldsymbol{g}
t ¨ B = W R B B T B + g B T B = [ 0 , 0 , T ] ⊤ { }^B \boldsymbol{T}_B=[0,0, T]^{\top} B T B = [ 0 , 0 , T ] ⊤
T = ∑ i T i , i ∈ { 1 , 2 , 3 , 4 } T=\sum_i T_i, i \in\{1,2,3,4\} T = ∑ i T i , i ∈ { 1 , 2 , 3 , 4 }
g = [ 0 , 0 , − g ] ⊤ \boldsymbol{g}=[0,0,-g]^{\top} g = [ 0 , 0 , − g ] ⊤
由B B B N N N
N R B = N R W W R B
{ }^N \boldsymbol{R}_B={ }^N \boldsymbol{R}_W{ }^W \boldsymbol{R}_B
N R B = N R W W R B 则其对时间的导数为
N R ˙ B = d d t ( N R W ) W R B + N R W d d t ( W R B ) = − [ N Ω N ] × N R B + N R B [ B Ω B ] ×
\begin{aligned}{ }^N \dot{\boldsymbol{R}}_B & =\frac{d}{d t}\left({ }^N \boldsymbol{R}_W\right){ }^W \boldsymbol{R}_B+{ }^N \boldsymbol{R}_W \frac{d}{d t}\left({ }^W \boldsymbol{R}_B\right) \\& =-\left[{ }^N \boldsymbol{\Omega}_N\right]_{\times}{ }^N \boldsymbol{R}_B+{ }^N \boldsymbol{R}_B\left[{ }^B \boldsymbol{\Omega}_B\right]_{\times}\end{aligned}
N R ˙ B = d t d ( N R W ) W R B + N R W d t d ( W R B ) = − [ N Ω N ] × N R B + N R B [ B Ω B ] × 下一节会用到的四元数为
N q ˙ B = N q ˙ W ⊙ W q B + N q W ⊙ W q ˙ B = − 1 2 N Ω N ⊙ N q B + 1 2 N q B ⊙ B Ω B
\begin{aligned}{ }^N \dot{\boldsymbol{q}}_B & ={ }^N \dot{\boldsymbol{q}}_W \odot{ }^W \boldsymbol{q}_B+{ }^N \boldsymbol{q}_W \odot{ }^W \dot{\boldsymbol{q}}_B \\& =-\frac{1}{2}{ }^N \boldsymbol{\Omega}_N \odot{ }^N \boldsymbol{q}_B+\frac{1}{2}{ }^N \boldsymbol{q}_B \odot{ }^B \boldsymbol{\Omega}_B\end{aligned}
N q ˙ B = N q ˙ W ⊙ W q B + N q W ⊙ W q ˙ B = − 2 1 N Ω N ⊙ N q B + 2 1 N q B ⊙ B Ω B 此时只有( N R W g − N R W a N ) \left({ }^N \boldsymbol{R}_W \boldsymbol{g}-{ }^N \boldsymbol{R}_W \boldsymbol{a}_N\right) ( N R W g − N R W a N ) N R W g { }^N \boldsymbol{R}_W \boldsymbol{g} N R W g N a N ( = N R W a N ) { }^N \boldsymbol{a}_N\left(={ }^N \boldsymbol{R}_W \boldsymbol{a}_N\right) N a N ( = N R W a N )
MEMS IMU传感器的真实加速度可利用目标坐标系下的负重力计算得到，为
N a N = N R W [ 0 0 − g ] + N [ a ^ x a ^ y a ^ z ]
{ }^N \boldsymbol{a}_N={ }^N \boldsymbol{R}_W\left[\begin{array}{c}0 \\0 \\-g\end{array}\right]+{ }^N\left[\begin{array}{l}\hat{a}^x \\\hat{a}^y \\\hat{a}^z\end{array}\right]
N a N = N R W 0 0 − g + N a ^ x a ^ y a ^ z 其中a ^ x , a ^ y , a ^ z \hat{a}^x, \hat{a}^y, \hat{a}^z a ^ x , a ^ y , a ^ z
N v ˙ B = − [ N β N ] × N p B − 2 [ N Ω ^ N ] × N v B − [ N Ω ^ N ] × 2 N p B + N R B B T B − N a ^ N
\begin{aligned}{ }^N \dot{\boldsymbol{v}}_B= & -\left[{ }^N \boldsymbol{\beta}_N\right]_{\times}{ }^N \boldsymbol{p}_B-2\left[{ }^N \widehat{\boldsymbol{\Omega}}_N\right]_{\times}{ }^N \boldsymbol{v}_B-\left[{ }^N \widehat{\boldsymbol{\Omega}}_N\right]_{\times}^2{ }^N \boldsymbol{p}_B \\& +{ }^N \boldsymbol{R}_B{ }^B \boldsymbol{T}_B-{ }^N \widehat{\boldsymbol{a}}_N\end{aligned}
N v ˙ B = − [ N β N ] × N p B − 2 [ N Ω N ] × N v B − [ N Ω N ] × 2 N p B + N R B B T B − N a N 其中N a ^ N , N Ω ^ N { }^N \widehat{\boldsymbol{a}}_N ,{ }^N \widehat{\boldsymbol{\Omega}}_N N a N , N Ω N
CoNi-MPC ：Cooperative Non-inertial Frame Based Model Predictive Control
系统的状态 为x = [ N p B ; N v B ; N q B ; N a ^ N ; N Ω ^ N ; N β N ] ∈ R 19 \boldsymbol{x}=\left[{ }^N \boldsymbol{p}_B ;{ }^N \boldsymbol{v}_B ;{ }^N \boldsymbol{q}_B ;{ }^N \widehat{\boldsymbol{a}}_N ;{ }^N \widehat{\boldsymbol{\Omega}}_N ;{ }^N \boldsymbol{\beta}_N\right] \in \mathbb{R}^{19} x = [ N p B ; N v B ; N q B ; N a N ; N Ω N ; N β N ] ∈ R 19
角速度的导数满足d d t ( N Ω ^ N ) = N β N \frac{d}{d t}\left({ }^N \widehat{\boldsymbol{\Omega}}_N\right)={ }^N \boldsymbol{\beta}_N d t d ( N Ω N ) = N β N
假设线加速度和角加速度的变化率为0，即∗ ∗ N a ^ ˙ N = 0 , β ˙ N = 0 ∗ ∗ **{ }^N \dot{\hat{\boldsymbol{a}}}_N=0 ,\dot{\boldsymbol{\beta}}_N=0** ∗ ∗ N a ^ ˙ N = 0 , β ˙ N = 0 ∗ ∗
系统的输入 为u = [ T ; B Ω B x ; B Ω B y ; B Ω B z ] ∈ R 4 \boldsymbol{u}=\left[T ;{ }^B \Omega_B^x ;{ }^B \Omega_B^y ;{ }^B \Omega_B^z\right] \in \mathbb{R}^4 u = [ T ; B Ω B x ; B Ω B y ; B Ω B z ] ∈ R 4
定义二次函数形式的成本 为
C ( x , u ) = ∥ x ( t ) − x ( t ) r e f ∥ Q + ∥ u ( t ) − u h ∥ R
\boldsymbol{C}(\boldsymbol{x}, \boldsymbol{u})=\left\|\boldsymbol{x}(t)-\boldsymbol{x}(t)_{r e f}\right\|_{\boldsymbol{Q}}+\left\|\boldsymbol{u}(t)-\boldsymbol{u}_h\right\|_{\boldsymbol{R}}
C ( x , u ) = ∥ x ( t ) − x ( t ) re f ∥ Q + ∥ u ( t ) − u h ∥ R 其中∥ x ∥ M = x ⊤ M x \|\boldsymbol{x}\|_{\boldsymbol{M}}=\boldsymbol{x}^{\top} \boldsymbol{M} \boldsymbol{x} ∥ x ∥ M = x ⊤ M x u h = [ g ; 0 ; 0 ; 0 ] \boldsymbol{u}_h=[g ; 0 ; 0 ; 0] u h = [ g ; 0 ; 0 ; 0 ]
最终的离散优化问题 为
min u 0 , … , u N − 1 ∑ k = 0 N − 1 ( ∥ x ( k ) − x ( k ) r e f ∥ Q + ∥ u ( k ) − u h ∥ R ) + ∥ x ( N ) − x ( N ) r e f ∥ Q f i n a l s.t. x ( 0 ) = x 0 x ( k + 1 ) = f d ( x ( k ) , u ( k ) ) T min ≤ T ≤ T max ∥ B Ω B x ∥ ≤ Ω r p ∥ B Ω B y ∥ ≤ Ω r p ∥ B Ω B z ∥ ≤ Ω y a w
\begin{aligned}
\min _{\boldsymbol{u}_0, \ldots, \boldsymbol{u}_{N-1}} & \sum_{k=0}^{N-1}\left(\left\|\boldsymbol{x}(k)-\boldsymbol{x}(k)_{{ref }}\right\|_{\boldsymbol{Q}}+\left\|\boldsymbol{u}(k)-\boldsymbol{u}_h\right\|_{\boldsymbol{R}}\right) \\
& +\left\|\boldsymbol{x}(N)-\boldsymbol{x}(N)_{{ref }}\right\|_{\boldsymbol{Q}_{{final }}} \\
\text { s.t. } & \boldsymbol{x}(0)=\boldsymbol{x}_0 \\
& \boldsymbol{x}(k+1)=f_d(\boldsymbol{x}(k), \boldsymbol{u}(k)) \\
& T_{\min } \leq T \leq T_{\max } \\
& \left\|^B \Omega_B^x\right\| \leq \Omega_{\mathrm{rp}} \\
& \left\|^B \Omega_B^y\right\| \leq \Omega_{\mathrm{rp}} \\
& \left\|^B \Omega_B^z\right\| \leq \Omega_{\mathrm{yaw}}
\end{aligned}
u 0 , … , u N − 1 min s.t. k = 0 ∑ N − 1 ( ∥ x ( k ) − x ( k ) re f ∥ Q + ∥ u ( k ) − u h ∥ R ) + ∥ x ( N ) − x ( N ) re f ∥ Q f ina l x ( 0 ) = x 0 x ( k + 1 ) = f d ( x ( k ) , u ( k )) T m i n ≤ T ≤ T m a x B Ω B x ≤ Ω rp B Ω B y ≤ Ω rp B Ω B z ≤ Ω yaw 文中把参考轨迹分为两类：
固定点方案（Fixed Point Scheme，Leader and Follower） 固定计划方案（Fixed Plan Scheme，Complex Trajectories） 上层控制器：使用ACADO工具包部署CoNi-MPC控制器，生成控制指令u 0 \boldsymbol{u}_0 u 0 下层控制器：多级PID CoNi-MPC通过多次打靶技术（multiple shooting technique）和Runge-Kutta积分求解优化问题 对非惯性系下的IMU数据进行平均信号滤波（average signal filter） 时间窗口为2s，离散时间步长为0.1s，MPC实际的控制回路实际约10ms N a ^ N , N Ω ^ N , N β N { }^N \widehat{\boldsymbol{a}}_N,{ }^N \widehat{\boldsymbol{\Omega}}_N,{ }^N \boldsymbol{\beta}_N N a N , N Ω N , N β N Q ( i , i ) = 0 , ∀ i = 10 … 19 \boldsymbol{Q}(i, i)=0, \forall i=10 \ldots 19 Q ( i , i ) = 0 , ∀ i = 10 … 19 在仿真和实验中，角加速度很难获取，故在参数估计（estimation）和参考值（reference）中都设置为0 把包含xy平面距离、线速度和角速度的( r , v , ω ) (r, v, \omega) ( r , v , ω )
N v B x = ( N R W t ˙ B ) x − v N v B y = ( N R W t ˙ B ) y − w r N v B z = ( N R W t ˙ B ) z N v ˙ B x = 2 ω ( N R W t ˙ B ) y + ( N R B B T B ) x − ( N a ^ N ) x N v ˙ B y = − 2 ω ( N R W t ˙ B ) x + 2 ω v + ( N R B B T B ) y − ( N a ^ N ) y N v ˙ B z = ( N R B B T B ) z − 9.8
\begin{aligned}{ }^N \boldsymbol{v}_B^x & =\left({ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_B\right)^x-v \\{ }^N \boldsymbol{v}_B^y & =\left({ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_B\right)^y-w r \\{ }^N \boldsymbol{v}_B^z & =\left({ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_B\right)^z \\{ }^N \dot{\boldsymbol{v}}_B^x & =2 \omega\left({ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_B\right)^y+\left({ }^N \boldsymbol{R}_B{ }^B \boldsymbol{T}_B\right)^x-\left({ }^N \widehat{\boldsymbol{a}}_N\right)^x \\{ }^N \dot{\boldsymbol{v}}_B^y & =-2 \omega\left({ }^N \boldsymbol{R}_W \dot{\boldsymbol{t}}_B\right)^x+2 \omega v+\left({ }^N \boldsymbol{R}_B{ }^B \boldsymbol{T}_B\right)^y-\left({ }^N \widehat{\boldsymbol{a}}_N\right)^y \\{ }^N \dot{\boldsymbol{v}}_B^z & =\left({ }^N \boldsymbol{R}_B{ }^B \boldsymbol{T}_B\right)^z-9.8\end{aligned}
N v B x N v B y N v B z N v ˙ B x N v ˙ B y N v ˙ B z = ( N R W t ˙ B ) x − v = ( N R W t ˙ B ) y − w r = ( N R W t ˙ B ) z = 2 ω ( N R W t ˙ B ) y + ( N R B B T B ) x − ( N a N ) x = − 2 ω ( N R W t ˙ B ) x + 2 ω v + ( N R B B T B ) y − ( N a N ) y = ( N R B B T B ) z − 9.8 其余没细看o(￣︶￣ )o
提出了四旋翼无人机在非惯性系中具有线运动和角运动的动力学模型 CoNi-MPC避免在世界坐标系下的全局状态估计 避免依赖先验知识，不需要进行复杂的轨迹重规划 相对状态（姿态和速度）、角速度和加速度可分别由相对定位方法和MEMS IMU传感器获得 
求学之路|Left4Learning
