[发明专利]基于混合视觉里程计和多尺度地图的无人机SLAM方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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