CN107843259A - 一种基于vckf的多机器人协同导航定位方法 - Google Patents

一种基于vckf的多机器人协同导航定位方法 Download PDF

Info

Publication number
CN107843259A
CN107843259A CN201711021203.9A CN201711021203A CN107843259A CN 107843259 A CN107843259 A CN 107843259A CN 201711021203 A CN201711021203 A CN 201711021203A CN 107843259 A CN107843259 A CN 107843259A
Authority
CN
China
Prior art keywords
mobile robot
multiple mobile
collaborative navigation
noise
vckf
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
Application number
CN201711021203.9A
Other languages
English (en)
Other versions
CN107843259B (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201711021203.9A priority Critical patent/CN107843259B/zh
Publication of CN107843259A publication Critical patent/CN107843259A/zh
Application granted granted Critical
Publication of CN107843259B publication Critical patent/CN107843259B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Automation & Control Theory (AREA)
  • Operations Research (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

一种基于VCKF的多移动机器人协同导航定位方法，包括如下步骤：根据多移动机器人工作环境，确定协同导航系统的初始值；建立多移动机器人协同导航系统的非线性系统方程；对多移动机器人协同导航系统按照CKF滤波框架进行时间更新；多移动机器人对工作环境中的固定路标点以及其它机器人进行实时观测，获取相对距离和方位角作为观测信息；利用观测到的量测信息和系统方程，利用VCKF算法完成多移动机器人协同导航系统的量测更新；对多移动机器人的位姿信息进行更新；完成多移动机器人的高精度协同导航定位。本发明的运用了基于VCE的非线性滤波器CKF，可以实时估计出系统的过程噪声和量测噪声方差阵，有效解决的非线性问题，提高系统的定位精度和适应性。

Description

一种基于VCKF的多机器人协同导航定位方法
技术领域
本发明涉及机器人导航定位领域，具体是一种基于VCKF的多机器人协同导航定位方法。
背景技术
由于多移动机器人具有单机器人所不具有的诸多优势，如执行任务更加复杂、工作效率更高等，多移动机器人逐渐成为机器人领域的一个研究热点。然而机器人协同导航是其安全、高效的完成任务的前提和保障，因此需要一种高精度多移动机器人协同导航算法。
由于实际系统具有非线性特性，因此在进行多移动机器人协同导航时需要利用非线性滤波器。CKF是将非线性变换作用于具有相同权值的2n个点，从而计算出系统近似的高斯分布，完成滤波过程。与传统的扩展卡尔曼滤波和无迹卡尔曼滤波相比，CKF不仅具有更高的滤波精度，而且对高维系统具有更好的稳定性。因此，CKF逐渐得到广泛的关注。然而CKF只有在系统的数学模型精确已知且外部干扰噪声(包括系统噪声和观测噪声)为零均值、白噪声时，滤波算法的结果才是最优的。这在实际的多移动机器人系统中，由于外部环境的不确定性以及自身元器件的误差漂移等因素的影响，很难得到精确的系统模型或保证系统噪声的特性，因此得到的滤波结果会存在加大的误差，严重的甚至会导致滤波发散。
为了正确估计系统中的未知参数，观测值的权必须准确给出，由此产生了方差分量估计(Variance Component Estimation，VCE)算法。VCE算法在统计学和大地测量学文献中已经被广泛的研究，其中应用比较广泛的是基于Helmert的VCE算法。本发明就应用VCE对CKF进行改进，提出一种改进的协同导航定位方法，可以同时解决实际多移动机器人系统中的非线性和不确定性问题，从而提高多移动机器人协同导航定位精度。
发明内容
本发明的目的是提供一种精度更高的基于VCKF的多移动机器人协同导航定位方法。
本发明的目的是通过以下步骤来实现的：
步骤1：根据多移动机器人工作环境，确定协同导航系统的初始值，包括多移动机器人的初始位姿和路标点的位置；
步骤2：建立多移动机器人协同导航系统的非线性系统方程；
步骤3：对多移动机器人协同导航系统按照CKF滤波框架进行时间更新；
步骤4：多移动机器人对工作环境中的固定路标点以及其它机器人进行实时观测，获取相对距离和方位角作为观测信息；
步骤5：利用观测到的量测信息和系统方程，利用VCKF算法完成多移动机器人协同导航系统的量测更新；
步骤6：对多移动机器人的位姿信息进行更新；
步骤7：重复步骤3～步骤6，完成多移动机器人的高精度协同导航定位。
进一步地，在步骤5中，利用改进的VCKF算法完成协同导航系统的量测更新过程包括以下步骤：
首先利用CKF框架完成协同导航系统的量测更新，估计出多机器人的位姿信息；
利用VCE方法对系统的过程噪声和量测噪声进行估计：1)在多移动机器人非线性系统时间更新的基础上建立包含系统过程噪声和观测噪声在内的三组伪观测向量；2)根据残差理论计算出伪观测向量的残差方程以及残差向量的方差矩阵；3)计算出对应伪观测向量的多余观测分量以及过程噪声方差分量和量测噪声方差分量的多余观测分量；4)根据Herlmet方差分量估计方法计算出系统的过程噪声方差阵和量测噪声方差阵，完成系统噪声的实时估计。
本发明的优势在于：(1)由于运用了非线性滤波器CKF，可以有效解决多移动机器人协同导航系统中的非线性问题，提高系统的定位精度；(2)利用VCE原理，实时估计出系统的过程噪声和量测噪声方差阵，不需要精确已知系统噪声的先验知识，进一步提高协同导航定位的精度和适应性。
附图说明
图1为本发明的算法流程图；
图2为使用本发明所计算的归一化估计误差平方的曲线；
图3为使用传统协同导航算法所计算的归一化估计误差平方的曲线。
具体实施方式
以下结合具体实施案例，对本发明进行详细说明。
本发明是一种基于CKF和VCE方法的多移动机器人协同导航定位算法，结合图1所示的算法流程框图，其具体实施方式为：
步骤1：首先根据多移动机器人的工作任务，搭建合理的工作环境，确定多移动机器人周围环境中的固定路标点，并测量出各个固定路标点在工作环境中的位置以及各个移动机器人的初始位置和姿态等信息；
步骤2：构建包括多机器人位置和姿态在内的状态向量：
X＝[X1 X2 … Xn]T
其中Xi＝[xi,yi,θi]T表示第i个机器人的位姿，根据机器人运动学方程可以表示为：
构建包括距离和方位角度的量测向量Z＝[ZRR ZRL]T，包括机器人与机器人之间的相互观测ZRR和机器人与固定路标点之间的观测ZRL，则ZRR和ZRL可表示为：
其中[xi,yi,θi]T和[xj,yj,θj]T是第i个机器人和第j个机器人的位姿，(xl,yl)是路标点l的位置。
在此基础上，综合考虑多移动机器人协同导航的非线性和不确定性因素，根据移动机器人动力学原理，建立多移动机器人的非线性系统方程：
其中，f(·)是系统的非线性状态转移方程，w(k)是系统的噪声矩阵，w(k)～N(0,Q(k))；h(·)是系统的非线性观测方程，η(k)是系统的观测噪声矩阵，且有η(k)～N(0,R(k))。
步骤3：各移动机器人开始工作，按照CKF滤波框架对整个协同导航系统中各个机器人的状态进行时间更新；
首先根据多移动机器人协同导航系统的维数N来确定CKF滤波所需要的容积点集[ξi，ωi]，其中i＝1,2,…,2N：
按照CKF滤波框架对系统进行时间更新：
步骤4：各移动机器人分别对工作环境中的固定路标点以及其它机器人进行实时观测，获取机器人之间的相互距离和方位角作为观测量ZRR、机器人与路标点之间的距离和方位角作为观测量ZRL，共同构成系统的观测向量Z(k+1)；
步骤5：利用改进的VCKF算法完成系统的量测更新，
然后计算出系统的状态估计：
同时利用量测更新结果和VCE方法估计出系统的噪声方差阵，首先根据多移动机器人的系统方程建立三组伪观测量：
根据残差理论可以得到残差向量的方差矩阵：
则可以计算出过程噪声方差分量和量测噪声方差分量的多余观测分量为：
根据Herlmet方差分量估计可知方差分量因子可由残差向量与相应的多余观测分量按照如下公式得到：
那么，在任意k时刻，观测向量lz(k)(即向量z(k))的方差因子可以按照下面方程计算得到：
其中i＝1,2,...,p，p为Δ(k)的维数。
则系统的量测噪声方差阵R和过程噪声方差阵Q分别为：
至此，系统噪声方差阵和系统状态量可以同时估计出来。
步骤6：对多移动机器人中各机器人的位姿信息进行更新，完成多移动机器人高精度协同导航定位过程。
利用实际试验数据对本发明的效果进行验证，具体如下：
采用University of Toronto,Institute of Aerospace Studies(UTIAS)实验室的自主机器人数据，机器人上均安装有单目视觉和里程计等传感器，单目视觉用来获取机器人与机器人之间以及机器人与路标点之间的方位角和距离等信息，而里程计用来度量机器人自身的运动信息。此外，利用VICON系统(定位精度为0.001m，采样频率为100Hz)作为试验基准。
试验开始后，5个机器人自主运动进行协同导航。利用本发明对该次试验数据进行分析，传统的基于CKF的协同导航定位算法作为对照算法，分别对多移动机器人协同导航系统中5个机器人的导航定位情况进行分析。为更加清晰的说明本发明的优越性，对机器人协同导航定位误差的RMS值进行统计，如表1所示。
表1利用传统协同导航定位算法和利用本发明所得的定位误差RMS值比较
为了进一步验证本发明的优势，对改进算法的一致性进行分析，采用一致性估计误差平方(Normal Estimation Error Squared,NEES)值来衡量算法的一致性，分析结果如图2和图3所示。
从表1、图2和图3可以看出，利用本发明方法可以有效地将5个机器人的位姿估计出来，且所得的各机器人协同定位误差RMS值均小于利用传统协同导航定位算法所得的定位误差RMS值；此外，利用传统定位算法后多移动机器人协同导航系统的NEES值有8.24％超出了95％置信区间，而利用本发明方法后多移动机器人协同导航系统的NEES值仅有3.78％的数据超出了95％置信区间，因此利用本发明算法可以取得更好的算法一致性。
综上所述，本发明提供的协同导航定位方法可以有效提高多移动机器人系统的协同导航定位能力。

