基于動態(tài)逆模型的機(jī)器人軌跡跟隨糾偏控制?

林榮霞

（廣東工業(yè)大學(xué)華立學(xué)院 廣州 511325）

1 引言

隨著人工智能技術(shù)和工業(yè)自動化技術(shù)的發(fā)展，機(jī)器人不斷取代傳統(tǒng)的機(jī)械作業(yè)模式，實現(xiàn)人工智能控制，提高了作業(yè)的精度，變剛度關(guān)節(jié)驅(qū)動機(jī)器人作為一種重要的仿生機(jī)器人，在執(zhí)行復(fù)雜環(huán)境下的搜救、探測以及開采等工作中發(fā)揮重要作用。變剛度關(guān)節(jié)驅(qū)動機(jī)器人在進(jìn)行行走和驅(qū)動過程中，受到環(huán)境擾動以及機(jī)械結(jié)構(gòu)部件的影響，導(dǎo)致變剛度關(guān)節(jié)驅(qū)動機(jī)器人的軌跡和末端位姿容易出現(xiàn)偏移，需要對變剛度關(guān)節(jié)驅(qū)動機(jī)器人進(jìn)行糾偏控制，提高變剛度關(guān)節(jié)驅(qū)動機(jī)器人的穩(wěn)定性，研究變剛度關(guān)節(jié)驅(qū)動機(jī)器人的穩(wěn)定控制模型，在整個機(jī)器人的穩(wěn)定控制過程中具有很好的應(yīng)用價值［1］。

變剛度關(guān)節(jié)驅(qū)動機(jī)器人控制系統(tǒng)采用集中式控制方法，即變剛度關(guān)節(jié)驅(qū)動機(jī)器人的全部控制由一臺微型計算機(jī)完成，另一種是分散（級）式控制，即采用多臺微機(jī)聯(lián)合總線控制方法，進(jìn)行軌跡跟隨糾偏控制，傳統(tǒng)方法中，主要的糾偏控制方法有自適應(yīng)傳感濾波控制方法、穩(wěn)態(tài)積分控制方法、三軸陀螺調(diào)節(jié)控制方法等［2～3］，通過控制指令實現(xiàn)對機(jī)器人的陀螺、繼電器、電磁閥及執(zhí)行電機(jī)的自適應(yīng)控制，提高對機(jī)器人的軌跡跟隨糾偏控制能力。文獻(xiàn)［4］中提出一種基于運動學(xué)解析模型的變剛度關(guān)節(jié)驅(qū)動機(jī)器人軌跡跟隨糾偏控制方法，解決變剛度關(guān)節(jié)驅(qū)動機(jī)器人在行走、姿態(tài)變換過程中的軌跡偏移問題，提高控制過程的抗擾動能力，但該方法的控制計算開銷較大，控制的實時性不好；文獻(xiàn)［5］中提出一種基于三維空間末端位姿調(diào)節(jié)的機(jī)器人軌跡跟隨糾偏控制方法，根據(jù)變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動部件對應(yīng)的質(zhì)心求得穩(wěn)定性運動慣量參數(shù)，實現(xiàn)機(jī)器人的位姿和軌跡的自動調(diào)整，但該方法在控制過程中的抗擾動性和魯棒性不好。針對上述問題，本文提出一種基于動態(tài)逆模型的機(jī)器人軌跡跟隨糾偏方法。首先構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的D-H 參數(shù)和運動學(xué)模型，采用擴(kuò)展卡爾曼濾波方法進(jìn)行的機(jī)器人軌跡跟隨參量采樣和自適應(yīng)融合處理，然后采用動態(tài)逆控制模型計算機(jī)器人的軌跡跟隨誤差，以機(jī)器人的幾何空間位姿參數(shù)的調(diào)節(jié)對象，進(jìn)行軌跡跟隨糾偏控制，利用關(guān)節(jié)軸線間的幾何關(guān)系求出模型中運動學(xué)參數(shù)，獲得的連桿模型參數(shù)能及時反饋給變剛度關(guān)節(jié)驅(qū)動機(jī)器人的控制系統(tǒng)，實現(xiàn)控制律優(yōu)化設(shè)計。最后進(jìn)行仿真實驗分析，展示了本文方法在實現(xiàn)機(jī)器人軌跡跟隨糾偏控制方面的優(yōu)化性能。

