[發(fā)明專利]一種輪式移動(dòng)舞臺(tái)機(jī)器人平擺線跟蹤控制方法

有效

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

1.一種輪式移動(dòng)舞臺(tái)機(jī)器人平擺線跟蹤控制方法，其特征在于，所述控制方法包括如下步驟：

1)、輸入需要跟蹤的平擺線方程，參見(jiàn)式(1)：

其中，常量a為拱的寬度，單位米，常量h為拱的寬度，單位米，常量w為平擺線旋轉(zhuǎn)角速度，單位弧度/秒，變量k表示采樣時(shí)刻；X(k)和Y(k)分別表示在k時(shí)刻平擺線在直角坐標(biāo)系中橫向和縱向上的位置坐標(biāo)；

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

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

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

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

其中，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)tanu₂(k)/l，建立輪式移動(dòng)舞臺(tái)機(jī)器人的離散時(shí)間預(yù)測(cè)模型，參見(jiàn)式(4)：

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

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

4)、令U(k)＝[u^T(k|k) u^T(k+1|k)…u^T(k+M-1|k)]^T表示在k時(shí)刻由2M個(gè)元素組成的一個(gè)控制序列，定義平擺線跟蹤誤差函數(shù)J(x(k),U(k))，參見(jiàn)式(5)：

其中，常量q₁、q₂、q₃和q₄分別為橫向位置偏差、縱向位置偏差、線速度和前輪偏角的加權(quán)系數(shù)；

5)、考慮模型式(2)，定義輪式移動(dòng)舞臺(tái)機(jī)器人的平擺線跟蹤控制問(wèn)題，參見(jiàn)式(6)：

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

6)、利用序列二次規(guī)劃算法求解式(6)，得到在k時(shí)刻的最優(yōu)控制序列U^*(k)，進(jìn)而定義輪式移動(dòng)舞臺(tái)機(jī)器人在k時(shí)刻的平擺線跟蹤控制量u(k)，參見(jiàn)式(7)：

u(k)＝[I₂ 0₂ …0₂]_2×2MU^*(k) (7)

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

7)、將控制量u(k)作用于輪式移動(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＝k+1并返回步驟5)，周而復(fù)始，直到輪式移動(dòng)舞臺(tái)機(jī)器人走完整一條平擺線為止。