一种地下无人机飞行方法及系统

技术领域

本发明涉及无人机飞行技术领域，尤其是涉及一种地下无人机飞行方法及系统。

背景技术

随着技术的进步，无人机的应用越来越广泛。旋翼无人机具有体积小、成本低、灵活度高、使用便捷等优势，可以用于地下、侦查、搜索、灾后救援、能源探测等众多领域，尤其适合在一些比较危险或者比较狭小的地下空间工作，具有广阔的应用前景，旋翼无人机在地下环境中的应用在近些年一直是研究的热点。

无人机的姿态控制是无人机正常飞行并完成飞行任务的重要保障，而无人机姿态的确定是实现姿态控制的基础。但是，目前市场上最流行的旋翼无人机主要依靠GPS和IMU以及其他辅助传感器进行姿态矫正，而在地下空间，由于各方面技术的限制尤其是地下GPS信号弱的特点，无人机的姿态感知出现了困难，用于地下环境中进行搜索及救援的小型无人机还未得到普及。因此，需要一种能够用于地下空间的无人机姿态感知及飞行方法。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种地下无人机飞行方法及系统，使用IMU测量数据计算得到第一位姿信息，使用超声波传感器和单目相机测量数据计算得到第二位姿信息，计算第一位姿信息和第二位姿信息与IMU测得的标准位姿信息之间的误差信息，将误差信息融合后得到姿态误差信息并进行姿态修正和飞行控制，能获取精确度更高的姿态误差信息，且计算速度快，应用成本低，实现了无人机在无GPS信号的地下空间的飞行控制。

本发明的目的可以通过以下技术方案来实现：

一种地下无人机飞行方法，所述无人机上搭载有单目相机、超声波传感器和IMU，包括：

基于IMU测量的标准位姿信息和IMU的测量数据计算第一误差信息；

基于IMU测量的标准位姿信息和单目相机与超声波传感器的测量数据计算第二误差信息；

使用滤波算法将第一误差信息和第二误差信息进行融合，得到姿态误差信息；根据姿态误差信息进行无人机的飞行控制。

进一步的，IMU包括加速度计和陀螺仪，IMU的测量数据包括IMU加速度和IMU角速度，计算第一误差信息具体为：

A1：建立IMU运动学模型，将IMU加速度和IMU角速度代入IMU运动学模型，得到第一位姿信息；

A2：基于第一位姿信息和标准位姿信息得到第一误差状态方程；

A3：求解第一误差状态方程，得到第一误差信息。

进一步的，计算第二误差信息具体为：

B1：获取单目相机的测量数据和超声波传感器的测量数据，根据单目相机和超声波传感器的测量数据，计算第二位姿信息；

B2：获取单目相机与IMU的位置关系，得到第二位姿信息与标准位姿信息之间的旋转平移矩阵，基于第二位姿信息、旋转平移矩阵和标准位姿信息得到第二误差状态方程；

B3：求解第二误差状态方程，得到第二误差信息。

更进一步的，步骤B1中，根据单目相机的测量数据和超声波传感器的测量数据，使用最小二乘法进行尺度求解，使用ORB-SLAM算法计算所述第二位姿信息。

进一步的，以第一误差信息为预测值，以第二误差信息为观测值，使用扩展卡尔曼滤波算法将第一误差信息与第二误差信息进行融合，得到姿态误差信息。

一种地下无人机飞行系统，包括：

第一误差计算单元，根据IMU测量的标准位姿信息和IMU的测量数据计算第一误差信息；

第二误差计算单元，根据IMU测量的标准位姿信息和单目相机与超声波传感器的测量数据计算第二误差信息；

误差融合单元，使用滤波算法将第一误差信息和第二误差信息进行融合，得到姿态误差信息；

飞行控制单元，基于姿态误差信息进行无人机的姿态修正和飞行控制。

进一步的，IMU包括加速度计和陀螺仪，IMU的测量数据包括IMU加速度和IMU角速度，所述第一误差计算单元基于IMU加速度、IMU角速度和预先建立的IMU运动学方程计算得到第一位姿信息，基于第一位姿信息和标准位姿信息建立第一误差状态方程并求解，得到第一误差信息。

进一步的，所述第二误差计算单元基于单目相机和超声波传感器的测量数据计算第二位姿信息，基于预先获取的单目相机与IMU的位置关系得到第二位姿信息与标准位姿信息之间的旋转平移矩阵，基于第二位姿信息、旋转平移矩阵和标准位姿信息建立第二误差状态方程并求解，得到第二误差信息。

更进一步的，第二误差计算单元中，根据单目相机的测量数据和超声波传感器的测量数据，使用最小二乘法进行尺度求解，使用ORB-SLAM算法计算所述第二位姿信息。

进一步的，所述误差融合单元以第一误差信息为预测值，以第二误差信息为观测值，使用扩展卡尔曼滤波算法将第一误差信息与第二误差信息进行融合，得到姿态误差信息。

与现有技术相比，本发明使用IMU测量数据计算得到第一位姿信息，使用超声波传感器和单目相机测量数据计算得到第二位姿信息，计算第一位姿信息和第二位姿信息与IMU测得的标准位姿信息之间的误差信息，将误差信息融合后得到姿态误差信息并进行姿态修正和飞行控制，能获取精确度更高的姿态误差信息，且计算速度快，应用成本低，实现了无人机在无GPS信号的地下空间的飞行控制。

