戈新生　朱　寧

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

?

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

戈新生?朱寧

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

研究3D剛體擺在有初始擾動(dòng)情況下的姿態(tài)運(yùn)動(dòng)最優(yōu)控制問題。結(jié)合 3D 剛體擺轉(zhuǎn)動(dòng)的姿態(tài)與角速度特點(diǎn)， 針對(duì)外部擾動(dòng)設(shè)計(jì)閉環(huán)反饋?zhàn)藨B(tài)跟蹤控制器。首先， 利用Legendre偽譜法規(guī)劃出3D剛體擺開環(huán)的姿態(tài)運(yùn)動(dòng)軌跡。然后， 將系統(tǒng)的運(yùn)動(dòng)方程線性化， 并以 3D 剛體擺的實(shí)際運(yùn)動(dòng)姿態(tài)軌跡與參考運(yùn)動(dòng)姿態(tài)軌跡之間的差值作為控制量， 將姿態(tài)跟蹤問題轉(zhuǎn)換為線性時(shí)變系統(tǒng)的姿態(tài)調(diào)節(jié)問題。最后， 對(duì)基于 Legendre 偽譜法的3D剛體擺姿態(tài)最優(yōu)控制的閉環(huán)控制方法進(jìn)行仿真分析， 驗(yàn)證在具有初始擾動(dòng)情況下算法的有效性。

3D剛體擺； 姿態(tài)控制； 最優(yōu)控制； 偽譜法

北京大學(xué)學(xué)報(bào)（自然科學(xué)版）第52卷第4期2016年7月

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

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

偽譜法采用全局正交多項(xiàng)式對(duì)系統(tǒng)的狀態(tài)空間進(jìn)行逼近， 從而將最優(yōu)控制問題轉(zhuǎn)化為非線性規(guī)劃問題進(jìn)行求解［3］。近年來， 偽譜法因其良好的計(jì)算效率和精度以及對(duì)初始參數(shù)取值不敏感的優(yōu)點(diǎn)， 成為近年來求解最優(yōu)控制問題［4-6］新方法。偽譜法在航天領(lǐng)域多用于軌跡優(yōu)化問題， 對(duì)姿態(tài)運(yùn)動(dòng)最優(yōu)控制問題研究鮮有報(bào)道。

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

本文在開環(huán)控制的基礎(chǔ)上， 設(shè)計(jì)閉環(huán)軌跡跟蹤控制器。開環(huán)優(yōu)化的實(shí)時(shí)性是保證重規(guī)劃算法正常運(yùn)行的必要條件， Bellman 最優(yōu)性原理則是重規(guī)劃算法的核心思想。如圖 1 所示， Bellman 最優(yōu)性原理可以簡(jiǎn)單地描述為: 如果從P1點(diǎn)到P2點(diǎn)的軌跡是最優(yōu)的， 且P3點(diǎn)為該最優(yōu)軌跡上不同于P1， P2的任意一點(diǎn)， 則從P2點(diǎn)到P3點(diǎn)的軌跡仍是最優(yōu)的。

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

本文主要研究3D剛體擺在有初始擾動(dòng)情況下的軌跡姿態(tài)控制問題， 并以開環(huán)偽譜法最優(yōu)控制為基礎(chǔ)， 采用內(nèi)外環(huán)控制結(jié)構(gòu)的算法， 設(shè)計(jì)帶有狀態(tài)反饋的閉環(huán)最優(yōu)姿態(tài)軌跡跟蹤控制器。將系統(tǒng)的運(yùn)動(dòng)方程相對(duì)于參考軌跡線性化， 且以3D剛體擺實(shí)際運(yùn)行軌跡與參考軌跡之間的偏差為控制量， 將姿態(tài)跟蹤問題轉(zhuǎn)化為線性時(shí)變系統(tǒng)的姿態(tài)調(diào)節(jié)問題。結(jié)合Legendre偽譜法， 針對(duì)時(shí)變的線性化系統(tǒng)設(shè)計(jì)最優(yōu)調(diào)節(jié)控制器， 從而實(shí)現(xiàn) 3D 剛體擺的狀態(tài)閉環(huán)的軌跡跟蹤控制。

