一種雙軸輪室內服務機器人小車伺服控制系統

李碩明　陳越　吳雄英

摘 要 基于32位ARM7微型處理器LPC2119開發移動機器人伺服控制系統，設計了適應于雙軸獨立驅動的移動小車伺服控制PI算法，采用PWM脈寬調速技術結合光電編碼反饋來構成閉環電機伺服系統.系統方案在自主設計的室內移動機器人小車XMU3上進行了驗證，實驗結果能達到室內服務機器人平臺應用的控制精度要求.

關鍵詞 雙軸輪；伺服控制系統；ARM7；PI算法

中圖分類號 TP393文獻標識碼 A文章編號 10002537（2016）02005906

A ServoControl System for Biaxial Wheel Indoor Service Robots

LI Shuoming1， CHEN Yue 1，2， WU Xiongying3*

（1.School of Information Engineering， Zhongshan Polytechnic， Zhongshan 528404， China；

2.School of Information and Electronics， South China University of Technology， Guangzhou 510641， China；

3.Shenzhen Branch of Emerson Network Power Company Ltd.， Guangzhou 518057， China）

Abstract This paper designs a servocontrol system for mobile robots based on 32bit ARM7 microprocesser. The approach designs PI algorithm which suits biaxial wheel mobile robots servocontrol system， adopting PWM speed control and utilizing lightelectronic devices for feedbacking components to set up a closed motor servocontrol system. The system is tested on the selfdesigned XMU3 indoor intelligent mobile robots vehicle， which proves to be able to match the control accuracy requirements and meet the potential application platform needs of indoor service robot.

Key words biaxial wheel； servocontrol system； ARM7； PIalgorithm

一直以來， 移動機器人以其廣泛的應用性和高技術性而備受關注[1].近年來移動機器人技術已經成為計算機、自動化以及人工智能等交叉領域的一個研究熱點.輪式移動機器人具備相對簡單的機械結構，靈活的運動，便捷的操作，較高的效率[2].不僅在工業自動化、智能交通、遠程醫療等行業中得到廣泛的應用，在智能家居、城市安全、空間探測領域等有害與危險場合得到很好的應用[34].伺服系統位于移動機器人控制系統的底層，直接負責驅動電機、采集傳感器信息和人機交互[5]，是整個輪式移動機器人研發的核心部分之一，在理論研究和工程實踐方面都有重要意義[67].

由服務機器人代替人力完成家務勞動，將是一項具有良好應用前景的高技術，特別是在21世紀中國即將步入老齡化社會，服務機器人市場前景廣闊[8].本課題研究的室內服務自主式機器人的基礎應用平臺已經開發完成，本文的內容對應其雙軸輪伺服控制系統的設計.

1 XMU3移動小車方案

室內自主移動機器人小車XMU3采用“1+2”三輪式底盤設計，其底盤形狀如圖1所示.前輪為可以自由活動的萬向輪，兩個后輪為小車提供驅動力由兩臺直流電機分別控制.直流電機與其對應的驅動輪采用速度或位置反饋閉環控制， 可以實現對小車速度和方向的控制[9].通過編程設置兩個伺服電機的電流供給可以控制驅動輪之間的相對轉速，進而實現小車的運動控制.當兩個電機以同轉速同方向運動時能實現小車的直線運動，當方向相反轉動時能使車體繞兩輪軸線中點自轉.底盤設計上參照Scott Jantz的研究結果[10]：圓形平面能避免棱角產生的碰撞，對于移動機器人平臺來說基座設計成圓形便于鎖定目標.

1.1 小車移動能力模型

機器人小車在運行時要同時克服摩擦力和重力，其受力分析如圖2所示.計算這兩個力的合力，可以得到式（1）.

Fapp=+Fw， （1）

式中，Fapp為機器人移動需要的動力，表示啟動時車輪所處的滑動摩擦力， 為機器人移動受到的來自重力的阻力.機器人在水平面上移動時Fw為0.當車輪開始轉動，此時的滾動摩擦力比啟動前的摩擦阻力要小，因此在絕大多數時間電機都是處于過功率的狀態.驅動系統一定程度上受到驅動輪的直徑大小的影響，即當驅動輪的直徑增大時，輸出速度對應增大，驅動力減小，反之亦然.XMU3小車主要擬用于室內地面移動，所以，輸出的轉矩除以驅動輪的半徑，便可以得到驅動輪的驅動力.

1.2 直流電機選型分析

擬定小車總質量為m=8 kg，啟動時為靜摩擦，摩擦系數取μ=0.4；行進中為滾動摩擦，滾動摩阻系數取μ=0.01， 已知驅動輪外徑D=0.074 m，則r=0.037 m；并假設額定速度v=0.4 m/s.

直流伺服電機能實現大范圍無級調速，同時能方便的實現直流調速系統地精確控制策略，滿足室內應用場景對調速性能的要求 [2，11].綜合考慮到成本因素，本課題以直流電機為對象設計伺服系統.據受力分析計算，直流電機TG3812620060K 額定轉速≥91.3（r/min），可以滿足設計要求.

