CN104062977A - 基于视觉slam的四旋翼无人机全自主飞行控制方法 - Google Patents

Abstract

本发明涉及无人机全自主飞行控制方法，为提供一种基于嵌入式架构的四旋翼无人机视觉控制系统，实现在无GPS信号环境下，不依赖任何环境信息的无人机自主定位与飞行控制功能；本发明采用的技术方案是：基于视觉SLAM的四旋翼无人机全自主飞行控制方法，包括如下步骤：利用安装在四旋翼无人机底部的摄像头采集图像信息，利用集成惯性导航单元获取无人机的姿态角及加速度信息，这两部分信息作为输入，运行改进的视觉SLAM算法获取四旋翼无人机三维位置和姿态信息；采用扩展卡尔曼滤波器融合视觉位置信息和惯性导航单元提供的三维加速度，设计PID控制器，实现四旋翼无人机的全自主飞行控制。本发明主要应用于无人机全自主飞行控制。

Description

基于视觉SLAM的四旋翼无人机全自主飞行控制方法

技术领域

本发明涉及一种四旋翼无人机全自主飞行控制方法，特别是涉及一种在GPS信号缺失环境下基于视觉SALM的四旋翼无人机自主定位与控制问题。

背景技术

无人机是一种能够通过无线遥控或程序来操纵的无人驾驶飞行器。近年来，无人机在军事及民用等诸多领域的应用引起了人们的广泛关注。比如，在军事上能够进行侦查、监测及小范围内的攻击等；在民用上，可用于航拍、测绘、遥感、农药喷洒、高压输电线路的巡线及地震抢险等。四旋翼无人机作为小型无人机的一种，具有机动性强、结构设计简单、安全性高等突出优点，且其能够近距离靠近目标，更加适合室内等复杂环境中的飞行作业。

四旋翼无人机的定位问题主要是指利用自身传感器确定无人机在飞行环境中相对于惯性坐标系的位置和姿态信息。准确的位姿估计是实现四旋翼无人机安全飞行、轨迹规划以及目标跟踪等复杂飞行任务的前提和基础。广泛使用的定位方法主要是基于GPS的定位方法，然而在一些特殊环境，例如楼群之间、大型建筑物内部等，GPS信号很弱或缺失，传统的基于GPS信号的定位方法较难应用。目前正在研究的不依赖GPS的定位方法主要分为基于激光雷达的定位方法和基于视觉的定位方法。在基于视觉的定位方法中，基于视觉SLAM(同步定位与建图，Simultaneous Localization and Mapping)的定位方法仅使用机载摄像头作为外部传感器，具有体积小、重量轻、价格低、精度高、应用范围广等优势，这也使得视觉导航逐渐成为四旋翼无人机自主控制研究的主要趋势。

常用的视觉SLAM主要依靠PTAM(并行跟踪与建图，Parallel Tracking and Mapping)来实现，PTAM是由Klein和Murray于2007年提出来的(会议：the6th IEEE and ACM International Symposium on Mixed and Augmented Reality；著者：Klein and D.Murray；出版年月：2007年；文章题目：Parallel Tracking and Mapping for Small AR Workspaces；页码：225-234)。其基本原理是通过跟踪地图中关键帧及关键帧中的特征点进行位置和姿态估计，并且建立的地图能够随定位过程而逐步扩充。

