【论文阅读】《Robust Visual Servoing Control for Ground Target Tracking of Quadrotors》

本文2020年是发表在IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY上有关于无人机的视觉伺服跟踪控制方面的论文。

1. 摘要

在本文中，针对具有向下单目相机的四旋翼无人机在室外环境中执行地面目标跟踪任务的鲁棒位置视觉伺服控制问题进行了研究。目的是通过使用最小传感器组来调节四旋翼相对于地面目标的位置，所述的最小传感器组包括惯性测量单元（IMU）、超声波传感器和视觉传感器。在视觉伺服控制系统中设计了鲁棒补偿器，以提高视觉伺服控制系统对平移和旋转四旋翼动力学系统中的非线性、耦合和不确定性（包括参数不确定性和外部干扰）的鲁棒性。理论分析和实验结果证明了所提出的鲁棒视觉伺服控制方法的有效性。

2. 简介

无人机（UAVs）飞行器近年来备受关注，可应用于农业植保、物流运输、环境监测等多个领域[1]。四旋翼飞行器作为一种经典的飞行器，其机械结构简单、成本低，具有垂直起降和低空悬停的能力。四旋翼飞行器的自主控制需要通过不同的传感器组合来估计其状态的知识。因此，经常使用全球定位系统(GPS)和基于视觉的系统。然而，GPS在杂乱的城市地区，受卫星信号衰减的影响，不可靠，无法满足高精度的要求。另一种方法是将GPS与低成本传感器（如惯性测量单元（IMU））集成，如[2]所示。然而，局部精度仍然难以满足四旋翼飞行器着陆或跟踪地面目标等任务。在这种情况下，通常使用视觉传感器来提高定位精度。视觉伺服是一种通过视觉信息控制车辆运动以达到期望跟踪的技术，详见[3]。视觉伺服可以分为两大分支：基于位置的视觉伺服(PBVS)(参见[4]，[5])和基于图像的视觉伺服(IBVS)(参见[6]-[8]，仅举几例)，这取决于视觉测量是否直接应用于控制律设计。在PBVS方法中，图像特征被视为控制系统的中间步骤，用于重建目标的位置和速度。PBVS的优点是将控制器设计问题与基于视觉的状态估计问题分离开来。然而，如[9]所阐述的，它需要视觉目标的先验几何信息，并且在状态估计过程中计算量大，对摄像机的标定精度要求高。相比之下，IBVS方法直接使用图像特征来调节四旋翼机的运动，如[7]所示。IBVS方法计算效率高，对摄像机标定误差具有较强的鲁棒性。然而，如[10]所述，IBVS的实现引入了高度耦合的图像雅可比矩阵，这使得控制方法的设计变得复杂。许多研究对无人机PBVS方法做出了重要贡献。在[5]中，开发了一种多摄像机视觉姿态估计器来估计四旋翼的位置。文献[11]研究了一种基于视觉的特征跟踪系统，用于跟踪目标以估计车辆运动。[5]和[11]中应用的位置和姿态控制器是基于经典的比例积分微分(PID)控制方法开发的。传统的PID控制器可以在较小的不确定性和干扰下保持可接受的性能。然而，由于动力学的高非线性和强不确定性，采用典型的经典控制方法无法保证闭环系统的跟踪性能。为了提高所构建的闭环控制系统对四旋翼动力学非线性的鲁棒性，开发了几种非线性控制器来实现四旋翼飞行器的PBVS控制。文献[12]研究了带有视觉反馈的四旋翼机的反馈线性化技术和类步进控制方法，以实现理想的跟踪。在[13]中，利用视觉反馈对四旋翼飞行器的三种非线性控制技术，即嵌套饱和法、基于backstepping的控制方案和滑模控制方案进行了比较。[13]采用视觉反馈对嵌套饱和法、基于backstepping的控制方案和滑模控制方法进行了比较。然而，对于构建的四旋翼动力系统的不确定抗扰问题，文献[12]和[13]未作进一步分析。在[4]中，采用了一种基于误差sgn鲁棒积分的非线性位置控制器来抑制四旋翼位置动力学中存在的结构/非结构不确定性。然而，在[4]中并没有讨论四旋翼姿态动力学中的不确定性，并且[4]中的工作依赖于假设采用PID控制方法的姿态环具有完美的跟踪性能。对于涉及高度非线性和不确定动力学的四旋翼飞行器，这一假设比较难以满足。为了实现四旋翼飞行器的IBVS控制，已经开发了许多非线性控制器。在[14]中，基于积分反演方法设计了非线性位置控制器，以处理非线性对构建的控制系统的影响。文献[9]定义了一个虚拟图像平面来实现解耦的图像特征动力学，并利用反演技术设计了非线性位置控制器。然而，对于所构建的闭环系统的不确定性拒绝问题，文献[9]和[14]也没有进一步分析。在[7]中，开发了一种自适应反步位置控制器来补偿不确定性，如空气动力学常数误差和姿态估计偏差。然而，[7]中的不确定性没有得到充分的讨论，[7]和[9]的工作也依赖于采用PID控制方法的姿态环具有完美跟踪性能的假设。在[15]中，采用反步和动态曲面控制技术，设计了一种自适应控制系统，用于抑制四旋翼飞行器平移和旋转动力学中的非线性和外部干扰。但该控制器的有效性仅在仿真中得到验证，未进一步讨论参数不确定性的影响。尽管基于经典控制律的四旋翼视觉伺服控制已经有了很多研究，但据我们所知，针对平移和旋转四旋翼动力学中高度非线性和不确定性的鲁棒视觉伺服控制器设计仍然是一个开放的领域。因此，本文提出了一种鲁棒视觉伺服控制器，用于四旋翼飞行器在室外环境中跟踪地面目标。**将视觉信息与来自IMU和超声波传感器的数据融合，以估计四旋翼飞行器相对于地面目标的位置。**与以往关于四旋翼机视觉伺服控制的研究相比，本文的设计和分析结果具有以下特点：

