[發(fā)明專利]一種用于雙移動(dòng)機(jī)器人剛體協(xié)作搬運(yùn)的協(xié)同定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201911154494.8
• 申請(qǐng)日: 2019-11-22
• 公開(kāi)（公告）號(hào): CN110967017B
• 公開(kāi)（公告）日: 2023-03-31
• 發(fā)明（設(shè)計(jì)）人: 劉劍鋒;普杰信;孫力帆;張松燦;司彥娜;王燕玲
• 申請(qǐng)（專利權(quán)）人: 河南科技大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C1/00
• 代理公司: 洛陽(yáng)公信知識(shí)產(chǎn)權(quán)事務(wù)所(普通合伙) 41120
• 代理人: 王學(xué)鵬

1.一種用于雙移動(dòng)機(jī)器人剛體協(xié)作搬運(yùn)的協(xié)同定位方法，其特征在于，包括以下步驟：

S1、構(gòu)建基于剛性約束的雙機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)模型和量測(cè)模型；

S2、初始化高斯-厄米特求積分卡爾曼濾波器；

S3、基于運(yùn)動(dòng)學(xué)模型對(duì)雙機(jī)器人系統(tǒng)進(jìn)行狀態(tài)推演，得到剛體狀態(tài)預(yù)測(cè)值；

S4、獲取雙機(jī)器人系統(tǒng)的狀態(tài)量測(cè)值，并且將狀態(tài)量測(cè)值與量測(cè)模型相結(jié)合計(jì)算卡爾曼增益；

S5、利用卡爾曼增益對(duì)剛體狀態(tài)預(yù)測(cè)值進(jìn)行更新后得到剛體狀態(tài)估計(jì)值；

S6、將更新后的剛體狀態(tài)估計(jì)值轉(zhuǎn)換為雙機(jī)器人系統(tǒng)中的兩個(gè)移動(dòng)機(jī)器人的位姿狀態(tài)；

所述的步驟S1具體包括以下步驟：

S1.1、設(shè)定剛體狀態(tài)向量并對(duì)剛體狀態(tài)向量賦初值，剛體狀態(tài)向量記為表示所搬運(yùn)剛體的位置，其中(x_c,y_c)為兩個(gè)移動(dòng)機(jī)器人的幾何中心連線中點(diǎn)在導(dǎo)航坐標(biāo)系下的坐標(biāo)；/為(x_c,y_c)到其中一個(gè)移動(dòng)機(jī)器人R_i幾何中心(x_i,y_i)的延長(zhǎng)線與導(dǎo)航坐標(biāo)系橫軸X_Nav正半軸的夾角，表示剛體的姿態(tài)；θ為移動(dòng)機(jī)器人的朝向與X_Nav正半軸的夾角，表示剛體的運(yùn)動(dòng)方向；

S1.2、基于剛體狀態(tài)向量構(gòu)建帶有剛性約束的雙機(jī)器人系統(tǒng)離散運(yùn)動(dòng)方程，并得到相應(yīng)的運(yùn)動(dòng)學(xué)模型：

X_k+1＝f(X_k,u_k,w_k)＝X_k+T·Φ_k(u_k+w_k)；

其中f(·)為非線性狀態(tài)轉(zhuǎn)移方程；u_k＝[v_i,k,v_j,k,ω_k]^T為控制向量；v_i,k,v_j,k分別表示兩個(gè)移動(dòng)機(jī)器人R_i和R_j的線速度，ω_k表示兩個(gè)移動(dòng)機(jī)器人共同的角速度，w_k＝[δv_i,k,δv_j,k,δω_k]^T為過(guò)程噪聲并且服從高斯分布N(0,Q)，為過(guò)程噪聲方差陣，k表示時(shí)刻，X_k為k時(shí)刻的剛體狀態(tài)，T為采樣時(shí)間，L為剛體約束距離；

S1.3、構(gòu)建絕對(duì)量測(cè)與相對(duì)量測(cè)相聯(lián)合的量測(cè)模型：

其中Z_k為k時(shí)刻的量測(cè)向量，h(·)為非線性量測(cè)方程，為移動(dòng)機(jī)器人R_i的絕對(duì)位置量測(cè)值，γ_i,k,γ_j,k分別是移動(dòng)機(jī)器人R_i和R_j的旋轉(zhuǎn)機(jī)構(gòu)角度傳感器的測(cè)量值，v_k～N(0,R)為量測(cè)噪聲，/為量測(cè)噪聲方差陣，/為兩機(jī)器人所搬運(yùn)剛體的姿態(tài)角；

所述的步驟S2具體包括以下步驟：

S2.1、初始化剛體狀態(tài)向量及其協(xié)方差矩陣：

其中，X₀為剛體狀態(tài)向量的初值，E(·)為求期望操作；

S2.2、將高斯-厄米特求積分卡爾曼濾波器的積分點(diǎn)個(gè)數(shù)記為m，計(jì)算厄米特多項(xiàng)式H_m(λ)的m個(gè)根

S2.3、計(jì)算對(duì)應(yīng)的單變量配置高斯積分點(diǎn)ξ_i及其系數(shù)a_i：

S2.4、通過(guò)克羅內(nèi)克張量積將單變量配置擴(kuò)展得到n維狀態(tài)，得到n維變量的高斯積分點(diǎn)配置向量及系數(shù)/其中l(wèi)＝mⁿ，/

所述的步驟S3具體包括以下步驟：

/

其中，S_k-1|k-1為對(duì)k-1時(shí)刻的協(xié)方差矩陣P_k-1|k-1進(jìn)行Cholesky分解得到的下三角矩陣，X_i表示剛體狀態(tài)向量的第i個(gè)積分點(diǎn)。