1 引言（Introduction）

过去十年中，多旋翼无人机的研究取得了巨大进展，低成本和小型化的无人机被广泛应用于影视航拍、巡检勘测等领域。在仅具有被动观测能力的旋翼无人机系统上加装末端执行器等作业装置可使其实现与环境的物理交互，这种具有主动作业能力的新型机器人系统被称为飞行机械臂（AM）^[1-2]。

飞行机械臂可以飞抵人类或地面机器人难以到达的地点进行检查和维护，具有丰富的应用场景。早期，飞行机械臂的研究主要聚焦于负载运输^[3]和目标抓取^[4-5]，对于此类场景，作业过程中接触力短暂存在，因此往往不考虑其对系统的影响。近年来，飞行机械臂的研究范围已逐渐扩展到基于接触作业的无损检测^[6]、表面清理^[7]、传感器安装拆除^[8]等场景。通过在作业装置末端加装涡流、超声波等传感器可实现飞行机械臂对金属管道^[9]、桥梁^[10]、轮船^[11]等设施的检测，然而上述方法需要传感器直接接触待测表面，并在测量过程中施加恒定的接触力。此类接触作业过程中机械臂需要与作业环境长时间交互^[12]，产生的接触力不可忽略。

在与环境进行长时间接触作业时，飞行机器人运动自由度受限，给系统的稳定性带来了较大的考验；并且对于很多特定交互作业任务，需要对接触力进行控制，增加了控制难度。飞行过程中的移动接触任务已逐渐成为近年来的研究热点，文[13] 在四旋翼飞行机器人上装配多自由度机械手和轮式末端执行器以提升系统冗余度，并通过机械臂的运动保证接触过程中的接触力以及姿态稳定，实现了墙上涂鸦的清除，这种在旋翼飞行机器人系统上添加冗余自由度的方法将使控制系统更加复杂，并且额外的负载也将减少系统的续航时间。文[14] 在飞行机械臂上装配了具有平移自由度的机械手，实现了在2维表面上长时间提供横向力，为未来基于触觉的飞行机器人导航提供了基础。文[15] 的飞行机械臂配备了单自由度执行器，通过同时控制俯仰和偏航角度实现了对垂直平面的长时间、高强度稳定接触，但在接触过程中无法在任意方向上产生运动。文[16] 使用欠驱动旋翼无人机和单自由度机械臂，通过对接触状态下旋翼飞行机械臂系统进行受力分析，得到了接触力与重力和俯仰角之间的关系，从而实现了前向水平方向的恒定接触力控制，但旋翼无人机系统在接触过程中的位置移动需要依靠偏航角的改变。

旋翼飞行机器人的欠驱动特性给其横向力交互带来了一定挑战。无论是在旋翼无人机上安装多自由度机械臂，使机械臂进行运动以完成与环境的稳定接触，还是通过改变飞行机器人姿态来间接提供交互力都具有一定的局限性。通过增加动力单元数目以及改变动力单元安装角度或位置等方式^[17-18]，可实现无人机动力单元推力方向不一致，从而克服传统旋翼无人机的位置控制和姿态控制相互耦合的缺点，使旋翼飞行机器人产生水平方向推力，从而提升与环境进行物理交互的能力，因此更适用于接触任务。文[19] 中的飞行机械臂由全驱动六旋翼与末端执行器组成，采用导纳控制方法实现对交互力的控制，并通过非线性扰动观测器实现了在没有力传感器情况下对交互力的估计。文[20] 采用力/运动混合控制方法将运动和力控制进行解耦，实现接触过程的平稳过渡，并完成了对表面刚度的估计。全驱动旋翼飞行机械臂大多采用单杆结构作为执行机构，并通过控制全驱动旋翼无人机实现飞行机械臂系统与环境的交互力控制。

本文针对接触作业过程中普通旋翼无人机难以同时保持姿态稳定并提供横向力的问题，设计了一种全驱动旋翼飞行机械臂，通过改变动力单元旋翼安装角度的方式使得每个动力单元产生非平行方向的推力，并从控制结构上实现位置与姿态的独立控制。户外飞行实验验证了所设计的全驱动旋翼无人机在运动过程中位置移动不依赖于姿态的改变，具备独立控制水平方向推力的能力。在此基础上进一步设计了移动接触作业实验，将接触作业分为接近和移动接触2个阶段，并采用力/运动混合控制器对接触力进行控制，并对比了不同横向移动速度与期望力下接触力的控制效果。

2 机构设计（Mechanisms design）

