CN102506892A

CN102506892A - Configuration method for information fusion of a plurality of optical flow sensors and inertial navigation device

Info

Publication number: CN102506892A
Application number: CN2011103491463A
Authority: CN
Inventors: 刘小明; 陈万春; 邢晓岚
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2011-11-08
Filing date: 2011-11-08
Publication date: 2012-06-20
Anticipated expiration: 2031-11-08
Also published as: CN102506892B

Abstract

A method for information fusion configuration of optical flow multi-sensors and inertial navigation devices, which has five major steps: 1. Establish the linear disturbance kinematics equation for the aircraft that needs to be installed with optical flow sensors; Arranged on the aircraft; 3. According to the installation position and direction of each optical flow sensor on the aircraft, the measurement equation of the optical flow sensor is established; 4. The EKF is the extended Kalman filter method and the UKF is the unscented Kalman filter method. Estimate the flight state of the aircraft; Fifth, use the estimated state information to realize a specific flight task of the aircraft. The invention uses a plurality of optical flow sensors and an inertial navigation device, has light weight, small volume and low power consumption, is convenient to install and arrange on the aircraft, does not radiate electromagnetic signals to the outside, and improves the concealment of the aircraft. It has practical value and broad application prospect in the technical field of measuring and estimating aircraft attitude, flying speed and height.

Description

An optical flow multi-sensor and inertial navigation device information fusion configuration method

(一)技术领域： (1) Technical field:

本发明涉及一种光流多传感器和惯导器件信息融合配置方法，属于飞行器姿态、飞行速度和高度的测量与估计技术领域。The invention relates to an optical flow multi-sensor and inertial navigation device information fusion configuration method, which belongs to the technical field of measurement and estimation of aircraft attitude, flight speed and altitude.

(二)背景技术： (two) background technology:

飞行器主要依靠前视雷达、雷达高度表和升降速度表来测量离地高度和升降速度，而对于小型飞行器来说，激光测距仪(Laser Rangefinders，LRF)和雷达都显得过于笨重。SICKLMS291是一款典型的激光测距仪，一般用于机器人领域，它的质量大约是4.5公斤。用于无人驾驶航空器(Unmanned Aerial Vehicle，UAV)上的最小的合成孔径雷达可能是美国圣地亚实验室(Sandia National Labs)制造的miniSAR，其质量约为4～5公斤，这么重的设备增加了无人机的重量和体积，降低了其续航能力和带载能力。Aircraft mainly rely on forward-looking radar, radar altimeter, and vertical speedometer to measure the height above the ground and the vertical speed. For small aircraft, laser rangefinders (Laser Rangefinders, LRF) and radar are too bulky. SICKLMS291 is a typical laser range finder, which is generally used in the field of robotics, and its mass is about 4.5 kg. The smallest synthetic aperture radar used on unmanned aerial vehicles (Unmanned Aerial Vehicle, UAV) may be the miniSAR manufactured by Sandia National Labs in the United States, and its mass is about 4-5 kg. The weight and volume of the UAV are reduced, and its endurance and carrying capacity are reduced.

光流传感器质量小，只有10克左右，完全被动地接收外部光线，无辐射，相对于雷达高度计具有质量小、隐蔽性好的优点，除飞行器的飞行速度、飞行高度、俯仰角速度外，本方法还可以估计出飞行器的攻角、俯仰角、升降速度等其它飞行信息，这些信息有利于帮助无人机完成探测、救灾等特定飞行任务。The mass of the optical flow sensor is small, only about 10 grams. It receives external light completely passively and has no radiation. Compared with the radar altimeter, it has the advantages of small mass and good concealment. It can also estimate other flight information such as the angle of attack, pitch angle, and lift speed of the aircraft. This information is beneficial to help the drone to complete specific flight tasks such as detection and disaster relief.

(三)发明内容： (3) Contents of the invention:

1、目的：本发明的目的是提供一种光流多传感器和惯导器件信息融合配置方法，它使用2～4个光流传感器和一个惯导器件，重量轻、体积小、功耗低，便于在小型飞行器上安装布置，不对外辐射电磁信号，提高了飞行器的隐蔽性。1. Purpose: The purpose of this invention is to provide a method for information fusion configuration of optical flow multi-sensors and inertial navigation devices, which uses 2 to 4 optical flow sensors and one inertial navigation device, which is light in weight, small in size and low in power consumption. It is convenient to install and arrange on a small aircraft, does not radiate electromagnetic signals to the outside, and improves the concealment of the aircraft.

2、技术方案：昆虫在移动时，周围环境的亮度模式在视网膜上形成一系列连续变化的图像，这一系列连续变化的信息不断“流过”视网膜，好像是一种光的“流”，故称这种图像亮度模式的表观运动为光流。国外的某些实验室，已经研制出了光流传感器的物理样机，并利用光流传感器实现了无人驾驶飞行器的自主避障、等高飞行、自动着陆、风速估计、目标检测和空中悬停，这些技术在探测、救灾等方面将有非常重要的应用价值。根据光流的定义和图1中所示的几何关系，可得出光流的表达式为：2. Technical solution: When an insect is moving, the brightness pattern of the surrounding environment forms a series of continuously changing images on the retina. This series of continuously changing information continuously "flows" through the retina, as if it is a "flow" of light. Therefore, the apparent motion of this image brightness pattern is called optical flow. Some foreign laboratories have developed physical prototypes of optical flow sensors, and used optical flow sensors to realize autonomous obstacle avoidance, contour flight, automatic landing, wind speed estimation, target detection and air hovering of unmanned aerial vehicles. , these technologies will have very important application value in detection and disaster relief. According to the definition of optical flow and the geometric relationship shown in Figure 1, the expression of optical flow can be obtained as:

$f f = = \frac{v v {cos cos}^{22} θ θ}{h h} + + ω ω - - - - - - ((11))$

式中，f为光流(1/s)，v为光流传感器的水平速度(m/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)。In the formula, f is the optical flow (1/s), v is the horizontal speed of the optical flow sensor (m/s), h is the height of the optical flow sensor from the ground (m), θ is the distance between the optical axis and the vertical direction Angle (rad), ω is the rotational speed of the optical flow sensor (rad/s).

