CN111136660B - 机器人位姿定位方法及系统 - Google Patents

机器人位姿定位方法及系统 Download PDF

Info

Publication number
CN111136660B
CN111136660B CN202010102048.9A CN202010102048A CN111136660B CN 111136660 B CN111136660 B CN 111136660B CN 202010102048 A CN202010102048 A CN 202010102048A CN 111136660 B CN111136660 B CN 111136660B
Authority
CN
China
Prior art keywords
pose
current
time
state vector
value
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
Application number
CN202010102048.9A
Other languages
English (en)
Other versions
CN111136660A (zh
Inventor
张凯
黄鑫
董宇涵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen International Graduate School of Tsinghua University
Original Assignee
Shenzhen International Graduate School of Tsinghua University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen International Graduate School of Tsinghua University filed Critical Shenzhen International Graduate School of Tsinghua University
Priority to CN202010102048.9A priority Critical patent/CN111136660B/zh
Publication of CN111136660A publication Critical patent/CN111136660A/zh
Application granted granted Critical
Publication of CN111136660B publication Critical patent/CN111136660B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1602Programme controls characterised by the control system, structure, architecture

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了机器人位姿定位方法及系统。涉及机器人定位领域,其中,方法通过采集IMU里程计数据作为局部参考系,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵,对上一时刻位姿状态向量进行采样,并对采样点进行无迹变换,利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,滤波结束后通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。利用无迹卡尔曼滤波算法结合IMU里程计数据和GPS卫星或视觉系统采集的实际测量值得到全局位姿估计值,对复杂环境具有鲁棒性,提高定位精度。

Description

机器人位姿定位方法及系统
技术领域
本发明涉及机器人定位领域,尤其是涉及一种机器人位姿定位方法及系统。
背景技术
在机器人领域中,当需要解决未知环境下的定位问题通常采用同时定位与地图构建(simultaneous localization and mapping,SLAM)的方法,其核心思想是利用传感器数据实时构建周围环境地图同时实现对机器人自身的定位。而针对机器人的SLAM问题,主要是机器人的定位问题,其本质为一个状态估计问题,因此通常采用卡尔曼滤波算法(KF)进行状态估计以实现定位。但是经典的卡尔曼滤波算法只针对线性系统,实际问题往往基于较为复杂的非线性系统。针对非线性系统多采用近似线性化处理的扩展卡尔曼滤波(EKF)算法,其利用一阶泰勒展开处理非线性问题的线性化问题,忽略泰勒展开的高阶项,求解存在一定误差。因此需要提出一种能够提高定位精度的机器人位姿定位方法,实现复杂非线性状态的机器人位姿定位。
发明内容
本发明旨在至少解决现有技术中存在的技术问题之一。为此,本发明提出一种机器人位姿定位方法,能够提高定位精度,实现复杂非线性状态的机器人位姿定位。
第一方面,本发明的一个实施例提供了:一种机器人位姿定位方法,包括:
采集IMU里程计数据作为局部参考系,根据所述局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵;
对所述上一时刻位姿状态向量进行采样,并对采样点进行无迹变换;
利用系统模型对经过无迹变换的上一时刻位姿状态向量和所述上一时刻协方差矩阵进行预测得到当前时刻预测值,所述当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
结合实际测量值对所述当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,所述实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
滤波结束后通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据所述当前时刻全局位姿估计值进行机器人位姿定位。
进一步地,所述采集IMU里程计数据作为局部参考系,根据所述局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵具体包括:
根据所述局部参考系下的相对位姿和预设滑动窗口内前N个局部参考系相对于全局参考系的相对位姿得到位姿状态向量估计值;
根据所述位姿状态向量的估计值得到上一时刻位姿状态向量和上一时刻协方差矩阵。
进一步地,所述对所述上一时刻位姿状态向量进行采样,并对采样点进行无迹变换具体包括:
对所述上一时刻位姿状态向量进行Sigma点采样,并对采样的Sigma点进行无迹卡尔曼滤波变换。
进一步地,通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值具体包括:根据局部参考系与全局参考系的坐标变换关系,将所述局部参考系下的当前时刻相对位姿测量值转换为所述全局参考系下的当前时刻全局位姿估计值。
进一步地,还包括将采集的IMU里程计的初始数据作为局部参考系的初始参考值进行数据初始化。
第二方面,本发明的一个实施例提供了:一种机器人位姿定位系统,包括:
获取模块:用于采集IMU里程计数据作为局部参考系,根据所述局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵;
采样模块:用于对所述上一时刻位姿状态向量进行采样,并对采样点进行无迹变换;
预测模块:用于利用系统模型对经过无迹变换的上一时刻位姿状态向量和所述上一时刻协方差矩阵进行预测得到当前时刻预测值,所述当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
滤波模块:用于结合实际测量值对所述当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,所述实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
位姿定位模块:用于滤波结束后通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据所述当前时刻全局位姿估计值进行机器人位姿定位。
第三方面,本发明的一个实施例提供了:一种机器人位姿定位设备,包括:
至少一个处理器,以及与所述至少一个处理器通信连接的存储器;
其中,所述处理器通过调用所述存储器中存储的计算机程序,用于执行如第一方面任一项所述的方法。
第四方面,本发明的一个实施例提供了:一种计算机可读存储介质,所述计算机可读存储介质存储有计算机可执行指令,所述计算机可执行指令用于使计算机执行如第一方面任一项所述的方法。
本发明的有益效果是:
本发明通过采集IMU里程计数据作为局部参考系,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵,对上一时刻位姿状态向量进行采样,并对采样点进行无迹变换,利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,滤波结束后通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。利用无迹卡尔曼滤波算法结合IMU里程计数据和GPS卫星或视觉系统采集的实际测量值得到全局位姿估计值,对复杂环境具有鲁棒性,提高定位精度。
应当理解的是,以上的一般描述和后文的细节描述仅是示例性和解释性的,并不能限制本公开。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本公开的实施例,并与说明书一起用于解释本公开的原理。显而易见地,下面描述中的附图仅仅是本公开的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。在附图中:
图1是本发明实施例中位姿定位方法的一具体实施例流程示意图;
图2是本发明实施例中位姿定位方法的一具体实施例一种具体实现方式流程示意图;
图3是本发明实施例中位姿定位系统的一具体实施例结构框图。
具体实施方式
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对照附图说明本发明的具体实施方式。显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图,并获得其他的实施方式。
除非另有定义,本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的,不是旨在于限制本发明。
附图中所示的流程图仅是示例性说明,不是必须包括所有的内容和操作/步骤,也不是必须按所描述的顺序执行。例如,有的操作/步骤还可以分解,而有的操作/步骤可以合并或部分合并,因此实际执行的顺序有可能根据实际情况改变。
实施例一:
本发明实施例一提供一种位姿定位方法。
本实施例采用无迹卡尔曼滤波算法,该算法利用Sigma点采样和无迹变换来实现概率密度分布近似,能够保留高阶项,提高求解精度,但是无迹卡尔曼滤波算法的前提是已知系统噪声方差矩阵Q和观测噪声方差矩阵R,实际应用中,这两者的数学模型很难构建,或者其时刻发生变换,无法进行准确描述,如果直接采用无迹卡尔曼滤波算法则可能导致求解的状态估计局部最优,最后使得移动机器人定位结果出现较大误差。
因此本实施例中结合IMU里程计作为局部参考系,结合GPS卫星、视觉系统采集的数据进行无迹卡尔曼滤波估计,得到当前时刻相对位姿测量值,经过坐标转换和校准得到全局位姿估计值,能够在仅有GPS卫星、IMU里程计和视觉系统数据的情况下,在变化的局部参考系下计算相对位姿进行全局位姿估计,对复杂的室外环境具有鲁棒性,能够完成移动机器人的高精度定位过程。
图1为本发明实施例提供的一种位姿定位方法的流程示意图,如图1所示,该方法包括以下步骤:
S1:将采集的IMU里程计的初始数据作为局部参考系的初始参考值进行数据初始化。
S2:采集IMU里程计数据作为局部参考系,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵。
S3:对上一时刻位姿状态向量进行采样,并对采样点进行无迹变换;
S4:利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
S5:结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
S6:滤波结束后通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。
具体的,在本实施例的一种实现场景中,步骤S1中,IMU里程计数据中包括:重力加速度数据、IMU加速度计数据、IMU陀螺仪数据等,数据初始化具体包括一下部分:
1)在本实施例的定位过程中,将状态向量的局部参考系设为每个时间步长相应的IMU里程计的局部参考系。
2)初始化全局位姿和IMU里程计的相对位姿,例如将其均设为
Figure BDA00023871901000000511
并且设定位姿的误差为0。
3)初始化局部重力加速度,例如将其设为IMU加速度计的首次测量值,或将其设为采集的IMU加速度计初始数据的均值。
4)将初始采集的IMU里程计的加速度计测量值消除重力影响后,得到对应的IMU加速度计偏差,将初始采集IMU陀螺仪偏差设置为IMU里程计测量值的均值;
5)初始化重力加速度计、IMU加速度计、IMU陀螺仪的误差,分别表示为:
Figure BDA0002387190100000051
Figure BDA0002387190100000052
其中,ΔT为初始化时间,
Figure BDA0002387190100000053
分别为重力加速度计、IMU陀螺仪和IMU加速度计的高斯白噪声方差,I3为3x3的单位矩阵。
经过上述数据初始化后进行后续定位。
步骤S2中,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵的过程如下所述。
首先设本实施例中k时刻的位姿状态向量为xk,则其表示为:
Figure BDA0002387190100000054
其中,
Figure BDA0002387190100000055
表示xk的估计值,即位姿状态向量的估计值,
Figure BDA0002387190100000056
表示局部参考系{Rk}下的位姿状态向量,
Figure BDA0002387190100000057
表示预设滑动窗口内前N个局部参考系相对于全局参考系的相对位姿。
进一步地:
Figure BDA0002387190100000058
其中,
Figure BDA0002387190100000059
表示全局坐标系{G}在局部参考系{Rk}下的参数信息,
Figure BDA00023871901000000510
表示从局部参考系到当前IMU里程计坐标系下的IMU里程计数据测量值。
进一步地,
Figure BDA0002387190100000061
表示为:
Figure BDA0002387190100000062
其中,
Figure BDA0002387190100000063
表示从全局参考系{G}到局部参考系{Rk}的旋转单位四元数,该四元数用于表示两个坐标系之间的旋转矩阵,
Figure BDA0002387190100000064
表示全局参考系{G}在局部参考系{Rk}的位置,
Figure BDA0002387190100000065
表示局部参考系下的重力加速度。
Figure BDA0002387190100000066
表示为:
Figure BDA0002387190100000067
其中,
Figure BDA0002387190100000068
表示从局部参考系{Rk}到IMU当前坐标系{Iτ}之间的旋转矩阵和平移矩阵,
Figure BDA0002387190100000069
表示IMU里程计数据中的测量速度,
Figure BDA00023871901000000610
分别表示IMU陀螺仪偏差和IMU加速度计偏差。
另外,
Figure BDA00023871901000000611
表示为:
Figure BDA00023871901000000612
其中,
Figure BDA00023871901000000613
表示分别表示从局部参考系{Ri-1}到局部参考系{Ri}之间的相对旋转估计值和平移估计值。
上述各坐标系之间的旋转矩阵或平移矩阵都能够通过先验信息获得。
本实施例中,根据局部参考系下的相对位姿和预设滑动窗口内前N个局部参考系相对于全局参考系的相对位姿得到位姿状态向量估计值,然后根据位姿状态向量的估计值得到上一时刻位姿状态向量和上一时刻协方差矩阵,表示为:
Figure BDA00023871901000000614
Figure BDA00023871901000000615
其中,
Figure BDA00023871901000000616
表示k时刻的状态估计值,xk表示定位过程中k时刻位姿状态向量,
Figure BDA00023871901000000617
即表示上一时刻位姿状态向量,Pk-1表示上一时刻协方差矩阵。
步骤S3的一种实现方式为:对上一时刻位姿状态向量进行Sigma点采样,并对采样的Sigma点进行无迹卡尔曼滤波变换。
Sigma点即为采样点。例如满足条件为:X~N(μ,σ2),Y=sin(X),要求计算随机变量Y的期望和方差。利用Sigma点采样的方式是:对X进行采样,比如取500个采样点(将这些采样点称为sigma点),然后求取这些采样点的期望和方差,当采样值足够大时,结果与理论值接近。
本实施例中对采样点进行无迹变换,表示为:
Figure BDA0002387190100000071
其中,
Figure BDA0002387190100000072
表示k-1时刻(上一时刻)位姿状态向量的第i个采样点,
Figure BDA0002387190100000073
表示k-1时刻位姿状态向量的均值,λ表示决定Sigma点集缩放比例的参数,而L为k-1时刻位姿状态向量的协方差矩阵Pk-1的Cholesky分解,即满足条件:P=L×LT
Cholesky分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解,又称平方根法。
具体的,上述参数λ表示为:
λ=α2(n+κ)-n (9)
其中,α表示Sigma点集中点到均值之间的距离(通常设置较小的值),n表示位姿状态向量的维度,κ取值一般是0或者3-n,需要满足条件:n+κ=3。
进行无迹卡尔曼滤波变换后,步骤S4中利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵,系统模型即机器人定位系统的状态方程,是一个非线性函数,根据实际参数建立,该步骤即为Sigma点进行非线性处理,根据k-1时刻(即上一时刻)位姿状态向量和协方差矩阵预测k时刻(即当前时刻)的位姿状态向量和协方差矩阵,预测过程表示为:
Figure BDA0002387190100000074
Figure BDA0002387190100000075
Figure BDA0002387190100000076
其中,f(·)表示非线性函数的系统模型,
Figure BDA0002387190100000077
表示Sigma点,
Figure BDA0002387190100000078
表示Sigma点对应的权值,Qk表示k时刻下的系统噪声方差矩阵,
Figure BDA0002387190100000081
表示当前时刻位姿状态向量,Pk|k-1表示当前时刻协方差矩阵。
具体的,Sigma点对应的权值
Figure BDA0002387190100000082
表示为:
Figure BDA0002387190100000083
Wc 0=λ/(n+λ)+(1-α2+β) (14)
Figure BDA0002387190100000084
其中,
Figure BDA0002387190100000085
表示Sigma点均值的权值,Wc 0为Sigma点协方差的权值,进一步地,高斯分布下,β的值设为2。
步骤S4中,结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据,即融合GPS卫星数据、视觉系统采集数据和IMU里程计数据进行机器人位姿定位,提高机器人局部工作区域内的定位精度,同时在某些GPS信号遮盖区域仍可保证较高的定位精度。
滤波处理过程表示为:
Figure BDA0002387190100000086
Figure BDA0002387190100000087
Figure BDA0002387190100000088
Figure BDA0002387190100000089
其中,h(·)表示非线性函数的测量模型,通常根据实际机器人系统构建,
Figure BDA00023871901000000810
表示k时刻下第i个状态Sigma采样点的预估测量值,
Figure BDA00023871901000000811
表示加权预估测量值,zk表示k时刻下的实际测量值,包括GPS卫星、视觉系统采集的当前时刻实际相对位姿数据,
Figure BDA00023871901000000812
表示k时刻下实际测量值与加权预估测量值之间的误差,PZ表示k时刻下预估的协方差矩阵,Rk表示观测噪声方差矩阵。
进行无迹卡尔曼滤波时,需要计算卡尔曼增益K,更新位姿状态向量xk和协方差矩阵Pk,表示为:
Figure BDA0002387190100000091
Figure BDA0002387190100000092
Pk=Pk|k-1-KkPZKk T (22)
得到当前时刻相对位姿测量值后,需要进行全局位姿估计值的转换,步骤S6中根据局部参考系与全局参考系的坐标变换关系,将局部参考系下的当前时刻相对位姿测量值转换为全局参考系下的当前时刻全局位姿估计值,转换过程表示为:
Figure BDA0002387190100000093
其中,
Figure BDA0002387190100000094
GP分别表示局部参考系{Rk}和全局参考系{G}下的位姿,即当前时刻相对位姿测量值和当前时刻全局位姿估计值,
Figure BDA0002387190100000095
表示从局部参考系{Rk}和全局参考系{G}的坐标转换关系,例如旋转矩阵等。
如果后续还存在GPS数据、IMU里程计数据或视觉系统采集的数据,则重复上述步骤S2~S6,以实现移动机器人实时高精度位姿估计。
如图2所示,为本实施例的一种具体实现方式流程示意图。从图中可见:
1)首先进行数据初始化,包括:初始化当前局部参考系、初始化全局位姿和IMU里程计的相对位姿、初始化局部重力加速度、初始化IMU加速度计偏差和IMU陀螺仪偏差、初始化重力加速度计、IMU加速度计、IMU陀螺仪的误差。
2)初始化成功后,则读取IMU里程计上一时刻位姿状态向量和上一时刻协方差矩阵,更新机器人环境地图。
3)对上一时刻的位姿状态向量和上一时刻协方差矩阵进行Sigma采样,并对采样点进行无迹变换;
4)进行时间更新,即预测当前时刻的位姿状态向量和协方差矩阵。
5)读取GPS、IMU里程计、视觉系统的实际测量值进行滤波处理,并更新得到当前时刻相对位姿测量值;
6)计算卡尔曼增益,并更新位姿状态向量和协方差矩阵;
7)判断是否滤波结束,如果结束则通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。
本实施例在融合GPS卫星、IMU里程计和视觉系统采集数据的情况下,针对机器人定位状态方程的非线性问题,通过无迹卡尔曼滤波,来减少或消除干扰数据,通过可变换的局部参考系作为全局位姿估计的参考系,计算当前时刻的相对位姿,进行坐标转换后进行移动机器人实时高精度位姿估计,实现了机器人的全局位姿定位。提高机器人局部工作区域内的定位精度,同时在某些GPS信号遮盖区域仍可保证较高的定位精度。
实施例二:
本实施例提供一种机器人位姿定位系统,用于执行如实施例一所述的方法,如图3所示,为本实施例的机器人位姿定位系统结构框图,包括:
获取模块100:用于采集IMU里程计数据作为局部参考系,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵;
采样模块200:用于对上一时刻位姿状态向量进行采样,并对采样点进行无迹变换;
预测模块300:用于利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
滤波模块400:用于结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
位姿定位模块500:用于滤波结束后通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。
上述中机器人位姿定位系统中各模块的具体细节已经在实施例一对应的机器人位姿定位方法中进行了详细的描述,因此此处不再赘述。
另外,本发明还提供机器人位姿定位设备,包括:
至少一个处理器,以及与所述至少一个处理器通信连接的存储器;
其中,所述处理器通过调用所述存储器中存储的计算机程序,用于执行如实施例一所述的方法。计算机程序即程序代码,当程序代码在机器人位姿定位设备上运行时,程序代码用于使机器人位姿定位设备执行本说明书上述实施例一部分描述的机器人位姿定位方法中的步骤。
另外,本发明还提供一种计算机可读存储介质,计算机可读存储介质存储有计算机可执行指令,其中计算机可执行指令用于使计算机执行如实施例一所述的方法。
不失一般性,所述计算机可读介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机存储介质包括RAM、ROM、EPROM、EEPROM、闪存或其他固态存储其技术,CD-ROM、DVD或其他光学存储、磁带盒、磁带、磁盘存储或其他磁性存储设备。当然,本领域技术人员可知所述计算机存储介质不局限于上述几种。
本发明通过采集IMU里程计数据作为局部参考系,根据局部参考系获取上一时刻位姿状态向量和上一时刻协方差矩阵,对上一时刻位姿状态向量进行采样,并对采样点进行无迹变换,利用系统模型对经过无迹变换的上一时刻位姿状态向量和上一时刻协方差矩阵进行预测得到当前时刻预测值,结合实际测量值对当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,滤波结束后通过坐标变换根据当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据当前时刻全局位姿估计值进行机器人位姿定位。利用无迹卡尔曼滤波算法结合IMU里程计数据和GPS卫星或视觉系统采集的实际测量值得到全局位姿估计值,对复杂环境具有鲁棒性,提高定位精度。
以上各实施例仅用以说明本发明的技术方案,而非对其限制,尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围,其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (6)