1. 首先，将视觉信息与IMU和超声波传感器数据相结合，估计出载具的位置和速度，然后反馈到控制回路。该过程不需要地面目标的先验几何信息，与[4]、[5]和[11]中的传统PBVS方法相比，计算效率更高，并且可以避免在[7]、[9]和[14]中的IBVS方法中引入图像雅可比公式。
2. 其次，在这个简短的四旋翼模型涉及全旋转和平动非线性和耦合动力学。但是，在[4]、[7]和[9]中，为了方便控制器的设计过程，采用了简化的四旋翼模型。
3. 第三，在四旋翼飞行器的平动和转动动力学模型中考虑了不确定性，包括外部干扰和参数不确定性。证明了位置和姿态跟踪误差最终收敛到原点的给定邻域，该邻域由给定的小值限定。但在以往的文献中，[4]、[7]、[15]只讨论了部分不确定性，而在[5]、[9]、[11]-[14]中忽略了不确定性拒绝问题。

3. 问题建模

3.1 四旋翼模型描述

如Fig.1. 中所示，定义：
E ^ H = { e ^ H o , e ^ H x , e ^ H y , e ^ H z } \hat{E}^{H}=\{\hat{e}_{Ho},\hat{e}_{Hx},\hat{e}_{Hy},\hat{e}_{Hz}\} E^H={e^Ho​,e^Hx​,e^Hy​,e^Hz​}为局部水平坐标系，原点${\hat{e}}_{Ho}$表示四旋翼起飞的位置。
E ^ B = { e ^ B o , e ^ B x , e ^ B y , e ^ B z } \hat{E}^{B}=\{\hat{e}_{Bo},\hat{e}_{Bx},\hat{e}_{By},\hat{e}_{Bz}\} E^B={e^Bo​,e^Bx​,e^By​,e^Bz​}为机体坐标系，原点${\hat{e}}_{Bo}$位于四旋翼的质心。
$p^H=[p_x^H,p_y^H,p_z^H{]}^T\in {\mathbb{R}}^{3\times 1}$表示四旋翼在${\hat{E}}^H$中的位置。
$v^H=[v_x^H,v_y^H,v_z^H{]}^T\in {\mathbb{R}}^{3\times 1}$表示四旋翼在${\hat{E}}^H$中的速度。
${\Theta }^B=[\varphi ,\theta ,\psi ]\in {\mathbb{R}}^{3\times 1}$表示四旋翼在${\hat{E}}^B$中的欧拉角。
${\Omega }^B=[{\Omega }_i^B]\in {\mathbb{R}}^{3\times 1}]$表示四旋翼在${\hat{E}}^B$中的角速度。
那么，四旋翼飞行器的平移动力学模型和旋转动力学模型如下:
$$\begin{gathered} {\dot{p}}^H=v^H+d_p^H \\ m{\dot{v}}^H=R_f{T}_p+d_v^H \\ {\dot{\Theta }}^B=R_{\Theta }{\Omega }^B+d_{\Theta }^B \\ J{\dot{\Omega }}^B={\tau }_b-C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B+d_{\Omega }^B \end{gathered}$$
其中$m$表示四旋翼的重量，$J$是在${\hat{E}}^B$下的惯性矩阵，$R_f\in SO(3)$是[16]中表示从${\hat{E}}^B$到${\hat{E}}^H$的变换矩阵，并且$C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B$是[17]中描述的科里奥利项。欧拉角${\Theta }^B$和角速度${\Omega }^B$之间的关系由变换矩阵$R_{\Theta }$描述，该变换矩阵满足：
$$R_{\Theta }=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi \sec \theta & \cos \varphi \sec \theta \end{array}\right]$$
$d_i^H(i=p,v)$和$d_j^B(j=\Theta ,\Omega )$表示外部的有界扰动。$T_p$表示外力，其中$T_p=c_{3,3}{T}_s-mg{R}_f^T{c}_{3,3}$其中$c_{n,j}$表示$n\times 1$向量，第$j$个元素为1，其他元素为$0$,g为重力常数。${\tau }_b$是在坐标系${\hat{E}}^B$中的四旋翼上的外扭矩，其中${\tau }_b=[{\tau }_x,{\tau }_y,{\tau }_z{]}^T$,${\tau }_x=l_a(T_1-T_2-T_3+T_4)/\sqrt{2}$，${\tau }_y=l_a(T_1+T_2-T_3-T_4)/\sqrt{2}$，${\tau }_z=k_t(T_1-T_2+T_3-T_4)$，其中$l_a$表示四旋翼的质心到转子之间的距离，$k_t$表示从力到扭矩的转换比例因子，$T_s$为转子合力，$T_s=T_1+T_2+T_3+T_4$。$T_i(i=1,2,3,4)$表示转子$i$产生的推力，由$T_i=k_{\omega }{\omega }_i^2$给出，其中$k_{\omega }$是正常数。