由式(1)可以看出，光流的测量值与光流传感器的姿态、高度和速度耦合，同时光流传感器具有体积小、重量轻、功耗低、可组网的特点，于是考虑将多个光流传感器固联在飞行器上，结合惯导器件——速率陀螺，利用多传感器的信息融合技术，实现对飞行器姿态信息的估计，并利用估计信息实现飞行器的超低空突防任务。光流传感器与惯导器件的结合布置如图2所示。It can be seen from formula (1) that the measured value of the optical flow is coupled with the attitude, height and speed of the optical flow sensor. At the same time, the optical flow sensor has the characteristics of small size, light weight, low power consumption, and can be networked. Multiple optical flow sensors are fixedly connected to the aircraft, combined with the inertial navigation device - the rate gyroscope, and the multi-sensor information fusion technology is used to realize the estimation of the attitude information of the aircraft, and use the estimated information to realize the ultra-low altitude penetration mission of the aircraft. The combined layout of the optical flow sensor and the inertial navigation device is shown in Figure 2.

本发明一种光流多传感器和惯导器件信息融合配置方法，该方法具体步骤如下：The present invention is an optical flow multi-sensor and inertial navigation device information fusion configuration method. The specific steps of the method are as follows:

步骤一：针对需要安装光流传感器的飞行器，在铅垂平面内建立其线化扰动运动学方程；Step 1: For the aircraft that needs to install the optical flow sensor, establish its linear disturbance kinematics equation in the vertical plane;

$(\begin{matrix} \overset{\cdot &Center Dot;}{α α} \\ \overset{\cdot \cdot}{θ θ} \\ \overset{\cdot \cdot \cdot \cdot}{θ θ} \\ \overset{\cdot \cdot}{h h} \\ \overset{\cdot \cdot \cdot &Center Dot;}{h h} \end{matrix}) = = (\begin{matrix} {a a}_{1111} & 00 & 11 & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 \\ {a a}_{3131} & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 \\ {a a}_{5151} & 00 & 00 & 00 & 00 \end{matrix}) (\begin{matrix} α α \\ θ θ \\ \overset{\cdot \cdot}{θ θ} \\ h h \\ \overset{\cdot \cdot}{h h} \end{matrix}) + + (\begin{matrix} {b b}_{1111} \\ 00 \\ {b b}_{3131} \\ 00 \\ {b b}_{5151} \end{matrix}) {δ δ}_{z z} + + ω ω ((t t)) - - - - - - ((22))$

式中，α为飞行器攻角(rad)，

为飞行器俯仰角(rad)，δ_z为飞行器舵偏角(rad)，h为飞行器质心高度(m)，a₁₁、a₃₁、a₅₁、b₁₁、b₃₁、b₅₁为常系数，它们与飞行器的气动特性和质量特性有关，ω(t)为白噪声过程，E[ω(t)]＝0，E[ω(t)ω^T(τ)]＝qδ(t-τ)，q为ω(t)的方差强度阵。where α is the angle of attack of the aircraft (rad),

is the pitch angle of the aircraft (rad), δ _z is the rudder angle of the aircraft (rad), h is the height of the center of mass of the aircraft (m), a ₁₁ , a ₃₁ , a ₅₁ , b ₁₁ , b ₃₁ , and b ₅₁ are constant coefficients, and they It is related to the aerodynamic and mass characteristics of the aircraft, ω(t) is a white noise process, E[ω(t)]=0, E[ω(t)ω ^T (τ)]=qδ(t-τ), q is the variance intensity matrix of ω(t).

步骤二：将多个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；Step 2: Arrange multiple optical flow sensors on the aircraft at multiple points. If the space permits, the distance between the sensors should be as far as possible and point to different directions. This will help improve the subsequent estimation accuracy;

其中，“多个”是指，2～4个，“多点布置”是指，光流传感器要安装在飞行器的不同位置，典型位置是头部、中间和尾部；“距离要尽量远”是指，安装在头部或者尾部的光流传感器，在不影响其它机载设备的情况下，要尽量靠近机体的最前端或者最后端，这样就保证了头部和尾部光流传感器间的距离尽可能大些。Among them, "multiple" means 2 to 4, and "multi-point arrangement" means that the optical flow sensor should be installed in different positions of the aircraft, the typical positions are the head, middle and tail; "the distance should be as far as possible" means It means that the optical flow sensor installed on the head or tail should be as close as possible to the front end or rear end of the airframe without affecting other airborne equipment, so as to ensure the distance between the head and tail optical flow sensors as much as possible. Maybe bigger.

步骤三：根据各光流传感器在飞行器上的安装位置和方向，建立光流传感器的量测方程，利用飞行器上自带的惯导器件——速率陀螺，或者在飞行器上另外安装一个速率陀螺，建立速率陀螺的量测方程，与光流传感器的量测方程一起，构成系统的光流和惯导多传感器量测方程；Step 3: According to the installation position and direction of each optical flow sensor on the aircraft, establish the measurement equation of the optical flow sensor, use the inertial navigation device on the aircraft - the rate gyro, or install another rate gyro on the aircraft, Establish the measurement equation of the rate gyro, together with the measurement equation of the optical flow sensor, constitute the optical flow and inertial navigation multi-sensor measurement equation of the system;

第i个光流传感器的量测方程为：The measurement equation of the i-th optical flow sensor is:

式中，α为飞行器攻角(rad)，

为飞行器俯仰角(rad)，h为飞行器质心高度(m)，where α is the angle of attack of the aircraft (rad),

is the pitch angle of the aircraft (rad), h is the height of the center of mass of the aircraft (m),

V表示飞行器相对于地面的飞行速度(m/s)，

表示第i个光流传感器在飞行器上的安装角度(rad)，

表示飞行器俯仰角速度(rad/s)。V represents the flight speed of the aircraft relative to the ground (m/s),

Indicates the installation angle (rad) of the i-th optical flow sensor on the aircraft,

