CN110702091B - 一种沿地铁轨道移动机器人的高精度定位方法 - Google Patents

一种沿地铁轨道移动机器人的高精度定位方法 Download PDF

Info

Publication number
CN110702091B
CN110702091B CN201910669662.0A CN201910669662A CN110702091B CN 110702091 B CN110702091 B CN 110702091B CN 201910669662 A CN201910669662 A CN 201910669662A CN 110702091 B CN110702091 B CN 110702091B
Authority
CN
China
Prior art keywords
pose
odometer
mobile robot
error
equation
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
CN201910669662.0A
Other languages
English (en)
Other versions
CN110702091A (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.)
Wuhan University WHU
China Railway Siyuan Survey and Design Group Co Ltd
Original Assignee
Wuhan University WHU
China Railway Siyuan Survey and Design Group Co Ltd
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 Wuhan University WHU, China Railway Siyuan Survey and Design Group Co Ltd filed Critical Wuhan University WHU
Priority to CN201910669662.0A priority Critical patent/CN110702091B/zh
Publication of CN110702091A publication Critical patent/CN110702091A/zh
Application granted granted Critical
Publication of CN110702091B publication Critical patent/CN110702091B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

本发明公开了一种沿地铁轨道移动机器人的高精度定位方法，包括以下几个步骤：移动机器人里程计位姿估计，得到机器人实时位姿(x1,y1,θ)，为了消除里程计定位存在的累积误差，采用扩展卡尔曼滤波(EKF)实现里程计和全站仪测量融合的定位方法，建立了基于EKF的全站仪CPⅢ/里程计融合定位模型，通过EKF算法将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位。

Description

