多模式驅動的機器人關節控制器設計及實驗

夏磊 范堅 戈亦文 郝風吉 閆雪 羅翔

(東南大學機械工程學院，南京 211189)

在機器人技術領域，作為動力源的直流電機被廣泛應用于機器人各關節或執行機構中.常見的關節驅動方式大多采用直流伺服電機加減速傳動裝置［1］，如法國的 BIP2000［2］、MIT 的 M2［3］、日本的ASIMO［4-5］等.而直流伺服電機中最常用的是直流電機.與無刷電機與步進電機相比，直流電機具有結構緊湊、質量輕便、控制簡單等特點，因此應用于很多機器人，如HRP-2［6］和BIP2000都選用了直流電機.

人類步行時，各個關節協調工作，不僅能按照預想的軌跡運動，同時還能提供所需的主動力，完成相應的動作.因此對于步行機器人的驅動關節也有同樣的要求，即需要既能精確實現運動軌跡的跟蹤又能提供所需的力矩.對于直流電機的驅動控制，研究者大多關注直流電機的位置控制或速度控制(即按照期望的軌跡或速度運動)，并提出了很多直流電機閉環反饋調速的方法.但是，對于機器人而言，各驅動關節不僅要能實現位置控制或速度控制，完成預期的動作，很多時候還必須實現力矩控制，即讓電機輸出期望的力矩.比如機械手抓取物體時，手指必須施加適當的力以保證物體不會因力過小而滑落或因力過大而被擠壞.步行機器人步行時，在行走階段，各驅動關節根據規劃的軌跡，在電機位置控制模式下完成預期動作；在蹬地階段，除了各關節協調動作以外，踝關節必須提供合適的力矩才能實現機器人的成功邁步.除此之外，很多研究者通過控制離支撐面最近的踝關節的角度和力矩來調節ZMP位置，從而保證機器人步行的穩定性［7-8］.所以，驅動關節在不同條件下需要不同的控制模式.

目前在關節位置控制上，大多利用位移傳感器和采用反饋調節的方法實現位置控制，如基于光電編碼器的PID或模糊自適應PID位置反饋控制［9］.而在力/力矩控制上，常采用力/力矩傳感器以實現檢測反饋［10-11］.但是，這種方法往往需要檢測x，y，z三個方向力和力矩，通過計算得出合力/力矩，因此比較復雜.Kong等［12］將直流電機和扭簧相結合，利用扭簧在一定范圍內扭角與扭矩的線性關系，將力矩控制轉化為扭角的位置控制，雖然力矩控制效果較好，但增加了結構和控制上的復雜性.

針對現行的機器人關節驅動控制的特殊要求，本文設計了具備位置控制和力矩控制功能為一體的關節控制器，并研究了這2種模式的切換控制方法.通過仿真和實驗結果可以看出，所設計的控制器具備位置控制與力矩控制功能，且二者間能穩定切換.

1 系統建模與控制方法

1.1 控制理論與系統建模

機器人運動時，通過對驅動關節的控制可實現機器人按照預期的規劃完成相應動作.機器人關節控制主要是對關節驅動電機的控制.關節位置控制是對電機的轉角進行控制，以按照期望的轉角完成相應的軌跡運動.而考慮到直流電機的驅動力矩與通過電機的電流成正比，所以關節力矩控制是對通過電機的電流進行控制，實現電機輸出期望轉矩.

本文提出的關節位置控制與力矩控制，建立在直流電機的數學模型上.

由經典的直流等效電路［13］可得到在零初始條件下的電壓與電流的傳遞函數，以及電流與反電動勢的傳遞函數，即

式中，Id為回路的電樞電流；E為反電動勢；Tl=L/R為電樞回路的電磁時間常數，R為直流電機電樞電阻，L為回路的總電感；IdL=TL/Cm為負載電流，TL為電機的負載轉矩，Cm為常數；Tm=JmR/(CeCm)為機電時間常數，Ce為電機常數.因此，針對位置控制與力矩控制的要求，控制變量就是模型中的轉速n(s)和電樞電流Id(s).

1.2 控制方法

本文采用的控制方法是建立在PID控制思想上的.圖1為設計的控制系統框圖.

圖1 控制系統框圖

位置控制與力矩控制分別利用PID控制算法進行閉環反饋控制，以達到高控制精度和動態調節的平衡要求.但何時采用位置控制或力矩控制，則需要根據狀態監測器(算法程序)實時監測和響應來完成控制模式的選擇.當需要位置控制時，閉合位置反饋控制系統開關，打開力矩反饋控制系統開關；而需要力矩控制時，則閉合后者，打開前者.在位置控制與力矩控制下因控制的輸入不同，其控制增益參數也不同，故在相應控制模式下需選用不同的PID參數，而PID參數需預先進行整定.

