一種基于卡爾曼濾波的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于卡爾曼濾波的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定方法，該方法通過(guò)建立機(jī)器人的運(yùn)動(dòng)學(xué)模型得到機(jī)器人末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系的位姿變換矩陣。通過(guò)測(cè)量?jī)x器可以得到機(jī)器人末端實(shí)際位姿矩陣P。再通過(guò)微分運(yùn)動(dòng)學(xué)將末端位姿矩陣全微分可得到機(jī)器人末端位姿誤差與機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差間的線性關(guān)系?？紤]到系統(tǒng)中存在噪聲對(duì)標(biāo)定結(jié)果的影響，利用卡爾曼濾波算法對(duì)運(yùn)動(dòng)學(xué)參數(shù)進(jìn)行卡爾曼濾波估計(jì)。該方法考慮了系統(tǒng)噪聲的影響，標(biāo)定結(jié)果更準(zhǔn)確能使機(jī)器人達(dá)到更高的定位精度。
【專(zhuān)利說(shuō)明】
一種基于卡爾曼濾波的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定的方法，尤其涉及一種基于卡爾曼濾波的機(jī)器 人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定方法。
【背景技術(shù)】
[0002] 在工業(yè)生產(chǎn)、醫(yī)療、航空航天等相關(guān)領(lǐng)域，機(jī)器人扮演著越來(lái)越重要的角色。在完 成某些高精度作業(yè)時(shí)，對(duì)機(jī)器人的末端定位精度要求很高。而由于生產(chǎn)加工、裝配過(guò)程中誤 差，關(guān)節(jié)傳動(dòng)間隙，機(jī)器人連桿變形等多種因數(shù)會(huì)造成機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)與理論設(shè)計(jì)值之 間存在偏差。
[0003] 機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)通常指D-Η參數(shù)，它包含關(guān)節(jié)軸之間的夾角α、連桿長(zhǎng)度a、連桿 偏距d、關(guān)節(jié)轉(zhuǎn)角Θ四個(gè)參數(shù)。
[0004] 機(jī)器人末端位姿通過(guò)機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)計(jì)算而得，要想提高機(jī)器人末端定位精 度，就要對(duì)機(jī)器人運(yùn)動(dòng)學(xué)這四個(gè)參數(shù)進(jìn)行精確標(biāo)定。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明所涉及的機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定的方法，通過(guò)建立機(jī)器人的運(yùn)動(dòng)學(xué)模型得到機(jī) 器人末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系的位姿變換矩陣。通過(guò)測(cè)量?jī)x器可以得到機(jī)器人末 端實(shí)際位姿矩陣P。再通過(guò)微分運(yùn)動(dòng)學(xué)將末端位姿矩陣全微分可得到機(jī)器人末端位姿誤差 與機(jī)器人運(yùn)動(dòng)學(xué)誤差間的線性關(guān)系?？紤]到系統(tǒng)中存在噪聲對(duì)標(biāo)定結(jié)果的影響，利用卡爾 曼濾波算法對(duì)運(yùn)動(dòng)學(xué)參數(shù)進(jìn)行卡爾曼濾波估計(jì)。該方法考慮了系統(tǒng)噪聲的影響，標(biāo)定結(jié)果 更準(zhǔn)確能使機(jī)器人達(dá)到更高的定位精度。
[0006] -種基于卡爾曼濾波的機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定方法，其特征在于：
[0007] 第一、建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，得到機(jī)器人末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系 的位姿轉(zhuǎn)換矩陣。
[0008] 第二、通過(guò)測(cè)量?jī)x器得到機(jī)器人末端實(shí)際位姿矩陣P。
[0009] 第三、通過(guò)微分運(yùn)動(dòng)學(xué)將機(jī)器人末端實(shí)際位姿矩陣P全微分得到機(jī)器人末端位姿 誤差與機(jī)器人運(yùn)動(dòng)學(xué)誤差間的線性關(guān)系，即運(yùn)動(dòng)學(xué)線性誤差模型:超定線性方程組J*X=W， 其中J為機(jī)器人運(yùn)動(dòng)學(xué)誤差矩陣，X為待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣，w為末端位姿誤差矩 陣;利用所述運(yùn)動(dòng)學(xué)線性誤差模型計(jì)算待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X。
[0010] 第四、將辨識(shí)出的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X作為卡爾曼濾波的初始狀態(tài)值，在模型中 加入噪聲后，利用卡爾曼濾波估計(jì)算法估計(jì)出最佳的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣x 2。
[0011] 最后補(bǔ)償機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差，達(dá)到提高機(jī)器人末端絕對(duì)定位精度的目的。
[0012] 同現(xiàn)有技術(shù)相比，本發(fā)明的技術(shù)方案具有以下優(yōu)點(diǎn):本發(fā)明的所述方法，考慮了系 統(tǒng)噪聲對(duì)標(biāo)定結(jié)果的影響，使最終的標(biāo)定精度更高，簡(jiǎn)易可行，成本低，可廣泛用于機(jī)器人 系統(tǒng)的標(biāo)定。
【附圖說(shuō)明】
[0013] 圖1是本發(fā)明實(shí)施方式的流程圖。
【具體實(shí)施方式】
[0014] 如圖1所示，本發(fā)明實(shí)施方式的流程圖，具有如下步驟：
[0015] 步驟一，建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，得到機(jī)器人末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo) 系的位姿轉(zhuǎn)換矩陣。具體又包括如下步驟：
[0016] 定義機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)D-H，編寫(xiě)相鄰關(guān)節(jié)間齊次變換矩陣，其中，所述參數(shù)D-Η包 含關(guān)節(jié)軸之間的夾角α、連桿長(zhǎng)度a、連桿偏距d、關(guān)節(jié)轉(zhuǎn)角Θ等四個(gè)參數(shù);給定名義D-Η參數(shù)名 義值 ai，ai，di，0i;
[0017] 建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，其中關(guān)于關(guān)節(jié)轉(zhuǎn)角Θ,的機(jī)器人運(yùn)動(dòng)學(xué)方程和位姿轉(zhuǎn)換 矩陣分別為：
[0020] 其中，機(jī)器人運(yùn)動(dòng)學(xué)誤差方程為：
[0021] J = f(ai,ai,di,0i)
[0022] 其中，超定線性方程組為： Am
[0023] W Aa =,/"Λ' Μ ?θ L J ο
[0024] 步驟二，輸入關(guān)節(jié)轉(zhuǎn)角0,和通過(guò)測(cè)量?jī)x器得到的機(jī)器人末端實(shí)際位姿矩陣Ρ。
[0025] 步驟三，通過(guò)微分運(yùn)動(dòng)學(xué)將機(jī)器人末端實(shí)際位姿矩陣Ρ全微分得到機(jī)器人末端位 姿誤差與機(jī)器人運(yùn)動(dòng)學(xué)誤差間的線性關(guān)系，即運(yùn)動(dòng)學(xué)線性誤差模型:超定線性方程組J*X = W，其中J為機(jī)器人運(yùn)動(dòng)學(xué)誤差矩陣，X為待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣，W為末端位姿誤差矩 陣;利用所述運(yùn)動(dòng)學(xué)線性誤差模型計(jì)算待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X。在該步驟中，通過(guò) 最小二乘法求解超定線性方程組J*X=W。
[0026] 步驟四，將辨識(shí)出的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X作為卡爾曼濾波的初始狀態(tài)值，在系統(tǒng) 模型中加入噪聲后，利用卡爾曼濾波估計(jì)算法估計(jì)出最佳的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣 X 2。具體又包括如下步驟：
[0027] (4.1)將運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X作為初始值，根據(jù)狀態(tài)方程X(k | k-1) =X(k_l | k-Ι) 預(yù)測(cè)K時(shí)刻的狀態(tài)；
[0028] (4.2)根據(jù)系統(tǒng)Κ-l時(shí)刻的協(xié)方差估計(jì)第Κ時(shí)刻的系統(tǒng)協(xié)方差Ρ(Κ I K-1);
[0029] (4.3)計(jì)算卡爾曼增益Kg;
[0030] (4.4)估計(jì)系統(tǒng)1(時(shí)刻最優(yōu)運(yùn)動(dòng)學(xué)參數(shù)乂(1(|1〇;
[0031] (4.5)計(jì)算當(dāng)前系統(tǒng)協(xié)方差矩陣P(K IK);
[0032] (4.6)驗(yàn)證誤差是否收斂，若是，則進(jìn)行下一步;若否，則返回步驟(4.1);
[0033] (4.7)輸出機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X2。
[0034] 最后補(bǔ)償機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差，達(dá)到提高機(jī)器人末端絕對(duì)定位精度的目的。
[0035] 以上應(yīng)用了具體個(gè)例對(duì)本發(fā)明的原理及實(shí)施方式進(jìn)行了闡述，以上實(shí)施例的說(shuō)明 只是用于幫助理解本發(fā)明的方法及其核心思想；同時(shí)，對(duì)于本領(lǐng)域的一般技術(shù)人員，依據(jù)本 發(fā)明的思想，在【具體實(shí)施方式】及應(yīng)用范圍上均會(huì)有改變之處，綜上所述，本說(shuō)明書(shū)內(nèi)容不應(yīng) 理解為對(duì)本發(fā)明的限制。
【主權(quán)項(xiàng)】
1. 一種基于卡爾曼濾波的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定方法，其特征在于，包括如下步驟： (1) 建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，得到機(jī)器人末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系的位姿 轉(zhuǎn)換矩陣； (2) 通過(guò)測(cè)量?jī)x器得到機(jī)器人末端實(shí)際位姿矩陣P; (3) 通過(guò)微分運(yùn)動(dòng)學(xué)將機(jī)器人末端實(shí)際位姿矩陣P全微分得到機(jī)器人末端位姿誤差與 機(jī)器人運(yùn)動(dòng)學(xué)誤差間的線性關(guān)系，即運(yùn)動(dòng)學(xué)線性誤差模型:超定線性方程組J*X=W，其中J 為機(jī)器人運(yùn)動(dòng)學(xué)誤差矩陣，X為待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣，W為末端位姿誤差矩陣;利用 所述運(yùn)動(dòng)學(xué)線性誤差模型計(jì)算待辨識(shí)的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X; (4) 將辨識(shí)出的運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X作為卡爾曼濾波的初始狀態(tài)值，在模型中加入噪 聲后，利用卡爾曼濾波估計(jì)算法估計(jì)出最佳的機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣?yán)?. 如權(quán)利要求1的所述方法，其特征在于，所述步驟(1)又包括W下步驟： 定義機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)D-H，編寫(xiě)相鄰關(guān)節(jié)間齊次變換矩陣，其中，所述參數(shù)D-H包含關(guān) 節(jié)軸之間的夾角α、連桿長(zhǎng)度a、連桿偏距d、關(guān)節(jié)轉(zhuǎn)角Θ等四個(gè)參數(shù);給定名義D-H參數(shù)名義值 曰i , Eli , di ,白 i ; 建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，其中關(guān)于關(guān)節(jié)轉(zhuǎn)角θι的機(jī)器人運(yùn)動(dòng)學(xué)方程和位姿轉(zhuǎn)換矩陣 分別為：3. 如權(quán)利要求2的所述方法，其特征在于，所述步驟(2)還包括輸入關(guān)節(jié)轉(zhuǎn)角θι的步驟。4. 如權(quán)利要求1至3中任一所述方法，其特征在于，在所述步驟(3)中，通過(guò)最小二乘法 求解所述超定線性方程組J巧二W。5. 如權(quán)利要求4的所述方法，其特征在于，所述步驟(4)又包括W下步驟： (4.1) 將運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣X作為初始值，根據(jù)狀態(tài)方程X(k I k-1) =Χ化-11 k-1)預(yù)測(cè) Κ時(shí)刻的狀態(tài)； (4.2) 根據(jù)系統(tǒng)Κ-1時(shí)刻的協(xié)方差估計(jì)第拙寸刻的系統(tǒng)協(xié)方差Ρ化IΚ-1); (4.3) 計(jì)算卡爾曼增益Kg; (4.4) 估計(jì)系統(tǒng)拙寸刻最優(yōu)運(yùn)動(dòng)學(xué)參數(shù)X化IK); (4.5) 計(jì)算當(dāng)前系統(tǒng)協(xié)方差矩陣P化IK); (4.6) 驗(yàn)證誤差是否收斂，若是，則進(jìn)行下一步;若否，則返回步驟(4.1); (4.7) 輸出所述機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)誤差矩陣?yán)?br>【文檔編號(hào)】G06T7/20GK106097390SQ201610411182
【公開(kāi)日】2016年11月9日
【申請(qǐng)日】2016年6月13日 公開(kāi)號(hào)201610411182.0, CN 106097390 A, CN 106097390A, CN 201610411182, CN-A-106097390, CN106097390 A, CN106097390A, CN201610411182, CN201610411182.0
【發(fā)明人】蔣志宏, 周偉剛, 莫洋, 李輝, 黃強(qiáng)
【申請(qǐng)人】北京理工大學(xué)