1.一种机器人位姿定位方法,其特征在于,包括:
采集IMU里程计数据作为局部参考系,根据所述局部参考系下的相对位姿和预设滑动窗口内前N个局部参考系相对于全局参考系的相对位姿得到位姿状态向量估计值;根据所述位姿状态向量的估计值得到上一时刻位姿状态向量和上一时刻协方差矩阵;
对所述上一时刻位姿状态向量进行Sigma点采样,并对采样的Sigma点进行无迹卡尔曼滤波变换;
利用系统模型对经过无迹变换的上一时刻位姿状态向量和所述上一时刻协方差矩阵进行预测得到当前时刻预测值,所述当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
结合实际测量值对所述当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,所述实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
滤波结束后通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据所述当前时刻全局位姿估计值进行机器人位姿定位。
2.根据权利要求1所述的一种机器人位姿定位方法,其特征在于,通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值具体包括:根据局部参考系与全局参考系的坐标变换关系,将所述局部参考系下的当前时刻相对位姿测量值转换为所述全局参考系下的当前时刻全局位姿估计值。
3.根据权利要求1所述的一种机器人位姿定位方法,其特征在于,还包括将采集的IMU里程计的初始数据作为局部参考系的初始参考值进行数据初始化。
4.一种机器人位姿定位系统,其特征在于,包括:
获取模块:用于采集IMU里程计数据作为局部参考系,根据所述局部参考系下的相对位姿和预设滑动窗口内前N个局部参考系相对于全局参考系的相对位姿得到位姿状态向量估计值;根据所述位姿状态向量的估计值得到上一时刻位姿状态向量和上一时刻协方差矩阵;
采样模块:对所述上一时刻位姿状态向量进行Sigma点采样,并对采样的Sigma点进行无迹卡尔曼滤波变换;
预测模块:用于利用系统模型对经过无迹变换的上一时刻位姿状态向量和所述上一时刻协方差矩阵进行预测得到当前时刻预测值,所述当前时刻预测值包括:当前时刻的位姿状态向量和当前时刻协方差矩阵;
滤波模块:用于结合实际测量值对所述当前时刻预测值进行滤波处理得到当前时刻相对位姿测量值,所述实际测量值包括通过GPS卫星、视觉系统采集的当前时刻实际相对位姿数据;
位姿定位模块:用于滤波结束后通过坐标变换根据所述当前时刻相对位姿测量值得到当前时刻全局位姿估计值,并根据所述当前时刻全局位姿估计值进行机器人位姿定位。
5.一种机器人位姿定位设备,其特征在于,包括:
至少一个处理器;以及,与所述至少一个处理器通信连接的存储器;
其中,所述处理器通过调用所述存储器中存储的计算机程序,用于执行如权利要求1至3任一项所述的方法。
6.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有计算机可执行指令,所述计算机可执行指令用于使计算机执行如权利要求1至3任一项所述的方法。
CN202010102048.9A 2020-02-19 2020-02-19 机器人位姿定位方法及系统 Active CN111136660B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010102048.9A CN111136660B (zh) 2020-02-19 2020-02-19 机器人位姿定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010102048.9A CN111136660B (zh) 2020-02-19 2020-02-19 机器人位姿定位方法及系统

