基于一級(jí)倒立擺模型仿人機(jī)器人控制算法研究*

胡 靜 ，姜大偉 ，王華振 ，姚 禹 ，譚海東

（1.長春工業(yè)大學(xué) 軟件職業(yè)技術(shù)學(xué)院，吉林 長春 130012；2.長春工業(yè)大學(xué) 機(jī)電工程學(xué)院，吉林 長春 130012）

0 引言

仿人機(jī)器人是機(jī)器人研究領(lǐng)域中的一個(gè)重要分支，仿人機(jī)器人的步態(tài)行走系統(tǒng)是在以往深入研究仿人行走系統(tǒng)的各項(xiàng)技術(shù)環(huán)節(jié)，以提高仿人機(jī)器人行走環(huán)節(jié)的擬人化程度，使其各項(xiàng)功能最大程度地接近人類運(yùn)動(dòng)，從而保證仿人機(jī)器人在不同環(huán)境下完成任務(wù)的能力的基礎(chǔ)上提出的，是步行機(jī)器人研究中的一個(gè)重要而又關(guān)鍵的技術(shù)。但是仿人機(jī)器人的步態(tài)行走系統(tǒng)是一個(gè)多變量、非線性、強(qiáng)耦合的復(fù)雜動(dòng)力學(xué)系統(tǒng)，要實(shí)現(xiàn)和提高機(jī)器人的行走性能，必須研究實(shí)用而有效的步態(tài)控制方法，實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)穩(wěn)定步行[1-2]。很多學(xué)者采用較少的變量來獲得仿人機(jī)器人的步態(tài)行走軌跡，將倒立擺模型應(yīng)用于機(jī)器人步態(tài)規(guī)劃中，取得滿意的控制效果[3-5]。

本文在深入研究倒立擺模型基礎(chǔ)上，應(yīng)用PID控制算法對(duì)仿人行走系統(tǒng)進(jìn)行控制，并在開發(fā)以STM32+CPLD為核心19軸集成運(yùn)動(dòng)控制平臺(tái)中進(jìn)行驗(yàn)證，通過對(duì)比理論與原理樣機(jī)實(shí)驗(yàn)數(shù)值，關(guān)節(jié)輸出最大相對(duì)誤差為2.25%，表明了該控制方法的準(zhǔn)確性和有效性。

1 仿人機(jī)器人行走問題描述

設(shè)計(jì)的10自由度仿人行走系統(tǒng)的機(jī)構(gòu)模型如圖1所示。

采用左右腿對(duì)稱的結(jié)構(gòu)方式。以右腿為例，其自由度分配為：髖關(guān)節(jié)2個(gè)自由度，膝關(guān)節(jié)1個(gè)自由度，踝關(guān)節(jié)2個(gè)自由度。主要結(jié)構(gòu)參數(shù)：大腿長度：214 mm；小腿長度：214 mm；腳部長度：55 mm；大腿質(zhì)量：6.5 kg；小腿質(zhì)量：3.5 kg；腳部質(zhì)量：2.5 kg。仿人行走系統(tǒng)的運(yùn)動(dòng)過程為：上位機(jī)下達(dá)控制指令給驅(qū)動(dòng)電機(jī)；驅(qū)動(dòng)電機(jī)通過同步齒形帶驅(qū)動(dòng)各關(guān)節(jié)轉(zhuǎn)動(dòng)，從而實(shí)現(xiàn)仿人行走系統(tǒng)的步行運(yùn)動(dòng)。各關(guān)節(jié)輸出范圍如表1所示。驅(qū)動(dòng)電機(jī)主要參數(shù)如表2所示，驅(qū)動(dòng)電機(jī)輸出端安裝減速比為1:50的減速器，提高了電機(jī)的最大力矩值。

圖1 10自由度仿人行走系統(tǒng)的機(jī)構(gòu)模型

表1 仿人行走系統(tǒng)各關(guān)節(jié)輸出范圍

表2 57號(hào)步進(jìn)電機(jī)技術(shù)參數(shù)

2 基于倒立擺模型的仿人機(jī)器人數(shù)學(xué)模型的建立

