一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法
【專利摘要】本發(fā)明是一種基于改進擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法。針對現(xiàn)有兩輪自平衡機器人的姿態(tài)解算方法不能很好的滿足精度、實時性、簡便性等要求，本發(fā)明方法利用改進的擴展卡爾曼濾波算法，有效的融合了慣性傳感器姿態(tài)測量數(shù)據(jù)，補償了陀螺儀隨機漂移誤差，減小了兩輪自平衡機器人運動時位移加速度對姿態(tài)解算的影響。該姿態(tài)解算方法同時可以適用于兩輪自平衡電動車。通過兩輪自平衡機器人的靜態(tài)實驗、模擬平臺實驗以及實際動態(tài)實驗，驗證了該方法能提高兩輪自平衡機器人的姿態(tài)解算精度。
【專利說明】一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人 姿態(tài)解算方法
【技術領域】
[0001] 本發(fā)明是一種基于改進擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法。 該發(fā)明適用于兩輪自平衡機器人的姿態(tài)解算，同時也適用于兩輪自平衡電動車。
【背景技術】
[0002] 兩輪自平衡機器人為典型的非完整、非線性、欠驅動系統(tǒng)。為了實現(xiàn)兩輪自平衡機 器人運動過程中的平衡控制，必須具有能實時檢測其姿態(tài)信息的檢測系統(tǒng)，并將姿態(tài)信息 傳遞給控制器，以實現(xiàn)對兩輪自平衡機器人的精確控制。由于組成姿態(tài)檢測系統(tǒng)的慣性傳 感器本身特性，它們受溫度及噪聲的影響很大，當慣性傳感器長時間工作時，誤差將隨著時 間累積導致系統(tǒng)無法正常工作。
[0003] 目前兩輪自平衡機器人姿態(tài)解算方法主要有卡爾曼濾波法、擴展卡爾曼濾波法、 無味卡爾曼濾波法、粒子濾波法?？柭鼮V波法姿態(tài)解算方法建模簡單，實時性較好，但是 忽略了非線性因素尤其是載體位移加速度對姿態(tài)測量信息的影響；擴展卡爾曼濾波法姿態(tài) 解算方法實時性較好，但是線性化損失了部分二次項值，線性化誤差較大。無味卡爾曼濾波 法與粒子濾波精度較高，實時性較差，且不夠簡便。
【發(fā)明內容】
[0004] 本發(fā)明主要解決的技術問題是針對上述【背景技術】的不足，提供了一種基于改進擴 展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法。本發(fā)明為實現(xiàn)上述發(fā)明目的采用如 下技術方案。
[0005] 基于改進擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法，包括如下步 驟：
[0006] 步驟1 :該兩輪自平衡機器人是一種新型的交通工具，采用兩輪并排固定的方式， 由兩個直流無刷電動機驅動，采用姿態(tài)感知系統(tǒng)、控制算法及機械裝置自動協(xié)調控制車體 平衡，無需人來控制平衡。人站在車上只要身體帶動車體一起前傾（或后傾）就可以實現(xiàn) 小車前進（或后退）。該兩輪自平衡機器人利用STM32快速采集三軸陀螺儀三軸加速度計， 采樣頻率IMHZ ;同時，在采樣間隔中，處理姿態(tài)數(shù)據(jù)與輸出控制，頻率為100HZ。俯仰角在運 行中幅度為-10?10。
[0007] 步驟2 :陀螺儀能直接測量兩輪自平衡機器人的姿態(tài)角速度，并通過積分運算獲 得角度信息，但是陀螺儀測量模型存在著相應的漂移誤差，因此，結合測量模型與兩輪自平 衡機器人姿態(tài)測量系統(tǒng)，通過實驗獲得陀螺儀在零輸入狀態(tài)下的誤差漂移特性，根據(jù)數(shù)據(jù) 擬合模型，結合非線性最小二乘法擬合實驗數(shù)據(jù)，建立陀螺儀誤差數(shù)學模型。
[0008] 步驟3 :兩輪自平衡機器人是由控制系統(tǒng)根據(jù)姿態(tài)信息驅動兩輪同軸安裝分別驅 動的電機保持動態(tài)平衡。由于不存在固定轉軸，一般角度或角速度傳感器無法測量，而慣性 測量單元能夠測量轉軸不固定運動系統(tǒng)的角速度和角度。而慣性傳感器三軸陀螺儀三軸加 速度能夠組成低成本的捷聯(lián)式慣導姿態(tài)檢測系統(tǒng)。并根據(jù)兩輪自平衡機器人姿態(tài)測量系統(tǒng) 建立MEMS陀螺儀加速度計姿態(tài)解算數(shù)學模型。
[0009] 步驟4 :由于慣性傳感器自身的性能和特點，單獨使用陀螺儀或加速度計等慣性 測量元件解算兩輪自平衡機器人的姿態(tài)，都會存在嚴重的誤差問題，因此利用MEMS慣性傳 感器誤差模型和兩輪自平衡機器人姿態(tài)模型構建擴展卡爾曼濾波器，對系統(tǒng)姿態(tài)測量數(shù)據(jù) 進行數(shù)據(jù)融合，抑制噪聲干擾，補償誤差。根據(jù)非線性系統(tǒng)的微分數(shù)學模型：
【權利要求】
1. 一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法，其特征在 于它的步驟如下： (1) 利用STM32交互采集處理兩輪自平衡機器人慣性姿態(tài)測量數(shù)據(jù)； (2) 利用兩輪自平衡機器人姿態(tài)測量系統(tǒng)建立MEMS陀螺儀誤差數(shù)學模型； (3) 利用兩輪自平衡機器人姿態(tài)測量系統(tǒng)建立MEMS MU姿態(tài)解算數(shù)學模型； (4) 利用MEMS IMU姿態(tài)解算數(shù)學模型構建擴展卡爾曼濾波器模型； (5) 利用改進的擴展卡爾曼濾波算法對兩輪自平衡機器人姿態(tài)測量數(shù)據(jù)進行數(shù)據(jù)融 合； (6) 利用二階近似解算減少非線性函數(shù)非線性截斷誤差； (7) 利用位移加速度判斷公式，自適應量測噪聲矩陣對兩輪自平衡機器人姿態(tài)進行補 償； (8) 利用兩步量測更新的方法提高改進的擴展卡爾曼濾波的先驗解算精度。
2. 根據(jù)權利要求1所述的一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人 姿態(tài)解算方法，其特征在于，所述步驟（4)為：利用MEMS慣性傳感器誤差模型和兩輪自平衡 機器人姿態(tài)模型構建擴展卡爾曼濾波器： 系統(tǒng)狀態(tài)向量：
系統(tǒng)量測向量：
3. 根據(jù)權利要求1所述的一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人 姿態(tài)解算方法，其特征在于，所述步驟（5)為：利用改進的擴展卡爾曼濾波算法對兩輪自平 衡機器人姿態(tài)測量數(shù)據(jù)進行數(shù)據(jù)融合。該改進的擴展卡爾曼濾波算法為兩階兩步量測的自 適應擴展卡爾曼濾波算法。其步驟為： 1)時間更新：分別得到狀態(tài)矩陣非線性部分的Jacobian和Hessian矩陣；狀態(tài)矩陣離 散化；對狀態(tài)量與過程協(xié)方差進行二階時間更新。 2) 量測更新第一步：對陀螺儀測量數(shù)據(jù)進行量測更新，得到更新后的狀態(tài)量與協(xié)方差 量。 3) 自適應加速度計量測噪聲矩陣：根據(jù)加速度計測量值判斷是否具有外部加速度，同 時修正加速度計量測噪聲矩陣。 4) 量測更新第二步：分別得到量測矩陣非線性部分的Jacobian和Hessian矩陣；對加 速度進行二階量測更新。
4.根據(jù)權利要求1所述的一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人 姿態(tài)解算方法，其特征在于，所述步驟（7)為：利用以下方程考慮兩輪自平衡機器人是否存 在非重力載體位移加速度： f(ax，ay，az) = I I |fa,k| |2_g| < 5 式中，S與加速度計的噪聲有關。如果上式滿足，則表明兩輪自平衡機器人靜止或保 持穩(wěn)定狀態(tài)，此時加速度計只受到重力加速度的作用，自適應量測噪聲矩陣Rk, a:
否則：存在著非重力載體位移加速度的影響。
【文檔編號】G01C21/20GK104316055SQ201410489632
【公開日】2015年1月28日 申請日期:2014年9月19日 優(yōu)先權日:2014年9月19日
【發(fā)明者】周翟和, 胡佳佳, 虞波, 沈超, 趙慶濤 申請人:南京航空航天大學
山東科威數(shù)控機床有限公司銑床官方網(wǎng)站今天是：2024-10-23切換城市[全國]-網(wǎng)站地圖
專業(yè)數(shù)控銑床產品供應商
20年生產經驗，實力工廠，精工品質，質量有保障