在控制模式切換中，需考慮2種切換模式:①基于時間的切換.即在某一時刻由位置控制切換為力矩控制或由力矩控制切換為位置控制.② 基于電流的切換.即當檢測電樞電流大于某個閾值時由位置控制切換為力矩控制，當檢測到電流小于某個閾值時由力矩控制切換為位置控制，此方法和實際應用類似，如機械手在抓取到物體時，由于物體反作用力的作用，負載力矩增大，即驅動電機的電流隨著增大，此時切換為力矩控制，一方面提供期望轉矩，另一方面起限流作用，保護電機.在電機運行過程中，狀態監測器根據實時判斷模式切換條件來選擇閉合位置控制環還是力矩控制環.

2 仿真

2.1 仿真搭建

為了驗證所提出控制方法的可行性，利用Matlab/Simulink軟件對直流電機數學模型采用PID控制算法進行了驅動控制系統的仿真.本文以MAXON直流電機(RE2520W 118752)作為控制對象，在仿真時，將MAXON RE2520W 直流電機相關參數［14］代入經典數學模型中，同時根據需要加入PID控制環節、DAC延時環節、PWM驅動延時環節、ADC延時環節和反饋延時環節［13］，位置控制仿真框圖如圖2所示.

圖2 位置控制Matlab仿真框圖

2.2 仿真結果

2.2.1 位置控制仿真

位置控制仿真時，期望的關節轉角為一正弦信號，如圖3(a)所示.由圖可以看出，仿真時實際轉角在PID調節下其跟蹤期望轉角的性能良好.

2.2.2 力矩控制仿真

力矩控制仿真時，期望電流為1 A，而負載電流IL先緩慢增加，斜率為0.2 A/s，然后在1 s時刻，負載電流突變至1 A，結果如圖3(b)所示.由圖可以看出，在1 s前由于負載較小，驅動電機電樞電流大于負載電流，根據動力學平衡方程，此時電機的轉速會逐漸增大；而在1 s時刻，負載電流突然增加至1 A，此時在電壓平衡方程、動力學平衡方程以及PID調節下，電樞電流快速響應，直到與負載電流相等，達到動力學平衡，此時電機轉速維持恒定，電機輸出恒定轉矩.

2.2.3 位置控制與力矩控制切換仿真

位置控制與力矩控制切換仿真時，切換方法是基于時間的切換.先進行位置控制后進行力矩控制，切換時刻為1 s.位置控制時給予電機以恒定轉速，即期望轉角為斜坡信號，負載電流以0.4 A/s的速度線性增加.當時間運行至1 s，進行位置控制與力矩控制的切換，同時，負載電流突變至1 A.此時，期望電流為1 A，通過PID控制使電樞電流達到1 A，完成由位置控制轉為力矩控制，得到電機電流、轉速仿真結果如圖3(c)、(d)所示.

由圖3可以看出，位置控制時，在PID調節下電機轉速快速增加直至恒定值，此時電機的啟動電流較大.而后在電壓平衡方程及動力學平衡方程調節下，電樞電流與負載電流相等，轉速維持恒定.當位置控制切換為力矩控制時，負載電流突變至1 A，在PID調節下，電樞電流快速響應，直至與負載電流相等并保持恒定.在負載突然增大至完成力矩控制調節的過程中，電機轉速下降，調節完成后，電機轉速維持不變.

3 控制系統軟硬件設計

3.1 硬件設計

控制系統采用C8051F040作為主控制器，直流電機作為控制對象.為了實現位置控制和力矩控制，在硬件上必須實現電機的驅動和系統的反饋.所設計的硬件主要包含以下幾個部分.

圖3 Matlab/Simulink仿真結果

1)單片機C8051F040.作為主控制器有如下功能:①實時中斷響應、控制模式切換等；②利用單片機PCA工作在8位脈寬調制器的方式產生PWM(脈寬調制)控制信號并輸出；③利用單片機定時器/計數器對光電編碼器反饋產生的脈沖數進行計數，以監測實時位置，進行位置控制；④ 利用單片機AD轉換模塊，對經過放大后的反饋電流(轉換成電壓)進行AD轉換，以監測實時電流，進行電流控制(力矩控制)；⑤ 利用單片機串口、CAN通訊模塊進行數據的實時采集，以便后續處理和分析.

