[發明專利]一種機器人移動控制方法、系統及慣性傳感控制裝置

有效

• 申請號: 201711232485.7
• 申請日: 2017-11-30
• 公開（公告）號: CN108051001B
• 公開（公告）日: 2020-09-04
• 發明（設計）人: 金學波;項娜;楊念香;孔建磊;蘇婷立
• 申請（專利權）人: 北京工商大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 北京科家知識產權代理事務所(普通合伙) 11427
• 代理人: 戴麗偉

1.一種機器人移動控制方法，其特征在于，包括：

S1，獲取慣性傳感器的角速度數據；

S2，對所述角速度數據進行在線自適應濾波的預處理；

所述S2包括：

S2.1，初始化狀態量和系統自適應參數，所述狀態量包括角速度、角速度一階導數和角速度二階導數；

S2.2，建立具有系統自適應參數的狀態過程模型；

S2.3，根據建立的具有系統自適應參數的狀態過程模型對所述狀態量進行預測，獲取狀態預測值以及狀態協方差預測值；

S2.4，根據所述狀態預測值、測量數據值和狀態協方差預測值對所述狀態量進行更新，獲取狀態估計值；

S2.5，根據所述狀態估計值計算角速度二階導數均值及角速度二階導數估計值；

利用下式計算角速度二階導數均值；

其中為0至k時刻角速度二階導數均值，為k時刻的狀態估計值的第三行值，k為采樣時刻；并按照下式獲取系統k-1時刻和k時刻的角速度二階導數估計值

其中為k-1時刻狀態估計的第三行值，為k時刻狀態估計的第三行值；

S2.6，根據所述角速度二階導數估計值對所述系統自適應參數進行修正；

根據采樣時刻k值的大小，選擇修正系統自適應參數α和的方法，若k小于等于4進入步驟2.6.1，若k大于4進入步驟2.6.2，

2.6.1當采樣時刻k小于等于4時，因為采樣數據較少，采用當前統計模型的參數取值方法，按如下方式計算系統自適應參數α和

α＝α₀其中α₀為系統自適應參數α的初值，

如果則取

如果則取

如果則取(0,10]之間的任意數，

其中，為k時刻角速度二階導數估計值，π為圓周率，取為3.14，a_M為正的常數，取為3，a_-M為與a_M絕對值相等的負常數，取為-3；

2.6.2當采樣時刻k大于4時，按下式計算系統自適應參數α和

其中b是大于1的常數，r_k(1)為k時刻角速度二階導數向前一步相關函數，r_k-1(1)為k-1時刻角速度二階導數向前一步相關函數，和分別為k-1時刻和k時刻角速度二階導數估計值；r_k(0)為k-1時刻角速度二階導數自相關函數，r_k-1(0)為k-1時刻角速度二階導數自相關函數；

自適應參數α和可按照下式計算得到：

其中，r_k(1)為k時刻的加速度向前一步相關函數，r_k(0)為k時刻的加速度自相關函數，ln為取以e為底的對數計算；α和為系統自適應參數，T為采樣間隔；

S2.7，根據所述角速度二階導數均值和所述修正后的系統自適應參數更新所述狀態過程模型，獲取在線自適應濾波后的角速度數據；

S3，根據經在線自適應濾波預處理的所述角速度數據建立四元數微分方程，利用龍格-庫塔法求解所述四元數微分方程，獲取包括目標姿態角的姿態矩陣；

S4，將目標姿態角從載體坐標系轉換為導航坐標系；

S5，將所述導航坐標系中的目標姿態角中不在閾值范圍內的目標姿態角排除；

S6，根據閾值范圍內的目標姿態角控制機器人動作。

2.根據權利要求1所述的方法，其特征在于，所述S3包括：

S3.1，利用濾波后的角速度數據和四元數建立四元數微分方程；

S3.2，利用四階龍格-庫塔法求解所述四元數微分方程，獲取由四元數描述的姿態矩陣，通過更新四元數的元值從而更新姿態矩陣，更新目標姿態角；

其中，利用四階龍格-庫塔法求解所述四元數微分方程所需的斜率初始值由四元數的初始值確定，所述四元數的初始值由目標姿態角的初始值確定。

3.根據權利要求2所述的方法，其特征在于，所述利用龍格-庫塔法求解所述四元數微分方程，獲取由四元數描述的姿態矩陣如下：

其中，K為斜率，t為當前時刻，h為更新的步長，式中為三軸角速度的矩陣表達式，q(t)表示四元數。