一种光流多传感器和惯导器件信息融合配置方法

1.一种光流多传感器和惯导器件信息融合配置方法，该方法具体步骤如下：
步骤一：针对需要安装光流传感器的飞行器，在铅垂平面内建立其线化扰动运动学方程；
式中，α为飞行器攻角，为飞行器俯仰角，δz为飞行器舵偏角，h为飞行器质心高度，a11、a31、a51、b11、b31、b51为常系数，它们与飞行器的气动特性和质量特性有关，ω(t)为白噪声过程，E[ω(t)]=0，E[ω(t)ωT(τ)]=qδ(t-τ)，q为ω(t)的方差强度阵；
步骤二：将多个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；
步骤三：根据各光流传感器在飞行器上的安装位置和方向，建立光流传感器的量测方程，利用飞行器上自带的惯导器件——速率陀螺，或者在飞行器上另外安装一个速率陀螺，建立速率陀螺的量测方程，与光流传感器的量测方程一起，构成系统的光流和惯导多传感器量测方程；
第i个光流传感器的量测方程为：
式中，α为飞行器攻角，为飞行器俯仰角，h为飞行器质心高度，
V表示飞行器相对于地面的飞行速度，表示第i个光流传感器在飞行器上的安装角度，表示飞行器俯仰角速度，di为第i个传感器相对于机体质心的安装位置；
而飞行器俯仰角速度 由速率陀螺测出，故系统的光流和惯导多传感器量测方程为：
式中， 表示飞行器俯仰角速度,fm表示第m个光流传感器的输出，v(t)为量测噪声，T
假设其为均值为0的白噪声，即E[v(t)]=0，且E[v(t)v(τ)]=rδ(t-τ)，r为v(t)的方差强度阵，δ(t-τ)定义为：
步骤四：分别选用EKF法即扩展卡尔曼滤波和UKF法即无迹卡尔曼滤波对飞行器的飞行状态进行估计，对比两者的稳定性、快速性和准确性，并考虑飞行器上计算机的运算能力，选择一种滤波方法，实现对飞行器的状态估计；
步骤五：利用估计的状态信息，实现飞行器的特定飞行任务。
2.根据权利要求1所述的一种光流多传感器和惯导器件信息融合配置方法，其特征在于：步骤二中所述的“多个光流传感器”，其数量为2～4个。
3.根据权利要求1所述的一种光流多传感器和惯导器件信息融合配置方法，其特征在于：步骤二中所述的“多点布置”是指光流传感器要安装在飞行器的不同位置，是头部、中间和尾部。
4.根据权利要求1所述的一种光流多传感器和惯导器件信息融合配置方法，其特征在于：步骤二中所述的“距离要尽量远”是指安装在头部或者尾部的光流传感器，在不影响其它机载设备的情况下，要靠近机体的最前端或者最后端，这样就保证了头部和尾部光流传感器间的距离尽可能大些。