近几年，一些国内外高校和研究机构开始尝试采用视觉SLAM进行四旋翼无人机自主控制，并取得了一些初步成果。瑞士苏黎世联邦理工学院率先将PTAM算法应用于四旋翼无人机控制上，并先后实现了基于地面站架构和基于嵌入式架构的自主飞行控制(期刊：Journal of Field Robotics；著者：Weiss S,Achtelik M W,Lynen S,Achtelik M C,Kneip L,Chli M,Siegwart R；出版年月：2013年；文章题目：Monocular Vision for Long-term Micro Aerial Vehicle State Estimation:A Compendium；页码：803-830)。德国慕尼黑工业大学应用视觉SLAM算法实现了四旋翼无人机的自主定位与导航。该研究组使用AR.Drone无人机作为研究平台，视觉SLAM算法运行在地面站上，但尚未实现机载运算，视觉控制系统存在较大的时间延迟(期刊：IMU；著者：Engel J,Sturm J；出版年月：2012年；文章题目：Accurate figure flying with a quadrocopter using onboard visual and inertial sensing；页码：240-245)。美国加州大学河滨分 校的研究人员中也使用了视觉SLAM作为位置信息来源，实现四旋翼无人机的自主定位功能，但该算法运行在地面站上，并未实现机载控制(期刊：Autonomous Robots；著者：Ghadiok V,Goldin J,Ren W；出版年月：2012年；文章题目：On the design and development of attitude stabilization,vision-based navigation,and aerial gripping for a low-cost quadrotor；页码：41-68)。

上述研究方案虽取得了一定的成果，但是大多数研究机构均采用基于地面站架构的控制方法，在实时性、有效性和通用性等方面还存在不足。在这种结构中，机载摄像头采集到的图像信息需经Wi-Fi无线网络传送到地面站处理，处理后产生的控制命令再经Wi-Fi发给无人机的飞行控制器，控制过程存在较大的时间延迟，系统的实时性较差。此外，采用无线数据传输还限制了无人机的飞行范围，增加了其对传输线路的依赖性，也容易受到数据传输过程中的其他信号干扰。

发明内容

本发明所要解决的技术问题是，提供一种基于嵌入式架构的四旋翼无人机视觉控制系统，实现在无GPS信号环境下，不依赖任何环境信息的无人机自主定位与飞行控制功能。

本发明采用的技术方案是：基于视觉SLAM的四旋翼无人机全自主飞行控制方法，包括如下步骤：

利用安装在四旋翼无人机底部的摄像头采集图像信息，利用集成惯性导航单元获取无人机的姿态角及加速度信息，这两部分信息作为输入，运行改进的视觉SLAM算法获取四旋翼无人机三维位置和姿态信息；采用扩展卡尔曼滤波器融合视觉位置信息和惯性导航单元提供的三维加速度，从而得到精确的位置信息，采用上述算法得到的水平位置和速度信息及气压计获得的高度信息作为反馈量，设计PID控制器，实现四旋翼无人机的全自主飞行控制。

所述的将改进的视觉SLAM算法用于四旋翼无人机三维位置和姿态信息的获取是：

采用普通微型机载摄像头获得环境信息的实时彩色图像，通过机载嵌入式计算机运行改进的视觉SLAM算法，手动平行移动机载摄像头，根据同一个特征点的移动描绘出弧线，再根据所有特征点的弧线确定一个特定的平面。

所述的融合视觉位置信息和惯性导航单元提供的三维加速度是采用扩展卡尔曼滤波方法，系统在k时刻的非线性过程方程和测量方程分别为：

x_k＝f(x_k-1,u_k-1)+ω_k-1

z_k＝h(x_k)+ν_k

其中，x_k＝[p_x p_y p_z v_x v_y v_z]^T是系统k时刻的状态向量，[p_x p_y p_z]^T为惯性坐标系下四旋翼无人机的三维位置向量，[v_x v_y v_z]^T为惯性坐标系下四旋翼无人机的三维速度向量，u_k＝[a_x a_y a_z]^T是系统k时刻的输入向量，其中a_x、a_y、a_z为惯性导航单元测得的加速度信息，z_k＝[p_xp_y p_z]^T是系统k时刻的观测向量；f(x_k-1,u_k-1)表示关于x_k-1 和u_k-1的函数；ω_k-1和υ_k为k-1时刻过程激励噪声和k时刻观测噪声；假设过程噪声和观测噪声是相互独立的，且均为零均值白噪声向量，即ω_k～N(0,Q),υ_k～N(0,R)；

