随着无人机应用的不断深化，无人机正逐渐成为一种空中作业平台，除航拍外还用于完成诸如负载运输^[1]、空中抓取作业等任务^[2–3]。无人机携带负载主要有夹持和吊挂^[4-6]这2种方式。负载运输的应用场景包括快递运送、救援物资投放等。应用场景的复杂性、负载类型的多样性，给无人机的总体设计、机械结构和控制策略带来新的挑战。

对于大重量负载，无人机需要提供更大的升力；对于大尺寸负载，则需要改进无人机的构型以便运输。因此，运输大质量大尺寸负载，无人机的尺寸与质量也会随之增大，并需配备大容量电池^[7]。无人机质量和尺寸的增加不仅会影响其灵活性^[8]，而且会产生更高的制造和维护成本。

针对单架无人机运输大质量大尺寸负载的局限性^[9-10]，有人考虑使用多架无人机配备高精度传感器进行协同搬运，从而在不改变无人机尺度的前提下，有效提高其负载能力^[11]。然而，多无人机在定位、控制和协作等方面存在较多问题，通信带宽的限制是一项重要挑战^[12]。一般来讲，多无人机之间的通信方式包括显式通信和隐式通信2类。显式通信一般用于将控制信号或传感器数据等信息直接传递给其他机器人^[13]，隐式通信是指无人机与环境或其他无人机进行物理交互(如相互之间的交互力)或非物理交互(如视觉观察)的作用，这种方式所需的信息由附着在无人机上的传感器获取。研究最多和最常用的方式是显式通信方式，该方式易于分析，协作效率高，但在通信环境较差的情况下，可能会出现严重的问题。

协作搬运负载是多无人机协同作业应用的一个重要方向，应用发展前景良好，国外开展了大量研究。文献[14]分析了使用一组无人机通过电缆来运输大的有效载荷的问题，该结构的选择是为了在电缆张力影响的情况下保证有效载荷的静态平衡。文献[15]针对2架无人机对大型物体的运输，研究了基于被动力控制的无人机协同控制问题。然而，这些研究方法多采用软绳或电缆进行运输，负载的摆动容易引起无人机的不稳定甚至失控。

为了解决上述多无人机系统通信以及负载摆动带来的问题，本文设计了一种基于硬连接的隐式通信方式的力位混合控制方法。每架无人机与负载通过球形关节硬连接^[16]，该球形关节具有3个运动自由度并进行限位^[17]，使得无人机的姿态动力学和负载的姿态动力学解耦，进而可以通过调整飞行器的姿态以及旋翼产生的推力大小来控制负载的运动。硬连接导致无人机与负载之间存在力的约束，为此采用导纳控制技术^[18]进行力的控制，其思想是通过感知外部的力来生成运动轨迹，调节无人机的运动，实现任意的力交互行为。本文设计的方法如下：1）给定主机期望的运动轨迹，主机和从机与负载硬连接，主机运动对负载产生力的作用，同时负载也对从机产生力的作用，从机通过力估计方法感知负载施加的作用力；2）通过导纳控制技术改变从机的运动轨迹以适应受到的负载的作用力；3）对导纳控制器输出的运动轨迹设计一个外部的控制回路，并通过线性二次型调节器^[19]实现主从机位置姿态的稳定控制，由于存在外部扰动力的作用，所以从机受到较小的力作用时导纳控制器不启动，仅通过位姿控制器实现位姿稳定控制，只有受到较大的力作用时，导纳控制器启动，从机根据受到的力作用进行运动。

1 数学模型 1.1 运动学建模

双无人机与负载之间的硬连接方式如图1所示，为双无人机协同搬运系统。定义坐标系 $ I：\left\{{O}_{I},{X}_{I},{Y}_{I},{Z}_{I}\right\} $ 为地球固连坐标系； $B：\left\{ {{O_{{B_i}}},{X_{{B_i}}},{Y_{{B_i}}},{Z_{{B_i}}}} \right\}$ 为与机身固连的机体坐标系，i表示第i架无人机，原点与飞机质心重合； $ P：\left\{{O}_{P},{X}_{P},{Y}_{P},{Z}_{P}\right\} $ 为与负载重心固连的坐标系。地球固连坐标系、机体坐标系和负载坐标系都满足右手定则。

图2为球形连接关节，外部球壳可以绕内部球头转动。