1　3D剛體擺的數(shù)學(xué)模型

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

在慣性坐標(biāo)系｛｝OXYZ下， 采用3-2-1歐拉姿態(tài)角描述方式， 并考慮重力矩的作用， 3D剛體擺的數(shù)學(xué)模型［12］為

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

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

3D 剛體擺在空間坐標(biāo)系內(nèi)存在兩個(gè)平衡位置，即懸垂平衡點(diǎn)和倒立平衡點(diǎn)［12-13］， 意味著 3D 剛體擺的最優(yōu)控制問題為從任意初始位置運(yùn)動(dòng)到目標(biāo)平衡位置的姿態(tài)運(yùn)動(dòng)控制的過程。

2　Legendre閉環(huán)偽譜法求解

在理想無干擾的環(huán)境中， 3D剛體擺可以根據(jù)控制目標(biāo)， 利用Legendre偽譜法規(guī)得到的 3D 剛體擺的最優(yōu)運(yùn)動(dòng)軌跡［7］。然后， 考慮 3D 剛體擺姿態(tài)控制過程中存在初始擾動(dòng)的情況， 應(yīng)用偽譜法并結(jié)合重規(guī)劃策略， 設(shè)計(jì)狀態(tài)閉環(huán)的軌跡跟蹤控制器。將3D 剛體擺姿態(tài)運(yùn)動(dòng)模型（式（1）和（2））在參考軌跡上線性化， 則有

其中，

根據(jù)線性化方程（6）， 利用偽譜法設(shè)計(jì)姿態(tài)穩(wěn)定控制器的目標(biāo)， 就是通過確定輸入修正量（）tΔu以及狀態(tài)偏差量（）tΔx， 使得最優(yōu)泛函

最小。式（7）中， S是半正定的末端加權(quán)矩陣， Q是半正定的狀態(tài)加權(quán)陣， R 是正定的控制加權(quán)陣， 則相應(yīng)的Hamilton函數(shù)為

于是， 根據(jù)最優(yōu)性必要條件， 有

與之對(duì)應(yīng)的邊值條件為

由上述分析可知， 采用閉環(huán)控制算法設(shè)計(jì)姿態(tài)調(diào)節(jié)器時(shí)， 需要開環(huán)的最優(yōu)控制兩點(diǎn)邊值問題能夠時(shí)刻求解。由于 3D 剛體擺模型具有強(qiáng)非線性特性，導(dǎo)致求解 Riccati 方程過程復(fù)雜， 運(yùn)算量大。因此，本節(jié)采用 Legendre 偽譜法將兩點(diǎn)邊值問題離散化，將其求解微分方程組問題簡(jiǎn)化為求解一組線性代數(shù)方程。

在 LGL （Legendre-Gauss-Lobatto）點(diǎn)ζη（η = 0，1， 2， …， N）上將狀態(tài)變量Δx及協(xié)態(tài)向量 λ 離散化，則有

其中， D = （Dkη）為（N+1）×（N+1）階導(dǎo)數(shù)矩陣:

兩點(diǎn)邊值問題在LGL點(diǎn)上離散化， 有

式中，

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

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

計(jì)算過程如下。

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

第2步， 在參考軌跡上將 3D 剛體擺的運(yùn)動(dòng)模型線性化并確定初值

第4步， 將代數(shù)方程組（17）的解代入方程（10），求得最優(yōu)控制修正量

3　仿真實(shí)驗(yàn)及分析

設(shè) 3D 剛體擺的質(zhì)量為 m=140 kg， 轉(zhuǎn)動(dòng)慣量為J = diag（40， 45， 50） kg · m2， 支點(diǎn)與質(zhì)心之間的距離為 l=0.1 m。假設(shè) 3D 剛體擺在 t =0 時(shí)刻的干擾誤差為

