一种复杂风场下的桥梁检测特种无人机自主稳定方法 |
|||||||
申请号 | CN202210439977.8 | 申请日 | 2022-04-25 | 公开(公告)号 | CN114942648B | 公开(公告)日 | 2024-05-03 |
申请人 | 西北工业大学; | 发明人 | 黄攀峰; 方国涛; 张夷斋; | ||||
摘要 | 本 发明 公开了一种复杂 风 场下的 桥梁 检测特种无人机自主稳定方法,首先对桥检多旋翼无人机进行动 力 学建模;然后设计多旋翼无人机抗风扰控制 算法 ,包括外控制环和内控制环;最后设计GPS 信号 阶段性失 锁 条件下的状态估计算法,实现了多旋翼无人机自主稳定。本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机 位置 伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性且具有较强的自主性。 | ||||||
权利要求 | 1.一种复杂风场下的桥梁检测特种无人机自主稳定方法,其特征在于,包括如下步骤: |
||||||
说明书全文 | 一种复杂风场下的桥梁检测特种无人机自主稳定方法技术领域[0001] 本发明属于无人机技术领域,具体涉及一种桥梁检测特种无人机自主稳定方法。 背景技术[0002] 近年来,我国交通基础设施建设飞速发展。与此同时,桥梁因反复承受车轮的磨损、冲击,遭受暴雨、日晒和冻融等自然因素的侵蚀破坏,部分建筑材料的性质随着使用时间的增长会发生衰变,导致桥梁老龄化问题突出。桥梁安全问题已成为关系国民经济、危及人民生命安全的核心问题,必须对桥梁定期病害检测和维护。传统人工检测手段应用于高空、深水、宽幅、结构复杂的大跨度桥梁时,受风力、桥梁振动影响大,属高危作业,安全隐患高。 [0003] 桥梁检测无人机具有机动性强、效率高、成本低、以及安全风险低等优势。但是,大跨度桥梁对卫星定位信号的遮蔽问题极其显著,主梁侧面和底部几何无任何卫星定位信号,易导致无人机GPS信号失锁。桥梁多采用钢筋混凝土结构或钢结构,结构内钢筋网产生的强磁场严重影响无人机磁罗盘性能,也会导致系统的准确性及鲁棒性降低。 [0004] 复杂风场也是桥梁检测特种无人机进行桥梁检测时必须面对的问题。因为大跨度桥梁往往都坐落在江、河、谷、海等风速较大的地方,无人机检测作业时会面临以下问题: [0005] 1.首先高海拔差跨度情况下,空气密度会发生较大变化,从而影响无人机各个旋翼的气动特性,即在不同的海拔高度,相同转速的电机带动桨叶产生的力和力矩是不同的,这将对无人机的稳定控制带来较大不利影响; [0006] 2.在复杂大风条件下,风场多变,风力等级较高,对于无人机桥梁检测作业易产生较大干扰。电机桨叶旋转产生的气流运动是多旋翼无人机的动力来源,而在大气环境中的风场运动能改变作用于桨叶上的相对来流,影响旋翼的力和力矩输出,从而影响无人机在空中的飞行姿态,飞行速度,和轨迹跟踪能力。 发明内容[0007] 为了克服现有技术的不足,本发明提供了一种复杂风场下的桥梁检测特种无人机自主稳定方法,首先对桥检多旋翼无人机进行动力学建模;然后设计多旋翼无人机抗风扰控制算法,包括外控制环和内控制环;最后设计GPS信号阶段性失锁条件下的状态估计算法,实现了多旋翼无人机自主稳定。本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机位置伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性且具有较强的自主性。 [0008] 本发明解决其技术问题所采用的技术方案包括如下步骤: [0009] 步骤1:桥梁检测特种无人机采用多旋翼无人机,对多旋翼无人机进行动力学建模; [0010] 步骤1‑1:先进行如下假设: [0011] (1)假设飞行区域的地面是一个平面,忽略地球自转,重力加速度设为常数; [0013] (3)多旋翼无人机六个电机及旋翼均对称安装,且除正反极性外其他参数均相同; [0014] (4)多旋翼无人机机体的质量分布均匀且质心与外形中心重合; [0016] 步骤1‑2:建立机体坐标系和地面坐标系; [0017] 为确定无人机飞行中的位置,建立惯性坐标系E‑XYZ,地面坐标系原点是无人机起飞时的质心,X轴的正方向是起飞时无人机头的航向正方向,Y轴的正方向是起飞时无人机水平向左的方向,Z轴的正方向是起飞时无人机垂直向上的方向; [0018] 为确定无人机的姿态,建立机体坐标系B‑XbYbZb,机体坐标系原点是无人机的质心,Xb轴的正方向是机头的方向,Zb轴的正方向是垂直于机体平面向上,Yb轴的正方向由右手准则确定; [0019] 步骤1‑3:定义φ为无人机绕Xb轴转动的横滚角;θ为无人机绕Yb轴转动的俯仰角;ψT为旋翼无人机绕Zb轴转动的偏航角,则无人机姿态表示为Θ=[φ θ ψ];假设每个旋翼的升力和无人机平面垂直并指向上,此时每个旋翼的升力fi近似为: [0020] [0021] 其中,Ωi是旋翼转速; [0022] 此时六旋翼整体的推力表达为 方向沿着‑bz的方向;总升力在惯性坐3 T 标系中表示为‑f Re3∈R,其中e3=[0,0,1],R表示本体坐标系到惯性坐标系的旋转矩阵; [0024] τi=±cτfi (2) [0025] 其中,cτ是常数,表示升力和扭矩的近似关系; [0026] 得到六旋翼总的升力f和总力矩M=[M1 M2 M3]T与各旋翼升力之间的关系: [0027] [0028] 其中d表示从螺旋桨中心到机体坐标系的距离; [0029] 步骤1‑5:最终得到多旋翼无人机的动力学模型表示为: [0030] 无人机位置动力学模型为:F1: [0032] 步骤2:设计多旋翼无人机抗风扰控制算法; [0033] 步骤2‑1:采用基于几何控制理论的级联PID控制器作为无人机的控制器,由外控制环和内控制环构成,外控制环包括位置PID控制器和速度PID控制器,内控制环包括姿态PID控制器和角速度PID控制器,具体如下: [0034] 无人机的轨迹包括期望位置xd和期望航向ψd,位置跟踪误差定义为ex=xd‑x;位置PID控制器设计为: [0035] [0036] 其中,vd表示期望速度,kPx、kDx和kIx分别表示位置PID控制器的比例、微分和积分参数;x表示实际位置; [0037] 给定期望速度后,定义速度跟踪误差ev=vd‑v,速度PID控制器设计如下: [0038] [0039] 其中,Fd表示期望三轴力,kPv、kDv和kIv表示速度PID控制器的比例、微分和积分参数;v表示实际速度; [0040] 步骤2‑2:引入高增益加速度反馈算法,包括线加速度反馈和角加速度反馈; [0041] 在没有干扰的理想情况下无人机位置动力学模型中的力学方程表示为: [0042] [0043] 其中 是期望加速度,而考虑实际系统中存在扰动矢量df与设计的线加速度反馈项vf,则式(6)表示为: [0044] [0045] 式中,是无人机真实的线加速度;df和vf满足: [0046] [0047] 其中,Q(s)为滤波器;s为拉普拉斯算子; [0048] 结合以上式(7)和(8)得到设计的线加速度反馈项: [0049] [0050] 其中,a表示正常数; [0051] 此时,Fd与vf的和作为内控制环的输入,根据几何控制思想,Fd与vf被分解为模长f=||Fd+vf||和方向 结合期望轨迹中的航向信息,计算出期望旋转矩阵Rd,根据几何控制,姿态跟踪误差表示为: [0052] [0053] 其中,映射 将SO(3)映射成为一个3维向量,表示向量的反对称运算的逆运算,SO(3)表示一种特殊正交群;则姿态PID控制器为: [0054] [0055] 其中,ωd表示期望加速度,kPR、kDR和kIR为姿态PID控制器的比例、微分和积分参数; [0056] 根据期望角速度ωd定义为角速度误差eω=RTRdωd‑ω,角加速度PID控制器为: [0057] [0058] 其中, 为期望角加速度,kPω、kDω和kIω为角加速度PID控制器的比例、微分和积分参数;ω表示实际角速度; [0059] 得到如下形式的角加速度反馈控制器: [0060] [0061] 其中,Md是期望力矩,是无人机机体角加速度矢量;vτ(t)表示角加速度反馈控制器;b为常数; [0062] 步骤3:设计GPS信号阶段性失锁条件下的状态估计算法; [0064] 将无人机动力学模型统一转化为: [0065]T [0066] 式中,f(x)=[F1 F2] , w为系统高斯白噪声; [0067] 步骤3‑2:考虑到无人机作业时GPS信号阶段性失锁的情况,系统的观测方程分以下两种情况: [0068] 第一种情况:当GPS传感器输出信息可用时,此时无人机系统的测量方程为: [0069] [0070] 其中,xk,yk,zk, θk,ψk分别表示k时刻的各项值; 表示测量值; [0071] 考虑测量噪声,无人机系统的状态空间方程为: [0072] [0073] 式中,υ为测量传感器高斯白噪声,u表示控制输入,hk(.)表示k时刻测量方程,f(.)表示系统状态函数,zk表示k时刻的测量函数; [0074] 第二种情况:当GPS信号失锁时: [0075] 由于桥梁检测无人机的飞行航向和速度阶段性保持不变,因此无人机的速度是先验已知的,即无人机飞行一定时间内的位移是先验已知的,因此,得到如下状态约束伪测量方程: [0076] [0077] 式中,vu为无人机飞行速度,tu为飞行时间, 为k+1时刻无人机位置状态的期望值;xk‑1、yk‑1、zk‑1分别表示k‑1时刻的位置坐标; [0078] 步骤3‑2:令 将无人机的位置动力学模型变形为: [0079] [0080] 其中, [0081] [0082] 式(18)作为无人机另一个伪测量位置状态,即: [0083] [0084] 其中,wk表示k时刻的系统噪声; 表示k+1时刻无人机位置状态的期望值; [0085] 联立以上式(18)和(19)得到系统的观测方程为: [0086] [0087] 经离散后,系统的动力学模型为: [0088] [0089] 式中,ΔT为离散时间,wk‑1∈(0,Qk‑1)和vk∈(0,Rk)为离散后的高斯白噪声;Qk‑1表示系统噪声协方差矩阵,Rk表示测量噪声协方差矩阵; [0090] 步骤4:经过以上步骤,实现多旋翼无人机的自主稳定。 [0091] 进一步地,所述高增益加速度反馈具体如下: [0092] 首先利用欧拉‑拉格朗日方程建立通用的动力学方程: [0093] [0094] 其中 为广义位置向量,包括位置和角度; 为惯性矩阵,为哥氏力项, 为重力项, 为摩擦阻力项, 为驱动力或力矩 项, 为扰动力或力矩; [0095] 考虑无人机动力学模型中的任意一个自由度: [0096] [0097] [0098] 其中Jii是自由度的惯性系数;τi是控制输入;τui由各种耦合力以及扰动不确定项组成; 表示哥氏力项矩阵的第i行第j元素,Gii(q)表示重力项矩阵中第i个对角线元素, 表示摩擦阻力项矩阵中第i个对角线元素;qj表示状态分量中第j个元素; [0099] 高增益加速度反馈设计如下: [0100] [0101] 其中ka是正常数,v是上一层控制器的输出;qi表示表示状态分量中第i个元素; [0102] 联立以上两式得到: [0103] [0104] 若ka足够大,即ka>>1且ka>>Jii,则上式近似为: [0105] [0106] 此时,得到了一个加速度跟踪器,直接跟踪给定加速度信号,同时消除了扰动项τui的影响; [0107] 引入前置滤波器,得到: [0108] [0109] 此时引入前置滤波器 得到: [0110] [0111] 无人机的任何一个自由度重新表示如下: [0112] [0113] 其中,A(s)、B(s)分别表示拉普拉斯变换后的多项式表达; [0114] 令ka→+∞得: [0115] [0116] [0117] 通过上式知不确定项τui经过一个滤波器Q(s)之后会对系统产生影响: [0118] [0119] 因为多旋翼无人机受到的扰动处于低频范围,因此选取Q(s)为高通滤波器的形式从而抑制低频扰动,选取: [0120] [0121] 其中a是正常数,系统表示为: [0122] [0123] 此时,扰动中的低频部分会被抑制,而高通滤波器的截止频率正好是a。 [0124] 进一步地,所述自适应扩展卡尔曼滤波算法步骤如下: [0125] 第一步:状态预测: [0126] xk=xk‑1+ΔTf(xk‑1) [0128] 第二步:协方差预测: [0129] [0130] [0131] 式中,In为单位矩阵;Pk‑1表示表示k‑1时刻的协方差矩阵,Qk‑1表示k‑1时刻的系统噪声方差矩阵, 表示k‑1时刻的预测状态; [0132] 第三步:计算卡尔曼滤波增益: [0133] [0134] 其中,h表示测量函数, 表示k时刻的预测状态; [0135] 第四步:更新状态变量,即计算状态变量的估计值: [0136] [0137] 第五步:更新状态估计误差协方差矩阵: [0138] [0139] 第六步:噪声协方差矩阵自适应调整策略: [0140] 新息定义为测量变量的实际值与预测值之间的误差,即: [0141] [0142] 新息的理论协方差矩阵: [0143] Ck=E(vkvkT)=HkPkHkT+Rk [0144] 新息的协方差矩阵计算方法如下: [0145] [0146] 式中,M为滑动窗口的长度; [0147] 当 时减小Rk;当 时,保持Rk不变; [0148] 定义的Rk调整因子为: [0149] [0150] 采用自适应协方差调整策略不更新Q和R,只在计算卡尔曼滤波增益和状态估计误差协方差矩阵给出Q和R修正量; [0151] 在更新滤波器增益时,自适应调整方法如下: [0152] [0153] 状态估计误差协方差矩阵的自适应调整方法为: [0154] [0155] 式中,ak表示波器增益自适应调整系数阵,κ表示协方差矩阵的自适应调整系数。 [0156] 本发明的有益效果如下: [0157] (1)本发明仅使用低成本GPS传感器和机载姿态确定装置实现无人机定位,无需成本高昂的高精度视觉传感器; [0158] (2)本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机位置伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性; [0160] 图1为六旋翼无人机坐标系示意图 具体实施方式[0162] 下面结合附图和实施例对本发明进一步说明。 [0163] 本发明的目的是提出一种阶段性GPS信号失锁条件下的桥梁检测无人机自主稳定新方法。采用六旋翼无人机作为飞行平台;在采用低成本GPS传感器和机载姿态确定装置实现无人机定位,设计状态高精度估计算法,提出旋翼无人机抗风扰方法,实现复杂强风场干扰的无人机的高精度稳定控制。 [0164] 一种复杂风场下的桥梁检测特种无人机自主稳定方法,包括如下步骤: [0165] 步骤1:桥梁检测特种无人机采用多旋翼无人机,对多旋翼无人机进行动力学建模; [0166] 步骤1‑1:先进行如下假设: [0167] (5)假设飞行区域的地面是一个平面,忽略地球自转,重力加速度设为常数; [0168] (6)将多旋翼无人机机体结构和旋翼都视为刚体,忽略多旋翼无人机机体的弹性形变及振动; [0169] (7)多旋翼无人机六个电机及旋翼均对称安装,且除正反极性外其他参数均相同; [0170] (8)多旋翼无人机机体的质量分布均匀且质心与外形中心重合; [0171] 无人机在桥梁检测过程中受力包括:多旋翼无人机本体的重力、旋翼产生的升力、空气阻力等;力矩包括:旋翼升力产生的空气动力矩、旋转产生的反扭力、空气阻力以及摩擦力产生的阻力矩等; [0172] 步骤1‑2:建立机体坐标系和地面坐标系; [0173] 为确定无人机飞行中的位置,建立地面坐标系E‑XYZ,地面坐标系原点是无人机起飞时的质心,X轴的正方向是起飞时无人机头的航向正方向,Y轴的正方向是起飞时无人机水平向左的方向,Z轴的正方向是起飞时无人机垂直向上的方向; [0174] 为确定无人机的姿态,建立惯性坐标系B‑XbYbZb,机体坐标系原点是无人机的质心,Xb轴的正方向是机头的方向,Zb轴的正方向是垂直于机体平面向上,Yb轴的正方向由右手准则确定; [0175] 步骤1‑3:如附图1所示,φ为无人机绕Xb轴转动的横滚角;θ为无人机绕Yb轴转动的T俯仰角;ψ为旋翼无人机绕Zb轴转动的偏航角,则无人机姿态表示为Θ=[φ θ ψ];假设每个旋翼的升力和无人机平面垂直并指向上,此时每个旋翼的升力fi近似为: [0176] [0177] 其中,Ωi是旋翼转速; [0178] 此时六旋翼整体的推力表达为 方向沿着‑bz的方向;总升力在惯性坐3 T 标系中表示为‑f Re3∈R,其中e3=[0,0,1]; [0179] 步骤1‑4:为了保证力矩平衡,在六旋翼中3个旋翼顺时针旋转,另外3个逆时针旋转;假设旋翼产生的扭矩与其升力成正比,此时得到每个旋翼的扭矩: [0180] τi=±cτfi (2) [0181] 其中,cτ是常数,表示升力和扭矩的近似关系; [0182] 得到六旋翼总的升力f和总力矩M=[M1 M2 M3]T与各旋翼升力之间的关系: [0183] [0184] 其中d表示从螺旋桨中心到机体坐标系的距离; [0185] 步骤1‑5:最终得到多旋翼无人机的动力学模型表示为: [0186] 无人机位置动力学模型为:F1: [0187] 无人机姿态动力学模型:F2: 其中,x是无人机相对于惯性坐标系的位置,m为无人机质量,g为重力加速度,df为扰动矢量,J为转动惯量,ω为角速度,dτ为扰动力矩矢量; [0188] 步骤2:设计多旋翼无人机抗风扰控制算法; [0189] 首先描述传统的高增益加速度反馈方法,通过引入带前置滤波器的加速度反馈方法,避免实际应用中高增益无法实现的问题。加速度反馈方法是一个通用的方法,适用于多自由度刚体系统本文的旋翼无人机系统可以利用欧拉–拉格朗日方程建立通用的动力学方程: [0190] [0191] 其中 为广义位置向量,包括位置和角度; 为惯性矩阵,为哥氏力项, 为重力项, 为摩擦阻力项, 为驱动力或力矩 项, 为扰动力或力矩;高增益加速度反馈是一种适用于单输入单输出系统的增强型控制方法,并已经在许多系统中得到验证。 [0192] 考虑无人机动力学模型中的任意一个自由度: [0193] [0194] [0195] 其中Jii是自由度的惯性系数(质量或转动惯量);τi是控制输入;τui由各种耦合力(力矩)以及扰动不确定项组成,将该项整体考虑,高增益反馈的核心思想是将上式中的力(力矩)驱动转换成加速度(角/线加速度驱动),从而达到抑制扰动的目的。; [0196] 高增益加速度反馈设计如下: [0197] [0198] 其中ka是正常数,v是上一层控制器的输出; [0199] 联立以上两式得到: [0200] [0201] 若ka足够大,即ka>>1且ka>>Jii,则上式近似为: [0202] [0203] 此时,得到了一个加速度跟踪器,直接跟踪给定加速度信号,同时消除了扰动项τui的影响; [0204] 引入前置滤波器,得到: [0205] [0206] 此时引入前置滤波器 得到: [0207] [0208] 无人机的任何一个自由度重新表示如下: [0209] [0210] 令ka→+∞得: [0211] [0212] [0213] 通过上式知不确定项τui经过一个滤波器Q(s)之后会对系统产生影响: [0214] [0215] 因为多旋翼无人机受到的扰动处于低频范围,因此选取Q(s)为高通滤波器的形式从而抑制低频扰动,选取: [0216] [0217] 其中a是正常数,系统表示为: [0218] [0219] 此时,扰动中的低频部分会被抑制,而高通滤波器的截止频率正好是a。 [0220] 步骤2‑1:采用基于几何控制理论的级联PID控制器作为无人机的控制器,由外控制环和内控制环构成,外控制环包括位置PID控制器和速度PID控制器,内控制环包括姿态PID控制器和角速度PID控制器,具体如下: [0221] 为了实现无人机在GPS信号阶段失锁条件下的稳定控制‑估计一体化,要求桥梁检测无人机在跟踪桥梁快速初检期望轨迹时满足飞行方向与飞行速度阶段性保持不变。无人机的轨迹包括期望位置xd和期望航向ψd,位置跟踪误差定义为ex=xd‑x;位置PID控制器设计为: [0222] [0223] 其中,vd表示期望速度,kPx、kDx和kIx分别表示位置PID控制器的比例、微分和积分参数; [0224] 给定期望速度后,定义速度跟踪误差ev=vd‑v,速度PID控制器设计如下: [0225] [0226] 其中,Fd表示期望三轴力,kPv、kDv和kIv表示速度PID控制器的比例、微分和积分参数; [0227] 步骤2‑2:引入高增益加速度反馈算法,包括线加速度反馈和角加速度反馈; [0228] 在没有干扰的理想情况下无人机位置动力学模型中的力学方程表示为: [0229] [0230] 其中 是期望加速度,而考虑实际系统中存在扰动矢量df与设计的线加速度反馈项vf,则式(6)表示为: [0231] [0232] 式中,是无人机真实的线加速度;df和vf满足: [0233] [0234] 其中,Q(s)为滤波器; [0235] 结合以上式(7)和(8)得到设计的线加速度反馈项: [0236] [0237] 此时,Fd与vf的和作为内控制环的输入,根据几何控制思想,Fd与vf被分解为模长f=||Fd+vf||和方向 结合期望轨迹中的航向信息,计算出期望旋转矩阵Rd,根据几何控制,姿态跟踪误差表示为: [0238] [0239] 其中,映射 将SO(3)映射成为一个3维向量,表示向量的反对称运算的逆运算;则姿态PID控制器为: [0240] [0241] 其中,ωd表示期望加速度,kPR、kDR和KIR为姿态PID控制器的比例、微分和积分参数; [0242] 根据期望角速度ωd定义为角速度误差eω=RTRdωd‑ω,角加速度PID控制器为: [0243] [0244] 其中, 为期望角加速度,kPω、kDω和kIω为角加速度PID控制器的比例、微分和积分参数; [0245] 得到如下形式的角加速度反馈控制器: [0246] [0247] 其中,Md是期望力矩, 是无人机机体角加速度矢量; [0248] 步骤3:设计GPS信号阶段性失锁条件下的状态估计算法; [0249] 步骤3‑1:系统状态的实时获取是反馈控制的前提。为满足系统外控制环和内控制环的状态反馈需要,采用低成本GPS传感器和机载姿态确定装置获取无人机位姿信息,采用自适应卡尔曼滤波算法估计无人机的实时位置和姿态; [0250] 将无人机动力学模型统一转化为: [0251]T [0252] 式中,f(x)=[F1 F2] , w为系统高斯白噪声; [0253] 步骤3‑2:考虑到无人机作业时GPS信号阶段性失锁的情况,系统的观测方程分以下两种情况: [0254] 第一种情况:当GPS传感器输出信息可用时,此时无人机系统的测量方程为: [0255] [0256] 考虑测量噪声,无人机系统的状态空间方程为: [0257] [0258] 式中,υ为测量传感器高斯白噪声; [0259] 第二种情况:当GPS信号失锁时: [0260] 由于桥梁检测无人机的飞行航向和速度阶段性保持不变,因此无人机的速度是先验已知的,即无人机飞行一定时间内的位移是先验已知的,因此,得到如下状态约束伪测量方程: [0261] [0262] 式中,vu为无人机飞行速度,tu为飞行时间, 为k+1时刻无人机位置状态的期望值; [0263] 步骤3‑2:令 将无人机的位置动力学模型变形为: [0264] [0265] 其中, [0266] [0267] 式(18)作为无人机另一个伪测量位置状态,即: [0268] [0269] 联立以上式(18)和(19)得到系统的观测方程为: [0270] [0271] 由于桥梁宽度有限,无人机桥梁检测作业时GPS信号处于阶段性失锁状态而且失锁时间较短,利用系统的动力学方程进行系统状态伪测量并结合无人机飞行的位移约束信息,在一定时间内可实现无人机位置状态的估计。 [0272] 经离散后,系统的动力学模型为: [0273] [0274] 式中,ΔT为离散时间,wk‑1∈(0,Qk‑1)和vk∈(0,Rk)为离散后的高斯白噪声; [0275] 自适应扩展卡尔曼滤波算法步骤如下: [0276] 第一步:状态预测: [0277] xk=xk‑1+ΔTf(xk‑1) [0278] 第二步:协方差预测: [0279] [0280] [0281] 式中,In为单位矩阵; [0282] 第三步:计算卡尔曼滤波增益: [0283] [0284] [0285] 第四步:更新状态变量,即计算状态变量的估计值: [0286] [0287] 第五步:更新状态估计误差协方差矩阵: [0288] [0289] 第六步:噪声协方差矩阵自适应调整策略: [0290] 新息定义为测量变量的实际值与预测值之间的误差,即: [0291] [0292] 新息的理论协方差矩阵: [0293] Ck=E(vkvkT)=HkPkHkT+Rk [0294] 新息的协方差矩阵计算方法如下: [0295] [0296] 式中,M为滑动窗口的长度; [0297] 当 时减小Rk;当 时,保持Rk不变; [0298] 定义的Rk调整因子为: [0299] [0300] 采用自适应协方差调整策略不更新Q和R,只在计算卡尔曼滤波增益和状态估计误差协方差矩阵给出Q和R修正量; [0301] 在更新滤波器增益时,自适应调整方法如下: [0302] [0303] 状态估计误差协方差矩阵的自适应调整方法为: [0304] [0305] 步骤4:经过以上步骤,实现多旋翼无人机的自主稳定。 |