CN109029417A - 基于混合视觉里程计和多尺度地图的无人机slam方法 - Google Patents

Abstract

本发明公开了一种基于混合视觉里程计和多尺度地图的无人机SLAM方法，属于无人机自主导航技术领域。本方法无人机平台上搭载有俯视单目相机和前视双目相机以及机载计算机。单目相机用于基于直接法的视觉里程计，双目相机用于基于特征点法的视觉里程计。混合视觉里程计将这两种视觉里程计的输出进行信息融合，构造用于定位的局部地图，得到无人机实时位姿。再将位姿反馈给飞行控制***，控制无人机的位置。机载计算机将实时位姿以及采集的图像传输给地面站，地面站依据构建的全局地图实时规划飞行路径，将航点信息发送给无人机，实现无人机自主飞行。本发明实现了无人机在非GPS环境下的实时位姿估计及环境感知，极大地提高无人机的智能化水平。

Description

基于混合视觉里程计和多尺度地图的无人机SLAM方法

技术领域

本发明涉及一种基于混合视觉里程计和多尺度地图的无人机SLAM方法，属于无人机自主导航技术领域。

背景技术

随着无人机(Unmanned Aerial Vehicle,UAV)技术的发展，各种类型的无人机被广泛应用，无人机也开始在工业领域发挥着越来越重要的作用，无人机也不再是简单的航模飞行器。如何感知环境以及自身状态信息成为了无人机的关键技术之一，也就是机器人领域研究的同步定位与构图(Simultaneous Localization and Mapping,SLAM)问题。

相对于地面移动机器人，无人机的载荷有限，对实时性要求高。传统的激光雷达传感器由于重量比较重，容易受到光照影响等因素不适合于无人机平台，因而视觉传感器更适合于无人机的定位和环境感知。同时，视觉传感器可以用于估计无人机的自身位置，从而无人机可以在没有GPS信号或者GPS信号受到干扰时飞行。视觉传感器中主要包含单目、双目和RGB-D相机，单目相机拥有尺度不确定性，RGB-D相机有效测量范围有限并且不适合于室外环境，双目相机虽然可以确定尺度但是计算耗时，所以结合单目相机和双目相机可以更好更快地用于状态估计。视觉SLAM的主要包括视觉里程计和建图，视觉里程计就是通过视觉传感器采集到的图像感知自身位姿变化，建图则是用于感知环境。当前视觉里程计分为两种方法：直接法和特征点法，其中直接法的优点是速度快，但是容易跟踪丢失，特征点法的鲁棒性更好，但是实时性不够好。建图的作用是建立环境模型，从而实现更高级的任务，例如自主导航、避障飞行等。

考虑到无人机对稳定性、实时性、准确性的要求比较高，传统的特征点法和直接法都不能很好地适应无人机定位要求，需要设计一种稳定并且准确的视觉里程计方法。同时，建图是对计算能力要求比较高，不适合直接在无人机上处理，需要依靠拥有强大计算能力的地面站。通过建立环境地图，无人机可以自主规划航线，实现完全自主飞行。

发明内容

为了满足无人机状态估计对稳定性、实时性、准确性的要求，本发明提出了一种基于混合视觉里程计和多尺度地图的无人机SLAM方法，实现在没有GPS信号的情况下，仅仅依靠相机采集到的图像来估计无人机的位置，同时建立环境地图，规划航线，实现自主飞行。

该方法无人机平台上除了基本飞行控制***，还搭载有俯视单目相机和前视双目相机以及用于运算处理的机载计算机。单目相机采集到的图像作为基于直接法的视觉里程计的输入，双目采集到的图像作为基于特征点法的视觉里程计的输入，这两个视觉里程计同时还构造用于定位的局部地图。混合视觉里程计将这两种里程计的输出信息进行信息融合和位姿矫正，得到无人机实时精确的位姿信息，然后反馈给飞行控制***，用于控制无人机的位置。机载计算机将实时位姿以及相机的图像发送给地面站，地面站负责建立环境全局地图，同时依据环境信息实时规划飞行路径，将航点信息发送给无人机，可以实现无人机的自主飞行。

本发明为解决其技术问题采用如下技术方案：

一种基于混合视觉里程计和多尺度地图的无人机SLAM方法，包括以下步骤：