如图 1所示，本文提出的全驱动旋翼飞行机械臂系统由全驱动六旋翼无人机和一个与其刚性连接的接触装置组成，无人机机架和接触装置的操作臂连杆均使用碳纤维材料，同时操作臂连杆采用空心结构，在减轻重量的同时满足了强度的需求，并在机架底部和机臂处均安装了固定支架，以保证末端装置与机身的稳定连接。接触装置末端安装了弹性装置，可以吸收部分接触瞬间产生的冲击力，并在移动接触过程中使用滚轮减小摩擦力，提升末端的移动接触能力。

全驱动旋翼飞行机械臂系统使用实时动态差分（RTK）定位模块估计无人机的位置与速度，使用激光测距模块和飞行控制器内置的惯性测量单元（IMU）分别估计无人机的高度与姿态信息。在接触任务中，通过激光测距模块测量与接触表面的距离。飞行机械臂系统搭载了板载计算机NVIDIA Jetson TX2，并在其中安装运行Ubuntu 18.04和ROS Melodic环境，将包含距离和力信息的ROS话题通过mavros工具转为MavLink协议数据，数据信息通过串口传输到飞行控制器Pixhawk 4上。Pixhawk 4通过无线接收器接收遥控器数据，实现操作人员对旋翼无人机的控制。

全驱动旋翼无人机轴距550 mm，为了保证较高的飞行效率和较低的能耗，动力单元倾转角度选30$ ^{\circ} $^[21]。接触装置的操作臂总长620 mm，可调节其与无人机的安装角度以适应更多任务场景，装置末端安装单轴力传感器，用于测量接触任务中的接触力。全驱动旋翼飞行机械臂的相关参数见表 1。

3 系统建模（System modeling）

假设全驱动旋翼飞行机械臂为刚体，其姿态和位置一般在地球坐标系中表示，而动力单元产生的力与力矩则在旋翼坐标系下描述更为直观。全驱动旋翼飞行机械臂系统坐标系的定义如图 2所示，采用北东地坐标系（NED），定义地球固联坐标系$ F_{\rm W}:\{ O_{\rm W}\text{-}X_{\rm W}Y_{\rm W}Z_{\rm W} \} $，在旋翼无人机质心处定义机体坐标系$ F_{\rm B}:\{ O_{\rm B}\text{-}X_{\rm B}Y_{\rm B}Z_{\rm B} \} $，在接触装置与表面的接触处定义末端执行器坐标系$ F_{\rm E}:\{ O_{\rm E}\text{-}X_{\rm E}Y_{\rm E} $ $ Z_{\rm E} \} $，其中$ Y_{\rm E} $平行$ Y_{\rm B} $，$ X_{\rm E} $沿接触装置操作臂方向，$ X_{\rm E} $与$ X_{\rm B} $之间的夹角为$ \eta $。

在动力单元的旋翼中心处定义6个旋翼的坐标系$ F_{{\rm P}i}:\{ O_{{\rm P}i}\text{-}X_{{\rm P}i}Y_{{\rm P}i}Z_{{\rm P}i} \} $，其中$ i=1, 2, \cdots, 6 $。记旋翼坐标系原点$ O_{{\rm P}i} $为旋翼中心处，$ X_{{\rm P}i} $沿机臂方向，$ Z_{{\rm P}i} $沿动力单元的电机推力方向。$ \alpha_{i} $表示第$ i $个动力单元绕$ X_{{\rm P}i} $轴的旋转角度，即电机绕机臂的倾转角度，且满足$ \alpha_{1} =-\alpha_{2} =\alpha_{3} =-\alpha_{4} =\alpha_{5} =-\alpha_{6} =\alpha $，其中$ \alpha \in (0, {\rm{\mathsf{π}}} /2) $，以保证无人机在悬停状态下各动力单元的推力方向对称。全驱动旋翼无人机系统通过将动力单元的旋翼倾斜来提供非平行的推力，从而克服旋翼无人机系统的欠驱动特性。

3.1 运动学建模

定义$ O_{\rm B} $在地球固连坐标系$ F_{\rm W} $下的位置矢量为$ \mathit{\boldsymbol{p}}\in \mathbb{R}^{3} $，$ \mathit{\boldsymbol{v}}=[v_{x}, v_{y}, v_{z} ] $为旋翼无人机系统在坐标系$ F_{\rm W} $下的线速度，则旋翼无人机在地球固连坐标系下的平动运动学方程为

$ \begin{align} \mathit{\boldsymbol{\dot{p}}}=\mathit{\boldsymbol{v}} \end{align} $

(1)

刚体绕定轴的转动可使用四元数表示，相较于欧拉角，四元数表示方法不存在奇异点。四元数可以表示为

