CN1740746B

CN1740746B - Micro-miniature dynamic carrier attitude measuring device and its measuring method

Info

Publication number: CN1740746B
Application number: CN 200510011763
Authority: CN
Inventors: 熊沈蜀; 周兆英; 王立代; 魏强
Original assignee: Tsinghua University
Current assignee: Tsinghua University
Priority date: 2005-05-23
Filing date: 2005-05-23
Publication date: 2010-08-04
Anticipated expiration: 2025-05-23
Also published as: CN1740746A

Abstract

A micro-miniature dynamic carrier attitude measurement device, which belongs to the field of signal measurement and navigation technology, is characterized in that it is small in size, light in weight, suitable for high dynamic environments, and can be used for attitude measurement of aircraft, vehicles, ships or other carriers; the device includes 3-axis rate gyroscope, 3-axis magnetometer, uniaxial accelerometer, temperature sensor, analog-to-digital conversion circuit, microprocessor and memory, serial communication interface, and corresponding software measurement methods; 3-axis rate gyroscope and other output connections for analog-to-digital conversion The input end of the circuit, the control signal and the data signal of the analog-to-digital conversion circuit are respectively connected with the corresponding ends of the microprocessor and the memory; the output ends of the microprocessor and the memory are connected with the serial port communication unit. Connect the measuring device to the aircraft, initialize, collect sensor signals, compensate, calibrate, update state variables, calculate and output the current attitude angle. The invention has low requirements on sensor performance and high accuracy of attitude measurement, and is especially suitable for dynamic attitude measurement.

Description

Micro-miniature dynamic carrier attitude measuring device and its measuring method

技术领域technical field

本发明涉及一种微小型载体姿态测量装置，特别适用于动态载体姿态测量，属于信号处理、测量、导航领域。The invention relates to a miniature carrier attitude measurement device, which is especially suitable for dynamic carrier attitude measurement and belongs to the fields of signal processing, measurement and navigation.

本发明还涉及根据此装置的一种测量方法。The invention also relates to a measuring method according to the device.

背景技术Background technique

姿态信息的测量对于飞机、车辆、船舶等运动载体具有十分重要的意义。传统方法中，人们用陀螺仪测量运动载体的姿态角。好的陀螺仪具有相当高的精度，但同时它也存在价格昂贵、结构复杂、体积大、质量重等缺点，这使它的应用受到了很大限制。速率陀螺价格较低、体积小、质量轻，但它的精度较低，漂移严重，因此不能直接用来测量姿态角。The measurement of attitude information is very important for moving carriers such as aircraft, vehicles, and ships. In traditional methods, people use gyroscopes to measure the attitude angle of the moving carrier. A good gyroscope has quite high precision, but at the same time it also has disadvantages such as expensive, complex structure, large volume, and heavy quality, which greatly limits its application. The rate gyro is relatively cheap, small in size and light in weight, but its accuracy is low and its drift is serious, so it cannot be directly used to measure the attitude angle.

利用加速度传感器和地磁传感器测量重力加速度和地磁场的3轴分量也可以解算出载体的姿态角(如美国专利20020188416)。这类传感器的体积能够做到很小，但由于加速度传感器同时也敏感运动加速度，因此容易受到运动加速度干扰，不能用在非平衡状态下。The attitude angle of the carrier can also be calculated by using the acceleration sensor and the geomagnetic sensor to measure the acceleration of gravity and the 3-axis component of the geomagnetic field (such as US Patent 20020188416). The volume of this type of sensor can be made very small, but because the acceleration sensor is also sensitive to motion acceleration, it is easily disturbed by motion acceleration and cannot be used in an unbalanced state.

另一种方法是在加速度和地磁传感器的基础上，加入速率陀螺，构造基于四元数的卡尔曼滤波器，对姿态角进行实时估计(如已授权的美国专利6647352)。此方法利用3轴速率陀螺输出更新状态量，采用3轴加速度和3轴磁场计的测量值作为观测值，在最小二乘意义下得到状态变量的最优估计，进而得到姿态角的估计值。此方法的优点在于能够较好的滤除传感器信号的随机干扰，补偿陀螺的漂移，有效地提高了精度。但它要求在长时间内重力加速度的测量值是无偏的，也就是说，当载体处于长时间处于加速(或者减速、转弯)状态时，此方法的精度将严重降低，甚至不可用。为了解决这一问题，人们提出了根据不同状态自动调整滤波器噪声的方法。这种方法在长时间处于加、减速或者转弯状态时，增加对陀螺信号的依赖程度。其前提是陀螺信号在这一段时间内漂移没有明显变化，这对陀螺的性能提出了较高的要求，但使用高精度陀螺，会增加系统的价格和重量，而且随着时间的增长，误差会不断累积，因此仍然不能解决这一问题。Another method is to add a rate gyro on the basis of acceleration and geomagnetic sensors, construct a Kalman filter based on quaternion, and estimate the attitude angle in real time (as authorized US Patent No. 6,647,352). This method uses the output of the 3-axis rate gyro to update the state variables, and uses the measured values of the 3-axis acceleration and the 3-axis magnetometer as the observation values to obtain the optimal estimation of the state variables in the sense of least squares, and then obtain the estimated value of the attitude angle. The advantage of this method is that it can better filter out the random interference of the sensor signal, compensate the drift of the gyroscope, and effectively improve the accuracy. But it requires that the measured value of gravitational acceleration is unbiased in a long time, that is to say, when the carrier is in a state of acceleration (or deceleration, turning) for a long time, the accuracy of this method will be seriously reduced, or even unusable. To solve this problem, methods to automatically adjust filter noise according to different states have been proposed. This method increases the dependence on the gyro signal when accelerating, decelerating or turning for a long time. The premise is that the drift of the gyro signal does not change significantly during this period of time, which puts forward higher requirements on the performance of the gyro, but using a high-precision gyro will increase the price and weight of the system, and the error will increase with time. keeps accumulating, so it still doesn't solve the problem.

