CN113280808A - 一种移动机器人定位精度提升方法及系统 - Google Patents

一种移动机器人定位精度提升方法及系统 Download PDF

Info

Publication number
CN113280808A
CN113280808A CN202110571321.7A CN202110571321A CN113280808A CN 113280808 A CN113280808 A CN 113280808A CN 202110571321 A CN202110571321 A CN 202110571321A CN 113280808 A CN113280808 A CN 113280808A
Authority
CN
China
Prior art keywords
pose
robot
odometer
motion model
mobile robot
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
Application number
CN202110571321.7A
Other languages
English (en)
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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
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 University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202110571321.7A priority Critical patent/CN113280808A/zh
Publication of CN113280808A publication Critical patent/CN113280808A/zh
Pending legal-status Critical Current

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种移动机器人定位精度提升方法及系统。该方法包括：建立里程计运动模型；对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；获取激光传感器采集的机器人的位姿，记为第二位姿；确定在线估算噪声的统计特性；将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。本发明具有计算简单、使用方便、适合复杂环境且精确度高的特点。

Description

一种移动机器人定位精度提升方法及系统
技术领域
本发明涉及机器人定位技术领域，特别是涉及一种移动机器人定位精度提升方法及系统。
背景技术
随着工作环境复杂度的提高，只使用基于激光传感器的绝对定位将无法满足移动机器人的定位需求，即单传感器机器人定位不能满足移动机器人在复杂施工环境下的定位需求。本发明将使用组合定位进行移动机器人在复杂施工环境下的定位，利用多传感器进行数据融合，提升移动机器人的定位精度，从而使移动机器人完成精准作业。其中，融合定位算法是多传感器信息融合定位的关键技术与核心。
卡尔曼滤波算法是目前使用最广泛之一的融合算法，其在线性系统中应用可以得到较好的效果。且在使用卡尔曼滤波算法作为定位融合算法时，需要事先知道噪声的统计特性，一般的处理方法为假设噪声矩阵满足某种分布，其对于定位的精确度有一定的影响。
因此，如何提供一种计算简单、使用方便、适合复杂环境且精确度高的移动机器人定位方法，是本领域技术人员亟待解决的技术问题。
发明内容
本发明的目的是提供一种计算简单、使用方便、适合复杂环境且精确度高的移动机器人定位精度提升方法及系统。
为实现上述目的，本发明提供了如下方案：
一种移动机器人定位精度提升方法包括：
建立里程计运动模型；
对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；
根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；
获取激光传感器采集的机器人的位姿，记为第二位姿；
确定在线估算噪声的统计特性；
将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。
可选地，所述建立里程计运动模型，具体包括：
建立里程计运动模型：
Figure BDA0003082796210000021
其中，
Figure BDA0003082796210000022
Xk+1表示k+1时刻系统的里程计的状态变量，
Figure BDA0003082796210000023
为机器人在全局坐标系下的初始位姿，
Figure BDA0003082796210000024
为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
可选地，所述对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型，具体包括：
采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
可选地，所述确定在线估算噪声的统计特性，具体包括：
根据渐消记忆指数加权法采用公式
Figure BDA0003082796210000031
确定k时刻的指数权值；
根据指数权值采用公式
Figure BDA0003082796210000032
和
Figure BDA0003082796210000033
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure BDA0003082796210000034
为
Figure BDA0003082796210000035
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure BDA0003082796210000036
为实时估计的过程噪声均值，
Figure BDA0003082796210000037
为实时估计的过程噪声方差
Figure BDA0003082796210000038
Figure BDA0003082796210000039
为实时估计的观测噪声均值
Figure BDA00030827962100000310
Figure BDA00030827962100000311
为实时估计的观测噪声方差
Figure BDA00030827962100000312
εk,τk,i,μk,i分别为中间变量，
Figure BDA00030827962100000313
为sigam对应的权值。
可选地，所述将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿，具体包括：
将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure BDA00030827962100000314
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure BDA00030827962100000315
为k时刻机器人的位姿，
Figure BDA00030827962100000316
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure BDA00030827962100000317
为观测预测值。
一种移动机器人定位精度提升系统包括：
里程计运动模型建立模块，用于建立里程计运动模型；
里程计运动模型降噪模块，用于对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；
第一位姿确定模块，用于根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；
第二位姿确定模块，用于获取激光传感器采集的机器人的位姿，记为第二位姿；
统计特性确定模块，用于确定在线估算噪声的统计特性；
位姿融合模块，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。
可选地，所述里程计运动模型建立模块具体包括：
里程计运动模型建立单元，用于建立里程计运动模型：
Figure BDA0003082796210000041
其中，
Figure BDA0003082796210000042
Xk+1表示k+1时刻系统的里程计的状态变量，
Figure BDA0003082796210000043
为机器人在全局坐标系下的初始位姿，
Figure BDA0003082796210000044
为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
可选地，所述里程计运动模型降噪模块具体包括：
里程计运动模型降噪单元，用于采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
可选地，所述统计特性确定模块具体包括：
指数权值确定单元，用于根据渐消记忆指数加权法采用公式
Figure BDA0003082796210000051
确定k时刻的指数权值；
统计特性确定单元，用于根据指数权值采用公式
Figure BDA0003082796210000052
和
Figure BDA0003082796210000053
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure BDA0003082796210000054
为
Figure BDA0003082796210000055
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure BDA0003082796210000056
为实时估计的过程噪声均值，
Figure BDA0003082796210000057
为实时估计的过程噪声方差
Figure BDA0003082796210000058
Figure BDA0003082796210000059
为实时估计的观测噪声均值
Figure BDA00030827962100000510
Figure BDA00030827962100000511
为实时估计的观测噪声方差
Figure BDA00030827962100000512
εk,τk,i,μk,i分别为中间变量，
Figure BDA00030827962100000513
为sigam对应的权值。
可选地，所述位姿融合模块具体包括：
位姿融合单元，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure BDA00030827962100000514
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure BDA0003082796210000061
为k时刻机器人的位姿，
Figure BDA0003082796210000062
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure BDA0003082796210000063
为观测预测值。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明基于扩展卡尔曼滤波的移动机器人定位精度提升方法，利用多传感器进行数据融合，将激光传感器和里程计进行融合，帮助移动机器人在复杂环境下进行较高精度的定位，有助于施工、仓储物流等领域的可移动式机器人实现更准确的定位，以更高的精度完成作业。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明移动机器人定位精度提升方法流程图；
图2为本里程计模型坐标系示意图；
图3为本发明的基于扩展卡尔曼滤波定位方法的中卡尔曼滤波算法流程图；
图4为本发明移动机器人定位精度提升系统结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种计算简单、使用方便、适合复杂环境且精确度高的移动机器人定位方法及系统，帮助移动机器人在复杂环境下进行较高精度的定位，有助于施工、仓储物流等领域的可移动式机器人实现更准确的定位，以更高的精度完成作业。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
在本发明中，提出使用无迹卡尔曼滤波算法融合里程计和激光传感器信息，提高移动机器人的定位精度和稳定性的解决办法。基于卡尔曼滤波和无迹卡尔曼滤波算法融合多传感器信息时，需要事先知道噪声的统计特性，考虑到环境噪声的可变性，本发明还提出了基于极大后验估计和指数加权的自适应无迹卡尔曼滤波定位算法。图1为本发明移动机器人定位精度提升方法流程图。如图1所示，一种移动机器人定位精度提升方法包括：
步骤101：建立里程计运动模型。
如图2所示，建立全局坐标系{XG,OG,YG}和移动机器人坐标系{XR,OR,YR}，假设机器人初始位置已知，即机器人在全局坐标系下的坐标已知。所述里程计运动模型是指系统采集移动机器人的轮子的速度，以计算相邻时刻的机器人的移动距离和方位角的变化，进而推算移动机器人的位姿的模型。
全局坐标系为固定坐标系，在机器人移动过程中可用于表示机器人的实时位置，机器人坐标系建立在移动机器人上，本实例采取建立在小车底盘的中心位点上，为移动坐标系，跟随机器人移动。移动机器人按照底层控制模型主要可以分为两种：差速模型和全向模型，其中，差速模型适用于双轮差速移动机器人，全向模型适用于全向移动机器人，本实例中以全向移动机器人作为本发明的实例对象。
假设机器人初始位置已知，即机器人在全局坐标系下的坐标已知。本实例的里程计模型采取增量式编码器进行推算。在机器人初始位姿已知的情况下，系统采集移动机器人的轮子(本实例中采取麦克纳姆轮作为实例中移动机器人的轮子)的速度，以计算相邻时刻的机器人的移动距离和方位角的变化，进而推算移动机器人的位姿。机器人航位推算模型的选取决定了机器人的运动方程，是里程计运动模型建立的主要依据。
假设移动机器人在全局坐标系下的初始位姿为
Figure BDA0003082796210000071
经过Δt时间后到达位姿
Figure BDA0003082796210000072
Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻移动机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻移动机器人在机器人坐标系Y方向上位移增量，则可以得到里程计运动模型：
Figure BDA0003082796210000081
其中，
Figure BDA0003082796210000082
Figure BDA0003082796210000083
其中，Xk+1表示——k+1时刻系统的里程计的状态变量，
Figure BDA0003082796210000084
为机器人在全局坐标系下的初始位姿，
Figure BDA0003082796210000085
为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，此实例中为公式，其中ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
步骤102：对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型，具体包括：
采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
基于里程计的航位推算可以计算机器人的位姿，其优点在于不依赖外部传感器就可以知道机器人的位姿，但是使用里程计的定位存在误差，不适合远距离定位，且里程计所得的数据含有一定的噪声，本实例中的噪声定义为高斯白噪声，其去除噪声的过程为采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型。
步骤103：根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿。
使用里程计运动模型的状态变量所搭建的卡尔曼滤波的状态方程表示为：
Xk+1＝FXk+Wk (4)
Zk+1＝HXk+1+Vk+1 (5)
其中，Xk+1表示k时刻里程计的状态变量，观测变量Zk是基于里程计观测得到k时刻的机器人位姿，Wk代表过程噪声，Vk+1代表观测噪声，F、H分别表示状态变换矩阵和观测矩阵，H一般取单位矩阵。
其中里程计运动模型的过程噪声为高斯白噪声，当Wk和Vk+1为高斯白噪声时，其统计特性为：
Figure BDA0003082796210000091
此处q和r分别代表过程噪声和观测噪声均值，由于噪声为高斯白噪声，故q和r皆为0，Q和R分别表示其方差。
由公式4，公式5和公式6便可得到由里程计得到的较为可信的机器人的位姿，以用于所述步骤4的与激光传感器所得位姿的融合过程。
步骤104：获取激光传感器采集的机器人的位姿，记为第二位姿。
步骤105：确定在线估算噪声的统计特性，具体包括：
根据渐消记忆指数加权法采用公式
Figure BDA0003082796210000092
确定k时刻的指数权值；
根据指数权值采用公式
Figure BDA0003082796210000101
Figure BDA0003082796210000102
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure BDA0003082796210000103
为
Figure BDA0003082796210000104
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure BDA0003082796210000105
为实时估计的过程噪声均值，
Figure BDA0003082796210000106
为实时估计的过程噪声方差
Figure BDA0003082796210000107
Figure BDA0003082796210000108
为实时估计的观测噪声均值
Figure BDA0003082796210000109
Figure BDA00030827962100001010
为实时估计的观测噪声方差
Figure BDA00030827962100001011
εk,τk,i,μk,i分别为中间变量，
Figure BDA00030827962100001012
为sigam对应的权值。
在将里程计数据和激光数据融合过程中，将由里程计模型所得的位姿(位置和姿态)作为预测值，将激光传感器所得的绝对位姿作为观测值。由于使用扩展卡尔曼滤波算法融合传感器的数据时，需要事先知道噪声的统计特性，本示例中的实时噪声估计算法如下：
采用渐消记忆指数加权法。k时刻的指数权值dk满足下面的公式：
Figure BDA00030827962100001013
其中，b为遗忘因子，并且0<b<1。
实时估计的过程噪声均值
Figure BDA00030827962100001014
和过程噪声方差
Figure BDA00030827962100001015
可以通过下面的式子计算：
Figure BDA00030827962100001016
实时估计的观测噪声均值
Figure BDA0003082796210000111
和观测噪声方差
Figure BDA0003082796210000112
可以通过下面的式子计算得到：
Figure BDA0003082796210000113
通过上述公式即可实现实时估计噪声特性，包括过程噪声均值
Figure BDA0003082796210000114
过程噪声方差
Figure BDA0003082796210000115
观测噪声均值
Figure BDA0003082796210000116
和观测噪声方差
Figure BDA0003082796210000117
步骤106：将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿，具体包括：
将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure BDA0003082796210000118
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure BDA0003082796210000119
为k时刻机器人的位姿，
Figure BDA00030827962100001110
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure BDA00030827962100001111
为观测预测值。
所述传感器数据的融合算法，是本发明的关键。基于里程计的航位推算可以计算机器人的位姿，其优点在于不依赖外部传感器就可以知道机器人的位姿，但是使用里程计的定位存在误差，不适合远距离定位。本发明所述的基于扩展卡尔曼滤波的定位精度提升方法为使用扩展卡尔曼滤波融合里程计和激光传感器数据，并使用自适应噪声估算方法去除融合过程中的噪声，提高移动机器人的定位精度和可靠性。
图3为本发明的基于扩展卡尔曼滤波定位方法的中卡尔曼滤波算法流程图。本发明所述的传感器融合算法为扩展卡尔曼滤波算法，并将由里程计模型所得的位姿(位置和姿态)作为预测值，将激光传感器所得的绝对位姿作为观测值。使用扩展卡尔曼滤波算法融合传感器的数据时，本算法中的状态方程可表示为：
Xk+1＝FXk+BUk+Wk (10)
Zk+1＝HXk+1+Vk+1 (11)
其中，Xk+1表示k时刻系统的状态变量，观测变量Zk是基于激光传感器得到的机器人位姿，Wk代表过程噪声，Vk+1代表观测噪声，控制量Uk表示里程计的输入；F、B、H分别表示状态变换矩阵、控制矩阵和观测矩阵。
根据步骤105在线估算噪声得到噪声的统计特性的方法，确定Wk和Vk+1为统计特性为：
Figure BDA0003082796210000121
Figure BDA0003082796210000122
本实例中里程计噪声为高斯白噪声时，其统计特性为：
p(Wk)～N(0，Q) (14)
p(Vk)～N(0，R) (15)
其中，Q和R分别代表过程噪声和观测噪声的方差。
将噪声的统计特性带入本发明的状态方程可得如下形式：
Xk+1＝f(Xk，Uk)+q+μk (16)
Zk+1＝h(Xk+1)+r+ηk+1 (17)
其中：
Figure BDA0003082796210000123
由于本系统为非线性系统，扩展卡尔曼滤波需要引入无迹变换。无迹变换根据状态变量Xk的维度L计算(2L+1)个sigma点，并将得到的sigma点进行非线性变换，计算出相应的均值与协方差。状态变量Xk为全局坐标系下移动机器人的位姿，根据其维度L＝3，将取7个sigma点ηk,i(i＝0，…，6)，这些sigma点均值为
Figure BDA0003082796210000131
协方差为Pk。sigma点与其对应权值的计算过程为：
Figure BDA0003082796210000132
其中ξ＝ψ2(L+κ)-L，是一个尺度参数；κ也是一个尺度参数，通常被设为0，ψ用于控制sigma点的分散程度，通常取一个比较小的正值。
sigma点对应的权值计算如下：
Figure BDA0003082796210000133
其中，ν表示采样得到的sigma点的分布情况，当sigma点服从正态分布时，则ν＝2。
为了方便后续计算，可将无迹变换过程简化为如下：
Figure BDA0003082796210000134
传感器融合过程中所使用的扩展卡尔曼滤波算法可以分为预测阶段和更新阶段，预测阶段又分为两个步骤，系统启动时，基于激光获得的位姿(激光传感器所得的)作为移动机器人的初始位姿，初始协方差矩阵P0为零矩阵。
预测阶段的第一步是通过k时刻机器人位姿
Figure BDA0003082796210000135
和协方差Pk计算k+1时刻先验预测值
Figure BDA0003082796210000136
和先验协方差Pk+1|k，过程如下：
Figure BDA0003082796210000137
在本发明中，一般取ψ＝1，ν＝1.2，κ＝0.98，L＝3。
预测阶段的第二步是通过上述公式计算出来的
Figure BDA0003082796210000138
和Pk+1|k计算观测预测值
Figure BDA0003082796210000141
互协方差矩阵
Figure BDA0003082796210000142
和观测变量的协方差矩阵
Figure BDA0003082796210000143
过程如下：
Figure BDA0003082796210000144
预测阶段完成后，进行更新阶段过程，即可得到k+1时刻机器人的位姿
Figure BDA0003082796210000145
协方差Pk+1和增益矩阵Kk+1。
Figure BDA0003082796210000146
Figure BDA0003082796210000147
Figure BDA0003082796210000148
其中，观测值Zk+1是基于激光传感器计算出的位姿，
Figure BDA0003082796210000149
为本发明得出k+1时刻移动机器人的准确的位姿。
图4为本发明移动机器人定位精度提升系统结构图。如图4所示，一种移动机器人定位精度提升系统包括：
里程计运动模型建立模块201，用于建立里程计运动模型；
里程计运动模型降噪模块202，用于对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；
第一位姿确定模块203，用于根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；
第二位姿确定模块204，用于获取激光传感器采集的机器人的位姿，记为第二位姿；
统计特性确定模块205，用于确定在线估算噪声的统计特性；
位姿融合模块206，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。
所述里程计运动模型建立模块201具体包括：
里程计运动模型建立单元，用于建立里程计运动模型：
Figure BDA0003082796210000151
其中，
Figure BDA0003082796210000152
Xk+1表示k+1时刻系统的里程计的状态变量，
Figure BDA0003082796210000153
为机器人在全局坐标系下的初始位姿，
Figure BDA0003082796210000154
为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
所述里程计运动模型降噪模块202具体包括：
里程计运动模型降噪单元，用于采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
所述统计特性确定模块205具体包括：
指数权值确定单元，用于根据渐消记忆指数加权法采用公式
Figure BDA0003082796210000155
确定k时刻的指数权值；
统计特性确定单元，用于根据指数权值采用公式
Figure BDA0003082796210000161
和
Figure BDA0003082796210000162
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure BDA0003082796210000163
为
Figure BDA0003082796210000164
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure BDA0003082796210000165
为实时估计的过程噪声均值，
Figure BDA0003082796210000166
为实时估计的过程噪声方差
Figure BDA0003082796210000167
Figure BDA0003082796210000168
为实时估计的观测噪声均值
Figure BDA0003082796210000169
Figure BDA00030827962100001610
为实时估计的观测噪声方差
Figure BDA00030827962100001611
εk,τk,i,μk,i分别为中间变量，
Figure BDA00030827962100001612
为sigam对应的权值。
所述位姿融合模块206具体包括：
位姿融合单元，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure BDA00030827962100001613
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure BDA00030827962100001614
为k时刻机器人的位姿，
Figure BDA00030827962100001615
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure BDA00030827962100001616
为观测预测值。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种移动机器人定位精度提升方法，其特征在于，包括：
建立里程计运动模型；
对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；
根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；
获取激光传感器采集的机器人的位姿，记为第二位姿；
确定在线估算噪声的统计特性；
将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。
2.根据权利要求1所述的移动机器人定位精度提升方法，其特征在于，所述建立里程计运动模型，具体包括：
建立里程计运动模型：
Figure FDA0003082796200000011
其中，
Figure FDA0003082796200000012
Xk+1表示k+1时刻系统的里程计的状态变量，[Gxk，Gyk，Gθk]为机器人在全局坐标系下的初始位姿，[Gxk+1，Gyk+1,Gθk+1]为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
3.根据权利要求1所述的移动机器人定位精度提升方法，其特征在于，所述对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型，具体包括：
采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
4.根据权利要求1所述的移动机器人定位精度提升方法，其特征在于，所述确定在线估算噪声的统计特性，具体包括：
根据渐消记忆指数加权法采用公式
Figure FDA0003082796200000021
确定k时刻的指数权值；
根据指数权值采用公式
Figure FDA0003082796200000022
和
Figure FDA0003082796200000023
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure FDA0003082796200000024
为
Figure FDA0003082796200000025
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure FDA0003082796200000026
为实时估计的过程噪声均值，
Figure FDA0003082796200000027
为实时估计的过程噪声方差
Figure FDA0003082796200000028
Figure FDA0003082796200000029
为实时估计的观测噪声均值
Figure FDA00030827962000000210
Figure FDA00030827962000000211
为实时估计的观测噪声方差
Figure FDA00030827962000000212
εk,τk,i,μk,i分别为中间变量，
Figure FDA00030827962000000213
为sigam对应的权值。
5.根据权利要求1所述的移动机器人定位精度提升方法，其特征在于，所述将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿，具体包括：
将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure FDA0003082796200000031
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure FDA0003082796200000032
为k时刻机器人的位姿，
Figure FDA0003082796200000033
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure FDA0003082796200000034
为观测预测值。
6.一种移动机器人定位精度提升系统，其特征在于，包括：
里程计运动模型建立模块，用于建立里程计运动模型；
里程计运动模型降噪模块，用于对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型；
第一位姿确定模块，用于根据所述降噪后的里程计运动模型，确定机器人的位姿，记为第一位姿；
第二位姿确定模块，用于获取激光传感器采集的机器人的位姿，记为第二位姿；
统计特性确定模块，用于确定在线估算噪声的统计特性；
位姿融合模块，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿。
7.根据权利要求6所述的移动机器人定位精度提升系统，其特征在于，所述里程计运动模型建立模块具体包括：
里程计运动模型建立单元，用于建立里程计运动模型：
Figure FDA0003082796200000035
其中，
Figure FDA0003082796200000036
Xk+1表示k+1时刻系统的里程计的状态变量，[Gxk,Gyk,Gθk]为机器人在全局坐标系下的初始位姿，[Gxk+1,Gyk+1,Gθk+1]为机器人经过Δt时间后到达的位姿，Δθ表示相邻时刻移动机器人方位角的变化量，Δx表示相邻时刻机器人在机器人坐标系X方向上位移增量，Δy表示相邻时刻机器人在机器人坐标系Y方向上位移增量，Vx、Vy和ω由移动机器人底轮运动模型计算得到，ω1，ω2，ω3，ω4分别代表四个底轮的角速度，Δt为编码器的采样周期。
8.根据权利要求6所述的移动机器人定位精度提升系统，其特征在于，所述里程计运动模型降噪模块具体包括：
里程计运动模型降噪单元，用于采用卡尔曼滤波器对所述里程计运动模型进行降噪，得到降噪后的里程计运动模型：
Xk+1＝FXk+Wk；
其中，Xk+1表示k时刻里程计的状态变量，Xk表示k-1时刻里程计的状态变量，Wk代表过程噪声，F表示状态变换矩阵。
9.根据权利要求6所述的移动机器人定位精度提升系统，其特征在于，所述统计特性确定模块具体包括：
指数权值确定单元，用于根据渐消记忆指数加权法采用公式
Figure FDA0003082796200000041
确定k时刻的指数权值；
统计特性确定单元，用于根据指数权值采用公式
Figure FDA0003082796200000042
和
Figure FDA0003082796200000043
确定在线估算噪声的统计特性；
其中，b为遗忘因子，并且0<b<1，dk为k时刻的指数权值，Zk是基于人工路标定位得到的机器人位姿，
Figure FDA0003082796200000051
为
Figure FDA0003082796200000052
和Pk|k-1计算所得的观测预测值，ηk-1,i和μk-1,i为sigma点的特性参数，Uk-1表示里程计的输入，
Figure FDA0003082796200000053
为实时估计的过程噪声均值，
Figure FDA0003082796200000054
为实时估计的过程噪声方差
Figure FDA0003082796200000055
Figure FDA0003082796200000056
为实时估计的观测噪声均值
Figure FDA0003082796200000057
Figure FDA0003082796200000058
为实时估计的观测噪声方差
Figure FDA0003082796200000059
εk,τk,i,μk,i分别为中间变量，
Figure FDA00030827962000000510
为sigam对应的权值。
10.根据权利要求6所述的移动机器人定位精度提升系统，其特征在于，所述位姿融合模块具体包括：
位姿融合单元，用于将所述第一位姿、所述第二位姿和所述统计特性采用扩展卡尔曼滤波方法融合，得到移动机器人的融合位姿：
Figure FDA00030827962000000511
其中，Zk+1是基于激光传感器计算出的k+1时刻的位姿，即第二位姿，
Figure FDA00030827962000000512
为k时刻机器人的位姿，
Figure FDA00030827962000000513
为k+1时刻移动机器人的准确的位姿，即移动机器人的融合位姿，Kk+1为增益矩阵，
Figure FDA00030827962000000514
为观测预测值。
CN202110571321.7A 2021-05-25 2021-05-25 一种移动机器人定位精度提升方法及系统 Pending CN113280808A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110571321.7A CN113280808A (zh) 2021-05-25 2021-05-25 一种移动机器人定位精度提升方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110571321.7A CN113280808A (zh) 2021-05-25 2021-05-25 一种移动机器人定位精度提升方法及系统

