CN109520497B - 基于视觉和imu的无人机自主定位方法 - Google Patents

Abstract

本发明涉及ROS系统、计算机视觉、图像处理等领域，为提出一种针对无人机的自主定位方法。为此，本发明采用的技术方案是，基于视觉和imu的无人机自主定位方法，利用单目摄像机采集无人机前方的图像；利用imu读取无人机加速度和角速度的信息，且单目相机和imu是硬件同步，进一步地具体步骤如下：首先通过无人机上搭载的摄像头来感知周围环境获取图像信息，然后采用光流的方法来对图像进行处理，实现运动追踪。本发明主要用于无人机自主控制场合。

Description

基于视觉和imu的无人机自主定位方法

技术领域

本发明涉及ROS系统、计算机视觉、图像处理等领域，涉及一种基于单目相机的无人机自主定位方法，解决在无信号环境中无人机的定位问题。

背景技术

无人机具有极高的灵活性和自主性，能够在没有人类干预或者较少干预的情况下执行任务，帮助人类完成具有危险性或者重复性的劳动。近年来，无人机技术的快速发展使其在军事和民用领域得到了广泛的应用，在情报探测、战场救援、抗震救灾、火警监控等方面发挥了重要作用。由于无人机应用场景的多样性与飞行任务的复杂性，以小型与微型无人机为典型代表，具备自主定位功能的智能无人机正逐渐成为当前无人机发展的重要趋势。

GPS(全球定位系统)与高精度imu(惯性测量单元)是大中型无人机实现自主定位的有效工具。相对于大中型无人机，小型与微型无人机的负载与飞行能力有限，飞行环境的复杂性无法保证GPS信号的可靠接收与解算，所搭载的低精度imu无法满足其自主定位的需求。

视觉-惯性里程计(Visual-Inertial Odomtry，VIO)是无人机与计算机视觉领域的一个重要研究方向，被认为是实现无人机在陌生环境下自主定位导航的关键技术。通过自身携带的传感器对周围环境的观测，无人机可以实时估计自身在空间中的位置。由于无人机的应用场景复杂多样，对在未知环境中自身位置的精确估计是影响其发展的一个很重要的因素，只有在此技术领域有所突破，无人机才能够有更长远的发展，而VIO是目前解决这一问题优选方案，也是目前的前沿热点问题，通过研究这一技术可以较好的解决无人机在多障碍等复杂环境中自主定位问题，具有极大地研究意义。

发明内容

为克服现有技术的不足，本发明旨在提出一种针对无人机的自主定位方法。为此，本发明采用的技术方案是，基于视觉和imu的无人机自主定位方法，利用单目摄像机采集无人机前方的图像；利用imu读取无人机加速度和角速度的信息，且单目相机和imu是硬件同步，进一步地具体步骤如下：首先通过无人机上搭载的摄像头来感知周围环境获取图像信息，然后采用光流的方法来对图像进行处理，实现运动追踪，在图像中提取角点用于初始化，初始化采用极线约束的方法，对图像中获取的角点进行匹配，基于此求解出基于图像的位置信息；对于imu传感器获取到的加速度和角速度信息，将加速度信息和角速度信息进行预积分处理，求解出基于imu的位置信息；接下来将视觉求解出的位置信息和imu求解出的位置信息基于手眼标定法进行配准，从而解决imu的漂移和单目相机中的尺度问题，然后基于滑动窗口中进行非线性优化，同时在系统中进行回环检测并进行回环校正从而减小累积误差，最终求得无人机的精确位姿。

在数据预处理部分，首先是对imu获取到的加速度信息和角速度信息的预积分处理。来自与IMU的原始陀螺仪和加速度计的测量值为：

其中，b_at为在体坐标系中的加速度偏置，b_ωt为角速度偏置，

为世界坐标系到体坐标系的转换矩阵，g^ω为重力加速度值，n_a为加速度噪声，n_ω为角速度噪声。

对imu的信息进行预积分处理，得到的结果为：

其中，P为位置信息，V为速度信息，q为旋转信息。b_k+1和b_k为两帧图像，ω为角速度信息。由上式可以看出，经过预积分处理后，可以得到位置信息、速度信息和旋转信息。

第二部分为初始化部分。在初始化部分，首先利用视觉的信息去进行一个初始化求解。利用极限约束方程，采用五点法对其进行求解，最终得到两帧之间的相对平移和旋转信息。然后对两帧中匹配的特征点进行三角化恢复深度。得到3D信息之后即可进行PnP方法对滑动窗口中的其他帧的位姿进行求解。在基于视觉信息求解出位姿信息之后，将其与imu预积分得到的信息进行配准，首先是对重力偏置的校准，在b_k和b_k+1两帧之间，通过图像信息可以得到旋转四元数