符号汇总

符号	意义
$p$	四旋翼在${\hat{E}}^H$坐标系中的位置
$v$	四旋翼在${\hat{E}}^H$坐标系中的速度
$d_p^H$	四旋翼在${\hat{E}}^H$坐标系中的位置外部有界扰动
$m$	四旋翼无人机质量
$R_f$	从${\hat{E}}^B$到${\hat{E}}^H$的变换矩阵：$$R_f=R_z^T(\psi )R_y^T(\theta )R_x^T(\varphi )$$
$T_i$	转子$i$产生的推力，$$T_i=k_{\omega }{\omega }_i^2$$
$k_{\omega }$	拉力系数，正常数
$T_s$	总推力，$$T_s=T_1+T_2+T_3+T_4$$
$T_p$	合外力，$$T_p=T_s-mg{R}_f^T$$
$d_p^H$	四旋翼在${\hat{E}}^H$坐标系中的速度外部有界扰动
${\Theta }^B$	四旋翼在${\hat{E}}^B$坐标系中的欧拉角
${\Omega }^B$	${\Omega }^B=[{\Omega }_i^B]\in {\mathbb{R}}^{3\times 1}]$表示四旋翼在${\hat{E}}^B$中的角速度
$R_{\Theta }$	变换矩阵用于描述欧拉角和角速度之间的关系，$${\dot{\Theta }}^B=R_{\Theta }{\Omega }^B$$
$d_{\Theta }^B$	四旋翼在${\hat{E}}^H$坐标系中的欧拉角外部有界扰动
$J$	四旋翼的转动惯量，$J\in {\mathbb{R}}^{3\times 3}$
${\tau }_b$	螺旋桨在机体轴上产生的力矩，$${\tau }_b=[{\tau }_x,{\tau }_y,{\tau }_z{]}^T$$
${\tau }_x$	$${\tau }_x=l_a{k}_{\omega }\frac{\sqrt{2}}{2}({\omega }_1^2-{\omega }_2^2-{\omega }_3^2+{\omega }_4^2)$$
${\tau }_y$	$${\tau }_y=l_a{k}_{\omega }\frac{\sqrt{2}}{2}({\omega }_1^2+{\omega }_2^2-{\omega }_3^2-{\omega }_4^2)$$
${\tau }_z$	$${\tau }_z=k_t({\omega }_1^2-{\omega }_2^2+{\omega }_3^2-{\omega }_4^2)$$
$k_t$	转矩系数，正常数
$C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B$	科里奥利项，${\mathbb{R}}^{3\times 3}$$$\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$$
$c_{11}$	$0$
$c_{12}$	$$(J_{\theta }-J_{\psi })(\dot{\theta }\cos \varphi \sin \varphi +\dot{\psi }\sin {}^2\varphi \cos \theta )/J_{\varphi }+[(J_{\psi }-J_{\theta })\dot{\psi }\cos {}^2\varphi \cos \theta -J_{\varphi }\dot{\psi }\cos \theta ]/J_{\varphi }$$
$c_{13}$	$$(J_{\psi }-J_{\theta })\dot{\psi }\cos \varphi \sin \varphi {\cos }^2\theta /J_{\varphi }$$
$c_{21}$	$$(J_{\psi }-J_{\theta })(\dot{\theta }\cos \varphi \sin \varphi +\dot{\psi }\sin {}^2\varphi \cos \theta )/J_{\theta }+[(J_{\theta }-J_{\psi })\dot{\psi }\cos {}^2\varphi \cos \theta +J_{\varphi }\dot{\psi }\cos \theta ]/J_{\theta }$$
$c_{22}$	$$(J_{\psi }-J_{\theta })\dot{\varphi }\cos \varphi \sin \varphi /J_{\theta }$$
$c_{23}$	$$[-J_{\varphi }\dot{\psi }\sin \theta \cos \theta +J_{\psi }\dot{\psi }\cos {}^2\varphi \cos \theta \sin \theta ]/J_{\theta }+\dot{\psi }\sin {}^2\varphi \cos \theta \sin \theta$$
$c_{31}$	$$[(J_{\theta }-J_{\psi })\dot{\psi }{\cos }^2\theta \sin \varphi \sin \theta -J_{\varphi }\dot{\theta }\cos \theta ]/J_{psi}$$
$c_{32}$	$$(J_{\psi }-J_{\theta })(\dot{\theta }\cos \varphi \sin \varphi \sin \theta +\dot{\varphi }{\sin }^2\varphi \cos \theta )/J_{\psi }+[(J_{\theta }-J_{\psi })\dot{\varphi }{\cos }^2\varphi \cos \theta +J_{\varphi }\dot{\psi }\sin \theta \cos \theta ]/J_{\psi }-J_{\theta }\dot{\psi }{\sin }^2\varphi \sin \theta \cos \theta /J_{\psi }-\dot{\psi }{\cos }^2\varphi \cos \theta \sin \theta$$
$c_{33}$	$$[-J_{\psi }\dot{\theta }{\cos }^2\varphi \sin \theta \cos \theta -J_{\theta }\dot{\theta }{\sin }^2\varphi \cos \theta \sin \theta ]/J_{\psi }+[(J_{\theta }-J_{\psi })\dot{\varphi }\cos \varphi \sin \varphi {\cos }^2\theta +J_{\varphi }\dot{\theta }\sin \theta \cos \theta ]/J_{\psi }$$