Publications (1)

Publication Number Publication Date
CN113280808A true CN113280808A (zh) 2021-08-20

Family

ID=77281580

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110571321.7A Pending CN113280808A (zh) 2021-05-25 2021-05-25 一种移动机器人定位精度提升方法及系统

Country Status (1)

Country Link
CN (1) CN113280808A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984064A (zh) * 2021-10-27 2022-01-28 山东亚历山大智能科技有限公司 一种用于室内移动机器人的定位方法及系统
CN115235453A (zh) * 2022-08-01 2022-10-25 湖北工业大学 一种移动机器人定位方法及系统
CN116026335A (zh) * 2022-12-26 2023-04-28 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统
CN116659487A (zh) * 2022-11-02 2023-08-29 丰疆智能(深圳)有限公司 位姿调整方法、位姿调整装置、电子设备及可读存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN109974714A (zh) * 2019-04-29 2019-07-05 南京航空航天大学 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110543727A (zh) * 2019-09-05 2019-12-06 北京工业大学 一种基于改进粒子群算法的全向移动智能轮椅机器人参数辨识方法
CN111308490A (zh) * 2020-02-05 2020-06-19 浙江工业大学 基于单线激光雷达的平衡车室内定位与导航系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN109974714A (zh) * 2019-04-29 2019-07-05 南京航空航天大学 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110543727A (zh) * 2019-09-05 2019-12-06 北京工业大学 一种基于改进粒子群算法的全向移动智能轮椅机器人参数辨识方法
CN111308490A (zh) * 2020-02-05 2020-06-19 浙江工业大学 基于单线激光雷达的平衡车室内定位与导航系统

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
孙玉山等: "改进的自适应Kalman滤波方法及其在AUV组合导航中的应用", 《高技术通讯》 *
常皓等: "基于全向移动模型的Gmapping算法", 《计量与测试技术》 *
张立川等: "《深远海工程装备与高技术丛书 自主水下航行器导航与控制技术》", 31 October 2020, 上海科学技术出版社 *
王大轶等: "《航天器多源信息融合自主导航技术》", 31 March 2018, 北京理工大学出版社 *
班朝等: "基于IMU的机器人姿态自适应EKF测量算法研究", 《仪器仪表学报》 *
范彬彬等: "融合惯性导航与激光的服务机器人自定位研究", 《网络新媒体技术》 *
谭永丽等: "基于路标的轮式机器人定位", 《湖北第二师范学院学报》 *
黄加俊等: "基于激光SLAM的全方位移动机器人控制系统设计", 《科技通报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984064A (zh) * 2021-10-27 2022-01-28 山东亚历山大智能科技有限公司 一种用于室内移动机器人的定位方法及系统
CN115235453A (zh) * 2022-08-01 2022-10-25 湖北工业大学 一种移动机器人定位方法及系统
CN116659487A (zh) * 2022-11-02 2023-08-29 丰疆智能(深圳)有限公司 位姿调整方法、位姿调整装置、电子设备及可读存储介质
CN116659487B (zh) * 2022-11-02 2024-07-12 丰疆智能(深圳)有限公司 位姿调整方法、位姿调整装置、电子设备及可读存储介质
CN116026335A (zh) * 2022-12-26 2023-04-28 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统
CN116026335B (zh) * 2022-12-26 2023-10-03 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统