$ \begin{align} \mathit{\boldsymbol{q}}=[q_{0}, q_{1}, q_{2}, q_{3}]^{\rm T}=\left[\cos \frac{\beta} {2}, \sin \frac{\beta} {2}{{\mathit{\boldsymbol{r}}}}\right]^{\rm T} \end{align} $

(2)

其中，$ {{\mathit{\boldsymbol{r}}}}\in \mathbb{R}^{3} $为转动轴方向上的单位向量，$ \beta $表示绕轴$ {{\mathit{\boldsymbol{r}}}} $旋转的角度。

定义单位四元数$ \mathit{\boldsymbol{q}}=[q_{0}, q_{1}, q_{2}, q_{3}]^{\rm T} $表示$ O_{\rm B} $在坐标系$ F_{\rm W} $下的姿态矢量，机体坐标系到地球固连坐标系的旋转矩阵为

$ \begin{align} & \mathit{\boldsymbol{R}}_{\rm B}^{\rm W} \\ =& \begin{bmatrix} q_{0}^{2} +q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2(q_{1} q_{2}-q_{0} q_{3}) & 2(q_{1} q_{3}+q_{0} q_{2}) \\ 2(q_{1} q_{2}+q_{0} q_{3}) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2(q_{2} q_{3}-q_{0} q_{1}) \\ 2(q_{1} q_{3}-q_{0} q_{2}) & 2(q_{2} q_{3}+q_{0} q_{1}) & q_{0}^{2}-q_{1}^{2} -q_{2}^{2}+q_{3}^{2} \end{bmatrix} \end{align} $

(3)

定义$ \mathit{\boldsymbol{\varOmega}} =[\varOmega_{x}, \varOmega_{y}, \varOmega_{z} ] $为旋翼无人机系统在机体坐标系下的角速度。则基于四元数表示的旋翼无人机系统的姿态运动学模型为

$ \begin{align} \begin{cases} \dot{q}_{0} =-\dfrac{1}{2}\mathit{\boldsymbol{q}}_{\mathit{\boldsymbol{v}}}^{\rm T}\mathit{\boldsymbol{\varOmega}} \\ \mathit{\boldsymbol{\dot{q}}}_{\mathit{\boldsymbol{v}}} =\dfrac{1}{2}\left(q_{0} \mathit{\boldsymbol{I}}_{3} +[\mathit{\boldsymbol{q}}_{\mathit{\boldsymbol{v}}}]_{\times}\right)\mathit{\boldsymbol{\varOmega}} \end{cases} \end{align} $

(4)

其中，$ \mathit{\boldsymbol{q}}_{\mathit{\boldsymbol{v}}} =[q_{1}, q_{2}, q_{3}]^{\rm T} $，$ \mathit{\boldsymbol{I}}_{3} $为3阶单位矩阵，$ [\mathit{\boldsymbol{q}}_{\mathit{\boldsymbol{v}}}]_{\times} $为$ \mathit{\boldsymbol{q}}_{\mathit{\boldsymbol{v}}} $的反对称矩阵。

3.2 动力学建模

旋翼无人机系统的动力学方程可用牛顿—欧拉形式描述：

$ \begin{align} \begin{cases} m\mathit{\boldsymbol{\ddot{p}}}=-mg\mathit{\boldsymbol{e}}_{3} +\mathit{\boldsymbol{R}}_{\rm B}^{\rm W} \mathit{\boldsymbol{f}}^{\rm B}+\mathit{\boldsymbol{R}}_{\rm B}^{\rm W} \mathit{\boldsymbol{f}}_{\rm ext}^{\rm B} \\ \mathit{\boldsymbol{J\dot{\varOmega}}}=-\mathit{\boldsymbol{\varOmega}} \times \mathit{\boldsymbol{J\varOmega}} +\mathit{\boldsymbol{\tau}}^{\rm B}+\mathit{\boldsymbol{\tau}}_{\rm ext}^{\rm B} \end{cases} \end{align} $

(5)

其中，$ m $和$ \mathit{\boldsymbol{J}}={\text{diag}}(J_{xx}, J_{yy}, J_{zz}) $分别为旋翼无人机的质量和惯性矩阵，$ g $为重力加速度，$ \mathit{\boldsymbol{f}}^{\rm B}=[ f_{x}^{\rm B}, f_{y}^{\rm B}, $ $ f_{z}^{\rm B}] $和$ \mathit{\boldsymbol{\tau}}^{\rm B}=[ \tau_{x}^{\rm B}, \tau_{y}^{\rm B}, \tau_{z}^{\rm B} ] $分别为动力单元在机体坐标系上产生的力与力矩，$ \mathit{\boldsymbol{f}}_{\rm ext}^{\rm B} \in \mathbb{R}^{3} $和$ \mathit{\boldsymbol{\tau}}_{\rm ext}^{\rm B} \in \mathbb{R}^{3} $为外部扰动在机体坐标系上产生的力与力矩，单位向量$ \mathit{\boldsymbol{e}}_{3} =[0, 0, -1]^{\rm T} $。