一种沿地铁轨道移动机器人的高精度定位方法
技术领域
本发明涉及一种沿地铁轨道移动机器人的高精度定位方法。
背景技术
随着无线通信、互联网技术以及人工智能的快速发展，定位技术得到了广泛关注,当前定位技术主要有：全球导航卫星系统(Global Navigation Satellite System,GNSS)、惯性导航系统 (Inertial Navigation System)、激光雷达导航、视觉导航等。这些导航方法，均可以独立完成位姿计算、提取与估计。比较传统的GNSS卫星惯导组合进行位姿估计的方法在室外巡检机器人或小型无人机上应用的最多，技术也相对比较成熟，位姿估计的精度较高。然而，GPS 卫星信号易受到环境的影响，在室内或者室外有遮挡的环境中出现通讯失调的情况，这会极大的影响机器人的正常工作。对于激光雷达来说，其环境适用范围广，适合室内、室外环境信息的采集，同时具有精度高，占用资源小的特点，但它的体积相对较大且价格昂贵，且激光雷达在机器人上的安装位姿也会对机器人产生影响。另外，单纯使用惯性传感器实现姿态估计，由于惯性传感器本身具有计算频率高，累积误差大的特点，长时间工作会导致位姿估计精度过低，因此不适合长时间独立工作。
当移动机器人的工作环境比较复杂时，由于外界因素的干扰和传感器本身存在的观测误差，会导致定位传感器信息不准确，再加上移动机器人自身状态存在很大的不确定性，因此，仅仅依靠某一种定位传感器很难达到移动机器人定位的高精度和高可靠性的要求。为了提高定移动机器人的定位精度和可靠性，通常会给机器人携带多种不同的定位传感器，通过将不同传感器的互补定位信息按照一定的数据融合方法实现多传感器的组合定位，从而保证了移动机器人实时、可靠的定位。
现有的移动机器人大多基于GNSS或视觉系统进行定位，对于沿地铁轨道做病害检测的移动机器人，在隧道，地下空间等复杂空间工作时，其定位受环境影响较大，极大影响了机器人的正常工作，且对移动机器人的定位精度较低。
发明内容
本发明技术克服现有无人沿轨道移动机器人面对复杂环境定位准确性问题，解决了传统基于某一种定位传感器定位精度和可靠性较差的问题。
为解决上述问题，本发明公开了一种沿地铁轨道移动机器人高精度定位方法，具体包括以下步骤：
步骤1、采用里程计圆弧计算模型，根据航位推算法，得到基于里程计的移动机器人离散运动模型，由此可得到实时的移动机器人位姿(x1,y1,θ),具体是定义里程计采样周期为ΔT，根据里程计圆弧计算模型，可得出XW与YW方向上的位移增加量为：
Figure GDA0003647746060000021
根据上述航位推算法，可得到基于里程计的移动机器人离散运动模型：
Figure GDA0003647746060000022
其中，
Figure GDA0003647746060000023
ΔSL,k和ΔSR,k分别表示从k到k+1时刻，左右车轮行驶距离，由此可得到实时的移动机器人位姿(x1,y1,θ)；
步骤2、根据轨道线路两侧的CPⅢ控制点，使用三维边角后方交汇法；进行智能型全站仪位姿的高精测量,具体是：
斜距观测值S及其改正数vs与其测站点坐标XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，直接建立以下的关系式
Figure GDA0003647746060000024
式中，测站坐标未知数XS,YS,ZS，可用其坐标近似值
Figure GDA0003647746060000025
及改正数dx,dy,dz代入，自由设站中照准点CPⅢ坐标为已知量；因dx,dy,dz为微小量，按泰勒公式展开并略去dx,dy,dz 的二次及以上的各项，得斜距误差方程式
Figure GDA0003647746060000026
式中
Figure GDA0003647746060000027
为测站点至目标点之间的斜距近似值；is为自由设站点仪器高；vi为
Figure GDA0003647746060000028
目标点的棱镜高；
Figure GDA0003647746060000029
为两点之间的水平距离近似值；k为大气折光系数，用其近似值k0带入计算；R为地球平均半径；
水平方向误差方程测算；水平方向观测值L及其改正数vL与未知点坐标，通过坐标方位角T建立如下关系
Figure GDA0003647746060000031
式中，ω＝ω0+dω是全站仪上整组方向平差值所共有的定向角，定向角近似值ω0可按下式进行计算
Figure GDA0003647746060000032
式中，
Figure GDA0003647746060000033
是各个目标方向的近似方位角；n 为全站仪观测的目标点个数；
水平方向观测方程同样按泰勒级数展开，得水平方向误差方程
Figure GDA0003647746060000034
式中，ρ＝205 265"；
天顶距误差方程解算；天顶距观测值A及其改正数vA，与其测站点坐标XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，满足以下关系式
Figure GDA0003647746060000035
上式按泰勒级数展开，得天顶距误差方程
Figure GDA0003647746060000036
式中，
Figure GDA0003647746060000037
为天顶距近似值；k＝k0+dk为大气折光系数；k0为大气折光系数近似值；dk 为大气折光改正数，作为未知参数一起参与平差计算；
根据以上推导的观测量的误差方程，可计算出观测量误差方程的系数矩阵
Figure GDA0003647746060000038
和系数矩阵
Figure GDA0003647746060000039
则误差方程的矩阵形式为
Figure GDA00036477460600000310
式中，n为观测的目标点个数；dX＝[dx dy dz dω dk]T为未知参数向量；
按最小二乘原理，误差方程的解为dX＝(BTPB)-1BTPl，P为权阵；
故可得全站仪三维坐标为
Figure DA00036477460663477972
其坐标误差为
Figure GDA0003647746060000042
式中，Q＝(BTPB)-1为坐标协因矩阵；σ0为验后单位权方差；
步骤3、建立基于EKF的全站仪CPⅢ/里程计融合定位模型为系统模型和观测模型，通过EKF算法将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位；具体是首先基于EKF的全站仪CP Ⅲ/里程计融合定位模型为系统模型和观测模型，计算融合算法系统模型：设移动机器人的位姿向量为X＝[x,y,θ]T，位姿向量预测方程为：
Figure GDA0003647746060000043
式中，
Figure GDA0003647746060000044
表示k时刻，移动机器人位姿向量的先验估计值，
Figure GDA0003647746060000045
表示k时刻，位姿向量的后验估计值，uk-1＝(ΔSr,k-1,ΔSl,k-1)为里程计控制输入量，其中，ΔSr,k-1、ΔSl,k-1分别表示移动机器人从k-1时刻到k时刻，左右轮行驶的距离，b为左右两侧车轮轮距；设FX,k为系统函数f(X,u)关于状态向量X的雅克比矩阵，Fu,k为系统方程关于里程计输入uk-1＝(ΔSr,k-1,ΔSl,k-1)的雅克比矩阵，Σk-1为里程计位姿估计的误差协方差矩阵，分别表示为：
Figure GDA0003647746060000051
Figure GDA0003647746060000052
根据非线性误差传播规律，位姿估计误差协方差矩阵预测方程可表示为：
Figure GDA0003647746060000053
Figure GDA0003647746060000054
其中，Qk-1为系统误差协方差矩阵，kl和kr为里程计误差常量，表示电机驱动以及车轮与地面交互的不确定参数，该参数取决于移动机器人的工作环境；
解算融合算法观测模型；设k时刻，全站仪在x、y方向上的观测值分别为 xCPш,k和yCPш,k，则观测值Zk与位姿向量X的关系为：
Figure GDA0003647746060000055
其中，nx,k和ny,k分别表示k时刻，全站仪在x和y方向上的观测噪声，可分别近似为服从(0,σ2 x,k)和(0,σ2 y,k)分布的高斯自噪声；则观测误差协方差矩阵Rk可表示为：
Figure GDA0003647746060000061
观测方程h(X)关于机器人位姿向量X的雅克比矩阵Hk为：
Figure GDA0003647746060000062
扩展卡尔曼滤波增益Kk为：
Figure GDA0003647746060000063
根据位姿预测方程，得到k时刻的位姿向量先验估计值
Figure GDA0003647746060000064
由观测方程得到k时刻的预测观测量
Figure GDA0003647746060000065
为：
Figure GDA0003647746060000066
用来对位姿先验估计
Figure GDA0003647746060000067
进行校正的量为测量残差γk，即实际观测值与观测预测值之差：
Figure GDA0003647746060000068
根据测量残余对位姿向量的先验估计值
Figure GDA0003647746060000069
进行更新校正，则在k时刻，位姿向量后验估计值
Figure GDA00036477460600000610
和后验估计误差协方差P+ k分别为：
Figure GDA00036477460600000611
Figure GDA00036477460600000612
其中，位姿向量后验估计值
Figure GDA00036477460600000613
即为k时刻移动机器人的精确位姿矩阵。
因此本发明具有如下优点:1.现有移动机器人大多基于GNSS或视觉系统进行定位，但对于沿地铁轨道做病害检测的移动机器人，在隧道，地下空间等复杂环境工作时，相应传感器无法使用或受影响极大，定位不精准，极大影响机器人的正常工作。本发明采用轨检小车所带的智能全站仪结合CPⅢ控制网测量技术，实现对轨道移动机器人的高精定位，能适应隧道，地下空间等复杂环境且具有较强定位精度。2.在隧道，地下空间等复杂空间工作时，由于外界因素的干扰和传感器本身存在的观测误差，会导致定位传感器信息不准确，再加上移动机器人自身状态存在很大的不确定性，因此，仅仅依靠某一种定位传感器很难达到移动机器人定位的高精度和高可靠性的要求。本发明采用里程计与智能全站仪信息融合定位技术，通过EKF算法将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位，且定位系统具有较强的鲁棒性。
附图说明
附图1为里程计圆弧模型。
附图2为全站仪三维坐标采集流程。
附图3为全站仪三维边角交会原理图。
附图4为EKF算法流程图。
附图5为基于EKF的全站仪CPⅢ/里程计融合定位结构图。
附图6为本发明的方法流程示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。
一种沿地铁轨道移动机器人的高精度定位方法，具体包括以下步骤：
步骤S1：设移动机器人的初始位姿P0＝(x0,y0,θ0)，经过ΔT时间后到达位姿点 P1＝(x0+Δx,y0+Δy,θ0+Δθ)。其中(x0,y0)为移动机器人初始位姿，θ0为初始方位角，即移动机器人坐标系XR轴正方向与全局坐标系XW轴正方向的夹角，b为左右车轮轮距，R为移动机器人运动圆弧的半径，ΔSR、ΔSL、ΔS分别表示左右轮和中心点在ΔT时间内运动的距离，Δx、Δy、Δθ分别表示移动机器人在XW方向和YW方向上的位移与方位角增加量。
假设在运动过程中，车轮没有发生打滑及碰撞现象。则：
Figure GDA0003647746060000071
里程计采样周期为，根据附图1所示的里程计圆弧计算模型，可得出XW与YW方向上的位移增加量为：
Figure GDA0003647746060000072
步骤S2：根据上述航位推算法，可得到基于里程计的移动机器人离散运动模型：
Figure GDA0003647746060000073
其中，
Figure GDA0003647746060000074
ΔSL,k和ΔSR,k分别表示从k到k+1时刻，左右车轮行驶距离，由此可得到实时的移动机器人位姿(x1,y1,θ)。
步骤S3：移动机器人每移动5m或当机器人检测到轨道“病害”时，全站仪开始工作。根据线路两侧的CPⅢ控制点，使用三维边角后方交汇法(见附图3)，进全站仪位姿的高精测量。
斜距观测值S及其改正数vs与其测站点坐标XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，可直接建立以下的关系式
Figure GDA0003647746060000081
式中，测站坐标未知数XS,YS,ZS，可用其坐标近似值
Figure GDA0003647746060000082
及改正数dx,dy,dz代入，自由设站中照准点CPⅢ坐标为已知量。因dx,dy,dz为微小量，按泰勒公式展开并略去dx,dy,dz 的二次及以上的各项，得斜距误差方程式
Figure GDA0003647746060000083
式中
Figure GDA0003647746060000084
为测站点至目标点之间的斜距近似值；is为自由设站点仪器高；vi为
Figure GDA0003647746060000085
目标点的棱镜高；
Figure GDA0003647746060000086
为两点之间的水平距离近似值；k为大气折光系数，用其近似值k0带入计算；R为地球平均半径。
步骤S4：水平方向误差方程测算。水平方向观测值L及其改正数vL与未知点坐标，通过坐标方位角T建立如下关系
Figure GDA0003647746060000087
式中，ω＝ω0+dω是全站仪上整组方向平差值所共有的定向角，定向角近似值ω0可按下式进行计算
Figure GDA0003647746060000088
式中，
Figure GDA0003647746060000089
是各个目标方向的近似方位角；n为全站仪观测的目标点个数。
水平方向观测方程同样按泰勒级数展开，得水平方向误差方程
Figure GDA00036477460600000810
式中，ρ＝205 265"。
步骤S5：天顶距误差方程解算。天顶距观测值A及其改正数vA，与其测站点坐标 XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，满足以下关系式
Figure GDA0003647746060000091
上式按泰勒级数展开，得天顶距误差方程
Figure GDA0003647746060000092
式中，
Figure GDA0003647746060000093
为天顶距近似值；k＝k0+dk为大气折光系数；k0为大气折光系数近似值；dk为大气折光改正数，作为未知参数一起参与平差计算。
步骤S6：根据以上推导的观测量的误差方程，可计算出观测量误差方程的系数矩阵
Figure GDA0003647746060000094
和系数矩阵
Figure GDA0003647746060000095
则误差方程的矩阵形式为
Figure GDA0003647746060000096
式中，n为观测的目标点个数；dX＝[dx dy dz dω dk]T为未知参数向量。
按最小二乘原理，误差方程的解为dX＝(BTPB)-1BTPl，P为权阵。
故可得全站仪三维坐标为
Figure GDA0003647746060000097
其坐标误差为
Figure GDA0003647746060000098
式中，Q＝(BTPB)-1为坐标协因矩阵；σ0为验后单位权方差。
步骤S7:计算融合算法系统模型。设移动机器人的位姿向量为X＝[x,y,θ]T，位姿向量预测方程为：
Figure GDA0003647746060000099
式中，
Figure GDA0003647746060000101
表示k时刻，移动机器人位姿向量的先验估计值，
Figure DA00036477460663533683
表示k时刻，位姿向量的后验估计值，uk-1＝(ΔSr,k-1,ΔSl,k-1)为里程计控制输入量，其中，ΔSr,k-1、ΔSl,k-1分别表示移动机器人从k-1时刻到k时刻，左右轮行驶的距离，b为左右两侧车轮轮距。设FX,k为系统函数f(X,u)关于状态向量X的雅克比矩阵，Fu,k为系统方程关于里程计输入uk-1＝(ΔSr,k-1,ΔSl,k-1)的雅克比矩阵，Σk-1为里程计位姿估计的误差协方差矩阵，分别表示为：
Figure GDA0003647746060000103
Figure GDA0003647746060000104
根据非线性误差传播规律，位姿估计误差协方差矩阵预测方程可表示为：
Figure GDA0003647746060000105
Figure GDA0003647746060000106
其中，Qk-1为系统误差协方差矩阵，kl和kr为里程计误差常量，表示电机驱动以及车轮与地面交互的不确定参数，该参数取决于移动机器人的工作环境。
步骤S8：解算融合算法观测模型。设k时刻，全站仪在x、y方向上的观测值分别为xCPш,k和yCPш,k，则观测值Zk与位姿向量X的关系为：
Figure GDA0003647746060000111
其中，nx,k和ny,k分别表示k时刻，全站仪在x和y方向上的观测噪声，可分别近似为服从(0,σ2 x,k)和(0,σ2 y,k)分布的高斯自噪声。则观测误差协方差矩阵Rk可表示为：
Figure GDA0003647746060000112
观测方程h(X)关于机器人位姿向量X的雅克比矩阵Hk为：
Figure GDA0003647746060000113
步骤S9：扩展卡尔曼滤波增益Kk为：
Figure GDA0003647746060000114
根据位姿预测方程，得到k时刻的位姿向量先验估计值
Figure GDA0003647746060000115
由观测方程得到k时刻的预测观测量
Figure GDA0003647746060000116
为：
Figure GDA0003647746060000117
用来对位姿先验估计
Figure GDA0003647746060000118
进行校正的量为测量残差γk，即实际观测值与观测预测值之差：
Figure GDA0003647746060000119
根据测量残余对位姿向量的先验估计值
Figure GDA00036477460600001110
进行更新校正，则在k时刻，位姿向量后验估计值
Figure GDA00036477460600001111
和后验估计误差协方差P+ k分别为：
Figure GDA00036477460600001112
Figure GDA00036477460600001113
步骤S10：将经过扩展卡尔曼滤波后得到的准确坐标(x3,y3,θ)作为移动机器人的精确位姿坐标，对里程计的坐标进行校正。由此，将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (1)

