CN116026335A - 一种适用于未知室内环境的移动机器人定位方法与*** - Google Patents
一种适用于未知室内环境的移动机器人定位方法与*** Download PDFInfo
- Publication number
- CN116026335A CN116026335A CN202211675418.3A CN202211675418A CN116026335A CN 116026335 A CN116026335 A CN 116026335A CN 202211675418 A CN202211675418 A CN 202211675418A CN 116026335 A CN116026335 A CN 116026335A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- state information
- acquiring
- point
- matching
- 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.)
- Granted
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及移动机器人定位技术领域,尤其涉及一种适用于未知室内环境的移动机器人定位方法与***,其方法包括步骤A:通过移动机器人获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;步骤B:通过移动机器人获取图像,匹配连续两帧图像信息,计算获取所述移动机器人的第二状态信息;步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,计算获取所述移动机器人的第三状态信息;步骤D:通过获取的第一状态信息、第二状态信息以及第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。本发明可实现未知室内环境下轮式移动机器人的高精准定位。
Description
技术领域
本发明涉及移动机器人定位技术领域,尤其涉及一种适用于未知室内环境的移动机器人定位方法与***。
背景技术
定位是室内移动机器人开展自主导航的基本要求,当前可靠的定位方法大多基于已知环境,需要预先在工作环境中设置二维码标签、电子标签、通讯基站等,或者提供已知地图进行实时地图匹配。寻求未知环境的精准定位有利于拓宽移动机器人作业的适用范围。
在缺乏先验信息和通讯信号的未知室内环境下作业,准确获取移动机器人运动位移显得尤为重要。轮式里程计是广泛应用的基础的相对定位方法,这种基于运动学模型推算位姿的定位方式不可避免会因地面打滑或机械组件负载变形等产生误差。而基于视觉信息的定位方法利用丰富的环境纹理信息进行特征匹配进而得到移动机器人位移,摆脱了对移动机器人运动学模型的依赖,但常常受到环境中光照条件的影响,观测过程中存在较大的随机误差;基于激光信息的定位方法不依赖于光照条件,且检测精度高,但也存在感知信息稀疏等问题。综上,鉴于单一的定位方式难以实现未知室内环境下移动机器人的精确定位,因此本专利提出一种适用于未知室内环境的移动机器人多源信息融合定位方法与***。
发明内容
本发明的目的在于提出一种适用于未知室内环境的移动机器人定位方法与***,采用多源信息融合定位方法,结合各传感器优势来实现未知室内环境的精准定位。
为达此目的,本发明采用以下技术方案:
一种适用于未知室内环境的移动机器人定位方法,包括以下步骤:
步骤A:通过移动机器人获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;
步骤B:通过移动机器人获取图像,读取连续两帧图像的图像信息进行匹配分析,基于观测模型计算获取所述移动机器人的第二状态信息;
步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于激光观测模型计算获取所述移动机器人的第三状态信息;
步骤D:通过基于运动学模型获取的第一状态信息和基于观测模型获取的第二状态信息以及第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
优选的,所述步骤A,具体包括以下步骤:
步骤A1:在移动机器人运动过程中,基于左驱动轮安装的光电编码盘输出的脉冲数分为NL,右驱动轮安装的光电编码盘输出的脉冲数分为NR,光电编码盘的转速为P线/rad,驱动轮的半径为r,车轮运动的距离为ΔS,分别获取左驱动轮的位移增量ΔSL和右驱动轮的位移增量ΔSR为:
步骤A2:基于移动机器人的左驱动轮和右驱动轮的移动距离分别为ΔSl和ΔSr,左驱动轮和右驱动轮之间的距离为d,获取移动机器人发生转动的角度为Δθ=(ΔSr-ΔSl)/d,以及移动机器人运动的距离为ΔS=(ΔSl+ΔSr)/2;
步骤A3:基于移动机器人的运动学模型,可通过移动机器人的单位时间转动的角度Δθ和运动距离ΔS以及当前k时刻的移动机器人位姿[x(k),y(k),θ(k)]T得到第k+1时刻的移动机器人状态X(k+1)=[x(k+1),y(k+1),θ(k+1)]T:
因此,移动机器人在在初始位姿上进行位移累加可求得移动机器人在T时刻的位姿。
优选的,所述步骤B,具体包括以下步骤:
步骤B1:通过判断函数,分别对两帧图像进行ORB特征点的提取,其中判断函数如下:
其中p为候选特征点,circle(p)为以候选特征点p为原点建立的圆,x为circle(p)内的点,G(x)为圆周上的点的灰度值,G(p)为候选特征点的灰度值,ξ为灰度值的阈值,E表示圆圈内像素的灰度值与候选点的灰度值的差异;若E大于或等于阈值ξ,则p为特征点;
步骤B2:获取每一个ORB特征点的BRIEF描述子,使用描述子来描述该特征点的特征属性,并对两帧图像的BRIEF描述子使用汉明距离进行粗匹配,获得粗匹配结果;
步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,利用选出来的特征点对采用EPnP算法来计算两帧图像的相对位姿;
步骤B4:根据两帧图像的相对位姿,获取ORB特征点的投影误差,并根据投影误差再次对ORB特征点对进行筛选;
步骤B5:重复执行步骤B4直到所有剩余的ORB特征点对的投影误差皆在允许范围之内,则得出最终的相对位姿,至此,一次图像匹配完成。
优选的,所述步骤B2:获取每一个ORB特征点的BRIEF描述子,具体包括以下步骤:
其中BRIEF描述子为一个二进制数,其第i位的值为Bi;
步骤B22:采用灰度质心法确定特征点的方向,包括:
步骤B221:首先定义中间变量m00,m10和m01为:
其中M为以特征点p为圆心且R为半径的圆形的邻域;
步骤B222:获取邻域M的灰度质心C:
步骤B223:获取射线pC与坐标轴y轴的夹角:
θ=arctan(m01/m10) (8)
射线pC的方向即为特征点p的方向。
优选的,所述步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,利用选出来的特征点对采用EPnP算法来计算两帧图像的相对位姿;所述EPnP算法具体包括以下步骤:
步骤B31:基于匹配的特征点集为{Pi,Qi}(i=1,2,3,...n),分别获取两组匹配的特征点的质心:
步骤B32:对所有的匹配点计算其去质心坐标:
步骤B33:根据去质心坐标,获取矩阵H:
步骤B34:对公式(11)作奇异值分解:
H=UΣVT (12)
其中U是酉矩阵,Σ是半正定对角矩阵,而VT同是酉矩阵,是V的共轭转置;
步骤B35:基于公式(9)和公式(12),获取两帧图像对应的摄像头坐标系的相对旋转矩阵以及平移向量为:
其中R表示为旋转矩阵,t表示为平移向量,两者为视觉观测模型的观测量。
优选的,所述步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于激光观测模型计算获取所述移动机器人的第三状态信息,具体包括以下步骤:
步骤C1:采用迭代最近点算法进行扫描匹配;首先,定义激光雷达在t时刻采集到一组数据点{qt},定义在t+1时刻采集到一组数据点{qt+1},其次,对点集{qt+1}的每个点进行坐标变换,将其重投影到坐标系XYt中,投影点集记为变换关系如下所示:
步骤C4:对点集{qt}中的每个点都执行步骤C1至步骤C3,找到在点集{qt+1}中的匹配点,计算每对匹配点之间的重投影误差,然后将每个重投影误差的平方累加起来,即可构造最小二乘的目标函数,如下式所示:
求解使得公式(18)中目标函数取得最小值的旋转矩阵R和平移向量t,两者为激光观测模型的观测量。
所述步骤D中,所述运动学模型可表示为:
xk+1=f(xk,uk+1)+wk+1 (19)
其中xk+1表示k+1时刻的状态变量,包含位置和速度,xk表示k时刻的状态变量,uk+1表示k+1时刻的控制量,f(·)表示k+1时刻的状态变量xk+1与k时刻的状态变量xk之间的函数关系,wk+1表示k+1时刻的运动噪声;
所述视觉观测模型和激光观测模型统一表示为:
zk+1=h(xk,xk+1)+vk+1 (20)
其中zk+1表示k+1时刻的***观测量,h(·)表示状态变量xk和xk+1与观测量zk+1之间的函数关系,vk+1表示k+1时刻的观测噪声。
优选的,所述步骤D:通过基于运动学模型获取的第一状态信息和基于观测模型获取的第二状态信息以及第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。其中扩展卡尔曼滤波算法中单次滤波循环的具体步骤如下:
步骤D2:利用一阶泰勒展开公式将运动模型展开成线性方程:
步骤D4:利用一阶泰勒展开公式将观测模型展开成线性方程:
步骤D6:用测量值更新状态变量,得到状态变量的后验估计值:
步骤D7:将协方差矩阵更新为状态变量后验估计值的协方差矩阵:
其中I为协方差矩阵维数相同的单位矩阵;
对于视觉观测模型和激光观测模型分别获取的第二状态信息和第三状态信息,分别参与循环与第一状态信息融合。如此反复步骤D1到D7,从而不断更新获得移动机器人的最新的状态。。
一种适用于未知室内环境的移动机器人定位***,采用上述所述的一种适用于未知室内环境的移动机器人定位方法,所述***包括移动机器人硬件和定位软件,所述移动机器人硬件包括电源组件、主控组件、运动组件、感知组件和显示组件,所述定位软件包括编码盘定位模块、视觉定位模块、激光定位模块和扩展卡曼滤波融合模块;
所述编码盘定位模块用于通过光电编码盘获取电机位移运动信息,并结合运动学模型,计算获取得到所述移动机器人的第一状态信息;
所述视觉定位模块用于通过深度摄像头获取图像,读取连续两帧图像的图像信息,基于观测模型计算获取所述移动机器人的第二状态信息;
所述激光定位模块用于通过激光雷达获取激光数据,匹配当前时刻和下一时刻的激光数据,基于观测模型计算获取所述移动机器人的第三状态信息;
所述扩展卡尔曼滤波融合模块用于获取所述第一状态信息、第二状态信息和第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
上述技术方案中的一个技术方案具有以下有益效果:本发明通过融合多种传感器信息实现未知室内环境下轮式移动机器人的高精准定位。
附图说明
图1是本发明方法的软件算法原理示意图;
图2是本发明方法的图像匹配的流程示意图;
图3是本发明***的硬件组成图;
具体实施方式
下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。
一种适用于未知室内环境的移动机器人定位方法,包括以下步骤:
步骤A:通过移动机器人获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;步骤B:通过移动机器人获取图像,读取连续两帧图像的图像信息,基于观测模型计算获取所述移动机器人的第二状态信息;其中图像信息包括旋转矩阵和平移向量;步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于观测模型计算获取移动机器人的第三状态信息;步骤D:通过基于运动学模型获取的第一状态信息和基于观测模型获取的第二状态信息以及第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
目前移动机器人大多采用单一的定位方式,包括轮式里程计计算方法、基于视觉信息的定位方法和基于激光传感器的定位方法,但各个定位方式都包含或多或少的缺点,其中轮式里程计计算方法是一种目前被广泛应用的相对定位方法,其通过安装在车轮上的光电编码器等来检测移动机器人位移,结合初始位姿进而估算当前移动机器人坐标,这种基于运动学模型推算位姿的定位方式不可避免会因地面打滑或机械组件负载变形等产生误差。而基于视觉信息的定位方法虽然摆脱了对移动机器人运动学模型的依赖,但常常受到环境中光照条件的影响,检测存在较大的随机误差;即使是激光传感器不依赖于光照条件,且检测精度较高,但也存在感知信息稀疏等问题。
因此本专利寻求多源信息融合,结合多传感器各自的优势来实现未知室内环境的精准定位。如图1所示,在本实施例中,本发明分别通过移动机器人配置的编码盘、视觉传感器和激光传感器实时检测状态信息,并基于运动学模型计算得到第一状态信息,基于观测模型计算得到第二状态信息和第三状态信息,最后基于卡尔曼滤波算法,融合上述多源信息,实现在未知室内环境下轮式移动机器人的高精准定位。
更进一步地说明,所述步骤A,具体包括以下步骤:
步骤A1:在移动机器人运动过程中,基于左驱动轮安装的光电编码盘输出的脉冲数分为NL,右驱动轮安装的光电编码盘输出的脉冲数分为NR,光电编码盘的转速为P线/rad,驱动轮的半径为r,车轮运动的距离为ΔS,分别获取左驱动轮的位移增量ΔSL和右驱动轮的位移增量ΔSR为:
步骤A2:基于移动机器人的左驱动轮和右驱动轮的移动距离分别为ΔSl和ΔSr,左驱动轮和右驱动轮之间的距离为d,获取移动机器人发生转动的角度为Δθ=(ΔSr-ΔSl)/d,以及移动机器人运动的距离为ΔS=(ΔSl+ΔSr)/2;
步骤A3:基于移动机器人的运动学模型,可通过移动机器人的单位时间转动的角度Δθ和运动距离ΔS以及当前k时刻的移动机器人位姿[x(k),y(k),θ(k)]T得到第k+1时刻的移动机器人状态X(k+1)=[x(k+1),y(k+1),θ(k+1)]T:
因此,移动机器人在在初始位姿上进行位移累加可求得移动机器人在T时刻的位姿。
如图2所示,本发明分别对两帧图片进行ORB特征点提取,然后计算每一个特征点的描述子,再对两帧图的ORB特征点描述子进行粗匹配,由于粗匹配会存在大量的错误匹配,因此需要对粗匹配的结果进行筛选,筛选后则利用选出来的特征点对计算两帧图像的相对位姿。然后利用计算得到的相对位姿对特征点对进行投影误差计算,并根据误差大小再次筛选特征点对,如此重复直到所有剩余的特征点对的投影误差皆在允许范围之内,则得出最终的相对位姿,至此,一次图像匹配完成。
更进一步地说明,ORB算法最早由Rublee等人提出,它改良自经典的FAST(Features from Accelerated Segment Test)角点提取算法,其特征点的全称为OrientedFAST,其采用的描述子为BRIEF(Binary Robust Independent ElementaryFeatures)描述子。所述步骤B,具体包括以下步骤:
步骤B1:通过判断函数,分别对两帧图像进行ORB(ORiented Brief)特征点的提取,包括:选定候选特征点,以候选特征点为圆心确定一个圆形,检测该圆内的像素灰度值,若在这一圆内有足够多像素的灰度值与候选特征点的灰度值的差异大于或等于阈值,则将该候选点确认为特征点,其中判断函数如下:
其中p为候选特征点,circle(p)为以候选特征点p为原点建立的圆,x为circle(p)内的点,G(x)为圆周上的点的灰度值,G(p)为候选特征点的灰度值,ξ为灰度值的阈值,E表示圆圈内像素的灰度值与候选点的灰度值的差异;若E大于或等于阈值ξ,则p为特征点;
步骤B2:获取每一个ORB特征点的BRIEF描述子,使用描述子来描述该特征点的特征属性,并对两帧图像的BRIEF描述子使用汉明距离进行粗匹配,获得粗匹配结果;汉明距离是用于比较两个二进制数据字符串的度量,其中当比较两个相等长度的二进制字符串时,汉明距离则比较两个比特不同的比特位置数。
步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,获取两帧图像的相对位姿;由于粗匹配会存在大量的错误匹配,因此需要对粗匹配的结果进行筛选。
步骤B4:根据两帧图像的相对位姿,获取ORB特征点的投影误差,并根据投影误差再次对ORB特征点对进行筛选;
步骤B5:重复执行步骤B4直到所有剩余的ORB特征点对的投影误差皆在允许范围之内,则得出最终的相对位姿,至此,一次图像匹配完成。
更进一步地说明,所述步骤B2:获取每一个ORB特征点的BRIEF描述子,具体包括以下步骤:
其中BRIEF描述子为一个二进制数,其第i位的值为Bi;
步骤B22:为了让特征点具有旋转不变性,采用灰度质心法确定特征点的方向,包括:
步骤B221:首先定义中间变量m00,m10和m01为:
其中M为以特征点p为圆心且R为半径的圆形的邻域;
步骤B222:获取邻域M的灰度质心C:
步骤B223:获取射线pC与坐标轴y轴的夹角:
θ=arctan(m01/m10) (8)
射线pC的方向即为特征点p的方向。
若图像发生了旋转,则射线pC的方向也会发生相应的旋转。因此,只要在获取BRIEF描述子时,以射线pC为坐标轴建立直角坐标系,则无论图像如何旋转,计算出来的BRIEF描述子都将会是一致的。
更进一步地说明,所述步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,利用选出来的特征点对采用EPnP(Efficient Perspective-n-Point)算法来计算两帧图像的相对位姿;所述EPnP算法具体包括以下步骤:
步骤B31:基于匹配的特征点集为{Pi,Qi}(i=1,2,3,...n),分别获取两组匹配的特征点的质心:
步骤B32:对所有的匹配点计算其去质心坐标:
步骤B33:根据去质心坐标,获取矩阵H:
步骤B34:基于公式(11),作奇异值分解:
H=UΣVT (12)
其中U是酉矩阵,Σ是半正定对角矩阵,而VT同是酉矩阵,是V的共轭转置;
步骤B35:基于公式(9)和公式(12),获取两帧图像对应的摄像头坐标系的相对旋转矩阵以及平移向量为:
其中R表示为旋转矩阵,t表示为平移向量,两者为视觉观测模型的观测量。
在获得了移动机器人连续两帧图像之间的特征点匹配结果后,则可以进一步求解两帧图像间得相对位姿。本申请选用速度较快的EPnP(Efficient Perspective-n-Point)算法进行移动机器人位姿的估计。
更进一步地说明,所述步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于激光观测模型计算获取所述移动机器人的第三状态信息,具体包括以下步骤:
步骤C1:采用迭代最近点算法(Iterative ClosestPoint,ICP)进行扫描匹配;首先,定义激光雷达在t时刻采集到一组数据点{qt},定义在t+1时刻采集到一组数据点{qt+1},其次,对点集{qt+1}的每个点进行坐标变换,将其重投影到坐标系XYt中,投影点集记为变换关系如下所示:
步骤C4:对点集{qt}中的每个点都执行步骤C1至步骤C3,找到在点集{qt+1}中的匹配点,计算每对匹配点之间的重投影误差,然后将每个重投影误差的平方累加起来,即可构造最小二乘的目标函数,如下式所示:
求解使得公式(18)中目标函数取得最小值的旋转矩阵R和平移向量t,两者为激光观测模型的观测量。
更进一步地说明,卡尔曼滤波算法(Kalman Filter,KF)是信息融合算法常用的一种基于最优估计的算法。该算法认为,对于一个控制***来说,它的控制方程与观测方程都受到高斯白噪音的影响,从而导致该***的实际状态与理想状态之间出现偏差,其滤波过程主要为根据这些噪音的协方差矩阵对输入信号进行融合估计,输出最优估计值。考虑到卡尔曼滤波算法只适用于线性***,而在本设计中移动机器人的运动学模型和观测模型是非线性的,因此需要采用扩展卡尔曼滤波算法(Extended Kalman Filter,EKF),它是卡尔曼滤波在非线性***中的一种扩展,将非线性函数进行泰勒展开,保留一阶项,而一阶项正是泰勒展开的线性部分,以此实现了非线性***的线性化。
更进一步地说明,在步骤D中,所述运动学模型可表示为:
其中xk+1表示k+1时刻的状态变量,包含位置和速度,xk表示k时刻的状态变量,uk+1表示k+1时刻的控制量,f(·)表示k+1时刻的状态变量xk+1与k时刻的状态变量xk之间的函数关系,wk+1表示k+1时刻的运动噪声;
所述视觉观测模型和激光观测模型统一表示为:
zk+1=h(xk,xk+1)+vk+1 (20)
其中zk+1表示k+1时刻的***观测量,h(·)表示状态变量xk和xk+1与观测量zk+1之间的函数关系,vk+1表示k+1时刻的观测噪声。
更进一步地说明,所述步骤D:通过基于运动学模型获取的第一状态信息和基于观测模型获取的第二状态信息以及第三状态信息,采用扩展卡尔曼滤波(Extended KalmanFilter,EKF)算法进行数据融合,得到移动机器人实时状态信息,其中扩展卡尔曼滤波算法中单次滤波循环的具体步骤如下:
步骤D2:利用一阶泰勒展开公式将运动模型展开成线性方程:
步骤D4:利用一阶泰勒展开公式将观测模型展开成线性方程:
步骤D6:用测量值更新状态变量,得到状态变量的后验估计值:
步骤D7:将协方差矩阵更新为状态变量后验估计值的协方差矩阵:
其中I为协方差矩阵维数相同的单位矩阵;
对于视觉观测模型和激光观测模型分别获取的第二状态信息和第三状态信息,分别参与循环与第一状态信息融合。如此反复步骤D1到D7,从而不断更新获得移动机器人的最新的状态。
一种适用于未知室内环境的移动机器人定位***,采用上述所述的一种适用于未知室内环境的移动机器人定位方法,所述***包括移动机器人硬件及定位软件,所述移动机器人硬件包括电源组件、主控组件、运动组件、感知组件、显示组件。
如图3所示,在本实施例中,电源组件为***其他硬件提供24v或12v的直流供电,感知组件获取环境信息,运动组件驱动移动机器人移动的同时监测运动状态,主控组件则接收感知组件和运动组件的信息进行分析处理同时发送控制命令给运动组件。
更进一步说明,所述主控组件由工控机组成,用于集中分析处理来自所述运动组件和所述感知组件的信息,并调整和发送控制指令,控制移动机器人运动。所述主控组件选配了华北工控品牌的工业控制计算机。其CPU为奔腾I77700处理器,操作***为微软的Windows 7专业版,内存为双通道8G的DDR3接口内存。该款工控机拥有出色的性能以及小巧的结构,整体防尘、防电波干扰且坚固耐用,能较好承担移动机器人的各种控制任务。
所述感知组件中,深度摄像头和激光雷达均通过USB接口与工控机相连接,所述深度摄像头选配了具备深度信息检测能力的Kinect摄像头,Kinect由一个RGB摄像头、一个深度传感器和一个麦克风组成,其中的深度传感器使用的是红外光栅测距技术,它通过一个红外线发射器发射出一道雷射光,并利用发射器镜头前的光栅使其均匀散射到前方的现实环境中,再由红外线摄像头捕捉环境中投影的光斑并以此计算深度信息。Kinect的可视范围为:水平视角57度、垂直视角43度、深度传感范围1.2-3.5米,Kinect的数据流为彩色图640x480,32bit,30fps,深度图320x240,16bit,30fps,所述激光雷达装配为德国SCIK公司LM11x-10100系列传感器。该型号传感器探测角度范围为-45~225°,可测量的最大距离20m,分辨率最小值可设置为0.25°。该款激光传感器具有测量方位广、空间分辨率高、测量距离远、速度分辨率高和抗干扰能力强的特点,传感器和工控机通过以太网口连接,通过全双工TCP/IP协议完成信息传递。
所述运动组件中,驱动器、直流电机以及光电编码器组成一个闭环的整体结构,其中直流电机选用的是深圳信邦公司的直流无刷电机,型号为XB3000。该款电机的输入电压为24V,功率最高达200W,同时在电机的顶端配有一个用于检测电机实时速度等参数的光栅编码器。而相对应的,本申请选用了Elmo数字伺服驱动器来作为电机的驱动。该款驱动器输入电压为直流24V,拥有5种不同的电机控制模式,可以通过CAN总线与计算机进行连接并使用软件进行控制以及参数调整,本申请根据运动学控制的需求使用的是速度控制模式。
所述显示组件中,包含小体积的电池电量显示屏以及17寸触控显示器,独立开来的电池电量显示屏可实时显示电池电量。17寸触控显示器一方面可显示***各参数和变量情况,另一方面可通过接受触控命令来操作***软件。
更进一步地说明,所述定位软件包括编码盘定位模块、视觉定位模块、激光定位模块以及扩展卡曼滤波融合模块。
所述编码盘定位模块用于通过光电编码盘获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;
所述视觉定位模块用于通过深度摄像头获取图像,读取连续两帧图像的图像信息,基于观测模型计算获取所述移动机器人的第二状态信息;其中图像信息包括旋转矩阵和平移向量;
所述激光定位模块用于通过激光雷达获取激光数据,匹配当前时刻和下一时刻的激光数据,基于观测模型计算获取所述移动机器人的第三状态信息;
所述扩展卡尔曼滤波融合模块用于获取所述第一状态信息、第二状态信息和第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
本发明通过融合多种传感器信息实现未知室内环境下轮式移动机器人的高精准定位。
以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理,而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释,本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式,这些等同的变型或替换均包含在本申请权利要求所限定的范围内。
Claims (9)
1.一种适用于未知室内环境的移动机器人定位方法,其特征在于,包括以下步骤:
步骤A:通过移动机器人获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;
步骤B:通过移动机器人获取图像,读取连续两帧图像的图像信息进行匹配分析,基于视觉观测模型计算获取所述移动机器人的第二状态信息;
步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于激光观测模型计算获取所述移动机器人的第三状态信息;
步骤D:通过基于运动学模型获取的第一状态信息、基于视觉观测模型获取的第二状态信息以及基于激光观测模型获取的第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
2.根据权利要求1所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤A,具体包括以下步骤:
步骤A1:在移动机器人运动过程中,基于左驱动轮安装的光电编码盘输出的脉冲数分为NL,右驱动轮安装的光电编码盘输出的脉冲数分为NR,光电编码盘的转速为P线/rad,驱动轮的半径为r,车轮运动的距离为ΔS,分别获取左驱动轮的位移增量ΔSL和右驱动轮的位移增量ΔSR为:
步骤A2:基于移动机器人的左驱动轮和右驱动轮的移动距离分别为ΔSl和ΔSr,左驱动轮和右驱动轮之间的距离为d,获取移动机器人发生转动的角度为Δθ=(ΔSr-ΔSl)/d,以及移动机器人运动的距离为ΔS=(ΔSl+ΔSr)/2;
步骤A3:基于移动机器人的运动学模型,可通过移动机器人的单位时间转动的角度Δθ和运动距离ΔS以及当前k时刻的移动机器人位姿[x(k),y(k),θ(k)]T得到第k+1时刻的移动机器人状态X(k+1)=[x(k+1),y(k+1),θ(k+1)]T:
因此,移动机器人在初始位姿上进行位移累加可求得移动机器人在任意时刻的位姿。
3.根据权利要求1所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤B,具体包括以下步骤:
步骤B1:通过判断函数,分别对两帧图像进行ORB特征点的提取,其中判断函数如下:
其中p为候选特征点,circle(p)为以候选特征点p为原点建立的圆,x为circle(p)内的点,G(x)为圆周上的点的灰度值,G(p)为候选特征点的灰度值,ξ为灰度值的阈值,E表示圆圈内像素的灰度值与候选点的灰度值的差异;若E大于或等于阈值ξ,则p为特征点;
步骤B2:获取每一个ORB特征点的BRIEF描述子,使用BRIEF描述子来描述该特征点的特征属性,使用最小汉明距离匹配法对两帧图片中ORB特征点的BRIEF描述子进行粗匹配,获得粗匹配结果;
步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,利用选出来的特征点对采用EPnP算法来计算两帧图像的相对位姿;
步骤B4:根据两帧图像的相对位姿,获取ORB特征点的投影误差,并根据投影误差再次对ORB特征点对进行筛选;
步骤B5:重复执行步骤B4直到所有剩余的ORB特征点对的投影误差皆在允许范围之内,则得出最终的相对位姿,至此,一次图像匹配完成。
4.根据权利要求3所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤B2:获取每一个ORB特征点的BRIEF描述子,具体包括以下步骤:
其中BRIEF描述子为一个二进制数,其第i位的值为Bi;
步骤B22:采用灰度质心法确定特征点的方向,包括:
步骤B221:首先定义中间变量m00,m10和m01为:
其中M为以特征点p为圆心且R为半径的圆形的邻域;
步骤B222:获取邻域M的灰度质心C:
步骤B223:获取射线pC与坐标轴y轴的夹角:
θ=arctan(m01/m10) (8)
射线pC的方向即为特征点p的方向。
5.根据权利要求3所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤B3:对粗匹配结果进行筛选,根据筛选后的粗匹配结果,选出对应的ORB特征点对,利用选出来的特征点对采用EPnP算法来计算两帧图像的相对位姿;所述EPnP算法具体包括以下步骤:
步骤B31:基于匹配的特征点集为{Pi,Qi}(i=1,2,3,...n),分别获取两组匹配的特征点的质心:
步骤B32:对所有的匹配点计算其去质心坐标:
步骤B33:根据去质心坐标,获取矩阵H:
步骤B34:对公式(11)作奇异值分解:
H=UΣVT (12)
其中U是酉矩阵,Σ是半正定对角矩阵,而VT同是酉矩阵,是V的共轭转置;
步骤B35:基于公式(9)和公式(12)获取两帧图像对应的摄像头坐标系的相对旋转矩阵以及平移向量为:
其中R表示为旋转矩阵,t表示为平移向量,两者为视觉观测模型的观测量。
6.根据权利要求1所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤C:通过移动机器人获取激光数据,匹配当前时刻和下一时刻的激光数据,基于激光观测模型计算获取所述移动机器人的第三状态信息,具体包括以下步骤:
步骤C1:采用迭代最近点算法进行扫描匹配;首先,定义激光雷达在t时刻采集到一组数据点{qt},定义在t+1时刻采集到一组数据点{qt+1},其次,对点集{qt+1}的每个点进行坐标变换,将其重投影到坐标系XYt中,投影点集记为变换关系如下所示:
步骤C4:对点集{qt}中的每个点都执行步骤C1至步骤C3,找到在点集{qt+1}中的匹配点,计算每对匹配点之间的重投影误差,然后将每个重投影误差的平方累加起来,即可构造最小二乘的目标函数,如下式所示:
求解使得公式(18)中目标函数取得最小值的旋转矩阵R和平移向量t,两者为激光观测模型的观测量。
8.根据权利要求7所述的一种适用于未知室内环境的移动机器人定位方法,其特征在于,所述步骤D:通过基于运动学模型获取的第一状态信息和基于观测模型获取的第二状态信息以及第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息;其中扩展卡尔曼滤波算法中单次滤波循环的具体步骤如下:
步骤D2:利用一阶泰勒展开公式将运动模型展开成线性方程:
步骤D3:预测状态向量先验估计值的协方差矩阵Pk-+1:
步骤D4:利用一阶泰勒展开公式将观测模型展开成线性方程:
步骤D6:用测量值更新状态变量,得到状态变量的后验估计值:
步骤D7:将协方差矩阵更新为状态变量后验估计值的协方差矩阵:
其中I为协方差矩阵维数相同的单位矩阵;
对于视觉观测模型和激光观测模型分别获取的第二状态信息和第三状态信息,分别参与循环与第一状态信息融合。如此反复步骤D1到D7,从而不断更新获得移动机器人的最新的状态。
9.一种适用于未知室内环境的移动机器人定位***,其特征在于,采用如权利要求1至8任意一项所述的一种适用于未知室内环境的移动机器人定位方法,所述***包括移动机器人硬件和定位软件,所述移动机器人硬件包括电源组件、主控组件、运动组件、感知组件和显示组件,所述定位软件包括编码盘定位模块、视觉定位模块、激光定位模块和扩展卡曼滤波融合模块;
所述编码盘定位模块用于通过光电编码盘获取电机位移运动信息,并结合运动学模型,计算获取所述移动机器人的第一状态信息;
所述视觉定位模块用于通过深度摄像头获取图像,读取连续两帧图像的图像信息,基于观测模型计算获取所述移动机器人的第二状态信息;
所述激光定位模块用于通过激光雷达获取激光数据,匹配当前时刻和下一时刻的激光数据,基于观测模型计算获取所述移动机器人的第三状态信息;
所述扩展卡尔曼滤波融合模块用于获取所述第一状态信息、第二状态信息和第三状态信息,采用扩展卡尔曼滤波算法进行数据融合,得到移动机器人实时状态信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211675418.3A CN116026335B (zh) | 2022-12-26 | 2022-12-26 | 一种适用于未知室内环境的移动机器人定位方法与*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211675418.3A CN116026335B (zh) | 2022-12-26 | 2022-12-26 | 一种适用于未知室内环境的移动机器人定位方法与*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116026335A true CN116026335A (zh) | 2023-04-28 |
CN116026335B CN116026335B (zh) | 2023-10-03 |
Family
ID=86080502
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211675418.3A Active CN116026335B (zh) | 2022-12-26 | 2022-12-26 | 一种适用于未知室内环境的移动机器人定位方法与*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116026335B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113175925A (zh) * | 2021-04-14 | 2021-07-27 | 武汉理工大学 | 定位与导航***以及方法 |
CN113280808A (zh) * | 2021-05-25 | 2021-08-20 | 上海大学 | 一种移动机器人定位精度提升方法及*** |
CN113706626A (zh) * | 2021-07-30 | 2021-11-26 | 西安交通大学 | 一种基于多传感器融合及二维码校正的定位与建图方法 |
CN113985465A (zh) * | 2021-10-28 | 2022-01-28 | 江西省智能产业技术创新研究院 | 传感器融合定位方法、***、可读存储介质及计算机设备 |
CN115015956A (zh) * | 2022-04-12 | 2022-09-06 | 南京邮电大学 | 一种室内无人车的激光与视觉slam*** |
US20230066441A1 (en) * | 2020-01-20 | 2023-03-02 | Shenzhen Pudu Technology Co., Ltd. | Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium |
-
2022
- 2022-12-26 CN CN202211675418.3A patent/CN116026335B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20230066441A1 (en) * | 2020-01-20 | 2023-03-02 | Shenzhen Pudu Technology Co., Ltd. | Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium |
CN113175925A (zh) * | 2021-04-14 | 2021-07-27 | 武汉理工大学 | 定位与导航***以及方法 |
CN113280808A (zh) * | 2021-05-25 | 2021-08-20 | 上海大学 | 一种移动机器人定位精度提升方法及*** |
CN113706626A (zh) * | 2021-07-30 | 2021-11-26 | 西安交通大学 | 一种基于多传感器融合及二维码校正的定位与建图方法 |
CN113985465A (zh) * | 2021-10-28 | 2022-01-28 | 江西省智能产业技术创新研究院 | 传感器融合定位方法、***、可读存储介质及计算机设备 |
CN115015956A (zh) * | 2022-04-12 | 2022-09-06 | 南京邮电大学 | 一种室内无人车的激光与视觉slam*** |
Non-Patent Citations (1)
Title |
---|
禹鑫燚,朱熠琛,詹益安,欧林林: "SLAM 过程中的机器人位姿估计优化算法研究", 高技术通讯, vol. 28, no. 8, pages 712 - 718 * |
Also Published As
Publication number | Publication date |
---|---|
CN116026335B (zh) | 2023-10-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111612760B (zh) | 用于检测障碍物的方法和装置 | |
Alismail et al. | Automatic calibration of a range sensor and camera system | |
CN112509057B (zh) | 相机外参标定方法、装置、电子设备以及计算机可读介质 | |
Stückler et al. | Model learning and real-time tracking using multi-resolution surfel maps | |
Shi et al. | Extrinsic calibration and odometry for camera-LiDAR systems | |
CN110120093A (zh) | 一种多元特征混合优化的rgb-d室内三维测图方法及*** | |
Jin et al. | Visual slam with rgb-d cameras | |
CN113137968A (zh) | 基于多传感器融合的重定位方法、重定位装置和电子设备 | |
Wietrzykowski et al. | PlaneLoc: Probabilistic global localization in 3-D using local planar features | |
Dryanovski et al. | Incremental registration of RGB-D images | |
Luo et al. | Calib-anything: Zero-training lidar-camera extrinsic calibration method using segment anything | |
WO2022088613A1 (zh) | 一种机器人的定位方法及装置、设备、存储介质 | |
Wang et al. | Improved LeGO-LOAM method based on outlier points elimination | |
Li et al. | Extrinsic calibration of non-overlapping multi-camera system with high precision using circular encoded point ruler | |
Sheng et al. | Mobile robot localization and map building based on laser ranging and PTAM | |
CN116026335B (zh) | 一种适用于未知室内环境的移动机器人定位方法与*** | |
Huang et al. | B-splines for purely vision-based localization and mapping on non-holonomic ground vehicles | |
Chen et al. | Binocular vision localization based on vision SLAM system with multi-sensor fusion | |
Cupec et al. | Global localization based on 3d planar surface segments | |
Su | Vanishing points in road recognition: A review | |
CN115060268B (zh) | 一种机房融合定位方法、***、设备及存储介质 | |
CN116309883B (zh) | 一种3D目标6DoF精确定位方法及*** | |
Gong et al. | Scene-aware Online Calibration of LiDAR and Cameras for Driving Systems | |
Das et al. | Sensor fusion in autonomous vehicle using LiDAR and camera sensor with Odometry | |
Zhuang et al. | 3D-laser-based visual odometry for autonomous mobile robot in outdoor environments |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |