基于Legendre 偽譜法的3D剛體擺姿態軌跡跟蹤控制

戈新生　朱　寧

北京信息科技大學理學院， 北京 100192； ? E-mail: gebim@vip.sina.com

?

基于Legendre 偽譜法的3D剛體擺姿態軌跡跟蹤控制

戈新生?朱寧

北京信息科技大學理學院， 北京 100192； ? E-mail: gebim@vip.sina.com

研究3D剛體擺在有初始擾動情況下的姿態運動最優控制問題。結合 3D 剛體擺轉動的姿態與角速度特點， 針對外部擾動設計閉環反饋姿態跟蹤控制器。首先， 利用Legendre偽譜法規劃出3D剛體擺開環的姿態運動軌跡。然后， 將系統的運動方程線性化， 并以 3D 剛體擺的實際運動姿態軌跡與參考運動姿態軌跡之間的差值作為控制量， 將姿態跟蹤問題轉換為線性時變系統的姿態調節問題。最后， 對基于 Legendre 偽譜法的3D剛體擺姿態最優控制的閉環控制方法進行仿真分析， 驗證在具有初始擾動情況下算法的有效性。

3D剛體擺； 姿態控制； 最優控制； 偽譜法

北京大學學報（自然科學版）第52卷第4期2016年7月

Acta Scientiarum Naturalium Universitatis Pekinensis， Vol. 52， No. 4 （July 2016）

3D 剛體擺是一個剛體繞無摩擦的固定支點旋轉， 剛體受到恒重力（或萬有引力）作用， 并且具有3個轉動自由度［1-2］。該力學模型假設剛體的質心與固定支點不重合， 剛體擺具有懸垂和倒置兩個相對平衡位置。若剛體擺為軸對稱情形， 并且對稱軸為剛體的慣性主軸， 當繞對稱軸角速度為零時等價于球面擺， 當繞對稱軸角速度為常數時等價于 Lagrange陀螺（倒置）， 當對稱軸為鉛垂而角速度為常數時則等價于睡陀螺。因此， 3D剛體擺是一種廣義剛體擺模型。

偽譜法采用全局正交多項式對系統的狀態空間進行逼近， 從而將最優控制問題轉化為非線性規劃問題進行求解［3］。近年來， 偽譜法因其良好的計算效率和精度以及對初始參數取值不敏感的優點， 成為近年來求解最優控制問題［4-6］新方法。偽譜法在航天領域多用于軌跡優化問題， 對姿態運動最優控制問題研究鮮有報道。

本文基于偽譜法， 研究 3D 剛體擺的姿態運動最優控制問題。針對3D 剛體擺， 通過 Legendre 偽譜法， 同時離散化系統狀態變量和控制變量， 將最優控制問題轉化為非線性規劃問題， 得到 3D 剛體擺的姿態最優控制軌跡， 即 3D 剛體擺將按照預先設定好的參考軌跡運行， 此時運動規劃問題為開環控制［7］。然而， 3D 剛體擺在工程實踐中， 難以避免外界干擾、初始擾動等因素的影響， 開環控制器往往會導致 3D 剛體擺的實際運行軌跡偏離期望的參考軌跡值， 從而導致系統性能的下降， 并使開環控制優化過程失去意義。Lu［8］通過 Enler-Simpson 方法將二次型調節問題近似， 并結合滾動時域策略，得到閉環軌跡跟蹤控制器。Yan 等［9］基于 Legendre偽譜法， 將兩點邊值問題得到的線性時變方程離散化求解。莊宇飛等［10-11］采用實時重規劃策略， 實現穩定控制器狀態的反饋， 從而完成欠驅動剛性航天器對參考軌跡的閉環跟蹤。

本文在開環控制的基礎上， 設計閉環軌跡跟蹤控制器。開環優化的實時性是保證重規劃算法正常運行的必要條件， Bellman 最優性原理則是重規劃算法的核心思想。如圖 1 所示， Bellman 最優性原理可以簡單地描述為: 如果從P1點到P2點的軌跡是最優的， 且P3點為該最優軌跡上不同于P1， P2的任意一點， 則從P2點到P3點的軌跡仍是最優的。

