[發(fā)明專(zhuān)利]雙舵輪自動(dòng)導(dǎo)引車(chē)軌跡追蹤控制方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010169468.9
• 申請(qǐng)日: 2020-03-12
• 公開(kāi)（公告）號(hào): CN111273551B
• 公開(kāi)（公告）日: 2021-03-16
• 發(fā)明（設(shè)計(jì)）人: 張健;齊嘉暉;汪威;吳穎穎
• 申請(qǐng)（專(zhuān)利權(quán)）人: 山東大學(xué)
• 主分類(lèi)號(hào): G05B13/04
• 分類(lèi)號(hào): G05B13/04;G05D1/02
• 代理公司: 濟(jì)南圣達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 37221
• 代理人: 李圣梅

1.雙舵輪自動(dòng)導(dǎo)引車(chē)軌跡追蹤控制方法，其特征是，包括：

建立雙舵輪自動(dòng)導(dǎo)引車(chē)AGV運(yùn)動(dòng)學(xué)模型：雙舵輪AGV轉(zhuǎn)彎時(shí)，AGV車(chē)體四角各為一個(gè)從動(dòng)輪，沿中線(xiàn)前后并置兩輪為獨(dú)立驅(qū)動(dòng)的驅(qū)動(dòng)輪；由于車(chē)體運(yùn)動(dòng)通過(guò)動(dòng)態(tài)調(diào)整前后舵輪來(lái)實(shí)現(xiàn)的，因此，在不影響運(yùn)動(dòng)學(xué)建模分析的情況下，將AGV的運(yùn)動(dòng)模型簡(jiǎn)化為前后驅(qū)動(dòng)輪的運(yùn)動(dòng)模型；

基于上述模型，建立雙舵輪前后舵輪的位置坐標(biāo)即AGV車(chē)體位置坐標(biāo)，基于車(chē)體位置坐標(biāo)，車(chē)體運(yùn)動(dòng)過(guò)程中，結(jié)合舵輪運(yùn)動(dòng)速度及航向角，得其角速度；

建立平面直角坐標(biāo)系{XOY}，O_f、O_b分別為前后驅(qū)動(dòng)輪與地面接觸點(diǎn)；軸距為L(zhǎng)；考慮AGV車(chē)身處于轉(zhuǎn)彎圓弧軌跡時(shí)，由于圓弧軌跡各處曲率相同，因此行進(jìn)過(guò)程中前后驅(qū)動(dòng)輪轉(zhuǎn)角相同；前驅(qū)動(dòng)輪與后驅(qū)動(dòng)輪行進(jìn)方向與機(jī)器人體坐標(biāo)系{X’CY’}X軸夾角為θ_f、θ_b，V_f、V_b為前后輪行進(jìn)速度，轉(zhuǎn)彎半徑為R；從后驅(qū)動(dòng)輪進(jìn)入轉(zhuǎn)彎圓弧開(kāi)始計(jì)時(shí)，經(jīng)過(guò)t₁時(shí)間，前輪走過(guò)的軌跡弧長(zhǎng)為2Rθ_f；則有：

即驅(qū)動(dòng)輪行進(jìn)速度V_f恒定的前提下，車(chē)體轉(zhuǎn)角2θ_f與其行進(jìn)時(shí)間t₁成正比；根據(jù)轉(zhuǎn)彎軌跡三角關(guān)系，可知：

雙舵輪AGV定位算法：車(chē)體幾何中心(X₀,Y₀)為前舵輪中點(diǎn)O_f和后舵輪中點(diǎn)O_b的連線(xiàn)中點(diǎn)，在負(fù)載均勻的情況下，認(rèn)為幾何中心(X₀,Y₀)即為AGV車(chē)體質(zhì)心，其初始值為(X_0c,Y_0c)；θ_f、θ_b分別為AGV前后輪轉(zhuǎn)角角度V_f、V_b為前后輪線(xiàn)速度；O點(diǎn)為小車(chē)轉(zhuǎn)彎時(shí)瞬時(shí)運(yùn)動(dòng)軌跡圓心，則R_f、R_b為前后輪轉(zhuǎn)彎過(guò)程中的軌跡半徑；則其前后舵輪的位置坐標(biāo)可表示為：