旋翼的推力和反扭矩均正比于旋翼转速的平方，定义$ \omega_{i} >0 $为旋翼$ i $的转速，则在旋翼坐标系下旋翼$ i $的推力和反扭矩为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{f}}_{i}^{\rm P}=c_{f} \omega_{i}^{2} \mathit{\boldsymbol{e}}_{3} \\ \tau_{i}^{\rm P}=(-1)^{i}c_{\tau} \omega_{i}^{2} \mathit{\boldsymbol{e}}_{3} \end{cases} \end{align} $

(6)

其中，$ i=1, \cdots, 6 $，常数$ c_{f} $、$ c_{\tau} $分别表示旋翼推力系数和反扭矩系数。

定义机臂长度$ l $为$ O_{{\rm P}i} $与$ O_{\rm B} $之间的距离，$ O_{{\rm P}i} $在机体坐标系$ F_{\rm B} $下的位置矢量$ \mathit{\boldsymbol{p}}_{{\rm P}i}^{\rm B} $为

$ \begin{align} \mathit{\boldsymbol{p}}_{{\rm P}i}^{\rm B} =\mathit{\boldsymbol{R}}_{z} \left((i-1)\frac{{\rm{\mathsf{π}}}} {3}\right)[l, 0, 0]^{\rm T}, \; \; i=1, \cdots, 6 \end{align} $

(7)

其中，$ \mathit{\boldsymbol{R}}_{x} (\cdot) $表示绕$ X $轴的旋转矩阵，则旋翼坐标系到机体坐标系的旋转矩阵可表示为

$ \begin{align} \mathit{\boldsymbol{R}}_{{\rm P}i}^{\rm B} =\mathit{\boldsymbol{R}}_{z} \left((i-1)\frac{{\rm{\mathsf{π}}}}{3}\right)\mathit{\boldsymbol{R}}_{x} \left((-1)^{i-1}\alpha\right), \; \; i=1, \cdots, 6 \end{align} $

(8)

其中，$ \mathit{\boldsymbol{R}}_{z} (\cdot) $表示绕$ Z $轴的旋转矩阵。

旋翼作用在无人机质心处的总推力和力矩为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{f}}^{\rm B}=\sum\limits_{i=1}^6 \mathit{\boldsymbol{R}}_{{\rm P}i}^{\rm B} ( \alpha ) \mathit{\boldsymbol{f}}_{i}^{\rm P} =\mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{f}}} ({\alpha}) \mathit{\boldsymbol{u}} \\ \mathit{\boldsymbol{\tau}}^{\rm B}=\sum\limits_{i=1}^6 \left(\left(\mathit{\boldsymbol{p}}_{{\rm P}i}^{\rm B} \times \mathit{\boldsymbol{f}}_{i}^{\rm P}\right) + \mathit{\boldsymbol{R}}_{{\rm P}i}^{\rm B} (\alpha)\mathit{\boldsymbol{\tau}}_{i}^{\rm P} \right)=\mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{\tau}}} (\alpha) \mathit{\boldsymbol{u}} \end{cases}\end{align} $

(9)

其中，电机转速$ \mathit{\boldsymbol{u}}=[f_{1}, f_{2}, f_{3}, f_{4}, f_{5}, f_{6} ]^{\rm T} $为控制输入量，$ \mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{f}}} (\alpha) \in \mathbb{R}^{3\times 6} $和$ \mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{\tau}}} (\alpha) \in \mathbb{R}^{3\times 6} $均为仅与$ \alpha $相关的常数矩阵。进而，式(5) 可改写为

$ \begin{align} \begin{cases} m\mathit{\boldsymbol{\ddot{p}}}=-mg\mathit{\boldsymbol{e}}_{\mathit{\boldsymbol{3}}} +\mathit{\boldsymbol{R}}_{\rm B}^{\rm W} \mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{f}}} ( {\alpha}){\mathit{\boldsymbol{u}}}+\mathit{\boldsymbol{R}}_{\rm B}^{\rm W} \mathit{\boldsymbol{f}}_{\rm ext}^{\rm B} \\ \mathit{\boldsymbol{J\dot{\varOmega}}}=-\mathit{\boldsymbol{\varOmega}} \times \mathit{\boldsymbol{J\varOmega}} +\mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{\tau}}} ({\alpha})\mathit{\boldsymbol{u}}+\mathit{\boldsymbol{\tau}}_{\rm ext}^{\rm B} \end{cases} \end{align} $