2 變剛度關(guān)節(jié)驅(qū)動機(jī)器人運動學(xué)分析和參量解算

2.1 變剛度關(guān)節(jié)驅(qū)動機(jī)器人運動學(xué)模型

在三維空間中構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動學(xué)解析模型，采用三軸加速度計、磁力計和三軸陀螺儀進(jìn)行變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動參數(shù)采集，在笛卡爾空間中進(jìn)行機(jī)器人的行為參數(shù)和動力學(xué)參數(shù)分析，在慣性參考系ΣI下，構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動解析模型，用4×4 的齊次坐標(biāo)矩陣IT0(α0,β0,γ0)表示變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動部件的力學(xué)參數(shù)，為（≡IT0(θ1,θ2,θ3)），包含3方向的平移常量以及繞3 個軸的旋轉(zhuǎn)，在機(jī)器人的動態(tài)逆分布模型中，分別用機(jī)器人的位姿偏移角α0，俯仰角β0和轉(zhuǎn)向角γ0表示機(jī)器人的行為空間參數(shù)，其中，機(jī)器人的位姿偏移角α0是繞z 軸的旋轉(zhuǎn)角，得到機(jī)器人軌跡跟隨控制的伴隨跟蹤矩陣Rz(α0)表示為

設(shè)變剛度關(guān)節(jié)驅(qū)動機(jī)器人的末端效應(yīng)器在笛卡爾空間的位姿、速度的自適應(yīng)調(diào)節(jié)模態(tài)函數(shù)分別為pe、p˙e∈R6×1，確定線性補(bǔ)償，得到機(jī)器人的軌跡跟隨控制的角位置、角速度分別為θ、θ˙∈R10×1，β0是繞y 軸的旋轉(zhuǎn)角，控制律導(dǎo)引曲線偏差矩陣Ry(β0)表示為

其中，γ0是繞x 軸的旋轉(zhuǎn)角，引入機(jī)器人控制的滑模誤差偏移角度，得到旋轉(zhuǎn)矩陣Rx(γ0)表示為

在機(jī)器人的多狀態(tài)隨機(jī)分布特征空間中，采用多通道加權(quán)控制方法進(jìn)行機(jī)器人的動態(tài)逆反饋調(diào)節(jié)，可得末端效應(yīng)器坐標(biāo)系Σe(≡Σ7)相對于慣性坐標(biāo)系ΣI的齊次變換矩陣IT7(θ)為

構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的D-H 參數(shù)和運動學(xué)模型，得到變剛度關(guān)節(jié)驅(qū)動機(jī)器人從腰部到手臂末端效應(yīng)器的位形θ 和末端效應(yīng)器位姿pe的動態(tài)逆運動學(xué)關(guān)系式:

采用擴(kuò)展卡爾曼濾波方法進(jìn)行的機(jī)器人軌跡跟隨參量采樣和自適應(yīng)融合處理［6］，由此構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人軌跡跟隨糾偏控制的誤差反饋調(diào)節(jié)系統(tǒng)模型如圖1所示。

2.2 機(jī)器人的軌跡分布位置參量解算

采用擴(kuò)展卡爾曼濾波方法進(jìn)行的機(jī)器人軌跡跟隨參量采樣和自適應(yīng)融合處理，結(jié)合諧振比率控制方法進(jìn)行機(jī)器人軌跡跟隨的誤差反饋調(diào)節(jié)［7］，構(gòu)造最佳專家補(bǔ)償函數(shù):

在機(jī)器人的行為軌跡空間點的相機(jī)坐標(biāo)系中，采用自適應(yīng)參數(shù)調(diào)節(jié)方法，進(jìn)行機(jī)器人軌跡的線性空間規(guī)劃［8］，得到機(jī)器人軌跡跟隨糾偏的等價非線性時變反饋系統(tǒng)為

圖1 機(jī)器人軌跡跟隨糾偏控制的誤差反饋調(diào)節(jié)模型

引入質(zhì)心運動狀態(tài)方程，采用欠驅(qū)動步行穩(wěn)定控制方法，進(jìn)行機(jī)器人的前饋控制，采用動態(tài)逆模型進(jìn)行機(jī)器人的軌跡分布位置自適應(yīng)解算，在控制律S域中，得到:

在剛性均質(zhì)桿控制下，機(jī)器人的軌跡糾偏誤差狀態(tài)轉(zhuǎn)移概率函數(shù)為

根據(jù)變剛度關(guān)節(jié)驅(qū)動機(jī)器人的運動部件對應(yīng)的質(zhì)心求得穩(wěn)定性運動慣量參數(shù)，當(dāng)變剛度關(guān)節(jié)驅(qū)動機(jī)器人處于非加速運動狀態(tài)時，將陀螺儀采集的原始數(shù)據(jù)進(jìn)行濾波處理［9］。確定補(bǔ)償器控制系數(shù)矩陣D，得到系統(tǒng)的穩(wěn)態(tài)誤差限定在:

當(dāng)變剛度關(guān)節(jié)驅(qū)動機(jī)器人處于任意姿態(tài)時，通過慣性參量調(diào)節(jié)，使得H(s)嚴(yán)格正實，從而得到機(jī)器人的軌跡跟隨誤差調(diào)節(jié)的過程是全局漸進(jìn)穩(wěn)定的［10］，即

綜上分析，采用動態(tài)逆控制模型計算機(jī)器人的軌跡跟隨誤差，以機(jī)器人的幾何空間位姿參數(shù)的調(diào)節(jié)對象［11］，得到機(jī)器人軌跡跟隨控制模糊約束方程為

式中，Km為變剛度關(guān)節(jié)驅(qū)動機(jī)器人的實際D-H 控制偏差，δ 為慣性擾動系數(shù)，θ 為滑模誤差偏移角度，其中sin θp=θp，cos θp=1。

3 軌跡跟隨糾偏控制優(yōu)化

3.1 動態(tài)逆模型控制

在構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的D-H 參數(shù)和運動學(xué)模型，采用擴(kuò)展卡爾曼濾波方法進(jìn)行的機(jī)器人軌跡跟隨參量采樣和自適應(yīng)融合處理的基礎(chǔ)上，進(jìn)行機(jī)器人的軌跡跟隨糾偏控制優(yōu)化，本文提出一種基于動態(tài)逆模型的機(jī)器人軌跡跟隨糾偏方法，在變剛度關(guān)節(jié)驅(qū)動機(jī)器人初始零位狀態(tài)下，以第一關(guān)節(jié)平面的法線作為基坐標(biāo)系的Z 軸方向，第二關(guān)節(jié)平面的法線作為基坐標(biāo)系的X 軸方向，得到變剛度關(guān)節(jié)驅(qū)動機(jī)器人末端位姿跟蹤控制過程的動態(tài)逆調(diào)節(jié)的協(xié)方差矩陣為:

運用變剛度關(guān)節(jié)驅(qū)動機(jī)器人單關(guān)節(jié)獨立運動控制的方法，取方差的上界作為調(diào)節(jié)系數(shù)，采用修正LESLIE-GOWER模型進(jìn)行機(jī)器人的軌跡偏離修正，得到解向量的子空間w(k)與V(k)具有相關(guān)性，計算關(guān)節(jié)圓的擬合誤差為

形成實際變剛度關(guān)節(jié)驅(qū)動機(jī)器人空間D-H 參數(shù)模型，得到變剛度關(guān)節(jié)驅(qū)動機(jī)器人的慣性力矩系數(shù)，引入了輔助矩陣，采用自適應(yīng)誤差調(diào)節(jié)方法進(jìn)行反饋跟隨識別，得到軌跡偏差修正的周期解為

運用動態(tài)跟蹤原理，控制變剛度關(guān)節(jié)驅(qū)動機(jī)器人運行至空間任意位置［12］，得到動態(tài)逆模型控制的對偶問題可表達(dá)為

3.2 控制律優(yōu)化

以機(jī)器人的幾何空間位姿參數(shù)的調(diào)節(jié)對象，進(jìn)行軌跡跟隨糾偏控制，利用關(guān)節(jié)軸線間的幾何關(guān)系求出模型中運動學(xué)參數(shù)為

獲得的連桿模型參數(shù)，變剛度關(guān)節(jié)驅(qū)動機(jī)器人的控制模糊核函數(shù)為

獲得的連桿模型參數(shù)及時反饋給變剛度關(guān)節(jié)驅(qū)動機(jī)器人，得到機(jī)器人的軌跡跟隨糾偏二項式核為