做出如下假设：1）每个飞行器的机体坐标系 ${B_i}$ 到负载参考坐标系 $P$ 的相对位置是已知的，用向量 $_P{{\boldsymbol{\varUpsilon}} _{P{B_i}}}$ 表示；2） $_P{{\boldsymbol{R}}_{P{B_i}}}$ 表示飞行器机体系 ${B_i}$ 到负载坐标系 $P$ 的旋转矩阵。从而可以得到系统的运动学方程^[20]为

$ {}_{I}{p}_{I{B}_{i}}=_{I}{p}_{IP}+_{I}{{\boldsymbol{R}}}_{IP}\times {}_{P}{{\boldsymbol{\varUpsilon}} }_{P{B}_{i}} $

$ _I{v_{I{B_i}}}{ = _I}{v_{IP}}{ + _P}{\boldsymbol{\omega}} { \times _P}{{\boldsymbol{\varUpsilon}} _{P{B_i}}} $

$ {}_I{\dot v_{I{B_i}}} = {}_I{\dot v_{IP}} + {}_P\dot {\boldsymbol{\omega}} \times {}_P{{\boldsymbol{\varUpsilon}} _{P{B_i}}} + {}_P{\boldsymbol{\omega}} \times ({}_P{\boldsymbol{\omega}} \times {}_P{{\boldsymbol{\varUpsilon}} _{P{B_i}}}) $

式中： $ _I{p_{I{B_i}}} $ 、 $ _I{v_{I{B_i}}} $ 和 $ {}_I{\dot v_{I{B_i}}} $ 分别为第 $i$ 个飞行器在地球固连坐标系中的质心位置、平动速度以及加速度； $ _I{p_{IP}} $ 、 $ _I{v_{IP}} $ 和 $ {}_I{\dot v_{IP}} $ 分别为负载在地球固连坐标系中的质心位置、平动速度以及加速度。由于球形关节的连接，飞行器的旋转运动学与负载的运动学是相互独立的，负载的三维姿态角速度 $_P{\boldsymbol{\omega}}$ 以及姿态角加速度 ${}_P\dot {\boldsymbol{\omega}}$ 则只能通过惯性测量单元（inertial measurement unit，IMU）测量得出。

1.2 动力学建模

本文的双无人机搬运系统的动力学模型通过牛顿–欧拉方程得到。为了方便后续控制方案的设计与研究，本文中系统的平动方程在地球固连坐标系下描述，转动方程在负载坐标系下描述：

