基于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