[发明专利]用于SLAM过程的基于差分进化算法进行BA问题求解的方法

说明书

本发明公开了一种用于SLAM过程的基于差分进化算法进行BA问题求解的方法，属于移动机器人同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)领域，包括以下步骤：读取机器人的视觉图像，提取当前帧与上一帧图像的特征点，并获取所有匹配的特征点对；计算上一帧图像中与当前帧图像相匹配的所有特征点各自对应的空间坐标；根据空间坐标的重投影误差计算误差函数；利用差分进化算法求取误差函数的最优解所对应的相机的位姿即完成BA问题的求解。本发明的方法能够避免现存方法中需要进行求导运算而导致运算量过大的问题，能够减小BA过程的计算量，保证SLAM过程的实时性与快速性。

技术领域

本发明涉及移动机器人同步定位与地图构建(Simultaneous Localization andMapping，SLAM)领域，特别是一种用于SLAM过程的基于差分进化算法进行BA问题求解的方法。

背景技术

随着室内移动服务机器人的发展，与室内移动服务机器人相关的关键技术成为了研究热点，导航定位系统是室内移动服务机器人的核心，因此SLAM问题是机器人实现自主导航的关键也是现在和未来人们研究的重点。

目前普遍认为SLAM问题领域的最优方法是基于平滑的方法，基于平滑的方法是一种批量式的SLAM方法，其是将SLAM分为图构造和图优化两部分。在图优化框架的视觉SLAM算法中，光束平差法(Bundle Adjustment，BA)起到核心作用，主要采用了非线性最小二乘的方法来对机器人全局的位姿和地图进行最优估计，能够使机器人长期稳定的运行。

在SLAM的视觉里程计部分，涉及到PnP(Perspective-n-Point)问题，即通过已知的n个3D空间点及其在相机平面的投影位置求解相机位姿的问题。通常使用P3P或EPnP等方法估计出相机位姿后，需要通过光束平差法(BA)对估计值进行调整。

目前BA的常规解法分为以高斯牛顿法为代表的线性搜索方法，和以列文伯格-马夸尔特方法为代表的信赖区域方法。这两种方法存在计算量大、速度慢、精度不够高等问题。

差分进化算法(Differential Evolution Algorithm，DE)是在遗传算法等进化思想的基础上针对参数优化设计提出的，其本质是一种针对连续变量的多目标优化算法，用于求解多维空间中整体最优解。

发明内容

本发明的目的在于提供一种可以整合到任何使用BA进行优化的SLAM方案中，且能快速进行SLAM程序在视觉里程计部分中求解相机位姿以及后端优化的解算方法。

实现本发明目的的技术解决方案为：一种用于SLAM过程的基于差分进化算法进行BA问题求解的方法，包括以下步骤：

步骤1、读取机器人的视觉图像，提取当前帧与上一帧图像的特征点，并获取所有匹配的特征点对(a_i，b_i)；a_i为上一帧图像中的特征点，b_i为当前帧图像中的特征点；其中，0＜i≤m，m为特征点对的数目；

步骤2、计算上一帧图像中所有特征点a_i各自对应的空间坐标P_i；

步骤3、根据步骤2求得的各个特征点a_i的空间坐标P_i以及当前帧的对应特征点b_i计算误差函数；

步骤4、利用差分进化算法求取误差函数的最优解所对应的相机的位姿ξ即完成BA问题的求解。

本发明与现有技术相比，其显著优点为：1)利用差分进化算法(DE)求解通过光束平差法构造的非线性最小二乘问题，避免了使用高斯牛顿法和列文伯格-马夸尔特法等技术的求导问题，能够提高SLAM程序的执行效率，提高求解速度、且精度更高；2)本发明的方法可以无缝接入现有的SLAM方案，不用对SLAM方案的其他部分进行修改，方便应用。