根據(jù)k-1 時刻的姿態(tài)角預(yù)測，在變剛度關(guān)節(jié)驅(qū)動機(jī)器人運動學(xué)模型的基礎(chǔ)上，本文采用圓周點法來測量機(jī)器人的軌跡跟蹤的姿態(tài)偏移，得到偏移距離可以轉(zhuǎn)化為

由此獲得連桿模型參數(shù)，采用自適應(yīng)參數(shù)調(diào)整方法將誤差信息反饋給變剛度關(guān)節(jié)驅(qū)動機(jī)器人的控制系統(tǒng)，實現(xiàn)控制律優(yōu)化設(shè)計。

4 仿真實驗與結(jié)果分析

為了測試本文方法在實現(xiàn)機(jī)器人軌跡跟隨糾偏控制中的應(yīng)用性能，進(jìn)行仿真實驗，實驗采用自主研發(fā)的Motoman UP6 型變剛度關(guān)節(jié)驅(qū)動機(jī)器人，控制算法設(shè)計采用Matlab 7 設(shè)計，設(shè)置機(jī)器人的行為軌跡偏差為50mm，在相機(jī)坐標(biāo)系中的初始位置為（0，0.2，2），目標(biāo)位置為（1，2.4，6），軌跡的最大偏差為1.5 mm，機(jī)器人的軌跡跟蹤速度為12 mm/s，控制器的采樣時間為0.01s。根據(jù)上述仿真環(huán)境和參數(shù)設(shè)定，進(jìn)行機(jī)器人的控制仿真，得到機(jī)器人的動態(tài)參量采集結(jié)果如圖2所示。

圖2 參數(shù)采集輸出

以圖2 采集的數(shù)據(jù)作為機(jī)器人的軌跡跟隨控制，采用動態(tài)逆控制模型計算機(jī)器人的軌跡跟隨誤差，以機(jī)器人的幾何空間位姿參數(shù)的調(diào)節(jié)對象，進(jìn)行軌跡跟隨糾偏控制，得到軌跡糾偏仿真結(jié)果如圖3所示。

圖3 機(jī)器人軌跡糾偏控制仿真結(jié)果

圖4 控制性能對比

分析圖3 得知，采用本文方法進(jìn)行機(jī)器人軌跡跟隨的糾偏性能較好。測試不同方法進(jìn)行機(jī)器人軌跡糾偏控制性能，得到控制性能曲線對比如圖4所示。

分析圖4 得知，采用本文方法進(jìn)行機(jī)器人軌跡跟隨糾偏的誤差修正能力較好，糾偏控制能力較強(qiáng)，誤差比傳統(tǒng)方法降低12.7%，展示了較好的軌跡跟隨糾偏控制性能。

5 結(jié)語

對變剛度關(guān)節(jié)驅(qū)動機(jī)器人進(jìn)行糾偏控制，提高變剛度關(guān)節(jié)驅(qū)動機(jī)器人的穩(wěn)定性，本文提出一種基于動態(tài)逆模型的機(jī)器人軌跡跟隨糾偏方法。構(gòu)建變剛度關(guān)節(jié)驅(qū)動機(jī)器人的D-H 參數(shù)和運動學(xué)模型，采用擴(kuò)展卡爾曼濾波方法進(jìn)行的機(jī)器人軌跡跟隨參量采樣和自適應(yīng)融合處理，結(jié)合諧振比率控制方法進(jìn)行機(jī)器人軌跡跟隨的誤差反饋調(diào)節(jié)，采用動態(tài)逆控制模型計算機(jī)器人的軌跡跟隨誤差，以機(jī)器人的幾何空間位姿參數(shù)的調(diào)節(jié)對象，進(jìn)行軌跡跟隨糾偏控制，利用關(guān)節(jié)軸線間的幾何關(guān)系求出模型中運動學(xué)參數(shù)，獲得的連桿模型參數(shù)能及時反饋給變剛度關(guān)節(jié)驅(qū)動機(jī)器人的控制系統(tǒng)，實現(xiàn)控制律優(yōu)化設(shè)計。研究得知，本文方法進(jìn)行機(jī)器人控制的軌跡糾偏性能較好，誤差較低，控制魯棒性較高。