[發明專利]一種電驅動哺乳構型足式機器人足地滑移估計方法

在審

• 申請號: 202110646523.3
• 申請日: 2021-06-10
• 公開（公告）號: CN113405548A
• 公開（公告）日: 2021-09-17
• 發明（設計）人: 劉宇飛;高海波;丁亮;王志瑞;邢伯陽;江磊;蘇波
• 申請（專利權）人: 中國北方車輛研究所;智能移動機器人（中山）研究院
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G06F17/16
• 代理公司: 中國兵器工業集團公司專利中心 11011
• 代理人: 王雪芬

1.一種電驅動哺乳構型足式機器人足地滑移估計方法，其特征在于，包括以下步驟：

步驟一、設計哺乳構型足式機器人本體傳感數據解算方法，得到的機器人在導航坐標系中的加速度；

步驟二、將步驟一驗證得到的機器人在導航坐標系中的加速度，作為基于擴展卡爾曼濾波的哺乳構型足式機器人狀態估計框架的輸入變量，從而建立基于擴展卡爾曼濾波的哺乳構型足式機器人狀態估計框架，得到哺乳構型足式機器人位置、速度估計變量；

步驟三、將步驟二得到的哺乳構型足式機器人位置、速度估計變量，用于作為單腿動力學方程的輸入變量，建立哺乳構型足式機器人足地滑移估計模型，得到哺乳構型足式機器人足地滑移加速度模型；

步驟四、利用步驟三得到的哺乳構型足式機器人足地滑移加速度模型，建立基于擴展卡爾曼濾波的哺乳構型足地滑移狀態估計方法；

步驟五、基于步驟一至步驟四進行哺乳構型足式機器人的足地滑移檢測。

2.如權利要求1所述的方法，其特征在于，步驟一中，對慣導等傳感器的數據進行標定及補償，通過標定加速度及角加速度信號來抑制哺乳構型牽引力運動方向速度和姿態解算的誤差，最終輸出準確的哺乳構型機器人牽引力方向本體速度及三維姿態信息，慣導傳感實時數據具體解算結果為：

式中，為導航坐標系到機體坐標系的轉換矩陣，f_B為空間中的真實加速度和重力導致的加速度之差的力矢量，g_n為重力矢量，為地球自轉角度在導航坐標系中的投影，為機器人在導航坐標系中的速度，v_n為機器人在導航坐標系中的速度。

3.如權利要求2所述的方法，其特征在于，步驟二中，將步驟一驗證得到的機器人在導航坐標系中的加速度作為基于擴展卡爾曼濾波的哺乳構型足式機器人狀態估計框架的輸入變量從而建立基于擴展卡爾曼濾波的哺乳構型足式機器人狀態估計框架。

4.如權利要求3所述的方法，其特征在于，步驟二中，根據足式機器人單腿及質心狀態變量，建立狀態估計過程中的預測模型，其具體建模結果為：

式中，f為三維方向的加速度真實值；b_f為加速度計的偏差；b_w為角加速度計的偏差；w_f為加速度計的隨機誤差；w_w為角加速度計的隨機誤差；w為三維方向的角速率真實值；為三維方向的加速度測量值；為三維方向的角速率測量值；w_bf為加速度計噪聲矩陣；w_bw為角加速度計噪聲矩陣。

建立狀態估計過程中的測量模型，其具體建模結果為：

式中，p_c為機器人的位置修正量；v_c為機體的速度修正量；p為機器人的位置測量變量；v為機體的速度測量變量。

根據狀態變量的預測更新和測量更新，基于反饋校正方法得到足式機器人的狀態變量結果如下所示：

式中，為位置估計值，為速度估計值，Δt為時間周期變量；為導航坐標系轉換矩陣。

5.如權利要求4所述的方法，其特征在于，步驟三中，將步驟二公式(4)計算得到的哺乳構型足式機器人位置、速度估計變量，用于作為單腿動力學方程的輸入變量，建立哺乳構型足式機器人足地滑移估計模型。