3.2 控制问题描述

本文的控制目标是设计一种鲁棒的视觉伺服控制器，利用视觉系统提供的位置信息，在四旋翼动力学的高非线性和不确定性条件下跟踪地面目标。

1. 设$p_r^H=[p_{ri}^H]\in {\mathbb{R}}^{3\times 1}$表示在坐标系${\hat{E}}^H$中的参考位置，$v_r^H$表示在坐标系${\hat{E}}^H$中的参考速度，${\Theta }_r^B=[{\varphi }_r,{\theta }_r,{\psi }_r{]}^T\in {\mathbb{R}}^{3\times 1}$表示在${\hat{E}}^B$坐标系中的参考姿态角，以及${\Omega }_r^B=[{\Omega }_{ri}^B]\in {\mathbb{R}}^{3\times 1}$表示在${\hat{E}}^B$坐标系中的参考角速度。
2. 设$u_p=[u_{pi}]\in {\mathbb{R}}^{3\times 1}$表示位置控制器的输出，$u_v=[u_{vi}]\in {\mathbb{R}}^{3\times 1}$表示速度控制器的输出，$u_{\Omega }=[u_{\Omega i}]$表示角速度控制器的输出。$u_{pi}(i=1,2,3)$也表示四旋翼飞行器在坐标系${\hat{E}}^H$下的速度控制器的参考，$u_{vi}(i=1,2,3)$表示四旋翼飞行器在坐标系${\hat{E}}^H$下的姿态控制器的横滚和俯仰参考，$u_{\Theta i}$和$u_{\Omega i}$表示是控制分配器的输入(横滚、俯仰、偏航和推力)，它将输入转换成执行器的输出，驱动电机。
3. 设$G_v$和$G_{\Omega }$分别表示常数矩阵$G_v=\text{diag}(g,-g,k_{st}/m)$和$G_{\Omega }=J^{-1}\text{diag}(k_{sx},k_{sy},k_{sz})$。其中$k_{sx}、k_{sy}、k_{sz}$和$k_{st}$表示横滚、俯仰、偏航和推力的标度值。并且满足${\tau }_x=k_{sx}{u}_{\Omega 1}$，${\tau }_y=k_{sy}{u}_{\Omega 2}$，${\tau }_z=k_{sz}{u}_{\Omega 3}$和$T_s=k_{st}{u}_{v3}$
4. 设$G_v=G_v^N+G_v^{\Delta }$，$G_{\Omega }=G_{\Omega }^N+G_{\Omega }^{\Delta }，J=J^N+J^{\Delta }$，其中$G_v^N，G_{\Omega }^N$和$J$表示标称参数，并假设参数不定性$G_v^{\Delta }$，$G_{\Omega }^{\Delta }$和$J^{\Delta }$是有界的。那么无人机的平移和旋转动力学可以重写为：
   $$\begin{gathered} {\dot{p}}^H=u_p+\Delta p \\ {\dot{v}}^H=G_v^H{u}_v-g{c}_{3,3}+\Delta v \\ {\dot{\Theta }}^B=R_{\Theta }{u}_{\Theta }+\Delta \Theta \\ {\dot{\Omega }}^B=G_{\Omega }^N{u}_{\Omega }-(J^N{)}^{-1}C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B+{\Delta }_{\Omega } \end{gathered}$$
   其中${\Delta }_i(i=p,v,\Theta ,\Omega )$为等效扰动，包括非线性和不确定性，其中：
   $$\begin{gathered} \Delta p=d_p^H \\ \Delta v=m^{-1}({\Delta }_{N_v}+d_v^H+G_v^{\Delta }+u_v) \\ {\Delta }_{\Theta }=d_{\Theta }^B \\ {\Delta }_{\Omega }=-J^{-1}C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B+G_{\Omega }^{\Delta }{u}_{\Omega }+(J^N{)}^{-1}C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B+J^{-1}{d}_{\Omega }^B \end{gathered}$$
   其中${\Delta }_{N_v}$描述了如下的非线性项：
   $$\Delta N_v=\left[\begin{array}{c}{k}_{st}{u}_{v3}\sin \varphi \sin \psi +k_{st}{u}_{v3}\cos \varphi \cos \psi \sin \theta \\ -m{G}_{v1}^N{u}_{v1}-k_{st}{u}_{v3}\cos \varphi \sin \varphi \sin \psi +k_{st}{u}_{v3}\cos \psi \sin \varphi \\ +m{G}_{v2}^N{u}_{v2}m(\cos \theta \cos \varphi G_{v3}{u}_{v3}-G_{v3}^N{u}_{v3})\end{array}\right]$$
   滚转角$\varphi$和俯仰角$\theta$满足$\varphi \in (-\pi /2,\pi /2)$和$\theta \in (-\pi /2,\pi /2)$，以免出现欧拉角中的奇异性问题。通过忽略等效干扰${\Delta }_i(i=p,v,\Theta ,\Omega )$，这个模型可以被视作标称四旋翼模型。${\Delta }_i$中的不确定性包括参数不确定性和外部干扰。在实际应用中，四旋翼无人机的模型参数无法精确策略，导致参数不确定性$G_v^{\Delta }$,$J_{\Delta }$和$G_{\Omega }^{\Delta }$。在${\Delta }_i$中，$d_p^H$,$d_v^H$,$d_{\Theta }^B$,$d_{\Omega }^B$是作用在四旋翼上的外部干扰。例如，室外环境中的风干扰。因此，真实的四旋翼模型可以看作是加了等效扰动的正常四旋翼动力模型。