步骤1：无人机平台搭载有俯视单目相机和前视双目相机，单目相机每秒采集30帧图像，双目相机每秒采集10帧图像；机载计算机同时开启两个线程，主线程以30Hz的频率运行基于直接法的视觉里程计，另一个线程以10Hz的频率运行基于特征点法的视觉里程计；假如k时刻获取同步的单目相机图像和双目相机的左右相机对应的图像 将作为基于直接法的视觉里程计的输入，将作为基于特征点法的视觉里程计的输入；特征点法视觉里程计输出当前双目图像中的左边相机图像相对于地图中的参考帧的左边相机图像的位姿变换矩阵直接法视觉里程计输出当前帧图像相对于前一帧图像的位姿变换矩阵同时还构建用于定位的局部点云地图；

步骤2：混合视觉里程计将步骤1中这两种里程计的输出信息和进行信息融合和位姿矫正，构造用于定位的局部地图，得到无人机实时精确的位姿信息

步骤3：机载计算机的混合视觉里程计通过处理前视双目相机图像和俯视单目相机图像，得到无人机实时精确的位姿信息然后将对应的位置信息通过串口发送给飞行控制***中的飞行控制单元，作为位置控制环的测量值；

步骤4：机载计算机将得到的实时位姿以及采集的图像通过WIFI传输给地面站，地面站通过恢复三维环境特征，构建全局地图，然后依据构建的全局地图实时规划飞行轨迹，得到中间航点，再将航点信息发送给无人机。

步骤1中基于特征点法的双目视觉里程计的具体过程如下：

(1)首先进行运动估计，依据无人机的运动速度不变模型，估计左边相机当前帧图像相对于左边相机第一帧图像的位姿变换矩阵，假如上一帧图像和上上帧图像相对于第一帧图像的位姿变换矩阵分别为和那么前一个时间间隔内的速度为：

其中log(·)代表李群SE(3)矩阵到李代数se(3)的对数映射，依据速度不变的假设：当前帧图像相对于第一帧图像的估计位姿变换矩阵为：

其中：为李代数元素，其中ρ为三维平移分量，φ为三维旋转分量，指数映射其中φ₁，φ₂，φ₃为φ的三个分量；

(2)依据当前帧的预测位姿将局部地图中的所有关键帧对应的特征点投影至当前帧，选择投影的特征点数目最多的关键帧作为参考帧；当有多个参考帧存在时，选择距离当前帧时间戳最近的关键帧作为参考帧；

(3)在当前帧的左边图像上进行特征提取，得到特征点然后将当前帧特征点和参考帧的左边相机图像中的特征点进行特征匹配，得到匹配的特征点对后，首先获取参考帧特征点的深度信息，然后依照3D-2D运动求解方法求解当前帧相对于参考帧的位姿变换矩阵

(4)在得到了当前帧相对于参考帧的位姿变换矩阵后，求得对应的6维速度矢量其中：v为线速度，w为角速度，假如速度矢量的模大于5m/s，那么当前帧作为关键帧加入到局部地图；

(5)假如当前帧被加入局部地图，在当前帧的右边图像上进行特征提取，得到特征点然后将特征点和特征点进行特征匹配，计算出匹配特征点对的视差，依据视差计算出该特征点对应的深度信息，获取点云图，同时深度信息用于后面的3D-2D运动求解。

步骤1中基于直接法的单目视觉里程计的具体过程如下：

(1)将前一帧图像的特征点投影至当前帧图像，然后通过最小化像素深度差来求得位姿变换矩阵也就是稀疏图像配准

其中π(·)指针孔相机模型，将空间中投影至二维像素平面，指的是特征点的深度值，代表着k时刻的图像，为k-1时刻的图像；

(2)通过三角测量的方法计算出当前帧的特征点的深度信息获取点云图，同时深度信息用于后面的运动估计。

步骤2的具体过程如下：

步骤2.1：在得到了双目视觉里程计当前帧相对于参考帧的位姿变换矩阵和单目视觉里程计当前帧相对于前一帧的位姿变换矩阵后，分别叠加参考帧和前一帧图像相对于第一帧图像的位姿变换矩阵和得到当前帧相对于第一帧图像的位姿变换矩阵和

步骤2.2：假如此时单目视觉里程计的位姿变换矩阵的尺度还未确定，则需要确定利用双目视觉里程计的位姿变换矩阵矩阵来确定单目视觉里程计中的尺度因子scaler；假若双目相机的左边相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为单目相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为那么理论上满足：

其中：