$ \left\{ \begin{gathered} {}_I{{\dot v}_{IP}} = \frac{1}{{{m_{{\text{sys}}}}}}{}_I{{\boldsymbol{R}}_{IP}}({}_P{{\boldsymbol{f}}^{{\text{mav}}}} - {}_P{{\boldsymbol{f}}^{{\text{drag}}}}) - {}_Ig \hfill \\ {}_P{{\boldsymbol{f}}^{{\text{mav}}}} = \sum\limits_{i = 1}^N {{}_P{{\boldsymbol{R}}_{P{B_i}}}{}_{{B_i}}{f_i}^{{\text{thrust}}}} \hfill \\ {m_{{\text{sys}}}} = {m_p} + \sum\limits_{i = 1}^N {{m_i}} \hfill \\ \end{gathered} \right. $

式中： $ {}_I{\dot v_{IP}} $ 为负载在机体系下的加速度； ${}_I{{\boldsymbol{R}}_{IP}}$ 为负载坐标系到机体系的旋转矩阵； ${}_P{{\boldsymbol{R}}_{P{B_i}}}$ 为机体系到负载坐标系的旋转矩阵； ${}_P{{\boldsymbol{f}}^{{\text{mav}}}}$ 为所有飞行器对负载施加的总升力； $ {}_{{B_i}}{f_i}^{{\text{thrust}}} $ 为单架无人机产生的升力； ${{\boldsymbol{f}}^{{\text{drag}}}}$ 为与负载速度成比例的阻力，大小取决于负载的空气动力特性； $ {m_{{\text{sys}}}} $ 、 $ {m_{\text{p}}} $ 和 $ {m_i} $ 分别为整个系统、负载和单架无人机的质量。整个系统的旋转动力学在负载的参考坐标系 $P$ 中表示为

$ \left\{ \begin{gathered} {}_P\dot{\boldsymbol {\omega}} = {}_P{{\boldsymbol{I}}_{{\text{sys}}}}^{ - 1}({}_P{{\boldsymbol{\tau}} ^{{\text{mav}}}} - {}_P{\boldsymbol{\omega }} \times {}_P{{\boldsymbol{ I}}_{{\text{sys}}}}{}_P{\boldsymbol{\omega }} - {}_P{{\boldsymbol{\tau }}^{{\text{drag}}}}) \hfill \\ {}_P{{{\boldsymbol{\tau}}}^{{\text{mav}}}} = \sum\limits_{i = 1}^N {{}_P{{\boldsymbol{\varUpsilon}} _{P{B_i}}} \times ({}_P{{\boldsymbol{R}}_{P{B_i}}}{}_{{B_i}}{{\boldsymbol{f}}_i}^{{\text{thrust}}})} \hfill \\ \end{gathered} \right. $

式中： $ {}_P{\boldsymbol{\omega }} $ 和 ${}_P\dot {\boldsymbol{\omega}}$ 分别为负载的加速度和角加速度； ${}_P{{\boldsymbol{\tau}} ^{{\text{mav}}}}$ 为所有飞行器对负载施加的总力矩； ${}_P{{\boldsymbol{\tau }}^{{\text{drag}}}}$ 为与空气动力学相关的干扰力矩； ${}_P{{\boldsymbol{\varUpsilon}} _{P{B_i}}}$ 为第i架无人机到负载的相对位置向量； ${}_P{{\boldsymbol{ I}}_{{\text{sys}}}}$ 为整个系统总的惯性，与负载惯性以及飞行器的质量和位置有关。

2 控制策略

由于主机和从机与负载之间都通过球形关节进行连接且相互之间不存在显示通信，考虑采用基于导纳控制^[21-23]的控制策略，即2架飞行器携带负载悬停在同一高度，向主机发送目标轨迹指令，从机通过感知负载传递的力进行跟随运动，从机的控制策略如图3所示。

2.1 力估计

主机接收指令并带动负载一起运动，会导致负载对从机产生作用力，这个力采用卡尔曼滤波器算法^[24-26]来估计，并确保滤波器是闭环稳定的^[18]。选取状态量 ${{\boldsymbol{\xi} } _{\text{K}}}{\text{ = (}}{\boldsymbol{\xi} } {\text{,}}F{\text{)}}$ ，给定系统为

$ \left\{ \begin{gathered} {{{{\dot{\boldsymbol{ \xi}} }}}_{\text{K}}} = {{\boldsymbol{A}}_{\text{K}}}{{\boldsymbol{\xi} }_{\text{K}}} + {{\boldsymbol{B}}_{\text{K}}}{\boldsymbol{u}} + {{\boldsymbol{W}}_{\text{K}}} \hfill \\ {{\boldsymbol{y}}_{\text{K}}} = {{\boldsymbol{C}}_{\text{K}}}{{\boldsymbol{\xi} }_{\text{K}}} + {{\boldsymbol{V}}_{\text{K}}} \hfill \\ \end{gathered} \right. $

式中： ${{\boldsymbol{\xi} } _{\text{K}}}{\text{ = (}}{\boldsymbol{\xi} } {\text{,}}F{\text{)}}$ 为选取的状态变量， ${{\dot {\boldsymbol{\xi} } }_{\text{K}}}$ 为其导数， ${\boldsymbol{\xi} }$ 为从机三维位置、速度、欧拉角、角速度和角加速度信息，F为估计的力大小， ${\boldsymbol{u} }$ 为输入的力和力矩， $ {{\boldsymbol{A}}_{\text{K}}} $ 、 $ {{\boldsymbol{B}}_{\text{K}}} $ 和 $ {{\boldsymbol{C}}_{\text{K}}} $ 为常数矩阵， ${{\boldsymbol{W}}_{\text{K}}}$ 和 ${\boldsymbol{V}_{\text{K}}}$ 分别为过程噪声和测量噪声协方差矩阵。估计的状态量为

$ \dot {\hat {\;{\boldsymbol{\xi} }_{\text{K}}}} = ({{\boldsymbol{A}}_{\text{K}}} - {{\boldsymbol{K}}_{\text{K}}}{{\boldsymbol{C}}_{\text{K}}}){{\boldsymbol{\hat {\boldsymbol{\xi} } }}_{\text{K}}} + {{\boldsymbol{K}}_{\text{K}}}{{\boldsymbol{y}}_{\text{K}}} + {{\boldsymbol{B}}_{\text{K}}}{\boldsymbol{u}} $

式中： ${\hat {\boldsymbol{\xi} } _{\text{K}}}$ 、 $\dot {\hat{ \;{\boldsymbol{\xi} }_{\text{K}}}}$ 为状态量的估计值及其导数， ${\boldsymbol{K}_{\text{K}}}$ 为卡尔曼增益矩阵。

建立飞行器模型，平动动力学表示为

$ \left\{ \begin{gathered} {}_I{\boldsymbol{\dot p}} = {}_I{\boldsymbol{v}} \hfill \\ {}_I{\boldsymbol{\dot v}} = \frac{1}{m}{{\boldsymbol{R}}_{IB}}\left[ {\begin{array}{*{20}{c}} 0 \\ 0 \\ F \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} 0 \\ 0 \\ g \end{array}} \right] + \frac{1}{m}{}_I{{\boldsymbol{f}}^{{\text{ext}}}} \hfill \\ \end{gathered} \right. $

式中： $ {}_I{\boldsymbol{p}} $ 和 $ {}_I{\boldsymbol{v}} $ 为从机的三维位置和速度向量， $ {}_I{\boldsymbol{\dot p}} $ 和 $ {}_I{\boldsymbol{\dot v}} $ 分别是它们的导数， $ m $ 为从机的质量， $ {{\boldsymbol{R}}_{IB}} $ 为从机机体系到地球固连坐标系的旋转矩阵， $ F $ 为从机旋翼产生的总推力， ${{\boldsymbol{f}} ^{{\text{ext}}}}$ 是施加在无人机上的外力。

转动动力学可以近似表示为

$ \left\{ \begin{gathered} {\boldsymbol{\dot \eta }} = {\boldsymbol{W}}{}_B{\boldsymbol{\omega }} \hfill \\ {{\boldsymbol{I}}_B}{}_B{\boldsymbol{\dot \omega }} + {}_B{\boldsymbol{\omega }} \times {{\boldsymbol{I}}_B}{}_B{\boldsymbol{\omega }} = {}_B{{\boldsymbol{M}}^{{\text{ext}}}} \hfill \\ \end{gathered} \right. $

式中： ${\boldsymbol{\eta}} {\text{ = }}{\left[ {\phi ,\theta ,\psi } \right]^{\text{T}}}$ 为从机的欧拉角， $ {\boldsymbol{\dot \eta }} $ 为角速度， $ {{\boldsymbol{I}}_B} $ 为从机的转动惯量， ${}_B{\boldsymbol{\omega}} {\text{ = }}{\left[ {p,q,r} \right]^{\text{T}}}$ 为从机在机体系下的机体角速度， $ {}_B{\boldsymbol{\dot \omega }} $ 为机体的角加速度， $ {}_B{M^{{\text{ext}}}} $ 为机体所受的外力矩，且

$ {\boldsymbol{W} } = \left[ {\begin{array}{*{20}{c}} 1&{\tan \theta \sin \phi }&{\tan \theta \cos \phi } \\ 0&{\cos \phi }&{ - \sin \phi } \\ 0&{\sin \phi /\cos \theta }&{\cos \phi /\cos \theta } \end{array}} \right] $

在上述模型基础上，卡尔曼滤波的状态变量选择为

$ {{\boldsymbol{\xi }}_{\text{K}}} = \left[ {\begin{array}{*{20}{c}} {{}_I{{\boldsymbol{p}}^{\text{T}}}}&{{}_I{{\boldsymbol{v}}^{\text{T}}}}&{{{\boldsymbol{\eta }}^{\text{T}}}}&{{}_B{{\boldsymbol{\omega }}^{\text{T}}}}&{{}_I{{\boldsymbol{f}}^{{\text{ext}}}}^{{\text{ T}}}}&{{}_B{{\boldsymbol{M}}^{{\text{ext}}}}^{{\text{ T}}}} \end{array}} \right] $

由于外力和外力矩的动力学是未知的，所以假设它们仅由零均值白噪声驱动。测量向量考虑了白噪声的影响，定义为

$ {{\boldsymbol{y}}_{\text{K}}} = \left[ {{}_I {{{\boldsymbol{p}}^{\text{T}}}}\quad{{{\boldsymbol{\eta }}^{\text{T}}}} } \right] $

滤波器的输入向量由姿态和总推力给出^[27]：

$ {\boldsymbol{u}} = \left[ {\begin{array}{*{20}{c}} {{\phi _{{\text{cmd}}}}}&{{\theta _{{\text{cmd}}}}}&{{\psi _{{\text{cmd}}}}}&F \end{array}} \right] $

式中 $ {\phi _{{\text{cmd}}}} $ 、 $ {\theta _{{\text{cmd}}}} $ 和 $ {\psi _{{\text{cmd}}}} $ 分别为输入的滚转角、俯仰角和偏航角指令。

2.2 导纳控制

导纳控制提供了一种飞行器与外界的物理交互方式，即一个位置控制的机械系统，它可以通过感知外部的力来生成一个运动轨迹，调节飞行器的运动，实现任意的交互行为。因此，从机可以看成一个被动运动的质点。这种力和运动间的动态关系可以表示为

$ {\boldsymbol{M}}({}_I{{\boldsymbol{\ddot \varLambda }}_d} - {}_I{{\boldsymbol{\ddot \varLambda }}_r}) + {\boldsymbol{D}}({}_I{{\boldsymbol{\dot \varLambda }}_d} - {}_I{{\boldsymbol{\dot \varLambda }}_r}) + {\boldsymbol{K}}({}_I{{\boldsymbol{\varLambda }}_d} - {}_I{{\boldsymbol{\varLambda }}_r}) = - {}_I{{\boldsymbol{\tilde f}}^{{\text{ext}}}} $

式中： $ {{\boldsymbol{\tilde f}}^{{\text{ext}}}} \in {{\boldsymbol{R}}^3} $ 为卡尔曼滤波器输出的估计力； ${}_I{{\boldsymbol{\varLambda }}_{\text{d}}}{\text{ = (}}{x_{\text{d}}},{y_{\text{d}}},{z_{\text{d}}}) \in {{\boldsymbol{R}}^3}$ 为输入导纳控制器的期望轨迹，可以人为给定； ${}_I{{\boldsymbol{\varLambda }}_{\text{r}}}{\text{ = }}({x_{\text{r}}},{y_{\text{r}}},{z_{\text{r}}}) \in {{\boldsymbol{R}}^3}$ 为导纳控制器在线生成的参考轨迹；对角矩阵 ${\boldsymbol{M}}= {\rm{diag}}\{{M_x}, {M_y},$ ${M_z}\}$ 、 ${\boldsymbol{D}} = {\text{diag}}\{{D_x},{D_y},{D_z}\}$ 、 ${\boldsymbol{K}} = {\text{diag}}\{{K_x},{K_y},{K_z}\}$ 分别与从机的表观惯性、阻尼以及刚度有关^[15]。为了保证从机的运动完全依赖于负载对其施加的力，这里选刚度矩阵 ${\boldsymbol{K}}{\text{ = }}{\boldsymbol{0}}$ ，同时令 ${\dot {\boldsymbol{\varLambda}} _d}$ = ${\ddot {\boldsymbol{\varLambda}} _d}$ =0，这表示如果没有外力的施加，从机将保持其原来的位置。

导纳控制器可以表示为状态空间方程形式，选取状态量 ${{\boldsymbol{\xi}} _{{\text{ad}}}} = ({}_I{\boldsymbol{\varLambda} _{\text{r}}},{}_I{{\boldsymbol{v}}_{\text{r}}})$ 和输入量 ${{\boldsymbol{u}}_{{\text{ad}}}} = ({}_I{{\boldsymbol{\varLambda }}_{\text{d}}},{}_I{{\boldsymbol{\tilde f}}^{{\text{ext}}}})$ ，可以得到：

$ \left\{ \begin{gathered} {}_I{{{\boldsymbol{\dot \varLambda }}}_{\text{r}}}{\text{ = }}{}_I{{\boldsymbol{v}}_{\text{r}}} \hfill \\ {}_I{{{\boldsymbol{\dot v}}}_{\text{r}}}{\text{ = }} - {{\boldsymbol{M}}^{ - 1}}{\boldsymbol{K}}{}_I{{\boldsymbol{\varLambda }}_{\text{r}}} - {{\boldsymbol{M}}^{ - 1}}{\boldsymbol{D}}{}_I{{\boldsymbol{v}}_{\text{r}}} + {{\boldsymbol{M}}^{ - 1}}{\boldsymbol{K}}{}_I{{\boldsymbol{\varLambda }}_{\text{d}}} + {{\boldsymbol{M}}^{ - 1}}{}_I{{{\boldsymbol{\tilde f}}}^{{\text{ext}}}} \hfill \\ \end{gathered} \right. $

2.3 位姿控制器设计

线性二次型调节器（LQR）其控制对象为线性系统，而性能指标为控制输入和对象状态组成的二次型函数，最优解具有统一的表达式，容易求得闭环形式的最优控制，且易于工程实现^[19]。对于单个无人机本身则分别设计位置控制器和姿态控制器。

假设位置控制的状态空间模型为

$ \left\{ \begin{gathered} {{{\boldsymbol{\dot X}}}_P}(t) = {{\boldsymbol{A}}_P}{{\boldsymbol{X}}_P}(t) + {{\boldsymbol{B}}_P}{}_I{{\boldsymbol{U}}_F}(t) \hfill \\ {{\boldsymbol{Y}}_P}(t) = {{\boldsymbol{C}}_P}{{\boldsymbol{X}}_P}(t) \hfill \\ \end{gathered} \right. $

状态量、输入量和输出量分别为

$ \left\{ \begin{gathered} {{\boldsymbol{X}}_P}(t) = \left[ {\begin{array}{*{20}{c}} {x - {x_{\text{d}}}}\;{(x - {x_{\text{d}}})'}\;{y - {y_{\text{d}}}}\;{(y - {y_{\text{d}}})'}\;{z-{z_{\text{d}}}}\;{(z - {z_{\text{d}}})'} \end{array}} \right]{^{\text{T}}} \hfill \\ {}_I{{\boldsymbol{U}}_F}(t) = \left[ {{F_x} \quad {F_y} \quad{F_z} - {m_i}g} \right]{^{\text{T}}} \hfill \\ {{\boldsymbol{Y}}_P}(t) = \left[ {x - {x_{\text{d}}} \quad y - {y_{\text{d}}} \quad z - {z_{\text{d}}}} \right]{^{\text{T}}} \hfill \\ \end{gathered} \right. $

式中： $\left[ {{x_{\text{d}}} \quad{y_{\text{d}}} \quad{z_{\text{d}}}} \right]{{\text{ }}^{\text{T}}}$ 为输入LQR控制器的位置期望， $\left[ {x \quad y \quad z} \right]{^{\text{T}}}$ 为控制器输出的位置， $\left[ {{F_x} \quad {F_y} \quad {F_z}} \right]{^{\text{T}}}$ 为构造的中间控制量，系数矩阵为

$ {{\boldsymbol{A}}_P} = \left[ {\begin{array}{*{20}{c}} 0\;\;\;1\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;1 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \end{array}} \right],{{\boldsymbol{{ B}}}_P} = \left[ {\begin{array}{*{20}{c}} 0&0&0 \\ {1/m}&0&0 \\ 0&0&0 \\ 0&{1/m}&0 \\ 0&0&0 \\ 0&0&{1/m} \end{array}} \right] $

类似的，姿态控制的状态空间模型为

$ \left\{ \begin{gathered} {{{\boldsymbol{\dot X}}}_\Theta }(t) = {{\boldsymbol{A}}_\Theta }{{\boldsymbol{X}}_\Theta }(t) + {{\boldsymbol{B}}_\Theta }{}_B{{\boldsymbol{U}}_{{M}}}(t) \hfill \\ {{\boldsymbol{Y}}_\Theta }(t) = {{\boldsymbol{C}}_\Theta }{{\boldsymbol{X}}_\Theta }(t) \hfill \\ \end{gathered} \right. $

其中状态量 $ {{\boldsymbol{X}}_\Theta }(t) $ 、输入量 $ {}_B{{\boldsymbol{U}}_M}(t) $ 和输出量 $ {{\boldsymbol{Y}}_\Theta }(t) $ 分别表示为

$ \left\{ \begin{gathered} {{\boldsymbol{X}}_\Theta }(t) = \left[ {\phi - {\phi _{\text{d}}},(\phi - {\phi _{\text{d}}})',\theta - {\theta _{\text{d}}},(\theta - \theta )',\psi - {\psi _{\text{d}}},(\psi - {\psi _{\text{d}}})'} \right]{^{\text{T}}} \hfill \\ {}_B{{\boldsymbol{U}}_M}(t) = \left[ {{M_x}\quad{M_y}\quad{M_z}} \right]^{\text{T}} \hfill \\ {{\boldsymbol{Y}}_\Theta }(t) = \left[ {\phi - {\phi _d}\quad\theta - {\theta _d}\quad\psi - {\psi _d}} \right]^{\text{T}} \hfill \\ \end{gathered} \right. $

式中： $\left[ {{\phi _{\text{d}}} \quad {\theta _{\text{d}}} \quad {\psi _{\text{d}}}} \right]{ ^{\text{T}}}$ 为姿态期望， $\left[ {\phi \quad \theta \quad \psi } \right]{ ^{\text{T}}}$ 为控制器输出的姿态， $\left[ {{M_x} \quad {M_y} \quad {M_z}} \right] ^{\text{T}}$ 为构造的中间控制量， ${}_B{\boldsymbol{I}} = {\text{diag}}\left\{ {{I_{xx}},{I_{yy}},{I_{zz}}} \right\}$ 为单个飞行器的惯性矩阵，系数矩阵为

$ {{\boldsymbol{A}}_\Theta } = \left[ {\begin{array}{*{20}{c}} 0\;\;\;1\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;1\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;1 \\ 0\;\;\;0\;\;\;0\;\;\;0\;\;\;0\;\;\;0 \end{array}} \right],{{\boldsymbol{B}}_\Theta } = \left[ {\begin{array}{*{20}{c}} 0&0&0 \\ {1/{I_{xx}}}&0&0 \\ 0&0&0 \\ 0&{1/{I_{yy}}}&0 \\ 0&0&0 \\ 0&0&{1/{I_{zz}}} \end{array}} \right] $

定义LQR的性能指标为

$ {\boldsymbol{J}} = \frac{1}{2}\int {\left[ {{{\boldsymbol{X}}^{\text{T}}}(t){\boldsymbol{QX}}(t) + {{\boldsymbol{U}}^{\text{T}}}(t){\boldsymbol{RU}}(t)} \right]} {\text{ d}t} $

式中Q、R是对状态变量和输入量的加权矩阵。

求解二次型最优控制即找到状态反馈控制律 ${\boldsymbol{U}}(t)$ ,使得性能指标 ${\boldsymbol{J}}$ 取得极小值，可采用Ricatti方程^[28]：

$ {\boldsymbol{\dot K + Q - }}{{\boldsymbol{K}}^{\text{T}}}{\boldsymbol{B}}{{\boldsymbol{R}}^{ - 1}}{{\boldsymbol{B}}^{\text{T}}}{{\boldsymbol{K}}^{\text{T}}}{\boldsymbol{ + KA + }}{{\boldsymbol{A}}^{\text{T}}}{{\boldsymbol{K}}^{\text{T}}}{\boldsymbol{ = }}0 $

可以得到

$ {\boldsymbol{U}}(t) = - {{\boldsymbol{R}}^{ - 1}}{{\boldsymbol{B}}^{\text{T}}}{\boldsymbol{KX}}(t) $

经过分析试验选取使系统性能良好的位置和姿态控制器的Q、R分别为

$ \left\{\begin{gathered} {{\boldsymbol{Q}}_P} = {\text{diag}}\left\{ {92,10,92,10,92,10} \right\} \hfill \\ {{\boldsymbol{R}}_P} = {\text{diag}}\left\{ {1,1,1} \right\} \hfill \\ {{\boldsymbol{Q}}_\Theta } = {\text{diag}}\left\{ {17,3,17,3,17,3} \right\} \hfill \\ {{\boldsymbol{R}}_\Theta } = {\text{diag}}\left\{ {1,1,1} \right\} \hfill \\ \end{gathered} \right.$

3 仿真验证

为了验证本文提出方法的有效性，设计了基于Matlab/Simulink环境下搭建双无人机协同搬运的仿真，系统仿真参数如下：单架飞行器质量（ $ {m_i} $ ）为2.12 kg，负载质量（ $ {m_{\text{p}}} $ ）为1.06 kg，x轴/y轴转动惯量（ ${I_{xx}}/{I_{yy}}$ ）为10.52 kg·m²，z轴转动惯量（ ${I_{zz}}$ ）为0.91 kg·m²，重力加速度（ $ g $ ）为9.81 m/s²，升力系数和阻力系数（ $ {k_{\text{f}}} $ / $ {k_{\text{m}}} $ ）分别为3.07×10⁻⁵和5.33×10⁻⁷。

3.1 单架无人机位姿稳定控制仿真

对于主机和从机分别进行位姿稳定控制实验，验证LQR控制器对于单架飞行器的控制效果，且由文献[21]可知，单独设计导纳控制器不会影响从机内部回路的整体运动学。首先测试位置姿态控制器的性能，无人机初始位置 ${}_I{{\boldsymbol{P}}_0} = {[\begin{array}{*{20}{c}} 0&0&0 \end{array}]^{{\text{ T}}}}{\text{ m}}$ ，前10 s垂直起飞至目标点 ${}_I{{\boldsymbol{P}}_1} = {[\begin{array}{*{20}{c}} 0&0&1 \end{array}]^{{\text{ T}}}}{\text{ m}}$ ，进而到达期望位置点 ${}_I{{\boldsymbol{P}}_{\text{d}}} = {[\begin{array}{*{20}{c}} 3&3&3 \end{array}]^{{\text{ T}}}}{\text{ m}}$ ，同时设定姿态期望为 ${{\boldsymbol{\eta}} _{\text{d}}} = {\left[ {\begin{array}{*{20}{c}} 0&0&0 \end{array}} \right]^{{\text{ T}}}}(^\circ )$ 。飞行轨迹如图4所示，图5为同步的姿态曲线。

由图4和图5可见，采用LQR控制器的无人机能够迅速平稳地飞向目标点并悬停在目标点；与此同时，飞行器能够保持姿态在很小的范围内变化。所以LQR控制器的稳定控制效果较为理想。

3.2 双机协同位姿追踪仿真

当双机进行协同搬运任务时，通过仿真结果验证控制策略的有效性以及控制器的控制性能。

考虑主机以及负载运动对从机的影响，并根据选取的连接关节，假定负载对从机的力作用于从机的质心。若没有外力作用，则前述控制方案简化为底层的位置姿态控制器，但是由于系统内外的干扰，会导致估计的力不为零，所以对于导纳控制器设计一个控制“开关”，只有当估计的力高于某一阈值时，导纳控制器启动，反之，对于较小的力，导纳控制器不启动。

3.2.1 从机受瞬时负载力的仿真

选取从机刚度矩阵 ${\boldsymbol{K}}{\text{ = }}{\boldsymbol{0}}$ ，当从机x轴方向受到瞬时负载力时，导纳控制器输出响应以及从机估计的力响应如图6所示，图7和图8为相应主机和从机姿态响应。

从机的运动依赖于外部受到的力，由于滤波器进行力估计需要收敛时间，如图6，估计的力和实际的力之间存在延迟。且由于扰动力的存在，在导纳控制器对力进行响应之前，飞行器已经开始运动。如图7和图8所示，当从机受到瞬时力，由于硬连接产生的相互作用，主机同时会受到负载的拉力，主机和从机姿态角都会出现一定程度的波动，但10 s后主机和从机的姿态都恢复稳定，可以看出LQR控制器能够保证主从机姿态的稳定。

3.2.2 主从机连续轨迹追踪仿真

对双机协同搬运负载进行连续时间轨迹仿真，给定主机如式（1）的连续轨迹期望，主机和从机的三维轨迹曲线，即螺旋线轨迹如图9所示。图10和图11为追踪螺旋线轨迹时相应主机和从机姿态响应曲线。

$_I {\boldsymbol{P}}_{\rm{d}}=[5\sin(\pi t/10)\quad 5\cos(\pi t/10)-5 \quad 0.5t]^{\rm{T}} $

(1)

结果表明，对于连续的螺旋线轨迹，从机能够保证位置的稳定跟踪。主机和从机与负载刚性连接，在三维空间能够保持相对位置不变。图10和图11表明主从机在跟踪螺旋线轨迹过程中，主机姿态角虽然会出现波动，但能够快速保持稳定，从机由于被动力驱动，姿态角会出现持续的较小幅度的变化。总体来说，本文设计的控制方法，能够实现双机稳定地协同搬运负载的任务。

4 结论

本文对双无人机协作搬运系统控制方案进行了研究。该方案在位置姿态控制器基础之上采用卡尔曼滤波器估计施加在从机上的外力，并通过导纳控制器根据受到的外力生成运动轨迹。实验结果可以证明本文提出方案的有效性。在今后研究中，可以进一步开发力位混合控制策略，深入考虑分析风扰等外部干扰产生的问题，加强系统鲁棒性，进而提升该系统执行困难任务的能力。