Indicates the aircraft pitch rate (rad/s).

而飞行器俯仰角速度

可由速率陀螺测出，故系统的光流和惯导多传感器量测方程为：while the aircraft pitch rate

It can be measured by the rate gyroscope, so the optical flow and inertial navigation multi-sensor measurement equation of the system is:

$Z Z = = {(\begin{matrix} \overset{\cdot &Center Dot;}{θ θ} & {f f}_{11} & {f f}_{22} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {f f}_{m m} \end{matrix})}^{T T} + + v v ((t t)) - - - - - - ((44))$

式中，

表示飞行器俯仰角速度(rad/s)，f_m表示第m个光流传感器的输出，v(t)为量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v^T(τ)]＝rδ(t-τ)，r为v(t)的方差强度阵，δ(t-τ)定义为：

δ (t - τ) = \{\begin{matrix} 1 & t = τ \\ 0 & t &NotEqual; τ \end{matrix} .

In the formula,

Indicates the pitch rate of the aircraft (rad/s), f _m indicates the output of the mth optical flow sensor, v(t) is the measurement noise, assuming that it is white noise with a mean value of 0, that is, E[v(t)]= 0, and E[v(t)v ^T (τ)]=rδ(t-τ), r is the variance intensity matrix of v(t), and δ(t-τ) is defined as:

δ (t - τ) = \{\begin{matrix} 1 & t = τ \\ 0 & t &NotEqual; τ \end{matrix} .

步骤四：分别选用EKF(Extend Kalman Filter，扩展卡尔曼滤波)法和UKF(UnscentedKalman Filter，无迹卡尔曼滤波)法对飞行器的飞行状态进行估计，对比两者的稳定性、快速性和准确性，并考虑飞行器上计算机的运算能力，选择一种合适的滤波方法，实现对飞行器的状态估计；Step 4: EKF (Extend Kalman Filter, Extended Kalman Filter) method and UKF (Unscented Kalman Filter, Unscented Kalman Filter) method are used to estimate the flight state of the aircraft, and the stability, speed and accuracy of the two are compared , and considering the computing power of the computer on the aircraft, choose an appropriate filtering method to realize the state estimation of the aircraft;

步骤五：利用估计的状态信息，实现飞行器的特定飞行任务。Step 5: Use the estimated state information to realize the specific flight mission of the aircraft.

3、优点及功效：本发明一种光流多传感器和惯导器件信息融合配置方法，其优点是：(1)测量元件体积小、重量轻、功耗低，便于在飞行器上布置、安装和使用；(2)测量元件不对外辐射电磁信号，有利于飞行器完成隐蔽性任务；(3)除飞行器的飞行速度、飞行高度、俯仰角速度外，本方法还可以估计出飞行器的攻角、俯仰角、升降速度等其它飞行信息。3. Advantages and effects: The present invention provides a method for information fusion and configuration of optical flow multi-sensors and inertial navigation devices. (2) The measuring element does not radiate electromagnetic signals to the outside, which is beneficial for the aircraft to complete concealed tasks; (3) In addition to the flight speed, flight altitude, and pitch angle velocity of the aircraft, this method can also estimate the attack angle and pitch angle of the aircraft. , lift speed and other flight information.

(四)附图说明： (4) Description of drawings:

图1是光流传感器测量关系图Figure 1 is a measurement relationship diagram of the optical flow sensor

图2是光流传感器与惯导器件的结合示意图Figure 2 is a schematic diagram of the combination of the optical flow sensor and the inertial navigation device

图3是本发明流程框图Fig. 3 is a flow chart of the present invention

图4是光流传感器在铅垂平面内的布置图Figure 4 is a layout diagram of the optical flow sensor in the vertical plane

图5a是EKF对攻角的估计效果示意图Figure 5a is a schematic diagram of the estimation effect of EKF on the angle of attack

图5b是EKF对俯仰角的估计效果示意图Figure 5b is a schematic diagram of the estimation effect of EKF on the pitch angle

图5c是EKF对俯仰角速度的估计效果示意图Figure 5c is a schematic diagram of the estimation effect of EKF on the pitch angular velocity

图5d是EKF对飞行高度的估计效果示意图Figure 5d is a schematic diagram of the effect of EKF on the estimation of flight altitude

图6a是无人机起伏运动时EKF对攻角的估计效果示意图Figure 6a is a schematic diagram of the estimation effect of EKF on the angle of attack when the UAV fluctuates

图6b是无人机起伏运动时EKF对俯仰角的估计效果示意图Figure 6b is a schematic diagram of the estimation effect of the EKF on the pitch angle when the UAV fluctuates

图6c是无人机起伏运动时EKF对俯仰角速度的估计效果示意图Figure 6c is a schematic diagram of the estimation effect of the EKF on the pitch angular velocity when the UAV fluctuates

图6d是无人机起伏运动时EKF对飞行高度的估计效果示意图Figure 6d is a schematic diagram of the estimation effect of EKF on the flight height when the UAV fluctuates

图7a是UKF对攻角的估计效果示意图Figure 7a is a schematic diagram of the estimation effect of UKF on the angle of attack

图7b是UKF对俯仰角的估计效果示意图Figure 7b is a schematic diagram of the estimation effect of the UKF on the pitch angle

图7c是UKF对俯仰角速度的估计效果示意图Figure 7c is a schematic diagram of the estimation effect of the UKF on the pitch angular velocity

图7d是UKF对飞行高度的估计效果示意图Figure 7d is a schematic diagram of the effect of UKF on the estimation of flight altitude

图8a是无人机起伏运动时UKF对攻角的估计效果示意图Figure 8a is a schematic diagram of the estimation effect of UKF on the angle of attack when the UAV fluctuates

图8b是无人机起伏运动时UKF对俯仰角的估计效果示意图Figure 8b is a schematic diagram of the estimation effect of the UKF on the pitch angle when the UAV fluctuates

图8c是无人机起伏运动时UKF对俯仰角速度的估计效果示意图Figure 8c is a schematic diagram of the estimation effect of the UKF on the pitch angular velocity during the undulating motion of the UAV

图8d是无人机起伏运动时UKF对飞行高度的估计效果示意图Figure 8d is a schematic diagram of the estimation effect of UKF on the flight height when the UAV fluctuates

图9是基于EKF的高度控制框图Figure 9 is a block diagram of height control based on EKF

图10是基于EKF的高度控制效果Figure 10 is the height control effect based on EKF

图中符号说明如下：The symbols in the figure are explained as follows:

图1中，v为光流传感器的水平速度(rad/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)。In Figure 1, v is the horizontal velocity of the optical flow sensor (rad/s), h is the height of the optical flow sensor from the ground (m), θ is the angle between the optical axis and the vertical direction (rad), and ω is the optical flow The rotational speed of the sensor (rad/s).

图4中，为机体俯仰角(rad)，h为机体质心的高度(m)，α为攻角(rad)，V为无人机相对于地面的速度(m/s)。d_i为第i个传感器相对于机体质心的安装位置(m)，

为第i个传感器相对于机体纵轴的安装角(rad)。Figure 4, is the body pitch angle (rad), h is the height of the center of mass of the body (m), α is the angle of attack (rad), and V is the speed of the UAV relative to the ground (m/s). d _i is the installation position (m) of the i-th sensor relative to the center of mass of the body,

is the installation angle (rad) of the i-th sensor relative to the longitudinal axis of the body.

图9中，H_c为指令高度(m)，为高度的估计值(m)，Δh为两者之差(m)，δ_Z为舵偏角(rad)，Z为传感器组的测量向量，

为状态向量的估计值。In Fig. 9, _Hc is command height (m), is the estimated value of the height (m), Δh is the difference between the two (m), δ _Z is the rudder deflection angle (rad), Z is the measurement vector of the sensor group,

is the estimated value of the state vector.

rad表示弧度，rad/s表示弧度每秒。rad means radians, and rad/s means radians per second.

(五)具体实施方式： (5) Specific implementation methods:

根据图1所示的光流传感器测量关系图和图2所示的光流传感器与惯导器件的结合示意图，我们提出了一种铅垂平面内光流传感器在飞行器上的布置方案和融合算法。光流传感器可以测得飞行器前方、下方、后方的光流信息，这些信息为全面估计飞行器所处的周边环境提供了依据。According to the measurement relationship diagram of the optical flow sensor shown in Figure 1 and the schematic diagram of the combination of the optical flow sensor and the inertial navigation device shown in Figure 2, we propose a layout scheme and fusion algorithm for the optical flow sensor on the aircraft in the vertical plane . The optical flow sensor can measure the optical flow information of the front, bottom and rear of the aircraft, which provides a basis for comprehensively estimating the surrounding environment of the aircraft.

为了降低问题的复杂程度，简化系统数学模型，现仅研究飞行器铅垂平面内的运动，并作出如下假设：In order to reduce the complexity of the problem and simplify the mathematical model of the system, only the movement in the vertical plane of the aircraft is studied, and the following assumptions are made:

1)飞行器周围环境的质地纹理是杂乱的，光流是可测的；1) The texture texture of the surrounding environment of the aircraft is messy, and the optical flow is measurable;

