CN104897156A - 一种旋转调制惯性导航系统的旋转控制方法 - Google Patents

Abstract

本发明提供一种旋转调制惯性导航系统的旋转控制方法，属于旋转调制惯性导航系统领域。本发明在自抗扰控制方法和滑模变结构控制方法的基础上，提出了一种融合策略，形成了旋转调制惯性导航系统的复合控制方法。当控制误差较大时，主要采用滑模变结构控制方法，加快系统响应速度，迅速减小误差；随着控制误差的减小，利用自抗扰控制的精确控制能力，保证控制的精度，提高控制的平稳度和鲁棒性。本发明提出的控制方法提高了系统的响应速度和控制精度，同时也抑制了滑模变结构控制带来的抖振，从而减小了旋转平台控制效果的不理想给导航精度带来的负面影响。

Description

一种旋转调制惯性导航系统的旋转控制方法

技术领域

本发明属于旋转调制惯性导航系统领域，尤其涉及一种旋转调制惯性导航系统的旋转控制方法。

背景技术

惯性导航系统是复杂的高精度机电综合系统，由于具有完全自主性的优点而广泛应用于陆海空天领域。惯性敏感元件的误差是惯性导航系统误差的主要决定因素。从工艺上提高惯性敏感元件的精度，技术难度大、周期长。因此，在惯性敏感元件的精度达到一定要求后，通常采用系统技术补偿元件误差，而旋转调制技术就是一种行之有效的方法。该方法将惯性元件或者IMU外面再加上旋转平台和控制机构，利用翻转或者旋转来平均掉惯性元件漂移对导航的影响，从而提高惯导的导航精度。目前有关旋转调制惯导的大部分工作都集中到惯导解算算法、旋转方案的设计、初始对准以及误差标定等领域。这些研究都是以假定理想控制旋转平台为前提的。但是实际上，旋转平台的控制是存在误差的，且这些误差也会对导航精度产生很大的影响。如果控制精度差，不仅不能补偿惯性传感器的误差，还会引入新的误差，影响导航精度。

旋转方案的设计是旋转式惯导领域的研究热点之一，很多旋转方案相继被提出，其中采用最多的是连续正反旋转和多位置转停方案。在工作时，这两类方案要求旋转平台重复地进行换向旋转、迅速停止和快速启动。而旋转平台是一个复杂的伺服机构，不平衡力矩、电机力矩波动、转动惯量变化等因素会给旋转平台在进行上述操作时带来很大的干扰。此外旋转调制技术的工作转速较低，一般在1°/s到50°/s之间。低速情况下的非线性摩擦干扰力矩也会严重影响旋转平台的控制效果。目前工程上的控制方法主要采用PID算法。PID算法简单有效，容易实现，但是容易出现超调，且鲁棒性不高，响应速度偏低，难以满足系统高精度的性能要求。

发明内容

为解决上述问题，本发明提供一种旋转调制惯性导航系统的旋转控制方法，提高了系统的响应速度和控制精度，同时也抑制了滑模变结构控制带来的抖振，从而减小了旋转平台控制效果的不理想给导航精度带来的负面影响。

本发明的旋转调制惯性导航系统的旋转控制方法，该旋转调制惯性导航系统包括：旋转平台、自抗扰控制器和滑模变结构控制器，控制方法包括以下步骤：

步骤1，建立旋转平台的控制模型；

步骤11，根据旋转平台转轴的动力学方程和电机的工作原理得到单轴数据模型 $\stackrel{\·\·}{\mathrm{\θ}}=-\frac{1}{J}{k}_e{k}_T\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J}{k}_p{k}_T{u}_r-\frac{1}{J}{M}_d---\left(1\right);$

其中k₁/R_a＝k_T，θ为转轴的旋转角度，J为转轴和旋转平台的转动惯量，k_t为旋转平台电机的电磁转矩系数，k_e为旋转平台电机的反电势常数，k_p为旋转平台的功率放大器倍数，R_a为旋转平台电机的电枢回路总电阻，u_r为作用于旋转平台的控制量，M_d为作用于旋转平台的干扰量；

步骤12，令x₁、x₂分别表示θ、则将(1)改写成状态空间方程(2)：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_1\\ {\stackrel{\·}{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right]u_r+\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]---\left(2\right)$

令 $X=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right],\tilde{A}=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right],\tilde{B}=\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right],{M_d}^{\′}=\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]$ 则将(2)改写为(3)

$\stackrel{\·}{X}=\tilde{A}X+\tilde{B}{u}_r+{M_d}^{\′}---\left(3\right)$

步骤13，将公式(3)离散化可得公式(4)

$X(k+1)=\mathrm{AX}\left(k\right)+B{u}_r\left(k\right)+L\widetilde{M_d}\left(k\right)---\left(4\right)$

其中，k、k+1代表时刻，u_r(k)表示k时刻控制量u_r的取值； $\begin{array}{c}A=e^{\tilde{A}T}\\ B={\∫}_0^T{e}^{\tilde{A}t}\tilde{B}\mathrm{dt}\\ L={\∫}_0^T{e}^{\tilde{A}t}\mathrm{dt}\end{array},$ T为旋转周期；

步骤2，建立自抗扰控制器的反馈模型，所述自抗扰控制器包括：跟踪微分器、扩张状态观测器和非线性反馈控制器；

步骤21，根据迭代公式 $\left\{\begin{array}{c}{e}_k=v_1\left(k\right)-r\left(k\right)\\ v_1(k+1)=v_1\left(k\right)+h{v}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+h\·\mathrm{fhan}(e_k,v_2\left(k\right),r_0,h_0)\end{array}\right.$ 获得跟踪微分器第k+1时刻的跟踪信号v₁(k+1)、微分信号v₂(k+1)，并输出至非线性反馈控制器，其中，跟踪信号、微分信号的初始值为给定值，fhan为最速综合控制函数，其为通用函数，h为积分步长，h₀是滤波因子，r₀是速度因子；r(k)为系统输入位置指令；

步骤22，根据迭代公式(5)获得扩张状态观测器的第k+1时刻的干扰估计输出量z₃(k+1)，并输出至非线性反馈控制器，其中，z₁(k)、z₂(k)、z₃(k)分别为状态量x₁、x₂、的观测估计，初始值为给定值；β₀₁、β₀₂、β₀₃为可调参数，为经验值；

$\left\{\begin{array}{c}{e}_0\left(k\right)=z_1\left(k\right)-x_1\left(k\right)\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\mathrm{\β}}_{01}{e}_0\left(k\right))\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\mathrm{\β}}_{02}{e}_0\left(k\right)+\frac{1}{J}{k}_p{k}_T{u}_r\left(k\right)-\frac{1}{J}{k}_e{k}_T{z}_2\left(k\right))\\ z_3(k+1)=z_3\left(k\right)+h(-{\mathrm{\β}}_{03}{e}_0\left(k\right))\end{array}\right.---\left(5\right)$

步骤23，根据跟踪信号v₁(k+1)、微分信号v₂(k+1)和干扰估计输出量z₃(k+1)利用迭代公式(6)获得非线性误差反馈控制器的输出u_ADRC(k)，其中α₁、α₂、δ₁、β₁、β₂为可调参数，为经验值；fal()为具有线性段的连续的幂次函数；

