[發明專利]里程計累計推算誤差的數據源糾正方法及裝置

有效

• 申請號: 202110282290.3
• 申請日: 2021-03-16
• 公開（公告）號: CN113063441B
• 公開（公告）日: 2022-11-18
• 發明（設計）人: 李金波;周啟龍
• 申請（專利權）人: 李金波
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C22/00
• 代理公司: 長沙永星專利商標事務所(普通合伙) 43001
• 代理人: 周詠;米中業

1.一種里程計累計推算誤差的數據源糾正方法，包括如下步驟：

S1.獲取里程計輸出的第一位姿信息；

S2.采用第二定位系統獲取第二位姿信息；

S3.進行第二位姿信息的置信度判定；

S4.在第二位姿信息的置信度滿足設定的置信度的判定準則時進行第一位姿信息的誤差等級判定；

S5.當第一位姿信息符合誤差等級判斷時，將第二位姿信息作為數據源，對數據源進行糾正。

2.根據權利要求1所述的里程計累計推算誤差的數據源糾正方法，其特征在于步驟S1所述的第一位姿信息包括第一位置信息、第一角度信息和第一速度信息。

3.根據權利要求2所述的里程計累計推算誤差的數據源糾正方法，其特征在于步驟S2，具體為第二定位系統提供位置輸出，通過與里程坐標系進行角度換算，得到第二位姿信息；所述的第二位姿信息包括第二位置信息，或者第二位姿信息包括第二角度信息和第二速度信息；當第二位姿信息包括第二角度信息和第二速度信息時，直接輸出第二位姿信息；當第二位姿信息不包括第二角度信息或第二角度信息由于第二位置信息的置信度低于設定值時，通過當前的第二位姿信息與在設定時間窗口內的若干組第二位姿信息歷史值及間隔時間，計算當前移動載體的第二角度信息。

4.根據權利要求3所述的里程計累計推算誤差的數據源糾正方法，其特征在于計算當前移動載體的第二角度信息，具體為GNSS天線安裝到待測機器人的后部的中間位置，待測機器人在t_k-1時刻的第一GNSS位置及t_k時刻的第二GNSS位置獲取GNSS定位信號；待測機器人在t_k-1時刻處于出發位置，此時待測機器人在里程坐標系中的位置定為(0,0)，里程坐標系采用通用橫墨卡托格網系統坐標系；將t_k時刻的待測機器人在里程坐標系中的方向角記作ψ_k，將t_k時刻的在里程坐標系內的待測機器人中心位置記作(x_k,y_k)；設定待測機器人兼容的坐標系方向為，x軸向前，y軸向左，待測機器人在水平面上運動；待測機器人的天線在t_k-1時刻和t_k時刻的GNSS經緯度數值轉換所得的通用橫墨卡托格網系統位置分別記為utm_k-1和utm_k，其中t_k時刻的通用橫墨卡托格網系統位置包括北向分量utmN_k和東向分量utmE_k；通用橫墨卡托格網系統位置由GNSS經緯度數值轉換；因此，根據待測機器人的天線在t_k-1時刻和t_k時刻在通用橫墨卡托格網系統坐標系中的位置，采用逆正切法計算出t_k-1時刻到t_k時刻待測機器人的天線的里程位移相對于正東方向的角度，記為γ：

γ＝atan2(utmN_k-utmN_k-1,utmE_k-utmE_k-1)

將α_t設為t_k時刻待測機器人的中心相對于t_k-1時刻的待測機器人中心的方向角增量：

α_t＝atan2(y_k-y_k-1,x_k-x_k-1)

D_a為t_k時刻待測機器人的天線相對于t_k-1時刻待測機器人的中心的位移：

其中，D_c為待測機器人的中心在t_k-1時刻與t_k時刻之間發生的里程位移、L_a為待測機器人中心與待測機器人的天線的安裝位置之間的距離；

δ為D_n與待測機器人在t_k-1時刻時中心與天線之間的方向的夾角：

其中，D_n為待測機器人的天線在t_k-1時刻與t_k時刻之間發生的里程位移、L_a為待測機器人的中心與機器人的天線的安裝位置之間的距離、D_a為t_k時刻待測機器人的天線相對于t_k-1時刻待測機器人的中心的位移。