[發(fā)明專利]基于數(shù)值加速度控制的全方位移動(dòng)機(jī)器人軌跡跟蹤方法

有效

• 申請?zhí)枺?/td>: 201710957396.2
• 申請日: 2017-10-16
• 公開（公告）號(hào): CN107656500B
• 公開（公告）日: 2019-11-12
• 發(fā)明（設(shè)計(jì)）人: 王俊鋒;米澤碩玉;楊俊友;王義娜;常洪彬;馬昕宇
• 申請（專利權(quán)）人: 廣東智愛機(jī)器人科技有限公司
• 主分類號(hào): G05B19/19
• 分類號(hào): G05B19/19;G05B19/416
• 代理公司: 天津市北洋有限責(zé)任專利代理事務(wù)所 12201
• 代理人: 潘俊達(dá)

1.一種基于數(shù)值加速度控制的全方位移動(dòng)機(jī)器人軌跡跟蹤方法，其特征在于，包括以下步驟：

步驟一，全方位移動(dòng)機(jī)器人的物理建模，全方位移動(dòng)部件為四個(gè)萬向輪，考慮地面對全方位移動(dòng)機(jī)器人的非線性摩擦力影響的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型分別如式(1)和式(2)所示，

其中，(x,y)為絕對坐標(biāo)系，θ為機(jī)器人的姿勢角，整數(shù)變量i∈{1，2，3，4}表示萬向輪的標(biāo)號(hào)，F(xiàn)_i為作用在第i個(gè)萬向輪上的驅(qū)動(dòng)力，f_i為地面第i個(gè)萬向輪上的非線性摩擦力,v_i為第i個(gè)萬向輪的速度，M為機(jī)器人的質(zhì)量，I為機(jī)器人的轉(zhuǎn)動(dòng)慣量，D為驅(qū)動(dòng)力相對機(jī)器人中心位置的力臂長度；

當(dāng)

F(t)＝[F₁(t),F₂(t),F₃(t),F₄(t)]^T，

f(t)＝[f₁(t),f₂(t),f₃(t),f₄(t)]^T，

時(shí)，

動(dòng)力學(xué)模型以表示，其中M₀為機(jī)器人慣性矩陣；

步驟二，假設(shè)在時(shí)刻kT之后，經(jīng)過非常微小時(shí)間后的時(shí)刻為kT⁺，在時(shí)刻t＝kT⁺時(shí)，作用于全方位移動(dòng)機(jī)器人萬向輪上的驅(qū)動(dòng)力為F(kT⁺)，在一個(gè)采樣周期T內(nèi)作用于各個(gè)萬向輪驅(qū)動(dòng)力為不變的，則時(shí)刻t＝kT和時(shí)刻t＝kT⁺的全方位移動(dòng)機(jī)器人的離散型動(dòng)力學(xué)模型分別如式(3)和式(4)所示，

其中F[(k-1)T⁺]為時(shí)間區(qū)間[(k-1)T⁺,kT]內(nèi)作用于各個(gè)萬向輪的輸入驅(qū)動(dòng)力，F(xiàn)[(kT⁺)]是經(jīng)過一個(gè)采樣周期T后，下一個(gè)時(shí)間區(qū)間[kT⁺,(k+1)T]內(nèi)的作用于各個(gè)萬向輪的驅(qū)動(dòng)力；

步驟三，把X_cd(t)作為全方位移動(dòng)機(jī)器人的目標(biāo)軌跡，為了使全方位移動(dòng)機(jī)器人跟蹤目標(biāo)軌跡，時(shí)刻t＝kT的萬向輪驅(qū)動(dòng)力F[(k-1)T⁺]作為已知的，則在時(shí)刻t＝kT⁺，基于數(shù)值加速度控制，作用在各個(gè)萬向輪上的驅(qū)動(dòng)力F(kT⁺)的計(jì)算式如下：

其中，整數(shù)變量i∈{1，2，3，4}表示萬向輪的標(biāo)號(hào)；K_D為速度偏差系數(shù)矩陣，K_D＝diag(k_dii)、k_dii為第i個(gè)萬向輪的速度偏差系數(shù)；K_P為位置偏差系數(shù)矩陣，K_P＝diag(k_pii)、k_pii為第i個(gè)萬向輪的位置偏差系數(shù)；X_cd(kT⁺)為目標(biāo)軌跡，X_c(kT)為實(shí)際軌跡；

步驟四，決定K_D和K_P的最優(yōu)值，全方位移動(dòng)機(jī)器人跟蹤誤差定義為e(t)＝X_cd(t)-X_c(t)當(dāng)作用在全方位移動(dòng)機(jī)器人萬向輪上的驅(qū)動(dòng)力瞬間變化時(shí)，其全方位移動(dòng)機(jī)器人加速度也將瞬間變化，而全方位移動(dòng)機(jī)器人速度和位置是不會(huì)發(fā)生瞬間變化，又由全方位移動(dòng)機(jī)器人的離散型動(dòng)力學(xué)模型和作用于各個(gè)萬向輪上的驅(qū)動(dòng)力的計(jì)算式，得到全方位移動(dòng)機(jī)器人跟蹤誤差的離散模型如下：

在時(shí)間區(qū)間[kT⁺,(k+1)T]內(nèi)，

根據(jù)(6)～(8)三式得到下式，

令以及

得到Ε[(k+1)T]＝AΕ(kT)，

進(jìn)一步地，

得到Ε[(k+1)T]＝(A₁-A₂K)Ε(kT)，

對于上式的誤差狀態(tài)反饋系統(tǒng)，為保證系統(tǒng)穩(wěn)定性，用極點(diǎn)配置法或者最優(yōu)理論可以選出K_P和K_D的最優(yōu)值；

步驟五，基于離散型最優(yōu)預(yù)見控制理論，利用已知的未來目標(biāo)信息，進(jìn)一步改善系統(tǒng)動(dòng)態(tài)響應(yīng)性能，在時(shí)刻t＝kT⁺，基于數(shù)值加速度控制，導(dǎo)入目標(biāo)軌跡未來信息，計(jì)算作用在各個(gè)萬向輪上的驅(qū)動(dòng)力，進(jìn)行全方位移動(dòng)機(jī)器人系統(tǒng)的數(shù)值加速度控制器的設(shè)計(jì)。