一种光流多传感器和惯导器件信息融合配置方法\n(一)技术领域：\n[0001] 本发明涉及一种光流多传感器和惯导器件信息融合配置方法，属于飞行器姿态、飞行速度和高度的测量与估计技术领域。\n(二)背景技术：\n[0002] 飞行器主要依靠前视雷达、雷达高度表和升降速度表来测量离地高度和升降速度，而对于小型飞行器来说，激光测距仪(Laser Rangefinders，LRF)和雷达都显得过于笨重。SICKLMS291是一款典型的激光测距仪，一般用于机器人领域，它的质量大约是4.5公斤。用于无人驾驶航空器(Unmanned Aerial Vehicle，UAV)上的最小的合成孔径雷达可能是美国圣地亚实验室(Sandia National Labs)制造的miniSAR，其质量约为4～5公斤，这么重的设备增加了无人机的重量和体积，降低了其续航能力和带载能力。\n[0003] 光流传感器质量小，只有10克左右，完全被动地接收外部光线，无辐射，相对于雷达高度计具有质量小、隐蔽性好的优点，除飞行器的飞行速度、飞行高度、俯仰角速度外，本方法还可以估计出飞行器的攻角、俯仰角、升降速度等其它飞行信息，这些信息有利于帮助无人机完成探测、救灾等特定飞行任务。\n(三)发明内容：\n[0004] 1、目的：本发明的目的是提供一种光流多传感器和惯导器件信息融合配置方法，它使用2～4个光流传感器和一个惯导器件，重量轻、体积小、功耗低，便于在小型飞行器上安装布置，不对外辐射电磁信号，提高了飞行器的隐蔽性。\n[0005] 2、技术方案：昆虫在移动时，周围环境的亮度模式在视网膜上形成一系列连续变化的图像，这一系列连续变化的信息不断“流过”视网膜，好像是一种光的“流”，故称这种图像亮度模式的表观运动为光流。国外的某些实验室，已经研制出了光流传感器的物理样机，并利用光流传感器实现了无人驾驶飞行器的自主避障、等高飞行、自动着陆、风速估计、目标检测和空中悬停，这些技术在探测、救灾等方面将有非常重要的应用价值。根据光流的定义和图1中所示的几何关系，可得出光流的表达式为：\n[0006] \n[0007] 式中，f为光流(1/s)，v为光流传感器的水平速度(m/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)。\n[0008] 由式(1)可以看出，光流的测量值与光流传感器的姿态、高度和速度耦合，同时光流传感器具有体积小、重量轻、功耗低、可组网的特点，于是考虑将多个光流传感器固联在飞行器上，结合惯导器件——速率陀螺，利用多传感器的信息融合技术，实现对飞行器姿态信息的估计，并利用估计信息实现飞行器的超低空突防任务。光流传感器与惯导器件的结合布置如图2所示。\n[0009] 本发明一种光流多传感器和惯导器件信息融合配置方法，该方法具体步骤如下：\n[0010] 步骤一：针对需要安装光流传感器的飞行器，在铅垂平面内建立其线化扰动运动学方程；\n[0011] \n[0012] 式中，α为飞行器攻角(rad)，为飞行器俯仰角(rad)，δz为飞行器舵偏角(rad)，h为飞行器质心高度(m)，a11、a31、a51、b11、b31、b51为常系数，它们与飞行器的气动特T\n性和质量特性有关，ω(t)为白噪声过程，E[ω(t)]＝0，E[ω(t)ω(τ)]＝qδ(t-τ)，q为ω(t)的方差强度阵。\n[0013] 步骤二：将多个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；\n[0014] 其中，“多个”是指，2～4个，“多点布置”是指，光流传感器要安装在飞行器的不同位置，典型位置是头部、中间和尾部；“距离要尽量远”是指，安装在头部或者尾部的光流传感器，在不影响其它机载设备的情况下，要尽量靠近机体的最前端或者最后端，这样就保证了头部和尾部光流传感器间的距离尽可能大些。\n[0015] 步骤三：根据各光流传感器在飞行器上的安装位置和方向，建立光流传感器的量测方程，利用飞行器上自带的惯导器件——速率陀螺，或者在飞行器上另外安装一个速率陀螺，建立速率陀螺的量测方程，与光流传感器的量测方程一起，构成系统的光流和惯导多传感器量测方程；\n[0016] 第i个光流传感器的量测方程为：\n[0017] \n[0018] 式中，α为飞行器攻角(rad)，为飞行器俯仰角(rad)，h为飞行器质心高度(m)，[0019] V表示飞行器相对于地面的飞行速度(m/s)，表示第i个光流传感器在飞行器上的安装角度(rad)，表示飞行器俯仰角速度(rad/s)。\n[0020] 而飞行器俯仰角速度 可由速率陀螺测出，故系统的光流和惯导多传感器量测方程为：\n[0021] \n[0022] 式中，表示飞行器俯仰角速度(rad/s)，fm表示第m个光流传感器的输出，v(t)为T\n量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v(τ)]＝rδ(t-τ)，r为v(t)的方差强度阵，δ(t-τ)定义为：\n[0023] 步骤四：分别选用EKF(Extend Kalman Filter，扩展卡尔曼滤波)法和UKF(UnscentedKalman Filter，无迹卡尔曼滤波)法对飞行器的飞行状态进行估计，对比两者的稳定性、快速性和准确性，并考虑飞行器上计算机的运算能力，选择一种合适的滤波方法，实现对飞行器的状态估计；\n[0024] 步骤五：利用估计的状态信息，实现飞行器的特定飞行任务。\n[0025] 3、优点及功效：本发明一种光流多传感器和惯导器件信息融合配置方法，其优点是：(1)测量元件体积小、重量轻、功耗低，便于在飞行器上布置、安装和使用；(2)测量元件不对外辐射电磁信号，有利于飞行器完成隐蔽性任务；(3)除飞行器的飞行速度、飞行高度、俯仰角速度外，本方法还可以估计出飞行器的攻角、俯仰角、升降速度等其它飞行信息。\n(四)附图说明：\n[0026] 图1是光流传感器测量关系图\n[0027] 图2是光流传感器与惯导器件的结合示意图\n[0028] 图3是本发明流程框图\n[0029] 图4是光流传感器在铅垂平面内的布置图\n[0030] 图5a是EKF对攻角的估计效果示意图\n[0031] 图5b是EKF对俯仰角的估计效果示意图\n[0032] 图5c是EKF对俯仰角速度的估计效果示意图\n[0033] 图5d是EKF对飞行高度的估计效果示意图\n[0034] 图6a是无人机起伏运动时EKF对攻角的估计效果示意图\n[0035] 图6b是无人机起伏运动时EKF对俯仰角的估计效果示意图\n[0036] 图6c是无人机起伏运动时EKF对俯仰角速度的估计效果示意图\n[0037] 图6d是无人机起伏运动时EKF对飞行高度的估计效果示意图\n[0038] 图7a是UKF对攻角的估计效果示意图\n[0039] 图7b是UKF对俯仰角的估计效果示意图\n[0040] 图7c是UKF对俯仰角速度的估计效果示意图\n[0041] 图7d是UKF对飞行高度的估计效果示意图\n[0042] 图8a是无人机起伏运动时UKF对攻角的估计效果示意图\n[0043] 图8b是无人机起伏运动时UKF对俯仰角的估计效果示意图\n[0044] 图8c是无人机起伏运动时UKF对俯仰角速度的估计效果示意图\n[0045] 图8d是无人机起伏运动时UKF对飞行高度的估计效果示意图\n[0046] 图9是基于EKF的高度控制框图\n[0047] 图10是基于EKF的高度控制效果\n[0048] 图中符号说明如下：\n[0049] 图1中，v为光流传感器的水平速度(rad/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)。\n[0050] 图4中，为机体俯仰角(rad)，h为机体质心的高度(m)，α为攻角(rad)，V为无人机相对于地面的速度(m/s)。di为第i个传感器相对于机体质心的安装位置(m)，为第i个传感器相对于机体纵轴的安装角(rad)。\n[0051] 图9中，Hc为指令高度(m)，为高度的估计值(m)，Δh为两者之差(m)，δZ为舵偏角(rad)，Z为传感器组的测量向量，为状态向量的估计值。\n[0052] rad表示弧度，rad/s表示弧度每秒。\n(五)具体实施方式：\n[0053] 根据图1所示的光流传感器测量关系图和图2所示的光流传感器与惯导器件的结合示意图，我们提出了一种铅垂平面内光流传感器在飞行器上的布置方案和融合算法。光流传感器可以测得飞行器前方、下方、后方的光流信息，这些信息为全面估计飞行器所处的周边环境提供了依据。\n[0054] 为了降低问题的复杂程度，简化系统数学模型，现仅研究飞行器铅垂平面内的运动，并作出如下假设：\n[0055] 1)飞行器周围环境的质地纹理是杂乱的，光流是可测的；\n[0056] 2)每个光流传感器都能正常工作，它们的输出含有量测噪声，但不存在完全错误的野值；\n[0057] 3)光流传感器的视场角很小，测得的信息为镜头轴线上的光流信息；\n[0058] 4)飞行器在做超低空巡航，仅在铅垂平面内运动，弹道倾角非常小；\n[0059] 5)“瞬时平衡”假设是成立的。\n[0060] 基于以上假设，见图3，本发明一种光流多传感器和惯导器件信息融合配置方法，该方法具体步骤如下：\n[0061] 步骤一：针对需要安装光流传感器的飞行器，在铅垂平面内建立其线化扰动运动学方程；\n[0062] 某型无人机机体铅垂平面内的线化扰动运动学方程为：\n[0063] \n[0064] 式中，α为攻角(rad)，为俯仰角(rad)，表示飞行器俯仰角速度(rad/s)，表示\n2\n飞行器俯仰角加速度(rad/s)，δz为舵偏角(rad)，h为机体质心高度(m)，表示机体质\n2\n心高度变化率(m/s)，表示机体质心在铅垂方向上的加速度(m/s)，ω(t)为白噪声过程，T\nE[ω(t)]＝0，E[ω(t)ω(τ)]＝qδ(t-τ)，q为ω(t)的方差强度阵，δ(t-τ)定义为：\n[0065] 根据某型无人机具体参数，可以解得a11＝-2.083，a31＝-6.019988，a51＝\n83.1299，b11＝-0.0175，b31＝5.9245，b51＝1.318。将以上数据代入式(5)并将其离散化，得到离散机体运动方程：\n[0066] Xk＝ΦXk-1+Buk-1+Wk-1 (6)\n[0067] 式中，\n[0068] \n[0069] B＝(-0.0175Ts 0 5.9245Ts 0 1.318Ts)T，uk＝δz(tk)，Wk为系统激励噪声序列，且Ts为采样周期，Qk为系统噪声序列的方差阵。\n[0070] 步骤二：将3个光流传感器多点布置在飞行器上，并指向不同方向；\n[0071] 假设铅垂平面内安装m个即3个光流传感器，第i个传感器相对于机体质心的安装位置为di，相对于机体纵轴的安装角为 如图4所示。\n[0072] 图中，θ为机体俯仰角(rad)，h为机体质心的高度(m)，α为攻角(rad)，V为无人机相对于地面的速度(m/s)。di为第i个传感器相对于机体质心的安装位置(m)，为第i个传感器相对于机体纵轴的安装角(rad)。\n[0073] 步骤三：根据各光流传感器在飞行器上的安装位置和方向，建立光流传感器的量测方程，利用飞行器上自带的惯导器件——速率陀螺，或者在飞行器上另外安装一个速率陀螺，建立速率陀螺的量测方程，与光流传感器的量测方程一起，构成系统的光流和惯导多传感器量测方程；\n[0074] 忽略机体直径，由图4中的几何关系可以得出第i个光流传感器的量测方程为：\n[0075] \n[0076] 而机体俯仰角速度 可由弹载速率陀螺测出，故系统的光流和惯导多传感器量测方程为：\n[0077] \n[0078] 式中，fm表示第m个光流传感器的输出，v(t)为量测噪声，假设其为均值为0的白T\n噪声，即E[v(t)]＝0，且E[v(t)v(τ)]＝rδ(t-τ)，r为v(t)的方差强度阵，δ(t-τ)定义为：\n[0079] \n[0080] 步骤四：分别选用EKF法和UKF法对飞行器的飞行状态进行估计，对比两者的稳定性、快速性和准确性，并考虑飞行器上计算机的运算能力，选择一种合适的滤波方法，实现对飞行器的状态估计；\n[0081] EKF法：\n[0082] 对量测方程(8)进行线化和离散化处理，并初始条件设置如下：驱动噪声强度 阵 量测 噪 声强 度阵\n 无人机速度大小V＝200m/s；采样周期\nTs＝0.01s，机体上安装3个光流传感器，安装角 分别为 和 安装位置di分别为\n0.5m，0m和-0.5m。\n[0083] 假设控制量始终为零，即舵偏角δz≡0，得到的仿真结果如图5a-图5d所示。\n[0084] 图5a-图5d表明，EKF可以融合安装在机体的不同位置的多个光流传感器和一个速率陀螺的信息，实现对无人机攻角、俯仰角速度、飞行高度和高度变化率的估计，对俯仰角速度的估计偏差相对比较大，但俯仰角速度是由速率陀螺直接测量给出的，之所以出现较大偏差，是由于速率陀螺的测量噪声太大，可以通过提高陀螺的测量精度来提高俯仰角速度的估计精度。\n[0085] 现在令控制量δz＝0.2sin(πt+π/2)，使无人机在铅垂平面内做起伏运动，测试估计值对真实值的跟踪性能，得到的仿真结果如图6a-图6d所示。图6a-图6d表明，EKF不但可以准确地估计系统的状态变量，其算法的实时性能也非常好，估计值相对于真实值来讲，几乎没有滞后，这一点保证了EKF可以在实际工程中得到实时的应用。\n[0086] UKF法：\n[0087] 根据UKF算法条件，只对量测方程(8)进行离散化处理即可。由于系统状态维数和系统噪声维数均为5，量测方程的维数为4，故增广状态向量的维数为L＝5+5+4＝14，Sigma点的采样策略选用对称采样，其个数为2L+1＝29。\n[0088] 初始条件设置如下：Sigma点采样时，比例参数取0.5； 此\n处不取P0＝0，是为了防止下一步求(n+λ)Px的平方根时进行Cholesky分解产生奇异；其它初始条件与EKF法中相同，不再一一列出。\n[0089] 假设控制量始终为零，即舵偏角δz≡0，得到的仿真结果如图7a-图7d所示。\n[0090] 图7a-图7d表明，跟EKF一样，UKF也可以对系统状态进行准确的估计。在估计的初期，UKF收敛比较慢，并且有比较大的震荡；UKF收敛之后，其估计性能与EKF相当，对系统的状态估计与EKF的估计值几乎完全一致。\n[0091] 现在令控制量δz＝0.2sin(πt+π/2)，使无人机在铅垂平面内做起伏运动，测试估计值对真实值的跟踪性能，得到的仿真结果如图8a-图8d所示。UKF几乎是无滞后、无偏差地、完美地跟踪了实际状态变量的变化。\n[0092] 在本算例中，UKF的计算量要比EKF多得多，当仿真步长设置为0.01秒时，对于12秒的仿真时间，UKF法需要耗时3.93秒，而EKF仅需0.48秒，前者是后者的8倍多，这是因为量测方程结构形式复杂，需要占用很多CPU时间，在每一步估计循环中，EKF都只需要计算1次量测方程的导数即可，而UKF则需要计算29次量测方程，因为共有29个Sigma点要计算，这导致了UKF的计算量要远大于EKF的计算量。\n[0093] 步骤五：利用估计的状态信息，实现飞行器的特定飞行任务；\n[0094] 本算例中，UKF在估计精度上与EKF相当，但由于量测方程的复杂性，导致UKF的实时性要远低于EKF，所以本节将使用EKF作为信息融合算法，估计无人机的状态信息，实现铅垂平面内的高度控制，以期证明光流传感器在超低空突防中的应用价值。\n[0095] 假设驱动噪声为0，将式(5)简写成标准的状态空间形式：\n[0096] \n[0097] rank[B AB A2B A3B A4B]＝4＜5，系统不完全可控。经计算，系统存在三个0极点，为使系统稳定或渐近稳定，需要设计状态反馈阵，将系统的极点配置到s平面的左半平面，实现系统的镇定。\n[0098] 首先，通过线性变换将系统按能控性分解为\n[0099] \n[0100] 式中，T为正交变换阵，下标nc表示不可控，c表示可控。此分解可由Matlab中的ctrbf命令完成。\n[0101] 经计算，完全不能控子系统是渐近稳定的。于是，原系统是可以通过线性状态反馈镇定下来的。下面，对完全能控子系统进行极点配置。\n[0102] 任取四个期望极点为[-7，-5，-2-i，-2+i]，计算得状态反馈阵增益矩阵Kc＝[0.6267 -0.8319 6.2029 2.293]，于是可使原系统镇定下来的状态反馈矩阵为K＝(0 Kc)T＝(4.4340 4.4353 2.2980 0.3497 0.2884)\n[0103] 以EKF对状态的估计值作为状态反馈，搭建无人机高度控制模型如图9所示。图中Hc为指令高度(m)，为高度的估计值(m)，Δh为两者之差(m)，δZ为舵偏角(rad)，Z为传感器组的测量向量，为状态向量的估计值。\n[0104] 高度控制器可设计成一个PI控制器，其中比例系数Kp＝0.8，积分系数Ki＝0.4，舵偏角限制在±30°之内，初始指令高度为10m，第5s时刻将指令高度按指数规律下调到\n5m，时间常数为2s，仿真结果如图10所示。无人机可以无超调、无静差地跟踪高度指令，进行安全地超低空飞行，高度最终可以控制在期望高度的±0.5m之内，它证明了高度控制器的有效性，也同时证明了前文中传感器的布置、EKF算法的可行性。