0 概述

多旋翼无人机(Multi Rotor Unmanned Aerial Vehicle, MRUAV), 又被称为旋翼无人飞行器, 是一种具有3个及以上旋翼轴的无人飞行器, 搭载小型高精度摄像头。旋翼无人机作为一种具有欠驱动、半自主、强耦合、非线性等动力学特性于一身的无人飞行器, 被广泛地应用于军事、民用等领域。近年来, 旋翼无人机相关技术发展迅速, 其智能控制方法已经成为研究热点^[1-3], 尤其是对旋翼无人机目标追踪的研究备受国内外学者的青睐。文献[4]提出一种基于深度强化学习的方法控制旋翼无人机的着陆, 该方法网络结构相对复杂、计算量大, 对性能一般的旋翼无人机并不适用。文献[5]通过对自然条件下风速的变化特性进行分析, 建立相应的数学模型, 提出一种利用遗传算法优化模糊控制器规则以提高旋翼无人机控制器性能的方法, 但是该方法收敛速度不高, 计算量也相对较大。文献[6]提出一种多模式的PID控制器, 在外界干扰情况下使旋翼无人机具有较好的偏航控制性能, 同时抑制偏航饱和, 但是PID控制器结构相对简单, 在复杂环境下的鲁棒性较差、稳定性不高。

视觉伺服(Visual Servo, VS)包括基于图像的视觉伺服(Image-Based Visual Servo, IBVS)^[7]与基于位置的视觉伺服(Position-Based Visual Servo, PBVS)^[8]2种不同类型。PBVS的误差定义在三维笛卡尔空间, 因此PBVS模型对初始条件、噪声、摄像机参数误差和目标位姿的估计精度都非常敏感。而IBVS的误差直接定义在二维图像平面, 不需要估计空间模型, 通过使用当前目标的影像与期望影像的误差来形成闭环控制。由于旋翼无人机系统是一种强耦合、欠驱动的动力学系统, 因此, 本文提出一种解耦的IBVS追踪方法(JD-IBVS)来实现旋翼无人机的追踪, 并通过仿真追踪实验与实物追踪实验检测该方法的有效性与实用性。

1 基于解耦视觉伺服的旋翼无人机控制模型 1.1 旋翼无人机视觉模型

设定P=(X_p, Y_p, Z_p)^T为空间中的一点, P_i=(x, y, z)^T为P点在图像平面的投影, 图 1展示了旋翼无人机视觉模型。

根据小孔成像的原理可以得到空间中一点与像素平面上一点的函数关系如下:

$ \left[ {\begin{array}{*{20}{c}} x\\ y\\ z\\ 1 \end{array}} \right] = \frac{1}{{{K_{\rm{s}}}}}\left[ {\begin{array}{*{20}{c}} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0\\ 0&0&0&{{K_s}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{X_p}}\\ {{Y_p}}\\ {{Z_p}}\\ 1 \end{array}} \right] $

(1)

其中, K_s为比例因子, f为相机焦距。设定焦距为相机坐标系中心到图像平面坐标系中心的距离z=f。

视觉传感器采集的图像像素在计算机中用二元函数表示为f(u, v), 其中, (u, v)为图像平面上一点的坐标, f(u, v)为在该点处的像素值。图像平面坐标系中一点(u, v)与视觉传感器平面的坐标系中一点(x, y)的关系如式(2)所示。

$ \left[ {\begin{array}{*{20}{c}} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\frac{1}{{{d_x}}}}&0&{{u_q}}\\ 0&{\frac{1}{{{d_y}}}}&{{v_q}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x\\ y\\ 1 \end{array}} \right] $

(2)

其中, 视觉传感器平面坐标的原点O₁在图像平面的坐标为(u₀, v₀), d_x、d_y为从图像平面到视觉传感器平面的放缩比例。

1.2 旋翼无人机的动力学扩展

旋翼无人机动力学系统通过改变旋翼无人机的推进器转子的旋转速度产生升力, 进而使旋翼无人机运动。因此, 通过改变旋翼无人机4个推进器的转动速度, 可以控制旋翼无人机上下运动。如果相反地控制第2和第4推进器的旋转速度, 会引起翻滚运动; 如果控制第1和第3推进器的旋转速度, 会引起俯仰运动; 通过共同调节第1、第2、第3和第4推进器的旋转速度, 可使旋翼无人机产生偏航运动, 具体的推进器转子的旋转速度是由给定的加速度参数来实现控制的, 其函数关系如下:

$ {\left[ {{\omega _1},{\omega _2},{\omega _3},{\omega _4}} \right]^{\rm{T}}} = {\mathit{\boldsymbol{J}}_a}{\left[ {a_x^v,a_y^v,a_z^v,a_x^\omega ,a_y^\omega ,a_z^\omega } \right]^{\rm{T}}} $

(3)

其中, {ω₁, ω₂, ω₃, ω₄}为对应的推进器转子的转动速度, {a_x^v, a_y^v, a_z^v, a_x^ω, a_y^ω, a_z^ω}为6个自由度的加速度, J_a是推进器转动速度到加速度的映射矩阵。

如果旋翼无人机传入的控制参数只有4个自由度, 并假定旋翼无人机保持水平, 则需要一个滚转角θ_x或者俯仰角θ_y来实现旋翼无人机x或y方向运动。设定旋翼无人机在较低速的条件下飞行时, 滚转角θ_x或者俯仰角θ_y不是通过VS计算获得, 而是旋翼无人机内部控制对v_x和v_y进行运动学解析后计算出的对应的θ_x和θ_y, 属于底层控制。该方法的具体过程为:通过动力学原理可以计算出x、y方向的线加速度a_x^v、a_y^v, 计算公式为:

$ \left\{ \begin{array}{l} a_x^v = \frac{{{\rm{d}}{v_x}}}{{{\rm{d}}t}}\\ a_y^v = \frac{{{\rm{d}}{v_y}}}{{{\rm{d}}t}} \end{array} \right. $

(4)

以x方向为例, 当给定了一个预定的加速度, 则旋翼无人机的运动状态如图 2所示。

在图 2中, θ_x^*为旋翼无人机在x方向的期望滚转角, 其计算公式为:

$ \tan \theta _x^ * = \frac{{ma_x^v}}{{mg}} \Rightarrow {\theta _x} = \arctan \frac{{a_x^v}}{g} $

(5)

通过旋翼无人机自身携带的陀螺仪可以获取当前在x方向实际滚转角θ_x^c。则可以根据计算公式ω_x=(θ_x^*-θ_x^c)/Δt计算出x方向的角速度ω_x。其中, Δt为设定的运动时间, 同理可以计算出y方向的角速度ω_y, 计算公式为:

$ \left\{ \begin{array}{l} {\theta _y} = \arctan \left( {a_y^v/g} \right)\\ {\omega _y} = \left( {\theta _y^ * - \theta _y^c} \right)/\Delta t \end{array} \right. $

(6)

其中, θ_y^*为旋翼无人机在y方向的期望俯仰角, θ_y^c为陀螺仪获取的y方向当前角度。进而将速度扩展为:

$ \boldsymbol{v}=\left(v_{x}, v_{y}, v_{z}, \omega_{x}, \omega_{y}, \omega_{z}\right)^{\mathrm{T}} $

1.3 JD-IBVS模型

设ξ_c表示特征点在图像平面中当前的坐标向量(u_c, v_c), ξ_*为期望的特征点的坐标向量(u_*, v_*), 则特征误差向量可以表示为e=ξ_c-ξ_*。根据动力学关系可以得到特征误差随时间的变化率与角速度、线速度之间的函数关系:

$ \boldsymbol{\dot{e}}=\frac{\mathrm{d}\left(\boldsymbol{\xi}_{c}-\boldsymbol{\xi}_{*}\right)}{\mathrm{d} t}=\boldsymbol{J}\left(\begin{array}{l}{\boldsymbol{v}} \\ {\boldsymbol{\omega}}\end{array}\right) $

(7)

如果只有一个特征点, 图像雅可比矩阵为J⊆$ \mathbb{R}^{2 \times 6}$; 如果特征点的个数为N, 则图像雅可比矩阵为$ \boldsymbol{J} \subseteq \mathbb{R}^{2 N \times 6}$。v=(v_x, v_y, v_z)^T为旋翼无人机的线速度向量, ω=(ω_x, ω_y, ω_z)^T为旋翼无人机的俯仰角速度、滚转角速度、偏航角速度的向量。为了保证特征误差呈指数解耦下降, 令$\mathit{\boldsymbol{\dot e}} = - {\lambda _{v\omega }}\mathit{\boldsymbol{e}} $, 其中, λ_{v, ω}为伺服增益。同时为了保证系统的稳定性, 令λ_{v, ω}>0, 带入式(7)中可得:

$ \left( {\begin{array}{*{20}{c}} \mathit{\boldsymbol{v}}\\ \mathit{\boldsymbol{\omega }} \end{array}} \right) = - {\lambda _{v,\omega }}\mathit{\boldsymbol{\hat Je}} $

(8)

其中, $\hat{\boldsymbol{J}} $为矩阵J的伪逆。如果J为方阵, 则矩阵的伪逆就为矩阵的逆, 记为J^-1; 如果J为行列不同的矩阵, 则$ \hat{\boldsymbol{J}}=\left(\boldsymbol{J}^{\mathrm{T}} \cdot \boldsymbol{J}\right)^{-1} \cdot \boldsymbol{J}^{\mathrm{T}}$^[9]。

考虑到旋翼无人机的动力学特性, 当旋翼无人机与地面近乎直线飞行时可以通过姿态调节去除俯仰角与滚转角的影响^[10]。在低速状态下, 一个微小的姿态倾斜角度对视觉的影响可以忽略, 不会由于视觉计算产生俯仰角与滚转角。因此, 本文忽略滚转角与俯仰角的影响, 只考虑线速度与偏航角^[11]。在去除俯仰角与滚转角之后, 当只有一个特征点时, 图像雅可比矩阵可以写为$\boldsymbol{J} \in \mathbb{R}^{2 \times 4} $。

根据运动学公式, (X_p, Y_p, Z_p)随时间的变化率与线速度、角速度的关系如式(9)所示。

$ \left( {\begin{array}{*{20}{c}} {{{\dot X}_p}}\\ {{{\dot Y}_p}}\\ {{{\dot Z}_p}} \end{array}} \right) = \left[ {\begin{array}{*{20}{c}} { - 1}&0&0&{{Y_p}}\\ 0&{ - 1}&0&{ - {X_p}}\\ 0&0&{ - 1}&0 \end{array}} \right]\left( {\begin{array}{*{20}{c}} {{v_x}}\\ {{v_y}}\\ {{v_z}}\\ {{\omega _z}} \end{array}} \right) $

(9)

则图像雅可比矩阵计算公式如下:

$ \mathit{\boldsymbol{J}} = \left[ {\begin{array}{*{20}{c}} { - \frac{f}{{z \cdot {d_x}}}}&0&{\frac{x}{{z \cdot {d_x}}}}&{\frac{y}{{{d_x}}}}\\ 0&{ - \frac{f}{{z \cdot {d_f}}}}&{\frac{y}{{z \cdot {d_y}}}}&{ - \frac{x}{{{d_y}}}} \end{array}} \right] $

(10)

如果特征点的数目为N个, 特征点坐标集为{ξ_i|i=1, 2, …, N}, 则特征点误差为:

$ \mathit{\boldsymbol{e}} = {\left( {{\mathit{\boldsymbol{\xi }}_1} - \mathit{\boldsymbol{\xi }}_1^ * ,{\mathit{\boldsymbol{\xi }}_2} - \mathit{\boldsymbol{\xi }}_2^ * , \cdots ,{\mathit{\boldsymbol{\xi }}_N} - \mathit{\boldsymbol{\xi }}_N^ * } \right)^{\text{T}}} \subseteq {\mathbb{R}^{2N \times 1}} $

(11)

图像雅可比矩阵为:

$ \mathit{\boldsymbol{J}} = {\left( {{\mathit{\boldsymbol{J}}_1},{\mathit{\boldsymbol{J}}_2}, \cdots ,{\mathit{\boldsymbol{J}}_N}} \right)^{\text{T}}} \subseteq {\mathbb{R}^{2N \times 4}} $

对线速度与角速度分别用独立的伺服增益进行解耦, $\mathit{\boldsymbol{\dot e}} = \mathit{\boldsymbol{J}} \cdot {(\mathit{\boldsymbol{v}}, \mathit{\boldsymbol{\omega }})^{\rm{T}}} $转换为:

$ \left\{ \begin{array}{l} \mathit{\boldsymbol{v}} = - {\lambda _v} \cdot {{\mathit{\boldsymbol{\hat J}}}_v} \cdot \mathit{\boldsymbol{e}}\\ \mathit{\boldsymbol{\omega }} = - {\lambda _v} \cdot {{\mathit{\boldsymbol{\hat J}}}_v} \cdot \mathit{\boldsymbol{e}} \end{array} \right. $

(12)

其中, λ_v与λ_ω分别为线速度与角速度的伺服增益, $ \hat{\boldsymbol{J}}_{{v}} \subseteq \mathbb{R}^{3 \times 2 {N}}$为$\hat{\boldsymbol{J}} \subseteq \mathbb{R}^{4 \times 2 N} $的前3行所组成的子矩阵, $ \hat{\boldsymbol{J}}_{\omega} \subseteq \mathbb{R}^{1 \times 2 N}$为$ \hat{\boldsymbol{J}}$的第4行组成的子矩阵。

综上, 本文将(v_x, v_y, v_z, ω_z)作为旋翼无人机控制参数, 并在线速度、角速度上分别设置伺服增益, 建立了旋翼无人机解耦的视觉伺服模型。在实际应用中, 滚转角与俯仰角特别小但是不为0, 本文利用旋翼无人机的动力学原理, 通过v=(v_x, v_y, v_z, ω_z)^T来计算预计的滚转角和俯仰角, 再使用动力学扩展的方法计算俯仰角速度ω_x和滚转角速度ω_y, 即将v=(v_x, v_y, v_z, ω_z)^T扩展为v=(v_x, v_y, v_z, ω_x, ω_y, ω_z)^T。通过上述方法实现对线速度与角速度的动力学解耦, 并且文献[9-10]已通过李亚普诺夫稳定性原理证明了JD-IBVS的稳定性。

1.4 基于费尔曼链码的视觉特征提取算法

视觉特征提取算法是旋翼无人机VS控制的基础, 本文使用基于轮廓的图像特征提取算法, 使用费尔曼链码^[12]来描述提取目标的轮廓。费尔曼链码的轮廓像素记为A={P_i|i=1, 2, …, N}, 则对目标进行轮廓特征提取得到的轮廓链码表示为C={C_i|i=1, 2, …, N}, 其中, 费尔曼链码的方向数为K。在使用这种方法之前, 需要建立标准轮廓库, 将获取到的目标轮廓在标准轮廓库中找到其对应的形状。标准轮廓库中的轮廓为D={D_i|i=1, 2, …, N}, 使用一阶差分对费尔曼链码进行旋转归一化, 旋转归一化如式(13)所示。

$ {d_i} = \left\{ \begin{array}{l} {C_i} - {C_N},i = 1\\ {C_i} - {C_{i + 1}},i > 1\;且\;{C_i} - {C_{i + 1}} > 0\\ K + {C_i} - {C_{i - 1}},i > 1\;且\;{C_i} - {C_{i - 1}} \le 0 \end{array} \right. $

(13)

在进行费尔曼链码的旋转归一化后, 计算提取出的轮廓与标准轮廓之间的lenvenshtein距离^[13]记为L, 并设置阈值。如果L小于阈值则目标轮廓被正确识别, 反之则没有被成功识别。

在识别出目标的轮廓之后, 设定第i个目标的所识别的轮廓像素集合为A_i, p_jⁱ(x_jⁱ, y_jⁱ)为A_i中的某一个点坐标, n_i为A_i中轮廓像素点个数。f_i(x_i, y_i)为第i个目标的中心特征点, 则中心特征点为边缘轮廓像素的平均值。中心特征点就是本文所需要提取出的目标特征点, 其计算公式如(14)所示。

$ {f_i} = \sum\limits_{j = 1}^{{n_i}} {p_j^i/{n_i}} ,p_j^i \in {A_i} $

(14)

1.5 旋翼无人机解耦视觉伺服控制的框架

旋翼无人机通过底部视觉采集图像, 并将采集的图像无线传输到计算机上进行处理。视觉处理算法采用基于费尔曼链码的特征提取算法, 首先提取出目标的轮廓, 然后计算轮廓像素的平均值得到目标的中心特征点, 目标的中心特征点作为目标的特征点, 在提取出目标的中心特征点之后使用JD-IBVS控制形成基于特征误差的闭环控制, 其基本框架如图 3所示。

2 实验与结果分析 2.1 实验准备

本文实验在机器人操作系统(Robot OperatingSystem, ROS)^[14]下进行。Gazebo7.5^[15]提供了一种可以自行设计开发机器人模型的平台, 并可以模拟不同的现实世界场景, 因此, 本文仿真实验在Gazebo7.5下进行。本文通过设计PID追踪方法^[16]、IBVS追踪方法、JD-IBVS追踪方法的对比实验来验证本文追踪算法的效果。追踪实验从追踪不同线速度的相同目标时未丢失目标概率和追踪同一目标在不同角速度下的丢失目标的概率2个角度进行对比。轮式机器人的运动路径包括匀速直线行驶与以一定的角速度转弯90°, 然后继续直线运动一定的距离, 因此目标的运动包括直线行驶与匀速转弯2个不同的动作, 实验场景包括实物实验与仿真实验2种不同的场景。在实验开始前首先设置实验参数, 实验参数设置如表 1所示。

2.2 视觉图像处理

视觉伺服控制的基础是视觉特征点提取, 本文使用一种基于费尔曼链码的图像特征提取算法进行图像特征提取。该算法首先提取出目标的轮廓, 然后计算轮廓像素的平均值做为目标的中心特征点, 也就是所需要提取出目标的特征点。旋翼无人机底部视觉处理的实验如图 4所示。

2.3 旋翼无人机目标追踪实验

追踪实验场景如图 5所示。在实验场景中放置轮式机器人, 编写Python脚本使用键盘控制轮式机器人的运动, 设定每回合目标运动的路径相同、运动速度不同, 测试不同的追踪方法的追踪效果。将旋翼无人机起飞至轮式机器人的上方, 控制轮式机器人的运动, 测试在不同的追踪方法的情形下旋翼无人机的追踪效果。为了保证在视觉伺服的过程中, 旋翼无人机可以看到目标的全部特征, 设定旋翼无人机飞行至离目标1.0 m的正上方之后才开始进行视觉伺服控制实验。本文设定旋翼无人机的滚转角与俯仰角初始值为0°。在实际应用中, 通过旋翼无人机的位姿调节与1.2节介绍的方法进行动力学扩展。实际实验测得旋翼无人机的滚转角与俯仰角的波动范围为-5°~5°, 这说明了本文对模型进行简化即不由视觉伺服获取旋翼无人机的俯仰角与滚转角的合理性。

实验使用控制变量法, 控制目标以不同的线速度运动, 线速度范围为0.05 m/s~0.5 m/s。旋翼无人机起飞追踪, 保证目标以相同的角速度、不同的线速度运动。为了避免实验的偶然性, 对于目标每次不同的线速度运动, 旋翼无人机追踪10次, 计算为未丢失目标的概率(未丢失次数/总次数)。

图 6展示了仿真环境下的旋翼无人机追踪曲线图。在仿真环境下, PID追踪方法在目标运动速度为0.4 m/s时丢失目标, IBVS追踪方法与JD-IBVS追踪方法在0.45 m/s时丢失目标。在图 6(a)中, JD-IBVS在速度为0.25 m/s、0.35 m/s和0.4 m/s时的目标未丢失概率均高于IBVS; 在图 6(b)中, 在目标以不同的角速度运动时, 目标运动的角速度范围是1.1 rad/s~2.0 rad/s, JD-IBVS追踪方法的目标丢失概率低于PID追踪方法与IBVS追踪方法。

图 7展示了实物环境下的旋翼无人机追踪曲线图。相对于仿真条件下实验效果, 实物条件下的追踪效果较差, 这是由于实物实验中环境的不确定性导致的, 但是曲线整体的趋势与仿真实验一致。在图 7(a)中, PID追踪方法在0.4 m/s时丢失目标, IBVS追踪方法在0.45 m/s时丢失目标, JD-IBVS方法在0.5 m/s时丢失目标。在图 7(b)中, JD-IBVS追踪方法在1.9 rad/s时丢失目标, 相对于PID追踪方法、IBVS追踪方法目标丢失概率最小, 追踪效果最好。

仿真实验与实物实验结果表明, 随着目标的运动速度加快, 使用3种方法的未丢失目标的概率越来越小, 本文的解耦视觉伺服控制方法的追踪效果优于其他2种方法。

3 结束语

针对传统机器人PID控制与视觉伺服控制的不足, 考虑到旋翼无人机的欠驱动、强耦合的动力学特性, 本文提出一种旋翼无人机的解耦视觉伺服控制方法, 并将其应用于旋翼无人机的追踪场景。在仿真平台与实物平台中对目标设置不同的线速度和角速度进行追踪实验, 结果表明, 相比于传统的PID控制方法和基于图像视觉伺服控制方法, 本文方法可达到更好的追踪效果。