[發(fā)明專利]一種輪式移動(dòng)舞臺(tái)機(jī)器人在線優(yōu)化鎮(zhèn)定控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201710467447.3
• 申請(qǐng)日: 2017-06-20
• 公開(kāi)（公告）號(hào): CN107168064B
• 公開(kāi)（公告）日: 2020-08-04
• 發(fā)明（設(shè)計(jì)）人: 何德峰;姬超超;倪洪杰;郭曉慧
• 申請(qǐng)（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號(hào): G05B13/04
• 分類號(hào): G05B13/04
• 代理公司: 杭州斯可睿專利事務(wù)所有限公司 33241
• 代理人: 王利強(qiáng)

1.一種輪式移動(dòng)舞臺(tái)機(jī)器人在線優(yōu)化鎮(zhèn)定控制方法，其特征在于：所述控制方法包括如下步驟：

1)、建立輪式移動(dòng)舞臺(tái)機(jī)器人移動(dòng)過(guò)程的離散時(shí)間動(dòng)力學(xué)模型，參見(jiàn)式(1)：

其中，正常數(shù)T_s表示采樣周期，變量k表示采樣時(shí)刻；x₁(k)和x₂(k)分別表示在k時(shí)刻輪式移動(dòng)舞臺(tái)機(jī)器人在舞臺(tái)直角坐標(biāo)系中X方向和Y方向的位置坐標(biāo)；x₃(k)表示在k時(shí)刻輪式移動(dòng)舞臺(tái)機(jī)器人在直角坐標(biāo)系中的方位角；u₁(k)和u₂(k)分別表示在k時(shí)刻輪式移動(dòng)舞臺(tái)機(jī)器人的線速度和角速度；考慮模型式(1)，定義輪式移動(dòng)舞臺(tái)機(jī)器人的狀態(tài)列向量x＝[x₁ x₂x₃]^T和控制列向量u＝[u₁ u₂]^T，其中，符號(hào)T表示向量的轉(zhuǎn)置；

2)、將模型式(1)簡(jiǎn)記為式(2)：

x(k+1)＝f(x(k),u(k)) (2)

其中，f(x(k),u(k))＝[f₁(x(k),u(k))f₂(x(k),u(k))f₃(x(k),u(k))]^T，f₁(x(k),u(k))＝x₁(k)+T_su₁(k)cosx₃(k)，f₂(x(k),u(k))＝x₂(k)+T_su₁(k)sinx₃(k)，f₃(x(k),u(k))＝x₃(k)+T_su₂(k)，建立輪式移動(dòng)舞臺(tái)機(jī)器人的離散時(shí)間預(yù)測(cè)模型，參見(jiàn)式(3)：

x(k+j+1|k)＝f(x(k+j|k),u(k+j|k)),j＝0,1,...,M-1 (3)

其中，x(k+j|k)表示輪式移動(dòng)舞臺(tái)機(jī)器人控制系統(tǒng)在時(shí)刻k對(duì)未來(lái)時(shí)刻k+j狀態(tài)的預(yù)測(cè)向量；正整數(shù)M表示優(yōu)化時(shí)間窗口；

3)、令u_k,M＝[u^T(k|k)u^T(k+1|k)…u^T(k+M-1|k)]^T表示在k時(shí)刻由2M個(gè)元素組成的一個(gè)控制序列，將u_k,M代入式(3)得k+M時(shí)刻的預(yù)測(cè)狀態(tài)向量，參見(jiàn)式(4)：

x(k+M|k)＝φ_M(x(k|k),u_k,M) (4)

其中，符號(hào)φ_M表示模型式(2)中的模型函數(shù)f的M次組合函數(shù)；

4)、令x_d表示輪式移動(dòng)舞臺(tái)機(jī)器人的移動(dòng)目標(biāo)平衡點(diǎn)，定義輪式移動(dòng)舞臺(tái)機(jī)器人的鎮(zhèn)定控制誤差，參見(jiàn)式(5)：

e_k,M＝φ_M(x(k|k),u_k,M)-x_d (5)

并計(jì)算誤差e_k,M對(duì)輸入序列u_k,M的導(dǎo)數(shù)，參見(jiàn)式(6)：

其中，符號(hào)x_k表示輪式移動(dòng)舞臺(tái)機(jī)器人在k時(shí)刻的狀態(tài)測(cè)量值，且x_k＝x(k|k)；

5)、利用牛頓法和式(6)，更新在k時(shí)刻的控制序列，參見(jiàn)式(7)：

其中，符號(hào)v_k,M＝[v^T(k|k) v^T(k+1|k) … v^T(k+M-1|k)]^T，表示在k時(shí)刻更新后的控制序列，變量η_k表示更新步長(zhǎng)，滿足0η_k1，符號(hào)D_k⁺表示D_k的廣義逆矩陣；

6)、考慮式(7)，定義輪式移動(dòng)舞臺(tái)機(jī)器人在k時(shí)刻的控制量，參見(jiàn)式(8)：

u(k)＝[I₂ 0₂ … 0₂]_2×2Mv_k,M (8)

其中，符號(hào)I₂表示2階單位矩陣，符號(hào)0₂表示2階零矩陣；

7)、將控制量(8)作用于輪式移動(dòng)舞臺(tái)機(jī)器人，在下一個(gè)采樣時(shí)刻k+1到達(dá)后，檢測(cè)輪式移動(dòng)舞臺(tái)機(jī)器人的運(yùn)動(dòng)狀態(tài)x(k+1)，并更新k+1時(shí)刻的初始控制序列其中，符號(hào)表示隨機(jī)取值的2維列向量，然后令k＝k+1并返回步驟3)，周而復(fù)始，直到輪式移動(dòng)舞臺(tái)機(jī)器人運(yùn)動(dòng)到給定移動(dòng)目標(biāo)平衡點(diǎn)x_d位置為止。