(10)

定义控制分配矩阵$ \mathit{\boldsymbol{F}}(\alpha) $为

$ \begin{align} \mathit{\boldsymbol{F}}(\alpha)= \begin{bmatrix} \mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{f}}} (\alpha) \\ \mathit{\boldsymbol{F}}_{\mathit{\boldsymbol{\tau}}} (\alpha) \end{bmatrix} \end{align} $

(11)

控制分配矩阵是否满秩决定了旋翼无人机是否为全驱动系统，即能否独立控制位置和姿态^[22]。当6个动力单元的推力方向平行时，$ \alpha =0 $，$ {\rm rank }\mathit{\boldsymbol{F}}(\alpha)=4 $，无人机只能控制3个方向的力矩和1个维度的力；当$ \alpha \ne 0 $时，$ {\rm rank }\mathit{\boldsymbol{F}}(\alpha)=6 $，旋翼无人机为全驱动系统，具备提供横向推力的能力。

4 控制系统设计（The design of the control system） 4.1 全驱动旋翼无人机运动控制

欠驱动旋翼无人机$ X $、$ Y $方向的位置控制依赖于姿态控制器的调节，无法实现3维位置的独立控制，全驱动旋翼无人机则在底层控制结构上实现了位置与姿态的解耦控制，飞行机械臂的运动控制如图 3所示。

飞行机械臂的位置控制与姿态控制均采用P-PID（proportional-PID）级联控制，以避免飞行机械臂系统在目标值处发生振荡。定义位置误差$ \mathit{\boldsymbol{e}}_{\rm p} =\mathit{\boldsymbol{p}}_{\rm d} -\mathit{\boldsymbol{p}} $和速度误差$ \mathit{\boldsymbol{e}}_{\rm v} =\mathit{\boldsymbol{\dot{p}}}_{\rm d} -\mathit{\boldsymbol{\dot{p}}}=\mathit{\boldsymbol{v}}_{\rm d} -\mathit{\boldsymbol{v}} $，则位置控制器为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{v}}_{\rm d} =\mathit{\boldsymbol{K}}_{\rm Pv} \mathit{\boldsymbol{e}}_{\rm p} +\mathit{\boldsymbol{v}}_{\rm d} \\ \mathit{\boldsymbol{f}}_{\rm p} =\mathit{\boldsymbol{K}}_{\rm Pf} \mathit{\boldsymbol{e}}_{\rm v} +\mathit{\boldsymbol{K}}_{\rm If} {\int_{t_{0}} ^t } \mathit{\boldsymbol{e}}_{\rm v} {\rm d}t+\mathit{\boldsymbol{K}}_{\rm Df} \mathit{\boldsymbol{\dot{e}}}_{\rm v} \end{cases} \end{align} $

(12)

其中，$ \mathit{\boldsymbol{K}}_{\rm Pv} $、$ \mathit{\boldsymbol{K}}_{\rm Pf} $、$ \mathit{\boldsymbol{K}}_{\rm If} $、$ \mathit{\boldsymbol{K}}_{\rm Df} $为位置控制增益矩阵。

定义期望姿态为$ \mathit{\boldsymbol{q}}_{\rm d} $，则姿态误差为

$ \begin{align} \mathit{\boldsymbol{q}}_{\rm e} =\mathit{\boldsymbol{q}}^{-1}\otimes \mathit{\boldsymbol{q}}_{\rm d} = \left[ \cos \frac{\beta_{\rm e}} {2}, \sin \frac{\beta_{\rm e}} {2}{{\mathit{\boldsymbol{r}}}} \right] \end{align} $

(13)

定义角速度误差为$ \mathit{\boldsymbol{e}}_{\Omega} =\mathit{\boldsymbol{\varOmega}}_{\rm d} -\mathit{\boldsymbol{\varOmega}} $，则姿态控制器为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{\varOmega}}_{\rm d} =\mathit{\boldsymbol{K}}_{\rm P\Omega} \beta_{\rm e} {{\mathit{\boldsymbol{r}}}} \\ \mathit{\boldsymbol{\tau}}^{\rm B}=\mathit{\boldsymbol{K}}_{\rm P\tau} \mathit{\boldsymbol{e}}_{\Omega} +\mathit{\boldsymbol{K}}_{\rm I\tau} {\int_{t_{0}} ^t } \mathit{\boldsymbol{e}}_{\Omega} {\rm d}t +\mathit{\boldsymbol{K}}_{\rm D\tau} \mathit{\boldsymbol{\dot{e}}}_{\Omega} +\mathit{\boldsymbol{K}}_{\rm f\tau} \mathit{\boldsymbol{\varOmega}}_{\rm d} \end{cases}\end{align} $

