本文为PS论文笔记(3)中的动力学基础。
摘要
研究了绳载负载的四旋翼飞行器的动力学与控制问题。
系绳点偏离四旋翼飞行器质心的负载称为偏离中心的悬挂负载。利用 Kane方法的矩阵形式，得到了运动方程。该系统被分解为旋转子系统和平动子系统。通过利用系统的部分线性化，开发了一种非线性控制器来实现四旋翼的镇定。
1 引言
大多数控制器设计仍然局限于系绳点与四旋翼质心重合的构型。我们将该配置表示为居中有效负载配置。在本文中，偏心配置是指系绳点远离质心的情况。对于某些四旋翼无人机，负载配置在质心在机械上是不可行的，因为质心可能位于无人机内部。从而将系绳点固定质心上是非常困难的，此外，精确的坐标测量是困难的，因此一个偏离中心的系绳点是不可避免的。然而，对于偏心系绳构型的非线性控制器的研究还不够全面。主要难点在于偏离中心的载荷运动与四旋翼的旋转运动耦合，使得建模和控制难以推导。针对现有控制器的不足之处，研究了四旋翼飞行器偏心悬索载荷的动力学和控制问题。
本文的贡献在于两部分:利用Kane方法对偏置无人机有效载荷系统进行建模，利用模型分解进行非线性控制设计。
常用的拉格朗日方法是对无人机负载系统进行建模的常用方法。在偏心系绳构型下，载荷运动与四旋翼旋转耦合，载荷的动能包含四旋翼平动速度、旋转速度和载荷摆动速度，这使得拉格朗日法存在复杂性的后续微分。另一方面，利用偏导速度概念和D’Alembert’s原理发展的kane方法避免了对约束方程和系统拉格朗日函数的二阶微分，因此可以在没有符号计算工具箱的情况下用于推导动力学方程的显式形式。最后，凯恩方法的本质是外力和D’Alembert’s惯性力的力平衡在由偏导速度矩阵零空间上的投影，这给我们提供了一种简洁的使用矩阵形式的kane方法来建模存在偏心负载的绳载负载的四旋翼系统的。
在Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load | kumar、Geometric tracking control of a quadrotor UAV on SE(3) |的基础上，提出了一种姿态稳定控制方法，通过改变四旋翼升力矢量的方向来控制其平动和载荷运动。
2 具有偏心绳载负载的四旋翼飞行器动力学模型
A 矩阵形式的kane方程
kane法见Dynamics Theory and Applications – Thomas Kane and David Levinson | Luke Sy, PhD Candidate。
本文中的变量见表1
表1
| V | 机体坐标系 |
|---|---|
| I | 惯性坐标系 |
| ${R_{vi}}$ | 由惯性坐标系转换为机体坐标系的旋转矩阵 |
| $m_q$ | 四旋翼质量 |
| m | 负载质量 |
| $J_v$(3维实矩阵) | 四旋翼转动惯量 |
| L | 缆绳长度 |
| $g_i$ | 惯性坐标系下的重力加速度 |
| $r_v^{tv}$(长度为3的实向量) | 系绳点相对于四旋翼质心的偏移量 |
| $r_v^{pv}$(长度为3的实向量) | 负载在机体坐标系中的位置 |
| $v_v^{pv}$(长度为3的实向量) | 负载在机体坐标系中的速度 |
| ${r_p} = {\left[ {\matrix{ {r_{v,1}^{pv}} & {r_{v,2}^{pv}} } } \right]^T}$ | 负载在机体坐标系中的位置的X Y分量 |
| ${v_p} = {\left[ {\matrix{ {v_{v,1}^{pv}} & {v_{v,2}^{pv}} } } \right]^T}$ | 负载在机体坐标系中的速度的X Y分量 |
| $r_v^{vi}$ | 四旋翼在机体坐标系中的位置 |
| $v_v^{vi}$ | 四旋翼在惯性坐标系中的平动速度 |
| $\omega _v^{vi}$$ | 四旋翼在机体坐标系中的角速度 |
| ${f_v}$ | 在机体坐标系螺旋桨生成的力 |
| ${x_i}$ | 四旋翼在惯性坐标系中的位置 |
| 在机体坐标系螺旋桨生成的力矩 |
基于Dynamics Theory and Applications – Thomas Kane and David Levinson | Luke Sy, PhD Candidate，Implementation of Kane’s Method for a Spacecraft Composed of Multiple Rigid Bodies | aiaa.org，kane的运动方程可以通过引入局部速度矩阵U。注意，对于具有独立坐标的刚体系统，我们可以赋予广义速度来完全描述系统的运动。对于一个有外部力和D’Alembert’s惯性力的系统，kane方法是由以下虚功原理推导出来的:
$$\sum\limits_{k = 1}^N {({{\vec F}_k} + \vec F_k^*) \cdot {{\partial {{\vec v}_k}} \over {\partial {u_r}}} = 0,(r = 1,2,3,...m)} \tag{1}$$其中${{\vec F}_k},\vec F_k^*$,分别为外部力和D’Alembert’s惯性力；${{{\vec v}_k}}$是对应惯性力的速度；${{u_r}}$是广义速度。
${{\vec F}_k},\vec F_k^*,{{{\vec v}_k}}$都在惯性坐标系中表示，我们可以得到基于虚功原理的坐标形式： $$\sum\limits_{k = 1}^N {{{\left( {{{\partial {v_{k,i}}} \over {\partial u_r^T}}} \right)}^T}({F_{k,i}} + F_{k,i}^*) = 0,(r = 1,2,3,...m)} \tag{2}$$其中偏导速度项${{{\partial {v_{k,i}}} \over {\partial u_r^T}}}$为一个矩阵；其下标i表示惯性坐标系。以矢量形式，上面的公式可以转换为：
$$\eqalign{ & \left[ {\matrix{ {{{\left( {{{\partial {v_{1,i}}} \over {\partial u_r^T}}} \right)}^T}} & {...} & {{{\left( {{{\partial {v_{n,i}}} \over {\partial u_r^T}}} \right)}^T}} \cr } } \right]\left[ {\matrix{ {{F_{1,i}} + F_{1,i}^*} \cr {...} \cr {{F_{n,i}} + F_{n,i}^*} \cr } } \right] \cr & = U_r^T({F_i} + F_i^*) = 0,(r = 1,2,3,...m) \cr} \tag{3}$$其中$U_r^T$是一个其中每个元素都是矩阵的块矩阵。对于由n对 D’Alembert’s惯性力、外力和广义速度组成的系统，我们可以得到矩阵形式的运动方程：
$${U^T}({F_i} + F_i^*) = 0 \tag{4}$$其中：
$$\eqalign{ & U = \left[ {\matrix{ {{U_1}} & {{U_2}} & {...} & {{U_m}} \cr } } \right] \cr & {F_i} = \left[ {\matrix{ {F_{1,i}^T} & {...} & {F_{n,i}^T} \cr } } \right],F_i^* = \left[ {\matrix{ {F_{1,i}^{*,T}} & {...} & {F_{n,i}^{*,T}} \cr } } \right] \cr} \tag{5}$$公式(4)表明运动方程是力平衡在偏导速度矩阵零空间上的投影。根据文献Dynamics Theory and Applications – Thomas Kane and David Levinson | Luke Sy, PhD Candidate，刚体k的D’Alembert’s惯性力为:
$$\eqalign{ & F_{k,i}^* = - {m_k}{{\dot v}_{k,i}}, \cr & M_{k,i}^* = - {{\dot h}_{k,i}} = - {R_{ib,k}}({J_{k,b}}\dot \omega _b^{k,i} + \omega _b^{k,i \wedge }{J_{k,b}}\dot \omega _b^{k,i}) \cr} \tag{6}$$其中，$F_{k,i}^*，M_{k,i}^*$为 D’Alembert’s惯性力和惯性力矩；注意，在凯恩的运动方程中，表达惯性力矩的方式与惯性力是一样的。将系统第k个分量从机体坐标系转换到惯性坐标系的旋转矩阵为${R_{ib,k}}$。${J_{k,b}}$为第k个分量的转动惯量。∧的映射如下：
$${\phi ^ \wedge }: = \left[ {\matrix{ 0 & { - {\phi _3}} & {{\phi _2}} \cr {{\phi _3}} & 0 & { - {\phi _1}} \cr { - {\phi _2}} & {{\phi _1}} & 0 \cr } } \right] ,\phi \in {\mathbb{R}^{3}} \tag{7}$$在本文中，∧等价于叉积矩阵算子 ×。∧的逆映射算子定义为∨。
B 绳载负载的四旋翼系统动力学模型
偏心绳载负载的四旋翼系统结构如图1所示，其中参数定义见表一。由图一，可以得到如下的运动学关系：
$$r_i^{pi} = r_i^{vi} + {R_{iv}}(r_v^{tv} + r_v^{pv}) \tag{8}$$对于绳载负载的四旋翼系统，我们使用如下的广义速度u：
$$u = {\left[ {v_p^T,v_v^{vi,T},\omega _v^{vi,T}} \right]^T} \tag{9}$$通过对(8)微分，得到了惯性坐标系中负载的速度和加速度以及四旋翼的速度：
$$\eqalign{ & v_i^{pi} = {R_{iv}}(v_i^{vi} + B{v_p} + A\omega _v^{pv}) \cr & v_i^{vi} = {R_{iv}}v_v^{vi} \cr & \omega _i^{vi} = {R_{iv}}\omega _v^{vi} \cr} \tag{10}$$定义以下负载的诱导交叉项：
$$\eqalign{ & {{\dot R}_{iv}} = {R_{iv}}\omega _v^{vi, \wedge } \cr & A = - {(r_v^{tv} + r_v^{pv})^ \wedge };B = \left[ {\matrix{ {{1_{2 \times 2}}} \cr { - {{r_p^T} \over {\sqrt {{L^2} - r_p^T{r_p}} }}} \cr } } \right] \cr} \tag{11}$$根据(4),其偏导速度矩阵如下所示：
$${U^T} = \left[ {\matrix{ {{B^T}{R_{vi}}} & 0 & 0 \cr {{R_{vi}}} & {{R_{vi}}} & 0 \cr {A{R_{vi}}} & 0 & {{R_{vi}}} \cr } } \right] \tag{12}$$对(10)微分得到四旋翼和负载的加速度以及角动量的变化率$\dot h_i^{vi}$如下：
$$\eqalign{ & \dot v_i^{pi} = {R_{iv}}(\dot v_i^{vi} + B{{\dot v}_p} + A\dot \omega _v^{pv} + R) \cr & \dot v_i^{vi} = {R_{iv}}(\dot v_v^{vi}{\rm{ + }}P){\rm{ }} \cr & \dot h_i^{vi} = {R_{iv}}({J_v}\dot \omega _v^{vi} + Q) \cr} \tag{13}$$定义了下列速度诱导力的量为：
$$\eqalign{ & P = {(\omega _v^{vi})^ \wedge }v_v^{vi} \cr & Q = {(\omega _v^{vi})^ \wedge }{J_v}\omega _v^{vi} \cr & R = {(\omega _v^{vi})^ \wedge }(v_v^{vi} + A\omega _v^{vi} + 2B{v_p}) + \dot B{v_p} \cr} \tag{14}$$D’Alembert’s惯性力与外力的平衡可以用以下形式表示:
$$F_i^* = - \left[ {\matrix{ {m{R_{iv}}B} & {m{R_{iv}}} & {m{R_{iv}}A} \cr 0 & {{m_q}{R_{iv}}} & 0 \cr 0 & 0 & {{R_{iv}}{J_V}} \cr } } \right]\dot u - \left[ {\matrix{ {m{R_{iv}}R} \cr {{m_q}{R_{iv}}P} \cr {{R_{iv}}Q} \cr } } \right] \tag{15}$$ $${F_i} = {\left[ {\matrix{ {m{g_i}} & {{m_q}{g_i}} & 0 \cr } } \right]^T} + {\left[ {\matrix{ 0 & {{F_i}} & {{M_i}} \cr } } \right]^T} \tag{16}$$根据(4)，力平衡的投影为kane运动方程:
$$\eqalign{ & \left[ {\matrix{ {m{B^T}B} & {m{B^T}} & {m{B^T}A} \cr {mB} & {({m_q} + m)1} & {mA} \cr {m{A^T}B} & {m{A^T}} & {{J_v} + m{A^T}A} \cr } } \right]\left[ {\matrix{ {{{\dot v}_p}} \cr {\dot v_v^{vi}} \cr {\dot \omega _v^{vi}} \cr } } \right] = \cr & \left[ {\matrix{ {m{B^T}} \cr {({m_q} + m)1} \cr {m{A^T}} \cr } } \right]{R_{vi}}g + \left[ {\matrix{ {m{B^T}R} \cr { - {m_q}P - mR} \cr { - m{A^T}R - Q} \cr } } \right] + \left[ {\matrix{ {{0_{4 \times 1}}} \cr {{f_v}} \cr {{M_v}} \cr } } \right] \cr} \tag{17}$$3 控制设计
A 分解姿态运动
对于一个正定矩阵C，我们有如下的LDU分解：
$$C{\rm{ = }}\left[ {\matrix{ A & {{B^T}} \cr B & D \cr } } \right]{\rm{ = }}\left[ {\matrix{ 1 & 0 \cr {B{A^{ - 1}}} & 1 \cr } } \right]\left[ {\matrix{ A & 0 \cr 0 & S \cr } } \right]\left[ {\matrix{ 1 & {{A^{ - 1}}{B^T}} \cr 0 & 1 \cr } } \right] \tag{18}$$其中L和U是上三角形矩阵和下三角矩阵；D为一个对角矩阵，其中$S = D - B{A^{ - 1}}{B^T}$。利用LDU分解，可以将姿态运动与系统其他部分解耦。我们按以下方式划分质量矩阵：
$$\eqalign{ & A = \left[ {\matrix{ {m{B^T}B} & {m{B^T}} \cr {m{B^T}} & {({m_q} + m)1} \cr } } \right];B = [\matrix{ {m{A^T}B} & {m{A^T}} \cr } ] \cr & D = {J_v}{\rm{ + }}m{A^T}A;S = D - B{A^{ - 1}}{B^T} \cr} \tag{19}$$解耦后的姿态动力学方程为:
$$\eqalign{ & \dot \omega _v^{vi} = \tau = {S^{ - 1}}( - B{A^{ - 1}}{h_1} + {h_2} + {M_v}) \cr & {h_1} = \left[ {\matrix{ {m{B^T}} \cr {({m_q} + m)1} \cr } } \right]{R_{vi}}{g_i} + \left[ {\matrix{ { - m{B^T}R} \cr { - {m_q}P - mR} \cr } } \right] + \left[ {\matrix{ {{0_{4 \times 1}}} \cr {{f_v}} \cr } } \right] \cr & {h_2} = m{A^T}{R_{vi}}{g_i} - m{A^T}R - Q \cr} \tag{20}$$由上式可知，姿态运动是完全驱动的。定义饱和函数如下:
$$sat(x,{x_0}) = \left\{ \matrix{ {x_0},{x_0} < x \hfill \cr x, - {x_0} \le x \le {x_0},{x_0} > 0 \hfill \cr {x_0},x < - {x_0} \hfill \cr} \right. \tag{21}$$如果x是一个向量，那么饱和函数适用于每个元素。如果$x_0$为∞，则表示该通道没有限制。
在文献Geometric tracking control of a quadrotor UAV on SE(3) 、Nonlinear robust tracking control of a quadrotor UAV on SE(3)的基础上，我们得到以下控制律:
$$\eqalign{ & \tau = - {K_\Omega }\omega _v^{vi} - sat({K_R}{e_R},{x_{R,0}});{x_{R,0}} = {\left[ {\infty ,\infty ,0.1} \right]^T} \cr & {e_R} = {1 \over 2}{(R_{vi,c}^T{R_{vi}} - R_{vi}^T{R_{vi,c}})^ \vee } \cr} \tag{22}$$其中，$R_{vi,c}$为期望的姿态，期望角速度为$\omega _{v,c}^{vi} = 0$；$K_Ω$和$K_R$为正定矩阵。该限制适用于偏航角通道，以防止大偏航角引起的负载大幅度摆动的影响。
B 平移运动控制设计
基于姿态指令控制器，在文献Geometric tracking control of a quadrotor UAV on SE(3) 、Nonlinear robust tracking control of a quadrotor UAV on SE(3)的基础上设计了四旋翼飞行器的平移运动控制器,定义位置和速度误差为:
$${e_x} = {x_i} - {x_{i,d}};{e_v} = v_v^{vi} - v_{v,d}^{vi} \tag{23}$$四旋翼的速度是通将倾斜控制的升力矢量控制在期望的方向实现的。然后将位置控制器定义为
$$\eqalign{ & v_{v,d}^{vi} = - {k_x}{e_x};{R_{vi,c}} = 1 + {k_v}{\left[ {\matrix{ {sat({e_{v,y}},{v_0})} \cr { - sat({e_{v,x}},{v_0})} \cr 0 \cr } } \right]^ \wedge } \cr & {f_v} = {f_{v,e}} - {k_v}({m_q} + m)sat({e_{v,z}},{v_1}) \cr} \tag{24}$$通过在饱和函数中设置足够小的限制$v_0$，可以认为矩阵近似于SO(3)。${f_{v,e}}$即为四旋翼和有效载荷悬停时的纵倾力。由于姿态控制将使四旋翼飞行器最终达到接近$R_{vi}=1$的姿态，因此由$v_0$产生的小倾角使得$v_{v,d}^{vi} \approx v_{i,d}^{vi}$。因此，速度控制信号$v_{v,d}^{vi} = - {k_x}{e_x}$可以将四旋翼飞行器带到目标位置。