3.3 运动估计

如Fig1中所示，四旋翼从正以一定的姿态跟踪地面的目标。相机模型是基于针孔模型。定义 E ^ C = { e ^ C o , e ^ C x , e ^ C y , e ^ C z } \hat{E}^{C}=\{\hat{e}_{Co},\hat{e}_{Cx},\hat{e}_{Cy},\hat{e}_{Cz}\} E^C={e^Co​,e^Cx​,e^Cy​,e^Cz​}为相机坐标系。为了方便描述，将${\hat{E}}^C$和${\hat{E}}^B$对齐。设 E ^ I = { e ^ I o , e ^ I x , e ^ I y , e ^ I z } \hat{E}^{I}=\{\hat{e}_{Io},\hat{e}_{Ix},\hat{e}_{Iy},\hat{e}_{Iz}\} E^I={e^Io​,e^Ix​,e^Iy​,e^Iz​}表示以 { e ^ I x , e ^ I y } \{\hat{e}_{Ix},\hat{e}_{Iy}\} {e^Ix​,e^Iy​}作为基底，与 { e ^ C x , e ^ C y } \{\hat{e}_{Cx},\hat{e}_{Cy}\} {e^Cx​,e^Cy​}平行的二维图像平面。设$P_t$表示地面目标，其位置为$P_t^C=[p_{xt}^C,p_{yt}^C,p_{zt}^C{]}^T\in {\mathbb{R}}^{3\times 1}$在坐标系${\hat{E}}^C$中。$p_t^H=[p_{xt}^H,p_{yt}^H,p_{zt}^H{]}^T\in {\mathbb{R}}^{3\times 1}$表示地面目标相对于四旋翼无人机在${\hat{E}}^H$中的位置。设$P_t^{{}^{\prime }}$表示地面目标在图像坐标系${\hat{E}}^I$中的投影点，位置为$p_t^I=[p_{xt}^I,p_{yt}^I{]}^T=f_C[p_{xt}^C/p_{zt}^C,p_{yt}^C/p_{zt}^C]$，其中$f_C$表示相机的焦距。$p_t^C$和$p_t^H$之间的关系可以表示为：
$$[p_{xt}^H,p_{yt}^H,p_{zt}^H{]}^T=R_f[p_{xt}^C,p_{yt}^C,p_{zt}^C{]}^T$$对上式两边同时除以$p_{zt}^C$，可以得到：
$$R_f[p_{xt}^C/p_{zt}^C,p_{yt}^C/p_{zt}^C,1{]}^T=[p_{xt}^H/p_{zt}^C,p_{yt}^H/p_{zt}^C,p_{zt}^H/p_{zt}^C{]}^T$$其中$R_f$，$p_{xt}^C/p_{zt}^C$和$p_{yt}^C/p_{zt}^C$从IMU传感器和视觉测量中已知。为了计算深度值$p_{zt}^C$，可以使用目标到地面的信息，使用超声波传感器和IMU的数据估计$p_{zt}^H$，则可以根据上式计算出$p_{zt}^C$，用上式重构$p_{zt}^H$。这种方法实现方便，可以在不需要先验的地面目标的几何信息的情况下，精确的测量目标相对于四旋翼的位置。$p_t^H$直接馈送到卡尔曼滤波器中，用以估计四旋翼飞行器相对于地面目标的位置和速度。由于深度值$p_t^C$是使用IMU和超声波传感器估计的，因此仅一个视觉特征就足以计算四旋翼相对于地面目标的位置。估计的四旋翼的位置和速度，将在鲁棒控制器的设计过程中使用，这将在下一节中展示。