(14)

其中，$ \mathit{\boldsymbol{K}}_{\rm P\Omega} $、$ \mathit{\boldsymbol{K}}_{\rm P\tau} $、$ \mathit{\boldsymbol{K}}_{\rm I\tau} $、$ \mathit{\boldsymbol{K}}_{\rm D\tau} $、$ \mathit{\boldsymbol{K}}_{\rm f\tau} $为姿态控制增益矩阵，$ \mathit{\boldsymbol{K}}_{\rm f\tau} \mathit{\boldsymbol{\varOmega}}_{\rm d} $为期望的角速度前馈项。

4.2 飞行机械臂接触力控制

飞行机械臂的接触作业需要全驱动无人机与目标表面建立稳定的接触，对此设计力/运动混合控制器，并将接触过程分为接近和移动接触2个阶段，2个阶段的状态切换条件为传感器获得的接触力测量值。当接触力达到一定阈值时，进入移动接触阶段，无人机在接触面法线方向上进行力控制，并沿接触面表面滑动。每个阶段可分为2个正交子空间$ \mathit{\boldsymbol{M}}=\mathit{\boldsymbol{RNR}}^{\rm T} $和$ \widetilde{{\mathit{\boldsymbol{M}}}}=\mathit{\boldsymbol{R\overline{\mathit{\boldsymbol{N}}}R}}^{\rm T} $以完成任务过程中的解耦控制。其中$ \mathit{\boldsymbol{N}}={\text{diag}}(\sigma_{x}, \sigma_{y}, \sigma_{z}) $，$ \sigma $取0或1，$ \mathit{\boldsymbol{\overline{\mathit{\boldsymbol{N}}}}}={\text{diag}} (1, 1, 1)-\mathit{\boldsymbol{N}} $。$ \mathit{\boldsymbol{R}} $为接触装置$ X_{\rm E} $与旋翼无人机机体$ X_{\rm B} $间的旋转矩阵，即$ \mathit{\boldsymbol{R}}=\mathit{\boldsymbol{R}}_{y} (\eta) $。

在接近阶段，旋翼无人机使用速度控制器自动控制接近速度$ v_{x} $，使全驱动旋翼飞行机械臂系统以较慢的速度接近接触表面，以降低接触时的冲击，其他方向上的运动使用位置控制器由操作者控制。飞行机械臂系统接近阶段的速度/位置混合控制器如图 4所示。

定义接近阶段速度误差$ \mathit{\boldsymbol{e}}_{\rm av} =\mathit{\boldsymbol{v}}_{\rm ad} -\mathit{\boldsymbol{v}} $，则PID速度控制器为

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm av} =\mathit{\boldsymbol{K}}_{\rm Pav} \mathit{\boldsymbol{e}}_{\rm av} +\mathit{\boldsymbol{K}}_{\rm Iav} \int_{t_{0}} ^t \mathit{\boldsymbol{e}}_{\rm av} {\rm d}t+\mathit{\boldsymbol{K}}_{\rm Dav} \mathit{\boldsymbol{\dot{e}}}_{\rm av} \end{align} $

(15)

其中，$ \mathit{\boldsymbol{K}}_{\rm Pav} $、$ \mathit{\boldsymbol{K}}_{\rm Iav} $和$ \mathit{\boldsymbol{K}}_{\rm Dav} $为接近阶段速度控制增益矩阵。

飞行机械臂的末端执行器$ X_{\rm E} $垂直于接触面，则2个正交子空间的选择矩阵为$ \mathit{\boldsymbol{N}}_{\rm v} ={\text{diag}}(1, 0, 0) $，$ \mathit{\boldsymbol{\overline{\mathit{\boldsymbol{N}}}}}_{\rm v} ={\text{diag}}(0, 1, 1) $。接近阶段的速度/位置混合控制器可表示为

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm a} =\mathit{\boldsymbol{M}}_{\rm v} \mathit{\boldsymbol{f}}_{\rm av} +\widetilde{\mathit{\boldsymbol{M}}}_{\rm v} \mathit{\boldsymbol{f}}_{\rm p} \end{align} $

(16)

设置切换阈值为非零值，当力传感器检测接触力到达设定阈值时，则进入移动接触阶段，接触过程中需要对接触表面法线方向进行接触力控制，在其他方向上根据操作人员的遥控器指令进行位置移动，飞行机械臂系统在移动接触阶段的力/运动混合控制器如图 5所示。