Claims (2)

1.一种基于VCKF的多移动机器人协同导航定位方法，其特征在于，包括如下步骤：
步骤1：根据多移动机器人工作环境，确定协同导航系统的初始值，包括多移动机器人的初始位姿和路标点的位置；
步骤2：建立多移动机器人协同导航系统的非线性系统方程；
步骤3：对多移动机器人协同导航系统按照CKF滤波框架进行时间更新；
步骤4：多移动机器人对工作环境中的固定路标点以及其它机器人进行实时观测，获取相对距离和方位角作为观测信息；
步骤5：利用观测到的量测信息和系统方程，利用VCKF算法完成多移动机器人协同导航系统的量测更新；
步骤6：对多移动机器人的位姿信息进行更新；
步骤7：重复步骤3～步骤6，完成多移动机器人的高精度协同导航定位。
2.如权利要求1所述的一种基于VCKF的多移动机器人协同导航定位方法，其特征在于，步骤5中利用改进的VCKF算法完成协同导航系统的量测更新过程包括以下步骤：
首先利用CKF框架完成协同导航系统的量测更新，估计出多机器人的位姿信息；
然后利用VCE方法对系统的过程噪声和量测噪声进行估计：1)在多移动机器人非线性系统时间更新的基础上建立包含系统过程噪声和观测噪声在内的三组伪观测向量；2)根据残差理论计算出伪观测向量的残差方程以及残差向量的方差矩阵；3)计算出对应伪观测向量的多余观测分量，以及过程噪声方差分量和量测噪声方差分量的多余观测分量；4)根据Herlmet方差分量估计方法计算出系统的过程噪声方差阵和量测噪声方差阵，完成系统噪声的实时估计。
CN201711021203.9A 2017-10-27 2017-10-27 一种基于vckf的多机器人协同导航定位方法 Active CN107843259B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711021203.9A CN107843259B (zh) 2017-10-27 2017-10-27 一种基于vckf的多机器人协同导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711021203.9A CN107843259B (zh) 2017-10-27 2017-10-27 一种基于vckf的多机器人协同导航定位方法