Publications (2)

Publication Number Publication Date
CN111136660A CN111136660A (zh) 2020-05-12
CN111136660B true CN111136660B (zh) 2021-08-03

Family

ID=70527580

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010102048.9A Active CN111136660B (zh) 2020-02-19 2020-02-19 机器人位姿定位方法及系统

Country Status (1)

Country Link
CN (1) CN111136660B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111949929B (zh) * 2020-08-12 2022-06-21 智能移动机器人(中山)研究院 一种多传感器融合的四足机器人运动里程计设计方法
CN112025706B (zh) * 2020-08-26 2022-01-04 北京市商汤科技开发有限公司 机器人的状态确定方法及装置、机器人及存储介质
CN112230211A (zh) * 2020-10-15 2021-01-15 长城汽车股份有限公司 车辆定位方法、装置、存储介质及车辆
CN112444246B (zh) * 2020-11-06 2024-01-26 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN112815939B (zh) * 2021-01-04 2024-02-23 清华大学深圳国际研究生院 移动机器人的位姿估计方法及计算机可读存储介质
CN112880664A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN112947180B (zh) * 2021-02-04 2022-06-24 中国地质大学(武汉) 重型机械作业状态识别预测方法、装置、设备及存储介质
CN113459104B (zh) * 2021-07-12 2023-09-05 清华大学 基于支持向量回归的机器人定位误差预测方法及预测装置
CN114646311B (zh) * 2022-03-31 2024-08-27 华中科技大学 一种用于大型复杂构件爬壁机器人的全局定位方法
CN114740869A (zh) * 2022-05-17 2022-07-12 山东优宝特智能机器人有限公司 多传感器融合估计与迭代预搜索的机器人避障方法及系统
CN115342813B (zh) * 2022-07-20 2024-06-25 鹏城实验室 一种定位方法、相关装置和存储介质
CN117629204A (zh) * 2022-08-16 2024-03-01 北京三快在线科技有限公司 定位方法和设备
CN115792796B (zh) * 2023-02-13 2023-06-06 鹏城实验室 基于相对观测等效模型的协同定位方法、装置及终端
CN117518196A (zh) * 2023-12-19 2024-02-06 中联重科股份有限公司 一种激光雷达的运动补偿方法、装置、系统、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN103206954A (zh) * 2013-04-25 2013-07-17 佳木斯大学 一种基于ukf的移动机器人多传感器信息融合方法
CN103808316A (zh) * 2012-11-12 2014-05-21 哈尔滨恒誉名翔科技有限公司 室内飞行智能体惯性系统与激光测距仪组合导航改进方法
CN108362288A (zh) * 2018-02-08 2018-08-03 北方工业大学 一种基于无迹卡尔曼滤波的偏振光slam方法
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20140070722A (ko) * 2012-11-26 2014-06-11 한국전자통신연구원 다중속도시스템 결합 장치 및 운용 방법

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN103808316A (zh) * 2012-11-12 2014-05-21 哈尔滨恒誉名翔科技有限公司 室内飞行智能体惯性系统与激光测距仪组合导航改进方法
CN103206954A (zh) * 2013-04-25 2013-07-17 佳木斯大学 一种基于ukf的移动机器人多传感器信息融合方法
CN108362288A (zh) * 2018-02-08 2018-08-03 北方工业大学 一种基于无迹卡尔曼滤波的偏振光slam方法
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于三维激光的图优化SLAM策略;张天喜等;《激光与光电子学进展》;20191008;1-12 *