和人類步行相似，仿人機(jī)器人的步態(tài)行走是機(jī)器人的重心由支撐腿向前運(yùn)動(dòng)，擺動(dòng)腿由支撐腿的后方擺到支撐腿的前方，兩條腿的交換在瞬間完成的。在這樣的行走過程中，將仿人機(jī)器人行走系統(tǒng)簡化為一個(gè)倒立擺模型[6]，如圖 2所示。

圖2 倒立擺模型

質(zhì)量為m的小球固結(jié)于長度為L的細(xì)桿（可忽略桿的質(zhì)量）上，細(xì)桿又和質(zhì)量為M的小車鉸接相連。通過控制施加在小車上的力F（包括大小和方向）能夠使細(xì)桿處于θ＝0的穩(wěn)定倒立狀態(tài)。在忽略其他零件的質(zhì)量以及各種摩擦和阻尼的條件下，推導(dǎo)小車倒立擺系統(tǒng)的數(shù)學(xué)模型。設(shè)細(xì)桿擺沿順時(shí)針方向轉(zhuǎn)動(dòng)為正方向，水平向右方向?yàn)樗椒较蛏系恼较?。?dāng)細(xì)桿擺順時(shí)針往右運(yùn)動(dòng)時(shí)水平方向施加的力應(yīng)該為水平向右。

現(xiàn)對(duì)小車和細(xì)桿擺分別進(jìn)行隔離受力分析：

對(duì)小車有：

圖3 小車和小球的受力分析圖

對(duì)小球水平方向受力為：

式中 s=x+l sinθ。 由式(1)、(2)得：

小球垂直方向受力為：

由式(3)、式(4)得：

故可得以下運(yùn)動(dòng)方程組：

以上方程組為非線性方程組，故需做如下線性化處理：

下面進(jìn)行系統(tǒng)狀態(tài)空間方程的求解：以擺角 θ、角速度θ˙、小車位移 x、加速度x¨為系統(tǒng)狀態(tài)變量，Y為輸出，F(xiàn)為輸入。

由線性化后運(yùn)動(dòng)方程組得：

故空間狀態(tài)方程如下：

用MATLAB將狀態(tài)方程轉(zhuǎn)化成傳遞函數(shù)，取M=2 kg，m=0.1 kg，l=0.5 m代入，可以得出角度對(duì)力 F的傳遞函數(shù)為：

位移X對(duì)外力F的傳遞函數(shù)為：

3 仿人機(jī)器人步態(tài)行走控制器的設(shè)計(jì)

3.1 PID控制器的設(shè)計(jì)

由上節(jié)對(duì)倒立擺模型的描述，仿人機(jī)器人的步態(tài)行走要求它的姿態(tài)角達(dá)到一個(gè)期望值，當(dāng)仿人機(jī)器人受到干擾離開期望值時(shí)，如果姿態(tài)角不能及時(shí)恢復(fù)，仿人機(jī)器人姿態(tài)失衡，最終會(huì)導(dǎo)致機(jī)器人行走失敗。為了使仿人機(jī)器人姿態(tài)角穩(wěn)定在期望值，保持仿人機(jī)器人姿態(tài)平衡，實(shí)現(xiàn)仿人機(jī)器人穩(wěn)定行走的目的，本文采用以仿人機(jī)器人姿態(tài)角和位移實(shí)現(xiàn)雙閉環(huán)的PID控制算法[7-8]，干預(yù)姿態(tài)角和位移的變化，使其始終保持在穩(wěn)定狀態(tài)，從而實(shí)現(xiàn)仿人機(jī)器人的穩(wěn)定步態(tài)行走。

PID控制器是一種線性控制器，它將給定值r(t)與實(shí)際輸出值 c(r)的偏差的比例(P)、積分(I)、微分(D)通過線性組合構(gòu)成控制量，對(duì)控制對(duì)象進(jìn)行控制。是一種應(yīng)用較為廣泛的控制算法，其控制性能優(yōu)越，結(jié)構(gòu)簡單，方便調(diào)節(jié)[9]。PID調(diào)節(jié)器各校正環(huán)節(jié)的主要作用有：