此外，利用差分GPS或者GPS阵列的方法测量载体姿态角也是一种常用方法，这种方法容易受到卫星信号丢失的影响，在较多遮挡的情况下效果不好，GPS接收天线的位置和距离也会影响姿态测量的精度。In addition, it is also a common method to measure the attitude angle of the carrier by using differential GPS or GPS array. This method is easily affected by the loss of satellite signals, and the effect is not good in the case of more occlusions. It will affect the accuracy of attitude measurement.

发明内容Contents of the invention

本发明介绍了一种微小型动态载体姿态测量装置。该装置包括3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)、模数转换电路(106)、微处理器和存储器(107)、串行通讯接口(108)，以及相应软件测量方法。其中单轴速度传感器(105)为可选器件，没有它系统仍能正常工作，但加入单轴速度传感器(105)有助于提高测量精度。The invention introduces a miniature dynamic carrier attitude measuring device. The device includes a 3-axis rate gyroscope (101), a 3-axis magnetometer (102), a uniaxial accelerometer (103), a temperature sensor (104), a uniaxial speed sensor (105), an analog-to-digital conversion circuit (106), a micro Processor and memory (107), serial communication interface (108), and corresponding software measurement method. The uniaxial speed sensor (105) is an optional device, and the system can still work normally without it, but adding the uniaxial speed sensor (105) helps to improve measurement accuracy.

3轴速率陀螺(101)到单轴速度传感器(105)5种传感器的输出电压信号分别连接到模数转换电路(106)的各路输入端。模数转换电路(106)的控制信号和数据信号分别与微处理器和存储器(107)的对应端口相连接。微处理器和存储器(107)的输出端口与串口通讯单元(108)相连接。微处理器和存储器(107)按照预设的采样率控制模数转换电路(106)对各路传感器信号进行采样，并读取相应数据，进行处理，计算出姿态角，存储或输出到串口通讯单元(108)。The output voltage signals of the five types of sensors from the 3-axis rate gyroscope (101) to the single-axis speed sensor (105) are respectively connected to the input ends of the analog-to-digital conversion circuit (106). The control signal and data signal of the analog-to-digital conversion circuit (106) are respectively connected with corresponding ports of the microprocessor and the memory (107). The output ports of the microprocessor and memory (107) are connected with the serial port communication unit (108). The microprocessor and memory (107) control the analog-to-digital conversion circuit (106) to sample the sensor signals of various channels according to the preset sampling rate, and read the corresponding data, process them, calculate the attitude angle, store or output to the serial port communication unit (108).

利用上述测量装置测量载体姿态信息，步骤如下：Using the above measuring device to measure the attitude information of the carrier, the steps are as follows:

第一步，将测量装置与被测载体固连。定义地理坐标系和载体坐标系如图2所示。图2(a)为地理坐标系，N、E、D三个正交坐标轴分别指向北、东、地3个方向。图2(b)为载体坐标系，x、y、z三个正交轴分别与N、E、D三轴对齐时，各姿态角定义为零，方向以右手定则为准。姿态角的符号表示与范围可规定为：俯仰角(θ)范围-90°～90°，滚转角(Φ)范围-180°～180°，航向角(ψ)范围0°～360°。In the first step, the measuring device is fixedly connected to the measured carrier. Define the geographic coordinate system and carrier coordinate system as shown in Figure 2. Figure 2(a) is a geographic coordinate system, and the three orthogonal coordinate axes N, E, and D point to the three directions of north, east, and ground, respectively. Figure 2(b) is the carrier coordinate system. When the three orthogonal axes of x, y, and z are respectively aligned with the three axes of N, E, and D, each attitude angle is defined as zero, and the direction is based on the right-hand rule. The symbolic representation and range of the attitude angle can be specified as follows: the pitch angle (θ) ranges from -90° to 90°, the roll angle (Φ) ranges from -180° to 180°, and the heading angle (ψ) ranges from 0° to 360°.

按照航向→俯仰→滚转的顺序，由惯性坐标到载体坐标的方向余弦矩阵R可以表示为：According to the order of heading → pitch → roll, the direction cosine matrix R from inertial coordinates to vehicle coordinates can be expressed as:

$R R = = [\begin{matrix} cψ cψ \cdot &Center Dot; cθ cθ & sψ sψ \cdot &Center Dot; cθ cθ & - - sθ sθ \\ cψ cψ \cdot &Center Dot; sθ sθ \cdot \cdot sφ sφ - - sψ sψ \cdot &Center Dot; cφ cφ & sψ sψ \cdot &Center Dot; sθ sθ \cdot &Center Dot; sφ sφ + + cψ cψ \cdot \cdot cφ cφ & cθ cθ \cdot \cdot sφ sφ \\ cψ cψ \cdot \cdot sθ sθ \cdot &Center Dot; cφ cφ + + sψ sψ \cdot &Center Dot; sφ sφ & sψ sψ \cdot \cdot sθ sθ \cdot &Center Dot; cφ cφ - - cψ cψ \cdot &Center Dot; sφ sφ & cθ cθ \cdot &Center Dot; cφ cφ \end{matrix}]$

其中s和c分别为函数sin和cos的简写。Where s and c are shorthand for the functions sin and cos, respectively.

本发明用方向余弦矩阵R的第一列和第三列构造一个向量v，The present invention constructs a vector v with the first column and the third column of the direction cosine matrix R,

v＝[r₁₁ r₂₁ r₃₁ r₁₃ r₂₃ r₃₃]v=[r ₁₁ r ₂₁ r ₃₁ r ₁₃ r ₂₃ r ₃₃ ]

其中r_ij表示在R中的第i行j列个元素。Where r _ij represents the i-th row and j-column elements in R.

第二步，系统初始化。设定采样周期，设定状态变量的初始值，状态估计误差的协方差矩阵的初始值，测量噪声，过程噪声。将测量装置静态放置，设定3轴速率陀螺的零点。The second step is system initialization. Set the sampling period, set the initial value of the state variable, the initial value of the covariance matrix of the state estimation error, measurement noise, process noise. Place the measuring device statically, and set the zero point of the 3-axis rate gyro.

状态估计误差的协方差矩阵的初始值可任意给定，不影响系统的收敛性。测量噪声和过程噪声的大小则要根据具体传感器和应用环境决定。状态变量的初始值需要通过初始对准来确定，比如将载体放置于各姿态角均为零的位置，然后将状态变量的初值可以设置为The initial value of the covariance matrix of the state estimation error can be given arbitrarily without affecting the convergence of the system. The measurement noise and process noise should be determined according to the specific sensor and application environment. The initial value of the state variable needs to be determined by initial alignment, such as placing the carrier at a position where each attitude angle is zero, and then the initial value of the state variable can be set as

v₀＝[1 0 0 0 0 1]。v ₀ =[1 0 0 0 0 1].

第三步，采集传感器信号。由微处理器控制模数转换电路按照采样周期采集各路传感器数据，读取模数转换的结果到微处理器中。The third step is to collect sensor signals. The analog-to-digital conversion circuit is controlled by the microprocessor to collect sensor data from various channels according to the sampling period, and the result of the analog-to-digital conversion is read into the microprocessor.

第四步，对传感器数据进行补偿和标定。按照传感器温度特性曲线对传感器采样结果进行温度补偿。由于安装引起的误差也需要进行标定。The fourth step is to compensate and calibrate the sensor data. Perform temperature compensation on the sensor sampling results according to the sensor temperature characteristic curve. Errors due to installation also need to be calibrated.

传感器3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、单轴速度传感器(105)的温度特性曲线要经过预先测定，并保存在存储器中。根据采样时的温度值，微处理器从存储器中读取温度特性曲线对应值，对上述传感器数据进行补偿。如果不能保证加速度计(103)安装在载体的转动中心，则根据当前转动角速度与到转动中心的距离计算出由于转动引起的向心加速度，进行补偿。The temperature characteristic curves of sensors 3-axis rate gyroscope (101), 3-axis magnetometer (102), uniaxial accelerometer (103) and uniaxial speed sensor (105) should be measured in advance and stored in memory. According to the temperature value at the time of sampling, the microprocessor reads the corresponding value of the temperature characteristic curve from the memory, and compensates the above sensor data. If it cannot be ensured that the accelerometer (103) is installed on the rotation center of the carrier, the centripetal acceleration due to the rotation is calculated according to the current rotation angular velocity and the distance to the rotation center, and compensation is performed.

${a a}_{x x} = = {a a}_{x x}^{' '} - - (({ω ω}_{y the y}^{22} + + {ω ω}_{z z}^{22})) {X x}_{ax ax} - - - - - - ((11))$

其中X_ax为x轴加速度计到转动中心的距离，a_x′为加速度计的测量值，a_x为补偿由于转动和偏心引起的加速度后的加速度值。Among them, X _ax is the distance from the x-axis accelerometer to the center of rotation, a _x ′ is the measured value of the accelerometer, and a _x is the acceleration value after compensating the acceleration caused by rotation and eccentricity.

第五步，利用陀螺数据更新状态变量。利用3轴速率陀螺测量值和前一时刻的状态变量的估计值可以计算出当前时刻状态变量的一步预报估计值。The fifth step is to use the gyroscope data to update the state variables. Using the 3-axis rate gyro measurement and the estimated value of the state variable at the previous moment, the estimated value of the one-step forecast of the state variable at the current moment can be calculated.

更新公式如下：The update formula is as follows:

$\overset{^^}{v v} ((kh kh + + h h | | kh kh)) = = F f \cdot &Center Dot; \overset{^^}{v v} ((kh kh)) - - - - - - ((22))$

其中F为陀螺当前角速率的函数，为kh时刻状态变量v的估计值，