在滤波值附近，对上式做泰勒级数展开，忽略二阶以上的高阶项，原系统就近似成为一个线性系统，如下式所示：

x_k＝F_kx_k-1+B_ku_k+ω_k

z_k＝H_kx_k+υ_k

其中，

$F_k=\frac{\∂f(x,u)}{\∂x}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{cccccc}1& 0& 0& {\mathrm{\δ}}_t& 0& 0\\ 0& 1& 0& 0& {\mathrm{\δ}}_t& 0\\ 0& 0& 1& 0& 0& {\mathrm{\δ}}_{\text{t}}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$B_k=\frac{\∂f(x,u)}{\∂u}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{ccc}\frac{1}{2}{\mathrm{\δ}}_t^2& 0& 0\\ 0& \frac{1}{2}{\mathrm{\δ}}_t^2& 0\\ 0& 0& \frac{1}{2}{\mathrm{\δ}}_t^2\\ {\mathrm{\δ}}_t& 0& 0\\ 0& {\mathrm{\δ}}_t& 0\\ 0& 0& {\mathrm{\δ}}_t\end{array}\right]$

$H_k=\frac{\∂h\left(x\right)}{\∂x}{|}_{{\hat{x}}_{k|k-1}}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

在上述式子中δ_t由系统的采样时间确定；

扩展卡尔曼滤波器的循环工作过程为：时间更新方程将当前状态作为先验估计及时地向前投射到测量更新方程，测量更新方程校验先验估计以获得状态的后验估计；其时间更新方程为：

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}=F_x{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}\\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_{k-1}\end{array}\right.,$

状态跟踪方程为：

$\left\{\begin{array}{c}{K}_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R)}^{-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H{\hat{x}}_{k|k-1})\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}\right..$

所述的设计PID控制器，实现四旋翼无人机的自主飞行控制为：

采用提出的改进视觉SLAM和扩展卡尔曼滤波算法而获取四旋翼无人机实时三维位置和速度信息，再利用PID控制器进行水平横向和水平纵向方向控制，再将该控制量发送给飞行控制器，实现四旋翼无人机的水平位置控制；高度方向则采用气压计获得无人机的高度信息，采用PID控制器产生控制量，实现四旋翼无人机高度控制；其PID控制器设计如下：

$u_x=k_{\mathrm{Px}}{e}_x+k_{\mathrm{Ix}}\underset{0}{\overset{t}{\∫}}{e}_x\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dx}}({\stackrel{\·}{x}}_d-v_x)$

$u_y=k_{\mathrm{Py}}{e}_y+k_{\mathrm{Py}}\underset{0}{\overset{t}{\∫}}{e}_y\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dy}}({\stackrel{\·}{y}}_d-v_y)$

$u_z=k_{\mathrm{Pz}}{e}_z+k_{\mathrm{Iz}}\underset{0}{\overset{t}{\∫}}{e}_z\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dz}}{\stackrel{\·}{e}}_z$

e_x＝x_d-x,e_y＝y_d-y,e_z＝z_d-z

其中，k_P、k_I、k_D分别为比例、微分、积分系数，x_d、y_d、z_d为四旋翼无人机参考轨迹，x、y为视觉SLAM算法得到的四旋翼无人机的水平横向、水平纵向位置，z为气压计测得的无人机的高度，v_x、v_y为视觉SLAM算法得到的四旋翼无人机的水平速度，下标x、y、z分别代表水平横向、水平纵向及高度方向。

为避免传统PID控制中因微分运算引起的高频干扰，在水平横向和水平纵向方向使用速度的比例控制代替位置的微分运算。

本发明可带来如下技术效果：

本发明首先对传统视觉SLAM算法进行改进，提高了算法的鲁棒性与普适性，然后利用该算法得到无人机的三维位置信息。在此基础上采用扩展卡尔曼滤波器融合视觉信息和惯性导航单元提供的三维加速度信息，得到较为准确的位置信息，同时提高信号输出频率。最后，利用上述方法获取的无人机位置信息通过PID控制器设计，实现了四旋翼无人机的自主飞行控制。此外，本发明搭建了四旋翼无人机视觉控制系统实验平台，该平台采用嵌入式控制系统架构，使用机载嵌入式计算机运行论文中提出的算法，保证了控制系统的实时性。

