[发明专利]基于视觉和imu的无人机自主定位方法

权利要求书

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算法去除异常值，当内点的数量达到一定的阈值之后，将该候选帧视为正确的回环检测，接下来进行闭环修复。