$\left\{\begin{array}{c}{e}_1\left(k\right)=v_1\left(k\right)-x_1\left(k\right)\\ e_2\left(k\right)=v_2\left(k\right)-x_2\left(k\right)\\ f{e}_1\left(k\right)=\mathrm{fal}(e_1\left(k\right),{\mathrm{\α}}_1,{\mathrm{\δ}}_1)\\ f{e}_2\left(k\right)=\mathrm{fal}(e_2\left(k\right),{\mathrm{\α}}_2,{\mathrm{\δ}}_1)\\ u_0\left(k\right)={\mathrm{\β}}_{1}f{e}_1\left(k\right)+{\mathrm{\β}}_{2}f{e}_2\left(k\right)\\ u_{\mathrm{ADRC}}\left(k\right)=u_0\left(k\right)-\frac{J}{k_p{k}_T}{z}_3\left(k\right)\end{array}\right.---\left(6\right)$

步骤3，根据公式(7)计算基于指数趋近率的滑模变结构控制器的输出u_SMC(k)；

u_SMC(k)＝(C_eB)^-1(C_eR(k+1)-C_eAX(k)-C_eLZ_d(k)-s(k)+εTsgn(s(k))+qTs(k))   (7)

其中C_e＝[c  1]；

$\begin{array}{c}s\left(k\right)=C_{e}E\left(k\right)=C_e(R\left(k\right)-X\left(k\right))\\ =c(r\left(k\right)-x_1\left(k\right))+(\mathrm{dr}\left(k\right)-x_2\left(k\right))\end{array},Z_d\left(k\right)={\left[\begin{array}{cc}0& z_3\left(k\right)\end{array}\right]}^T;$

R(k+1)＝[r(k+1)  dr(k+1)]；

取R(k)＝[r(k)  dr(k)]，采用线性外推的方法可以得到R(k+1)＝[r(k+1)  dr(k+1)]；

$\begin{array}{c}r(k+1)=2r\left(k\right)-r(k-1)\\ \mathrm{dr}(k+1)=2\mathrm{dr}\left(k\right)-\mathrm{dr}(k-1)\end{array},$ c为决定滑模面斜率的可调参数，ε为克服摄动及外干扰的可调参数、q为改变系统向滑模面的趋近速度的可调参数；dr(k)为系统输入位置指令r(k)的变化率；

步骤4，根据公式(8)确定作用于旋转平台的控制量u_r(k)；

u_r(k)＝u_ADRC(k)·a(k)+u_SMC(k)·b(k)   (8)

其中，a(k)＝1-tanh(β|e(k)|)，b(k)＝1-a(k)＝tanh(β|e(k)|)；e(k)＝r(k)-x₁(k)，e(k)为k时刻的控制误差；β为融合因子，根据自抗扰控制器和滑模变结构控制器的控制误差得到；

步骤5，利用控制量u_r(k)控制旋转平台，实现旋转平台角度和速度的精确控制。

有益效果：

与已有的旋转调制惯导系统中旋转控制的方法比较，本发明不需要建立非线性摩擦和外在干扰的精确数学模型，自抗扰控制器包含的扩张状态观测器可以将所有外部扰动和包括系统参数变化在内的内部扰动扩张成一个新的变量进行观测，然后利用得到的观测值进行扰动补偿，这样为控制效果的精确度和平稳度提供了保障，提高了系统的鲁棒性。同时，当控制误差较大时，主要采用滑模变结构控制方法，加快系统响应速度，迅速减小控制误差；随着控制误差的减小，利用自抗扰控制的精确控制能力，保证控制的精度，提高平稳度和鲁棒性。这样结合这两种控制方法的优点，提高了系统的响应速度和控制精度，同时也抑制了滑模变结构控制带来的抖振，从而减小了旋转平台控制效果的不理想给导航精度带来的负面影响。

附图说明

图1为本发明的具体实施例中的输入参考位置信号示意图；

图2为本发明的具体实施例中的旋转平台平稳运行时的角速度误差比较图；

图3为本发明的具体实施例中的旋转平台换向时的角度超调误差和调节时间比较图；

图4为本发明的具体实施例中的旋转平台静止状态下的抖振误差比较图。

具体实施方式

本发明的旋转调制惯性导航系统的旋转控制方法，其设计的自抗扰控制器所包含的扩张状态观测器可以将所有外部扰动和包括系统参数变化在内的内部扰动扩张成一个新的变量，如式(6)的z₃(k+1)，进行观测，然后利用式(7)的进行扰动补偿，这样为控制效果的精确度和平稳度提供了保障，提高了系统的鲁棒性，提高了系统的响应速度和控制精度，同时利用式(8)的Z_d(k)＝[0  z₃(k)]^T抑制了滑模变结构控制带来的抖振，输出精准的u_SMC(k)，从而减小了旋转平台控制效果的不理想给导航精度带来的负面影响。

具体包括以下步骤：

步骤一、建立旋转平台的数学方程

步骤11，根据旋转平台转轴的动力学方程和电机的工作原理得到单轴数据模型 $\stackrel{\·\·}{\mathrm{\θ}}=-\frac{1}{J}{k}_e{k}_T\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J}{k}_p{k}_T{u}_r-\frac{1}{J}{M}_d---\left(1\right);$

