空間機器人末端抗干擾優化控制方法

朱 敏，陳 山

（1.常州信息職業技術學院，江蘇 常州 213164；2.江蘇大學，江蘇 鎮江 212013）

1 引言

從我國首顆人造地球衛星東方紅一號被成功送上空間軌道，到嫦娥五號完成首次轉移軌道修正，我國航天事業走過了五十多個年頭，在軌衛星數量達400余顆，僅次美國居世界第二。在航天事業的快速發展之際，空間機器人由于能夠替代航天員在太空中開展衛星捕獲、在軌維修以及垃圾清理等在軌服務，大大降低人類作業風險，其使用頻率也因此大大提高［1］。而受空間特殊環境影響，空間機械臂的軌跡跟蹤控制問題尤其受到國內外學者的廣泛關注和深入研究［2］。

與地面機械臂不同，太空失重環境下，自由漂浮空間機器人的機械臂各連桿之間、機械臂與基座之間的運動相互耦合，其運動控制具有強耦合、非線性的特性，一般地面機械臂控制方法將失效，現有的FFSR控制方法，根據控制器作用空間的不同，可分為關節空間控制方法和慣性空間控制方法。其中，文獻［3］提出了一種全局穩定的自適應路徑跟蹤方法，實現了FFSR系統的關節角和基座衛星姿態角的穩定跟蹤；文獻［4］建立了線性化的FFSR系統關節空間動力學模型，并采取PD控制方法，實現了對機械臂關節角的軌跡跟蹤；文獻［5］考慮控制力矩受限的約束，提出一種新的神經網絡自適應控制方法，實現了模型不確定情況下FFSR系統的漸進穩定。以上均為關節空間的控制方法，而由于FFSR基座位置的不固定，關節角的精確跟蹤卻無法保證末端抓手的跟蹤精度，因而近年來慣性空間下的控制方法更受關注。文獻［6］基于逆鏈逼近提出一種自適應控制方法，較好的解決了慣性參數不確定條件下FFSR末端軌跡跟蹤控制問題；文獻［7］采用神經網絡進行在線建模，并通過設計自適應控制器補償建模誤差和外部干擾，實現了不確定條件下的FFSR 末端精確跟蹤；文獻［8］建立了FFSR的擴展狀態空間模型，并采用模型預測控制方法實現了約束條件下的機械臂末端軌跡精確跟蹤。上述方法均較好的解決了不確定條件下的FFSR機械臂跟蹤控制問題，但均存在未考慮控制器輸出能量優化的不足。

基于狀態黎卡提方程（State-Dependent Riccati-Equation，SDRE）的控制方法能夠較好的解決非線性系統的反饋控制問題，且是一種最優控制方法［9］，具有較好的工程應用價值，近年來逐漸被運用到空間機械臂的控制當中，且取得了一定研究成果。文獻［10］針對力矩受限條件下的雙臂空間機器人的軌跡跟蹤問題，提出了一種基于SDRE的優化控制方法，但其討論的是關節空間，且未考慮外部干擾及模型不確定的影響；文獻［11］提出了一種兩級SDRE的FFSR控制方法，實現了末端軌跡的精確跟蹤，但僅考慮的模型參數的誤差而未考慮到外部干擾因素；文獻［12］針對任務空間下的空間機械臂軌跡跟蹤控制問題，提出了一種SDRE的基座與機械臂協作控制方法，但同樣未考慮模型誤差以及外部干擾帶來的不確定性因素。

這里針對傳統控制方法難以實現存在多種不確定性條件下的FFSR任務空間軌跡跟蹤優化控制的問題，設計了基于干擾觀測的SDRE優化控制器，實現空間機械臂末端抓手的精確跟蹤控制以及輸出力矩的能量優化。所提控制方法主要由兩部分構成：首先，不考慮系統不確定項，基于FFSR名義模型，設計SDRE優化控制器，實現輸出力矩和輸出誤差兩項指標的最優化。其次，設計非線性干擾觀測器對不確定部分進行在線估計，并將估計值反饋到控制器輸入中，進一步提高機器人末端軌跡跟蹤精度。最后，為驗證所提控制方法的有效性，以兩自由度的二連桿空間機器人為對象進行Matlab/Simulink仿真。