Publications (2)

Publication Number Publication Date
CN107843259A true CN107843259A (zh) 2018-03-27
CN107843259B CN107843259B (zh) 2021-04-20

Family

ID=61662954

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711021203.9A Active CN107843259B (zh) 2017-10-27 2017-10-27 一种基于vckf的多机器人协同导航定位方法

Country Status (1)

Country Link
CN (1) CN107843259B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109212472A (zh) * 2018-07-11 2019-01-15 中国科学院信息工程研究所 一种面向噪声环境下的室内无线定位方法及装置
CN109241228A (zh) * 2018-09-04 2019-01-18 山东理工大学 一种多移动机器人协作同步定位与地图构建策略
CN113084801A (zh) * 2021-03-30 2021-07-09 深圳市人工智能与机器人研究院 基于半正定规划优化的多机器人初始位姿相对定位方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104330083A (zh) * 2014-10-27 2015-02-04 南京理工大学 基于平方根无迹卡尔曼滤波的多机器人协同定位算法
CN106323291A (zh) * 2016-09-29 2017-01-11 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
CN106482736A (zh) * 2016-07-11 2017-03-08 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104330083A (zh) * 2014-10-27 2015-02-04 南京理工大学 基于平方根无迹卡尔曼滤波的多机器人协同定位算法
CN106482736A (zh) * 2016-07-11 2017-03-08 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
CN106323291A (zh) * 2016-09-29 2017-01-11 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张亚等: "Adaptive cubature Kalman filter based on the variance-covariance components estimation", 《THE JOURNAL OF GLOBAL POSITIONING SYSTEMS》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109212472A (zh) * 2018-07-11 2019-01-15 中国科学院信息工程研究所 一种面向噪声环境下的室内无线定位方法及装置
CN109212472B (zh) * 2018-07-11 2020-10-27 中国科学院信息工程研究所 一种面向噪声环境下的室内无线定位方法及装置
CN109241228A (zh) * 2018-09-04 2019-01-18 山东理工大学 一种多移动机器人协作同步定位与地图构建策略
CN113084801A (zh) * 2021-03-30 2021-07-09 深圳市人工智能与机器人研究院 基于半正定规划优化的多机器人初始位姿相对定位方法

