[發明專利]考慮采樣周期隨機變化的機器人緊組合導航方法及系統

有效

• 申請號: 202010206200.8
• 申請日: 2020-03-23
• 公開（公告）號: CN111578939B
• 公開（公告）日: 2021-11-02
• 發明（設計）人: 徐元;申濤;孫明旭;畢淑慧;王自鵬
• 申請（專利權）人: 濟南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01S17/86;G01C21/00;G01C21/16
• 代理公司: 濟南圣達知識產權代理有限公司 37221
• 代理人: 董雪

1.考慮采樣周期隨機變化的機器人緊組合導航方法，其特征在于，包括：

獲取機器人x方向和y方向的位置誤差、速度誤差、航向角以及當前時刻的采樣周期數據；

將上述獲取的數據作為擴展卡爾曼濾波器的狀態向量；

將航跡推算和激光雷達分別測量的機器人與特征點之間距離的平方的差以及當前時刻的采樣周期作為擴展卡爾曼濾波器的觀測向量進行數據融合，最終得到機器人的最優的狀態預估，實現對于機器人的路徑導航；

所述擴展卡爾曼濾波器的狀態方程具體為：

其中，(δx_n,δy_n)、(δx_n-1,δy_n-1)分別為n時刻和n-1時刻機器人在x和y方向的位置誤差；δV_n、δV_n-1分別為n時刻和n-1時刻的速度誤差；分別為n時刻和n-1時刻的航向角；Δs_n、Δs_n-1分別為n時刻和n-1時刻的采樣周期，ω_n-1為n-1時刻的系統噪聲；

所述擴展卡爾曼濾波器的觀測方程具體為：

其中，δd_i,n為n時刻航跡推算與激光雷達分別測量得到的機器人與第i個角特征點之間距離的平方的差；和分別為n時刻航跡推算與激光雷達分別測量得到的機器人與第i個角特征點之間的距離；i＝1,2,…,g；(x^D,y^D)為航跡推算的機器人位置，(x_i,y_i)為第i個角特征點的坐標；ν_n為觀測噪聲，其協方差陣為R。

2.如權利要求1所述的考慮采樣周期隨機變化的機器人緊組合導航方法，其特征在于，在使用激光雷達進行定位時，將室內環境中的角特征點作為特征點。

3.如權利要求1所述的考慮采樣周期隨機變化的機器人緊組合導航方法，其特征在于，所述擴展卡爾曼濾波器k時刻的狀態協方差矩陣具體為：

P_k＝(I-K_kH_k)P_k|k-1

P_k|k-1＝A_k-1P_k-1(A_k-1)^T+Q

其中，K_k為卡爾曼增益；H_k為狀態方程是線性方程時的轉換矩陣；P_k-1為k-1時刻的狀態協方差矩陣；Q為過程噪聲的方差；A_k-1為狀態方程是非線性方程時k-1時刻的狀態轉移矩陣。

4.如權利要求1所述的考慮采樣周期隨機變化的機器人緊組合導航方法，其特征在于，所述擴展卡爾曼濾波器k時刻一步狀態估計矩陣具體為：

X_k|k-1＝A_k-1X_k-1+ω_k-1

其中，A_k-1為狀態方程是非線性方程時k-1時刻的狀態轉移矩陣；X_k-1為k-1時刻的狀態矩陣；ω_k-1為過程噪聲。