2)驅動放大.采用L298N/L298P的H橋驅動電路，利用PWM信號，通過控制驅動橋三極管的通斷，完成驅動電壓的放大和PWM控制.

3)直流伺服電機.實驗時采用轉動慣量小、控制方便的MAXON公司的直流電機(MAXON RE2520W 118752).

4)編碼器.通過脈沖計數來精確計算位置，實時檢測電機轉角，反饋給單片機進行位置控制.實驗時采用MAXON公司附帶的HEDL-5540型、500線、三通道光電編碼器，可通過比較A，B通道信號相位之間的關系來判定旋轉方向.

5)電流采樣.利用電阻采樣的方法，結合L298芯片電流反饋功能，對通過直流電機的電流進行實時檢測并反饋，以完成電流控制(力矩控制).

6)放大.為了提高檢測精度，達到AD轉換的要求，將采集到的電流信號變成微小的電壓信號，并采用LM358D放大器對電壓信號進行放大.

3.2 軟件設計

在控制系統軟件設計中，主要利用循環中斷的思想.中斷函數處理包括判斷采用何種控制模式(位置控制或力矩控制)、設定期望轉角或期望電流、增量式PID函數處理、占空比更新、AD轉換完成數據處理、數據發送至PC機等.其中，最核心的部分是判斷采用位置控制還是力矩控制來完成期望的動作.這部分的實現是通過定時器0溢出中斷(每0.5 ms中斷一次)來完成.中斷響應中設定切換條件為電機電流是否達到切換閾值，若達到，則采用力矩控制，未達到則采用位置控制.當然切換條件是可以更改的，如基于位置控制可改為基于時間控制的切換.

4 實驗及結果分析

實驗系統采用驅動控制，該系統主要由驅動控制器、直流電流、實驗裝置組成.

直流電機額定轉矩為26.7 mN·m，經減速比317.86的減速箱減速增扭后，與傳動比1∶1的同步帶傳動機構的同步帶輪相連，同步帶傳動機構另一端與負載擺臂相連.負載擺臂重力G=5.782 N，等效臂長(重心距離擺動中心距離)約0.13 m.

4.1 力矩控制驗證實驗

為了驗證所設計的控制系統是否能夠通過設定期望電流值，利用PID的力矩控制功能，進行了力矩控制的驗證實驗.擺臂在重力作用下位置產生不同的負載力矩為

式中，G為重力；l為重力臂；β為擺臂與豎直方向的夾角.

擺臂初始位置豎直向下.在程序Matlab/Simulink中，給定一個期望電流，在PID調節下，擺臂會隨著驅動力矩的增加而上升，直到驅動力矩與負載力矩相等.根據電機動力學平衡方程［13］，此時擺臂會停在某個角度.因此，可根據角度β、重力G、重心位置l計算出此時負載力矩的大小，即近似等于驅動力矩大小.另一方面，采用由示波器檢測到的電樞電流有效值，計算出實際輸出力矩與負載力矩，并對其進行比較，由此驗證程序中設定的期望電流值與輸出轉矩的線性關系，通過實驗數據擬合得到的曲線如圖4(a)、(b)所示.

通過擬合的曲線可知，所設計的控制器能通過設定電流值而改變驅動力矩大小，達到力矩控制的目的.

圖4 電流與電壓/負載轉矩近似線性關系

4.2 位置控制實驗

位置控制的性能主要由其對期望轉角的跟蹤性能決定.實驗中，首先，設定期望的脈沖為一正弦信號，幅值為30000脈沖(折算到擺臂轉角約為67.95°)，而周期分別為 10，5，2.5 和 1.25 s，觀察不同頻率正弦信號的跟蹤性能；其次，同樣設定一定期望脈沖為正弦信號，周期為2.5 s，觀察幅值為(折算到擺臂的角度)40.77°，45.30°，56.63°，67.95°的正弦信號跟蹤性能.通過實驗發現，在給定幅值(30000脈沖，約67.95°)的情況下，位置控制的軌跡跟蹤和正弦信號的周期/頻率有關，信號周期大于等于5 s時，位置控制實現較理想，當頻率增大時，響應逐漸變差；類似地，在給定相同周期(均為2.5 s)的情況下，位置控制的軌跡跟蹤情況和正弦信號的幅值有關，信號幅值小于等于20000脈沖(折算到擺臂約45.30°)時，位置控制實現較理想，當幅值繼續增大時，響應逐漸變差.因此，需進一步分析響應變差原因.