2)每个光流传感器都能正常工作，它们的输出含有量测噪声，但不存在完全错误的野值；2) Each optical flow sensor can work normally, and their output contains measurement noise, but there is no completely wrong outlier;

3)光流传感器的视场角很小，测得的信息为镜头轴线上的光流信息；3) The field of view of the optical flow sensor is very small, and the measured information is the optical flow information on the lens axis;

4)飞行器在做超低空巡航，仅在铅垂平面内运动，弹道倾角非常小；4) The aircraft is cruising at ultra-low altitude and only moves in the vertical plane, with a very small ballistic inclination;

5)“瞬时平衡”假设是成立的。5) The "instantaneous equilibrium" assumption is valid.

基于以上假设，见图3，本发明一种光流多传感器和惯导器件信息融合配置方法，该方法具体步骤如下：Based on the above assumptions, see Figure 3, a method for information fusion configuration of optical flow multi-sensors and inertial navigation devices according to the present invention, the specific steps of the method are as follows:

某型无人机机体铅垂平面内的线化扰动运动学方程为：The linearized disturbance kinematics equation in the vertical plane of a certain type of UAV body is:

$(\begin{matrix} \overset{\cdot &Center Dot;}{α α} \\ \overset{\cdot &Center Dot;}{θ θ} \\ \overset{\cdot &Center Dot; \cdot &Center Dot;}{θ θ} \\ \overset{\cdot &Center Dot;}{h h} \\ \overset{\cdot &Center Dot; \cdot &Center Dot;}{h h} \end{matrix}) = = (\begin{matrix} {a a}_{1111} & 00 & 11 & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 \\ {a a}_{3131} & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 \\ {a a}_{5151} & 00 & 00 & 00 & 00 \end{matrix}) (\begin{matrix} α α \\ θ θ \\ \overset{\cdot &Center Dot;}{θ θ} \\ h h \\ \overset{\cdot &Center Dot;}{h h} \end{matrix}) + + (\begin{matrix} {b b}_{1111} \\ 00 \\ {b b}_{3131} \\ 00 \\ {b b}_{5151} \end{matrix}) {δ δ}_{z z} + + ω ω ((t t)) - - - - - - ((55))$

式中，α为攻角(rad)，

为俯仰角(rad)，

表示飞行器俯仰角速度(rad/s)，

表示飞行器俯仰角加速度(rad/s²)，δz为舵偏角(rad)，h为机体质心高度(m)，

表示机体质心高度变化率(m/s)，

表示机体质心在铅垂方向上的加速度(m/s²)，ω(t)为白噪声过程，E[ω(t)]＝0，E[ω(t)ω^T(τ)]＝qδ(t-τ)，q为ω(t)的方差强度阵，δ(t-τ)定义为：