附图说明

图1是本发明基于视觉SLAM的四旋翼无人机全自主飞行控制流程图；

图2是本发明采用的基于嵌入式架构的四旋翼无人机结构图；

1：四旋翼无人机本体 2：惯性导航单元

3：机载微型摄像头 4：机载嵌入式计算机

5：飞行控制器

图3a是四旋翼无人机悬停飞行实验中x方向位置变化曲线；

图3b是四旋翼无人机悬停飞行实验中y方向位置变化曲线；

图3c是四旋翼无人机悬停飞行实验中z方向位置变化曲线；

图4a是四旋翼无人机悬停飞行实验中x方向位置误差变化曲线；

图4b是四旋翼无人机悬停飞行实验中y方向位置误差变化曲线；

图4c是四旋翼无人机悬停飞行实验中z方向位置误差变化曲线；

图5a是四旋翼无人机悬停飞行实验中x方向速度变化曲线；

图5b是四旋翼无人机悬停飞行实验中y方向速度变化曲线；

图6a是四旋翼无人机悬停飞行实验中俯仰角变化曲线；

图6b是四旋翼无人机悬停飞行实验中滚转角变化曲线；

图6c是四旋翼无人机悬停飞行实验中偏航角变化曲线；

图7是四旋翼无人机悬停飞行实验中水平方向飞行轨迹。

具体实施方式

下面结合实施例和附图对本发明的基于视觉SLAM的四旋翼无人机自主飞行控制方法做出详细说明。

针对GPS信号缺失环境下无人机的自主飞行控制问题，本发明设计了一种基于视觉的自主定位与控制方法。本发明首先对传统视觉SLAM算法进行改进，提高了算法的鲁棒性与普适性，然后利用该算法得到无人机的三维位置信息。在此基础上采用扩展卡尔曼滤波器融合视觉信息和惯性导航单元提供的三维加速度信息，得到较为准确的位置信息，同时提高信号输出频率。最后，利用上述方法获取的无人机位置信息通过PID控制器设计，实现了四旋翼无人机的自主飞行控制。此外，本发明搭建了四旋翼无人机视觉控制系统实验平台，该平台采用嵌入式控制系统架构，使用机载嵌入式计算机运行论文中提出的算法，保证了控制系统的实时性。

如图1所示，本发明基于视觉的四旋翼无人机全自主飞行控制方法，包括以下步骤：

1)提出一种改进的视觉SLAM算法，并用于四旋翼无人机三维位置和姿态信息的获取：

常用的视觉SLAM主要依靠PTAM(并行跟踪与建图，Parallel Tracking and Mapping)来实现，PTAM是由Klein和Murray于2007年提出来的。其基本原理是通过跟踪地图中关键帧及关键帧中的特征点进行位置和姿态估计，并且建立的地图能够随定位过程而逐步扩充。

为提高无人机控制效果，本发明对于传统视觉SLAM算法做了以下的改进：第一，传统的视觉SLAM算法只能处理灰度图像，而普通微型机载摄像头无法直接提供灰度图像，本发明通过改进传统的视觉SLAM程序，增加从彩色图像到灰度图像的处理功能，适用于普通微型摄像头，增加了算法的普适性；第二，由于普通地面纹理较差，不能提供很好的特征点，且控制目标是实现无人机在较大区域内的全自主飞行，本发明通过增加特征点提取数量，同时优化关键帧存储，使得能够获取更多的环境信息，提高了控制算法的鲁棒性。

通过机载嵌入式计算机运行改进的视觉SLAM算法，手动平行移动机载摄像头，根据同一个特征点的移动描绘出弧线，再根据所有特征点的弧线确定一个特定的平面。通过这种特定的初始化方法，摆脱对于高度(深度)传感器的依赖，实现只依赖单目摄像头的三维位置、姿态估计。