設(shè) 3D 剛體擺的初始位置角速度為

約化姿態(tài)為

設(shè)目標(biāo)位置為懸垂平衡位置， 其角速度為

約化姿態(tài)為

插值點(diǎn) N=35， 仿真結(jié)果如圖3所示。

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

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

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

同樣， 在滿足相同初始擾動(dòng)存在的情況下， 設(shè)倒立平衡的初始角速度為約化姿態(tài)為

設(shè)目標(biāo)倒立平衡點(diǎn)角速度為

約化姿態(tài)為

3D 剛體擺在閉環(huán)反饋?zhàn)顑?yōu)控制作用下， 倒立平衡仿真結(jié)果如圖7～10所示。

圖 7 所示的倒立情況下， 開環(huán)控制的角速度受到初始擾動(dòng)， 逐漸偏離最優(yōu)參考軌跡， 失去控制效果， 而閉環(huán)控制能夠有效地抵抗初始擾動(dòng)的干擾。在初段被干擾時(shí)， 該控制策略通過反饋?zhàn)饔媚軌蜓杆僬{(diào)整3D剛體擺的角速度變化， 沿最優(yōu)軌跡使3D剛體擺到達(dá)目標(biāo)倒立平衡點(diǎn)， 且角速度趨近于 0。

圖 8 為倒立情況下 3D 剛體擺的約化姿態(tài)變化曲線， 說明在存在初始擾動(dòng)的情況下， 閉環(huán)控制能夠有效地抵抗初始擾動(dòng)的干擾， 迅速調(diào)整 3D 剛體擺的空間姿態(tài)， 使 3D 剛體擺沿最有參考軌跡運(yùn)動(dòng)到目標(biāo)倒立平衡點(diǎn)。相反， 開環(huán)最優(yōu)控制則不能消除初始擾動(dòng)的影響， 逐漸偏離最優(yōu)軌跡， 無法運(yùn)動(dòng)到目標(biāo)倒立平衡點(diǎn)。

圖9所示的最優(yōu)控制輸入力矩曲線在上下限間切換變化。圖 10 形象地反映了系統(tǒng)姿態(tài)在空間坐標(biāo)內(nèi)的運(yùn)動(dòng)軌跡。

4　結(jié)論

本文針對(duì) 3D 剛體擺運(yùn)動(dòng)姿態(tài)穩(wěn)定控制問題，設(shè)計(jì)了 Legendre 偽譜法閉環(huán)控制器。仿真結(jié)果表明， 基于Legendre偽譜法閉環(huán)控制算法能夠有效地抑制初始擾動(dòng)給 3D 剛體擺運(yùn)動(dòng)過程帶來的不利影響， 按照約束條件的要求達(dá)到目標(biāo)平衡位置， 進(jìn)一步證明了偽譜法求解 3D 剛體擺最優(yōu)控制問題的可行性和有效性， 也為求解類似復(fù)雜約束系統(tǒng)的最優(yōu)控制問題提供了新的途徑。

［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］ 雍恩米， 陳磊， 唐國金.飛行器軌跡優(yōu)化數(shù)值方法綜述. 宇航學(xué)報(bào)， 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] 朱寧， 戈新生. 勒讓德偽譜法求解三維剛體擺姿態(tài)運(yùn)動(dòng)最優(yōu)控制問題. 力學(xué)與實(shí)踐， 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］ 莊宇飛， 黃海濱. 欠驅(qū)動(dòng)航天器實(shí)時(shí)最優(yōu)控制算法設(shè)計(jì). 系統(tǒng)工程與電子技術(shù)， 2013， 35（7）: 1477-1485

［11］ 莊宇飛， 馬廣富， 黃海濱. 欠驅(qū)動(dòng)剛性航天器時(shí)間最優(yōu)軌跡規(guī)劃設(shè)計(jì). 控制與決策， 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

國家自然科學(xué)基金（11472058）資助

2015-12-20；

2016-02-03； 網(wǎng)絡(luò)出版日期: 2016-07-14