2 電機伺服控制系統設計及實現

輪式移動機器人伺服控制系統是集電子電路、嵌入式技術、探測控制技術于一體的綜合技術，其核心是對伺服電機轉動的控制，通過程序控制為電機提供特定大小的電流來獲得電機特定的轉速，進而對輪式移動小車行進速度和方位的控制.因此高精度的控制移動機器人，必須采取精準的控制策略和高性能的微處理器[12].

通常伺服系統中可以在驅動器前選擇性地設置幾處調節電流和反饋環路，從外向內依次為位置調節、速度調節、電流調節以及位置反饋、速度反饋和電流反饋，其構成如圖3所示.位置反饋信號可以從電機軸或負載上獲取出.本課題為了提高系統的響應性能，剔除了電流環和速度環，只采用位置反饋的單閉環結構，形成一種基于位置反饋的隨動伺服系統.

2.1 伺服控制系統設計

一個伺服系統通常由伺服電機、執行器和微處理控制器構成.本課題設計的數字化電機伺服系統以中央微處理器為控制核心部件，以PWM功放電路為驅動模塊，設計選用直流伺服電機作為系統控制對象，選用位置反饋進行調速，合理選擇主控芯片、測速方式并對電路進行設計，以光電編碼元件作為反饋來構成閉環電機伺服系統，系統結構圖如圖4所示.

2.2 系統工作原理

系統給定一個模擬位置量作為伺服系統的輸入量，經過包括位置調節子電路、速度調節子電路、電流調節子電路等一系列的硬件電路后輸出給功率放大器，驅動電機運動.由ARM7微型處理器（LPC2119）的PWM模塊產生單邊沿控制信號PWM5&6，與雙通道直流電機驅動器L298N的第6引腳、第11引腳相連，WM5、P4、P5引腳控制左輪電機，PWM6、P12、P13引腳控制右輪電機，通過設置寄存器PWMMR5&6的值可以調整PWM信號的占空比，從而改變電機的轉速，即改變機器人的移動速度.

2.3 PWM調速信號的產生

PWM（PulseWidth Modulation脈寬調制）技術是直流電機調速中最為有效的方法，具有功耗低、效率高、穩定可靠等優點.本課題采用單極性控制，在反復對比10～20 kHz頻率段內系統運行時的噪聲后，取11 kHz為脈沖切換頻率，系統電路如圖5.

2.4 基于光電編碼器的反饋閉環控制

本課題利用自制的光電編碼器實現反饋閉環控制，采用數字控制算法完成電機的閉環控制.在車輪的一側表面上粘貼一個自制的碼盤，該碼盤將直徑為70 mm的圓平分為64份（碼盤的直徑稍小于車輪的直徑），并成黑白相間條紋.然后將可辨別黑白色的反射式光電開關安裝在碼盤的正對位置.為了減少可見光對傳感器的影響，我們將傳感器置于電機正下方.當碼盤上的黑白條紋交替對準光電傳感器時，將產生開關的關斷，得到一個交替變化的開關量.然后經過如圖6所示的一個轉換電路，將開關量變為相應的脈沖量輸入到微處理器的CAP1.2和CAP1.3引腳.

本課題所采用的光電傳感器為直流三線式，工作電壓在10～30 VDC范圍內，檢測距離可調.根據傳感器的內部結構和引出線定義，選擇工作電壓12 VDC，與直流電機電壓一致可以避免增加新的電源模塊.將光電傳感器簡化為光電開關S1、S2，考慮到隔離干擾信號，采用TLP5122光電轉換芯片進行信號轉換.其管腳定義及內部結構如圖5所示，有2個光電轉換通道，左輪開關信號連接1、2引腳，右輪開關信號連接3、4引腳，當S1/S2閉合時1、2之間/3、4之間的發光二極管導通，使得7、8間/6、5間的光電三極管導通，這樣圖中的右邊形成回路，根據ARM7微處理器的DC操作條件，其輸入高電平范圍2.0～3.3 V，通過一個300 Ω和一個220 Ω的串連電阻分壓，在兩個電阻中間取出需要的電平信號，此時為高電平，且為2.73 V滿足電平要求.同理，當S1/S2斷開時，1、2之間/3、4之間的發光二極管不導通，使得7、8間/6、5間的光電三極管不導通，這時在兩個電阻中間取出的電平信號為低電平.將這兩個電平信號通過P0.17、P0.18管腳輸入ARM7微處理器，通過設置寄存器PINSEL0將P0.17、P0.18管腳定義為CAP1.2、CAP1.3，從而可以利用定時器的捕獲功能.再設置捕獲控制寄存器T1CCR為CAP1.2、CAP1.3上升沿、下降沿脈沖捕獲，產生事件中斷，并通過數字控制算法實現對系統的閉環控制.其中產生的事件中斷用以完成對應任務.

3 基于PI算法在雙軸驅動電機

在數字控制系統上實現PI控制規律的編程，其理論分析如下.取T0為采樣周期可以用矩形面積求和的方法近似積分作用，即