为状态变量的一步预报估计。Where F is the function of the current angular rate of the gyroscope, is the estimated value of the state variable v at time kh,

Estimates for one-step forecasts of state variables.

第六步，根据观测值和约束条件修正状态变量。将3轴磁场计、单轴加速度计、以及两个状态变量的约束条件作为测量值，对状态变量的一步预报值进行修正。The sixth step is to correct the state variables according to the observations and constraints. The three-axis magnetometer, single-axis accelerometer, and constraints of two state variables are used as measured values to correct the one-step forecast value of state variables.

根据测量值和约束条件，对一步预报得到的状态量

进行修正：According to the measured value and constraints, the state quantity obtained from the one-step forecast

Make a correction:

P(kh+h|kh)＝F·P(kh)·F^T+Φ_ev P(kh+h|kh)＝F·P(kh)·F ^T +Φ _ev

K(kh+h)＝P(kh+h|kh)·C^T[C·P(kh+h|kh)·C^T+Φ_ey]^-1 K(kh+h)＝P(kh+h|kh)·C ^T [C·P(kh+h|kh)·C ^T +Φ _ey ] ^-1

$\overset{^^}{v v} ((kh kh + + h h)) = = \overset{^^}{v v} ((kh kh + + h h | | kh kh)) + + K K ((kh kh + + h h)) \cdot &Center Dot; [[y the y ((kh kh + + h h)) - - C C \cdot \cdot \overset{^^}{v v} ((kh kh + + h h | | kh kh))]]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)。 (3)P(kh+h)=[I-K(kh+h)·C]·P(kh+h|kh). (3)

第七步，计算并输出当前姿态角。根据状态变量计算当前姿态角信息，并根据需要存储或者输出。The seventh step is to calculate and output the current attitude angle. Calculate the current attitude angle information according to the state variables, and store or output it as needed.

通过状态变量v解算出姿态角，具体公式如下：The attitude angle is calculated by solving the state variable v, and the specific formula is as follows:

俯仰角：Pitch angle:

θ＝-arcsin(r₁₃) (4)θ＝-arcsin(r ₁₃ ) (4)

滚转角：Roll angle:

航向角：Heading:

$ψ ψ = = \{\begin{matrix} arccos arccos (({r r}_{1111} / / cos cos ((θ θ)))),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} &GreaterEqual; &Greater Equal; 00 \\ 22 π π - - arccos arccos (({r r}_{1111} / / cos cos ((θ θ)))),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} < < 00 \end{matrix} - - - - - - ((66))$

第八步，跳转到第三步或者退出。跳转到第三步继续测量，或者停止退出。The eighth step, jump to the third step or exit. Jump to step 3 to continue measuring, or stop to exit.

第五步和第六步基于以下离散状态空间模型：The fifth and sixth steps are based on the following discrete state-space model:

v(kh+h)＝F·v(kh)+e_v(kh) (7)v(kh+h)=F·v(kh)+e _v (kh) (7)

y(kh+h)＝C·v(kh+h)+e_y(kh+h) (8)y(kh+h)=C·v(kh+h)+e _y (kh+h) (8)

其中in

$F f = = I I + + h h \times \times [\begin{matrix} Ω Ω ((ω ω)) & 00 \\ 00 & Ω Ω ((ω ω)) \end{matrix}],,$

y(kh+h)＝[m_x(kh+h)m_y(kh+h)m_z(kh+h)a_x(kh+h)1 1]^T y(kh+h)＝[m _x (kh+h)m _y (kh+h)m _z (kh+h)a _x (kh+h)1 1] ^T

$C C = = [\begin{matrix} cos cos β β & 00 & 00 & sin sin β β & 00 & 00 \\ 00 & cos cos β β & 00 & 00 & sin sin β β & 00 \\ 00 & 00 & cos cos β β & 00 & 00 & sin sin β β \\ 00 & 00 & 00 & g g & 00 & 00 \\ {r r}_{1111 kh kh} & {r r}_{21 twenty one kh kh} & {r r}_{3131 kh kh} & 00 & 00 & 00 \\ 00 & 00 & 00 & {r r}_{1313 kh kh} & {r r}_{23 twenty three kh kh} & {r r}_{3333 kh kh} \end{matrix}]$

e_v和e_y是功率谱密度为Φ_ev和Φ_ey的零均值白噪声。 _ev and _ey are zero-mean white noise with power spectral densities Φ _ev and Φ _ey .

以状态方程(7)，测量方程(8)构造离散卡尔曼滤波器，其中测量矩阵C由上一时刻状态变量构成。因为方程(8)中的测量值y仅含有a_x，所以不受y、z轴方向运动加速度项的影响。如前所述，如果利用a_x-&代替a_x将会消除x轴方向运动加速度的影响，这样在高动态环境下仍能保证高精度测量。Construct the discrete Kalman filter with the state equation (7) and the measurement equation (8), where the measurement matrix C is composed of the state variables at the previous moment. Since the measurement value y in equation (8) only contains a _x , it is not affected by the motion acceleration items in the y and z directions. As mentioned earlier, if a _x -& is used instead of a _x , the influence of the acceleration of motion in the x-axis direction will be eliminated, so that high-precision measurement can still be guaranteed in a highly dynamic environment.

