一种基于自抗扰控制的车载惯导行进间快速初始对准方法

摘要

一种基于自抗扰控制的车载惯导行进间快速初始对准方法,其特征在于：包括以下步骤：一、载车启动之前，对惯导进行初始化，完成惯导静基座条件下的粗对准；二、载车启动之后，利用惯导和里程计输出的速度信息构造观测量；三、利用观测量、惯导误差方程以及自抗扰控制方法对行进间精对准回路进行设计；四、自抗扰控制器参数的整定；五、通过观测器输出的失准角，构造反馈矩阵实现车载惯导行进间的初始对准；通过上述步骤，分别对载车发动之前的粗对准方法、观测量构造、自抗扰控制回路设计方法等进行了详细说明；该方法不需要已知噪声的统计特性，也不需要非常精确的模型就能解决惯导在车载环境下的快速对准问题及车载过程中外部扰动的补偿问题。

主权项

一种基于自抗扰控制的车载惯导行进间快速初始对准方法,其特征在于：具体包括以下步骤：步骤一：载车启动之前，对惯导进行初始化，完成惯导静基座条件下的粗对准；一般的陆用车载武器在启动之前都停驻在基地或者固定的地点，认为其初始位置为所在地的地理位置(L,λ，h)；静基座条件下，认为惯导三个方向的初始速度为零；而初始的姿态矩阵由粗对准来获得；惯导静基座条件下的粗对准又包括解析粗对准和一次修正粗对准；首先进行解析粗对准，解析粗对准依靠重力矢量g和地球自转角速率ω_ie直接估计从机体系即b系到地理坐标系即t系的初始姿态矩阵g和ω_ie在地理坐标系的分量都是确定的，它们表示为：g^t＝[g_E g_N g_U]^T＝[0 0 g]^T  (1)${\omega }_{ie}^t={\left[\begin{array}{ccc}{\omega }_{ieE}^t& {\omega }_{ieN}^t& {\omega }_{ieU}^t\end{array}\right]}^T={\left[\begin{array}{ccc}0& {\omega }_{ie}\cos L& {\omega }_{ie}\sin L\end{array}\right]}^T---\left(2\right)$式中g代表当地重力加速度；ω_ie是地球自转角速率；L代表当地的纬度；再构造出第三个与他们正交的向量r^t＝g^t×ω_ie^t；利用这三个矢量在机体系和地理系中的分量列阵，就求得捷联矩阵的表达式：$C_b^t\left(0\right)={\left[\begin{array}{c}{\left(g^t\right)}^T\\ {\left({\omega }_{ie}^t\right)}^T\\ {\left(r^t\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(g^b\right)}^T\\ {\left({\omega }_{ie}^b\right)}^T\\ {\left(r^b\right)}^T\end{array}\right]---\left(3\right)$将式(1)、(2)带入上述公式得：$C_b^p\left(0\right)=\left[\begin{array}{ccc}0& 0& \frac{1}{{\mathrm{g\omega }}_{ie}}\sec L\\ \frac{1}{g}\tan L& \frac{1}{{\omega }_{ie}}\sec L& 0\\ -\frac{1}{g}& 0& 0\end{array}\right]\left[\begin{array}{ccc}{g}_E^b& g_N^b& g_U^b\\ {\omega }_{ieE}^b& {\omega }_{ieN}^b& {\omega }_{ieU}^b\\ {\omega }_{ieU}^b{g}_N^b-{\omega }_{ieN}^b{g}_N^b& {\omega }_{ieE}^b{g}_U^b-{\omega }_{ieU}^b{g}_E^b& {\omega }_{ieN}^b{g}_E^b-{\omega }_{ieE}^b{g}_N^b\end{array}\right]---\left(4\right)$上式中的表示重力矢量在地理坐标系中三个方向的分量，用加速度计三个方向的输出替代；表示地球自转角速度矢量在地理坐标系中三个方向的分量，用陀螺三个方向的输出替代；起始点的地理纬度L、地球自转角速率ω_ie以及重力加速度g是已知参数；应用上述公式，能粗略的计算出的值；但是这里得到的并不是真的地理系即t系，而是一个与地理系有小角度差的平台坐标系即p系，因此将上述结果表示为再进行一次修正粗对准；经过解析粗对准之后得到的平台坐标系即p系与真实的地理坐标系即t系之间的误差角记为Φ；一次修正粗对准的目的就是利用陀螺仪和加速度计的信息求解误差角Φ以及误差角组成的反对称阵Φ^p，对粗对准结果进行进一步的修正得到考虑载体行进间的干扰以及惯性器件的误差，误差角Φ表达式写为：$\left\{\begin{array}{c}{\phi }_E^p=\frac{f_N^p}{g}+\frac{{▿}_N}{g}+\frac{a_{dN}}{g}\\ {\phi }_N^p=-\frac{f_E^p}{g}-\frac{{▿}_E}{g}-\frac{a_{dE}}{g}\\ {\phi }_U^p=\frac{{\omega }_{ibE}^p}{{\omega }_{ie}\cos L}+\frac{{ϵ}_E+{\omega }_{dE}}{{\omega }_{ie}\cos L}-\tan L\frac{{▿}_E+a_{dE}}{g}\end{array}\right.---\left(5\right)$其中表示加速度计水平方向的输出；是陀螺的东向输出；▽_E,▽_N代表加速度计水平方向的零点偏置；ε_E是陀螺仪东向的常值漂移；a_dE,a_dN是加速度计水平方向的测量误差；ω_dE代表陀螺仪东向的测量误差；g代表当地重力加速度；ω_ie是地球自转角速率；L代表当地的纬度；利用上述公式给出的误差角构造平台系到地理系的转换矩阵由于误差角为小量，因此简化为：$C_t^p=\left[\begin{array}{ccc}1& {\phi }_U^p& -{\phi }_N^p\\ -{\phi }_U^p& 1& {\phi }_E^p\\ {\phi }_N^p& -{\phi }_E^p& 1\end{array}\right]=(I-{\Phi }^p)---\left(6\right)$这样就能解析粗对准的结果进行修正：$C_b^n\left(0\right)=C_b^t\left(0\right)={\left(C_t^p\right)}^T{C}_b^p\left(0\right)---\left(7\right)$式中是粗对准解算的机体系即b系到平台系即p系的转换矩阵；是一次修正粗对准解算的地理系即t系到平台系即p系的转换矩阵；选择导航系即n系为地理坐标系即t系并且采用“东‑北‑天”即E‑N‑U标准，表示为那么捷联矩阵的初值就直接用上述结果表示；粗对准的特点是速度快、精度低，但是为之后的精对准提供一个满足要求的初始变换矩阵步骤二：载车启动之后，利用惯导和里程计输出的速度信息构造观测量；自抗扰控制器的输入是惯导和里程计的速度误差，因此在设计控制回路之前，必须要对观测量进行构造；载车启动之后，惯导根据加速度输出的比力信息来求解速度；首先将输出的比力转换到导航系中，再利用微分方程求得载体的加速度信息，最后经过积分后得到速度信息，记作Vⁿ；里程计安装在载车的车轮上，通过测量短时间内车轮转过的圈数得到载车的行驶速度信息，输出的速度信息是在测量坐标系即m系下的，记作里程计在安装过程中，安装角是已知的，利用安装信息构造里程计测量坐标系即m系到载车机体系即b系的转换矩阵再根据惯导解算的姿态矩阵将上述测量系下的速度转换到导航系，记作$V_D^n=C_b^n{C}_m^b{V}_D^m---\left(8\right)$由于里程计的测量误差非常小，因此认为近似表示为载车的理想速度，将二者速度之差视为观测量，表示为：$Y=\left[\begin{array}{c}{y}_1\\ y_2\end{array}\right]=\left[\begin{array}{c}{V}_E^n-V_{DE}^n\\ V_N^n-V_{DE}^n\end{array}\right]---\left(9\right)$其中是惯导解算的水平速度；代表里程计输出的水平速度；步骤三：利用观测量、惯导误差方程以及自抗扰控制方法对行进间精对准回路进行设计；精对准回路的设计以两个水平方向的速度误差为观测量，以惯导的误差方程为状态模型；略去系统中的两个水平回路交叉耦合项，原系统变成两个解耦的子系统，分别对两个子系统设计扩张状态观测器：a.东向扩张状态观测器ESO_E的设计由东向速度误差与北向失准角组成东向通道的一个子系统，忽略其他通道的影响，仅保留该子系统相耦合的变量，子系统1表示为：$\left\{\begin{array}{c}\delta {\stackrel{·}{V}}_E=\frac{V_N}{R}\tan {\mathrm{L\delta V}}_E-f_U{\phi }_N+{▿}_E+u_1\\ {\stackrel{·}{\phi }}_N=\frac{1}{R}{\mathrm{\delta V}}_E-{ϵ}_N+u_4\\ y_1={\mathrm{\delta V}}_E+v_1\end{array}\right.---\left(10\right)$式中δV_E是东向速度误差；V_N，L代表惯导时刻解算的北向速度和纬度；R是地球半径；f_U是加速度计天向输出；φ_N表示北向失准角；u₁、u₄代表反馈量；ε_N是陀螺仪的北向漂移；▽_E代表加速度计的东向零偏；y₁是子系统1的观测量；v₁是观测噪声；令x₁＝δV_E，第一个子系统经过变换写成：$\left\{\begin{array}{c}{\stackrel{·}{x}}_1=\frac{V_N}{R}\tan {\mathrm{Lx}}_1-f_U{x}_2+U_1\\ {\stackrel{·}{x}}_2=-\frac{1}{R}{x}_1+U_4\\ y_1=x_1+v_1\end{array}\right.---\left(11\right)$根据扩张状态观测器原理，子系统1的ESO_E设计为：$\left\{\begin{array}{c}{e}_{01}=z_1-y_1\\ {\stackrel{·}{z}}_1=\frac{V_N}{R}\tan L·z_1-f_U{z}_2-{\beta }_{11}·fal(e_{01},\alpha ,\delta )+U_1\\ {\stackrel{·}{z}}_2=-\frac{1}{R}{z}_1+z_3+\frac{{\beta }_{12}}{f_U}·fal(e_{01},\alpha /2,\delta )+U_4\\ {\stackrel{·}{z}}_3=\frac{{\beta }_{13}}{f_U}·fal(e_{01},\alpha /4,\delta )\end{array}\right.---\left(12\right)$其中y₁代表东向的速度误差观测值；z₁(t)和z₂(t)实时给出子系统的δV_E和的估计值；而z₃(t)则用于估计北向陀螺漂移ε_y以及所有不确定外扰动的总作用；e₀₁是实际观测值与估计值之差；fal幂次函数是一种非线性函数，α和δ是其参数；β₁₁,β₁₂,β₁₃是观测器的参数；将上述系统离散化之后，用于数值计算；U₁、U₄是经过变换之后的反馈量；b.北向扩张状态观测器ESO_N的设计由北向速度误差与东向失准角组成北向通道的一个子系统，忽略其他通道的影响，仅保留该子系统相耦合的变量，子系统2表示为：$\left\{\begin{array}{c}\delta {\stackrel{·}{V}}_N=f_U{\phi }_E-f_E{\phi }_U+{▿}_N+u_2\\ {\stackrel{·}{\phi }}_E=-\frac{1}{R}{\mathrm{\delta V}}_N-({\omega }_{ie}\cos L+\frac{V_E\tan L}{R}){\phi }_U-{ϵ}_E+u_3\\ {\stackrel{·}{\phi }}_U=({\omega }_{ie}\cos L+\frac{V_E}{R}){\phi }_E-{ϵ}_U+u_5\\ y_2={\mathrm{\delta V}}_N+v_2\end{array}\right.---\left(13\right)$式中δV_N是北向速度误差；V_E、L代表惯导时刻解算的东向速度和纬度；R是地球半径；f_E,f_U是加速度计东向和天向输出；φ_E,φ_U表示东向和天向失准角；u₂,u₃,u₅代表反馈量；ε_E,ε_U是陀螺仪的东向和天向漂移；▽_N代表加速度计的北向零偏；y₂是子系统2的观测量；v₂是观测噪声；在子系统2的模型中，令x₁＝δV_N，变换为：$\left\{\begin{array}{c}{\stackrel{·}{x}}_1=f_U{x}_2+U_2\\ {\stackrel{·}{x}}_2=-\frac{1}{R}{x}_1-({\omega }_{ie}\cos L+\frac{V_E\tan L}{R})x_3+U_3\\ {\stackrel{·}{x}}_3=({\omega }_{ie}\cos L+\frac{V_E}{R})x_2+U_5\end{array}\right.---\left(14\right)$利用上述系统设计扩张状态观测器ESO_N：$\left\{\begin{array}{c}{e}_{02}={\theta }_1-y_2\\ {\stackrel{·}{\theta }}_1=f_U{\theta }_2-{\beta }_{01}·fal(e_{02},\alpha ,\delta )+U_2\\ {\stackrel{·}{\theta }}_2=-\frac{1}{R}{\theta }_1-({\omega }_{ie}\cos L+\stackrel{·}{\lambda }){\theta }_3-{\beta }_{02}·fal(e_{02},\alpha /2,\delta )+U_3\\ {\stackrel{·}{\theta }}_3={\theta }_4-{\beta }_{03}·fal(e_{02},\alpha /4,\delta )+U_5\\ {\stackrel{·}{\theta }}_4=-{\beta }_{04}·fal(e_{02},\alpha /8,\delta )\end{array}\right.---\left(15\right)$其中y₂代表北向的速度误差观测；是惯导解算的经度变化率；θ₁(t)，θ₂(t)和θ₃(t)分别是第二个子系统中的δV_N，的估计值；而θ₄(t)则用于估计东向陀螺漂移ε_x以及所有不确定外扰动的总作用；e₀₂是实际观测值与估计值之差；α和δ是fal幂次函数的参数；β₀₁,β₀₂,β₀₃,β₀₄是观测器的参数；U₂、U₃、U₅是经过变换之后的反馈量；c.非线性状态误差反馈设计通过上述分析，得到了扩张状态观测器的模型，为了构建闭合回路，需要设计反馈控制律；自抗扰控制器的特点是不直接利用状态误差，而是对误差进行非线性组合，提高对误差的利用效率，克服系统快速性与超调之间的矛盾；非线性组合方法一般按照非线性函数fal函数以及误差方程来列写；反馈控制律表示为：$\left\{\begin{array}{c}{u}_1\left(k\right)=-{\beta }_1·fal\left(z_1\right(k),{\alpha }_1,{\delta }_0)-\frac{V_N}{R}{\mathrm{tanLz}}_1\left(k\right)-(2{\omega }_{ie}\sin L+\frac{V_E}{R})·{\theta }_1\left(K\right)-f_N{\theta }_3\left(k\right)+f_U{z}_2\left(k\right)\\ u_2\left(k\right)=-{\beta }_2·fal\left({\theta }_1\right(k),{\alpha }_2,{\delta }_0)+(2{\omega }_{ie}\sin L+\frac{2V_E}{R}\tan L)·z_1\left(k\right)-f_U{\theta }_2\left(k\right)+f_E{\theta }_3\left(k\right)\\ u_3\left(k\right)=-{\beta }_3·fal\left({\theta }_2\right(k),{\alpha }_3,{\delta }_0)-\frac{1}{R}{\theta }_1\left(k\right)-({\omega }_{ie}\sin L+\frac{V_E}{R}\tan L)·z_2\left(k\right)+(2{\omega }_{ie}\cos L+\frac{V_E}{R}){\theta }_3\left(k\right)\\ u_4\left(k\right)=-{\beta }_4·fal\left(z_2\right(k),{\alpha }_4,{\delta }_0)-\frac{1}{R}{z}_1\left(k\right)+({\omega }_{ie}\sin L+\frac{V_E}{R}\tan L)·{\theta }_2\left(k\right)-z_3\left(k\right)\\ u_5\left(k\right)=-{\beta }_5·fal\left({\theta }_3\right(k),{\alpha }_5,{\delta }_0)-\frac{1}{R}{\mathrm{tanLz}}_1\left(k\right)-({\omega }_{ie}\cos L+\frac{V_E}{R})·{\theta }_2\left(k\right)-\frac{V_N}{R}{z}_2\left(k\right)-{\theta }_4\left(k\right)\end{array}\right.---\left(16\right)$其中β₁,β₂,β₃,β₄,β₅表示待定参数，其他参数在上述步骤中已经定义；这样，就建立了基于自抗扰控制器的对准回路，根据实时测得的速度误差以及惯导给出的比力和解算的速度信息，对三个变量的值进行估计：这三个值并不是准确的三个方向的失准角，而是带有误差的估计值；仔细分析不难发现，失准角的估计误差与卡尔曼滤波是一致的；步骤四：自抗扰控制器参数的整定；通过上述分析看出，自抗扰控制器的参数较多，并且这些参数对控制器的性能有着关键的影响，因此必须对参数进行整定；根据分离定理，自抗扰控制器三大部分的参数是独立设置的；扩张状态观测器是自抗扰技术中最为关键的一个部分，首先对其参数的整定方法进行说明；以子系统1的扩张状态观测器为例，式(12)中需要整定的参数主要包括fal函数中的参数α和δ，以及三个参数β₁₁、β₁₂和β₁₃；α主要决定非线性形状，δ决定函数线性区间的大小；一般情况下，α和δ按照经验值可以选取为α＝1、δ＝0.5；β₁₁、β₁₂和β₁₃三个参数是影响闭环系统的动态特性的主要参数，需要根据对象的特征以及实际的观测信息来调整；首先要确保能精确跟踪观测对象的状态，β₁₁主要控制跟踪速度，β₁₂代表增益，β₁₃控制曲线平稳度；首先调整β₁₁使得z₁(t)能较快的平稳跟踪y₁(t)，再根据估计值调整β₁₂控制z₂(t)的大小；最后根据总体的控制效果进一步调整，适当调小β₁₃可是使振荡较大的输出曲线变得平稳；也按照一些工程上给出的经验公式，将参数取成：${\beta }_{11}=\frac{1}{h},{\beta }_{12}=\frac{1}{3h^2},{\beta }_{13}=\frac{2}{8^2{h}^3}$式中h代表仿真的采样时间间隔；非线性状态误差反馈作用是产生一个非线性控制律u(t)，需要设计的参数包括fal函数中的参数α和δ以及β₁,β₂,β₃,β₄,β₅；其中α和δ在上述内容中已经分析，这里不再赘述；而β₁,β₂,β₃,β₄,β₅中分开整定，其中β₁和β₄控制子系统1，而β₂、β₃和β₅控制的是子系统2；以子系统1为例，参数β₁和β₄的物理意义与PID控制器中微分系数和增益系数相似；当β₁较大时，调节速度变慢，但是曲线平稳；反之β₁较小时，调节速度变快，但是曲线的振荡变大，甚至不稳定；β₄相当于一个增益，调节增益大小能控制反馈律，尽量使得估计状态与期望值靠近；在调整过成中，一般先整定β₁，使得估计在速度和平稳度上比较优良，再调整β₄使结果与预期相符合；步骤五：通过观测器输出的失准角，构造反馈矩阵实现车载惯导行进间的初始对准；惯导在行进过程中利用惯性测量组件IMU的数据进行导航解算，给出载车的速度、位置和姿态信息；但是惯导解算的捷联矩阵不是准确的机体系到导航系的转换矩阵，而是与真实导航系有着偏差的n₁系之间的转换矩阵n₁系与n系之间的误差角正是待求解的失准角；在上述步骤中，观测器可以给出三个失准角的估计值为：$\left\{\begin{array}{c}{\hat{\phi }}_N={\phi }_N-\frac{{▿}_E}{f_U}\\ {\hat{\phi }}_E={\phi }_E+\frac{{▿}_N}{f_U}\\ {\hat{\phi }}_U={\phi }_U-\frac{{ϵ}_E}{{\omega }_{ie}\cos L+\stackrel{·}{\lambda }}\end{array}\right.---\left(17\right)$其中φ_N,φ_E,φ_U是真实的失准角；是自抗扰控制器估计的失准角；▽_E,▽_N代表加速度计水平方向的零点偏置；f_U是加速度计天向输出；是惯导解算的经度变化率；ω_ie是地球自转角速率；L代表当地的纬度；ε_E是陀螺仪东向漂移；因为三个失准角是小角度，因此能构造反馈矩阵来对修正捷联矩阵：$C_n^{n1}=\left[\begin{array}{ccc}1& -{\hat{\phi }}_U& {\hat{\phi }}_N\\ {\hat{\phi }}_U& 1& -{\hat{\phi }}_E\\ -{\hat{\phi }}_N& {\hat{\phi }}_E& 1\end{array}\right]---\left(18\right)$$C_b^n={\left(C_n^{n1}\right)}^T{C}_b^{n1}---\left(19\right)$这样惯导就得到了校正，平台系和导航系将逐步重合，就实现了行进间对准的目的；通过上述五个步骤，分别对载车发动之前的粗对准方法、观测量构造、自抗扰控制回路设计方法进行了详细说明；本发明提出的方法不需要已知噪声的统计特性、也不需要非常精确的模型就能解决惯导在车载环境下的快速对准问题以及车载过程中外部扰动的补偿问题。