1.一种沿地铁轨道移动机器人的高精度定位方法，其特征在于：包括以下步骤：
步骤1、采用里程计圆弧计算模型，根据航位推算法，得到基于里程计的移动机器人离散运动模型，由此得到实时的移动机器人位姿(x1,y1,θ),具体是定义里程计采样周期为ΔT，根据里程计圆弧计算模型，得出XW与YW方向上的位移增加量为：
Figure FDA0003647746050000011
根据上述航位推算法，得到基于里程计的移动机器人离散运动模型：
Figure FDA0003647746050000012
其中，
Figure FDA0003647746050000013
ΔSL,k和ΔSR,k分别表示从k到k+1时刻，左右车轮行驶距离，由此得到实时的移动机器人位姿(x1,y1,θ)；
步骤2、根据轨道线路两侧的CPⅢ控制点，使用三维边角后方交汇法；进行智能型全站仪位姿的高精测量,具体是：
斜距观测值S及其改正数vs与其测站点坐标XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，直接建立以下的关系式
Figure FDA0003647746050000014
式中，测站坐标未知数XS,YS,ZS，用其坐标近似值
Figure FDA0003647746050000015
及改正数dx,dy,dz代入，自由设站中照准点CPⅢ坐标为已知量；因dx,dy,dz为微小量，按泰勒公式展开并略去dx,dy,dz的二次及以上的各项，得斜距误差方程式
Figure FDA0003647746050000016
式中
Figure FDA0003647746050000017
为测站点至目标点之间的斜距近似值；is为自由设站点仪器高；vi为CPIII目标点的棱镜高；
Figure FDA0003647746050000018
为两点之间的水平距离近似值；k为大气折光系数，用其近似值k0带入计算；R为地球平均半径；
水平方向误差方程测算；水平方向观测值L及其改正数vL与未知点坐标，通过坐标方位角T建立如下关系
Figure FDA0003647746050000021
式中，ω＝ω0+dω是全站仪上整组方向平差值所共有的定向角，定向角近似值ω0按下式进行计算
Figure FDA0003647746050000022
式中，
Figure FDA0003647746050000023
是各个目标方向的近似方位角；n为全站仪观测的目标点个数；
水平方向观测方程同样按泰勒级数展开，得水平方向误差方程
Figure FDA0003647746050000024
式中，ρ＝205 265"；
天顶距误差方程解算；天顶距观测值A及其改正数vA，与其测站点坐标XS,YS,ZS及照准点CPⅢ坐标Xi,Yi,Zi，满足以下关系式
Figure FDA0003647746050000025
上式按泰勒级数展开，得天顶距误差方程
Figure FDA0003647746050000026
式中，
Figure FDA0003647746050000027
为天顶距近似值；k＝k0+dk为大气折光系数；k0为大气折光系数近似值；dk为大气折光改正数，作为未知参数一起参与平差计算；
根据以上推导的观测量的误差方程，计算出观测量误差方程的系数矩阵
Figure FDA0003647746050000028
和系数矩阵
Figure FDA0003647746050000029
则误差方程的矩阵形式为
Figure FDA0003647746050000031
式中，n为观测的目标点个数；dX＝[dx dy dz dω dk]T为未知参数向量；
按最小二乘原理，误差方程的解为dX＝(BTPB)-1BTPl，P为权阵；
故得全站仪三维坐标为
Figure FDA0003647746050000032
其坐标误差为
Figure FDA0003647746050000033
式中，Q＝(BTPB)-1为坐标协因矩阵；σ0为验后单位权方差；
步骤3、建立基于EKF的全站仪CPⅢ/里程计融合定位模型为系统模型和观测模型，通过EKF算法将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位；具体是首先基于EKF的全站仪CPⅢ/里程计融合定位模型为系统模型和观测模型，计算融合算法系统模型：设移动机器人的位姿向量为X＝[x,y,θ]T，位姿向量预测方程为：
Figure FDA0003647746050000034
式中，
Figure FDA0003647746050000035
表示k时刻，移动机器人位姿向量的先验估计值，
Figure FDA0003647746050000036
表示k时刻，位姿向量的后验估计值，uk-1＝(ΔSr,k-1,ΔSl,k-1)为里程计控制输入量，其中，ΔSr,k-1、ΔSl,k-1分别表示移动机器人从k-1时刻到k时刻，左右轮行驶的距离，b为左右两侧车轮轮距；设FX,k为系统函数f(X,u)关于状态向量X的雅克比矩阵，Fu,k为系统方程关于里程计输入uk-1＝(ΔSr,k-1,ΔSl,k-1)的雅克比矩阵，Σk-1为里程计位姿估计的误差协方差矩阵，分别表示为：
Figure FDA0003647746050000041
Figure FDA0003647746050000042
根据非线性误差传播规律，位姿估计误差协方差矩阵预测方程表示为：
Figure FDA0003647746050000043
Figure FDA0003647746050000044
其中，Qk-1为系统误差协方差矩阵，kl和kr为里程计误差常量，表示电机驱动以及车轮与地面交互的不确定参数，该参数取决于移动机器人的工作环境；
解算融合算法观测模型；设k时刻，全站仪在x、y方向上的观测值分别为xCPш,k和yCPш,k，则观测值Zk与位姿向量X的关系为：
Figure FDA0003647746050000045
其中，nx,k和ny,k分别表示k时刻，全站仪在x和y方向上的观测噪声，分别近似为服从(0,σ2 x,k)和(0,σ2 y,k)分布的高斯自噪声；则观测误差协方差矩阵Rk表示为：
Figure FDA0003647746050000051
观测方程h(X)关于机器人位姿向量X的雅克比矩阵Hk为：
Figure FDA0003647746050000052
扩展卡尔曼滤波增益Kk为：
Kk＝PkHk T(HkPkHk T+Rk)-1 (20)
根据位姿预测方程，得到k时刻的位姿向量先验估计值
Figure FDA0003647746050000053
由观测方程得到k时刻的预测观测量
Figure FDA0003647746050000054
为：
Figure FDA0003647746050000055
用来对位姿先验估计
Figure FDA0003647746050000056
进行校正的量为测量残差γk，即实际观测值与观测预测值之差：
Figure FDA0003647746050000057
根据测量残余对位姿向量的先验估计值
Figure FDA0003647746050000058
进行更新校正，则在k时刻，位姿向量后验估计值
Figure FDA0003647746050000059
和后验估计误差协方差P+ k分别为：
Figure FDA00036477460500000510
Figure FDA00036477460500000511
其中，位姿向量后验估计值
Figure FDA00036477460500000512
即为k时刻移动机器人的精确位姿矩阵。
CN201910669662.0A 2019-07-24 2019-07-24 一种沿地铁轨道移动机器人的高精度定位方法 Active CN110702091B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910669662.0A CN110702091B (zh) 2019-07-24 2019-07-24 一种沿地铁轨道移动机器人的高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910669662.0A CN110702091B (zh) 2019-07-24 2019-07-24 一种沿地铁轨道移动机器人的高精度定位方法