离散状态空间方程由下面的连续方程按照采样周期h离散化得到。The discrete state space equation is obtained by discretizing the following continuous equation according to the sampling period h.

对方向余弦矩阵R取微分，得到方程如下：Differentiate the direction cosine matrix R to get the following equation:

$\frac{dR d}{dt dt} = = Ω Ω ((ω ω)) R R - - - - - - ((99))$

其中 $Ω (ω) = [\begin{matrix} 0 & ω_{z} & - ω_{y} \\ - ω_{z} & 0 & ω_{x} \\ ω_{y} & - ω_{x} & 0 \end{matrix}]$ in $Ω (ω) = [\begin{matrix} 0 & ω_{z} & - ω_{the y} \\ - ω_{z} & 0 & ω_{x} \\ ω_{the y} & - ω_{x} & 0 \end{matrix}]$

因为v由R的第一列和第三列组成，所以由方程(9)可以得出：Since v consists of the first and third columns of R, it follows from equation (9):

$\frac{d d}{dt dt} v v = = [\begin{matrix} Ω Ω ((ω ω)) & 00 \\ 00 & Ω Ω ((ω ω)) \end{matrix}] v v + + {e e}_{v v} - - - - - - ((1010))$

其中0为3×3的零矩阵，e_v近似为零均值白噪声，其功率谱密度近似为Φ_ev。Where 0 is a 3×3 zero matrix, _ev is approximately zero-mean white noise, and its power spectral density is approximately Φ _ev .

重力加速度矢量在惯性坐标系中的坐标表示为a₀＝[0 0 g]^T。在载体坐标系中测量值为a＝[a_x a_y a_z]^T。重力加速度矢量由惯性坐标系转换到载体坐标系：The coordinates of the gravitational acceleration vector in the inertial coordinate system are expressed as a ₀ =[0 0 g] ^T . The measured value in the carrier coordinate system is a=[a _x a _y a _z ] ^T . The gravity acceleration vector is converted from the inertial coordinate system to the carrier coordinate system:

a＝Ra₀+e_a (11)a＝Ra ₀ +e _a (11)

其中e_a为测量噪声。当载体处于静态或者低动态环境时，e_a可以近似为功率谱密度为Φ_a的零均值白噪声，但当载体处于高动态环境时，加速度测量值与重力加速度分量相差很大，特别是存在长周期低频运动加速度时，如长时间盘旋，e_a不能认为是零均值白噪声，但此加速度对于x轴加速度分量的影响相对较小。根据方程(6)，加速度矢量在载体坐标系x轴方向的分量为：Where e _a is the measurement noise. When the carrier is in a static or low dynamic environment, e _a can be approximated as a zero-mean white noise with a power spectral density of Φ _a , but when the carrier is in a high dynamic environment, the measured acceleration value and the gravitational acceleration component differ greatly, especially when there is During long-period low-frequency motion acceleration, such as hovering for a long time, e _a cannot be considered as zero-mean white noise, but the impact of this acceleration on the x-axis acceleration component is relatively small. According to equation (6), the component of the acceleration vector in the x-axis direction of the carrier coordinate system is:

a_x＝r₁₃g+e_ax (12)a _x =r ₁₃ g+e _ax (12)

其中e_ax可近似为功率谱密度为Φ_ax的零均值白噪声。如果x轴方向加入速度传感器，用a_x-&代替上式中的a_x将会有助于进一步提高精度。Where e _ax can be approximated as zero-mean white noise with power spectral density Φ _ax . If a speed sensor is added in the x-axis direction, replacing a _x in the above formula with a _x -& will help to further improve the accuracy.

地磁矢量在惯性坐标系中的坐标表示为m₀＝[mcosβ 0 M sinβ]^T，其中M为地磁矢量的模(后面表述中将M作为单位长度量省略)，β为当地地磁倾角，忽略地磁偏角或解算出航向角后进行修正。在载体坐标系中测量值为m＝[m_x m_y m_z]^T。地磁矢量由惯性坐标系转换到载体坐标系：The coordinates of the geomagnetic vector in the inertial coordinate system are expressed as m ₀ =[mcosβ 0 M sinβ] ^T , where M is the modulus of the geomagnetic vector (M is omitted as a unit length in the following expressions), and β is the local geomagnetic dip, ignoring the geomagnetic Make corrections after declination or heading angle is calculated. The measured value in the carrier coordinate system is m=[m _x m _y m _z ] ^T . The geomagnetic vector is converted from the inertial coordinate system to the carrier coordinate system:

m＝Rm₀+e_m (13)m=Rm ₀ +e _m (13)

其中e_m为测量噪声，可近似为独立的零均值白噪声。Where _em is the measurement noise, which can be approximated as an independent zero-mean white noise.

注意到向量v的六个元素并不是相互独立的，存在如下两个约束条件：Note that the six elements of the vector v are not independent of each other, and there are two constraints as follows:

${r r}_{1111}^{22} + + {r r}_{21 twenty one}^{22} + + {r r}_{3131}^{22} = = 11,,$ ${r r}_{1313}^{22} + + {r r}_{23 twenty three}^{22} + + {r r}_{3333}^{22} = = 11 - - - - - - ((1414))$

