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

1.一种沿地铁轨道移动机器人的高精度定位方法，其特征在于：包括以下步骤：步骤1、采用里程计圆弧计算模型，根据航位推算法，得到基于里程计的移动机器人离散运动模型，由此得到实时的移动机器人位姿(x 1 ,y 1 ,θ),具体是定义里程计采样周期为ΔT，根据里程计圆弧计算模型，得出X W 与Y W 方向上的位移增加量为：根据上述航位推算法，得到基于里程计的移动机器人离散运动模型：其中， ΔS L,k 和ΔS R,k 分别表示从k到k+1时刻，左右车轮行驶距离，由此得到实时的移动机器人位姿(x 1 ,y 1 ,θ)；步骤2、根据轨道线路两侧的CPⅢ控制点，使用三维边角后方交汇法；进行智能型全站仪位姿的高精测量,具体是：斜距观测值S及其改正数v s 与其测站点坐标X S ,Y S ,Z S 及照准点CPⅢ坐标X i ,Y i ,Z i ，直接建立以下的关系式式中，测站坐标未知数X S ,Y S ,Z S ，用其坐标近似值 及改正数dx,dy,dz代入，自由设站中照准点CPⅢ坐标为已知量；因dx,dy,dz为微小量，按泰勒公式展开并略去dx,dy,dz的二次及以上的各项，得斜距误差方程式式中 为测站点至目标点之间的斜距近似值；i s 为自由设站点仪器高；v i 为CPIII目标点的棱镜高； 为两点之间的水平距离近似值；k为大气折光系数，用其近似值k 0 带入计算；R为地球平均半径；水平方向误差方程测算；水平方向观测值L及其改正数v L 与未知点坐标，通过坐标方位角T建立如下关系式中，ω＝ω 0 +dω是全站仪上整组方向平差值所共有的定向角，定向角近似值ω 0 按下式进行计算 式中， 是各个目标方向的近似方位角；n为全站仪观测的目标点个数；水平方向观测方程同样按泰勒级数展开，得水平方向误差方程式中，ρ＝205 265"；天顶距误差方程解算；天顶距观测值A及其改正数v A ，与其测站点坐标X S ,Y S ,Z S 及照准点CPⅢ坐标X i ,Y i ,Z i ，满足以下关系式上式按泰勒级数展开，得天顶距误差方程式中， 为天顶距近似值；k＝k 0 +dk为大气折光系数；k 0 为大气折光系数近似值；dk为大气折光改正数，作为未知参数一起参与平差计算；根据以上推导的观测量的误差方程，计算出观测量误差方程的系数矩阵 和系数矩阵 则误差方程的矩阵形式为式中，n为观测的目标点个数；dX＝[dx dy dz dω dk] T 为未知参数向量；按最小二乘原理，误差方程的解为dX＝(B T PB) -1 B T Pl，P为权阵；故得全站仪三维坐标为其坐标误差为式中，Q＝(B T PB) -1 为坐标协因矩阵；σ 0 为验后单位权方差；步骤3、建立基于EKF的全站仪CPⅢ/里程计融合定位模型为系统模型和观测模型，通过EKF算法将全站仪观测信息和里程计状态预测信息进行配准和位姿更新，从而实现对里程计累积误差的周期性校正，实现移动机器人的高精定位；具体是首先基于EKF的全站仪CPⅢ/里程计融合定位模型为系统模型和观测模型，计算融合算法系统模型：设移动机器人的位姿向量为X＝[x,y,θ] T ，位姿向量预测方程为：式中， 表示k时刻，移动机器人位姿向量的先验估计值， 表示k时刻，位姿向量的后验估计值，u k-1 ＝(ΔS r,k-1 ,ΔS l,k-1 )为里程计控制输入量，其中，ΔS r,k-1 、ΔS l,k-1 分别表示移动机器人从k-1时刻到k时刻，左右轮行驶的距离，b为左右两侧车轮轮距；设F X,k 为系统函数f(X,u)关于状态向量X的雅克比矩阵，F u,k 为系统方程关于里程计输入u k-1 ＝(ΔS r,k-1 ,ΔS l,k-1 )的雅克比矩阵，Σ k-1 为里程计位姿估计的误差协方差矩阵，分别表示为：根据非线性误差传播规律，位姿估计误差协方差矩阵预测方程表示为：其中，Q k-1 为系统误差协方差矩阵，k l 和k r 为里程计误差常量，表示电机驱动以及车轮与地面交互的不确定参数，该参数取决于移动机器人的工作环境；解算融合算法观测模型；设k时刻，全站仪在x、y方向上的观测值分别为x CPш,k 和y CPш,k ，则观测值Z k 与位姿向量X的关系为：其中，n x,k 和n y,k 分别表示k时刻，全站仪在x和y方向上的观测噪声，分别近似为服从(0,σ 2 x,k )和(0,σ 2 y,k )分布的高斯自噪声；则观测误差协方差矩阵R k 表示为：观测方程h(X)关于机器人位姿向量X的雅克比矩阵H k 为：扩展卡尔曼滤波增益K k 为：K k ＝P k H k T (H k P k H k T +R k ) -1 (20)根据位姿预测方程，得到k时刻的位姿向量先验估计值 由观测方程得到k时刻的预测观测量 为：用来对位姿先验估计 进行校正的量为测量残差γ k ，即实际观测值与观测预测值之差： 根据测量残余对位姿向量的先验估计值 进行更新校正，则在k时刻，位姿向量后验估计值 和后验估计误差协方差P + k 分别为：其中，位姿向量后验估计值 即为k时刻移动机器人的精确位姿矩阵。