[發明專利]一種沿地鐵軌道移動機器人的高精度定位方法

有效

• 申請號: 201910669662.0
• 申請日: 2019-07-24
• 公開（公告）號: CN110702091B
• 公開（公告）日: 2022-08-30
• 發明（設計）人: 肖曉暉;張浩;段寶閣;邱邵峰;陳瀟;耿明;何杰
• 申請（專利權）人: 武漢大學;中鐵第四勘察設計院集團有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 魯力

1.一種沿地鐵軌道移動機器人的高精度定位方法，其特征在于：包括以下步驟：

步驟1、采用里程計圓弧計算模型，根據航位推算法，得到基于里程計的移動機器人離散運動模型，由此得到實時的移動機器人位姿(x₁,y₁,θ),具體是定義里程計采樣周期為ΔT，根據里程計圓弧計算模型，得出X_W與Y_W方向上的位移增加量為：

根據上述航位推算法，得到基于里程計的移動機器人離散運動模型：

其中，ΔS_L,k和ΔS_R,k分別表示從k到k+1時刻，左右車輪行駛距離，由此得到實時的移動機器人位姿(x₁,y₁,θ)；

步驟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⁰帶入計算；R為地球平均半徑；

水平方向誤差方程測算；水平方向觀測值L及其改正數v_L與未知點坐標，通過坐標方位角T建立如下關系

式中，ω＝ω⁰+dω是全站儀上整組方向平差值所共有的定向角，定向角近似值ω⁰按下式進行計算式中，是各個目標方向的近似方位角；n為全站儀觀測的目標點個數；

水平方向觀測方程同樣按泰勒級數展開，得水平方向誤差方程

式中，ρ＝205 265；

天頂距誤差方程解算；天頂距觀測值A及其改正數v_A，與其測站點坐標X_S,Y_S,Z_S及照準點CPⅢ坐標X_i,Y_i,Z_i，滿足以下關系式

上式按泰勒級數展開，得天頂距誤差方程

式中，為天頂距近似值；k＝k⁰+dk為大氣折光系數；k⁰為大氣折光系數近似值；dk為大氣折光改正數，作為未知參數一起參與平差計算；

根據以上推導的觀測量的誤差方程，計算出觀測量誤差方程的系數矩陣和系數矩陣則誤差方程的矩陣形式為

式中，n為觀測的目標點個數；dX＝[dx dy dz dω dk]^T為未知參數向量；

按最小二乘原理，誤差方程的解為dX＝(B^TPB)^-1B^TPl，P為權陣；

故得全站儀三維坐標為

其坐標誤差為

式中，Q＝(B^TPB)^-1為坐標協因矩陣；σ₀為驗后單位權方差；

步驟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,σ²_x,k)和(0,σ²_y,k)分布的高斯自噪聲；則觀測誤差協方差矩陣R_k表示為：

觀測方程h(X)關于機器人位姿向量X的雅克比矩陣H_k為：

擴展卡爾曼濾波增益K_k為：

K_k＝P_kH_k^T(H_kP_kH_k^T+R_k)^-1 (20)

根據位姿預測方程，得到k時刻的位姿向量先驗估計值由觀測方程得到k時刻的預測觀測量為：

用來對位姿先驗估計進行校正的量為測量殘差γ_k，即實際觀測值與觀測預測值之差：根據測量殘余對位姿向量的先驗估計值進行更新校正，則在k時刻，位姿向量后驗估計值和后驗估計誤差協方差P⁺_k分別為：

其中，位姿向量后驗估計值即為k時刻移動機器人的精確位姿矩陣。