(1)比例環(huán)節(jié)：偏差一旦產(chǎn)生，調(diào)節(jié)器立即產(chǎn)生控制作用以減小偏差即時(shí)成比例地反應(yīng)控制系統(tǒng)的偏差信號(hào)e(t)。

(2)積分環(huán)節(jié)：主要用于消除靜差，積分作用的強(qiáng)弱取決于積分時(shí)間常數(shù)，進(jìn)提高系統(tǒng)的無差度。

(3)微分環(huán)節(jié)：能反應(yīng)偏差信號(hào)的變化趨勢(shì)(變化速率)，相當(dāng)于引入一個(gè)有效的早期修正信號(hào)，從而加快系統(tǒng)的動(dòng)作速度，減小調(diào)節(jié)時(shí)間。

首先，建立PID控制器的微分方程為：

式中，e(t)=r(t)-c(t)。

再得到PID控制器的傳遞函數(shù)為：

3.2 PID控制器參數(shù)調(diào)節(jié)

在MATLAB/Simulink中建立仿人機(jī)器人模型，同時(shí)采用試湊法對(duì)PID參數(shù)進(jìn)行調(diào)節(jié)[10]。首先，建立沒校正之前的θ-F單閉環(huán)控制系統(tǒng)，由于未加進(jìn)控制環(huán)節(jié)，故系統(tǒng)輸出極易發(fā)散。給系統(tǒng)加入PID控制環(huán)節(jié)，如圖4所示。

圖4 θ-F單閉環(huán)控制系統(tǒng)框圖

設(shè)置系統(tǒng)穩(wěn)定值為0，給系統(tǒng)一個(gè)初始干擾沖擊信號(hào)，采用試湊法不斷調(diào)整PID參數(shù)，當(dāng)系統(tǒng)在時(shí)域內(nèi)達(dá)到穩(wěn)定后，進(jìn)行離散化分析，建立離散模型系統(tǒng)控制框圖如圖5所示。

圖5 離散模型系統(tǒng)控制框圖

如圖6所示，經(jīng)調(diào)節(jié)后，當(dāng) Kp=-110，Ti=-4，Td=-1 500，系統(tǒng)響應(yīng)穩(wěn)定，超調(diào)＜0.3%，調(diào)節(jié)時(shí)間＜0.2 s。至此，離散域的控制參數(shù)調(diào)節(jié)順利實(shí)現(xiàn)。圖7為建立的仿人機(jī)器人雙閉環(huán)控制系統(tǒng)。

圖6 控制系統(tǒng)響應(yīng)曲線圖

4 仿人機(jī)器人步態(tài)實(shí)驗(yàn)

4.1 仿人行走系統(tǒng)控制系統(tǒng)硬件

仿人機(jī)器人運(yùn)動(dòng)控制器在增加了CPLD芯片后，控制系統(tǒng)如圖8所示。實(shí)驗(yàn)?zāi)康氖球?yàn)證本文所提PID控制器的合理性及準(zhǔn)確性，該仿人機(jī)器人下肢有10個(gè)自由度，上肢有9個(gè)自由度，以STM32+CPLD為核心的控制系統(tǒng)總體方案，單一的關(guān)節(jié)控制可以使用一個(gè)51單片機(jī)芯片最小控制系統(tǒng)，控制步進(jìn)電機(jī)運(yùn)動(dòng)[11]。

圖7 位移-角度控制系統(tǒng)框圖

圖8 19自由度控制系統(tǒng)簡圖

4.2 數(shù)值對(duì)比結(jié)果分析

調(diào)節(jié)行走系統(tǒng)原理樣機(jī)末端執(zhí)行器位姿為給定位姿gs3T3(θ)。通過編碼器和傾角傳感器測(cè)得行走系統(tǒng)各關(guān)節(jié)輸出。比較數(shù)據(jù)值，如表 3所示。

通過驗(yàn)證數(shù)值對(duì)比結(jié)果得出，關(guān)節(jié)輸出最大相對(duì)誤差為2.25%，表明了本文采用仿人機(jī)器人步態(tài)行走系統(tǒng)PID控制算法的正確性及高精確度。