經過多次實驗，最終發現驅動系統硬件限制、電機轉速達到最大是電機跟蹤響應變差的主要原因.圖5為期望轉角為正弦信號、周期均為2.5 s、不同幅值(40.77°，45.30°，56.63°，67.95°)下的響應跟蹤曲線.可以看到，隨著幅值的增加，響應越來越差.與此同時，可以看到占空比長期處于0值的時間越來越長，說明電機長期處在全電壓(實驗中為18 V)工作狀態，因此，不管PID如何調節，驅動電壓始終達到最大值，轉速為全速.由此判定，響應變差是由于驅動系統硬件條件約束所致.

圖5 不同擺臂幅度的跟蹤響應

4.3 力矩控制實驗

首先，設定期望電流為正弦信號，幅值設定為40(數字量)，改變正弦信號頻率(周期分別為10，5，2.5，1.25 s)，觀察跟蹤響應情況；其次，同樣設定期望電流為正弦信號，周期均為2.5 s，改變正弦信號幅值(分別為10，20，30，均為程序中的數字量)，觀察跟蹤響應.由于電流始終為正，因此程序中必須將電流取絕對值.通過實驗發現，給定相同幅值而頻率不同的正弦電流值時，當周期大于等于5 s，其實際電流跟蹤期望電流的性能較理想，可以很好地實現力矩控制，當頻率再增大時，跟蹤響應變差；但是與位置控制不同的是，給定周期相同而幅值不同的正弦期望電流時，隨著幅值的減少，其響應并沒有逐漸變好，同時，占空比在多個幅值下均未長期處于0的狀態(見圖6)，也就是說，對于響應的制約硬件系統并不是主要的原因，因而需進一步分析響應變差原因.

圖6 不同電流幅值的跟蹤響應曲線

經過多種因素的驗證與排查，發現響應變差的主要因素為PID參數.對PID參數為Kp=0.2，Ki=0.1，Kd=3 和 Kp=0.2，Ki=0.325，Kd=3 的階躍響應進行實驗.實驗中，給定期望電流為相同的階躍值(均為20)，觀察不同PID參數下控制系統的階躍響應特性.根據控制理論，上升時間ts為從起始響應時間開始至進入允許誤差范圍(一般誤差允許范圍為小于5%)的時間.由此可得，Kp=0.2，Ki=0.1，Kd=3 的階躍響應上升時間 ts約為700 ms，而 Kp=0.2，Ki=0.325，Kd=3 的階躍響應上升時間ts約為60 ms，由此可以看出，后者響應明顯加快.

在以上結果的基礎上，改變PID參數為Kp=0.2，Ki=0.325，Kd=3，重新對周期相同而幅值不同的正弦電流跟蹤響應進行實驗，得到類似于圖6的實驗曲線.此時，PID參數調整后其跟蹤響應的性能明顯提高.

4.4 位置/力矩控制切換實驗

4.4.1 基于時間的切換

圖7為2種控制模式基于時間(切換時刻為20 s)的切換實驗結果.實驗時，在位置控制階段(20 s前)，給定的參考位置幅值為15000脈沖(約34°)、周期為10 s的正弦信號；力矩控制階段(20 s后)，給定的期望電流為幅值45、周期為10 s的余弦信號，由于電流沒有負值，因此取其絕對值作為期望值.

圖7 20 s時由位置控制切換為力矩控制

實驗時，程序中先設定位置控制，使電機按照預期正弦信號來回擺動，此時可以從圖7中看到在PID控制下電機實際轉角能夠較好地跟隨期望轉角；當切換時間達到20 s時，位置控制切換為力矩控制，為了防止電流突變，將記錄的電機實際電流作為期望電流，然后設定期望電流作為線性函數的起點，線性增加直到余弦信號的幅值為45.此后期望電流設定為幅值為45、周期10 s的余弦信號.由圖7中的曲線可看出，20 s切換后，首先電機電流線性增加，直至余弦信號幅值為45，然后數字量電流在PID控制下跟蹤期望電流，完成力矩控制.

4.4.2 基于電流的切換

圖8為2種控制模式切換前后檢測到的電機電流的變化曲線.在程序中，設定電流的閾值為60，若檢測到反饋的數字量電流值大于60時，則將位置控制切換為力矩控制.

圖8 基于電流的切換實驗