2 系統建模

FFSR系統由一個基座及若干連桿機械臂組成，各部件均為剛體結構，基座及機械臂均在同一平面內運動，如圖1所示。其中，OA為系統質心，∑I、∑B、∑E分別為慣性坐標系、基座坐標系和機械臂末端坐標系。

圖1 FFSR結構簡圖Fig.1 FFSR Structure Diagram

在太空環境中，可忽略微弱重力影響，且FFSR系統處于自由漂浮狀態，不受外力作用且基座無動力控制，因此滿足動力守恒定理。不失一般性，這里考慮兩連桿（即連桿數量n=2）的FFSR系統，進一步根據Lagrange第二類方程可獲得其關節空間的動力學方程以及關節角與末端位置之間的雅克比運動學方程分別為

式中：qb=θ0—基座姿態角；qm=—各連桿的關節角；τ=[τ1，τ2]T—關節控制力矩；XE=—末端抓手在慣性坐標系下的位置坐標；M(qb，qm)∈R2×2（以下簡寫為M）—一個正定對稱的慣性矩陣；∈R2×2（以下簡寫為C）—哥氏力和離心力矩陣；J(qb，qm)∈R2×2（以下簡寫為J）—雅克比矩陣，上述矩陣的具體表達式可參照文獻［13］，這里不再做具體推導，且上述矩陣滿足以下特性［13］。

（1）M為對稱正定陣。

實際工程中，空間機械臂送上太空后，隨著時間推移其模型參數易發生變化且外部干擾不可避免，其動力學模型往往難以精準獲取，因此式（1）又可表示為：

為進一步簡化動力學方程表達式，式（3）可寫成：

式中：f—系統模型偏差項，其滿足：

若忽略模型偏差，則可得名義動力學方程為：

3 控制器設計

為解決存在模型參數誤差和外部干擾的多種不確定性條件下的FFSR 任務空間軌跡跟蹤優化控制的問題，這里設計FFSR系統控制結構，如圖2所示。

圖2 控制系統結構Fig.2 Control System Structure

從圖中可以看出控制器由兩部分組成，其一是SDRE 控制器，基本思想是忽略動力學方程式（4）中的模型偏差項f，基于名義動力學方程式（6），設計標稱優化控制器，實現輸出力矩和跟蹤誤差的優化。其二是干擾觀測器，基本思想是設計觀測狀態方程，對模型不確定項在線觀測，通過選取合適的觀測矩陣使得觀測誤差按指數收斂，并趨近于零，從而使得整個系統保持穩定。

3.1 SDRE控制器設計

與線性二次型調節器（Linear Quadratic Regulator，LQR）方法類似，SDRE 是一種基于空間狀態如式（7）所示的一種優化控制方法：

定義系統期望輸出yd(t)，實際輸出y(t)，系統輸出誤差e=yd(t)-y(t)，則按SDRE控制原理，可給出如下形式的無限時間性能指標：

其中，Q(x)＞0，R(x)＞0為參數矩陣。

若式（7）滿足{A(x)，B(x)}逐點可控；{A(x)，C(x)}逐點可觀，則可以設計優化控制律［14］：

式中：P(x)—滿足代數Riccati微分方程：

Z(x)滿足矢量微分方程：

根據上述原理，為將SDRE控制方法運用于FFSR系統，需進一步對FFSR的動力學方程和運動學方程的表達式進行擴展。為此，定義增廣狀態變量x=，輸出變量y=XE，由式（2）、式（6）可推出：

其中，

其中，P(x)滿足代數Riccati方程：

3.2 干擾觀測器設計

實際工程中，模型偏差項f往往無法避免，根據式（12）可推出FFSR實際的狀態方程為：