Similar Documents

Publication Publication Date Title
CN113280808A (zh) 一种移动机器人定位精度提升方法及系统
CN109885883B (zh) 一种基于gk聚类算法模型预测的无人车横向运动的控制方法
CN113311411B (zh) 一种用于移动机器人的激光雷达点云运动畸变校正方法
CN104246825B (zh) 用于车辆摄像机的在线校准的方法和装置
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN111238535A (zh) 一种基于因子图的imu误差在线标定方法
CN110542429A (zh) 一种全向移动机器人误差补偿方法
CN110865403B (zh) 一种基于神经网络预学习和轮式里程计融合的定位方法
CN111189454A (zh) 基于秩卡尔曼滤波的无人车slam导航方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
CN111399021A (zh) 一种导航定位方法
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
Da Lio et al. Robust and sample-efficient estimation of vehicle lateral velocity using neural networks with explainable structure informed by kinematic principles
CN108709560A (zh) 基于直线段特征的载运机器人高精度定位导航方法
CN113432612A (zh) 飞行体的导航方法、装置及系统
CN114516050B (zh) 基于位形雅可比条件数优化的机器人外力估计方法及系统
CN112577512A (zh) 一种基于轮速融合的状态量误差确定方法及车载终端
CN113701755B (zh) 一种无高精度陀螺的光学遥感卫星姿态确定方法
Zarei et al. Performance improvement for mobile robot position determination using cubature Kalman filter
CN115420291A (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