附图说明

图1为地下无人机飞行方法的流程图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例1：

一种地下无人机飞行方法，无人机上搭载有单目相机、超声波传感器和IMU，本申请结合IMU、单目相机和超声波传感器精确获取无人机的位姿误差，从而更好地进行姿态感知与姿态矫正，从而实现无人机在无GPS信号的地下空间的飞行控制。

一种地下无人机飞行方法的流程图如图1所示，IMU能够直接测量得到无人机的标准位姿信息，但是随着时间的偏移，测量得到的值会有一个随时间累计的误差。

一方面，本申请基于IMU的测量数据和IMU的运动学模型，重新计算得到第一位姿信息，基于标准位姿信息与第一位姿信息得到第一误差信息。

另一方面，使用单目相机和超声波传感器的测量数据，得到第二位姿信息，基于标准位姿信息与第二位姿信息得到第二误差信息。

最后，将第一误差信息与第二误差信息进行融合，根据融合后得到的姿态误差信息进行飞行控制。

本申请考虑到IMU测量得到的标准位姿信息会存在误差，使用IMU测量数据计算得到第一位姿信息，使用超声波传感器和单目相机测量数据计算得到第二位姿信息，再使用滤波算法将第一位姿信息与标准位姿信息的误差、第二位姿信息与标准位姿信息的误差进行融合，从而得到了精度更高的姿态误差信息，且计算速度快，高输出频率的位姿估计结果可以充分满足小型旋翼无人机高机动特性对位姿信息实时性的需求，只使用IMU、单目相机和超声波传感器，应用成本低。

IMU包括加速度计和陀螺仪，IMU的测量数据包括IMU加速度和IMU角速度，考虑加速度计和陀螺仪的零偏与噪声，建立IMU运动学模型，将IMU加速度和IMU角速度代入IMU运动学模型得到第一位姿信息。之后，建立第一位姿信息与标准位姿信息之间的第一误差状态方程，求解第一误差状态方程得到第一误差信息。

在实际应用时，IMU运动学模型的建立、第一误差状态方程的建立以及求解第一误差状态方程都是本领域人员所掌握的知识，故不再赘述。

单目相机的测量数据结合超声波传感器的测量数据，使用最小二乘法进行尺度求解，使用ORB-SLAM算法计算第二位姿信息。ORB-SLAM算法是当前常用的基于视觉求解无人机位姿信息的算法，由于单目视觉无法获得绝对尺度信息，因此可以使用最小二乘法结合超声波传感器进行尺度求解，在其他实施方式中，也可以使用其他手段，基于单目相机和超声波传感器的测量数据进行第二位姿信息计算。

求解得到的第二位姿信息是相对于单目相机坐标系的，为了计算第二误差信息，要先进行坐标转换，由于单目相机与IMU的安装位置是固定的，故可以预先进行标定，根据二者的相对位置得到第二位姿信息与标准位姿信息之间的旋转平移矩阵，进行坐标变换后再建立第二位姿信息与标准位姿信息之间的第二误差状态方程，求解第二误差状态方程得到第二误差信息。

卡尔曼滤波算法是常用的滤波算法，在位姿解算、轨迹规划方面应用较多，扩展卡尔曼滤波是一种基于卡尔曼滤波的算法，在位姿解算上的精度更高。本申请以第一误差信息为预测值，以第二误差信息为观测值，使用扩展卡尔曼滤波算法将第一误差信息与第二误差信息进行融合，得到姿态误差信息，无人机的飞行控制器再根据姿态误差信息进行姿态修正以及飞行控制。

一种地下无人机飞行系统，包括：第一误差计算单元、第二误差计算单元、误差融合单元和飞行控制单元；第一误差计算单元根据IMU测量的标准位姿信息和IMU的测量数据计算第一误差信息；第二误差计算单元根据IMU测量的标准位姿信息和单目相机与超声波传感器的测量数据计算第二误差信息；误差融合单元使用滤波算法将第一误差信息和第二误差信息进行融合，得到姿态误差信息；飞行控制单元基于姿态误差信息进行无人机的姿态修正和飞行控制。

IMU包括加速度计和陀螺仪，IMU的测量数据包括IMU加速度和IMU角速度，第一误差计算单元基于IMU加速度、IMU角速度和预先建立的IMU运动学方程计算得到第一位姿信息，基于第一位姿信息和标准位姿信息建立第一误差状态方程并求解，得到第一误差信息。

第二误差计算单元基于单目相机和超声波传感器的测量数据计算第二位姿信息，基于预先获取的单目相机与IMU的位置关系得到第二位姿信息与标准位姿信息之间的旋转平移矩阵，基于第二位姿信息、旋转平移矩阵和标准位姿信息建立第二误差状态方程并求解，得到第二误差信息。

第二误差计算单元中，根据单目相机的测量数据和超声波传感器的测量数据，使用最小二乘法进行尺度求解，使用ORB-SLAM算法计算第二位姿信息。

误差融合单元以第一误差信息为预测值，以第二误差信息为观测值，使用扩展卡尔曼滤波算法将第一误差信息与第二误差信息进行融合，得到姿态误差信息。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。