[发明专利]基于视觉SLAM的四旋翼无人机全自主飞行控制方法

摘要

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

主权项

一种基于视觉SLAM的四旋翼无人机全自主飞行控制方法，其特征在于，包括如下步骤：利用安装在四旋翼无人机底部的摄像头采集图像信息，利用集成惯性导航单元获取无人机的姿态角及加速度信息，这两部分信息作为输入，运行改进的视觉SLAM算法获取四旋翼无人机三维位置和姿态信息；采用扩展卡尔曼滤波器融合视觉位置信息和惯性导航单元提供的三维加速度，从而得到精确的位置信息，采用上述算法得到的水平位置和速度信息及气压计获得的高度信息作为反馈量，设计PID控制器，实现四旋翼无人机的全自主飞行控制，所述的融合视觉位置信息和惯性导航单元提供的三维加速度是采用扩展卡尔曼滤波方法，系统在k时刻的非线性过程方程和测量方程分别为：xk＝f(xk‑1,uk‑1)+ωk‑1zk＝h(xk)+vk其中，下标x、y、z分别代表水平横向、水平纵向及高度方向，xk＝[px py pz vx vy vz]T是系统k时刻的状态向量，[px py pz]T为惯性坐标系下四旋翼无人机的三维位置向量，[vx vy vz]T为惯性坐标系下四旋翼无人机的三维速度向量，uk＝[ax ay az]T是系统k时刻的输入向量，其中ax、ay、az为惯性导航单元测得的加速度信息，zk＝[px py pz]T是系统k时刻的观测向量；f(xk‑1,uk‑1)表示关于xk‑1和uk‑1的函数；ωk‑1和υk为k‑1时刻过程激励噪声和k时刻观测噪声；假设过程激励噪声和观测噪声是相互独立的，且均为零均值白噪声向量，即ωk～N(0,Q),υk～N(0,R)；在滤波值附近，对上式做泰勒级数展开，忽略二阶以上的高阶项，原系统就近似成为一个线性系统，如下式所示：xk＝Fkxk‑1+Bkuk+ωkzk＝Hkxk+υk其中，Fk=∂f(x,u)∂x|x^k-1|k-1,uk=100δt000100δt000100δt000100000010000001]]>Bk=∂f(x,u)∂u|x^k-1|k-1,uk=12δt200012δt200012δt2δt000δt000δt]]>Hk=∂h(x)∂x|x^k|k-1=100000010000001000]]>在上述式子中δt由系统的采样时间确定；扩展卡尔曼滤波器的循环工作过程为：时间更新方程将当前状态作为先验估计及时地向前投射到状态更新方程，状态更新方程校验先验估计以获得状态的后验估计；其时间更新方程为：x^k|k-1=Fkx^k-1|k-1+Bk-1uk-1Pk|k-1=FkPk-1|k-1FkT+Qk-1,]]>状态更新方程为：Kk=Pk|k-1HkT(HkPk|k-1HkT+R)-1x^k|k=x^k|k-1+Kk(zk-Hx^k|k-1)Pk|k=(I-KkHk)Pk|k-1.]]>