Publications (2)

Publication Number Publication Date
CN110702091A CN110702091A (zh) 2020-01-17
CN110702091B true CN110702091B (zh) 2022-08-30

Family

ID=69193310

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910669662.0A Active CN110702091B (zh) 2019-07-24 2019-07-24 一种沿地铁轨道移动机器人的高精度定位方法

Country Status (1)

Country Link
CN (1) CN110702091B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111311679B (zh) * 2020-01-31 2022-04-01 武汉大学 一种基于深度相机的自由漂浮目标位姿估计方法
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN111721262B (zh) * 2020-07-10 2021-06-11 中国科学院武汉岩土力学研究所 一种场地高程测量中的全站仪跟踪自动引导方法
CN112045655B (zh) * 2020-08-14 2021-12-31 华中科技大学 用于大尺度多站点场景的移动机器人位姿测量方法及系统
CN112578389B (zh) * 2020-12-08 2021-09-24 浙江景奕智能控制技术有限公司 一种多源融合的rov实时路径规划方法
CN112947481B (zh) * 2021-03-24 2022-11-15 南京理工大学泰州科技学院 一种家庭服务机器人自主定位控制方法
CN113203419B (zh) * 2021-04-25 2023-11-10 重庆大学 基于神经网络的室内巡检机器人校正定位方法
CN113449254B (zh) * 2021-06-09 2023-09-01 华设设计集团股份有限公司 任意网型变形监测稳定性分析方法及监控点位置确定方法
CN114383610B (zh) * 2021-12-24 2024-07-16 郑州恒达智控科技股份有限公司 一种基于移动三维扫描技术的抖动检测分段滤波方法
CN115793656B (zh) * 2022-12-09 2023-08-25 武汉大学 一种基于线路标志的轨道巡检机器人定位方法、装置及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN108776487A (zh) * 2018-08-22 2018-11-09 中国矿业大学 一种矿用轨道式巡检机器人及其定位方法
CN109341700A (zh) * 2018-12-04 2019-02-15 中国航空工业集团公司西安航空计算技术研究所 一种低能见度下固定翼飞机视觉辅助着陆导航方法
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10101459B2 (en) * 2014-06-06 2018-10-16 Carlson Software, Inc. GNSS mobile base station and data collector with electronic leveling

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN108776487A (zh) * 2018-08-22 2018-11-09 中国矿业大学 一种矿用轨道式巡检机器人及其定位方法
CN109341700A (zh) * 2018-12-04 2019-02-15 中国航空工业集团公司西安航空计算技术研究所 一种低能见度下固定翼飞机视觉辅助着陆导航方法
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SLAM技术及其在测绘领域中的应用;黄鹤等;《测绘通报》;20181231(第3期);18-24 *
Uncertainty Characterisation of Mobile Robot Localisation Techniques using Optical Surveying Grade Instruments;McLoughlin Benjamin J. et al.;《SENSORS》;20180913;第18卷(第7期);1-16 *