和

通过imu的信息进行预积分能够得到相对的旋转，根据陀螺仪偏差线性化IMU预积分项，并最小化以下成本函数：

为陀螺仪偏差的一阶近似，

为雅各比矩阵，以此来更新陀螺仪偏差。同理可对加速度的偏置进行校准，最终得出较为准确的偏置值。基于计算得出的偏置值来更新预积分的值，到此整个初始化部分结束。

第三部分为滑动窗口中进行非线性优化。在此部分，我们使用视觉-惯性BA优化(Bundle Adjustment，光束平差法)，我们最小化所有测量残差的先验和马氏范数之和，以获得最大后验估计：

其中Huber函数定义为：

和

分别是imu和图像的测量残差。B代表所有的imu测量，C代表在当前滑动窗口中所提取的所有特征点，{r_p,H_p}是边缘化的先验信息,χ为姿态信息。通过非线性优化，可以得到优化后的位姿信息。

最后一步为回环检测部分，利用词袋模型来进行回环检测，对图像中再一次进行提取角点，将角点数量增加至400个，利用二进制描述子来表示每一个角点，当检测到回环时，通过检索特征的对应关系建立本地滑动窗口和闭环候选帧之间的连接，直接通过描述子之间的匹配来进行回环检测可能会导致有大量的异常值存在，因此利用RANSAC算法去除异常值，当内点的数量达到一定的阈值之后，将该候选帧视为正确的回环检测，接下来进行闭环修复。

本发明的特点及有益效果是：

由于视觉设备具有成本较低，并且能够采集到丰富的环境信息，随着社会的发展以及无人机行业应用的拓展，基于视觉传感器的自主定位技术已经成为了计算机视觉以及无人机领域的研究热点。近年来随着无人机浪潮的兴起，无人机的相关研究得到了高度的关注，针对无人机的自主定位成为重要的应用研究方向。此项发明提出一种基于视觉和imu的自主定位方法，对于无人机自主定位的研究具有十分重要的意义。该项发明计算量较小，精度较高，鲁棒性好，稳定性强，具有很高的可靠性。总而言之随着社会需求的提高，无人机有越来越多的功能需求及应用场景，要求无人机需要拥有更强的感知、决策以及执行能力，无人机自主定位的研究成为解决该问题的关键。

本发明主要具有如下特点和优点：

(1)无人机自主定位：本发明提出了一种基于视觉和imu的无人机自主定位方法，实现无人机的自主定位，为后续无人机进行轨迹规划、任务分配、避障等任务奠定了基础。

(2)定位精度高：本发明中提出的自主定位方法对图像信息和imu信息采用紧耦合的方法来进行融合，能够更好的融合图像和imu信息，使imu信息能够在高速旋转的情况下补偿图像信息，且能够利用图像信息求解出的运动来对imu产生的漂移进行消除从而提高算法的定位精度，并且提高算法的稳定性。

(3)定位方法效率高：由于无人机飞行具有高速的特点，因此在本发明中提出了应用稀疏光流法来进行运动追踪的自主定位方法，本方法提取图像中的角点，节省了大量的计算时间并减小了计算量。稀疏光流法基于图像中提取的角点来进行运动追踪，基于角点的光度不变假设来进行求解。采用此方法减小了算法的复杂度并且节省了计算空间，且在一定程度上缩短了计算时间，提高了算法的效率，能够满足无人机高速飞行时的需求，提供精确的位置信息。

附图说明：

附图1基于视觉和imu的无人机自主定位系统。

附图2基于视觉和imu的无人机自主定位方法流程图。

附图3自主定位方法实现效果图。

附图4无人机自主定位效果图。

具体实施方式

针对现有技术中的问题，本发明旨在提供一种针对中小型无人机的自主定位方法。无人机搭载摄像头和imu感知设备，搭载NUC处理器，首先通过无人机上搭载的摄像头来感知周围环境获取图像信息，然后采用光流的方法来对图像进行处理，实现运动追踪。在图像中提取角点用于对系统的初始化。初始化采用极线约束的方法，对图像中获取的角点进行匹配，基于此求解出基于图像的位置信息。对于imu传感器获取到的加速度和角速度信息，将加速度信息和角速度信息进行预积分处理，求解出基于imu的位置信息。接下来将视觉求解出的位置信息和imu求解出的位置信息基于手眼标定法进行配准，从而解决imu的漂移和单目相机中的尺度问题。然后基于滑动窗口中进行非线性优化，最终求得无人机的精确位姿。

本发明功能与特点如下：

(1)本发明带有一个单目摄像机，用于采集无人机前方的图像。带有一个imu，用来读取加速度和角速度的信息，且单目相机和imu是硬件同步的；

