劉飛飛，朱楊林

（江西理工大學 電氣工程與自動化學院，贛州 341000）

0 引言

與傳統(tǒng)移動機器人相比，動態(tài)自平衡機器人是一種多變量、高階次、強耦合，欠驅(qū)動的非線性系統(tǒng)，具備更快速、靈活的運行能力[1～7]。

如圖1所示，單球移動機器人由圓柱形載物主架體和驅(qū)動球組成，利用三個全向輪實時驅(qū)動球來保持機器人的平衡。對其研究最早可追溯到2005年，美國Lauwers等人設(shè)計的“Ballbot”機器人[2]，但由于結(jié)構(gòu)的限制，無法實現(xiàn)自轉(zhuǎn)。后來，日本Kumagai等人進行不斷改進先后設(shè)計了球上平衡機器人“BallIP1”[3]，“BallIP2”[4]和“BallIP-W”[5]，采用PID算法實現(xiàn)控制，雖實現(xiàn)了快速調(diào)整，但抗干擾能力弱。研究較為成功的是使用了LQR算法的“Rezero”機器人[6]和“Ball-Riding”機器人[1]不僅能實現(xiàn)動態(tài)平衡，而且具有較好的靈活性。但針對機器人的原點自平衡控制問題，沒有考慮姿態(tài)平衡和原點控制的權(quán)重分配，影響了整個系統(tǒng)的調(diào)節(jié)時間。

本文為進一步提高機器人的原點自平衡能力，提出一種基于遺傳算法優(yōu)化的自平衡控制方法，采用遺傳算法來優(yōu)化LQR控制器中的權(quán)重矩陣，平衡控制輸入與各被控狀態(tài)的權(quán)重關(guān)系，實現(xiàn)最優(yōu)控制。首先，將機器人簡化為二維的單球倒立擺模型，建立Eular-Lagrange動力學模型；其次，對全向輪和驅(qū)動球進行力學分析，找出全向輪與驅(qū)動球的移動規(guī)律；接著，基于遺傳算法的LQR控制方法設(shè)計自平衡控制器，對被控狀態(tài)進行歸一化，使單球移動機器人各狀態(tài)以統(tǒng)一的量綱表示，以歸一化后的二次型性能指標為尋優(yōu)標準，采用遺傳算法對權(quán)值矩陣Q進行優(yōu)化。最后，通過仿真對比驗證所設(shè)計控制器的有效性。

圖1 單球移動機器人實物圖

1 模型建立

1.1 動力學建模

將單球移動機器人簡化為二維的倒立擺如圖2所示，以驅(qū)動球心O為原點建立空間直角坐標系O-XYZ，在XOZ和YOZ平面分別建立兩個解耦的動力學模型。在建模中作如下假設(shè)：1）機器人主架體和驅(qū)動球都視為剛體；2）各部分的摩擦力（力矩）與相對速度（角速度）成正比；3）球體與全向輪、地面之間做純滾運動；4）XOZ和YOZ平面的兩個模型運動分析是可分離的，且運動方程相同，只有方向上的差別，故建模與控制器設(shè)計中只作XOZ的分析。

圖2 單球移動機器人簡圖

圖3 全向輪與驅(qū)動球的位置關(guān)系

表1 單球移動機器人物體參數(shù)表

模型中以θx、φx作為廣義坐標，分別為在XOZ平面上主架體的傾角位移和驅(qū)動球的角位移，τθ和τφ為對應(yīng)的廣義力矩。機器人的具體參數(shù)如表1所示。參照文獻[8]中太陽與地球相對位置得θ+ψ=90°，并建立Eular-Lagrange方程[7]：

其中，Mi（qi）為慣性矩陣，Ci（qi，qi）為離心力和科氏矢量，Gi（qi）為重力矢量，Di（qi）為耗散能；由式(1)可得：

由向量混合積的輪換對稱性得：

由式(7)和式(8)得驅(qū)動球X方向的速度與三個全向輪角速度的關(guān)系，求導(dǎo)可得：

將式(2)將線性化得機器人X方向動力學方程：

1.2 移動規(guī)律分析與運動轉(zhuǎn)化