将约束条件(14)作为向量v的“伪测量方程”，与方程(12)、(13)一起构成向量v的测量方程。方程(14)中含有非线性项，因此需要做线性化处理。将(14)中二次项中的一项写到输出方程中，即将方程(12)、(13)、(14)写成如下形式：The constraint condition (14) is used as the "pseudo-measurement equation" of the vector v, together with the equations (12) and (13) to form the measurement equation of the vector v. Equation (14) contains nonlinear terms, so it needs to be linearized. Write one of the quadratic terms in (14) into the output equation, that is, write equations (12), (13), and (14) as follows:

$[\begin{matrix} {m m}_{x x} \\ {m m}_{y the y} \\ {m m}_{z z} \\ {a a}_{x x} \\ 11 \\ 11 \end{matrix}] = = [\begin{matrix} cos cos β β & 00 & 00 & sin sin β β & 00 & 00 \\ 00 & cos cos β β & 00 & 00 & sin sin β β & 00 \\ 00 & 00 & cos cos β β & 00 & 00 & sin sin β β \\ 00 & 00 & 00 & g g & 00 & 00 \\ {r r}_{1111} & {r r}_{21 twenty one} & {r r}_{3131} & 00 & 00 & 00 \\ 00 & 00 & 00 & {r r}_{1313} & {r r}_{23 twenty three} & {r r}_{3333} \end{matrix}] \times \times [\begin{matrix} {r r}_{1111} \\ {r r}_{21 twenty one} \\ {r r}_{3131} \\ {r r}_{1313} \\ {r r}_{23 twenty three} \\ {r r}_{3333} \end{matrix}] + + {e e}_{y the y} - - - - - - ((1515))$

其中e_y＝[e_mx e_my e_mz e_ax e_c1 e_c2]^T，e_c1、e_c2为约束条件的计算误差，可近似为白噪声。Where e _y =[e _mx e _my e _mz e _ax e _c1 e _c2 ] ^T , e _c1 and e _c2 are calculation errors of constraints, which can be approximated as white noise.

将方程(10)和方程(15)按采样周期h离散化就会得到由方程(7)和方程(8)构成的离散系统方程。Discretizing Equation (10) and Equation (15) according to the sampling period h will result in a discrete system equation consisting of Equation (7) and Equation (8).

附图说明Description of drawings

图1是该姿态测量装置的硬件组成框图。Figure 1 is a block diagram of the hardware composition of the attitude measurement device.

图2是描述惯性坐标系与载体坐标系，其中(a)惯性坐标系(NED)，(b)载体坐标系(xyz)。Figure 2 is a description of the inertial coordinate system and the carrier coordinate system, where (a) the inertial coordinate system (NED), (b) the carrier coordinate system (xyz).

图3是测量方法流程图。Fig. 3 is a flow chart of the measurement method.

图4是飞行器坐标系。Figure 4 is the aircraft coordinate system.

具体实施方式Detailed ways

下面以测量飞行器姿态为例，介绍该姿态测量装置和方法的具体实时过程。Taking the attitude measurement of an aircraft as an example, the specific real-time process of the attitude measurement device and method will be introduced below.

图1为该姿态测量装置的硬件组成框图。3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)、模数转换电路(106)、微处理器和存储器(107)、串行通讯接口(108)，其中速度传感器(105)为可选器件。3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)等器件选用基于MEMS技术的传感器，体积小、重量轻。Figure 1 is a block diagram of the hardware composition of the attitude measurement device. 3-axis rate gyroscope (101), 3-axis magnetometer (102), single-axis accelerometer (103), temperature sensor (104), single-axis speed sensor (105), analog-to-digital conversion circuit (106), microprocessor and Memory (107), serial communication interface (108), wherein the speed sensor (105) is an optional device. 3-axis rate gyroscope (101), 3-axis magnetometer (102), single-axis accelerometer (103) and other devices use sensors based on MEMS technology, which are small in size and light in weight.

3轴速率陀螺(101)、3轴磁场计(102)、单轴加速度计(103)、温度传感器(104)、单轴速度传感器(105)5种传感器的输出电压信号分别连接到模数转换电路(106)的各路输入端。模数转换电路(106)的控制信号和数据信号分别与微处理器和存储器(107)的对应端口相连接。微处理器和存储器(107)的输出端口与串口通讯单元(108)相连接。3-axis rate gyroscope (101), 3-axis magnetometer (102), uniaxial accelerometer (103), temperature sensor (104), uniaxial speed sensor (105) and the output voltage signals of the 5 sensors are respectively connected to the analog-to-digital converter Each input end of the circuit (106). The control signal and data signal of the analog-to-digital conversion circuit (106) are respectively connected with corresponding ports of the microprocessor and the memory (107). The output ports of the microprocessor and memory (107) are connected with the serial port communication unit (108).

图3是测量方法流程图，说明了利用上述装置测量飞行器姿态的方法和步骤。Fig. 3 is a flow chart of the measurement method, illustrating the method and steps of using the above-mentioned device to measure the attitude of the aircraft.