Also Published As

Publication number Publication date
CN111136660A (zh) 2020-05-12

Similar Documents

Publication Publication Date Title
CN111136660B (zh) 机器人位姿定位方法及系统
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN112965063B (zh) 一种机器人建图定位方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN108645415A (zh) 一种船舶航迹预测方法
CN103776449B (zh) 一种提高鲁棒性的动基座初始对准方法
CN110763239A (zh) 滤波组合激光slam建图方法及装置
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
CN113538699A (zh) 基于三维点云的定位方法、装置、设备及存储介质
CN114638419A (zh) 一种塔吊物体轨迹预测的方法及设备
CN115388899A (zh) 基于变分贝叶斯的移动机器人视觉惯性融合slam方法
CN115711616A (zh) 一种室内室外穿越无人机的平顺定位方法及装置
CN114689047A (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN113340324B (zh) 一种基于深度确定性策略梯度的视觉惯性自校准方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN107843259B (zh) 一种基于vckf的多机器人协同导航定位方法
CN111812668B (zh) 绕机检查装置及其定位方法、存储介质
CN111553954B (zh) 一种基于直接法单目slam的在线光度标定方法
CN117268381A (zh) 一种航天器状态的判断方法
CN115930971B (zh) 一种机器人定位与建图的数据融合处理方法
CN109655057B (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