专利名称：非完全自由度惯性平台关键参数多位置加权自主检测方法
技术领域：
本发明涉及一种实现非完全自由度惯性平台的在线自检测的方法，适用于各类非完全和完全自由度的惯性平台系统自检测，属于参数在线自主检测技术领域。
背景技术：
非完全自由度惯性平台系统是复杂的高精度机电综合系统，由于具有完全自主性的优点而广泛应用于陆海空天领域。为了保证非完全自由度惯性平台系统能够正常稳定的工作，常常需要定期对该平台系统的陀螺的各种零漂、加速度计零偏和刻度因数等关键参数进行检测并适当补偿。目前已有的检测方法是将非完全自由度惯性平台系统从载体中取下，放在高精度的平台检测系统中进行关键参数的检测。待所有参数被记录之后，再将该平台系统装回载·体。这种方法的缺点是在实际应用中，考虑到载体的结构和安装空间等方面的因素，非完全自由度惯性平台系统的安装往往很复杂，这样为了保证系统的可靠和稳定的工作，需要定期将非完全自由度惯性平台系统从载体中取出。因此，这种方法是十分的不方便的。并且，这种方法不能够提供实时的检测数据。
发明内容
本发明的目的是为了克服已有技术存在的不足，提供一种在线自主检测的方法。本方案的基本原理是在不借助外加其他设备和不拆下平台系统的前提下，利用平台处于不同姿态时加速度计和陀螺的输出并结合加速度计和陀螺的输出模型，运用最小二乘法对平台的关键参数进行实时自检测。由于本方案中参数解算方法主要运用最小二乘的方法，计算速度十分迅速的。因此，在实时性要求较高系统中，采用本发明方法能够快速地进行平台关键参数的自主检测。与传统方法相比，本发明方法不仅省去了惯性平台系统拆装的不便，而且能够提供实时的系统检测数据，为惯性平台系统的稳定工作提供了更有利的保障。本发明的目的是通过下述技术方案实现的。实现非完全自由度惯性平台的在线自检测的方法，包括如下步骤步骤一、在东北天地理坐标系下，将X轴和Z轴加速度计为零且Z陀螺输出最小时定为初始位置，设此时平台坐标系为由地理坐标系依次绕北向轴、方位轴、东向轴旋转Cl 0,βο和Y O得到。则初始转移矩阵C“为
cos/0 sin/0 O COSy^0 O - sin β0 I OOC^0= —sin/0 cos/0 0 0 1 0 0 cos a0 Sina0
00 I sin β0 0 COSy^0 0 -Sina0 Cosa0其中α。，β。，Y。皆为小角度，对上式进行线性化后得I Yo ~βοCpn0 = -γ0 I α0
_ Po ~αο I _
'O '
· = O_-g-_ Γ gP0
= CpnJ^n = -ga0
_ s _
旳=M3OOM2(ZJ)M1 ⑷巧。
gv^oCOS/cosy^-gadsin/cosa + cos/siny^sinctO-gCsin/sina-cos/siny^cosa)
=-gP0 sin γ cos β - ga0 (cos γ cos a - sin 7 sin β sin a) - g(cos 7 sin a + sin γ sin β cos a) g>50sin々 + ga0 cos β ηα-g cos β cos a岭表示在重力加速度东北天地理坐标系三轴上的分布。表示重力加速度在平台坐标系X，Y，Z三轴上的分布。^表示在平台坐标系在分别绕X，Y，Z三轴旋转α，β，Y后重力加速度在三轴上的分布。地球自转角速度在地理坐标系中表示为CDe = mey ，其中ωεχ = 0，ω^, ω ez分别表示地球自转角速度在地理坐标系三
-① ez_
轴上的分布。则地速在初始平台坐标系中表示为ωβρ0 = % + eza0
_ ~ωβγα0 + ωβζ _在平台依次绕X轴，Y轴和Z轴分别转过a，β和Υ后有地速在平台坐标系中的表示为OJep = M3 (Y)M2 (β )MX (a)iyep。
(ωψχ0 -ω^φοχχοχ βΜωψ +^^Xsm^aBa+aB^sin^sinQr)+(--<2^ql +^Xsin^ina-ocs^sinyfecsQr)
={ωψγ0 -ω^β^^ γ(ΧΒβ)+{ωψ +ω^ο^μ&γο^α-^γ β η^Μτ^ψ^ +^XaB^sina+sin^sinyfecsa)
_~ο )^ Η%Υ -^4X-ocsAsin )+(-^^ +ω^βχ&α_
步骤二、在平台坐标系下建立加速度计和陀螺的输出模型。X加速度计线性输出模型为Zax = g β Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin a ) -g (sin Y sin a -cos
Ysin β cos a ) +ka0x_kalxg (sin γ sin a -cos γ sin β cos a ) - Θ pxg (cos γ sin a +sin γ sin βcos a ) + Θ oxgcos β cos a +eax
(I. DY加速度计线性输出模型为 Zay = -g β 0sin Y cos β -g a 0 (cos Y cos a -sin Y sin β sin a ) -g (cos Y sin a +sin
Ysin β cos a ) +ka0y_kalyg (cos γ sin a +sin γ sin β cos a ) - Θ pygcos β cos a + Θ oyg (sin γ sin a -cos γ sin β cos a ) +eay
(1.2)Z加速度计线性输出模型为Zax = g β 0sin β +g a Ocos β sin α -geos β cos α +ka0z-kalzgcos β cos α - θ pzg(sin Y sin α -cos Y sin β cos α )+ Θ ozg(cos Y sin a +sin
Ysin β cos a ) +eaz
(1.3)Zax, Zay, Zaz分别表示X，Y，Z轴上加速度计的输出值。kaQx，ka0y ka0z分别表示各个轴上加速度计的零偏。kalx，kaly，kalz分别表示各个轴的刻度因数误差。θρχ，θοχ θργ，θογ θρζ，θ μ分别表示平台各轴的安装误差。eax，eay，eaz表示随机误差。平台X陀螺仪输出模型为Zgx = kg0x+kgllx (g β Ocos Y cos β -g a 0 (sin γ cos a +cos γ sin β sin α ) -g (sin γ siη a -cos Y sin β cos α ))+kgl2x (-g β 0sin Y cos β -g a 0 (cos Y cos a -sin Y sin β sin α ) -g (cos Y sin a +siη γ sin β cos α ))+kgl3x (g β 0sin β +g a Ocos β sin α -geos β cos α )+ ( ω ey γ 0- ω ez β 0) cos Y cos β + ( ω ey+ ω ez α 0) (sin Y cos α +cos Y sin β sin α )+ (- ω ey α 0+ ω ez) (sin Y sin α -cos Y sin β cos α ) + ε gx(I. 4)平台Y陀螺仪输出模型为Zgy = kg0y+kglly (-g β 0sin Y cos β -g a 0 (cos Y cos a -sin Y sin β sin α ) -g (cos Y sin a +sin γ sin β cos α ))+kgl2y (g β Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin a ) -g (sin Y sin a -cos
Ysin β cos a ))-kgl3y (g β 0sin β +g a Ocos β sin a -geos β cos a )+ (ω ey γ 0- ω ez β 0) (-sin Y cos β ) + (ω ey+ ω ez a 0) (cos Y cos a -sin Y sin β sin a )+ (- ω ey a 0+ ω ez) (cos Y sin a +sin Y sin β cos a ) + ε gy
(I. 5)平台Z陀螺仪输出模型为 Zgz = kg0z+kgllz (g β 0sin β +g α Ocos β sin α -geos β cos α )+kgl2z (-g β 0sin Y cos β -g α 0 (cos Y cos α -sin Y sin β sin α ) -g (cos Y sin a +siη γ sin β cos α ))+kgl3z (g β Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin a ) -g (sin Y sin a -cos
Ysin β cos a ))+ ( ω ey γ 0- ω ez β 0) sin β + ( ω ey γ 0- ω ez β 0) (-cos β sin a ) + (- ω ey a。+ ω ez)cos β cos a + ε p7
(I. 6)式中，Zgx, Zgy, Zgz为平台系统X，Y，Z三轴上陀螺仪的输出。kgQx，kg0y, kg0z为陀螺仪的零漂。kgllx，
kgi2x，kgi3Skglly kgl2y，kgl3ykgllz，kgl2z，kgl3z 为平台系统X，Y，z三轴上陀螺仪的一次项系数。egx，egy, Sgz为陀螺仪随机漂移误差。ωΜ，(0^分别表示地球自转角速度在地理坐标系三轴上的分布。步骤三、采集非完全自由度惯性平台系统X，Y，Z三轴上的加速度计和陀螺仪的输出，并对关键参数进行自检测。根据步骤二已建立的加速度计和陀螺仪的输出模型，可以知道在平台多位置翻滚检测中，不同的位置，陀螺仪和加速度计各轴的激励都不相同，待检测参数的可观测性也不相同。因此需要将平台旋转到不同的位置采集陀螺仪和加速度计的输出。根据待测参数的数量，制定本发明方法的位置方案为将平台绕着X轴分别旋转三个小于60°的角度αχ1，a χ2, a χ3，然后绕着Y轴旋转三个小于60°的角度Pyl, β y2,^y3，最后将平台绕Z轴旋转三个小180°的角度Yzl，Yz2, Yz30平台在上述的9个位置上分别停留一段时间采集并记录平台系统X，Y，Z三轴上加速度计和陀螺的输出值。这样在9个位置就可以得到27个加速度计的输出值和27个陀螺仪输出值。将这些数据分别代入加速度计和陀螺仪的输出模型中即(1.1) (I. 6)，就可以得到27加速度计的输出方程和27陀螺仪的输出方程。对27加速度计的输出方程运用最小二乘法解算，便可得到加速度计需要自检测的参数。同理，对27陀螺仪的输出方程运用最小二乘法解算，便可得到陀螺仪需要自检测的参数。为了追求更高的参数自检测精度，可以增加更多的旋转位置。因此，位置方案不限于上述的9个位置，可以适当的增加位置。有益效果与已有的非完全自由度惯性平台系统检测方法比较，本发明方法大大减小拆装系统的不便，而且利用本方法可以对系统的关键参数进行实时自检测和反馈，有利于对系统进行及时的补偿平台系统的工作的精度和稳定性。另外，在参数解算过程中，传统的解算方法通常忽略将初始平台坐标系与地理坐标系之间的欧拉角Qci, β0, y0O这样就会影响其他关键参数的解算精度。本发明方法在对系统的关键参数进行实时自检测和反馈的同时，可以实现对平台系统误差角的实时估计，与传统的解算方法本发明方法提高了其他参数的解算精度。
图I.表示本发明的具体实施例中进行自主检测的三轴稳定平台结构示意图。图中各标号表不:
I.外框架轴(自由度-60° 60° )2.内框架轴(自由度-60° 60° )3.方位轴(全自由度)图2.表示本发明的具体实施例中进行自主检测的三轴稳定平台平台坐标系和框架坐标系的示意图。图3.表示本发明的具体实施例中平台自主检测中的位置方案示意图。
具体实施例方式本实施例中，结合实际惯性平台系统进行关键参数的自检测。平台系统中，三个加速度计的随机漂移均为IX 10_6m/s2，三个陀螺仪的随机漂移均为0.0001° /h。在实施过程中，平台绕X轴旋转的角度αχ1，αχ2，αχ3不失一般性取20。，40° ,60°，。同理，i3yl，3y2，Py3 不失一般性取 20°，40° ,60°，λζ1，yz2, Yz3 不失一般性取 45°，90° ,135°。其过程如下步骤一、在东北天地理坐标系下，将X轴和Z轴加速度计为零且Z陀螺输出最小时定为初始位置，设此时平台坐标系为由地理坐标系依次绕北向轴、方位轴、东向轴旋转Cl 0,β。和Y O得到。则地理坐标系到平台坐标系的初始转移矩阵C。为
cos/0 sin/0 O COSy^0 O - sin β0 I OO—sin/。cos/0 0 0 1 0 0 cos a0 Sina0
00 I sin β0 0 COSy^0 0 -Sina0 Cosa0其中α。，β。，Y。皆为小角度，对上式进行线性化后得
IYo ~βοCpn0= -r0 I a0
_ Po ~ao I _
'0 '
· = 0 _-g-_
Γ gP0
pQ=C, = -gcx0
_ s _g/^0cos^cos/^-g6tr0(sin^cos6tr + cos^sin/^sin6tr)-g(sin^sin6tr-cos^sin/^cos6tr)
=-g β0 sin γ cos β - ga0 (cos γ cos 6tr-sin ^sin β sin a) - g(cos ^sina + sin ^sin fico^a) gβ0 sin β + ga0 cos /^sina-geos/^cosa地球自转角速度在地理坐标系中表示为Oe = Oey ，其中 GJex = O,
则地速在初始平台坐标系中表示为
(^eyYo - (°eA= % + ωΒζα0
_-^ 0+^ζ_在平台依次绕X轴，Y轴和Z轴分别转过α，β和Υ后有地速在平台坐标系中的表示为o)ep = M3 (γ)Μ2 (P)M1 {α)ω ρ0
(ω^γ0 -ωεζφαΒ/(ΧΒβ+(ω^ +ω^χ αγο^α+ο^γ ηβ^ +^^ +ω^( αγ αα-οβ/ αβοβ )= (ω^χ0 -ω^β)(- ηχ(ΧΒβ-\-(ω^ +^c^XaB/oc6a-sin/sin>toa)+(-^cif) ^ω^(οχχ ηα-^ηχ ηβχΒ(
_(%r0 ~^Λ)^β<%Χο+^)oos^ocsa_步骤二、建立加速度计和陀螺的输出模型。在东北天的坐标系下，加速度计的输出模型表示如公式(I. 7)
Zal =ka01g0+(1 + kal1)^I1+eal
(1.7)其中巧为加速度计I轴的载体加速度，Zai为平台第i轴上加速度计的输出，ka(li为常值误差，kali为加速度计一次项系数，eai为测量随机误差。因此，平台X轴加速度计的输出模型为Zm=UlgQ+(l + U崎;平台X轴加速度计的输出模型为A=LA +(1 + U畸;平台X轴加速度计的输出I旲型为'Zaz= ka0zg0 + (I + D碑+eaz。陀螺仪的输出模型表示如公式(I. 8) (I. 9) (I. 10)平台X陀螺仪输出模型为
= kg0x + kg\\ J^px + kguJ^py + kguJ^pz + ω epx + Sgx
(1.8)平台Y陀螺仪输出模型
Zgv = kSOy +kgUy^y + kglly^p, ~ kgUy^pz + ωepy + 8gy
(1.9)平台Z陀螺仪输出模型为Zgz = kg0z +kgUzJ^pz +kgUzJ^px+mepz +Sgz
(1.10)式中，Zgx，Zgy, Zgz为平台系统X，Y, Z三轴上陀螺仪的输出。kgQx，kg0y, kg0z为陀螺仪的零漂。 kgiix，kgi2X，kgl3xkglly，kgl2y，kgl3ykgllz，kgl2z，kgl3z 为平台系统X，Y，z三轴上陀螺仪的一次项系数。egx，egy, Sgz为陀螺仪随机漂移误差。将步骤一中得到的@矩阵带入加速度计输出模型(I. 7)式中得X加速度计线性输出模型为 Zax = gi3 Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin α ) -g (sin Y sin a -cos
Ysin β cos a ) +ka0x_kalxg (sin γ sin a -cos γ sin β cos a ) - Θ pxg (cos γ sin a +sin γ sin βcos a ) + Θ oxgcos β cos a +eax(I. 11)Y加速度计线性输出模型为Zay = -g β 0sin Y cos β -g a 0 (cos Y cos a -sin Y sin β sin a ) -g (cos Y sin a +sin
Ysin β cos a ) +ka0y_kalyg (cos γ sin a +sin γ sin β cos a ) - Θ pygcos β cos a + Θ oyg (sin γ sin a -cos γ sin β cos a ) +eay(I. 12)Z加速度计线性输出模型为Zaz = g β 0sin β +g a Ocos β sin α -geos β cos α +ka0z-kalzgcos β cos α - θ pzg(sin Y sin α -cos Y sin β cos α )+ Θ ozg(cos Y sin a +sin
Ysin β cos a ) +eaz(I. 13)将步骤一中ω6ρ带入(1.8) (1.9) (I. 10)式中得平台X陀螺仪输出模型为Zgx = kg0x+kgllx (g β Ocos Y cos β -g a 0 (sin γ cos a +cos γ sin β sin α ) -g(sin γ siη a -cos Y sin β cos α ))+kgl2x (-g β 0sin Y cos β -g a 0 (cos Y cos a -sin Y sin β sin α ) -g (cos Y sin a +siη γ sin β cos α ))+kgl3x (g β 0sin β +g a Ocos β sin α -geos β cos α )+ ( ω ey γ 0- ω ez β 0) cos Y cos β + ( ω ey+ ω ez α 0) (sin Y cos α +cos Y sin β sin α )+ (- ω ey α 0+ ω ez) (sin Y sin α -cos Y sin β cos α ) + ε ex(I. 14)平台Y陀螺仪输出模型为Zgy = kg0y+kglly (~g& 0sin Y cos β -g a 0(cos Y cos a -sin Y sin β sin a )-g(cos Y sin a +sin γ sin β cos α ))+kgl2y (g β Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin a ) -g (sin Y sin a -cos
Ysin β cos a ))-kgl3y (g β 0sin β +g a Ocos β sin a -geos β cos a )+ (ω ey γ 0- ω ez β 0) (-sin Y cos β ) + (ω ey+ω ez a 0) (cos Y cos a -sin Y sin β sin a )+ (- ω ey a 0+ ω ez) (cos Y sin a +sin Y sin β cos a ) + ε gy
(I. 15)平台Z陀螺仪输出模型为Zgz = kg0z+kgllz (g β Qsin β +g α Ocos β sin α -geos β cos α )+kgl2z (-g β 0sin Y cos β -g α 0 (cos Y cos α -sin Y sin β sin α ) -g (cos Y sin a +siη γ sin β cos α ))+kgl3z (g β Ocos Y cos β -g a 0 (sin Y cos a +cos Y sin β sin a ) -g (sin Y sin a -cosY sin β cos a ))+ ( ω ey γ 0- ω ez β 0) sin β + ( ω ey γ 0- ω ez β 0) (-cos β sin a ) + (- ω ey a。+ ω ez)cos β cos a + ε p7(I. 16)·步骤三、采集非完全自由度惯性平台系统X，Y，Z三轴上的加速度计和陀螺仪的输出并对关键参数进行自检测。根据步骤二中已建立的加速度计和陀螺仪的输出模型，可以知道在平台多位置翻滚检测中，不同的位置，陀螺仪和加速度计各轴上的激励都不相同，待检测参数的可观测性也不相同。因此需要将平台旋转到不同的位置采集陀螺仪和加速度计的输出。确定加速度计需要自检测的参数α ο，β ο，kaQx，kalx，θ οχ, θ ρχ, ka0y, kaly, θ oy, θ py, ka0z, kalz, θ oz, Θ pz 共14个参数。确定陀螺仪需要检测的参数
Y ο kg0x，kgllx，kgl2x，kgl3x，kg0y, kglly, kgl2y, kgl3y, kg0z,kgllz, kgl2z，kgl3z，共13个参数。具体操作将平台绕着X轴分别旋转三个小于60°的角度αχ1，αχ2，％3如20°，40° ,60°，然后绕着Y轴旋转三个小于60°的角度β yl，β y2，β y3如20。，40。，60。，最后将平台绕Z轴旋转三个小180°的角度Yzl，yz2, Yz3如45°，90° ,135°。为了保证数据的准确和稳定性，平台在上述每个位置都停留约3分钟进行采集并记录平台系统X，Y，Z三轴上加速度计和陀螺的输出值。这样在每个位置上就可以得到3组加速度计输出值和3组陀螺仪输出值，对每组数据求算术平均，得到的6个平均值(Zax，Zay, Zaz, Zgx, Zgy, Zgz)作为系统在该位置的加速度计和陀螺输出值。这样在9个位置就可以得到27个加速度计的输出值和27个陀螺仪输出值。将27个加速度计的输出、αχ1，α χ2,α χ3> Pyi， ^y2^ βγ3> Li，Υζ2 Yz3 和 g 代入到加速度计的输出模型(I. 8) (I. 9) (I. 10)中就会得到27个非线性方程，即可解算出14个待定参数，可以运用最小二乘法求取。同理，将27个陀螺仪输出值、αχ1，αχ2，α x3、^yl, β y2，β y3、γ ζ1, γ ζ2, Y ζ3,由加速度计解算方程求出的α μ 、地球自转角速度在各轴上的分量^和g代入(I. 14) (I. 15) (I. 16)式中，可得到27个非线性方程，运用最小二乘法便可解算出13个待定参数。为了说明本发明方法的效果，这里给出所有待检测参数的自检测结果，如下表表I.加速度计相关参数自检测结果
权利要求
1.非完全自由度惯性平台关键参数多位置加权自主检测方法，其特征在于包括如下步骤 步骤一、在东北天地理坐标系下，将X轴和Z轴加速度计为零且Z陀螺输出最小时定为初始位置，设此时平台坐标系为由地理坐标系依次绕北向轴、方位轴、东向轴旋转Cttl, β ^和Y C1得到；则初始转移矩阵 τ。为
2.根据权利要求I所述的非完全自由度惯性平台关键参数多位置加权自主检测方法，其特征在于步骤三所述位置方案为将平台绕着X轴分别旋转三个小于60°的角度aXl，° Χ2 α Χ3，然后绕着Y轴旋转三个小于60°的角度Pyl, ^y2, Py3,最后将平台绕Z轴旋转三个小180°的角度Y ζ1，Y ζ2，Y ζ3 ;平台在上述的9个位置上分别停留一段时间采集并记录平台系统X，Y，Z三轴上加速度计和陀螺的输出值；这样在9个位置就可以得到27个加速度计的输出值和27个陀螺仪输出值；将这些数据分别代入加速度计和陀螺仪的输出模型中即(0. I) (0. 6)，就可以得到27加速度计的输出方程和27陀螺仪的输出方程；对27加速度计的输出方程运用最小二乘法解算，便可得到加速度计需要自检测的参数； 同理，对27陀螺仪的输出方程运用最小二乘法解算，便可得到陀螺仪需要自检测的参数。
3.根据权利要求I所述的非完全自由度惯性平台关键参数多位置加权自主检测方法，其特征在于步骤三所述位置方案不限于上述的9个位置；增加位置能够实现更高的参数自检测精度。
全文摘要
本发明涉及一种实现非完全自由度惯性平台的在线自检测的方法，适用于各类非完全和完全自由度的惯性平台系统自检测，属于参数在线自主检测技术领域。在不借助外加其他设备和不拆下平台系统的前提下，利用平台处于不同姿态时加速度计和陀螺的输出并结合加速度计和陀螺的输出模型，运用最小二乘法对平台的关键参数进行实时自检测，计算速度十分迅速。因此，在实时性要求较高系统中，采用本发明方法能够快速地进行平台关键参数的自主检测。与传统方法相比，本发明方法不仅省去了惯性平台系统拆装的不便，而且能够提供实时的系统检测数据，为惯性平台系统的稳定工作提供了更有利的保障。
文档编号G01C25/00GK102914318SQ201110439180
公开日2013年2月6日 申请日期2011年12月23日 优先权日2011年12月23日
发明者付梦印, 王博, 汪顺亭, 邓志红, 丰璐 申请人:北京理工大学