$ \mathit{\boldsymbol{f}}_{\rm d} $为接触力期望值，$ \mathit{\boldsymbol{f}}_{\rm m} $为力传感器得到的接触力测量值，定义接触阶段接触力误差为$ \mathit{\boldsymbol{e}}_{\rm f} = $ $ \mathit{\boldsymbol{f}}_{\rm d} -\mathit{\boldsymbol{f}} $，则接触力的PD控制器为

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm cf} =\mathit{\boldsymbol{K}}_{\rm Pc} \mathit{\boldsymbol{e}}_{\rm f} +\mathit{\boldsymbol{K}}_{\rm Dc} \mathit{\boldsymbol{\dot{e}}}_{\rm f} \end{align} $

(17)

其中，$ \mathit{\boldsymbol{K}}_{\rm Pc} $、$ \mathit{\boldsymbol{K}}_{\rm Dc} $为接触力控制增益矩阵。

移动接触过程中，2个正交子空间的选择矩阵为$ \mathit{\boldsymbol{N}}_{\rm f}={\text{diag}}(1, 0, 1) $和$ \mathit{\boldsymbol{\overline{\mathit{\boldsymbol{N}}}}}_{\rm f} ={\text{diag}}(0, 1, 0) $，力/运动混合控制器为

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm c} =\mathit{\boldsymbol{M}}_{\rm f} \mathit{\boldsymbol{f}}_{\rm cf} +\widetilde{\mathit{\boldsymbol{M}}}_{\rm f} \mathit{\boldsymbol{f}}_{\rm p} \end{align} $

(18)

5 实验与结果（Experiments and results） 5.1 稳定飞行实验

为验证所设计的全驱动旋翼飞行机械臂的运动控制效果，设置矩形运动轨迹以测试户外飞行效果，通过RTK定位模块和IMU传感器获取实验过程中飞行机械臂的位置、姿态信息，并将结果记录在飞行控制器中。设置无人机动力单元的旋翼安装角度$ \alpha =0 ^{\circ} $，可将飞行机械臂调整为欠驱动旋翼无人机系统，此时飞行机械臂系统无法实现横向推力的独立控制，水平方向的运动将依赖于姿态的改变。实验中分别控制传统欠驱动旋翼无人机与全驱动旋翼无人机起飞后在距地面2 m高处完成相同的矩形轨迹运动，飞行机械臂在此过程中的位置轨迹如图 6所示。

飞行过程中飞行机械臂的位置曲线和姿态曲线分别如图 7和图 8所示，其中0~20 s为起飞阶段，150 ~160 s为降落阶段，分别在50 s、85 s、110 s和135 s时改变飞行机械臂运动的方向。20~150 s为稳定飞行状态，此时飞行机械臂保持2 m高度并完成预设轨迹运动，欠驱动旋翼飞行机械臂系统在运动过程中姿态变化较大，俯仰角平均值为1.6183$ ^{\circ} $、标准差为1.4362$ ^{\circ} $，说明由于其欠驱动特性，传统旋翼无人机只能通过改变机身姿态的方式来获取水平方向上的推力，从而改变无人机在平面$ X_{\rm W} O_{\rm W} Y_{\rm W} $内的位置。而在此过程中全驱动旋翼飞行机械臂的俯仰角均值为0.0172$ ^{\circ} $、标准差为0.2905$ ^{\circ} $，俯仰角与滚转角保持平稳且变化范围控制在$ \pm 1.5^{\circ} $以内，在完成平行运动的同时保持了姿态的稳定，实现了位置与姿态的解耦控制。对比欠驱动无人机，本文设计的飞行机械臂在做水平方向移动时不需要改变其姿态，实现了对横向力的独立控制。

5.2 移动接触实验

飞行实验验证了所设计的全驱动飞行机械臂可实现稳定的飞行并具备独立提供横向推力的能力，在此基础上设计了移动接触实验，通过控制旋翼无人机的运动状态，实现对接触力的控制。移动接触实验中使用倾斜角$ 30^{\circ} $的光滑白板作为接触面，并设定接触装置操作臂与水平面的安装夹角$ \eta =30^{\circ} $，以保证接触装置垂直于接触面。飞行机械臂系统在户外环境下使用RTK定位模块获取无人机的位置和速度信息，力采集传感器采集接触力信息并将结果记录在板载计算机中。设定飞行机械臂接触过程中交互力的期望值为3 N，切换阈值为0.5 N，力传感器检测接触力在0.3 s内均大于设定阈值时，则飞行机械臂系统由接近阶段进入移动接触阶段，移动接触实验过程如图 9所示。

接触实验分为接近阶段和移动接触阶段，实验过程中旋翼飞行机械臂的速度曲线如图 10所示。接近阶段，无人机以0.25 m/s的恒定速度接近接触面，在5.5 s时，飞行机械臂与表面发生接触，接触力达到设定阈值后，进入移动接触阶段。在移动接触阶段，飞行机械臂系统对$ X $方向与$ Z $方向进行力控制，操作员使用遥控器来控制飞行机械臂的$ Y $向位置。