取中的平移分量和T_compose中的平移分量t_compose，通过最小化残差来计算尺度因子scaler，如下所示：

在获取了尺度信息scaler后，依据变换为拥有确定尺度信息的单目视觉里程计的位姿变换矩阵，同时单目视觉里程计的局部地图也乘以对应的尺度scaler，得到真实的地图；

步骤2.3：在得到了基于直接法的视觉里程计和基于特征点法的视觉里程计的输出后，混合视觉里程计利用来矫正添加一个偏移位姿T_offset来矫正矫正过程如下：

其中T_offset的计算公式为：

步骤2.4：获取无人机的实时精确的位姿信息并且假设第一帧图像对应的机体坐标系为世界坐标系，那么的计算方法为：

其中：为双目相机当前帧相对于世界坐标系的位姿变换矩阵，为双目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为双目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵；

另外当双目视觉里程计未更新时，混合视觉里程计仅仅通过单目视觉里程计计算无人机位姿并且这时候单目视觉里程计的尺度因子应该初始化完成，其中为单目相机当前帧相对于世界坐标系的位姿变换矩阵，为单目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为单目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵。

步骤4的具体过程如下：

步骤4.1：机载计算机将无人机的实时位姿以及采集的图像通过WIFI发送给地面站，然后地面站完成环境地图的构建；所述采集的图像为前视双目相机和俯视单目相机的图像，所述环境地图的构建为无人机前方和下方的环境地图；

步骤4.2：地面站在完成了全局环境地图的构建后，依据当前无人机的实时位姿信息和环境中障碍物信息，实时规划出一条无碰撞到达目的点的飞行路径，也就是生成一系列的中间航点；地面站再将这些中间航点位置信息通过WIFI发送给机载计算机，机载计算机再将这些中间航点以位置参考输入的方式通过串口发送给飞行控制***中的飞行控制单元，控制无人机的位置。

本发明的有益效果如下：

1、机载计算机运行的混合视觉里程计包括以30Hz频率运行的基于直接法的视觉里程计主线程和以10Hz运行的基于特征点法的视觉里程计线程，该方法结合了直接法的快速性以及特征点法的可靠性，同时还利用直接法的估计结果来矫正特征点法的估计结果，可以实现无人机的实时准确可靠定位。

2、基于特征点法的视觉里程计中，并不需要对每一帧图像的特征点计算深度信息。只有当前帧被加入局部地图时，才对右边相机图像进行特征提取和特征匹配，然后计算当前帧的特征点深度，这样可以有效减少运算时间，提高速度性能。同时是以局部地图中的关键帧作为参考帧，而不是常用的以上一帧图像作为参考帧，这样可以有效减小漂移误差。

3、使用了前视双目相机和俯视单目相机，可以同时构建无人机前方和下方的环境地图，视野更大，极大提高了无人机的安全性能。

4、机载计算机只负责建立用于定位的局部地图，地面站建立用于导航的全局环境地图，这种多尺度地图减轻了机载计算机的运算压力，实现了无人机的实时位姿估计以及环境感知，可以极大提高无人机的智能化水平。

5、通过视觉SLAM***和无人机飞行控制***相结合，正是由于视觉SLAM***提供给飞行控制***的位置控制环的参考输入值和测量值，无人机可以实时感知飞行环境并自主飞行。

附图说明

图1是本发明混合视觉里程计的***框图。

图2是本发明基于特征点法的双目视觉里程计的流程图。

图3是本发明基于直接法的单目视觉里程计的流程图。

图4是本发明基于多尺度地图的环境感知和自主导航***框图。

具体实施方式

下面结合附图对本发明作出详细说明。

图1为本发明混合视觉里程计的***框图，混合视觉里程计包含了基于直接法的单目视觉里程计和基于特征点法的双目视觉里程计两个线程。无人机平台搭载有着俯视单目相机和前视双目相机。单目相机每秒采集30帧图像，双目相机每秒采集10帧图像。机载计算机同时开启两个线程，主线程以30Hz的频率运行基于直接法的视觉里程计，另一个线程以10Hz的频率运行基于特征点法的视觉里程计。假如k时刻获取同步的单目相机图像和双目相机的左右相机对应的图像将作为基于直接法的视觉里程计的输入，将 作为基于特征点法的视觉里程计的输入。特征点法视觉里程计输出当前双目图像中的左边相机图像相对于地图中的参考帧的左边相机图像的位姿变换矩阵直接法视觉里程计输出当前帧图像相对于前一帧图像的位姿变换矩阵同时还构建用于定位的局部点云地图。

