[發(fā)明專利]基于陀螺儀的掃地機器人角度最優(yōu)估算方法

在審

• 申請?zhí)枺?/td>: 201910577374.2
• 申請日: 2019-06-28
• 公開（公告）號: CN110231030A
• 公開（公告）日: 2019-09-13
• 發(fā)明（設計）人: 高瑜
• 申請（專利權）人: 蘇州瑞久智能科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 蘇州吳韻知識產權代理事務所(普通合伙) 32364
• 代理人: 王銘陸

1.一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于，所述方法包括如下步驟：

步驟一、在全局坐標系{O,X,Y}中，建立雙輪驅動掃地機器人的連續(xù)運動學模型和離散化的運動學模型；

步驟二、基于系統(tǒng)上一時刻的狀態(tài)，利用系統(tǒng)的過程模型，預測系統(tǒng)當前k時刻的狀態(tài)；

步驟三、利用過程噪聲Q更新濾波均方誤差，得到當前時刻的預測均方誤差P_k|k-1；

步驟四、基于預測均方誤差P_k|k-1和測量噪聲R，計算卡爾曼增益K_k；

步驟五、結合預測值θ_k|k-1和陀螺儀的測量值Z_k，得到當前時刻的角度最優(yōu)估算值θ_k|k；

步驟六、更新當前時刻的濾波均方誤差，得到最優(yōu)估計值P_k|k；

步驟七、當系統(tǒng)進入下一時刻，重復步驟二至步驟六，實現自回歸運算。

2.如權利要求1所述的一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于：所述步驟一中，雙輪驅動掃地機器人的坐標表示為P＝[x y θ]^T，其中，P點為兩個驅動輪軸的中心，θ為兩個驅動輪軸與x軸的夾角；

設機器人前進的線速度為υ，轉向的角速度為ω，連續(xù)運動學模型表示為：

其中，υ_L和υ_R分別為左、右驅動輪的線速度，L是驅動輪之間的距離；

選取T為采樣周期時間，離散化的運動學模型為：

其中，θ_k-1為機器人前一時刻的角度，ω_k-1為前一時刻的轉向角速度，θ_k為當前時刻角度的計算值；

通過讀取采樣周期內左、右驅動輪的旋轉編碼器脈沖個數N_R和N_L，得到新的角度狀態(tài)表達式：

其中，r為驅動輪半徑，N為編碼器旋轉一周的脈沖個數，w_k為過程噪聲；

依據陀螺儀，讀取當前時刻的角度測量值Z_k：

Z_k＝θ_k+v_k (4)

其中，v_k為陀螺儀的測量噪聲。

3.如權利要求2所述的一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于：所述過程噪聲w_k以及陀螺儀的測量噪聲v_k均為高斯白噪聲。

4.如權利要求2所述的一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于：所述步驟二中，當前k時刻的狀態(tài)θ_k|k-1的表達式為：

式(5)中，θ_k|k-1是利用上一狀態(tài)預測的結果，即當前時刻角度的預測值，θ_k-1|k-1為上一時刻角度的最優(yōu)估計值。

5.如權利要求2所述的一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于：所述步驟三中，所述當前時刻的預測均方誤差P_k|k-1的表達式為：

P_k|k-1＝P_k-1|k-1+Q (6)

式(6)中，P_k-1|k-1為上一時刻的最優(yōu)均方誤差，P_k|k-1為當前時刻的預測均方誤差。

6.如權利要求2所述的一種基于陀螺儀的掃地機器人角度最優(yōu)估算方法，其特征在于：所述步驟四中，所述卡爾曼增益K_k的表達式為：

K_k＝P_k|k-1(P_k|k-1+R)^-1。 (7)。