(301)将该测量装置与飞行器固连，图4说明了坐标轴x、y、z分别与飞行器各对应轴向对齐。x轴与飞行器机头方向对其，y轴与飞行器机翼方向对其，z轴与飞行器机翼方向垂直。(301) The measuring device is fixedly connected to the aircraft. FIG. 4 illustrates that the coordinate axes x, y, and z are respectively aligned with the corresponding axes of the aircraft. The x-axis is aligned with the direction of the nose of the aircraft, the y-axis is aligned with the direction of the wings of the aircraft, and the z-axis is perpendicular to the direction of the wings of the aircraft.

(302)系统初始化。设定采样周期为25Hz；将载体放置于各姿态角均为零的位置，然后将状态变量的初值设置为；状态估计误差的协方差矩阵的初始值任意设定为单位矩阵；设定测量噪声，过程噪声的值。将测量装置静态放置，设定3轴速率陀螺的测量值为零点。(302) System initialization. Set the sampling period to 25Hz; place the carrier at a position where each attitude angle is zero, and then set the initial value of the state variable to ; the initial value of the covariance matrix of the state estimation error is arbitrarily set to the identity matrix; set the measurement Noise, the value of the process noise. Place the measurement device statically, and set the measurement value of the 3-axis rate gyro to zero.

(303)采集传感器信号。由微处理器控制模数转换电路按照采样周期采集各路传感器数据，读取模数转换的结果到微处理器中。(303) Collect sensor signals. The analog-to-digital conversion circuit is controlled by the microprocessor to collect sensor data from various channels according to the sampling period, and the result of the analog-to-digital conversion is read into the microprocessor.

(304)温度补偿和标定，根据温度传感器测量结果，从存储器中读出预先标定好的修正值，对传感器数据进行温度补偿。下面所用到的传感器信号均为经过温度补偿后的数据。(304) Temperature compensation and calibration. According to the measurement result of the temperature sensor, read out the pre-calibrated correction value from the memory, and perform temperature compensation on the sensor data. The sensor signals used below are all data after temperature compensation.

测量出x轴加速度计到转动中心的距离X_ax，根据当前转动角速度与X_ax，计算出由于转动引起的向心加速度，进行补偿。Measure the distance X _ax from the x-axis accelerometer to the center of rotation, calculate the centripetal acceleration caused by the rotation according to the current rotation angular velocity and X _ax , and perform compensation.

${a a}_{x x} = = {a a}_{x x}^{' '} - - (({ω ω}_{y the y}^{22} + + {ω ω}_{z z}^{22})) {X x}_{ax ax}$

其中a_x′为加速度计的测量值，a_x为补偿由于转动和偏心引起的加速度后的加速度值。Among them, a _x ′ is the measured value of the accelerometer, and a _x is the acceleration value after compensating the acceleration caused by rotation and eccentricity.

(305)利用陀螺数据更新状态变量。利用3轴速率陀螺测量值和前一时刻的状态变量的估计值可以计算出当前时刻状态变量的一步预报估计值。(305) Utilize the gyroscope data to update the state variables. Using the 3-axis rate gyro measurement and the estimated value of the state variable at the previous moment, the estimated value of the one-step forecast of the state variable at the current moment can be calculated.

更新公式如下：The update formula is as follows:

$\overset{^^}{v v} ((kh kh + + h h | | kh kh)) = = F f \cdot &Center Dot; \overset{^^}{v v} ((kh kh))$

其中F为陀螺当前角速率的函数，

为kh时刻状态变量v的估计值，

为状态变量的一步预报估计。Where F is the function of the current angular rate of the gyroscope,

is the estimated value of the state variable v at time kh,

Estimates for one-step forecasts of state variables.

(306)根据观测值和约束条件修正状态变量。将3轴磁场计、单轴加速度计、以及两个状态变量的约束条件作为测量值，对状态变量的一步预报值进行修正。(306) Correcting the state variable according to the observed value and the constraint condition. The three-axis magnetometer, single-axis accelerometer, and constraints of two state variables are used as measured values to correct the one-step forecast value of state variables.

根据测量值和约束条件，对一步预报得到的状态量

Make a correction:

P(kh+h|kh)＝F·P(kh)·F^T+Φ_ev P(kh+h|kh)＝F·P(kh)·F ^T +Φ _ev

$\overset{^^}{v v} ((kh kh + + h h)) = = \overset{^^}{v v} ((kh kh + + h h | | kh kh)) + + K K ((kh kh + + h h)) \cdot &Center Dot; [[y the y ((kh kh + + h h)) - - C C \cdot &Center Dot; \overset{^^}{v v} ((kh kh + + h h | | kh kh))]]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)；P(kh+h)=[I-K(kh+h)·C]·P(kh+h|kh);

(307)计算并输出当前姿态角。根据状态变量计算当前姿态角信息，并根据需要存储或者输出。(307) Calculate and output the current attitude angle. Calculate the current attitude angle information according to the state variables, and store or output it as needed.

俯仰角：Pitch angle:

θ＝-arcsin(r₁₃)θ=-arcsin(r ₁₃ )

滚转角：Roll angle:

航向角：Heading:

$ψ ψ = = \{\begin{matrix} arccos arccos (({r r}_{1111} / / cos cos ((θ θ)))),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} &GreaterEqual; &Greater Equal; 00 \\ 22 π π - - arccos arccos (({r r}_{1111} / / cos cos ((θ θ)))),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} < < 00 \end{matrix}$