δ (t - τ) = \{\begin{matrix} 1 & t = τ \\ 0 & t &NotEqual; τ \end{matrix} .

where α is the angle of attack (rad),

is the pitch angle (rad),

Indicates the aircraft pitch rate (rad/s),

Indicates the pitching angular acceleration of the aircraft (rad/s ² ), δz is the rudder deflection angle (rad), h is the height of the center of mass of the aircraft body (m),

Indicates the height change rate of the center of mass of the body (m/s),

Indicates the acceleration of the center of mass of the body in the vertical direction (m/s ² ), ω(t) is a white noise process, E[ω(t)]=0, E[ω(t)ω ^T (τ)]= qδ(t-τ), q is the variance intensity matrix of ω(t), δ(t-τ) is defined as:

δ (t - τ) = \{\begin{matrix} 1 & t = τ \\ 0 & t &NotEqual; τ \end{matrix} .

根据某型无人机具体参数，可以解得a₁₁＝-2.083，a₃₁＝-6.019988，a₅₁＝83.1299，b₁₁＝-0.0175，b₃₁＝5.9245，b₅₁＝1.318。将以上数据代入式(5)并将其离散化，得到离散机体运动方程：According to the specific parameters of a certain type of drone, it can be solved that a ₁₁ =-2.083, a ₃₁ =-6.019988, a ₅₁ =83.1299, b ₁₁ =-0.0175, b ₃₁ =5.9245, b ₅₁ =1.318. Substituting the above data into formula (5) and discretizing it, the discrete body motion equation is obtained:

X_k＝ΦX_k-1+Bu_k-1+W_k-1 (6)X _k ＝ΦX _k-1 +Bu _k-1 +W _k-1 (6)

式中，In the formula,

${X x}_{k k} = = {(\begin{matrix} α α (({t t}_{k k})) & θ θ (({t t}_{k k})) & \overset{\cdot &Center Dot;}{θ θ} (({t t}_{k k})) & h h (({t t}_{k k})) & \overset{\cdot &Center Dot;}{h h} (({t t}_{k k})) \end{matrix})}^{T T},,$ $Φ Φ = = (\begin{matrix} 11 - - 2.083 2.083 {T T}_{s the s} & 00 & {T T}_{s the s} & 00 & 00 \\ 00 & 11 & {T T}_{s the s} & 00 & 00 \\ - - 6.019988 6.019988 {T T}_{s the s} & 00 & 11 & 00 & 00 \\ 00 & 00 & 00 & 11 & {T T}_{s the s} \\ 83.1299 83.1299 {T T}_{s the s} & 00 & 00 & 00 & 11 \end{matrix}),,$

B＝(-0.0175T_s 0 5.9245T_s 0 1.318T_s)^T，u_k＝δ_z(t_k)，W_k为系统激励噪声序列，且

T_s为采样周期，Q_k为系统噪声序列的方差阵。B＝(-0.0175T _s 0 5.9245T _s 0 1.318T _s ) ^T , u _k ＝δ _z (t _k ), W _k is the system excitation noise sequence, and

T _s is the sampling period, and Q _k is the variance matrix of the system noise sequence.

步骤二：将3个光流传感器多点布置在飞行器上，并指向不同方向；Step 2: Arrange 3 optical flow sensors on the aircraft at multiple points and point to different directions;

假设铅垂平面内安装m个即3个光流传感器，第i个传感器相对于机体质心的安装位置为d_i，相对于机体纵轴的安装角为

如图4所示。Assuming that m or 3 optical flow sensors are installed in the vertical plane, the installation position of the i-th sensor relative to the center of mass of the body is d _i , and the installation angle relative to the longitudinal axis of the body is

As shown in Figure 4.

图中，θ为机体俯仰角(rad)，h为机体质心的高度(m)，α为攻角(rad)，V为无人机相对于地面的速度(m/s)。d_i为第i个传感器相对于机体质心的安装位置(m)，

为第i个传感器相对于机体纵轴的安装角(rad)。In the figure, θ is the pitch angle of the aircraft (rad), h is the height of the center of mass of the aircraft (m), α is the angle of attack (rad), and V is the speed of the UAV relative to the ground (m/s). d _i is the installation position (m) of the i-th sensor relative to the center of mass of the body,

忽略机体直径，由图4中的几何关系可以得出第i个光流传感器的量测方程为：Neglecting the diameter of the body, the measurement equation of the i-th optical flow sensor can be obtained from the geometric relationship in Figure 4 as:

而机体俯仰角速度

可由弹载速率陀螺测出，故系统的光流和惯导多传感器量测方程为：while the body pitch angular velocity

It can be measured by the missile-loaded rate gyro, so the optical flow and inertial navigation multi-sensor measurement equation of the system is:

$Z Z = = {(\begin{matrix} \overset{\cdot &Center Dot;}{θ θ} & {f f}_{11} & {f f}_{22} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {f f}_{m m} \end{matrix})}^{T T} + + v v ((t t)) - - - - - - ((88))$

式中，f_m表示第m个光流传感器的输出，v(t)为量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v^T(τ)]＝rδ(t-τ)，r为v(t)的方差强度阵，δ(t-τ)定义为：In the formula, f _m represents the output of the mth optical flow sensor, v(t) is the measurement noise, assuming that it is white noise with a mean value of 0, that is, E[v(t)]=0, and E[v( t)v ^T (τ)]=rδ(t-τ), r is the variance intensity matrix of v(t), and δ(t-τ) is defined as:

$δ δ ((t t - - τ τ)) = = \{\begin{matrix} 11 & t t = = τ τ \\ 00 & t t &NotEqual; &NotEqual; τ τ \end{matrix} . .$

步骤四：分别选用EKF法和UKF法对飞行器的飞行状态进行估计，对比两者的稳定性、快速性和准确性，并考虑飞行器上计算机的运算能力，选择一种合适的滤波方法，实现对飞行器的状态估计；Step 4: Choose the EKF method and the UKF method to estimate the flight state of the aircraft respectively, compare the stability, rapidity and accuracy of the two, and consider the computing power of the computer on the aircraft, choose an appropriate filtering method to realize the state estimation of the aircraft;

EKF法：EKF method:

对量测方程(8)进行线化和离散化处理，并初始条件设置如下：驱动噪声强度阵 $q = (\begin{matrix} 0.01 & 0 & 0 & 0 & 0 \\ 0 & 0.01 & 0 & 0 & 0 \\ 0 & 0 & 0.01 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0.1 \end{matrix});$ 量测噪声强度阵 $r = (\begin{matrix} 0.01 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0.1 \end{matrix});$ $P_{0} = (\begin{matrix} 0.1 & 0 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 & 0 \\ 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0.1 \end{matrix});$ ${\hat{X}}_{0} = {(\begin{matrix} 0 & 0 & 0 & 10 & 0 \end{matrix})}^{T};$ 无人机速度大小V＝200m/s；采样周期T_s＝0.01s，机体上安装3个光流传感器，安装角分别为

和

安装位置d_i分别为0.5m，0m和-0.5m。Linearize and discretize the measurement equation (8), and set the initial conditions as follows: driving noise intensity matrix

q = (\begin{matrix} 0.01 & 0 & 0 & 0 & 0 \\ 0 & 0.01 & 0 & 0 & 0 \\ 0 & 0 & 0.01 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0.1 \end{matrix});

Measurement Noise Intensity Array

r = (\begin{matrix} 0.01 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0.1 \end{matrix});

P_{0} = (\begin{matrix} 0.1 & 0 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 & 0 \\ 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0.1 \end{matrix});

{\hat{x}}_{0} = {(\begin{matrix} 0 & 0 & 0 & 10 & 0 \end{matrix})}^{T};

The speed of the UAV is V=200m/s; the sampling period T _s =0.01s, three optical flow sensors are installed on the body, and the installation angle respectively

and

The installation positions d _i are 0.5m, 0m and -0.5m respectively.

假设控制量始终为零，即舵偏角δ_z≡0，得到的仿真结果如图5a-图5d所示。Assuming that the control variable is always zero, that is, the rudder deflection angle δ _z ≡ 0, the obtained simulation results are shown in Fig. 5a-Fig. 5d.

图5a-图5d表明，EKF可以融合安装在机体的不同位置的多个光流传感器和一个速率陀螺的信息，实现对无人机攻角、俯仰角速度、飞行高度和高度变化率的估计，对俯仰角速度的估计偏差相对比较大，但俯仰角速度是由速率陀螺直接测量给出的，之所以出现较大偏差，是由于速率陀螺的测量噪声太大，可以通过提高陀螺的测量精度来提高俯仰角速度的估计精度。Figures 5a-5d show that EKF can fuse the information of multiple optical flow sensors and a rate gyroscope installed in different positions of the airframe to realize the estimation of UAV angle of attack, pitch angle velocity, flight altitude and altitude change rate. The estimated deviation of the pitch angular velocity is relatively large, but the pitch angular velocity is directly measured by the rate gyro. The reason for the large deviation is that the measurement noise of the rate gyro is too large. The pitch angular velocity can be improved by improving the measurement accuracy of the gyro The estimated accuracy of .

现在令控制量δ_z＝0.2sin(πt+π/2)，使无人机在铅垂平面内做起伏运动，测试估计值对真实值的跟踪性能，得到的仿真结果如图6a-图6d所示。图6a-图6d表明，EKF不但可以准确地估计系统的状态变量，其算法的实时性能也非常好，估计值相对于真实值来讲，几乎没有滞后，这一点保证了EKF可以在实际工程中得到实时的应用。Now let the control quantity δ _z = 0.2sin(πt+π/2), make the UAV do undulating motion in the vertical plane, and test the tracking performance of the estimated value to the real value. The obtained simulation results are shown in Figure 6a-Figure 6d shown. Figures 6a-6d show that EKF can not only accurately estimate the state variables of the system, but the real-time performance of the algorithm is also very good. Compared with the real value, the estimated value has almost no lag, which ensures that EKF can be used in actual engineering Get real-time applications.

UKF法：UKF method:

根据UKF算法条件，只对量测方程(8)进行离散化处理即可。由于系统状态维数和系统噪声维数均为5，量测方程的维数为4，故增广状态向量的维数为L＝5+5+4＝14，Sigma点的采样策略选用对称采样，其个数为2L+1＝29。According to the conditions of the UKF algorithm, only the measurement equation (8) needs to be discretized. Since the system state dimension and system noise dimension are both 5, and the dimension of the measurement equation is 4, the dimension of the augmented state vector is L=5+5+4=14, and the sampling strategy of the Sigma point is symmetrical sampling , and its number is 2L+1=29.

初始条件设置如下：Sigma点采样时，比例参数取0.5； $P_{0} = (\begin{matrix} 0.1 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0.1 \end{matrix}),$ 此处不取P₀＝0，是为了防止下一步求(n+λ)P_x的平方根时进行Cholesky分解产生奇异；其它初始条件与EKF法中相同，不再一一列出。The initial conditions are set as follows: when the Sigma point is sampled, the ratio parameter is set to 0.5; $P_{0} = (\begin{matrix} 0.1 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0.1 \end{matrix}),$ The purpose of not taking P ₀ =0 here is to prevent the singularity caused by Cholesky decomposition when calculating the square root of (n+λ)P _x in the next step; other initial conditions are the same as those in the EKF method and will not be listed one by one.

假设控制量始终为零，即舵偏角δ_z≡0，得到的仿真结果如图7a-图7d所示。Assuming that the control variable is always zero, that is, the rudder deflection angle δ _z ≡ 0, the obtained simulation results are shown in Fig. 7a-7d.

图7a-图7d表明，跟EKF一样，UKF也可以对系统状态进行准确的估计。在估计的初期，UKF收敛比较慢，并且有比较大的震荡；UKF收敛之后，其估计性能与EKF相当，对系统的状态估计与EKF的估计值几乎完全一致。Figures 7a-7d show that, like EKF, UKF can also accurately estimate the system state. In the early stage of estimation, UKF converges slowly and has relatively large shocks; after UKF converges, its estimation performance is equivalent to that of EKF, and the state estimation of the system is almost identical to the estimated value of EKF.

现在令控制量δ_z＝0.2sin(πt+π/2)，使无人机在铅垂平面内做起伏运动，测试估计值对真实值的跟踪性能，得到的仿真结果如图8a-图8d所示。UKF几乎是无滞后、无偏差地、完美地跟踪了实际状态变量的变化。Now let the control quantity δ _z = 0.2sin(πt+π/2), make the UAV do undulating motion in the vertical plane, and test the tracking performance of the estimated value to the real value. The obtained simulation results are shown in Figure 8a-Figure 8d shown. The UKF perfectly tracks the changes of the actual state variables almost without lag and without bias.

在本算例中，UKF的计算量要比EKF多得多，当仿真步长设置为0.01秒时，对于12秒的仿真时间，UKF法需要耗时3.93秒，而EKF仅需0.48秒，前者是后者的8倍多，这是因为量测方程结构形式复杂，需要占用很多CPU时间，在每一步估计循环中，EKF都只需要计算1次量测方程的导数即可，而UKF则需要计算29次量测方程，因为共有29个Sigma点要计算，这导致了UKF的计算量要远大于EKF的计算量。In this calculation example, the calculation amount of UKF is much more than that of EKF. When the simulation step size is set to 0.01 seconds, for a simulation time of 12 seconds, the UKF method takes 3.93 seconds, while the EKF method only takes 0.48 seconds. The former It is more than 8 times that of the latter. This is because the measurement equation has a complex structure and takes up a lot of CPU time. In each step of the estimation cycle, EKF only needs to calculate the derivative of the measurement equation once, while UKF requires Calculate the measurement equation of 29 times, because there are 29 Sigma points to be calculated, which leads to the calculation amount of UKF being much larger than that of EKF.

步骤五：利用估计的状态信息，实现飞行器的特定飞行任务；Step five: use the estimated state information to realize the specific flight mission of the aircraft;

本算例中，UKF在估计精度上与EKF相当，但由于量测方程的复杂性，导致UKF的实时性要远低于EKF，所以本节将使用EKF作为信息融合算法，估计无人机的状态信息，实现铅垂平面内的高度控制，以期证明光流传感器在超低空突防中的应用价值。In this calculation example, UKF is comparable to EKF in terms of estimation accuracy, but due to the complexity of the measurement equation, the real-time performance of UKF is much lower than that of EKF, so this section will use EKF as an information fusion algorithm to estimate the UAV’s State information, to achieve height control in the vertical plane, in order to prove the application value of optical flow sensor in ultra-low altitude penetration.

假设驱动噪声为0，将式(5)简写成标准的状态空间形式：Assuming that the driving noise is 0, formula (5) is abbreviated into the standard state space form:

$\overset{\cdot &Center Dot;}{X x} = = AX AX + + Bu Bu - - - - - - ((99))$

rank[B AB A²B A³B A⁴B]＝4＜5，系统不完全可控。经计算，系统存在三个0极点，为使系统稳定或渐近稳定，需要设计状态反馈阵，将系统的极点配置到s平面的左半平面，实现系统的镇定。rank[B AB A ² B A ³ B A ⁴ B]=4<5, the system is not fully controllable. After calculation, there are three 0 poles in the system. In order to make the system stable or asymptotically stable, it is necessary to design a state feedback array and arrange the poles of the system to the left half plane of the s-plane to realize the stability of the system.

首先，通过线性变换将系统按能控性分解为First, the system is decomposed according to controllability by linear transformation into

$\{\begin{matrix} \overset{~ ~}{A A} = = {TAT TAT}^{' '} = = (\begin{matrix} {A A}_{nc nc} & 00 \\ {A A}_{21 twenty one} & {A A}_{c c} \end{matrix}) \\ \overset{~ ~}{B B} = = TB TB = = (\begin{matrix} 00 \\ {B B}_{c c} \end{matrix}) \\ \overset{~ ~}{C C} = = {CT CT}^{' '} = = (\begin{matrix} {C C}_{nc nc} & {C C}_{c c} \end{matrix}) \end{matrix} - - - - - - ((1010))$

式中，T为正交变换阵，下标nc表示不可控，c表示可控。此分解可由Matlab中的ctrbf命令完成。In the formula, T is an orthogonal transformation matrix, the subscript nc means uncontrollable, and c means controllable. This decomposition can be done by the ctrbf command in Matlab.

经计算，完全不能控子系统<A_nc 0 C_nc>是渐近稳定的。于是，原系统是可以通过线性状态反馈镇定下来的。下面，对完全能控子系统<A_c B_c C_c>进行极点配置。It is calculated that the completely uncontrollable subsystem _{_{<A nc 0 C nc >}} is asymptotically stable. Therefore, the original system can be stabilized by linear state feedback. Next, configure the poles of the fully controllable subsystem _{_{_{<A c B c C c >}}} .

任取四个期望极点为[-7，-5，-2-i，-2+i]，计算得状态反馈阵增益矩阵K_c＝[0.6267 -0.8319 6.2029 2.293]，于是可使原系统镇定下来的状态反馈矩阵为K＝(0 K_c)T＝(4.4340 4.4353 2.2980 0.3497 0.2884)Take any four desired poles as [-7, -5, -2-i, -2+i], and calculate the state feedback array gain matrix K _c = [0.6267 -0.8319 6.2029 2.293], so the original system can be stabilized The state feedback matrix of is K＝(0 K _c )T＝(4.4340 4.4353 2.2980 0.3497 0.2884)

以EKF对状态的估计值作为状态反馈，搭建无人机高度控制模型如图9所示。图中H_c为指令高度(m)，

为高度的估计值(m)，Δh为两者之差(m)，δ_Z为舵偏角(rad)，Z为传感器组的测量向量，

为状态向量的估计值。Taking the estimated value of the state of the EKF as the state feedback, the height control model of the UAV is built, as shown in Figure 9. H _c in the figure is the command height (m),

is the estimated value of the height (m), Δh is the difference between the two (m), δ _Z is the rudder deflection angle (rad), Z is the measurement vector of the sensor group,

is the estimated value of the state vector.

高度控制器可设计成一个PI控制器，其中比例系数K_p＝0.8，积分系数K_i＝0.4，舵偏角限制在±30°之内，初始指令高度为10m，第5s时刻将指令高度按指数规律下调到5m，时间常数为2s，仿真结果如图10所示。无人机可以无超调、无静差地跟踪高度指令，进行安全地超低空飞行，高度最终可以控制在期望高度的±0.5m之内，它证明了高度控制器的有效性，也同时证明了前文中传感器的布置、EKF算法的可行性。The height controller can be designed as a PI controller, where the proportional coefficient K _p =0.8, the integral coefficient K _i =0.4, the rudder deflection angle is limited within ±30°, the initial command height is 10m, and the command height is set at the 5th moment by The exponential law is adjusted down to 5m, and the time constant is 2s. The simulation results are shown in Figure 10. The UAV can track the altitude command without overshoot and static error, and fly safely at ultra-low altitude. The altitude can be controlled within ±0.5m of the expected altitude. It proves the effectiveness of the altitude controller and also proves The layout of the sensors and the feasibility of the EKF algorithm in the previous article are verified.

Claims

1. An optical flow multi-sensor and inertial navigation device information fusion configuration method, the specific steps of the method are as follows:

Step 1: For the aircraft that needs to install the optical flow sensor, establish its linear disturbance kinematics equation in the vertical plane;

(\begin{matrix} \overset{\cdot &Center Dot;}{α α} \\ \overset{\cdot \cdot}{θ θ} \\ \overset{\cdot \cdot \cdot &Center Dot;}{θ θ} \\ \overset{\cdot &Center Dot;}{h h} \\ \overset{\cdot &Center Dot; \cdot &Center Dot;}{h h} \end{matrix}) = = (\begin{matrix} {a a}_{1111} & 00 & 11 & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 \\ {a a}_{3131} & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 \\ {a a}_{5151} & 00 & 00 & 00 & 00 \end{matrix}) (\begin{matrix} α α \\ θ θ \\ \overset{\cdot &Center Dot;}{θ θ} \\ h h \\ \overset{\cdot &Center Dot;}{h h} \end{matrix}) + + (\begin{matrix} {b b}_{1111} \\ 00 \\ {b b}_{3131} \\ 00 \\ {b b}_{5151} \end{matrix}) {δ δ}_{z z} + + ω ω ((t t)) - - - - - - ((11))

In the formula, α is the attack angle of the aircraft, θ is the pitch angle of the aircraft, δ _z is the rudder deflection angle of the aircraft, h is the height of the center of mass of the aircraft, a ₁₁ , a ₃₁ , a ₅₁ , b ₁₁ , b ₃₁ , and b ₅₁ are constant coefficients, They are related to the aerodynamic and mass characteristics of the aircraft, ω(t) is a white noise process, E[ω(t)]=0, E[ω(t)ω ^T (τ)]=qδ(t-τ), q is the variance intensity matrix of ω(t);

Step 2: Arrange multiple optical flow sensors on the aircraft at multiple points. If the space permits, the distance between the sensors should be as far as possible and point to different directions. This will help improve the subsequent estimation accuracy;

Step 3: According to the installation position and direction of each optical flow sensor on the aircraft, establish the measurement equation of the optical flow sensor, use the inertial navigation device on the aircraft - the rate gyro, or install another rate gyro on the aircraft, Establish the measurement equation of the rate gyro, together with the measurement equation of the optical flow sensor, constitute the optical flow and inertial navigation multi-sensor measurement equation of the system;

The measurement equation of the i-th optical flow sensor is:

In the formula, α is the angle of attack of the aircraft,

is the pitch angle of the aircraft, h is the height of the center of mass of the aircraft, V represents the flying speed of the aircraft relative to the ground,

Indicates the installation angle of the i-th optical flow sensor on the aircraft,

Indicates the pitching angular velocity of the aircraft;

while the aircraft pitch rate

It is measured by the rate gyro, so the optical flow and inertial navigation multi-sensor measurement equation of the system is:

Z Z = = {(\begin{matrix} \overset{\cdot \cdot}{θ θ} & {f f}_{11} & {f f}_{22} & \cdot \cdot \cdot &Center Dot; \cdot \cdot & {f f}_{m m} \end{matrix})}^{T T} + + v v ((t t)) - - - - - - ((33))

In the formula,

Indicates the pitch rate of the aircraft, f _m indicates the output of the mth optical flow sensor, v(t) is the measurement noise, assuming that it is white noise with a mean value of 0, that is, E[v(t)]=0, and E[ v(t)v ^T (τ)]=rδ(t-τ), r is the variance intensity matrix of v(t), and δ(t-τ) is defined as:

δ (t - τ) = \{\begin{matrix} 1 & t = τ \\ 0 & t &NotEqual; τ \end{matrix};

Step 4: Choose the EKF method (extended Kalman filter) and UKF method (unscented Kalman filter) to estimate the flight state of the aircraft, compare the stability, speed and accuracy of the two, and consider the computing power of the computer on the aircraft , select a filtering method to realize the state estimation of the aircraft;

Step 5: Use the estimated state information to realize the specific flight mission of the aircraft.

2. A method for information fusion configuration of optical flow multi-sensors and inertial navigation devices according to claim 1, characterized in that the number of "multiple optical flow sensors" described in step 2 is 2 to 4.

3. A kind of optical flow multi-sensor and inertial navigation device information fusion configuration method according to claim 1, characterized in that: the "multi-point arrangement" described in step 2 means that the optical flow sensor will be installed on different parts of the aircraft. Positions, typical positions are head, middle and tail.

4. A method for information fusion configuration of optical flow multi-sensors and inertial navigation devices according to claim 1, characterized in that: "the distance should be as far as possible" described in step 2 refers to the light installed at the head or tail The flow sensor should be close to the front end or the rear end of the body without affecting other airborne equipment, so that the distance between the head and tail optical flow sensors is as large as possible.