其中k_t/R_a＝k_T，θ为转轴的旋转角度，J为转轴和旋转平台的转动惯量，k_t为旋转平台电机的电磁转矩系数，k_e为旋转平台电机的反电势常数，k_p为旋转平台的功率放大器倍数，R_a为旋转平台电机的电枢回路总电阻，u_r为作用于旋转平台的控制量，M_d为作用于旋转平台的干扰量。

步骤12，令x₁、x₂分别表示θ、则将(1)改写成状态空间方程(2)：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_1\\ {\stackrel{\·}{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right]u_r+\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]---\left(2\right)$

令 $X=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right],\tilde{A}=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right],\tilde{B}=\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right],{M_d}^{\′}=\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]$ 则将(2)改写为(3)

$\stackrel{\·}{X}=\tilde{A}X+\tilde{B}{u}_r+{M_d}^{\′}---\left(3\right)$

步骤13，将模型(3)离散化可得(4)

$X(k+1)=\mathrm{AX}\left(k\right)+B{u}_r\left(k\right)+L\widetilde{M_d}\left(k\right)---\left(4\right)$

括号中的k、k+1代表时刻k、k+1，则u_r(k)表示k时刻控制量u_r的取值。

步骤二、设计合适的自抗扰控制器。

自抗扰控制器主要由跟踪微分器、扩张状态观测器和非线性反馈控制器三部分组成，这三个部分可以进行单独设计。

首先设计跟踪微分器，

步骤21，给定输入信号r的跟踪信号及其微分信号的初始值v₁(0)、v₂(0)，根据迭代公式(5)计算跟踪微分器第k+1时刻的跟踪信号v₁(k+1)、微分信号v₂(k+1)：

$\left\{\begin{array}{c}{e}_k=v_1\left(k\right)-r\left(k\right)\\ v_1(k+1)=v_1\left(k\right)+h{v}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+h\·\mathrm{fhan}(e_k,v_2\left(k\right),r_0,h_0)\end{array}\right.---\left(5\right)$

其中fhan为最速综合控制函数，其为通用函数。

h为积分步长，h₀是滤波因子。r₀是速度因子，决定对输入信号的跟踪速度，值越大跟踪速度越快。

步骤22，根据z₃(k+1)＝z₃(k)+h(-β₀₃e₀(k))计算线性扩张状态观测器的干扰估计输出量z₃(k+1)，公式中e₀(k)＝z₁(k)-x₁(k)，其中，z₁(k+1)＝z₁(k)+h(z₂(k)-β₀₁e₀(k))， $z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\mathrm{\β}}_{02}{e}_0\left(k\right)+\frac{1}{J}{k}_p{k}_T{u}_r\left(k\right)-\frac{1}{J}{k}_e{k}_T{z}_2\left(k\right)),$ 初始值均为设定值。

设计如下：

$\left\{\begin{array}{c}{e}_0\left(k\right)=z_1\left(k\right)-x_1\left(k\right)\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\mathrm{\β}}_{01}{e}_0\left(k\right))\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\mathrm{\β}}_{02}{e}_0\left(k\right)+\frac{1}{J}{k}_p{k}_T{u}_r\left(k\right)-\frac{1}{J}{k}_e{k}_T{z}_2\left(k\right))\\ z_3(k+1)=z_3\left(k\right)+h(-{\mathrm{\β}}_{03}{e}_0\left(k\right))\end{array}\right.---\left(6\right)$

式中，z₁、z₂、z₃分别是对状态量x₁、x₂、及其它干扰的观测估计。，

β₀₁、β₀₂、β₀₃为可调参数；

步骤23，根据跟踪信号v₁(k+1)、微分信号v₂(k+1)和干扰估计输出量z₃(k+1)求非线性误差反馈控制器的输出u_ADRC(k)，如下