Also Published As

Publication number Publication date
CN107843259B (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN107084714B (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
CN107367229B (zh) 自由双目立体视觉转轴参数标定方法
Li et al. High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation
CN108759665B (zh) 一种基于坐标转换的空间目标三维重建精度分析方法
US10306206B2 (en) 3-D motion estimation and online temporal calibration for camera-IMU systems
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN108645416B (zh) 基于视觉测量系统的非合作目标相对导航仿真验证方法
CN106482746B (zh) 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法
CN107843259A (zh) 一种基于vckf的多机器人协同导航定位方法
CN103968843A (zh) 一种gps/sins超紧组合导航系统自适应混合滤波方法
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN104331078B (zh) 一种基于位置映射算法的多机器人协作定位方法
CN107782309A (zh) 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法
CN110160522A (zh) 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN107621266A (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN109612476A (zh) 基于惯性导航技术的地图重构方法、装置、惯性导航系统及计算机存储介质
CN104477402A (zh) 一种考虑长桁基准对齐及直线度要求的飞机机身对接调姿方法
CN113870366A (zh) 基于位姿传感器的三维扫描系统的标定方法及其标定系统
CN106546229B (zh) 一种便于现场指挥的测量放样方法
CN110954080A (zh) 一种消除载体磁干扰的磁罗盘校准方法
CN107860388A (zh) 一种基于混合拓扑结构的多机器人协同导航定位算法
CN102354400A (zh) 刚体运动和结构参数测量的多点约束单目轨迹交会方法
CN106908078B (zh) 一种网络化大尺寸空间定位系统的现场标定方法
Sun et al. Mosaic research with 3D lidar point cloud of civil aircraft
CN104236469B (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