混合视觉里程计在得到了双目视觉里程计当前帧相对于参考帧的位姿变换矩阵和单目视觉里程计当前帧相对于前一帧的位姿变换矩阵后，进行位姿叠加，分别叠加参考帧和前一帧图像相对于第一帧图像的位姿变换矩阵和得到当前帧相对于第一帧图像的位姿变换矩阵和

假如此时单目视觉里程计的位姿变换矩阵的尺度还未确定，则需要确定利用双目视觉里程计的位姿变换矩阵矩阵来确定单目视觉里程计中的尺度因子scaler。假若双目相机的左边相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为单目相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为那么理论上满足：

其中：

由于***测量误差存在，所以上式中等式不成立，取中的平移分量和T_compose中的平移分量t_compose，通过最小化残差来计算尺度因子scaler，如下所示：

在获取了尺度信息scaler后，依据变换为拥有确定尺度信息的单目视觉里程计的位姿变换矩阵，同时单目视觉里程计的局部地图也乘以对应的尺度scaler，得到真实的地图。

考虑到经验上基于直接法的视觉里程计的精度高于基于特征点的视觉里程计的精度，混合视觉里程计利用来矫正添加一个偏移位姿T_offset来矫正矫正过程如下：

其中T_offset的计算公式为：

最后就是获取无人机的实时精确的位姿信息并且假设第一帧图像对应的机体坐标系为世界坐标系，那么的计算方法为：

其中：为双目相机当前帧相对于世界坐标系的位姿变换矩阵，为双目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为双目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵。

另外，考虑到基于直接法的视觉里程计的运行速率更快，所以当双目视觉里程计未更新时，混合视觉里程计仅仅通过单目视觉里程计计算无人机位姿并且这时候单目视觉里程计的尺度因子应该初始化完成，其中：为单目相机当前帧相对于世界坐标系的位姿变换矩阵，为单目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为单目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵。

图2是本发明基于特征点法的双目视觉里程计的流程图，首先进行运动估计，依据无人机的运动速度不变模型，估计当前帧图像相对于第一帧图像的位姿变换矩阵(对于双目相机，第一帧图像是指左边相机的图像)。假如上一帧图像和上上帧图像相对于第一帧图像的位姿变换矩阵分别为和那么前一个时间间隔内的速度为：

其中log(·)代表李群SE(3)矩阵到李代数se(3)的对数映射。依据速度不变的假设：当前帧图像相对于第一帧图像的估计位姿变换矩阵为：

其中：为李代数元素，其中ρ为三维平移分量，φ为三维旋转分量，指数映射其中φ₁，φ₂，φ₃为φ的三个分量，然后依据当前帧的预测位姿将局部地图中的所有关键帧对应的特征点投影至当前帧，选择投影的特征点数目最多的关键帧作为参考帧。当有多个参考帧存在时，选择距离当前帧时间戳最近的关键帧作为参考帧。

接下来在当前帧的左边图像上进行特征提取，得到特征点然后将当前帧特征点和参考帧的左边相机图像中的特征点进行特征匹配，得到匹配的特征点对后，首先获取参考帧特征点的深度信息，然后依照3D-2D运动求解方法求解当前帧相对于参考帧的位姿变换矩阵

在得到了当前帧相对于参考帧的位姿变换矩阵后，求得对应的6维速度矢量其中：v为线速度，w为角速度，假如速度矢量的模大于5m/s，那么当前帧作为关键帧加入到局部地图。

假如当前帧被加入局部地图，在当前帧的右边图像上进行特征提取，得到特征点然后将特征点和特征点进行特征匹配，计算出匹配特征点对的视差，依据视差计算出该特征点对应的深度信息，获取点云图，同时深度信息用于后面的3D-2D运动求解。

图3是本发明基于直接法的单目视觉里程计的流程图，首先将前一帧图像的特征点投影至当前帧图像，然后通过最小化像素深度差来求得位姿变换矩阵也就是稀疏图像配准。

其中π(·)指针孔相机模型，将空间中投影至二维像素平面，指的是特征点的深度值，代表着k时刻的图像，为k-1时刻的图像。

然后通过三角测量的方法计算出当前帧的特征点的深度信息获取点云图，同时深度信息用于后面的运动估计。