2)采用扩展卡尔曼滤波器融合视觉位置信息和惯性导航单元提供的三维加速度，从而得到精确的位置信息，并提高数据输出频率；

由于四旋翼无人机具有非线性、强耦合、欠驱动和静不稳定等特点，故要求控制算法具有较高的实时性。而视觉SLAM的计算量较大，受机载计算机硬件条件的限制，本发明中使用的视觉SLAM算法运行频率约为20Hz，较难满足四旋翼无人机对于实时性的要求。因此，采用扩展卡尔曼滤波器融合视觉SLAM和惯性导航单元提供的数据，提高系统运算频率至100Hz，同时得到较为平滑的位置信息。

根据扩展卡尔曼滤波器模型，系统在k时刻的非线性过程方程和测量方程分别为：

x_k＝f(x_k-1,u_k-1)+ω_k-1

(1)

z_k＝h(x_k)+ν_k

其中，x_k＝[p_x p_y p_z v_x v_y v_z]^T是系统k时刻的状态向量，[p_x p_y p_z]^T为惯性坐标系下四旋翼无人机的三维位置向量，[v_x v_y v_z]^T为惯性坐标系下四旋翼无人机的三维速度向量，u_k＝[a_x a_y a_z]^T是系统k时刻的输入向量，其中a_x、a_y、a_z为惯性导航单元测得的加速度信息，z_k＝[p_x p_y p_z]^T是系统k时刻的观测向量；f(x_k-1,u_k-1)表示关于x_k-1 和u_k-1的函数；ω_k-1和υ_k为k-1时刻过程激励噪声和k时刻观测噪声；假设过程噪声和观测噪声是相互独立的，且均为零均值白噪声向量，即ω_k～N(0,Q),υ_k～N(0,R)。

在滤波值附近，对(1)式做泰勒级数展开，忽略二阶以上的高阶项，原系统就近似成为一个线性系统，如(2)式所示：

x_k＝F_kx_k-1+B_ku_k+ω_k

(2)

z_k＝H_kx_k+υ_k

其中，

$F_k=\frac{\∂f(x,u)}{\∂x}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{cccccc}1& 0& 0& {\mathrm{\δ}}_t& 0& 0\\ 0& 1& 0& 0& {\mathrm{\δ}}_t& 0\\ 0& 0& 1& 0& 0& {\mathrm{\δ}}_{\text{t}}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]---\left(3\right)$

$B_k=\frac{\∂f(x,u)}{\∂u}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{ccc}\frac{1}{2}{\mathrm{\δ}}_t^2& 0& 0\\ 0& \frac{1}{2}{\mathrm{\δ}}_t^2& 0\\ 0& 0& \frac{1}{2}{\mathrm{\δ}}_t^2\\ {\mathrm{\δ}}_t& 0& 0\\ 0& {\mathrm{\δ}}_t& 0\\ 0& 0& {\mathrm{\δ}}_t\end{array}\right]---\left(4\right)$

$H_k=\frac{\∂h\left(x\right)}{\∂x}{|}_{{\hat{x}}_{k|k-1}}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]---\left(5\right)$

在式(3)(4)中δ_t由系统的采样时间确定。在本发明中，由于惯性导航单元数据输出频率为100Hz，故选取δ_t＝0.01s。

扩展卡尔曼滤波的循环工作过程为：时间更新方程将当前状态作为先验估计及时地向前投射到测量更新方程，测量更新方程校验先验估计以获得状态的后验估计。其时间更新方程为：

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}=F_x{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}\\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_{k-1}\end{array}\right.,---\left(6\right)$

状态跟踪方程为：

$\left\{\begin{array}{c}{K}_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R)}^{-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H{\hat{x}}_{k|k-1})\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}\right..---\left(7\right)$