單球移動機器人控制系統(tǒng)相似于倒立擺，但又區(qū)別于倒立擺，其平衡控制主要由三個全向輪實時靈活地進行啟停、方向和轉(zhuǎn)速控制。圖3中A、B、C分別為相切于球面的全向輪，在不考慮全向輪速度大小情況下，以他們方向作為控制對象可分為八種情況分析。本節(jié)以全向輪A、B、C分別按順時針、逆時針和逆時針情況進行分析，其他以此類推。假設(shè)機器人對球體受力均勻，并規(guī)定計算過程中負號“-”代表全向輪順時針，正號“+”為全向輪逆時針。為全向輪對球體的靜壓力，大小相等，方向都指向圓心。別為對應(yīng)的力矩。分別代表坐標X、Y、Z正方向上的單位矢量，驅(qū)動球轉(zhuǎn)動的方向以圖3(b)作為分析，靜壓力的合力及對球心的合力矩分別為：

由此可知，在該情況下球體向0°方向轉(zhuǎn)，即繞著X軸正轉(zhuǎn)。用相同的辦法，改變?nèi)蜉喫俣然蚍较?，可分別獲取驅(qū)動球向不同方向的運動。表2為全向輪不同狀態(tài)下驅(qū)動球的轉(zhuǎn)動狀態(tài)。橫軸代表三個全向輪的方向，縱軸代表驅(qū)動球在全向輪不同速度和方向下轉(zhuǎn)向，單位為度。

忽略驅(qū)動球繞Z軸自轉(zhuǎn)現(xiàn)象，根據(jù)文獻[7]，全向輪線速度與驅(qū)動球相對地面速度的運動轉(zhuǎn)化公式為：

2 自平衡控制器的設(shè)計與仿真

為使單球移動機器人能更好的達到原地自平衡狀態(tài)，設(shè)計了如圖4所示的自平衡控制系統(tǒng)。初始指令分別為單球移動機器人在XOZ平面和YOZ平面的傾角、位移及相應(yīng)速度。在兩個平面上分別設(shè)計了兩個獨立的基于遺傳算法的最優(yōu)控制器1和控制器2，輸入為初始指令與反饋信息的差值，輸出為X、Y方向機器人的加速度ux、uy，通過積分、運動轉(zhuǎn)化，將驅(qū)動球速度轉(zhuǎn)化為三個全向輪的速度，最后輸出電機執(zhí)行指令使機器人迅速達到原地自平衡狀態(tài)。

表2 驅(qū)動球在XOY平面的轉(zhuǎn)向

圖4 自平衡控制系統(tǒng)框圖

2.1 線性二次最優(yōu)控制器

線性二次型最優(yōu)控制是一種特殊的最優(yōu)控制方法，以二次型函數(shù)的積分為性能指標泛函，不僅同時兼顧系統(tǒng)性能和能量消耗，而且具有較好的魯棒性[9,10]。通過式(10)分析計算機器人線性化狀態(tài)空間模型為：

經(jīng)Matlab軟件編程驗證該系統(tǒng)可控但不穩(wěn)定。設(shè)計二次型指標函數(shù)：

當系統(tǒng)因外界干擾而偏離原點自平衡狀態(tài)時，通過施加的ux使系統(tǒng)回到零點附近，同時使二次型指標函數(shù)達到最小，其規(guī)律滿足：

K為線性最優(yōu)反饋增益矩陣，可通過Matlab軟件中的[K]=lqr（A，B，C，D）函數(shù)獲得。其中，權(quán)值矩陣Q、R用來平衡狀態(tài)向量和輸入向量的權(quán)重，其值直接決定了控制器的性能[11,12]。一般情況下，權(quán)重矩陣Q和R由工程人員根據(jù)經(jīng)驗主觀臆斷決定，而針對強耦合、多變量的單球移動機器人系統(tǒng)，通過試湊法尋求系統(tǒng)的最優(yōu)控制器，使各輸出變量的超調(diào)量、調(diào)整時間達到最優(yōu)化目標，并非易事，故本文提出采用一種遺傳算法來優(yōu)化控制器中的權(quán)值矩陣來優(yōu)化控制器的性能[13]。

2.2 基于遺傳算法的線性二次最優(yōu)控制器

遺傳算法[14]是模擬自然界生物進化機制的一種算法，以要尋優(yōu)的參數(shù)組成染色體，通過選擇、交叉，變異等操作，并行式地搜索使得種群朝著最優(yōu)的方向進化，最終獲得最優(yōu)解。具體流程如圖5所示。

圖5 遺傳算法操作流程