5 結(jié)論

本文針對(duì)仿人機(jī)器人步態(tài)行走不穩(wěn)定的問題，以倒立擺為控制對(duì)象，建立仿人機(jī)器人步態(tài)行走數(shù)學(xué)模型。在建立數(shù)學(xué)模型時(shí)，首先用牛頓—?dú)W拉方法建立數(shù)學(xué)模型，而后用動(dòng)態(tài)系統(tǒng)空間狀態(tài)方程法導(dǎo)出狀態(tài)方程系數(shù)矩陣，然后用MATLAB對(duì)數(shù)學(xué)模型進(jìn)行從狀態(tài)空間到傳遞函數(shù)的變換（包括傳遞函數(shù)的拉氏變換與Z變換），得到系統(tǒng)的傳遞函數(shù)模型。最后建立仿人機(jī)器人姿態(tài)角和位移的雙閉環(huán)控制系統(tǒng)，采用PID控制算法對(duì)仿人機(jī)器人姿態(tài)角和位移進(jìn)行調(diào)節(jié)。系統(tǒng)響應(yīng)穩(wěn)定，超調(diào)量＜0.3%，調(diào)節(jié)時(shí)間＜0.2 s，得到滿意的控制效果。以19自由度仿人機(jī)器人進(jìn)行實(shí)驗(yàn)驗(yàn)證，其關(guān)節(jié)的輸出相對(duì)誤差最大為2.25%，可實(shí)現(xiàn)仿人機(jī)器人穩(wěn)定的步態(tài)行走，表明了本文采用仿人機(jī)器人步態(tài)行走系統(tǒng)PID控制算法的正確性及高精確度。

表3 右腿各關(guān)節(jié)輸出的計(jì)算值與實(shí)驗(yàn)值比較

[1]畢盛，閔華清，陳強(qiáng).仿人機(jī)器人步態(tài)規(guī)劃反饋控制研究綜述[J].計(jì)算機(jī)工程與應(yīng)用，2011(7)：30-33.

[2]付根平，楊宜民，李靜.仿人機(jī)器人的步行控制方法綜述及展望[J].機(jī)床與液壓，2011(23)：154-159.

[3]Tang Zhe，ER M J.Humanoid 3D gait generationbased on inverted pendulum model[C].22nd IEEE International Symposium on Intelligent Control Part of IEEE Multi-conference on Systems and Control，2007.

[4]畢盛，閔華清，陳強(qiáng)，等.一種仿人機(jī)器人斜坡運(yùn)動(dòng)步態(tài)規(guī)劃方法[J].華南理工大學(xué)學(xué)報(bào)(自然科學(xué)版)，2010(11)：148-154，160.

[5]李龍澍，王唯翔，王凡.基于三維線性倒立擺的雙足機(jī)器人步態(tài)規(guī)劃[J].計(jì)算機(jī)技術(shù)與發(fā)展，2011(6)：66-69.

[6]黎君，閻世梁.一級(jí)倒立擺模糊 PID控制器設(shè)計(jì)[J].國外電子測(cè)量技術(shù)，2012(4)：50-52.

[7]郭宏，吳海洋，巫佩軍.基于變系數(shù) PID的無刷直流電動(dòng)機(jī)雙閉環(huán)系統(tǒng)[J].北京航空航天大學(xué)學(xué)報(bào)，2012(1)：1-5.

[8]王啟源，阮曉鋼.獨(dú)輪自平衡機(jī)器人雙閉環(huán)非線性PID控制[J].控制與決策，2012(4)：593-597.

[9]劉慧博，王靜，吳彥合.無刷直流電機(jī)模糊自適應(yīng) PID控制研究與仿真[J].控制工程，2014(4)：583-587.

[10]張斌，姚有峰.基于串行總線下最小控制系統(tǒng)實(shí)現(xiàn)方式的研究[J].現(xiàn)代電子技術(shù)，2009(11)：149-151.

[11]袁安富，徐金琦，王偉，等.基于雙 STM32多旋翼無人機(jī)控制系統(tǒng)設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2013，39(11)：136-138.