在位置控制階段，同樣給予一個正弦變化的期望轉角，使電機跟隨期望脈沖轉動；此時電機電流周期性變化；在滿足切換條件反饋數字量電流大于設定值60時，則由位置控制切換為力矩控制，此時給定一個期望數字量電流值70，通過PID控制使得電機電樞數字量電流達到70，從而輸出恒定力矩，完成力矩控制；當檢測到的數字量電流值滿足切換條件(反饋數字量電流小于設定值60)時，則由力矩控制切換為位置控制，繼續進行位置控制，如此循環.在實驗時，為了滿足切換條件，通過給擺臂一個人為的瞬間沖擊使電機停轉，因而使得負載力矩急劇上升.

圖8中的中心線框為位置控制時的電樞數字量電流值，設電流值隨著位置正弦信號的變化而呈周期性的變化；虛線框為力矩控制時的電樞數字量電流值，該電樞數字量電流值始終與設定的數字量電流值一致，由此可以看出，基于電流的切換是可行的.

5 結論

1)所設計的控制器能通過設定電流值來改變驅動力矩大小，達到力矩控制的目的；

2)所設計的控制器能實現位置控制，且影響其響應速度的因素主要是驅動系統硬件；

3)所設計的控制器能夠實現力矩控制，且影響其響應速度的因素主要是PID參數；

4)所設計的基于電流切換的控制器，能夠基于時間和電流平穩地實現位置控制和力矩控制的切換.因此，關節控制器能夠應用于機器人關節，從而實現關節的驅動和靈活的控制.

References)

［1］楊敏，殷晨波，董云海，等.擬人步行機器人下肢研究現狀［J］.機械傳動，2006，30(2):86-88.Yang Min，Yin Chenbo，Dong Yunhai，et al.Bipedal walking research with humanoid robots［J］.Journal of Mechanical Transmission，2006，30(2):86-88.(in Chinese)

［2］Espiau B，Sardain P.The anthropomorphic biped robot BIP2000［C］//Proceedings of the 2000 IEEE International Conference on Robotics and Automation.San Francisco，CA，USA，2000:3996-4001.

［3］楊新海.雙足步行方法與控制系統研究［D］.哈爾濱:哈爾濱工程大學機電工程學院，2006.

［4］Sakagami Y，Watanabe R，Aoyama C，et al.The intelligent ASIMO:system overview and integration［C］//Proceedings of the IEEE International Conference on Intelligent Robots and Systems.Lausanne，Switzer land，2002:2478-2483.

［5］Hirai K，Hirose M，Haikawa Y，et al.Development of Honda humanoid robot［C］//Proceedings of the IEEE International Conference on Robotics and Automation.Leuven，Belgium，1998:1321-1326.

［6］Kaneko K，Kanehiro F，Kajita S，et al.Humanoid robot HRP-2［C］//Proceedings of the 2004 IEEE International Conference on Robotics and Automation.New Orleans，LA，USA，2004:1083-1090.

［7］Lee B J，Stonier D，KIim Y D，et al.Modifiable walking pattern generation using real-time ZMP manipulation for humanoid robots［C］//Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems.San Diego，CA，USA，2007:4221-4226.

［8］Kumagai M，Emura T.Sensor-based walking of human type biped robot that has 14 degree of freedoms［C］//IEEE Proceedings of the Fourth Annual Conference on Mechatronics and Machine Vision in Practice.Toowoomba，Australia，1997:112-117.

［9］顧星，鐘鳴，姚玉峰.基于模糊自適應PID的加樣臂位置控制［J］.工程設計學報，2012，19(5):385-390.Gu Xing，Zhong Ming，Yao Yufeng.Position control for sampling arm based on self-adapting fuzzy-PID［J］.Chinese Journal of Engineering Design，2012，19(5):385-390.(in Chinese)

［10］李江道.空間機械臂機電一體化關節的設計與控制［D］.上海:上海交通大學電子信息與電氣工程學院，2012.

［11］孫金鳳，姜力.基于關節力矩傳感器的靈巧手指的阻抗控制［J］.傳感器與微系統，2008，27(7):66-74.Sun Jinfeng，Jiang Li.Impedance control of dexterous finger based on joint torque sensor［J］.Transducer and Microsystem Technologies，2008，27(7):66-74.(in Chinese)

［12］Kong K，Bae J，Tomizuka M.Control of rotary series elastic actuator for ideal force-mode actuation in human-robot interaction applications［J］.IEEE/ASME Transaction on Mechatronics，2009，14(1):105-118.

［13］周小波.基于DSP的直流伺服驅動的機器人關節控制系統［D］.合肥:安徽大學電氣工程與自動化學院，2010.

［14］蘇州均和伺服科技有限公司.MAXON電機手冊—產品目錄08/09［R］.蘇州:蘇州均和伺服科技有限公司，2008.