其中，質(zhì)心坐標(biāo)C＝(X₀,Y₀)可以表示為：

即為AGV車(chē)體位置坐標(biāo)；

質(zhì)心在全局坐標(biāo)系下的線(xiàn)速度狀態(tài)量為：

即線(xiàn)速度v_c可表示為：

其航向角θ為：

考慮剛體運(yùn)動(dòng)特性，車(chē)體繞O點(diǎn)運(yùn)動(dòng)時(shí)，前后舵輪中心點(diǎn)及車(chē)體質(zhì)心C運(yùn)動(dòng)的在地面平動(dòng)的角速度一致；車(chē)體運(yùn)動(dòng)過(guò)程中，結(jié)合舵輪運(yùn)動(dòng)速度及航向角，根據(jù)正弦定理得其角速度為：

將輪式自動(dòng)導(dǎo)引車(chē)系統(tǒng)位姿狀態(tài)分別表示車(chē)體質(zhì)心在全局坐標(biāo)系下的位置坐標(biāo)，將雙舵輪自動(dòng)導(dǎo)引車(chē)自身運(yùn)動(dòng)線(xiàn)速度與角速度作為控制輸入量，以車(chē)體質(zhì)心線(xiàn)速度和角速度表示，獲得雙舵輪自動(dòng)導(dǎo)引車(chē)運(yùn)動(dòng)學(xué)方程；

當(dāng)應(yīng)用模型預(yù)測(cè)控制糾正雙舵輪AGV軌跡路徑時(shí)，將輪式自動(dòng)導(dǎo)引車(chē)系統(tǒng)位姿狀態(tài)記為X＝[X₀ Y₀ θ]^T＝[x y θ]^T，x，y分別表示車(chē)體質(zhì)心在全局坐標(biāo)系{XOY}下的位置坐標(biāo)；將雙舵輪自動(dòng)導(dǎo)引車(chē)自身運(yùn)動(dòng)線(xiàn)速度與角速度作為控制輸入量u，以車(chē)體質(zhì)心線(xiàn)速度和角速度表示，記為u＝[v_c ω_c]^T＝[v ω]^T；

車(chē)輪與地面純滾動(dòng)接觸，即AGV在平面運(yùn)動(dòng)時(shí)沒(méi)有滑動(dòng)，可以給出系統(tǒng)運(yùn)動(dòng)學(xué)模型如下：

進(jìn)一步，雙舵輪自動(dòng)導(dǎo)引車(chē)運(yùn)動(dòng)學(xué)方程如式(11)所示：

求解雙舵輪自動(dòng)導(dǎo)引車(chē)運(yùn)動(dòng)學(xué)方程：將最優(yōu)控制問(wèn)題的求解轉(zhuǎn)化為實(shí)時(shí)數(shù)值解有約束優(yōu)化問(wèn)題，通過(guò)計(jì)算得到控制量作用于雙舵輪自動(dòng)導(dǎo)引車(chē)，實(shí)現(xiàn)雙舵輪AGV軌跡跟蹤控制；

雙舵輪AGV軌跡跟蹤誤差模型：為規(guī)劃AGV跟蹤路徑，設(shè)立運(yùn)行參考軌跡，參考軌跡任意點(diǎn)均滿(mǎn)足狀態(tài)空間表達(dá)式(12)，即：

表示參考軌跡上任意點(diǎn)的狀態(tài)空間表達(dá)式；

其中參考軌跡狀態(tài)量為X_r＝[x_r y_r θ_r]^T，其參考輸入控制量u_r＝[v_r,w_r]^T，(x_r,u_r)為參考軌跡車(chē)輛質(zhì)心坐標(biāo)，θ_r為參考航向角；為線(xiàn)性化預(yù)測(cè)控制模型，將式(12)在參考軌跡點(diǎn)(x_r,u_r)處進(jìn)行泰勒展開(kāi)，忽略高階項(xiàng)，保留一階項(xiàng)，獲得雙舵輪移動(dòng)機(jī)器人線(xiàn)性化誤差模型，其中，(13)式為參考軌跡保留一階項(xiàng)的泰勒級(jí)數(shù)展開(kāi)；線(xiàn)性誤差模型為(16)式：

