CN108253963B - 一种基于多传感器融合的机器人自抗扰定位方法以及定位*** - Google Patents
一种基于多传感器融合的机器人自抗扰定位方法以及定位*** Download PDFInfo
- Publication number
- CN108253963B CN108253963B CN201711385817.5A CN201711385817A CN108253963B CN 108253963 B CN108253963 B CN 108253963B CN 201711385817 A CN201711385817 A CN 201711385817A CN 108253963 B CN108253963 B CN 108253963B
- Authority
- CN
- China
- Prior art keywords
- fusion
- positioning
- inertial navigation
- robot
- odometer
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 160
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 230000000007 visual effect Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 86
- 230000001133 acceleration Effects 0.000 claims description 35
- 238000013519 translation Methods 0.000 claims description 27
- 238000001914 filtration Methods 0.000 claims description 26
- 238000005259 measurement Methods 0.000 claims description 20
- 230000005484 gravity Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 8
- 230000008901 benefit Effects 0.000 abstract description 5
- 230000009286 beneficial effect Effects 0.000 description 8
- 230000007774 longterm Effects 0.000 description 4
- 239000013589 supplement Substances 0.000 description 4
- 238000000605 extraction Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 235000019953 Simplesse® Nutrition 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于多传感器融合的机器人自抗扰定位方法,所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位方法包括以下步骤:步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);步骤S3、若α≥Threshold(α)且β≥Threshold(β)时,采用视觉惯导融合定位,否则采用轮式惯导融合定位。本发明提供的机器人自抗扰定位方法融合多种传感器的优势,优化机器人定位的计算方法,消除定位过程中出现的误差。
Description
技术领域
本发明涉及机器人定位技术领域,尤其涉及一种基于多传感器融合的机器人自抗扰定位方法以及定位***。
背景技术
传统的机器人定位方法是主要是依靠惯导和轮式里程计等来实现。其中航位推测法是最基本的定位方法,主要工作原理是通过运动估计推算移动机器人的位置信息。具体来讲是基于轮式编码器原理,主要通过测量轮子的旋转速率和轮子转的圈数来确定车轮的运动速度,所以有着短期精度高的特点,但也存在问题,只有在水平面进行运动时能够实现运动估计,无法计算垂直方向的变化,不能克服车轮打滑时引起得误差。同时,惯性测量单元结构上主要由三轴加速度计和三轴陀螺仪构成,通过对加速度和角速率进行积分计算机器人的姿态。由于惯性导航可以独立工作,能够依据自身测量数据完成姿态解算和航位推算,但由于在航位计算过程中,先要对加速度积分得到速度再对速度积分得到位移即要进行两次积分所以存在累计误差。
如今,越来越多的研究将视觉定位方法作为传统定位方法的补充,视觉定位方法适用于室内外、陆地及星际等多种环境,且定位精度高,但对图像序列质量要求高,图像中的移动物体、阴影都会影响其运动估计的准确性,而且外界环境,如光线和剧烈抖动也会严重影响图像的质量,从而导致定位精度下降。
因此,单一传感器的定位效果都会有各种各样的局限。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足,提供一种基于多传感器融合的机器人自抗扰定位方法,通过多传感器融合的思想来进一步优化机器人定位的计算方法,将不同传感器的优势集中起来,以消除定位过程中出现的误差问题。
本发明解决上述技术问题的技术方案如下:一种基于多传感器融合的机器人自抗扰定位方法,所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位方法包括以下步骤:
步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
步骤S3、若α≥Threshold(α)且β≥Threshold(β)时,采用视觉惯导融合定位,否则采用轮式惯导融合定位。
本发明的有益效果是:在左右特征匹配点对数α和前后跟踪匹配点对数β多时,视觉惯导融合定位的精度高,此时采用视觉惯导融合定位,当左右特征匹配点对数α和前后跟踪匹配点对数β少时,视觉惯导融合定位的精度低,此时采用轮式惯导融合定位。不同情况下两种融合定位算法的协同定位,主要以视觉惯导融合定位为主,以轮式惯导融合定位为辅,实现机器人长期和稳定的定位。
在上述技术方案的基础上,本发明还可以做如下改进:
进一步,所述视觉惯导融合定位具体包括以下子步骤:
步骤S31、通过所述双目视觉里程计获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵;
步骤S32、通过所述惯导获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息;
步骤S33、建立扩展卡尔曼滤波模型,并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
上述进一步方案的有益效果是:双目视觉里程计的定位精度高,但容易收到外界干扰,惯导数据稳定,但累计误差较为严重,本发明利用扩展卡尔曼滤波模型对双目视觉里程计和惯导进行融合互补,提高了定位精度和定位的稳定性。
进一步,所述子步骤S31进一步包括以下子步骤:
步骤S311、设定所述双目视觉里程计的双目相机平行安装,通过所述双目相机获取双目图像,对所述双目图像的特征点进行匹配,利用视差计算出所述特征点的三维坐标;
步骤S312、以所述双目相机的左相机为基准,建立相机坐标系下前后时刻对应特征点的运动方程组:
Pk=R Pk-1+T
式中:Pk表示k时刻所述特征点在相机坐标系中的三维坐标值,Pk-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值,R表示旋转矩阵,T表示平移矩阵;
去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。
上述进一步方案的有益效果是:通过双目视觉里程计获取旋转矩阵R以及平移矩阵T,以便与惯导获取的姿态角信息进行融合。
进一步,所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ,所述子步骤S32进一步包括以下子步骤:
步骤S321、通过所述惯导获取三轴加速度ax、ay以及az,仅重力作用时所述三轴加速度ax、ay以及az与俯仰角θ以及侧倾角φ之间满足以下关系式:
其中,g为重力加速度,求解上式得到俯仰角θ和侧倾角φ;
步骤S322、通过所述惯导获取角速度ωg,在k时刻至k+1时刻之间的时间段内对角速度ωg积分,得到航向角变化量Δψ,再将所述航向角变化量Δψ累加得到航向角ψ。
上述进一步方案的有益效果是:获取惯导输出的三轴加速度ax、ay以及az,以重力加速度g作为垂直方向参考计算机器人实时的俯仰角θ和侧倾角φ,避免了利用角速度ωg积分求解俯仰角θ和侧倾角φ带来的累积误差
进一步,所述子步骤S33进一步包括以下子步骤:
步骤S331、利用所述姿态角信息计算姿态四元素e0、e1、e2以及e3,定义所述扩展卡尔曼滤波模型的***状态变量D=[e0 e1 e2 e3]T,并建立***的状态方程:
Dk+1=q*Dk+w(k)
其中,DK表示k时刻的状态变量,DK+1表示k+1时刻的状态变量,q表示k时刻到k+1时刻机器人的旋转向量,q由旋转矩阵R计算得到,*为四元素乘法,w(k)表示所述状态方程的噪声向量,求解所述状态方程得到k+1时刻的状态变量Dk+1;
步骤S332、建立观测方程:
其中,v(k)表示观测过程的高斯噪声矩阵,求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角,所述融合姿态角包括融合俯仰角θk、融合侧倾角φk以及融合航向角变化量Δψk,利用所述融合姿态角计算融合旋转矩阵R1;
步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程:
上述进一步方案的有益效果是:采用扩展卡尔曼滤波模型实现双目视觉里程计的旋转矩阵与惯导的姿态角信息的信息融合,并结合双目视觉里程计平移矩阵实现视觉惯导融合定位。
进一步,所述轮式惯导融合定位包括以下子步骤:
步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1k和机器人的右轮的运动路程S2k;
步骤S42、通过所述惯导获取角速度ωg以及三轴加速度,并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk;
步骤S43、建立离散卡尔曼滤波模型,并通过所述离散卡尔曼滤波模型对所述运动路程S1k、运动路程S2k以及角速度ωg进行信息融合,生成融合位置信息,结合所述融合位置信息、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
上述进一步方案的有益效果是:单纯依靠轮式里程计进行定位的精度比较差,采用离散卡尔曼滤波模型将惯导定位和轮式里程计定位进行融合,提高定位精度。
进一步,所述子步骤S43进一步包括以下子步骤:
步骤S431、定义所述离散卡尔曼滤波模型的***状态变量X=[S1k S2k ωg]T,并建立***状态方程:
Xk+1=Ak+1|kXk+Wk
其中,Xk为k时刻的***状态变量,Xk+1为k+1时刻的***状态变量,Ak+1|k为k时刻到k+1时刻的状态转移矩阵,Wk为***状态方程的噪声矩阵,求解所述***状态方程得到k+1时刻的***状态变量Xk+1;
步骤S432、建立***测量方程:
Nk=HkXk+vk
其中,Hk为***量测矩阵并可从所述惯导以及所述轮式里程计中直接读出,vk为所述***量测方程的噪声矩阵,求解所述***状态方程以及***测量方程得到左轮的融合运动路程S3k、右轮的融合运动路程S4k以及融合航向角变化量Δψ1k,累加所述航向角变化量Δψ1k得到k时刻融合航向角ψ1k;
步骤S433、建立融合定位信息推算方程:
其中,l为所述左轮与右轮之间的距离,Δψ1k+1为k到k+1时刻间航向角变化量,xk为机器人在导航坐标系中的X轴坐标,yk为机器人在导航坐标系中的Y轴坐标,rk=S3kl/(S4k-S3k),求解所述融合位置信息推算方程得到融合位置信息Qk;
步骤S434、结合所述融合位置信息Qk、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
上述进一步方案的有益效果是:通过惯导的陀螺仪获取角速度,通过轮式里程计获取左轮的运动路程和右轮的运动路程,并利用离散卡尔曼滤波模型对角速度、左轮的运动路程以及右轮的运动路程进行融合生成融合位置信息,以提高定位精度。
本发明还提供一种基于多传感器融合的机器人自抗扰定位***,所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位***包括:
获取模块,所述获取模块连接所述双目视觉里程计,用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
控制模块,所述控制模块连接所述获取模块,用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
所述双目视觉里程计、轮式里程计以及惯导均与所述控制模块连接;
所述控制模块还用于比较α与Threshold(α)以及β与Threshold(β),若α≥Threshold(α)且β≥Threshold(β)时,所述控制模块控制所述双目视觉历程计以及惯导进行融合定位,否则控制所述轮式里程计以及惯导进行融合定位。
本发明提供的定位***的有益效果与定位方法的有益效果相同,在此不再赘述。
在上述技术方案的基础上,本发明还可以做如下改进:
进一步,所述定位***还包括扩展卡尔曼滤波器,所述双目视觉里程计以及所述惯导均与所述扩展卡尔曼滤波器连接;
所述双目视觉里程计用于获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵,所述惯导用于获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息,所述扩展卡尔曼滤波器用于对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
进一步,所述定位***还包括离散卡尔曼滤波器,所述轮式里程计以及所述惯导均与所述离散卡尔曼滤波器连接;
所述轮式里程计用于获取机器人的运动路程,所述惯导用于获取角速度以及三轴加速度,并根据所述三轴加速度计算俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标,所述离散卡尔曼滤波器用于对所述运动路程以及角速度进行信息融合,生成融合位置信息,结合所述融合位置信息、俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标得到轮式惯导融合定位信息。
附图说明
图1为本发明提供的一种基于多传感器融合的机器人自抗扰定位方法的流程图;
图2为本发明提供的一种基于多传感器融合的机器人自抗扰定位***的结构示意图;
图3为本发明提供的一种基于多传感器融合的机器人自抗扰定位***的扩展卡尔曼滤波器以及离散卡尔曼滤波器的连接关系示意图。
附图中,各标号所代表的部件列表如下:
1、双目视觉里程计,2、轮式里程计,3、惯导,4、获取模块,5、控制模块,6、扩展卡尔曼滤波器,7、离散卡尔曼滤波器。
具体实施方式
以下结合附图对本发明的原理和特征进行描述,所举实例只用于解释本发明,并非用于限定本发明的范围。
下面结合附图,对本发明进行说明。
实施例一
如图1所示,本发明实施例提供一种基于多传感器融合的机器人自抗扰定位方法(以下简称定位方法),所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位方法包括以下步骤:
步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
步骤S3、若α≥Threshold(α)且β≥Threshold(β)时,采用视觉惯导融合定位,否则采用轮式惯导融合定位。
双目视觉里程计精确程度非常依赖于较高的图像序列质量和足够数目的特征点匹配组,拍摄环境光线分布不均、双目视觉里程计出现碰撞、拍摄环境存在大量的白墙等情况都会导致双目视觉里程计匹配的特征点减少,而且误匹配的机率增大,甚至会导致双目视觉里程计的失效,定位精度下降。针对上述情况,本发明在双目视觉里程计失效的情况下,不再使用视觉惯导融合定位,而转为轮式惯导融合定位做一个短时补充,当双目视觉里程计恢复正常后,再重新使用视觉惯导融合定位,这样就能保证机器人长期和稳定的定位精度。
本发明针对视觉惯导融合定位过程中双目视觉里程计在外界环境干扰下容易失效和失真以及在视角内特征点稀少时定位不精确的情况,利用轮式惯导融合定位短期精确度高的优点,将其作为视觉惯导融合定位的补充,以实现机器人长期和稳定的定位。
优化上一实施例,所述视觉惯导融合定位具体包括以下子步骤:
步骤S31、通过所述双目视觉里程计获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵;
步骤S32、通过所述惯导获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息;
步骤S33、建立扩展卡尔曼滤波模型,并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
双目视觉里程计的定位精度高,但容易收到外界干扰,惯导数据稳定,但累计误差较为严重,本发明实施例利用扩展卡尔曼滤波模型对双目视觉里程计和惯导进行融合互补,提高了定位精度和定位的稳定性。
优化上一实施例,所述子步骤S31进一步包括以下子步骤:
步骤S311、设定所述双目视觉里程计的双目相机平行安装,通过所述双目相机获取双目图像,对所述双目图像的特征点进行匹配,利用视差计算出所述特征点的三维坐标;
步骤S312、以所述双目相机的左相机为基准,建立相机坐标系下前后时刻对应特征点的运动方程组:
Pk=R Pk-1+T
式中:Pk表示k时刻所述特征点在相机坐标系中的三维坐标值,Pk-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值,R表示旋转矩阵,T表示平移矩阵;
去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。
所述特征点包括:点状特征、线状特征和区域特征等。一般情况下,特征点的尺度越大,所包含的图像信息就越丰富,匹配所需特征点的数目也就越少,匹配速度越快。但是特征点的尺度越大,特征点的提取和描述也越复杂,定位精度越低。相反的,特征尺度越小,特征点描述相对越简单,定位精度越高,但是匹配所需数目偏多,所包含信息也比较少。因此选择合适的特征点提取方法至关重要。
为了得到精确的特征点的位置,首先对双目图像进行滤波处理,滤波处理方法包括5*5blob和角点标记等。对滤波后的双目图像使用非极大抑制和非极小抑制算法,产生候选的特征点,候选的特征点主要包括blob最大、blob最小、角点最大和角点最小四类。为了减少计算量,仅对上述四类特征点进行匹配。匹配时将特征点与无序的图像序列集进行对比,假设相机轨迹平滑,则不需要像SURF算法一样计算旋转尺度不变性特征描述符。选定两个特征点,特征点的窗口分为多个子窗口,计算每个子窗口的Sobel滤波响应,并将灰度差绝对值之和作为误差度量。
由于16个字节的灰度差绝对值之和可通过一种简单的SSE指令有效的计算,只需要两次调用水平和垂直方向的Sobel响应即可,因此为了加速提取匹配过程,将Sobel滤波响应量化为8位并采用稀疏特征点的方式,只计算中16个子窗口处的灰度差之和,来取代计算整个窗口中所有子窗口处的灰度差绝对值之和。
优化上一实施例,所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ,所述子步骤S32进一步包括以下子步骤:
步骤S321、通过所述惯导获取三轴加速度ax、ay以及az,仅重力作用时所述三轴加速度ax、ay以及az与俯仰角θ以及侧倾角φ之间满足以下关系式:
其中,g为重力加速度,求解上式得到俯仰角θ和侧倾角φ;
步骤S322、通过所述惯导获取角速度ωg,在k时刻至k+1时刻之间的时间段内对角速度ωg积分,得到航向角变化量Δψ,再将所述航向角变化量Δψ累加得到航向角ψ。
惯导由陀螺仪和加速度计两部分构成,通过加速度计获取三轴加速度ax、ay以及az,以重力加速度g作为垂直方向参考,计算机器人实时的俯仰角θ和侧倾角φ,这种方法的优势在于避免了利用角速度ωg积分计算俯仰角θ和侧倾角φ带来的累积误差。但这种方法不能计算航向角ψ,因此利用陀螺仪获取角速度ωg,对角速度ωg积分获取航向角ψ。用陀螺仪估计的ψ与加速度计估计的θ和φ修正双目视觉里程计的姿态估计,可以使得姿态角估计解耦,降低累积误差的影响。
优化上一实施例,所述子步骤S33进一步包括以下子步骤:
步骤S331、利用所述姿态角信息计算姿态四元素e0、e1、e2以及e3,定义所述扩展卡尔曼滤波模型的***状态变量D=[e0e1e2e3]T,并建立***的状态方程:
Dk+1=q*Dk+w(k)
其中,DK表示k时刻的状态变量,DK+1表示k+1时刻的状态变量,q表示k时刻到k+1时刻机器人的旋转向量,q由旋转矩阵R计算得到,*为四元素乘法,w(k)表示所述状态方程的噪声向量,求解所述状态方程得到k+1时刻的状态变量Dk+1;
步骤S332、建立观测方程:
其中,v(k)表示观测过程的高斯噪声矩阵,求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角,所述融合姿态角包括融合俯仰角θk、融合侧倾角φk以及融合航向角变化量Δψk,利用所述融合姿态角计算融合旋转矩阵R1;
步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程:
本方法采用扩展卡尔曼滤波模型实现双目视觉里程计的旋转矩阵和惯导的姿态角信息的融合,并结合双目视觉里程计的平移矩阵实现定位。
优化上一实施例,所述轮式惯导融合定位包括以下子步骤:
步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1k和机器人的右轮的运动路程S2k;
步骤S42、通过所述惯导获取角速度ωg以及三轴加速度,并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk;
步骤S43、建立离散卡尔曼滤波模型,并通过所述离散卡尔曼滤波模型对所述运动路程S1k、运动路程S2k以及角速度ωg进行信息融合,生成融合位置信息,结合所述融合位置信息、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
由双轮差动机器人的运动模型可知,单纯依靠轮式里程计获得的数据推算移动机器人的定位的精度比较差,采用卡尔曼滤波器将惯导的陀螺仪和轮式里程计获得的数据进行融合能提高定位精度。该***是线性,采用离散卡尔曼滤波算法对轮式里程计和陀螺仪采集的信息进行融合。
优化上一实施例,所述子步骤S43进一步包括以下子步骤:
步骤S431、定义所述离散卡尔曼滤波模型的***状态变量X=[S1k S2k ωg]T,并建立***状态方程:
Xk+1=Ak+1|kXk+Wk
其中,Xk为k时刻的***状态变量,Xk+1为k+1时刻的***状态变量,Ak+1|k为k时刻到k+1时刻的状态转移矩阵,Wk为***状态方程的噪声矩阵,求解所述***状态方程得到k+1时刻的***状态变量Xk+1;
步骤S432、建立***测量方程:
Nk=HkXk+vk
其中,Hk为***量测矩阵并可从所述惯导以及所述轮式里程计中直接读出,vk为所述***量测方程的噪声矩阵,求解所述***状态方程以及***测量方程得到左轮的融合运动路程S3k、右轮的融合运动路程S4k以及融合航向角变化量Δψ1k,累加所述航向角变化量Δψ1k得到k时刻融合航向角ψ1k;
步骤S433、建立融合定位信息推算方程:
其中,l为所述左轮与右轮之间的距离,Δψ1k+1为k到k+1时刻间航向角变化量,xk为机器人在导航坐标系中的X轴坐标,yk为机器人在导航坐标系中的Y轴坐标,rk=S3kl/(S4k-S3k),求解所述融合位置信息推算方程得到融合位置信息Qk;
步骤S434、结合所述融合位置信息Qk、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
本实施例通过建立离散卡尔曼滤波模型求解出融合位置信息Qk,并结合融合位置信息Qk、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk均可通过惯导直接测量计算得到,其中俯仰角θ和侧倾角φ优选使用步骤S321中方法计算得到。
实施例二
如图2所示,本发明实施例提供一种基于多传感器融合的机器人自抗扰定位***(以下简称定位***),所述多传感器包括双目视觉里程计1、轮式里程计2以及惯导3,所述定位***包括:
获取模块4,所述获取模块4连接所述双目视觉里程计1,用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
控制模块5,所述控制模块5连接所述获取模块4,用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
所述双目视觉里程计1、轮式里程计2以及惯导3均与所述控制模块5连接;
所述控制模块5还用于比较α与Threshold(α)以及β与Threshold(β),当α≥Threshold(α)且β≥Threshold(β)时,所述控制模块5控制所述双目视觉历程计1以及惯导3进行定位,否则控制所述轮式里程计2以及惯导3进行定位。
本实施例提供的定位***通过获取模块4获取左右特征匹配点对数α和前后跟踪匹配点对数β,通过控制模块5比较α与Threshold(α)以及β与Threshold(β),并根据比较结果选择不同的定位方法,当α≥Threshold(α)且β≥Threshold(β)时,控制模块5驱动双目视觉历程计1以及惯导3进行视觉惯导融合定位,否则,控制模块5驱动轮式里程计2以及惯导3进行轮式惯导融合定位。
本发明实施例提供的定位***针对视觉惯导融合定位过程中双目视觉里程计在外界环境干扰下容易失效和失真以及在视角内特征点稀少时定位不精确的情况,利用轮式惯导融合定位短期精确度高的优点,将轮式惯导融合定位作为视觉惯导融合定位的补充,以实现机器人长期和稳定的定位效果。
优化上一实施例,如图3所示,所述定位***还包括扩展卡尔曼滤波器6,所述双目视觉里程计1以及所述惯导3均与所述扩展卡尔曼滤波器6连接;
所述双目视觉里程计1用于获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵,所述惯导3用于获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息,所述扩展卡尔曼滤波器6用于对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
利用扩展卡尔曼滤波器6实现双目视觉里程计1和惯导3的融合定位。
优化上一实施例,如图3所示,所述定位***还包括离散卡尔曼滤波器7,所述轮式里程计2以及所述惯导3均与所述离散卡尔曼滤波器7连接;
所述轮式里程计2用于获取机器人的运动路程,所述惯导3用于获取角速度以及三轴加速度,并根据所述三轴加速度计算俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标,所述离散卡尔曼滤波器7用于对所述运动路程以及角速度进行信息融合,生成融合位置信息,结合所述融合位置信息、俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标得到轮式惯导融合定位信息。
利用离散卡尔曼滤波器7实现轮式里程计2和惯导3的融合定位。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (7)
1.一种基于多传感器融合的机器人自抗扰定位方法,其特征在于,所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位方法包括以下步骤:
步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
步骤S3、若α≥Threshold(α)且β≥Threshold(β)时,采用视觉惯导融合定位,否则采用轮式惯导融合定位;
其中,所述轮式惯导融合定位包括以下子步骤:
步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1k和机器人的右轮的运动路程S2k;
步骤S42、通过所述惯导获取角速度ωg以及三轴加速度,并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk;
步骤S43、定义离散卡尔曼滤波模型的***状态变量X=[S1k S2kωg]T,并建立***状态方程:
Xk+1=Ak+1|kXk+Wk
其中,Xk为k时刻的***状态变量,Xk+1为k+1时刻的***状态变量,Ak+1|k为k时刻到k+1时刻的状态转移矩阵,Wk为***状态方程的噪声矩阵,求解所述***状态方程得到k+1时刻的***状态变量Xk+1;
步骤S44、建立***测量方程:
Nk=HkXk+vk
其中,Hk为***量测矩阵并可从所述惯导以及所述轮式里程计中直接读出,vk为所述***量测方程的噪声矩阵,求解所述***状态方程以及***测量方程得到左轮的融合运动路程S3k、右轮的融合运动路程S4k以及融合航向角变化量Δψ1k,累加所述航向角变化量Δψ1k得到k时刻融合航向角ψ1k;
步骤S45、建立融合定位信息推算方程:
其中,l为所述左轮与右轮之间的距离,Δψ1k+1为k到k+1时刻间航向角变化量,xk为机器人在导航坐标系中的X轴坐标,yk为机器人在导航坐标系中的Y轴坐标,rk=S3kl/(S4k-S3k),求解所述融合位置信息推算方程得到融合位置信息Qk;
步骤S46、结合所述融合位置信息Qk、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
2.根据权利要求1所述基于多传感器融合的机器人自抗扰定位方法,其特征在于,所述视觉惯导融合定位具体包括以下子步骤:
步骤S31、通过所述双目视觉里程计获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵;
步骤S32、通过所述惯导获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息;
步骤S33、建立扩展卡尔曼滤波模型,并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
3.根据权利要求2所述基于多传感器融合的机器人自抗扰定位方法,其特征在于,所述步骤S31进一步包括以下子步骤:
步骤S311、设定所述双目视觉里程计的双目相机平行安装,通过所述双目相机获取双目图像,对所述双目图像的特征点进行匹配,利用视差计算出所述特征点的三维坐标;
步骤S312、以所述双目相机的左相机为基准,建立相机坐标系下前后时刻对应特征点的运动方程组:
Pk=R Pk-1+T
式中:Pk表示k时刻所述特征点在相机坐标系中的三维坐标值,Pk-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值,R表示旋转矩阵,T表示平移矩阵;
去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。
5.根据权利要求4所述基于多传感器融合的机器人自抗扰定位方法,其特征在于,所述步骤S33进一步包括以下子步骤:
步骤S331、利用所述姿态角信息计算姿态四元素e0、e1、e2以及e3,定义所述扩展卡尔曼滤波模型的***状态变量D=[e0 e1 e2 e3]T,并建立***的状态方程:
Dk+1=q*Dk+w(k)
其中,DK表示k时刻的状态变量,DK+1表示k+1时刻的状态变量,q表示k时刻到k+1时刻机器人的旋转向量,q由旋转矩阵R计算得到,*为四元素乘法,w(k)表示所述状态方程的噪声向量,求解所述状态方程得到k+1时刻的状态变量Dk+1;
步骤S332、建立观测方程:
其中,v(k)表示观测过程的高斯噪声矩阵,求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角,所述融合姿态角包括融合俯仰角θk、融合侧倾角φk以及融合航向角变化量Δψk,利用所述融合姿态角计算融合旋转矩阵R1;
步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程:
6.一种基于多传感器融合的机器人自抗扰定位***,其特征在于,所述多传感器包括双目视觉里程计、轮式里程计以及惯导,所述定位***包括:
获取模块,所述获取模块连接所述双目视觉里程计,用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β;
控制模块,所述控制模块连接所述获取模块,用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β);
所述双目视觉里程计、轮式里程计以及惯导均与所述控制模块连接;
所述控制模块还用于比较α与Threshold(α)以及β与Threshold(β),若α≥Threshold(α)且β≥Threshold(β)时,所述控制模块控制所述双目视觉历程计以及惯导进行融合定位,否则控制所述轮式里程计以及惯导进行融合定位;
还包括离散卡尔曼滤波器,所述轮式里程计以及所述惯导均与所述离散卡尔曼滤波器连接;
所述轮式里程计用于:
获取k时刻到k+1时刻机器人的左轮的运动路程S1k和机器人的右轮的运动路程S2k;
通过所述惯导获取角速度ωg以及三轴加速度,并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk;
定义所述离散卡尔曼滤波模型的***状态变量X=[S1k S2k ωg]T,并建立***状态方程:
Xk+1=Ak+1|kXk+Wk
其中,Xk为k时刻的***状态变量,Xk+1为k+1时刻的***状态变量,Ak+1|k为k时刻到k+1时刻的状态转移矩阵,Wk为***状态方程的噪声矩阵,求解所述***状态方程得到k+1时刻的***状态变量Xk+1;
建立***测量方程:
Nk=HkXk+vk
其中,Hk为***量测矩阵并可从所述惯导以及所述轮式里程计中直接读出,vk为所述***量测方程的噪声矩阵,求解所述***状态方程以及***测量方程得到左轮的融合运动路程S3k、右轮的融合运动路程S4k以及融合航向角变化量Δψ1k,累加所述航向角变化量Δψ1k得到k时刻融合航向角ψ1k;
建立融合定位信息推算方程:
其中,l为所述左轮与右轮之间的距离,Δψ1k+1为k到k+1时刻间航向角变化量,xk为机器人在导航坐标系中的X轴坐标,yk为机器人在导航坐标系中的Y轴坐标,rk=S3kl/(S4k-S3k),求解所述融合位置信息推算方程得到融合位置信息Qk;
结合所述融合位置信息Qk、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标zk得到轮式惯导融合定位信息。
7.根据权利要求6所述基于多传感器融合的机器人自抗扰定位***,其特征在于,还包括扩展卡尔曼滤波器,所述双目视觉里程计以及所述惯导均与所述扩展卡尔曼滤波器连接;
所述双目视觉里程计用于获取双目图像,并根据所述双目图像获取旋转矩阵以及平移矩阵,所述惯导用于获取角速度以及三轴加速度,并根据所述角速度和三轴加速度计算姿态角信息,所述扩展卡尔曼滤波器用于对所述姿态角信息以及所述旋转矩阵进行信息融合,计算出融合旋转矩阵,并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711385817.5A CN108253963B (zh) | 2017-12-20 | 2017-12-20 | 一种基于多传感器融合的机器人自抗扰定位方法以及定位*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711385817.5A CN108253963B (zh) | 2017-12-20 | 2017-12-20 | 一种基于多传感器融合的机器人自抗扰定位方法以及定位*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108253963A CN108253963A (zh) | 2018-07-06 |
CN108253963B true CN108253963B (zh) | 2021-04-20 |
Family
ID=62723522
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711385817.5A Active CN108253963B (zh) | 2017-12-20 | 2017-12-20 | 一种基于多传感器融合的机器人自抗扰定位方法以及定位*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108253963B (zh) |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109084785A (zh) * | 2018-07-25 | 2018-12-25 | 吉林大学 | 多车辆协同定位与地图构建方法、装置、设备及存储介质 |
CN109141411B (zh) * | 2018-07-27 | 2022-01-21 | 顺丰科技有限公司 | 定位方法、定位装置、移动机器人及存储介质 |
CN109143303B (zh) * | 2018-09-03 | 2021-04-20 | 河北雄安远度科技有限公司 | 飞行定位方法、装置及固定翼无人机 |
CN109447030A (zh) * | 2018-11-12 | 2019-03-08 | 重庆知遨科技有限公司 | 一种用于火灾场景的消防机器人动作实时指导算法 |
CN111322993B (zh) * | 2018-12-13 | 2022-03-04 | 杭州海康机器人技术有限公司 | 一种视觉定位方法和装置 |
CN110081880A (zh) * | 2019-04-12 | 2019-08-02 | 同济大学 | 一种融合视觉、轮速与惯导的清扫车局部定位***及方法 |
CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
CN110455301A (zh) * | 2019-08-01 | 2019-11-15 | 河北工业大学 | 一种基于惯性测量单元的动态场景slam方法 |
CN112394190B (zh) * | 2019-08-15 | 2022-11-04 | 纳恩博(北京)科技有限公司 | 角速度的确定方法及装置、存储介质、电子装置 |
CN110411457B (zh) * | 2019-08-27 | 2024-04-19 | 纵目科技(上海)股份有限公司 | 基于行程感知与视觉融合的定位方法、***、终端和存储介质 |
CN110689513B (zh) * | 2019-09-26 | 2022-09-02 | 石家庄铁道大学 | 彩色图像融合方法、装置和终端设备 |
CN110866482B (zh) * | 2019-11-08 | 2022-09-16 | 广东工业大学 | 一种里程计数据源动态选择方法、装置和设备 |
CN111360818A (zh) * | 2020-01-15 | 2020-07-03 | 上海锵玫人工智能科技有限公司 | 一种通过视觉定位的机械臂控制*** |
CN111504309B (zh) * | 2020-04-28 | 2021-09-10 | 东风汽车集团有限公司 | 低速运动中汽车位姿的计算方法 |
CN111595336B (zh) * | 2020-07-27 | 2020-10-27 | 北京云迹科技有限公司 | 一种机器人定位信息的确定方法及装置 |
CN112476433B (zh) * | 2020-11-23 | 2023-08-04 | 深圳怪虫机器人有限公司 | 一种基于识别阵列边界的移动机器人定位方法 |
CN113777615B (zh) * | 2021-07-19 | 2024-03-29 | 派特纳(上海)机器人科技有限公司 | 室内机器人的定位方法、***及清洁机器人 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102121827A (zh) * | 2010-11-29 | 2011-07-13 | 浙江亚特电器有限公司 | 一种移动机器人定位***及其定位方法 |
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
CN104680522A (zh) * | 2015-02-09 | 2015-06-03 | 浙江大学 | 基于智能手机前后摄像头同时工作的视觉定位方法 |
CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及*** |
CN106123908A (zh) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | 车辆导航方法和*** |
-
2017
- 2017-12-20 CN CN201711385817.5A patent/CN108253963B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102121827A (zh) * | 2010-11-29 | 2011-07-13 | 浙江亚特电器有限公司 | 一种移动机器人定位***及其定位方法 |
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
CN104680522A (zh) * | 2015-02-09 | 2015-06-03 | 浙江大学 | 基于智能手机前后摄像头同时工作的视觉定位方法 |
CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及*** |
CN106123908A (zh) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | 车辆导航方法和*** |
Also Published As
Publication number | Publication date |
---|---|
CN108253963A (zh) | 2018-07-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108253963B (zh) | 一种基于多传感器融合的机器人自抗扰定位方法以及定位*** | |
CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
CN110243358B (zh) | 多源融合的无人车室内外定位方法及*** | |
CN110044354B (zh) | 一种双目视觉室内定位与建图方法及装置 | |
CN106017463B (zh) | 一种基于定位传感装置的飞行器定位方法 | |
CN109506642B (zh) | 一种机器人多相机视觉惯性实时定位方法及装置 | |
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN110084832B (zh) | 相机位姿的纠正方法、装置、***、设备和存储介质 | |
CN106052584B (zh) | 一种基于视觉及惯性信息融合的轨道空间线形测量方法 | |
CN109648558B (zh) | 机器人曲面运动定位方法及其运动定位*** | |
US10757333B2 (en) | Method for determining bias in an inertial measurement unit of an image acquisition device | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN107941217A (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
CN112229405B (zh) | 一种基于图像跟踪与激光测距的无人机目标运动估计方法 | |
CN110207693B (zh) | 一种鲁棒立体视觉惯性预积分slam方法 | |
CN110865650A (zh) | 基于主动视觉的无人机位姿自适应估计方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN111307146B (zh) | 一种基于双目相机和imu的虚拟现实头戴显示设备定位*** | |
CN113503873B (zh) | 一种多传感器融合的视觉定位方法 | |
CN110736457A (zh) | 一种基于北斗、gps和sins的组合导航方法 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN115015956A (zh) | 一种室内无人车的激光与视觉slam*** | |
CN112284381B (zh) | 视觉惯性实时初始化对准方法及*** | |
CN112556681A (zh) | 一种基于视觉的果园机械导航定位方法 |
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 |