$\left\{\begin{array}{c}{e}_1\left(k\right)=v_1\left(k\right)-x_1\left(k\right)\\ e_2\left(k\right)=v_2\left(k\right)-x_2\left(k\right)\\ f{e}_1\left(k\right)=\mathrm{fal}(e_1\left(k\right),{\mathrm{\α}}_1,{\mathrm{\δ}}_1)\\ f{e}_2\left(k\right)=\mathrm{fal}(e_2\left(k\right),{\mathrm{\α}}_2,{\mathrm{\δ}}_1)\\ u_0\left(k\right)={\mathrm{\β}}_{1}f{e}_1\left(k\right)+{\mathrm{\β}}_{2}f{e}_2\left(k\right)\\ u_{\mathrm{ADRC}}\left(k\right)=u_0\left(k\right)-\frac{J}{k_p{k}_T}{z}_3\left(k\right)\end{array}\right.---\left(7\right)$

式中，α₁、α₂、δ₁、β₁、β₂为可调参数。

步骤三、求基于指数趋近率的滑模变结构控制器的输出u_SMC(k)。

u_SMC(k)＝(C_eB)^-1(C_eR(k+1)-C_eAX(k)-C_eLA_d(k)-s(k)+εTsgn(s(k))+qTs(k))   (8)

其中

$,C_e=\left[\begin{array}{cc}c& 1\end{array}\right],\begin{array}{c}s\left(k\right)=C_{e}E\left(k\right)=C_e(R\left(k\right)-X\left(k\right))\\ =c(r\left(k\right)-x_1\left(k\right))+(\mathrm{dr}\left(k\right)-x_2\left(k\right))\end{array},Z_d\left(k\right)={\left[\begin{array}{cc}0& z_3\left(k\right)\end{array}\right]}^T$

R(k+1)＝[r(k+1)  dr(k+1)]，系统输入位置指令为r(k)，其变化率为dr(k)，取R(k)＝[r(k)  dr(k)]，采用线性外推的方法可以得到R(k+1)＝[r(k+1)  dr(k+1)]，其中

                                r(k+1)＝2r(k)-r(k-1)

                             dr(k+1)＝2dr(k)-dr(k-1)

式中c、ε、q为可调参数。

c决定滑模面的斜率，其目的是保证滑模运动渐进稳定且具有较快的动态响应速度，其值越大，滑模运动段响应速度越快，但是值过大容易引起系统较大的抖动。q主要影响切换函数动态过渡过程，适当调整该参数能够改变系统向滑模面的趋近速度，其值越大，到达滑模面的速度越快，但是值过大会导致系统的抖动。ε影响系统克服摄动及外干扰的主要参数，其值越大，克服干扰的能力越强，但是值过大会增加系统抖振的幅度，因为系统抖振的幅度与其值成正比。

步骤四、设计复合控制算法求作用于旋转平台的控制量u_r(k)，复合控制算法为：

u_r(k)＝u_ADRC(k)·a(k)+u_SMC(k)·b(k)      (17)

其中

a(k)＝1-tanh(β|e(k)|)

                                      (18)

b(k)1-a(k)tank(β|e(k)|)

式中，e(k)为k时刻的控制误差，取值如下

e(k)＝r(k)-x₁(k)                      (19)

参数β为融合因子，它决定了系统响应阶段两种控制所占的比例，根据自抗扰控制器和滑模变结构控制器的控制误差得到。

步骤四，利用控制量控制旋转平台，实现旋转平台角度和速度的精确控制。

经过上述步骤即可在时刻k根据控制误差和观测得到的干扰值计算得到控制量然后作用于被控旋转平台，使旋转平台的位置跟踪上输入的参考位置信息。

为了说明本发明的效果，在Simulink平台上进行仿真实验。在相同的实验设置下，采用经典PID控制算法，自抗扰控制方法，滑模变结构控制方法和上述复合控制算法分别进行控制，然后比较控制效果。输入参考位置信号如图1所示时，其横坐标为时间，纵坐标为位置；平稳运行时的角速度误差比较图如图2所示，横坐标为时间，纵坐标为角速度误差；平台换向时的角度超调角误差和调节时间比较图如图3所示，横坐标为时间，纵坐标为位置误差；平台静止状态下的抖振误差比较图如图4所示，横坐标为时间，纵坐标为位置误差。

图2说明了在平稳角度跟踪时复合控制比传统PID控制精度更高；图3说明了在旋转平台转向时复合控制比传统PID控制角度超调误差更小，调节时间更短，系统响应速度更快；图4说明了旋转平台处于停止状态时复合控制比传统PID控制抖振误差更小。

自抗扰控制器包含的扩张状态观测器可以将所有外部扰动和包括系统参数变化在内的内部扰动扩张成一个新的变量进行观测，然后利用得到的观测值进行扰动补偿，这样为控制效果的精确度和平稳度提供了保障，提高了系统的鲁棒性。同时，当控制误差较大时，主要采用滑模变结构控制方法，加快系统响应速度，迅速减小控制误差；随着控制误差的减小，利用自抗扰控制的精确控制能力，保证控制的精度，提高平稳度和鲁棒性。这样结合这两种控制方法的优点，提高了系统的响应速度和控制精度，同时也抑制了滑模变结构控制带来的抖振，从而减小了旋转平台控制效果的不理想给导航精度带来的负面影响。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (1)

1.一种旋转调制惯性导航系统的旋转控制方法，其特征在于，该旋转调制惯性导航系统包括：旋转平台、自抗扰控制器和滑模变结构控制器，控制方法包括以下步骤：

步骤1，建立旋转平台的控制模型；

步骤11，根据旋转平台转轴的动力学方程和电机的工作原理得到单轴数据模型 $\stackrel{..}{\mathrm{\θ}}=-\frac{1}{J}{k}_e{k}_T\stackrel{.}{\mathrm{\θ}}+\frac{1}{J}{k}_p{k}_T{u}_r-\frac{1}{J}{M}_d---\left(1\right);$

其中k_t/R_a＝k_T，θ为转轴的旋转角度，J为转轴和旋转平台的转动惯量，k_t为旋转平台电机的电磁转矩系数，k_e为旋转平台电机的反电势常数，k_p为旋转平台的功率放大器倍数，R_a为旋转平台电机的电枢回路总电阻，u_r为作用于旋转平台的控制量，M_d为作用于旋转平台的干扰量；

步骤12，令x₁、x₂分别表示θ、则将(1)改写成状态空间方程(2)：

$\left[\begin{array}{c}{\stackrel{.}{x}}_1\\ {\stackrel{.}{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right]u_r+\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]---\left(2\right)$

令 $X=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right],\tilde{A}=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{J}{k}_e{k}_T\end{array}\right],\tilde{B}=\left[\begin{array}{c}0\\ \frac{1}{J}{k}_p{k}_T\end{array}\right],{M_d}^{\′}\left[\begin{array}{c}0\\ -\frac{1}{J}{M}_d\end{array}\right]$ 则将(2)改写为(3)