(308)跳转到第三步或者退出。跳转到第三步继续测量，或者停止退出。(308) Jump to the third step or exit. Jump to step 3 to continue measuring, or stop to exit.

该装置体积小、重量轻，与传统惯性导航装置相比，对传感器性能要求不高，可以选用体积很小的MEMS传感器，且通过特定算法，有效地消除了运动加速度的干扰，姿态测量精度能做到较高，因此特别适用于高动态环境中对飞行器、车辆、船舶或其他载体的动态姿态进行测量。The device is small in size and light in weight. Compared with the traditional inertial navigation device, it does not require high sensor performance. It can use a small MEMS sensor, and through a specific algorithm, the interference of motion acceleration is effectively eliminated, and the attitude measurement accuracy can be improved. It is relatively high, so it is especially suitable for measuring the dynamic attitude of aircraft, vehicles, ships or other carriers in a high dynamic environment.

Claims

1. A measurement method for carrier posture information measurement, characterized in that, the measurement method comprises the steps: the first step: the measuring device is fixedly connected with the measured carrier; define the geographic coordinate system and the carrier coordinate system;

In the order of heading→pitch→roll, construct the direction cosine matrix R from inertial coordinates to vehicle coordinates:

R R = = [\begin{matrix} cψ cψ \cdot \cdot cθ cθ & sψ sψ \cdot \cdot cθ cθ & - - sθ sθ \\ cψ cψ \cdot \cdot sθ sθ \cdot \cdot sφ sφ - - sψ sψ \cdot &Center Dot; cφ cφ & sψ sψ \cdot &Center Dot; sθ sθ \cdot &Center Dot; sφ sφ + + cψ cψ \cdot \cdot cφ cφ & cθ cθ \cdot \cdot sφ sφ \\ cψ cψ \cdot &Center Dot; sθ sθ \cdot \cdot cφ cφ + + sψ sψ \cdot &Center Dot; sφ sφ & sψ sψ \cdot \cdot sθ sθ \cdot &Center Dot; cφ cφ - - cψ cψ \cdot &Center Dot; sφ sφ & cθ cθ \cdot &Center Dot; cφ cφ \end{matrix}]

Among them, s and c are the abbreviations of the functions sin and cos respectively;

Construct a vector v using the first and third columns of the direction cosine matrix R,

v=[r ₁₁ r ₂₁ r ₃₁ r ₁₃ r ₂₃ r ₃₃ ]

Where r _ij represents the elements in row i and column j in R;

The second step, system initialization

Set the sampling period, set the initial value of the state variable, the initial value of the covariance matrix of the state estimation error, measurement noise, process noise;

Place the measuring device statically and set the zero point of the 3-axis rate gyroscope;

The third step is to collect the sensor signal

The microprocessor controls the analog-to-digital conversion circuit to collect sensor data from various channels according to the sampling period, and reads the results of the analog-to-digital conversion to the microprocessor;

The fourth step is to compensate and calibrate the sensor data

Perform temperature compensation on the sensor sampling results according to the sensor temperature characteristic curve;

Errors due to installation also need to be calibrated;

The fifth step is to use the gyroscope data to update the state variables

Using the 3-axis rate gyro measurement value and the estimated value of the state variable at the previous moment to calculate the one-step forecast value of the state variable at the current moment; the update formula is:

\overset{^^}{v v} ((kh kh + + h h | | kh kh)) = = F f \cdot \cdot \overset{^^}{v v} ((kh kh))

Where F is the function of the current angular rate of the gyroscope,

is the estimated value of the state variable v at time kh, is the one-step forecast estimate of the state variable;

The sixth step is to modify the state variables according to the observations and constraints

Using the 3-axis magnetometer, uniaxial accelerometer, and the constraints of the two state variables as measured values, the one-step forecast value of the state variable is corrected;

According to the measured value and constraints, the state quantity obtained from the one-step forecast

Make a correction:

P(kh+h|kh)＝F·P(kh)·F ^T +Φ _ev

K(kh+h)＝P(kh+h|kh)·C ^T [C·P(kh+h|kh)·C ^T +Φ _ey ] ^-1

\overset{^^}{v v} ((kh kh + + h h)) = = \overset{^^}{v v} ((kh kh + + h h | | kh kh)) + + K K ((kh kh + + h h)) \cdot \cdot [[y the y ((kh kh + + h h)) - - C C \cdot &Center Dot; \overset{^^}{v v} ((kh kh + + h h | | kh kh))]]

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)

The seventh step is to calculate and output the current attitude angle

Calculate the current attitude angle information according to the state variables, and store or output as needed;

The attitude angle is calculated by solving the state variable v, and the specific formula is as follows:

Pitch angle:

θ=-arcsin(r ₁₃ )

Roll angle:

Heading:

ψ ψ = = \{\begin{matrix} arccos arccos (({r r}_{1111} / / cos cos θ θ)),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} &GreaterEqual; &Greater Equal; 00 \\ 22 π π - - arccos arccos (({r r}_{1111} / / cos cos ((θ θ)))),, & {r r}_{3131} \times \times {r r}_{23 twenty three} - - {r r}_{3333} \times \times {r r}_{21 twenty one} < < 00 \end{matrix}

The eighth step, jump to the third step or exit;

Jump to the third step to continue the measurement, or stop and exit to complete the measurement.