3)采用上述算法得到的水平位置和速度信息及气压计获得的高度信息作为反馈量，设计PID控制器，实现四旋翼无人机的自主飞行控制。

采用提出的改进视觉SLAM和扩展卡尔曼滤波算法而获取四旋翼无人机实时三维位置和速度信息，再利用PID控制器进行水平横向和水平纵向方向控制，再将该控制量发送给飞行控制器，实现四旋翼无人机的水平位置控制。高度方向则采用气压计获得无人机的高度信息，采用PID控制器产生控制量，实现四旋翼无人机高度控制。其PID控制器设计如下：

$u_x=k_{\mathrm{Px}}{e}_x+k_{\mathrm{Ix}}\underset{0}{\overset{t}{\∫}}{e}_x\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dx}}({\stackrel{\·}{x}}_d-v_x)$

$u_y=k_{\mathrm{Py}}{e}_y+k_{\mathrm{Py}}\underset{0}{\overset{t}{\∫}}{e}_y\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dy}}({\stackrel{\·}{y}}_d-v_y)---\left(8\right)$

$u_z=k_{\mathrm{Pz}}{e}_z+k_{\mathrm{Iz}}\underset{0}{\overset{t}{\∫}}{e}_z\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dz}}{\stackrel{\·}{e}}_z$

e_x＝x_d-x,e_y＝y_d-y,e_z＝z_d-z (9)

其中，k_P、k_I、k_D分别为比例、微分、积分系数，x_d、y_d、z_d为四旋翼无人机参考轨迹，x、y为视觉SLAM算法得到的四旋翼无人机的水平横向、水平纵向位置，z为气压计测得的无人机的高度，v_x、v_y为视觉SLAM算法得到的四旋翼无人机的水平速度，下标x、y、z分别代表水平横向、水平纵向及高度方向。PID表示比例-积分-微分控制器，为避免传统PID控制中因微分运算引起的高频干扰，本发明在水平横向和水平纵向方向使用速度的比例控制代替位置的微分运算。

本发明设计了基于视觉SLAM的无人机的自主定位与控制方法，并搭建了基于嵌入式架构的四旋翼无人机飞行实验平台，进行了较为精确的室外悬停实验。首先对传统的视觉SLAM算法进行了改进，增加算法的鲁棒性与普适性。再采用扩展卡尔曼滤波融合视觉位置信息和惯性导航单元数据，提高输出信号的精度和频率。最后结合PID控制器设计，实现四旋翼无人机的自主控制。本发明采用的控制方法优势在于采用嵌入式机载计算机运行所有算法，减少了图像和控制信号传输过程中的干扰和延时。飞行实验结果表明，在GPS信号缺失的环境下，本发明中提出的控制系统取得了较好的四旋翼无人机自主定位与控制效果。

4)设计机载硬件控制系统，运行本发明提出的改进视觉SLAM算法和PID控制器，能够完全脱离地面站而实现全自主的飞行控制。

本发明自主搭建了基于嵌入式架构的四旋翼无人机自主飞行控制实验平台。其中四旋翼无人机搭载了嵌入式计算机、机载摄像头和飞行控制器(含惯性导航单元和气压计模块等)。其控制结构为：安装在四旋翼无人机底部的摄像头用于采集图像信息，集成惯性导航单元用于获取无人机的姿态角及加速度信息，这两部分信息分别通过通过USB接口和RS232串口传输到机载嵌入式计算机。该机载计算机搭载Linux嵌入式操作系统和ROS机器人操作系统， 运行本文提出的改进视觉SLAM算法、扩展卡尔曼数据融合算法和PID控制算法，产生相应的四旋翼无人机的控制信息，该控制量通过RS232串口发送给嵌入式飞行控制器，实现四旋翼无人机的全自主飞行控制。

下面给出具体的实例：

一、系统硬件连接及配置