式中：τ=τ1+τ2，其中，τ1—2.1節中設計的SDRE控制器；τ2—本節待設計的干擾觀測器。

為抑制不確定項對系統帶來的影響，根據狀態方程式（15），設計系統干擾觀測如下：

結合式（15），可進一步將式（19）簡化為：

綜上，干擾觀察器輸出為：

4 仿真實例

如圖1 所示，以兩連桿的FFSR 為例進行Matlab/Simulink 數值仿真，驗證所設計控制器的有效性。FFSR的真實模型參數和名義模型參數，如表1所示。

表1 系統參數Tab.1 System Parameters

末端抓手期望軌跡為：

系統運動的初始值為：

外部干擾為：τd=（單位：N·m，e=yd(t)-y(t)為跟蹤誤差）。設置仿真時長t=20.0s，為驗證這里所提控制方法是否能有效克服系統模型偏差項帶來的不確定性影響，采取增益控制的方式，分別獲取開啟/關閉干擾觀測器狀態下的仿真結果，如圖3～圖7所示。圖3、圖4直接反應了存在模型參數誤差以及外部干擾情況下，傳統控制方法（即關閉干擾觀測器）以及這里所提基于干擾觀測器的控制方法的控制效果。從圖3（a）、圖4（a）可以看出，在傳統控制方法控制下，由于系統存在不確定項，運動軌跡始終存在一定跟蹤誤差，尤其是在t=（10～12）s，受擾動影響，誤差范圍增大。從圖3（b）、圖4（b）可以看出，當開啟干擾觀測器，使用這里所提方法進行控制時，能夠有效克服不確定項的影響，實現末端軌跡的精確跟蹤，跟蹤誤差始終保持在較小范圍內。

圖3 關閉/開啟干擾觀測器末端軌跡跟蹤情況Fig.3 Terminal Trajectory Tracking with Disturbance Observer off/on

圖4 關閉/開啟干擾觀測器末端軌跡跟蹤誤差Fig.4 Terminal Trajectory Tracking Error with Disturbance Observer off/on

FFSR 的兩個關節角的運動情況以及控制力矩，如圖5、圖6所示。從圖5（a）可以看出，由于受模型偏差的影響，在傳統控制方法作用下，其關節角運動在t=（10～12）s這段時間內會出現持續的抖動；從圖6（a）可以看出，為克服模型偏差影響使系統保持相對穩定，傳統控制方法的輸出力矩將急劇增大。上述這兩點均與實際工程要求不符。而從圖5（b）、圖6（b）中可以看出，當開啟干擾觀測器時，兩個關節角能平滑運動，且輸出力矩也得到了優化。

圖5 關閉/開啟干擾觀測器各關節角變化情況Fig.5 Changes of Joint Angles with Disturbance Observer off/on

圖6 關閉/開啟干擾觀測器輸出力矩Fig.6 Output Torque with Disturbance Observer Off/On

干擾觀測器對如式（5）所示的系統不確定項f的在線觀測情況，如圖7所示。從圖中可以看出，通過2.2節中對干擾觀測器中的相關函數的設計，能夠實現對FFSR系統中的參數誤差以及外部干擾帶來的不確定項的實時精確觀測。

圖7 不確定項觀測情況Fig.7 Observation of Uncertainty

5 結論

這里針對傳統控制方法難以實現存在多種不確定性條件下的FFSR任務空間軌跡跟蹤優化控制的問題，設計了基于干擾觀測的SDRE優化控制器，實現了空間機械臂末端抓手的精確跟蹤控制以及輸出力矩的能量優化。所提方法具有以下優點：（1）實現了任務空間下的FFSR系統跟蹤控制，更具工程應用價值；（2）通過觀測器的設計實現了模型參數誤差以及外部干擾的在線觀測，能夠有效克服模型不精確帶來的影響；（3）基于SDRE控制原理實現了輸出力矩的優化。