4. 鲁棒控制器设计

4.1 控制律设计

整个闭环四旋翼控制系统如上图中所示。在整个系统中，基于视觉的估计系统为位置控制器提供位置估计，$Z_k$和$X_k$表示卡尔曼滤波器的测量和状态。定义$e_p=[e_{px},e_{py},e_{pz}{]}^T=[e_{pi}]\in {\mathbb{R}}^{3\times 1}$为位置误差，$e_v=[e_{vi}]\in {\mathbb{R}}^{3\times 1}$为速度误差，$e_{\Theta }=[e_{\Theta i}]\in {\mathbb{R}}^{3\times 1}$为角度误差，$e_{\Omega }=[e_{\Omega i}]\in {\mathbb{R}}^{3\times 1}$为角速度误差，其中$e_p=p^H-p_r^H$,$e_v=v^H-v_r^H$,$e_{\Theta }={\Theta }^B-{\Theta }_r^B$
和$e_{\Omega }={\Omega }^B-{\Omega }_r^B$。由前述推导的模型，位置姿态控制率可以构造为：
$$\begin{gathered} u_p=-K_{pe}{e}_p+{\dot{p}}_r^H+u_p^R \\ {u}_v=(G_v^N{)}^{-1}(-K_{ve}+{\dot{v}}_r^H+g{c}_{3,3}+u_v^R) \\ {u}_{\Theta }=(R_{\Theta }{)}^{-1}(-K_{\Theta e}{e}_{\Theta }+{\dot{\Theta }}_r^B+u_{\Theta }^R) \\ {u}_{\Omega }=(G_{\Omega }^N{)}^{-1}(-K_{\Omega e}+{\dot{\Omega }}_r^B+(J^N{)}^{-1}C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B+u_{\Omega }^R) \end{gathered}$$其中正对角矩阵$K_{ie}(i=p,v,\Theta ,\Omega )$表示标称反馈控制器增益，$K_{ie}=\text{diag}(k_{i1},k_{i2},k_{i3})$。令$u_i^R(i=p,v,\Theta ,\Omega )$表示鲁棒控制补偿输入，并且可以有下式给出：
$$u_i^R(s)=-F_i(s)\Delta i(s)$$
其中$F_i(s)=\text{diag}(F_{i1}(s),F_{i2}(s),F_{i3}(s))$并且$F_{ij}(s)=f_{ij}/(f_{ij}+s)$，正常数$f_{ij}(j=1,2,3)$待定。u_{i}^{R}是低通滤波器的输出，如果适当选择滤波器参数$f_{ij}$，它可以抑制等效干扰。然而，在实际应用中，${\Delta }_i(i=p,v,\Theta ,\Omega )$并不能直接获得，由下式推导：
$$\begin{gathered} {\Delta }_p=v^H-u_p \\ {\Delta }_v={\dot{v}}^H+g{c}_{3,3}-G_v^N{u}_v \\ {\Delta }_{\Theta }={\dot{\Theta }}^B-R_{\Theta }{u}_{\Theta } \\ {\Delta }_{\Omega }={\dot{\Omega }}^B+(J^N{)}^{-1}C({\Theta }^B,{\dot{\Theta }}^B){\dot{\Theta }}^B-G_{\Omega }^N{u}_{\Omega } \end{gathered}$$
将上述两式结合，可以得到$u_p^R$和$u_v^R$表示如下：
$$\begin{gathered} {\dot{x}}_p=-f_p{x}_p+f_p^2{p}^H+f_p{u}_p \\ {u}_p^R=-f_p{p}^H+x_p \\ {\dot{x}}_v=-f_v{x}_v+f_v^2{v}^H+fv(G_v^N{u}_v-g{c}_{3,3}) \\ {u}_v^R=-f_v{v}^H+x_v \end{gathered}$$
其中$x_p$和$x_v$表示鲁棒滤波器的状态。正矩阵$f_i(i=p,v)$满足$f_i=\text{diag}(f_{i1},f_{i2},f_{i3})$。$u_{\Theta }^R$和$u_{\Omega }^R$可以用类似的状态空间形式来实现。