一種慣導(dǎo)和角度傳感器融合的蛇形機(jī)器人姿態(tài)估計(jì)方法

基本信息

申請?zhí)?/td> CN201811032821.8 申請日 -
公開（公告）號 CN109282810A 公開（公告）日 2019-01-29
申請公布號 CN109282810A 申請公布日 2019-01-29
分類號 G01C21/16;G01C21/00 分類 測量；測試;
發(fā)明人 程龍;邴振山;黃凱 申請（專利權(quán)）人 廣州市蚺靈科技有限公司
代理機(jī)構(gòu) 廣州圣理華知識產(chǎn)權(quán)代理有限公司 代理人 頓海舟
地址 511486 廣東省廣州市番禺區(qū)橋南街南庭路274號
法律狀態(tài) -

摘要

摘要 本發(fā)明公開了一種慣導(dǎo)和角度傳感器融合的蛇形機(jī)器人姿態(tài)估計(jì)方法，該方法包括以下步驟：S1、獲取蛇形機(jī)器人關(guān)節(jié)模塊的角度值，通過D?H分析法對關(guān)節(jié)模塊進(jìn)行建模；S2、利用SVD分解法建立虛擬坐標(biāo)系，將關(guān)節(jié)模塊模型轉(zhuǎn)移到虛擬坐標(biāo)系中；S3、獲取蛇形機(jī)器人關(guān)節(jié)模塊的慣導(dǎo)信息，建立蛇形機(jī)器人關(guān)節(jié)模塊運(yùn)動學(xué)模型并根據(jù)擴(kuò)展卡爾曼濾波算法建立關(guān)節(jié)模塊的預(yù)測方程和觀測方程；S4、利用擴(kuò)展卡爾曼濾波算法，估算蛇形機(jī)器人的角速度，加速度，速度以及虛擬坐標(biāo)系的姿態(tài)四元素；S5、根據(jù)姿態(tài)四元素利用MATLAB建立蛇形機(jī)器人動畫模型。本發(fā)明可以精確地計(jì)算出蛇形機(jī)器人每個關(guān)節(jié)地運(yùn)行情況，提高系統(tǒng)模型的準(zhǔn)確性，可以更加直觀地觀察機(jī)器人的運(yùn)動情況。