本发明的基于视觉的四旋翼无人机自主飞行控制方法采用基于嵌入式架构的飞行控制结构，所搭建的实验平台包括四旋翼无人机本体、地面站、遥控器等。其中四旋翼无人机如图2所示，搭载了嵌入式计算机(该计算机内嵌Intel Core i3双核处理器，主频1.8GHz)、机载摄像头(分辨率为640*480像素)和飞行控制器(含惯性导航单元和气压计模块等)。地面站包括一台装有Linux操作系统的笔记本计算机，用于机载程序的启动及远程监控。该平台可通过遥控器进行手动起飞和降落，并在发生意外时紧急切换为手动模式，以确保实验安全。

二、飞行实验结果

本实施例对上述实验平台进行了多组飞行控制实验，飞行实验环境为室外楼群之间。控制目标是实现四旋翼无人机在指定位置的全自主悬停功能，实验中设定目标位置为x_d＝0.38米，y_d＝-0.22米，z_d＝4.74米，期望姿态角为φ_d＝0度，θ_d＝0度，度。

稳定悬停状态下无人机的飞行数据曲线如图所示。其中，图3显示了四旋翼无人机在X、Y、Z三个方向的位置变化曲线，图4显示了目标位置与实际位置之间的误差曲线，图中其水平位置误差大都位于±0.2米以内，高度方向由于气压计受到风的干扰，控制误差较大，但仍位于±0.5米以内。图5显示了四旋翼无人机水平方向线速度图，其线速度值大部分位于±0.2米/秒以内。图6显示了四旋翼无人机姿态角曲线，滚转角和俯仰角控制误差大都在±2度以内，最大偏差在±3度以内，偏航角控制误差大都在±1度以内，保证了四旋翼无人机良好的的姿态稳定性。图7显示了悬停时四旋翼无人机二维轨迹图，本发明提出的控制策略取得了良好的控制效果。

Claims (5)

1.一种基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，包括如下步骤：利用安装在四旋翼无人机底部的摄像头采集图像信息，利用集成惯性导航单元获取无人机的姿态角及加速度信息，这两部分信息作为输入，运行改进的视觉SLAM算法获取四旋翼无人机三维位置和姿态信息；采用扩展卡尔曼滤波器融合视觉位置信息和惯性导航单元提供的三维加速度，从而得到精确的位置信息，采用上述算法得到的水平位置和速度信息及气压计获得的高度信息作为反馈量，设计PID控制器，实现四旋翼无人机的全自主飞行控制。

2.根据权利要求书1所述基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，所述的将改进的视觉SLAM算法用于四旋翼无人机三维位置和姿态信息的获取是：采用普通微型机载摄像头获得环境信息的实时彩色图像，通过机载嵌入式计算机运行改进的视觉SLAM算法，手动平行移动机载摄像头，根据同一个特征点的移动描绘出弧线，再根据所有特征点的弧线确定一个特定的平面。

3.根据权利要求书1所述基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，所述的融合视觉位置信息和惯性导航单元提供的三维加速度是采用扩展卡尔曼滤波方法，系统在k时刻的非线性过程方程和测量方程分别为：

x_k＝f(x_k-1,u_k-1)+ω_k-1

z_k＝h(x_k)+ν_k

其中，x_k＝[p_x p_y p_z v_x v_y v_z]^T是系统k时刻的状态向量，[p_x p_y p_z]^T为惯性坐标系下四旋翼无人机的三维位置向量，[v_x v_y v_z]^T为惯性坐标系下四旋翼无人机的三维速度向量，u_k＝[a_x a_y a_z]^T是系统k时刻的输入向量，其中a_x、a_y、a_z为惯性导航单元测得的加速度信息，z_k＝[p_x p_y p_z]^T是系统k时刻的观测向量；f(x_k-1,u_k-1)表示关于x_k-1和u_k-1的函数；ω_k-1和υ_k为k-1时刻过程激励噪声和k时刻观测噪声；假设过程噪声和观测噪声是相互独立的，且均为零均值白噪声向量，即ω_k～N(0,Q),υ_k～N(0,R)；

