[發明專利]多體機械系統的動態定位方法

有效

• 申請號: 201810376805.4
• 申請日: 2018-04-25
• 公開（公告）號: CN108717298B
• 公開（公告）日: 2020-10-20
• 發明（設計）人: 趙祚喜;馮榮;可欣榮;宋俊文;張丁山
• 申請（專利權）人: 華南農業大學
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10
• 代理公司: 廣州市華學知識產權代理有限公司 44245
• 代理人: 李君

本發明公開了一種多體機械系統的動態定位方法，所述方法假定多體機械系統興趣點在X軸、Y軸和Z軸三個方向上是相互獨立的，構建每個方向上解耦獨立運行的卡爾曼濾波器，利用圖像傳感器和慣性導航傳感器的融合實現多體機械系統興趣點的位置估計。本發明實現了多體機械系統點的連續三維定位，且定位精度不受短時間遮擋的影響，這為剛體的質心位置測量和姿態角測量以及模型驗證提供了基礎。

技術領域

本發明涉及一種動態定位方法，尤其是多體機械系統的動態定位方法，屬于多體機械系統動力學領域。

背景技術

多體機械系統都可看成包含不同數量的體和用于連接各體的不同類型的關節組成的系統，各體表面興趣點的動態定位對剛體質心位置及姿態角的求解以及對機械系統的模型驗證非常必要。

目前，多體機械系統興趣點動態三維位置的測量手段有雙目視覺定位和慣導系統定位，雙目圖像測量具有測量精度高、非接觸測量、同時實現多點測量等優點，但運動的機械系統容易導致標記點遮擋及測量視場小等缺點，這些情況出現時則雙目定位方法失效，而慣性導航系統(INS)體積小安裝方便，通過測量點的加速度和角速度來定位，定位不受遮擋影響，但存在慣性傳感器零偏引起的隨時間增長的加速度漂移，這種漂移一般隨時間增長而增大因而不宜長時間依賴其定位。

發明內容

本發明的目的是為了解決上述現有技術的缺陷，提供了一種多體機械系統的動態定位方法，該方法實現了多體機械系統點的連續三維定位，且定位精度不受短時間遮擋的影響，這為剛體的質心位置測量和姿態角測量以及模型驗證提供了基礎。

本發明的目的可以通過采取如下技術方案達到：

多體機械系統的動態定位方法，所述方法假定多體機械系統興趣點在X軸、Y軸和Z軸三個方向上是相互獨立的，構建每個方向上解耦獨立運行的卡爾曼濾波器，利用圖像傳感器和慣性導航傳感器的融合實現多體機械系統興趣點的位置估計。

進一步的，所述構建每個方向上解耦獨立運行的卡爾曼濾波器，具體包括：

構建第一卡爾曼濾波器的過程模型和測量模型，所述測量模型為圖像傳感器與慣性導航傳感器的輸出結果之差，第一卡爾曼濾波器實時估計慣性導航傳感器加速度零偏；

構建第二卡爾曼濾波器的過程模型和測量模型，所述測量模型包括圖像傳感器模型和慣性導航傳感器模型，在圖像傳感器有效時，第二卡爾曼濾波器按照圖像傳感器模型的濾波結果輸出，否則，第二卡爾曼濾波器按照慣性導航傳感器模型的濾波結果輸出。

進一步的，所述第一卡爾曼濾波器的過程模型由0階的狀態變量b的多項式表示；其中，b表示慣性導航傳感器加速度零偏。

進一步的，所述第二卡爾曼濾波器的過程模型由2階的三個狀態變量的多項式表示，如下：

其中，x表示位移，表示速度，表示加速度；u_s表示過程模型不準確度的過程噪音，其均值為0，均方差為Φ_s。

進一步的，所述圖像傳感器模型和慣性導航傳感器模型，分別如下式：

Z_IMG＝x+V_IMG

Z_INS＝x+0.5bt²

其中，Z_IMG為圖像傳感器的測量結果；Z_INS為慣性導航傳感器的測量結果；V_IMG為圖像測量的噪音，均值為0，協方差矩陣為R；b為慣性導航傳感器加速度零偏；t為時間。

進一步的，所述圖像傳感器有效是指圖像傳感器所采集的興趣點未被遮擋。