根據 Bellman 最優性原理， 在理想狀況下， 每次重規劃所得的最優軌跡均與原參考軌跡重合。這表明了重規劃算法的合理性， 即重規劃算法可以保證每次規劃的解仍是原問題的最優解， 或者說重復規劃的過程并不影響解的最優性。

本文主要研究3D剛體擺在有初始擾動情況下的軌跡姿態控制問題， 并以開環偽譜法最優控制為基礎， 采用內外環控制結構的算法， 設計帶有狀態反饋的閉環最優姿態軌跡跟蹤控制器。將系統的運動方程相對于參考軌跡線性化， 且以3D剛體擺實際運行軌跡與參考軌跡之間的偏差為控制量， 將姿態跟蹤問題轉化為線性時變系統的姿態調節問題。結合Legendre偽譜法， 針對時變的線性化系統設計最優調節控制器， 從而實現 3D 剛體擺的狀態閉環的軌跡跟蹤控制。

1　3D剛體擺的數學模型

如圖2所示， 3D剛體擺繞一固定且無摩擦的支撐點O進行三自由度旋轉。在慣性坐標系｛｝OXYZ下， O為原點， 其中Z軸方向與重力加速度方向相同， X與Y軸位于水平面內并與Z軸呈右手系； 構造連體坐標系， Z′軸方向由原點O指向擺的質心 C， X′軸和Y′軸分別沿著擺的慣性主軸方向。

在慣性坐標系｛｝OXYZ下， 采用3-2-1歐拉姿態角描述方式， 并考慮重力矩的作用， 3D剛體擺的數學模型［12］為

為 3D 剛體擺的慣量矩陣， m為3D剛體擺的質量， g是重力加速度， ρ表示為從原點O到質心 C 的矢量， e3為慣性坐標系中Z軸方向上的單位向量是 3D 剛體擺的角速度， u∈R3是控制輸入量， R表示為旋轉矩陣。對于矢量可表示為

令Γ =RTe3， 則 3D 剛體擺可描述為約化姿態的形式:

3D 剛體擺在空間坐標系內存在兩個平衡位置，即懸垂平衡點和倒立平衡點［12-13］， 意味著 3D 剛體擺的最優控制問題為從任意初始位置運動到目標平衡位置的姿態運動控制的過程。

2　Legendre閉環偽譜法求解

在理想無干擾的環境中， 3D剛體擺可以根據控制目標， 利用Legendre偽譜法規得到的 3D 剛體擺的最優運動軌跡［7］。然后， 考慮 3D 剛體擺姿態控制過程中存在初始擾動的情況， 應用偽譜法并結合重規劃策略， 設計狀態閉環的軌跡跟蹤控制器。將3D 剛體擺姿態運動模型（式（1）和（2））在參考軌跡上線性化， 則有

其中，

根據線性化方程（6）， 利用偽譜法設計姿態穩定控制器的目標， 就是通過確定輸入修正量（）tΔu以及狀態偏差量（）tΔx， 使得最優泛函

最小。式（7）中， S是半正定的末端加權矩陣， Q是半正定的狀態加權陣， R 是正定的控制加權陣， 則相應的Hamilton函數為

于是， 根據最優性必要條件， 有

與之對應的邊值條件為

由上述分析可知， 采用閉環控制算法設計姿態調節器時， 需要開環的最優控制兩點邊值問題能夠時刻求解。由于 3D 剛體擺模型具有強非線性特性，導致求解 Riccati 方程過程復雜， 運算量大。因此，本節采用 Legendre 偽譜法將兩點邊值問題離散化，將其求解微分方程組問題簡化為求解一組線性代數方程。

在 LGL （Legendre-Gauss-Lobatto）點ζη（η = 0，1， 2， …， N）上將狀態變量Δx及協態向量 λ 離散化，則有

其中， D = （Dkη）為（N+1）×（N+1）階導數矩陣:

兩點邊值問題在LGL點上離散化， 有

式中，

式中， I是n×n的單位矩陣， 0是n×n的零矩陣， ΔX0=Δx0I。

通過求解線性方程組， 可以得到兩點邊值問題的解析解， 且無需對方程進行積分運算。因此， 算法的精度得到很好的保證， 并減少了運算時間， 大幅降低求解過程的計算量。

計算過程如下。

第1步， 選取算法采樣周期。

第2步， 在參考軌跡上將 3D 剛體擺的運動模型線性化并確定初值

第4步， 將代數方程組（17）的解代入方程（10），求得最優控制修正量

3　仿真實驗及分析

設 3D 剛體擺的質量為 m=140 kg， 轉動慣量為J = diag（40， 45， 50） kg · m2， 支點與質心之間的距離為 l=0.1 m。假設 3D 剛體擺在 t =0 時刻的干擾誤差為

設 3D 剛體擺的初始位置角速度為

約化姿態為

設目標位置為懸垂平衡位置， 其角速度為

約化姿態為

插值點 N=35， 仿真結果如圖3所示。

圖 3 顯示 3D 剛體擺的角速度沿三軸的變化曲線。可以看出， 在存在初始擾動的情況下， 開環控制的角速度受到初始擾動逐漸偏離最優參考軌跡運動， 失去控制效果； 而閉環反饋控制能夠有效抵抗初始擾動的干擾， 迅速調整 3D 剛體擺的角速度變化， 沿最優軌跡使 3D 剛體擺到達目標懸垂平衡位置， 且角速度趨近于0。

圖4顯示 3D 剛體擺約化姿態變化曲線， 可以看出， 剛體擺受到初始擾動的影響， 開環控制不能有效地對 3D 剛體擺進行姿態調整， 逐漸偏離參考軌跡。利用提出的反饋控制方法， 3D 剛體擺在受到初始擾動偏離最優路徑軌跡， 能夠迅速響應做出調整， 返回到最優運動軌跡， 且沿此路徑最后到達懸垂平衡位置。

圖 5 所示的控制輸入在［-1， 1］間切換， 根據最優控制問題的必要條件， 即得到的控制力矩是原姿態運動最優控制問題的最優解。圖6為3D 剛體擺在最優控制輸入下的空間姿態軌跡變化曲線。

同樣， 在滿足相同初始擾動存在的情況下， 設倒立平衡的初始角速度為約化姿態為

設目標倒立平衡點角速度為

約化姿態為

3D 剛體擺在閉環反饋最優控制作用下， 倒立平衡仿真結果如圖7～10所示。

圖 7 所示的倒立情況下， 開環控制的角速度受到初始擾動， 逐漸偏離最優參考軌跡， 失去控制效果， 而閉環控制能夠有效地抵抗初始擾動的干擾。在初段被干擾時， 該控制策略通過反饋作用能夠迅速調整3D剛體擺的角速度變化， 沿最優軌跡使3D剛體擺到達目標倒立平衡點， 且角速度趨近于 0。

圖 8 為倒立情況下 3D 剛體擺的約化姿態變化曲線， 說明在存在初始擾動的情況下， 閉環控制能夠有效地抵抗初始擾動的干擾， 迅速調整 3D 剛體擺的空間姿態， 使 3D 剛體擺沿最有參考軌跡運動到目標倒立平衡點。相反， 開環最優控制則不能消除初始擾動的影響， 逐漸偏離最優軌跡， 無法運動到目標倒立平衡點。

圖9所示的最優控制輸入力矩曲線在上下限間切換變化。圖 10 形象地反映了系統姿態在空間坐標內的運動軌跡。

4　結論

本文針對 3D 剛體擺運動姿態穩定控制問題，設計了 Legendre 偽譜法閉環控制器。仿真結果表明， 基于Legendre偽譜法閉環控制算法能夠有效地抑制初始擾動給 3D 剛體擺運動過程帶來的不利影響， 按照約束條件的要求達到目標平衡位置， 進一步證明了偽譜法求解 3D 剛體擺最優控制問題的可行性和有效性， 也為求解類似復雜約束系統的最優控制問題提供了新的途徑。