图4是本发明基于多尺度地图的环境感知和自主导航***框图。其中机载计算机和地面站构成了视觉SLAM***，飞行控制单元指的是飞行控制***，通过将视觉SLAM***和飞行控制***相结合，实现了无人机的实时位姿估计以及环境感知，可以极大提高无人机的智能化水平。

机载计算机的混合视觉里程计通过处理前视双目相机图像和俯视单目相机图像，得到无人机实时精确的位姿信息然后将对应的位置信息通过串口发送给飞行控制***中的飞行控制单元，作为位置控制环的测量值，用于控制无人机的位置。

机载计算机的主要负责实时定位和构建用于定位的局部地图，还没有完成环境感知功能，也就是没有建图。

为了保证定位的实时性，建图无法通过机载计算机完成，机载计算机将无人机的实时位姿以及相机的图像通过WIFI发送给地面站，然后地面站完成环境地图的构建。在这里同时发送前视双目相机和俯视单目相机的图像给地面站，可以构建无人机前方和下方的环境地图，这样视野更大，提高了无人机的安全性。

地面站在完成了全局环境地图的构建后，依据当前无人机的实时位姿信息和环境中障碍物信息，可以实时规划出一条无碰撞到达目的点的飞行路径，也就是生成一系列的中间航点。在计算出这些中间航点后，地面站将这些中间航点位置信息通过WIFI发送给机载计算机，机载计算机再将这些中间航点以位置参考输入的方式通过串口发送给飞行控制***中的飞行控制单元，通过改变位置控制环的参考输入，从而控制无人机的位置，从而最终实现无人机的自主飞行。

Claims (5)

1.一种基于混合视觉里程计和多尺度地图的无人机SLAM方法，其特征在于，包括以下步骤：

步骤1：无人机平台搭载有俯视单目相机和前视双目相机，单目相机每秒采集30帧图像，双目相机每秒采集10帧图像；机载计算机同时开启两个线程，主线程以30Hz的频率运行基于直接法的视觉里程计，另一个线程以10Hz的频率运行基于特征点法的视觉里程计；假如k时刻获取同步的单目相机图像和双目相机的左右相机对应的图像 将作为基于直接法的视觉里程计的输入，将作为基于特征点法的视觉里程计的输入；特征点法视觉里程计输出当前双目图像中的左边相机图像相对于地图中的参考帧的左边相机图像的位姿变换矩阵直接法视觉里程计输出当前帧图像相对于前一帧图像的位姿变换矩阵同时还构建用于定位的局部点云地图；

步骤2：混合视觉里程计将步骤1中这两种里程计的输出信息和进行信息融合和位姿矫正，构造用于定位的局部地图，得到无人机实时精确的位姿信息

步骤3：机载计算机的混合视觉里程计通过处理前视双目相机图像和俯视单目相机图像，得到无人机实时精确的位姿信息然后将对应的位置信息通过串口发送给飞行控制***中的飞行控制单元，作为位置控制环的测量值；

步骤4：机载计算机将得到的实时位姿以及采集的图像通过WIFI传输给地面站，地面站通过恢复三维环境特征，构建全局地图，然后依据构建的全局地图实时规划飞行轨迹，得到中间航点，再将航点信息发送给无人机。

2.根据权利要求1所述的基于混合视觉里程计和多尺度地图的无人机SLAM方法，其特征在于，步骤1中基于特征点法的双目视觉里程计的具体过程如下：

(1)首先进行运动估计，依据无人机的运动速度不变模型，估计左边相机当前帧图像相对于左边相机第一帧图像的位姿变换矩阵，假如上一帧图像和上上帧图像相对于第一帧图像的位姿变换矩阵分别为和那么前一个时间间隔内的速度为：

其中log(·)代表李群SE(3)矩阵到李代数se(3)的对数映射，依据速度不变的假设：当前帧图像相对于第一帧图像的估计位姿变换矩阵为：

其中：为李代数元素，其中ρ为三维平移分量，φ为三维旋转分量，指数映射其中φ₁，φ₂，φ₃为φ的三个分量；

(2)依据当前帧的预测位姿将局部地图中的所有关键帧对应的特征点投影至当前帧，选择投影的特征点数目最多的关键帧作为参考帧；当有多个参考帧存在时，选择距离当前帧时间戳最近的关键帧作为参考帧；