Also Published As

Publication number Publication date
CN110702091A (zh) 2020-01-17

Similar Documents

Publication Publication Date Title
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN105628026B (zh) 一种移动物体的定位定姿方法和系统
CN109946730B (zh) 一种车路协同下基于超宽带的车辆高可靠融合定位方法
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
CN109579838A (zh) Agv小车的定位方法及定位系统
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN108036792A (zh) 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN113237482B (zh) 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN113155124A (zh) 一种多源辅助导航方法及装置
CN113405560A (zh) 车辆定位和路径规划统一建模方法
Hu et al. Kilometer sign positioning-aided INS/odometer integration for land vehicle autonomous navigation
CN112362052B (zh) 一种融合定位方法及系统
CN116242372B (zh) 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法
US9250078B2 (en) Method for determining the position of moving objects
TWI635302B (zh) 載具即時精準定位系統
CN116429121A (zh) 基于多传感器的定位方法、装置、自移动设备及存储介质
CN114384932B (zh) 一种基于距离测量的无人机导航对接方法
CN115183767A (zh) 一种基于arkf的单目vio/uwb室内组合定位方法
CN114777769A (zh) 一种室外变电站巡检机器人定位系统及其方法
Xu et al. Research on Indoor AGV Fusion Localization Based on Adaptive Weight EKF Using Multi-sensor
Chengbo et al. Research on wheeled mobile robot positioning based on ekf multi-sensor data fusion
Hyun et al. Pose-graph-based uwb slam with nlos factor estimation
CN115145294B (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
CB03 Change of inventor or designer information

Inventor after: Xiao Xiaohui

Inventor after: Zhang Hao

Inventor after: Duan Baoge

Inventor after: Qiu Shaofeng

Inventor after: Chen Xiao

Inventor after: Geng Ming

Inventor after: He Jie

Inventor before: Xiao Xiaohui

Inventor before: Zhang Hao

Inventor before: Duan Baoge

Inventor before: Qiu Shaofeng

Inventor before: Chen Xiao

Inventor before: Geng Ming

Inventor before: He Jie

CB03 Change of inventor or designer information