［1］ Shen J， Sanyal A K， Chaturvedi N A， et al. Dynamics and control of a 3D pendulum // Proceedings of the 43rd IEEE Conference on Decision and Control. Bahamas， 2004: 323-328

［2］ Chaturvedi N A， Lee T， Leok M， et al. Nonlinear dynamics of the 3D pendulum. SIAM Journal on Applied Dynamical Systems， 2007， 7（2）: 144-160

［3］ Gong Q， Ross I M， Kang W， et al. Connections between the convector mapping theorem and convergence of pseudospectral methods for optimal control. Computational Optimization and Applications， 2008，41: 307-335

［4］ 雍恩米， 陳磊， 唐國金.飛行器軌跡優化數值方法綜述. 宇航學報， 2008， 29（2）: 397-406

［5］ Fahroo F， Ross I M. Direct trajectory optimization by a Chebyshev pseudospectral method. Journal of Guidance， Control， and Dynamics， 2002， 25（1）: 160-166

［6］ Shamsi M， Dehghan M. Determination of a control function in three-dimensional parabolic equations by Legendre pseudospectral method. Numeral Methods Partial Differential Equation， 2012， 28: 74-93

[7] 朱寧， 戈新生. 勒讓德偽譜法求解三維剛體擺姿態運動最優控制問題. 力學與實踐， 2015（4）: 481-487

［8］ Lu P. Regulation about time-varying trajectories: precision entry guidance illustrated. Journal of Guidance， Control， and Dynamics， 1999， 22（6）: 784-790

［9］ Yan H， Fahroo F， Ross I M. Optimal feedback control laws by legendre pseudospectral approximations // Proceedings of the American Control Conference. Piscataway， 2001: 2388-2393

［10］ 莊宇飛， 黃海濱. 欠驅動航天器實時最優控制算法設計. 系統工程與電子技術， 2013， 35（7）: 1477-1485

［11］ 莊宇飛， 馬廣富， 黃海濱. 欠驅動剛性航天器時間最優軌跡規劃設計. 控制與決策， 2010， 25（10）: 1469-1473

［12］ Chaturvedi N A， McClamroch N H. Asymptotic stabilization of the hanging equilibrium manifold of the 3D pendulum. International Journal of Robust and Nonlinear Control， 2007， 17: 1435-1454

［13］ Chaturvedi N A， McClamroch N H， Bernstein D S. Stabilization of a specified equilibrium in the inverted equilibrium manifold of the 3D pendulum // Proceedings of the 2007 American Control Conference. New York， 2007: 2485-2490

Trajectory Tracking Control of 3D Rigid Body Pendulum Attitude Based on Legendre Pseudospectral Method

GE Xinsheng?， ZHU Ning

College of Science， Beijing Information Science and Technology University， Beijing 100192； ? E-mail: gebim@vip.sina.com

The optimal control of the attitude motion of 3D rigid pendulum with initial disturbance is investigated. Combined with the characteristics of the attitude and angular velocity of the 3D rigid pendulum， the closed-loop feedback attitude tracking controller is designed for the external disturbance. Firstly， 3D rigid pendulum attitude trajectory is designed for open loop by use of Legendre pseudospectra method. Then the system's motion equation is linearized， and the difference between the attitude reference trajectory and actual trajectory motion in 3D rigid pendulum is considered as control variable. Attitude tracking problem is converted to linear time-varying systems attitude regulation problem. Finally， the closed-loop control based on the Legendre pseudospectral method is simulated and analyzed for the optimal control of 3D rigid pendulum， and simulations show that the effectiveness in the case of initial disturbance.

3D rigid pendulum； attitude control； optimal control； pseudospectral method

O316

10.13209/j.0479-8023.2016.071

國家自然科學基金（11472058）資助

2015-12-20；

2016-02-03； 網絡出版日期: 2016-07-14