一种基于协方差交叉融合SLAM的机器人定位方法及装置与流程
本发明属于机器人定位技术领域，尤其涉及一种基于协方差交叉融合slam的机器人定位方法及装置。
背景技术：
机器人的定位是指机器人在未知环境的情况下，可以得到自身在环境中所处位置。目前，机器人的定位方法可以分为非自主定位与自主定位。非自主定位是在定位的过程中机器人需要借助机器人本身以外的装置如全球定位系统(gps)、全局视觉系统等进行定位；而自主定位是机器人仅仅依靠机器人本身携带的传感器进行定位，一般采用同时定位和地图构建(simultaneouslocalizationandmapping，slam)。在slam技术中，机器人从一个未知位置开始运动，利用自身携带的传感器，例如摄像头、激光雷达等来感知周围环境并提取有用的信息，创建增量式的环境地图，同时利用已经建立的地图来同步刷新自身的位置。
按照初始位姿是否未知，机器人的自主定位分为初始位姿已知的位姿跟踪(posetracking)和初始位姿未知的全局定位(globallocalization)。位姿跟踪是在移植机器人的初始位姿的条件下，在机器人的运动过程中通过将观测到的特征与地图中的特征进行匹配，求取他们之间的差别，进而更新机器人的位姿的定位方法。位姿跟踪通常采用扩展卡尔曼滤波器(extendedkalmanfilter，ekf)来实现，该方法采用高斯分布来近似地表示机器人位姿的后验概率分布。但是在真实应用环境中，slam算法的应用有一个很大的困难是需要保持机器人和所有路标估计之间有完整的关联结构，这需要花费大量的成本并且在线性误差方面缺乏鲁棒性。
技术实现要素：
本发明的目的是提供一种基于协方差交叉融合slam的机器人定位方法及装置，将协方差交叉(covarianceintersection，ci)应用于slam。ci算法是分布式信息融合中不需要计算局部估计误差之间的相关性、通过优化一定的目标函数得到的一种保守的分布式融合估计方法。其不需要寻求和计算位置的互协方差，可避免卡尔曼融合滤波的发散。将协方差交叉和slam算法结合，可在估计精确度和计算效率之间提供一种平衡，将给大多数实用性的系统提供最好可实现的结果。
为了实现上述目的，本发明技术方案如下：
一种基于协方差交叉融合slam的机器人定位方法，包括：
通过机器人的双目相机采集周边环境的图像并进行校正处理，得到校正后的图像；
对图像进行图像特征提取及匹配计算获得相机的位姿，并通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型；
根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值；
不断进行迭代，对机器人的位姿进行修正更新。
本发明的一种实现方式，所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，包括：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
本发明的另一种实现方式，所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，包括：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
进一步地，所述基于协方差交叉融合slam的机器人定位方法，还包括：
进行回环检测，在检测到曾经到达过的位置时，以之前到达该位置的图像帧为基础，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
本发明还提出了一种基于协方差交叉融合slam的机器人定位装置，包括：
图像采集模块，用于通过机器人的双目相机采集周边环境的图像并进行校正处理，得到校正后的图像；
地图构建模块，用于对图像进行图像特征提取及匹配计算获得相机的位姿，并通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型；
优化估计模块，用于根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，并不断进行迭代，对机器人的位姿进行修正更新。
进一步地，所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，执行如下操作：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
可选的，所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，执行如下操作：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
进一步地，所述基于协方差交叉融合slam的机器人定位装置，包括回环检测模块，所述回环检测模块用于进行回环检测，在检测到曾经到达过的位置时，以之前到达该位置的图像帧为基础，输入到优化估计模块采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
本发明提出的一种基于协方差交叉融合slam的机器人定位方法及装置，相比于传统的slam技术中用到的卡尔曼滤波方法，本发明提供的将协方差交叉融合算法(ci)应用于slam中，不需要保持机器人和路标估计之间有完整的关联结构，避免了寻求和计算位置的互协方差，而且避免了卡尔曼滤波的发散。在估计精确度和计算效率之间提供一种平衡，增强了在线性误差方面的鲁棒性，提高了滤波的精度，提升了机器人在不同实际应用环境中定位的准确性。该方法对环境适应性强，难度低，易于实施。
附图说明
图1为本发明基于协方差交叉融合slam的机器人定位方法流程图；
图2为本发明实施例通过不同滤波方法得到融合结果的协方差椭圆示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅用以解释本申请，并不用于限定本申请。
本申请提供的一种基于协方差交叉融合slam的机器人定位方法，如图1所示，包括：
步骤s1、通过机器人的双目相机采集周边环境的图像并进行校正处理，得到校正后的图像。
将机器人部署在应用环境中，本实施例机器人设置有双目相机，双目相机进行工作采集周边环境的图像，并对采集的图像进行校正处理，得到校正后的图像。通常双目相机采集到同一个物体的左右两幅图像，其中校正处理主要涉及到畸变校正和立体校正，校正处理的目的是将左右图像去除畸变，将两幅图像极线对齐。
关于双目相机采集图像并进行校正处理，在本领域已经是比较成熟的技术，这里不再赘述。
步骤s2、对图像进行图像特征提取及匹配计算获得相机的位姿，并通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型。
本实施例对校正后的图像进行图像特征提取，提取图像的特征点，并进行匹配。例如采用orb特征点方法来提取特征点，orb采用fast(featuresfromacceleratedsegmenttest)算法来检测特征点。这个定义基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。得到特征点后需要以某种方式描述这些特征点的属性，这些属性的输出称之为该特征点的描述子(featuredescritors)。orb采用brief算法来计算一个特征点的描述子，brief算法的核心思想是在关键点p的周围以一定模式选取n个点对，把这n个点对的比较结果组合起来作为描述子。关于图像特征提取及匹配，现有技术中已经有比较成熟的技术方案，本发明不限于具体的图像特征提取及匹配方法。
在得到匹配特征点后，采用pnp方法计算求解相机运动的旋转矩阵r和平移矢量t，并使用ba优化对r，t值进行多次迭代使得误差值最小化，获得相机的位姿，这里也不再赘述。
随后通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型。在slam技术中，slam的实现基础是建立机器人的状态空间模型以及机器人对路标的观测模型，其中:
状态空间模型为机器人在平面的运动方程，由机器人的位置和姿态角描述，即机器人的位姿表示为：xk＝[xk,yk,θk]t，反映了机器人在k时刻的位姿。(xk,yk)为k时刻机器人在平面的位置，θk为姿态角。
机器人对第i个路标的观测模型为：
关于通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型，在slam中已经是比较成熟的技术，这里不再赘述。
步骤s3、根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
具体地，本实施例中优化处理方法的过程如下：
步骤s3.1、由k-1时刻机器人位姿的优化估计值
其中uk是k时刻的系统控制量，a和b是系统参数。
步骤s3.2、由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1。其中：
pk|k-1＝apk-1|k-1at+qk-1
其中a是系统参数。
步骤s3.3、计算滤波增益wk。
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
需要说明的是，状态空间模型的误差协方差、过程噪声，以及观测模型的观测噪声和雅各布矩阵等，都是基于状态空间模型和观测模型进行数学变换计算得到，属于数学计算领域的基本常识，这里不再赘述。而滤波增益wk的计算公式，上面的具体实施例也是一种优选的方式，并不局限于上述公式，例如滤波增益wk还可以通过如下公式计算：
步骤s3.4、根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
本实施例协方差交叉ci算法是一种融合估计方法，尤其是在两种估计之间的关联未知的情况。假设有两个状态a和b，a和b估计的一致性是已知的，对它们估计的均值和协方差对分别为
其中当ω∈[0,1]时，估计的结果可以保持一致性。
如图2表示状态a估计和状态b估计的协方差椭圆以及修正结果c的不同值，图2左边的图形显示的修正结果是在a和b估计之间的关联为已知的情况下，通过卡尔曼滤波得到的，虚线表示了在a和b之间不同关联程度时通过卡尔曼修正得到的不同结果的协方差椭圆。图2右边是通过ci算法修正的结果，可以看到结果的协方差椭圆限定在交点区域之外。
步骤s4、不断进行迭代，对机器人的位姿进行修正更新。
需要通过更新当前时刻的状态协方差矩阵后，用于下一时刻的状态估计以此完成迭代操作，如此反复迭代下去，以此得到最佳的优化结果，直到结束地图的构建。
同时，由于传感器存在误差，而且误差会一直传递下去，最后一帧的位姿在环境坐标系里的误差有可能非常大，则利用回环检测来优化位姿，通过创建词袋的方法，来判断图像间的相似性，判断机器人是否曾经到达过先前的位置。位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是累计误差。
位姿约束都是与上一帧建立的，第五帧的位姿误差中便已经积累了前面四个约束中的误差。但如果发现第五帧位姿不一定要由第四帧推出来，还可以由第二帧推算出来，显然这样计算误差会小很多，因为只存在两个约束的误差了。像这样与之前的某一帧建立位姿约束关系就叫做回环。回环检测通过减少约束数，起到了减小累计误差的作用。回环检测在机器人构建地图技术领域，已经是比较成熟的技术方案，这里不再赘述。
在一个实施例中，本申请一种基于协方差交叉融合slam的机器人定位方法，还包括：
进行回环检测，在检测到曾经到达过的位置时，以之前到达该位置的图像帧为基础，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
在slam中，由于传感器(双目摄像机)存在误差，计算过程也会有误差，前面帧的误差会一直传递到后面去，导致最后一帧的位姿在环境坐标系里的误差有可能非常大，可以利用回环检测来优化位姿。回环检测实现方法通过创建词袋，袋子里装着每一帧图像中的特征元素，利用词袋比较每两帧图像的相似度，当相似度大于某一个阈值的时候，就认为这两幅图像是在同一点观测到的，相机回到了曾经到达过的位置。回环检测在slam中也是常用的技术，关于回环检测这里不再赘述。
例如机器人获取第3帧图像时经过a点，在获取第356帧图像时，经过回环检测，发现又经过了a点，则以第3帧图像的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，本申请还提供了一种基于协方差交叉融合slam的机器人定位装置，包括：
图像采集模块，用于通过机器人的双目相机采集周边环境的图像并进行校正处理，得到校正后的图像；
地图构建模块，用于对图像进行图像特征提取及匹配计算获得相机的位姿，并通过双目立体计算得到图像匹配特征点的深度信息，进而构建相应的地图点，得到机器人的状态空间模型以及机器人对路标的观测模型；
优化估计模块，用于根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，并不断进行迭代，对机器人的位姿进行修正更新。
其中，所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，执行如下操作：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
可选的，所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值，根据机器人对路标的观测模型得到当前时刻的观测量，结合当前时刻的观测量和预测值，采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值，执行如下操作：
由k-1时刻机器人位姿的优化估计值
由k-1时刻状态空间模型的误差协方差pk-1|k-1和过程噪声qk-1预测新的误差协方差pk|k-1，其中：pk|k-1＝apk-1|k-1at+qk-1，a是系统参数；
计算滤波增益wk：
其中hk是观测模型zk的雅各布矩阵，rk是观测噪声的协方差，sk是更新向量的协方差，更新向量为
根据计算所得的滤波增益，结合当前时刻的观测量zk和预测值
其中
在一个具体的实施例中，所述基于协方差交叉融合slam的机器人定位装置，包括回环检测模块，所述回环检测模块用于进行回环检测，在检测到曾经到达过的位置时，以之前到达该位置的图像帧为基础，输入到优化估计模块采用基于ci融合的滤波方法进行修正更新，得到当前时刻机器人位姿的优化估计值。
需要说明的是，关于基于协方差交叉融合slam的机器人定位装置的具体限定可以参见上文中对于基于协方差交叉融合slam的机器人定位方法的限定，在此不再赘述。上述基于协方差交叉融合slam的机器人定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。
- 还没有人留言评论。精彩留言会获得点赞！