(3)在当前帧的左边图像上进行特征提取，得到特征点然后将当前帧特征点和参考帧的左边相机图像中的特征点进行特征匹配，得到匹配的特征点对后，首先获取参考帧特征点的深度信息，然后依照3D-2D运动求解方法求解当前帧相对于参考帧的位姿变换矩阵

(4)在得到了当前帧相对于参考帧的位姿变换矩阵后，求得对应的6维速度矢量其中：v为线速度，w为角速度，假如速度矢量的模大于5m/s，那么当前帧作为关键帧加入到局部地图；

(5)假如当前帧被加入局部地图，在当前帧的右边图像上进行特征提取，得到特征点然后将特征点和特征点进行特征匹配，计算出匹配特征点对的视差，依据视差计算出该特征点对应的深度信息，获取点云图，同时深度信息用于后面的3D-2D运动求解。

3.根据权利要求1所述的基于混合视觉里程计和多尺度地图的无人机SLAM方法，其特征在于，步骤1中基于直接法的单目视觉里程计的具体过程如下：

(1)将前一帧图像的特征点投影至当前帧图像，然后通过最小化像素深度差来求得位姿变换矩阵也就是稀疏图像配准

其中π(·)指针孔相机模型，将空间中投影至二维像素平面，指的是特征点的深度值，代表着k时刻的图像，为k-1时刻的图像；

(2)通过三角测量的方法计算出当前帧的特征点的深度信息获取点云图，同时深度信息用于后面的运动估计。

4.根据权利要求1所述的基于混合视觉里程计和多尺度地图的无人机SLAM方法，其特征在于，步骤2的具体过程如下：

步骤2.1：在得到了双目视觉里程计当前帧相对于参考帧的位姿变换矩阵和单目视觉里程计当前帧相对于前一帧的位姿变换矩阵后，分别叠加参考帧和前一帧图像相对于第一帧图像的位姿变换矩阵和得到当前帧相对于第一帧图像的位姿变换矩阵和

步骤2.2：假如此时单目视觉里程计的位姿变换矩阵的尺度还未确定，则需要确定利用双目视觉里程计的位姿变换矩阵矩阵来确定单目视觉里程计中的尺度因子scaler；假若双目相机的左边相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为单目相机第一帧图像对应相机坐标系相对于机体坐标系的位姿变换矩阵为那么理论上满足：

其中：

取中的平移分量和T_compose中的平移分量t_compose，通过最小化残差来计算尺度因子scaler，如下所示：

在获取了尺度信息scaler后，依据变换为拥有确定尺度信息的单目视觉里程计的位姿变换矩阵，同时单目视觉里程计的局部地图也乘以对应的尺度scaler，得到真实的地图；

步骤2.3：在得到了基于直接法的视觉里程计和基于特征点法的视觉里程计的输出后，混合视觉里程计利用来矫正添加一个偏移位姿T_offset来矫正矫正过程如下：

其中T_offset的计算公式为：

步骤2.4：获取无人机的实时精确的位姿信息并且假设第一帧图像对应的机体坐标系为世界坐标系，那么的计算方法为：

其中：为双目相机当前帧相对于世界坐标系的位姿变换矩阵，为双目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为双目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵；

另外当双目视觉里程计未更新时，混合视觉里程计仅仅通过单目视觉里程计计算无人机位姿并且这时候单目视觉里程计的尺度因子应该初始化完成，其中为单目相机当前帧相对于世界坐标系的位姿变换矩阵，为单目视觉里程计当前帧相对于第一帧的位姿变换矩阵，为单目相机第一帧图像对应相机坐标系相对于世界坐标系的位姿变换矩阵。

5.根据权利要求1所述的基于混合视觉里程计和多尺度地图的无人机SLAM方法，其特征在于，步骤4的具体过程如下：

步骤4.1：机载计算机将无人机的实时位姿以及采集的图像通过WIFI发送给地面站，然后地面站完成环境地图的构建；所述采集的图像为前视双目相机和俯视单目相机的图像，所述环境地图的构建为无人机前方和下方的环境地图；

步骤4.2：地面站在完成了全局环境地图的构建后，依据当前无人机的实时位姿信息和环境中障碍物信息，实时规划出一条无碰撞到达目的点的飞行路径，也就是生成一系列的中间航点；地面站再将这些中间航点位置信息通过WIFI发送给机载计算机，机载计算机再将这些中间航点以位置参考输入的方式通过串口发送给飞行控制***中的飞行控制单元，控制无人机的位置。