(2)本发明通过计算机视觉、视觉-惯性里程计等技术对摄像头采集到的图像进行处理，通过极线约束来对单目获取的图像进行匹配，求解出无人机的位置信息。然后对imu的信息进行预积分处理，两个信息进行紧耦合，最终通过非线性优化求解出无人机的位置信息。

(3)前端采用稀疏光流法来求解运动，然后再对其进行优化。在图像处理中，提取复杂特征会耗费大量的时间并且有很大的计算量，因此在本方法中提取简单的角点，然后基于角点的光度不变假设的稀疏光流法来求解运动。因此本方法减小了计算复杂度，大大缩短了计算时间，使得定位方法的鲁棒性和稳定性更强。

(4)基于滑动窗口来对信息进行融合，窗口中保持固定数量帧数的图像，以此来降低处理器的计算量，并且与此同时能够保证算法的精确性。

本发明提出了一种基于视觉和imu的无人机自主定位方法，实验环境依托于ROS的分布式节点框架体系，硬件系统由无人机、摄像头以及Intel NUC处理器等构成。

技术方案如下：

无人机搭载摄像头和Intel NUC处理器，NUC是Next Unit of Computing(NUC)的缩写，是一款微型迷你电脑，性能超过了中端笔记本性能，是计算机视觉、无人机和图像处理等领域的优秀开发平台。利用该处理器运行定位方法，摄像头来采集环境图像。

自主定位方法由信息预处理、初始化、全局位姿图优化和回环检测四部分组成。首先是对传感器获取到的信息的预处理，主要是对imu的信息进行预积分处理。由于本定位方法是基于单目相机来做的，所以需要进行初始化。基于极线约束，对图像中提取的角点进行匹配，以此来进行初始化。然后是基于滑动窗口的非线性优化，通过imu预积分得出的信息和视觉得到的信息进行配准，基于光束平差法来进行非线性优化从而减小误差。接下来是回环检测部分，基于Bow2(Bag of Word，词袋模型)来进行回环检测，检测到的回环之后对其进行修正，最终得出无人机的精准位姿。

下面结合附图对本发明做进一步描述。

无人机自主定位系统的整体结构如图1所示。根据各硬件功能的不同，可以把无人机上除机身外的所有硬件划分为两个层次，即控制层和感知层。控制层包括Pixhawk控制器、电机、电调；感知层包括单目摄像头和Intel NUC处理器。

图2所示为自主定位方法流程图。整个系统由数据预处理、初始化、滑动窗口非线性优化、回环检测四部分组成。在数据预处理部分，首先是对imu获取到的加速度信息和角速度信息的预积分处理。来自与IMU的原始陀螺仪和加速度计的测量值为：

其中，b_at为在体坐标系中的加速度偏置，b_ωt为角速度偏置，

为世界坐标系到体坐标系的转换矩阵，g^ω为重力加速度值，n_a为加速度噪声，n_ω为角速度噪声。

对imu的信息进行预积分处理，得到的结果为：

其中，P为位置信息，V为速度信息，q为旋转信息。b_k+1和b_k为两帧图像，ω为角速度信息。由上式可以看出，经过预积分处理后，可以得到位置信息、速度信息和旋转信息。

第二部分为初始化部分。在初始化部分，首先利用视觉的信息去进行一个初始化求解。利用极限约束方程，采用五点法对其进行求解，最终得到两帧之间的相对平移和旋转信息。然后对两帧中匹配的特征点进行三角化恢复深度。得到3D信息之后即可进行PnP方法对滑动窗口中的其他帧的位姿进行求解。在基于视觉信息求解出位姿信息之后，将其与imu预积分得到的信息进行配准，首先是对重力偏置的校准，在b_k和b_k+1两帧之间，通过图像信息可以得到旋转四元数

和

通过imu的信息进行预积分能够得到相对的旋转，根据陀螺仪偏差线性化IMU预积分项，并最小化以下成本函数：

为陀螺仪偏差的一阶近似，

为雅各比矩阵，以此来更新陀螺仪偏差。同理可对加速度的偏置进行校准，最终得出较为准确的偏置值。基于计算得出的偏置值来更新预积分的值，到此整个初始化部分结束。

第三部分为滑动窗口中进行非线性优化。在此部分，我们使用视觉-惯性BA优化(Bundle Adjustment，光束平差法)，我们最小化所有测量残差的先验和马氏范数之和，以获得最大后验估计：

其中Huber函数定义为：

和

分别是imu和图像的测量残差。B代表所有的imu测量，C代表在当前滑动窗口中所提取的所有特征点，{r_p,H_p}是边缘化的先验信息,χ为姿态信息。通过非线性优化，可以得到优化后的位姿信息。