本文利用遺傳算法優(yōu)化控制器中的權(quán)值矩陣，使被控狀態(tài)的超調(diào)量，調(diào)整時間及輸入變量達到最優(yōu)。為提高編碼效率，根據(jù)經(jīng)驗取R=1，以矩陣Q中的對角元素q1、q2、q3和q4作為尋優(yōu)參數(shù)組成染色體。在控制系統(tǒng)中，相對于原點控制，姿態(tài)的平衡控制更為重要，因此傾角（x1）所對應(yīng)的Q中的元素值＞位移（x3）所對應(yīng)的Q中元素值。在平衡點附近時，機器人及驅(qū)動球的角速度均很小，對目標函數(shù)的影響也很小，因此q2和q4可以取較小值，據(jù)此對參數(shù)進行二進制編碼，即q1∈[0 210]、q2∈[027]、q3∈[029]、q4∈[027]。目標函數(shù)的計算和適應(yīng)值的選取是遺傳算法的核心，以被控系統(tǒng)歸一化的二次型性能指標作為遺傳算法目標函數(shù)，將各輸出變量歸一化，公式如下：

性能指標歸一化之后，整體參數(shù)不會一味朝著取值小的方向搜索，而是側(cè)重于各變量重要性分配，從總體的角度出發(fā)考慮系統(tǒng)的穩(wěn)定性，避免了算法的偏向搜索。以a=0.005s為一個時間單位，n為時間積分次數(shù)，對目標函數(shù)進行積分計算：

考慮到目標函數(shù)值積分后所獲得值過大而影響最終結(jié)果，調(diào)整適應(yīng)度函數(shù)為：

2.3 仿真結(jié)果與分析

本文設(shè)計的控制器用于機器人原點自平衡控制，其對控制時間具有較高要求，其次是機器人的抗干擾能力。因此，本文以調(diào)節(jié)時間和系統(tǒng)的抗干擾能力作為主要評估指標。分別對比采用人工整定法及遺傳算法優(yōu)化情況下，控制器對機器人傾角、位移的控制能力。

設(shè)狀態(tài)變量初始值x0=[0.05 0.01 0 0]T；遺傳算法中參數(shù)：種群50，染色體長度34，交叉概率0.7，變異概率0.005，經(jīng)過100次迭代后，權(quán)重矩陣：

相應(yīng)的反饋增益陣：

取一組人工整定的權(quán)值矩陣：

相應(yīng)的反饋增益陣：

由圖6和圖7可知：當機器人偏離平衡點以0.01m/s角速度向X正方向傾斜0.05rad時，控制器立即給出加速度命令，控制全向輪轉(zhuǎn)動，隨著機器人身體姿態(tài)逐漸平衡，加速度變小，經(jīng)過3s，機器人的，，x及都基本為零。表3和表4分別為基于遺傳算法優(yōu)化和人工整定的各狀態(tài)調(diào)節(jié)時間ts和超調(diào)量mp，從表中數(shù)據(jù)看，無論從哪方面，經(jīng)過遺傳算法優(yōu)化的控制器都較優(yōu)。

在第6s和14s，對優(yōu)化前后的控制器添加1m/s2的脈沖干擾，從圖8個狀態(tài)響應(yīng)過程可知，優(yōu)化后的控制器對脈沖干擾的響應(yīng)更快，抗干擾性更強。

綜上所述，遺傳算法優(yōu)化后的控制器可以使機器人更快地達到穩(wěn)定，且魯棒性好，更適用于單球移動機器人的實時控制。

表3 遺傳算法優(yōu)化后的控制參數(shù)

表4 人工整定后的控制參數(shù)

圖6 單球移動機器人的姿態(tài)和位置

圖7 單球移動機器人的控制輸入

圖8 單球移動機器人的在干擾下的姿態(tài)和位置

圖9 單球移動機器人的控制輸入

圖10 適應(yīng)值的迭代變化

3 結(jié)論

1）相比使用人工整定單球移動機器人最優(yōu)控制器的權(quán)重矩陣，使用遺傳算法可以更方便、更準確地對控制器參數(shù)進行優(yōu)化整定。

2）使用遺傳算法進行最優(yōu)控制的參數(shù)優(yōu)化后的控制器可以更快地實現(xiàn)平衡，更適用于單球移動機器人的站立平衡控制。

3）使用遺傳算法進行最優(yōu)控制的參數(shù)優(yōu)化后的控制器的抗干擾能力更強，魯棒性更好。