$\stackrel{.}{X}=\tilde{A}x+\tilde{B}{u}_r+{M_d}^{\′}---\left(3\right)$

步骤13，将公式(3)离散化可得公式(4)

$X(k+1)=\mathrm{AX}\left(k\right)+{\mathrm{Bu}}_r\left(k\right)+L{\tilde{M}}_d\left(k\right)---\left(4\right)$

其中，k、k+1代表时刻，u_r(k)表示k时刻控制量u_r的取值； $\begin{array}{c}A=e^{\tilde{A}T}\\ B={\∫}_0^T{e}^{\tilde{A}t}\tilde{B}\mathrm{dt}\\ L={\∫}_0^T{e}^{\tilde{A}t}\mathrm{dt}\end{array},$ T为旋转周期；

步骤2，建立自抗扰控制器的反馈模型，所述自抗扰控制器包括：跟踪微分器、扩张状态观测器和非线性反馈控制器；

步骤21，根据迭代公式 $\left\{\begin{array}{c}{e}_k=v_1\left(k\right)-r\left(k\right)\\ v_1(k+1)=v_1\left(k\right)+{\mathrm{hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+h\·\mathrm{fhan}(e_k,v_2\left(k\right),r_0,h_0)\end{array}\right.$ 获得跟踪微分器第k+1时刻的跟踪信号v₁(k+1)、微分信号v₂(k+1)，并输出至非线性反馈控制器，其中，跟踪信号、微分信号的初始值为给定值，fhan为最速综合控制函数，其为通用函数，h为积分步长，h₀是滤波因子，r₀是速度因子；r(k)为系统输入位置指令；

步骤22，根据迭代公式(5)获得扩张状态观测器的第k+1时刻的干扰估计输出量z₃(k+1)，并输出至非线性反馈控制器，其中，z₁(k)、z₂(k)、z₃(k)分别为状态量x₁、x₂、的观测估计，初始值为给定值；β₀₁、β₀₂、β₀₃为可调参数，为经验值；

$\left\{\begin{array}{c}{e}_0\left(k\right)=z_1\left(k\right)-x_1\left(k\right)\\ z_1(k+1)=z_1\left(k\right)+h(z_2\left(k\right)-{\mathrm{\β}}_{01}{e}_0\left(k\right))\\ z_2(k+1)=z_2\left(k\right)+h(z_3\left(k\right)-{\mathrm{\β}}_{02}{e}_0\left(k\right)+\frac{1}{J}{k}_p{k}_T{u}_r\left(k\right)-\frac{1}{J}{k}_e{k}_T{z}_2\left(k\right))\\ z_3(k+1)=z_3\left(k\right)+h(-{\mathrm{\β}}_{03}{e}_0\left(k\right))\end{array}\right.---\left(5\right)$

步骤23，根据跟踪信号v₁(k+1)、微分信号v₂(k+1)和干扰估计输出量z₃(k+1)利用迭代公式(6)获得非线性误差反馈控制器的输出u_ADRC(k)，其中α₁、α₂、δ₁、β₁、β₂为可调参数，为经验值；fal()为具有线性段的连续的幂次函数；

$\left\{\begin{array}{c}{e}_1\left(k\right)=v_1\left(k\right)-x_1\left(k\right)\\ e_2\left(k\right)=v_2\left(k\right)-x_2\left(k\right)\\ {\mathrm{fe}}_1\left(k\right)=\mathrm{fal}(e_l\left(k\right),{\mathrm{\α}}_1,{\mathrm{\δ}}_1)\\ {\mathrm{fe}}_2\left(k\right)=\mathrm{fal}(e_2\left(k\right),{\mathrm{\α}}_2,{\mathrm{\δ}}_1)\\ u_0\left(k\right)={\mathrm{\β}}_1{\mathrm{fe}}_1\left(k\right)+{\mathrm{\β}}_2{\mathrm{fe}}_2\left(k\right)\\ u_{\mathrm{ADRC}}\left(k\right)=u_0\left(k\right)-\frac{J}{k_p{k}_T}{z}_3\left(k\right)\end{array}\right.---\left(6\right)$

步骤3，根据公式(7)计算基于指数趋近率的滑模变结构控制器的输出u_SMC(k)；

u_SMC(k)＝(C_eB)^-1(C_eR(k+1)-C_eAX(k)-C_eLZ_d(k)-s(k)+εT sgn(s(k))+qTs(k))   (7)

其中C_e＝[c 1]；

$\begin{array}{c}s\left(k\right)=C_{e}E\left(k\right)=C_e(R\left(k\right)-X\left(k\right))\\ =c(r\left(k\right)-x_1\left(k\right))+(\mathrm{dr}\left(k\right)-x_2\left(k\right))\end{array},Z_d\left(k\right)={\left[\begin{array}{cc}0& z_3\left(k\right)\end{array}\right]}^T;$

R(k+1)＝[r(k+1) dr(k+1)]；

取R(k)＝[r(k) dr(k)]，采用线性外推的方法可以得到R(k+1)＝[r(k+1) dr(k+1)]；

$\begin{array}{c}r(k+1)=2r\left(k\right)-r(k-1)\\ \mathrm{dr}(k+1)=2\mathrm{dr}\left(k\right)-\mathrm{dr}(k-1)\end{array},$ c为决定滑模面斜率的可调参数，ε为克服摄动及外干扰的可调参数、q为改变系统向滑模面的趋近速度的可调参数；dr(k)为系统输入位置指令r(k)的变化率；

步骤4，根据公式(8)确定作用于旋转平台的控制量u_r(k)；

u_r(k)＝u_ADRC(k)·a(k)+u_SMC(k)·b(k)   (8)

其中，a(k)＝1-tanh(β|e(k)|)，b(k)＝1-a(k)＝tanh(β|e(k)|)；e(k)＝r(k)-x₁(k)，e(k)为k时刻的控制误差；β为融合因子，根据自抗扰控制器和滑模变结构控制器的控制误差得到；

步骤5，利用控制量u_r(k)控制旋转平台，实现旋转平台角度和速度的精确控制。