最后一步为回环检测部分。在本发明中，利用词袋模型来进行回环检测。对图像中再一次进行提取角点，将角点数量增加至400个，利用二进制描述子来表示每一个角点。当检测到回环时，通过检索特征的对应关系建立本地滑动窗口和闭环候选帧之间的连接。直接通过描述子之间的匹配来进行回环检测可能会导致有大量的异常值存在，因此利用RANSAC算法去除异常值，当内点的数量达到一定的阈值之后，将该候选帧视为正确的回环检测，接下来进行闭环修复。

通过以上所有步骤之后，即可得到无人机的精确位姿，然后无人机可以利用该位姿进行稳定的飞行。

图3为运行该自主定位方法后的结果图。通过摄像头获取环境信息，提取图像中的角点特征，获取imu信息，对两者进行融合。该方法的定位效果如图3所示，其中红色标志表示相机位姿，绿色实线表示无人机的运动轨迹。

图4所示为无人机的自主定位效果图。该方法能够估计相机的实时位姿，使其控制无人机自主飞行。由此可以得出该方法能够实现无人机的精准定位。

Claims (1)

1.一种基于视觉和imu的无人机自主定位方法，其特征是，利用单目摄像机采集无人机前方的图像；利用imu读取无人机加速度和角速度的信息，且单目相机和imu是硬件同步，进一步地具体步骤如下：首先通过无人机上搭载的摄像头来感知周围环境获取图像信息，然后采用光流的方法来对图像进行处理，实现运动追踪，在图像中提取角点用于初始化，初始化采用极线约束的方法，对图像中获取的角点进行匹配，基于此求解出基于图像的位置信息；对于imu传感器获取到的加速度和角速度信息，将加速度信息和角速度信息进行预积分处理，求解出基于imu的位置信息；接下来将视觉求解出的位置信息和imu求解出的位置信息基于手眼标定法进行配准，从而解决imu的漂移和单目相机中的尺度问题，然后基于滑动窗口中进行非线性优化，同时在系统中进行回环检测并进行回环校正从而减小累积误差，最终求得无人机的精确位姿；

在数据预处理部分，首先是对imu获取到的加速度信息和角速度信息的预积分处理，来自与IMU的原始陀螺仪和加速度计的测量值为：

其中，b_at为在体坐标系中的加速度偏置，b_ωt为角速度偏置，

为世界坐标系到体坐标系的转换矩阵，g^ω为重力加速度值，n_a为加速度噪声，n_ω为角速度噪声；

对imu的信息进行预积分处理，得到的结果为：

其中，P为位置信息，V为速度信息，q为旋转信息，b_k+1和b_k为两帧图像，ω为角速度信息，由上式看出，经过预积分处理后，得到位置信息、速度信息和旋转信息；

第二部分为初始化部分，在初始化部分，首先利用视觉的信息去进行一个初始化求解，利用极限约束方程，采用五点法对其进行求解，最终得到两帧之间的相对平移和旋转信息，然后对两帧中匹配的特征点进行三角化恢复深度，得到3D信息之后即可进行PnP方法对滑动窗口中的其他帧的位姿进行求解，在基于视觉信息求解出位姿信息之后，将其与imu预积分得到的信息进行配准，首先是对重力偏置的校准，在b_k和b_k+1两帧之间，通过图像信息得到旋转四元数

和

通过imu的信息进行预积分能够得到相对的旋转，根据陀螺仪偏差线性化IMU预积分项，并最小化以下成本函数：

为陀螺仪偏差的一阶近似，

为雅各比矩阵，以此来更新陀螺仪偏差，同理对加速度的偏置进行校准，最终得出较为准确的偏置值，基于计算得出的偏置值来更新预积分的值，到此整个初始化部分结束；

第三部分为滑动窗口中进行非线性优化，在此部分，使用视觉-惯性光束平差法BA优化，最小化所有测量残差的先验和马氏范数之和，以获得最大后验估计：

其中Huber函数定义为：

和

分别是imu和图像的测量残差，B代表所有的imu测量，C代表在当前滑动窗口中所提取的所有特征点，{r_p,H_p}是边缘化的先验信息,χ为姿态信息，通过非线性优化，得到优化后的位姿信息；

最后一步为回环检测部分，利用词袋模型来进行回环检测，对图像中再一次进行提取角点，将角点数量增加至400个，利用二进制描述子来表示每一个角点，当检测到回环时，通过检索特征的对应关系建立本地滑动窗口和闭环候选帧之间的连接，直接通过描述子之间的匹配来进行回环检测可能会导致有大量的异常值存在，因此利用RANSAC算法去除异常值，当内点的数量达到一定的阈值之后，将该候选帧视为正确的回环检测，接下来进行闭环修复。

Images