CN110118556A - 一种基于协方差交叉融合slam的机器人定位方法及装置 - Google Patents
一种基于协方差交叉融合slam的机器人定位方法及装置 Download PDFInfo
- Publication number
- CN110118556A CN110118556A CN201910294760.0A CN201910294760A CN110118556A CN 110118556 A CN110118556 A CN 110118556A CN 201910294760 A CN201910294760 A CN 201910294760A CN 110118556 A CN110118556 A CN 110118556A
- Authority
- CN
- China
- Prior art keywords
- robot
- current time
- covariance
- value
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明公开了一种基于协方差交叉融合SLAM的机器人定位方法及装置,通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像,对图像进行处理,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型,并根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。本发明将协方差交叉融合算法应用于SLAM中,增强了在线性误差方面的鲁棒性,提高了滤波的精度,提升了机器人在不同实际应用环境中定位的准确性。
Description
技术领域
本发明属于机器人定位技术领域,尤其涉及一种基于协方差交叉融合SLAM的机器人定位方法及装置。
背景技术
机器人的定位是指机器人在未知环境的情况下,可以得到自身在环境中所处位置。目前,机器人的定位方法可以分为非自主定位与自主定位。非自主定位是在定位的过程中机器人需要借助机器人本身以外的装置如全球定位系统(GPS)、全局视觉系统等进行定位;而自主定位是机器人仅仅依靠机器人本身携带的传感器进行定位,一般采用同时定位和地图构建(simultaneous localization and mapping,SLAM)。在SLAM技术中,机器人从一个未知位置开始运动,利用自身携带的传感器,例如摄像头、激光雷达等来感知周围环境并提取有用的信息,创建增量式的环境地图,同时利用已经建立的地图来同步刷新自身的位置。
按照初始位姿是否未知,机器人的自主定位分为初始位姿已知的位姿跟踪(Posetracking)和初始位姿未知的全局定位(Global localization)。位姿跟踪是在移植机器人的初始位姿的条件下,在机器人的运动过程中通过将观测到的特征与地图中的特征进行匹配,求取他们之间的差别,进而更新机器人的位姿的定位方法。位姿跟踪通常采用扩展卡尔曼滤波器(Extended Kalman Filter,EKF)来实现,该方法采用高斯分布来近似地表示机器人位姿的后验概率分布。但是在真实应用环境中,SLAM算法的应用有一个很大的困难是需要保持机器人和所有路标估计之间有完整的关联结构,这需要花费大量的成本并且在线性误差方面缺乏鲁棒性。
发明内容
本发明的目的是提供一种基于协方差交叉融合SLAM的机器人定位方法及装置,将协方差交叉(Covariance Intersection,CI)应用于SLAM。CI算法是分布式信息融合中不需要计算局部估计误差之间的相关性、通过优化一定的目标函数得到的一种保守的分布式融合估计方法。其不需要寻求和计算位置的互协方差,可避免卡尔曼融合滤波的发散。将协方差交叉和SLAM算法结合,可在估计精确度和计算效率之间提供一种平衡,将给大多数实用性的系统提供最好可实现的结果。
为了实现上述目的,本发明技术方案如下:
一种基于协方差交叉融合SLAM的机器人定位方法,包括:
通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像;
对图像进行图像特征提取及匹配计算获得相机的位姿,并通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型;
根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值;
不断进行迭代,对机器人的位姿进行修正更新。
本发明的一种实现方式,所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,包括:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1];
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
本发明的另一种实现方式,所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,包括:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1],σ为预设的纠正参数;
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
进一步地,所述基于协方差交叉融合SLAM的机器人定位方法,还包括:
进行回环检测,在检测到曾经到达过的位置时,以之前到达该位置的图像帧为基础,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
本发明还提出了一种基于协方差交叉融合SLAM的机器人定位装置,包括:
图像采集模块,用于通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像;
地图构建模块,用于对图像进行图像特征提取及匹配计算获得相机的位姿,并通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型;
优化估计模块,用于根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,并不断进行迭代,对机器人的位姿进行修正更新。
进一步地,所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,执行如下操作:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1];
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
可选的,所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,执行如下操作:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1],σ为预设的纠正参数;
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
进一步地,所述基于协方差交叉融合SLAM的机器人定位装置,包括回环检测模块,所述回环检测模块用于进行回环检测,在检测到曾经到达过的位置时,以之前到达该位置的图像帧为基础,输入到优化估计模块采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
本发明提出的一种基于协方差交叉融合SLAM的机器人定位方法及装置,相比于传统的SLAM技术中用到的卡尔曼滤波方法,本发明提供的将协方差交叉融合算法(CI)应用于SLAM中,不需要保持机器人和路标估计之间有完整的关联结构,避免了寻求和计算位置的互协方差,而且避免了卡尔曼滤波的发散。在估计精确度和计算效率之间提供一种平衡,增强了在线性误差方面的鲁棒性,提高了滤波的精度,提升了机器人在不同实际应用环境中定位的准确性。该方法对环境适应性强,难度低,易于实施。
附图说明
图1为本发明基于协方差交叉融合SLAM的机器人定位方法流程图;
图2为本发明实施例通过不同滤波方法得到融合结果的协方差椭圆示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅用以解释本申请,并不用于限定本申请。
本申请提供的一种基于协方差交叉融合SLAM的机器人定位方法,如图1所示,包括:
步骤S1、通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像。
将机器人部署在应用环境中,本实施例机器人设置有双目相机,双目相机进行工作采集周边环境的图像,并对采集的图像进行校正处理,得到校正后的图像。通常双目相机采集到同一个物体的左右两幅图像,其中校正处理主要涉及到畸变校正和立体校正,校正处理的目的是将左右图像去除畸变,将两幅图像极线对齐。
关于双目相机采集图像并进行校正处理,在本领域已经是比较成熟的技术,这里不再赘述。
步骤S2、对图像进行图像特征提取及匹配计算获得相机的位姿,并通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型。
本实施例对校正后的图像进行图像特征提取,提取图像的特征点,并进行匹配。例如采用ORB特征点方法来提取特征点,ORB采用FAST(features from accelerated segmenttest)算法来检测特征点。这个定义基于特征点周围的图像灰度值,检测候选特征点周围一圈的像素值,如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大,则认为该候选点为一个特征点。得到特征点后需要以某种方式描述这些特征点的属性,这些属性的输出称之为该特征点的描述子(Feature DescritorS)。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个路标的观测模型为:其中是将机器人观测和路标观测结合在一起而形成的观测噪声,该观测噪声为:Xk是机器人k时刻的位姿,pk,i是第i个路标的在k时刻的位置。即在2D平面中路标的所处的位置,可以描述为一个坐标:pk,i=[xk,i,yk,i]T。
关于通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型,在SLAM中已经是比较成熟的技术,这里不再赘述。
步骤S3、根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
具体地,本实施例中优化处理方法的过程如下:
步骤S3.1、由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:
其中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是更新向量的协方差,更新向量为zk是k时刻的观测量,为预测值,ω是人为引入的一个参数,取值范围为ω∈[0,1]。
需要说明的是,状态空间模型的误差协方差、过程噪声,以及观测模型的观测噪声和雅各布矩阵等,都是基于状态空间模型和观测模型进行数学变换计算得到,属于数学计算领域的基本常识,这里不再赘述。而滤波增益Wk的计算公式,上面的具体实施例也是一种优选的方式,并不局限于上述公式,例如滤波增益Wk还可以通过如下公式计算:σ为预设的纠正参数,用于对滤波增益进行修正,以获得更加准确的增益。
步骤S3.4、根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中当ω∈[0,1]时,估计的结果可以保持一致性,而且在交叉协方差未知的情况下可以证明该方法是最优的。
本实施例协方差交叉CI算法是一种融合估计方法,尤其是在两种估计之间的关联未知的情况。假设有两个状态A和B,A和B估计的一致性是已知的,对它们估计的均值和协方差对分别为和A和B之间的相关性是未知的。通过CI融合估计的结果的均值与协方差对记作过程如下:
其中当ω∈[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时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1];
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
可选的,所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,执行如下操作:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1],σ为预设的纠正参数;
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
在一个具体的实施例中,所述基于协方差交叉融合SLAM的机器人定位装置,包括回环检测模块,所述回环检测模块用于进行回环检测,在检测到曾经到达过的位置时,以之前到达该位置的图像帧为基础,输入到优化估计模块采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
需要说明的是,关于基于协方差交叉融合SLAM的机器人定位装置的具体限定可以参见上文中对于基于协方差交叉融合SLAM的机器人定位方法的限定,在此不再赘述。上述基于协方差交叉融合SLAM的机器人定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (8)
1.一种基于协方差交叉融合SLAM的机器人定位方法,其特征在于,所述基于协方差交叉融合SLAM的机器人定位方法,包括:
通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像;
对图像进行图像特征提取及匹配计算获得相机的位姿,并通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型;
根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值;
不断进行迭代,对机器人的位姿进行修正更新。
2.根据权利要求1所述的基于协方差交叉融合SLAM的机器人定位方法,其特征在于,所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,包括:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1];
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
3.根据权利要求1所述的基于协方差交叉融合SLAM的机器人定位方法,其特征在于,所述根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,包括:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1],σ为预设的纠正参数;
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
4.根据权利要求1所述的基于协方差交叉融合SLAM的机器人定位方法,其特征在于,所述基于协方差交叉融合SLAM的机器人定位方法,还包括:
进行回环检测,在检测到曾经到达过的位置时,以之前到达该位置的图像帧为基础,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
5.一种基于协方差交叉融合SLAM的机器人定位装置,其特征在于,所述基于协方差交叉融合SLAM的机器人定位装置,包括:
图像采集模块,用于通过机器人的双目相机采集周边环境的图像并进行校正处理,得到校正后的图像;
地图构建模块,用于对图像进行图像特征提取及匹配计算获得相机的位姿,并通过双目立体计算得到图像匹配特征点的深度信息,进而构建相应的地图点,得到机器人的状态空间模型以及机器人对路标的观测模型;
优化估计模块,用于根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,并不断进行迭代,对机器人的位姿进行修正更新。
6.根据权利要求5所述的基于协方差交叉融合SLAM的机器人定位装置,其特征在于,所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,执行如下操作:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1];
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
7.根据权利要求5所述的基于协方差交叉融合SLAM的机器人定位装置,其特征在于,所述优化估计模块根据前一时刻机器人位姿的优化估计值预测当前时刻机器人位姿的预测值,根据机器人对路标的观测模型得到当前时刻的观测量,结合当前时刻的观测量和预测值,采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值,执行如下操作:
由k-1时刻机器人位姿的优化估计值预测k时刻机器人位姿的预测值其中:uk是k时刻的系统控制量,A和B是系统参数;
由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是k时刻的观测量,为预测值,ω是参数,取值范围为ω∈[0,1],σ为预设的纠正参数;
根据计算所得的滤波增益,结合当前时刻的观测量zk和预测值采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值
其中
8.根据权利要求5所述的基于协方差交叉融合SLAM的机器人定位装置,其特征在于,所述基于协方差交叉融合SLAM的机器人定位装置,包括回环检测模块,所述回环检测模块用于进行回环检测,在检测到曾经到达过的位置时,以之前到达该位置的图像帧为基础,输入到优化估计模块采用基于CI融合的滤波方法进行修正更新,得到当前时刻机器人位姿的优化估计值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910294760.0A CN110118556A (zh) | 2019-04-12 | 2019-04-12 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910294760.0A CN110118556A (zh) | 2019-04-12 | 2019-04-12 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110118556A true CN110118556A (zh) | 2019-08-13 |
Family
ID=67521008
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910294760.0A Pending CN110118556A (zh) | 2019-04-12 | 2019-04-12 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110118556A (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108469729A (zh) * | 2018-01-24 | 2018-08-31 | 浙江工业大学 | 一种基于rgb-d信息的人体目标识别与跟随方法 |
CN110517324A (zh) * | 2019-08-26 | 2019-11-29 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN111044054A (zh) * | 2020-01-06 | 2020-04-21 | 哈尔滨工业大学 | 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法 |
CN111077889A (zh) * | 2019-12-23 | 2020-04-28 | 西北工业大学 | 面向车间托盘搬运的多移动机器人编队协作定位方法 |
CN111121768A (zh) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 一种机器人位姿估计方法、装置、可读存储介质及机器人 |
CN112698302A (zh) * | 2020-12-16 | 2021-04-23 | 南京航空航天大学 | 一种颠簸路况下的传感器融合目标检测方法 |
CN112880674A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市镭神智能系统有限公司 | 一种行驶设备的定位方法、装置、设备及存储介质 |
CN113093760A (zh) * | 2021-04-08 | 2021-07-09 | 洛阳理工学院 | 一种应用于灾后救援环境下的移动消防机器人建图方法 |
CN113091743A (zh) * | 2021-03-30 | 2021-07-09 | 浙江欣奕华智能科技有限公司 | 机器人的室内定位方法及装置 |
CN113091767A (zh) * | 2020-01-09 | 2021-07-09 | 舜宇光学(浙江)研究院有限公司 | 滤波式后端回环校正方法及其系统和电子设备 |
CN113257044A (zh) * | 2021-07-09 | 2021-08-13 | 中航信移动科技有限公司 | Ads-b数据的滤波方法、装置、计算机设备及存储介质 |
CN114353786A (zh) * | 2021-11-30 | 2022-04-15 | 安徽海博智能科技有限责任公司 | 一种基于改进卡尔曼滤波器的无人矿卡融合定位方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104236548A (zh) * | 2014-09-12 | 2014-12-24 | 清华大学 | 一种微型无人机室内自主导航方法 |
CN104330090A (zh) * | 2014-10-23 | 2015-02-04 | 北京化工大学 | 机器人分布式表征智能语义地图创建方法 |
WO2018159690A1 (ja) * | 2017-02-28 | 2018-09-07 | 国立研究開発法人理化学研究所 | 点群データの抽出方法、及び点群データの抽出装置 |
CN108648215A (zh) * | 2018-06-22 | 2018-10-12 | 南京邮电大学 | 基于imu的slam运动模糊位姿跟踪算法 |
CN108871337A (zh) * | 2018-06-21 | 2018-11-23 | 浙江工业大学 | 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法 |
CN109579825A (zh) * | 2018-11-26 | 2019-04-05 | 江苏科技大学 | 基于双目视觉和卷积神经网络的机器人定位系统及方法 |
-
2019
- 2019-04-12 CN CN201910294760.0A patent/CN110118556A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104236548A (zh) * | 2014-09-12 | 2014-12-24 | 清华大学 | 一种微型无人机室内自主导航方法 |
CN104330090A (zh) * | 2014-10-23 | 2015-02-04 | 北京化工大学 | 机器人分布式表征智能语义地图创建方法 |
WO2018159690A1 (ja) * | 2017-02-28 | 2018-09-07 | 国立研究開発法人理化学研究所 | 点群データの抽出方法、及び点群データの抽出装置 |
CN108871337A (zh) * | 2018-06-21 | 2018-11-23 | 浙江工业大学 | 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法 |
CN108648215A (zh) * | 2018-06-22 | 2018-10-12 | 南京邮电大学 | 基于imu的slam运动模糊位姿跟踪算法 |
CN109579825A (zh) * | 2018-11-26 | 2019-04-05 | 江苏科技大学 | 基于双目视觉和卷积神经网络的机器人定位系统及方法 |
Non-Patent Citations (2)
Title |
---|
张国良等: "《组合导航原理与技术》", 31 May 2008, 西安交通大学出版社 * |
曹梦龙等: "自主机器人SLAM改进算法", 《青岛科技大学学报(自然科学版)》 * |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108469729B (zh) * | 2018-01-24 | 2020-11-27 | 浙江工业大学 | 一种基于rgb-d信息的人体目标识别与跟随方法 |
CN108469729A (zh) * | 2018-01-24 | 2018-08-31 | 浙江工业大学 | 一种基于rgb-d信息的人体目标识别与跟随方法 |
CN110517324A (zh) * | 2019-08-26 | 2019-11-29 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN110517324B (zh) * | 2019-08-26 | 2023-02-17 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN111077889A (zh) * | 2019-12-23 | 2020-04-28 | 西北工业大学 | 面向车间托盘搬运的多移动机器人编队协作定位方法 |
CN111121768A (zh) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 一种机器人位姿估计方法、装置、可读存储介质及机器人 |
CN111044054A (zh) * | 2020-01-06 | 2020-04-21 | 哈尔滨工业大学 | 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法 |
CN113091767A (zh) * | 2020-01-09 | 2021-07-09 | 舜宇光学(浙江)研究院有限公司 | 滤波式后端回环校正方法及其系统和电子设备 |
CN113091767B (zh) * | 2020-01-09 | 2022-08-30 | 舜宇光学(浙江)研究院有限公司 | 滤波式后端回环校正方法及其系统和电子设备 |
CN112698302A (zh) * | 2020-12-16 | 2021-04-23 | 南京航空航天大学 | 一种颠簸路况下的传感器融合目标检测方法 |
CN112698302B (zh) * | 2020-12-16 | 2023-11-07 | 南京航空航天大学 | 一种颠簸路况下的传感器融合目标检测方法 |
CN112880674A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市镭神智能系统有限公司 | 一种行驶设备的定位方法、装置、设备及存储介质 |
CN113091743A (zh) * | 2021-03-30 | 2021-07-09 | 浙江欣奕华智能科技有限公司 | 机器人的室内定位方法及装置 |
CN113091743B (zh) * | 2021-03-30 | 2022-12-23 | 浙江欣奕华智能科技有限公司 | 机器人的室内定位方法及装置 |
CN113093760A (zh) * | 2021-04-08 | 2021-07-09 | 洛阳理工学院 | 一种应用于灾后救援环境下的移动消防机器人建图方法 |
CN113257044B (zh) * | 2021-07-09 | 2022-02-11 | 中航信移动科技有限公司 | Ads-b数据的滤波方法、装置、计算机设备及存储介质 |
CN113257044A (zh) * | 2021-07-09 | 2021-08-13 | 中航信移动科技有限公司 | Ads-b数据的滤波方法、装置、计算机设备及存储介质 |
CN114353786A (zh) * | 2021-11-30 | 2022-04-15 | 安徽海博智能科技有限责任公司 | 一种基于改进卡尔曼滤波器的无人矿卡融合定位方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110118556A (zh) | 一种基于协方差交叉融合slam的机器人定位方法及装置 | |
CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
US11295456B2 (en) | Visual-inertial odometry with an event camera | |
CN110118554A (zh) | 基于视觉惯性的slam方法、装置、存储介质和设备 | |
US10247556B2 (en) | Method for processing feature measurements in vision-aided inertial navigation | |
WO2021139590A1 (zh) | 基于蓝牙与slam的室内定位导航装置及其方法 | |
JP5987823B2 (ja) | 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム | |
CN109544636A (zh) | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 | |
CN110146099B (zh) | 一种基于深度学习的同步定位与地图构建方法 | |
KR101286135B1 (ko) | 상향식 단일 카메라를 이용한 중대형 공간에서의 자율적 위상지도 생성 방법 | |
CN110246182A (zh) | 基于视觉的全局地图定位方法、装置、存储介质和设备 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN108615246A (zh) | 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法 | |
CN109813319A (zh) | 一种基于slam建图的开环优化方法及系统 | |
US20210183100A1 (en) | Data processing method and apparatus | |
CN110260866A (zh) | 一种基于视觉传感器的机器人定位与避障方法 | |
KR101985344B1 (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
CN113763548B (zh) | 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 | |
CN114013449B (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
CN110749308B (zh) | 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法 | |
CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及系统 | |
CN113709662A (zh) | 一种基于超宽带的自主式三维反演定位方法 | |
CN112747749A (zh) | 一种基于双目视觉和激光融合定位导航系统 | |
KR102406240B1 (ko) | 강인한 스테레오 비주얼 관성 항법 장치 및 방법 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190813 |
|
RJ01 | Rejection of invention patent application after publication |