[發(fā)明專利]一種具有速度約束的機(jī)器人軌跡跟蹤方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811405669.3
• 申請(qǐng)日: 2018-11-23
• 公開(kāi)（公告）號(hào): CN109491389B
• 公開(kāi)（公告）日: 2021-08-10
• 發(fā)明（設(shè)計(jì)）人: 劉小峰;李金明
• 申請(qǐng)（專利權(quán)）人: 河海大學(xué)常州校區(qū)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司 32200
• 代理人: 丁濤

1.一種具有速度約束的機(jī)器人軌跡跟蹤方法，其特征在于，包括如下步驟：

(1)設(shè)置人運(yùn)動(dòng)區(qū)域、起始點(diǎn)、障礙物；

(2)進(jìn)行路徑規(guī)劃，先用改進(jìn)的快速探索隨機(jī)樹(shù)算法得出路徑，再對(duì)路徑進(jìn)行優(yōu)化和曲線擬合，得到一條光滑的路徑；

(3)根據(jù)機(jī)器人的三個(gè)輪子的速度界限和機(jī)器人的運(yùn)動(dòng)范圍，建立速度約束；

(4)建立系統(tǒng)預(yù)測(cè)模型，根據(jù)路徑節(jié)點(diǎn)預(yù)測(cè)下一時(shí)刻的最優(yōu)輸入速度；

(5)重復(fù)執(zhí)行步驟(4)，求出每一時(shí)刻的最優(yōu)輸入速度；

(6)根據(jù)最優(yōu)速度輸入控制機(jī)器人進(jìn)行運(yùn)動(dòng)；

所述步驟(2)中的改進(jìn)的快速探索隨機(jī)樹(shù)算法如下：

定義全局環(huán)境X，其中障礙空間Xobstacle，無(wú)障礙空間Xfree＝X/Xobstacle，假設(shè)障礙空間的障礙是事先已知的且靜止的；起點(diǎn)qinit和終點(diǎn)qend位于無(wú)障礙空間內(nèi)；要規(guī)劃的路徑是樹(shù)T的一部分，樹(shù)由一個(gè)個(gè)采樣點(diǎn)V和采樣點(diǎn)之間的連線E構(gòu)建形成，其中采樣點(diǎn)稱為葉子節(jié)點(diǎn)，連線稱為樹(shù)枝；

在X中取隨機(jī)點(diǎn)qrand，找到T中與qrand最近的節(jié)點(diǎn)qnear，連結(jié)形成向量Vector；在Vector上從qnear開(kāi)始取步長(zhǎng)step，形成新節(jié)點(diǎn)qnew；若qnew與qnear連線通過(guò)碰撞檢測(cè)發(fā)現(xiàn)發(fā)生了碰撞，則放棄此次生長(zhǎng)；

在qnew附近建立節(jié)點(diǎn)集合，以qnew為圓心，預(yù)設(shè)的R為半徑作圓所產(chǎn)生的圓形空間，比較將落入圓內(nèi)的樹(shù)節(jié)點(diǎn)和qnew連結(jié)后樹(shù)的累積代價(jià)，選擇代價(jià)最小的節(jié)點(diǎn)作為qnew的父節(jié)點(diǎn)qnear，加入樹(shù)中；

假設(shè)新加入樹(shù)的節(jié)點(diǎn)qnew為父節(jié)點(diǎn)，計(jì)算落入以它為半徑的圓內(nèi)節(jié)點(diǎn)的代價(jià)；選擇代價(jià)最小的節(jié)點(diǎn)qmin，將它與qnew連結(jié)，并刪除原來(lái)與qmin連結(jié)的枝；qmin作為qnew的子節(jié)點(diǎn)加入樹(shù)中；

重復(fù)上述步驟，得到一系列葉子節(jié)點(diǎn)；當(dāng)某一節(jié)點(diǎn)與終點(diǎn)qend的距離小于步長(zhǎng)時(shí)，連結(jié)兩點(diǎn)，若之間無(wú)碰撞，則找到了一條從起點(diǎn)qinit到終點(diǎn)qend的可行路徑；

所述步驟(4)中的系統(tǒng)預(yù)測(cè)模型由以下方法獲得：

全向移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型表示為其中是OMR在笛卡爾坐標(biāo)中的狀態(tài)，是全向移動(dòng)機(jī)器人在世界坐標(biāo)中的狀態(tài)，H是變換矩陣，θ_c為轉(zhuǎn)向角；

根據(jù)全向輪的模型，得到其中矩陣R由全向輪的參數(shù)得到，決定了全向移動(dòng)機(jī)器人的全向性，[v₁ v₂ v₃]^T為機(jī)器人三個(gè)輪子的速度；

OMR的逆運(yùn)動(dòng)學(xué)方程為

令S＝R^-1，則

推導(dǎo)出連續(xù)時(shí)間系統(tǒng)方程

對(duì)公式(1)離散化，得到其中T為采樣時(shí)間；

將其轉(zhuǎn)換為離散狀態(tài)空間模型，并得到X(k+1)＝AX(k)+BU(k)，其中X(k+1)為下一時(shí)刻的狀態(tài)，X(k)為該時(shí)刻的狀態(tài)，U(k)為速度的變化量，A、B為系數(shù)矩陣；

輸出模型為Y(k)＝CX(k)+DU(k)，其中C、D為系數(shù)矩陣；根據(jù)滾動(dòng)優(yōu)化的理論，在某一時(shí)刻的輸入無(wú)法影響該時(shí)刻的輸出，所以矩陣D為零矩陣，則Y(k)＝CX(k)；

所以預(yù)測(cè)模型為

所述步驟(5)中的最優(yōu)輸入速度由以下方法得到：

假設(shè)控制輸入為Δu(k_i),Δu(k_i+1),...,Δu(k_i+N_p-1)，共N_p個(gè)，

狀態(tài)為x(k_i+1|k_i),x(k_i+2|k_i),...,x(k_i+N_c|k_i),...,x(k_i+N_p|k_i)；

由步驟(4)得到的狀態(tài)空間模型得

定義向量

Y＝[y(k_i+1|k_i) y(k_i+2|k_i) y(k_i+3|k_i) ... y(k_i+N_p|k_i)]^T

ΔU＝[Δu(k_i) Δu(k_i+1) Δu(k_i+2) ... Δu(k_i+N_c-1)]^T

所以預(yù)測(cè)輸出Y＝Fx(k_i)+ΦΔU，其中

假設(shè)設(shè)定點(diǎn)信號(hào)r(k_i)，且k_i時(shí)刻的閉環(huán)性能調(diào)節(jié)參數(shù)為r_ω；令和轉(zhuǎn)換矩陣所以代價(jià)函數(shù)

通過(guò)最小化代價(jià)函數(shù)，獲得最優(yōu)控制輸入，即最小化預(yù)測(cè)系統(tǒng)輸出和期望參考輸出之間的差異；

考慮輸入約束Δu^min≤Δu(k)≤Δu^max和輸出約束Y^min≤Fx(k_i)+ΦΔU≤Y^max；

由于存在約束，無(wú)法獲得優(yōu)化問(wèn)題的解析解，使用數(shù)值優(yōu)化方法來(lái)解決；

帶約束的二次規(guī)劃的形式是

二次規(guī)劃問(wèn)題通過(guò)調(diào)用函數(shù)庫(kù)來(lái)解決，然后獲得最優(yōu)控制輸入。