式（4）中，Kp為比例系數，Ki為積分系數.u（k）即控制回路的輸出量，也是控制對象的輸入量，通常簡寫為CV.對于機器人電機的控制回路來說，CV是決定PWM信號占空比的一個不斷變化的值.將得到的CV作為設定PWM信號占空比的參數，完成對電機的閉環控制過程.

3.1 PI控制算法實現

在同軸輪式移動機器人運動控制中，要求同時對兩個后輪獨立驅動的電機提供有效的伺服控制.其典型的控制包括如下兩種：當左右兩輪電機同向同速運動時使得機器人能夠按照設計的路徑行走；設置兩輪之間特定的轉速差時可以實現小車的轉向控制.偽代碼如下：

Loop：PVLT=ReadLeftSpeed （ ） //讀左輪電機速度反饋值；

PVRT=ReadRightSpeed （ ） //讀右輪電機速度反饋值；

Isum=Isum + （PVLT -PVRT ） //積分項；

LeftErr=SP-PVLT//左輪電機偏差值；

RightErr=SP-PVRT //右輪電機偏差值；

CVLeft=Kp* LeftErr-Ki* Isum //左輪電機控制器輸出變量；

CVRight=Kp* RightErr+Ki* Isum//右輪電機控制器輸出變量；

SetRightPWM（CVRight） //根據CV值設置PWM信號占空比；

SetLeftPWM（CVLeft） //根據CV值設置PWM信號占空比；

Goto Loop

3.2 機器人小車運動狀態控制

根據L298N控制芯片引腳使能邏輯關系，通過設置控制芯片的方向控制信號引腳P0.4、P0.5和P0.12、P0.13實現對機器人小車運動狀態的控制，其控制規律如表1所示，幾種運動狀態的自由組合可以實現小車原地零半徑轉彎，增加轉向精度和反映速度.

3.3 左右輪電機最高轉速分析

根據CVLeft值分別設置左右輪電機PWM占空比，實現PWM調速閉環控制，其中80.91 r/min和7766 r/min為占空比100%即最高電壓下的左右輪電機轉速.根據實驗數據獲得最高電壓下左右輪轉速，取其數學平均值作為最高電壓轉速參數，數據如表2所示，以便在精確控制時補償左右輪電機本身參數和安裝精度等的誤差.

實驗表明該XMU3平臺可以實現對小車的精確伺服控制，能對小車伺服控制和零半徑自轉，平面旋轉精度達到±5度，最高行進速度達到77.66 r/min，約合每分鐘行進240 m，并可實現和ARM節點和遠程PC節點的實時CAN通信.

4 結論

本文設計了一項自主移動機器人平臺的伺服控制方案，建立了一個差分驅動雙軸輪式移動機器人運動學模型，并實現了零半徑自轉及平面內的全向轉動.實驗結果表明雙軸輪旋轉控制方案能保證電機轉動的準確性和穩定性，具有較強的魯棒性，可作為室內服務機器人平臺進行深度開發.

參考文獻：

[1] 葉長龍，馬書根，回 麗，等. 一種全方位移動機器人[J].中國科學，2011，41（2）：181189.

[2] 楊 敏. 輪式移動機器人控制算法研究及其伺服系統設計[D].南京： 南京航空航天大學，2014.

[3] 王珊珊. 輪式移動機器人控制系統設計[D].南京：南京理工大學，2013.

[4] 劉 磊，向 平，王永驥，等. 非完整約束下的輪式移動機器人軌跡跟蹤[J]. 清華大學學報：自然科學版，2007，S2（2）：18841889.

[5] 宋廣鋼，李文鋒，宋 威，等. 基于 ARM9 微處理器的移動機器人控制系統設計[J].微計算機信息， 2010，26（2）：146149.

[6] KOEI M. Trajectory tracking control of mobile robots without using longitudinal velocity measurements [J].Art Life Robot， 2009，14（14）：352356.

[7] CHANG W C. Visual navigation and control of mobile robots based on environment mapping[C]//International Conference on Advanced Mechatronic Systems （ICAMechS ）， Kumamoto， Japan， 2014：469474.

[8] MIYAZAKI T. Formation control of mobile robots with obstacle avoidance[C]//The 14th International Conference on Control， Automation & System （ICCAS）， Seoul， Korea， 2014：121126.

[9] 宋廣鋼，盧博友，雷永鋒，等.基于 ARM 的智能機器人小車控制系統設計[J].微計算機信息，2008，24（2）：239241.

[10] JANTZ S. Final report EEL 5666[R]. Machine intelligence laboratory， University of florida，2000，2532.

[11] BREZAK M. Timeoptimal trajectory planning along predefined path for mobile robots with velocity and acceleration constraints[J]. IEEE/ASME International conference on advanced intelligent mechatronics， 2011， 30（1）：942947.

[12] ZAMAN S. ROSbased diagnostic board for detecting and repairing hardware faults in autonomous mobile robots[J]. International conference on robotics & emerging allied technologies in engineering， 2014，32（1）：95102.

（編輯 CXM）