在滤波值附近，对上式做泰勒级数展开，忽略二阶以上的高阶项，原系统就近似成为一个线性系统，如下式所示：

x_k＝F_kx_k-1+B_ku_k+ω_k

z_k＝H_kx_k+υ_k

其中，

$F_k=\frac{\∂f(x,u)}{\∂x}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{cccccc}1& 0& 0& {\mathrm{\δ}}_t& 0& 0\\ 0& 1& 0& 0& {\mathrm{\δ}}_t& 0\\ 0& 0& 1& 0& 0& {\mathrm{\δ}}_{\text{t}}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$B_k=\frac{\∂f(x,u)}{\∂u}{|}_{{\hat{x}}_{k-1|k-1},u_k}=\left[\begin{array}{ccc}\frac{1}{2}{\mathrm{\δ}}_t^2& 0& 0\\ 0& \frac{1}{2}{\mathrm{\δ}}_t^2& 0\\ 0& 0& \frac{1}{2}{\mathrm{\δ}}_t^2\\ {\mathrm{\δ}}_t& 0& 0\\ 0& {\mathrm{\δ}}_t& 0\\ 0& 0& {\mathrm{\δ}}_t\end{array}\right]$

$H_k=\frac{\∂h\left(x\right)}{\∂x}{|}_{{\hat{x}}_{k|k-1}}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

在上述式子中δ_t由系统的采样时间确定；

扩展卡尔曼滤波器的循环工作过程为：时间更新方程将当前状态作为先验估计及时地向前投射到测量更新方程，测量更新方程校验先验估计以获得状态的后验估计；其时间更新方程为：

$\left\{\begin{array}{c}{\hat{x}}_{k|k-1}=F_x{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}\\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_{k-1}\end{array}\right.,$

状态跟踪方程为：

$\left\{\begin{array}{c}{K}_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R)}^{-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H{\hat{x}}_{k|k-1})\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}\right..$

4.根据权利要求书1所述基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，所述的设计PID控制器，实现四旋翼无人机的自主飞行控制为：采用提出的改进视觉SLAM和扩展卡尔曼滤波算法而获取四旋翼无人机实时三维位置和速度信息，再利用PID控制器进行水平横向和水平纵向方向的控制，再将该控制量发送给飞行控制器，实现四旋翼无人机的水平位置控制；高度方向则采用气压计获得无人机的高度信息，采用PID控制器产生控制量，实现四旋翼无人机高度控制；其PID控制器设计如下：

$u_x=k_{\mathrm{Px}}{e}_x+k_{\mathrm{Ix}}\underset{0}{\overset{t}{\∫}}{e}_x\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dx}}({\stackrel{\·}{x}}_d-v_x)$

$u_y=k_{\mathrm{Py}}{e}_y+k_{\mathrm{Py}}\underset{0}{\overset{t}{\∫}}{e}_y\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dy}}({\stackrel{\·}{y}}_d-v_y)$

$u_z=k_{\mathrm{Pz}}{e}_z+k_{\mathrm{Iz}}\underset{0}{\overset{t}{\∫}}{e}_z\left(\mathrm{\δ}\right)\mathrm{d\δ}+k_{\mathrm{Dz}}{\stackrel{\·}{e}}_z$

e_x＝x_d-x,e_y＝y_d-y,e_z＝z_d-z

其中，k_P、k_I、k_D分别为比例、微分、积分系数，x_d、y_d、z_d为四旋翼无人机参考轨迹，x、y为视觉SLAM算法得到的四旋翼无人机的水平横向、水平纵向位置，z为气压计测得的无人机的高度，v_x、v_y为视觉SLAM算法得到的四旋翼无人机的水平速度，下标x、y、z分别代表水平横向、水平纵向及高度方向。

5.根据权利要求书1所述基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，为避免传统PID控制中因微分运算引起的高频干扰，在水平横向和水平纵向方向使用速度的比例控制代替位置的微分运算。