即：

結(jié)合狀態(tài)空間表達(dá)式，可得：

將式(13)與式(12)做差，可得：

表示連續(xù)誤差模型的狀態(tài)空間表達(dá)式；

其中，x′＝x-x_r，表示車(chē)體實(shí)際運(yùn)動(dòng)位置與參考位置的狀態(tài)誤差；u′＝u-u_r則是控制輸入量的誤差；式(16)為連續(xù)模型，通過(guò)使用前向差分近似x′，得到以下AGV運(yùn)動(dòng)線(xiàn)性時(shí)變離散誤差模型：

其中，線(xiàn)性時(shí)變矩陣分別為：

其中，T為采樣周期，k為采樣時(shí)間；

軌跡跟蹤控制器模型：離散化誤差模型在系統(tǒng)k時(shí)刻增量式表達(dá)式為：

Δu(k)＝u(k)-u(k-1) (20)

則運(yùn)動(dòng)學(xué)模型的狀態(tài)空間表達(dá)式為：

其中，

這里有限預(yù)測(cè)時(shí)域?yàn)閜，在時(shí)域[τ,τ+p]內(nèi)對(duì)系統(tǒng)狀態(tài)進(jìn)行預(yù)測(cè)；控制時(shí)域?yàn)閏，在時(shí)域[τ,τ+c]內(nèi)生成系統(tǒng)控制序列；所以預(yù)測(cè)時(shí)域內(nèi)系統(tǒng)的預(yù)測(cè)方程為：

Y(k)＝φ_pψ(k)+Q_cΔU(k) (23)

其中，Y(k)為系統(tǒng)輸出量；φ_p為狀態(tài)量參數(shù)；Q_c為控制量參數(shù)；ΔU(k)為控制增量序列，用公式表達(dá)為：

在系統(tǒng)預(yù)測(cè)方程中，建立準(zhǔn)確的目標(biāo)函數(shù)求解控制增量ΔU(k)，從而得到控制時(shí)域[τ,τ+c]內(nèi)合適的控制量序列，這里建立目標(biāo)函數(shù)為：

目標(biāo)函數(shù)既保證了模型精準(zhǔn)跟蹤目標(biāo)軌跡，又保證控制量符合AGV實(shí)際性能限制；Q為預(yù)測(cè)時(shí)域權(quán)重因子；R為控制時(shí)域權(quán)重因子；_ρ為權(quán)重系數(shù)；γ為松弛變量；

AGV實(shí)際運(yùn)行中，考慮AGV行駛可行域、舵輪轉(zhuǎn)角和行進(jìn)速度現(xiàn)實(shí)約束條件，軌跡跟蹤控制器模型可描述為：

軌跡跟蹤控制器模型算法求解：為獲取系統(tǒng)最優(yōu)控制序列，將上述最優(yōu)化軌跡跟蹤問(wèn)題轉(zhuǎn)化為帶約束線(xiàn)性二次規(guī)劃問(wèn)題進(jìn)行求解；其形式如下：

將預(yù)測(cè)方程式(23)代入目標(biāo)函數(shù)式(26)，經(jīng)過(guò)整理可得：

其中G為控制增量系數(shù)矩陣，E_k為預(yù)測(cè)時(shí)域[τ,τ+p]內(nèi)軌跡跟蹤誤差；針對(duì)帶有線(xiàn)性約束的二次實(shí)函數(shù)，采用具有通用性的內(nèi)點(diǎn)法求解，求得[τ,τ+c]內(nèi)控制輸入增量為：

ΔU_t＝[Δu_t,Δu_t+1,…,Δu_t+c-1]^T (29)

將該控制序列中第一個(gè)元素作為實(shí)際輸入的控制量增量，通過(guò)計(jì)算得到控制量作用于實(shí)際系統(tǒng)，即：

u(t)＝u(t-1)+Δu_t (30)

當(dāng)系統(tǒng)進(jìn)入下一采樣周期后，系統(tǒng)重新計(jì)算控制輸入增量，循環(huán)迭代實(shí)現(xiàn)雙舵輪AGV軌跡跟蹤控制。