飞行机械臂在此过程中的姿态曲线如图 11所示，姿态角变化范围在接近阶段可控制在$ \pm 1.5^{\circ} $以内，在移动接触阶段可控制在$ \pm 3^{\circ} $以内，飞行机械臂在接触作业过程中的姿态角基本保持不变。

接触过程中力传感器测量得到的接触力数据是实现力控制的关键因素，飞行机械臂与平面的接触力曲线如图 12所示，接触瞬间飞行机械臂与接触面会产生较大冲击，接触装置末端的弹性装置对此过程中的冲击力进行一定的缓冲，此时接触力出现峰值并有一定波动。经过短暂的冲击后，移动接触阶段的接触力在一定的精度范围内稳定在3 N左右，实现了稳定的恒力接触。

飞行机械臂系统在稳定接触阶段接触力在2~4 N之间波动，使用飞行机械臂系统进行恒定力控制普遍存在一定的误差^{[7, 10, 16, 19-20, 23-24]}，其原因首先是飞行机械臂系统的一些固有缺陷。作为浮动平台，环境风以及旋翼转动产生的气流都会对其产生一定扰动，由于动力单元以及电池电压不稳定等原因，无人机自身也会产生一些不可避免的振动；并且飞行机械臂系统的实时位置具有一定的定位误差，特别是在户外环境，即使使用了RTK定位技术，但定位精度仍远低于毫米级别的运动捕捉系统等室内定位装置，因此导致接触力的跟踪控制效果会出现较大波动和误差。同时，力传感器在测量过程中的振动噪声和迟滞误差也是误差来源。虽然飞行机械臂系统的接触力控制效果不能与工业机械臂相比，但飞行机械臂的应用主要集中在建筑物检测^[10]以及表面清理^[16]等场景，相关文献也已验证了其接触力波动幅度可以满足相关工作的需求，因此本文所设计的全驱动旋翼飞行机械臂具备与物理环境进行接触作业的功能。

在工业接触检测任务中，根据不同的传感器类型与检测场景会使用定点接触检测或移动接触检测，飞行机械臂在接触过程中是否移动以及移动速度的大小都会影响到接触效果；同时，不同的应用场景对飞行机械臂与环境的交互力大小也有不同的需求。为研究接触过程中飞行机械臂平行于接触面的移动速度以及接触力大小对接触稳定性的影响，分别设定不同的移动速度以及接触力期望值进行移动接触实验，且其他设置保持不变。

飞行机械臂与接触面保持3 N的交互力，并分别设置接触过程中平行于接触面方向不移动以及移动速度为0.1 m/s和0.2 m/s，稳定接触阶段无人机运动速度的实时误差与接触力的实时误差情况如图 13和图 14所示。当飞行机械臂在接触过程中处于静止状态时，飞行机械臂的速度误差平均值以及波动范围更小，随着移动接触过程中移动速度的增大，无人机平台受到的气流扰动与阻力增长，飞行机械臂速度误差增大，飞行机械臂与环境接触力也出现较大波动。

分别设置接触力期望值为1.5 N、3 N、4.5 N和6 N，且飞行机械臂系统以0.1 m/s速度进行移动接触，稳定接触阶段飞行机械臂的速度实时误差与接触力实时误差的小提琴图分别如图 15和图 16所示。随着接触力增大，飞行机械臂系统也将在移动过程中受到更大的横向摩擦力，飞行机械臂系统需要提供更大的侧向力以保证接触力并且克服阻力，在此过程中，无人机平台速度波动范围增大，飞行机械臂与环境的接触力误差变大。

6 结论（Conclusion）

针对传统欠驱动旋翼无人机无法独立提供横向力的问题，本文通过倾斜旋翼安装角度的方式，完成了全驱动旋翼飞行机械臂的设计，并且从控制结构上实现了位置与姿态的解耦控制，进而针对接触作业的接近阶段和移动接触阶段设计了力/运动混合控制器，完成了对目标表面的稳定接触作业。在户外实验中，通过与欠驱动旋翼无人机的对比，证明了本文所设计的全驱动旋翼无人机可以提供水平方向的推力，实现位置与姿态的独立控制与稳定飞行；在接触实验中，飞行机械臂在移动过程中完成了对接触面法线方向的恒力控制，实现了与接触表面的稳定交互。在未来的工作中，将对系统稳定性与环境感知能力进行研究，以及对曲面以及实际工业场景中的接触检测任务进行测试。