CN114942648B - 一种复杂风场下的桥梁检测特种无人机自主稳定方法 - Google Patents
一种复杂风场下的桥梁检测特种无人机自主稳定方法 Download PDFInfo
- Publication number
- CN114942648B CN114942648B CN202210439977.8A CN202210439977A CN114942648B CN 114942648 B CN114942648 B CN 114942648B CN 202210439977 A CN202210439977 A CN 202210439977A CN 114942648 B CN114942648 B CN 114942648B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- rotor
- pid controller
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000001514 detection method Methods 0.000 title claims abstract description 29
- 230000006641 stabilisation Effects 0.000 title claims abstract description 8
- 238000011105 stabilization Methods 0.000 title claims abstract description 8
- 238000005259 measurement Methods 0.000 claims abstract description 20
- 230000008569 process Effects 0.000 claims abstract description 9
- 230000000737 periodic effect Effects 0.000 claims abstract description 6
- 238000007689 inspection Methods 0.000 claims abstract 3
- 230000001133 acceleration Effects 0.000 claims description 64
- 239000011159 matrix material Substances 0.000 claims description 45
- 230000005484 gravity Effects 0.000 claims description 11
- 230000003044 adaptive effect Effects 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 9
- 238000013461 design Methods 0.000 claims description 7
- 238000011045 prefiltration Methods 0.000 claims description 7
- 230000014509 gene expression Effects 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000008878 coupling Effects 0.000 claims description 3
- 238000010168 coupling process Methods 0.000 claims description 3
- 238000005859 coupling reaction Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000005489 elastic deformation Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims description 2
- 238000010276 construction Methods 0.000 description 2
- 230000000087 stabilizing effect Effects 0.000 description 2
- 229910000831 Steel Inorganic materials 0.000 description 1
- 238000005299 abrasion Methods 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000004566 building material Substances 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 201000010099 disease Diseases 0.000 description 1
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 239000011150 reinforced concrete Substances 0.000 description 1
- 230000003014 reinforcing effect Effects 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
- 238000010257 thawing Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Algebra (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种复杂风场下的桥梁检测特种无人机自主稳定方法,首先对桥检多旋翼无人机进行动力学建模;然后设计多旋翼无人机抗风扰控制算法,包括外控制环和内控制环;最后设计GPS信号阶段性失锁条件下的状态估计算法,实现了多旋翼无人机自主稳定。本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机位置伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性且具有较强的自主性。
Description
技术领域
本发明属于无人机技术领域,具体涉及一种桥梁检测特种无人机自主稳定方法。
背景技术
近年来,我国交通基础设施建设飞速发展。与此同时,桥梁因反复承受车轮的磨损、冲击,遭受暴雨、日晒和冻融等自然因素的侵蚀破坏,部分建筑材料的性质随着使用时间的增长会发生衰变,导致桥梁老龄化问题突出。桥梁安全问题已成为关系国民经济、危及人民生命安全的核心问题,必须对桥梁定期病害检测和维护。传统人工检测手段应用于高空、深水、宽幅、结构复杂的大跨度桥梁时,受风力、桥梁振动影响大,属高危作业,安全隐患高。
桥梁检测无人机具有机动性强、效率高、成本低、以及安全风险低等优势。但是,大跨度桥梁对卫星定位信号的遮蔽问题极其显著,主梁侧面和底部几何无任何卫星定位信号,易导致无人机GPS信号失锁。桥梁多采用钢筋混凝土结构或钢结构,结构内钢筋网产生的强磁场严重影响无人机磁罗盘性能,也会导致系统的准确性及鲁棒性降低。
复杂风场也是桥梁检测特种无人机进行桥梁检测时必须面对的问题。因为大跨度桥梁往往都坐落在江、河、谷、海等风速较大的地方,无人机检测作业时会面临以下问题:
1.首先高海拔差跨度情况下,空气密度会发生较大变化,从而影响无人机各个旋翼的气动特性,即在不同的海拔高度,相同转速的电机带动桨叶产生的力和力矩是不同的,这将对无人机的稳定控制带来较大不利影响;
2.在复杂大风条件下,风场多变,风力等级较高,对于无人机桥梁检测作业易产生较大干扰。电机桨叶旋转产生的气流运动是多旋翼无人机的动力来源,而在大气环境中的风场运动能改变作用于桨叶上的相对来流,影响旋翼的力和力矩输出,从而影响无人机在空中的飞行姿态,飞行速度,和轨迹跟踪能力。
发明内容
为了克服现有技术的不足,本发明提供了一种复杂风场下的桥梁检测特种无人机自主稳定方法,首先对桥检多旋翼无人机进行动力学建模;然后设计多旋翼无人机抗风扰控制算法,包括外控制环和内控制环;最后设计GPS信号阶段性失锁条件下的状态估计算法,实现了多旋翼无人机自主稳定。本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机位置伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性且具有较强的自主性。
本发明解决其技术问题所采用的技术方案包括如下步骤:
步骤1:桥梁检测特种无人机采用多旋翼无人机,对多旋翼无人机进行动力学建模;
步骤1-1:先进行如下假设:
(1)假设飞行区域的地面是一个平面,忽略地球自转,重力加速度设为常数;
(2)将多旋翼无人机机体结构和旋翼都视为刚体,忽略多旋翼无人机机体的弹性形变及振动;
(3)多旋翼无人机六个电机及旋翼均对称安装,且除正反极性外其他参数均相同;
(4)多旋翼无人机机体的质量分布均匀且质心与外形中心重合;
无人机在桥梁检测过程中受力包括:多旋翼无人机本体的重力、旋翼产生的升力、空气阻力;力矩包括:旋翼升力产生的空气动力矩、旋转产生的反扭力、空气阻力以及摩擦力产生的阻力矩;
步骤1-2:建立机体坐标系和地面坐标系;
为确定无人机飞行中的位置,建立惯性坐标系E-XYZ,地面坐标系原点是无人机起飞时的质心,X轴的正方向是起飞时无人机头的航向正方向,Y轴的正方向是起飞时无人机水平向左的方向,Z轴的正方向是起飞时无人机垂直向上的方向;
为确定无人机的姿态,建立机体坐标系B-XbYbZb,机体坐标系原点是无人机的质心,Xb轴的正方向是机头的方向,Zb轴的正方向是垂直于机体平面向上,Yb轴的正方向由右手准则确定;
步骤1-3:定义φ为无人机绕Xb轴转动的横滚角;θ为无人机绕Yb轴转动的俯仰角;ψ为旋翼无人机绕Zb轴转动的偏航角,则无人机姿态表示为Θ=[φ θ ψ]T;假设每个旋翼的升力和无人机平面垂直并指向上,此时每个旋翼的升力fi近似为:
其中,Ωi是旋翼转速;
此时六旋翼整体的推力表达为方向沿着-bz的方向;总升力在惯性坐标系中表示为-f Re3∈R3,其中e3=[0,0,1]T,R表示本体坐标系到惯性坐标系的旋转矩阵;
步骤1-4:为了保证力矩平衡,在六旋翼中3个旋翼顺时针旋转,另外3个逆时针旋转;假设旋翼产生的扭矩与其升力成正比,此时得到每个旋翼的扭矩:
τi=±cτfi (2)
其中,cτ是常数,表示升力和扭矩的近似关系;
得到六旋翼总的升力f和总力矩M=[M1 M2 M3]T与各旋翼升力之间的关系:
其中d表示从螺旋桨中心到机体坐标系的距离;
步骤1-5:最终得到多旋翼无人机的动力学模型表示为:
无人机位置动力学模型为:F1:
无人机姿态动力学模型:F2:其中,x是无人机相对于惯性坐标系的位置,m为无人机质量,g为重力加速度,df为扰动矢量,J为转动惯量,ω为角速度,dτ为扰动力矩矢量;
步骤2:设计多旋翼无人机抗风扰控制算法;
步骤2-1:采用基于几何控制理论的级联PID控制器作为无人机的控制器,由外控制环和内控制环构成,外控制环包括位置PID控制器和速度PID控制器,内控制环包括姿态PID控制器和角速度PID控制器,具体如下:
无人机的轨迹包括期望位置xd和期望航向ψd,位置跟踪误差定义为ex=xd-x;位置PID控制器设计为:
其中,vd表示期望速度,kPx、kDx和kIx分别表示位置PID控制器的比例、微分和积分参数;x表示实际位置;
给定期望速度后,定义速度跟踪误差ev=vd-v,速度PID控制器设计如下:
其中,Fd表示期望三轴力,kPv、kDv和kIv表示速度PID控制器的比例、微分和积分参数;v表示实际速度;
步骤2-2:引入高增益加速度反馈算法,包括线加速度反馈和角加速度反馈;
在没有干扰的理想情况下无人机位置动力学模型中的力学方程表示为:
其中是期望加速度,而考虑实际系统中存在扰动矢量df与设计的线加速度反馈项vf,则式(6)表示为:
式中,是无人机真实的线加速度;df和vf满足:
其中,Q(s)为滤波器;s为拉普拉斯算子;
结合以上式(7)和(8)得到设计的线加速度反馈项:
其中,a表示正常数;
此时,Fd与vf的和作为内控制环的输入,根据几何控制思想,Fd与vf被分解为模长f=||Fd+vf||和方向结合期望轨迹中的航向信息,计算出期望旋转矩阵Rd,根据几何控制,姿态跟踪误差表示为:
其中,映射将SO(3)映射成为一个3维向量,表示向量的反对称运算的逆运算,SO(3)表示一种特殊正交群;则姿态PID控制器为:
其中,ωd表示期望加速度,kPR、kDR和kIR为姿态PID控制器的比例、微分和积分参数;
根据期望角速度ωd定义为角速度误差eω=RTRdωd-ω,角加速度PID控制器为:
其中,为期望角加速度,kPω、kDω和kIω为角加速度PID控制器的比例、微分和积分参数;ω表示实际角速度;
得到如下形式的角加速度反馈控制器:
其中,Md是期望力矩,是无人机机体角加速度矢量;vτ(t)表示角加速度反馈控制器;b为常数;
步骤3:设计GPS信号阶段性失锁条件下的状态估计算法;
步骤3-1:采用GPS传感器和机载姿态确定装置获取无人机位姿信息,采用自适应卡尔曼滤波算法估计无人机的实时位置和姿态;
将无人机动力学模型统一转化为:
式中,f(x)=[F1 F2]T,w为系统高斯白噪声;
步骤3-2:考虑到无人机作业时GPS信号阶段性失锁的情况,系统的观测方程分以下两种情况:
第一种情况:当GPS传感器输出信息可用时,此时无人机系统的测量方程为:
其中,xk,yk,zk,θk,ψk分别表示k时刻的各项值;/>表示测量值;
考虑测量噪声,无人机系统的状态空间方程为:
式中,υ为测量传感器高斯白噪声,u表示控制输入,hk(.)表示k时刻测量方程,f(.)表示系统状态函数,zk表示k时刻的测量函数;
第二种情况:当GPS信号失锁时:
由于桥梁检测无人机的飞行航向和速度阶段性保持不变,因此无人机的速度是先验已知的,即无人机飞行一定时间内的位移是先验已知的,因此,得到如下状态约束伪测量方程:
式中,vu为无人机飞行速度,tu为飞行时间,为k+1时刻无人机位置状态的期望值;xk-1、yk-1、zk-1分别表示k-1时刻的位置坐标;
步骤3-2:令将无人机的位置动力学模型变形为:
其中,
式(18)作为无人机另一个伪测量位置状态,即:
其中,wk表示k时刻的系统噪声;表示k+1时刻无人机位置状态的期望值;
联立以上式(18)和(19)得到系统的观测方程为:
经离散后,系统的动力学模型为:
式中,ΔT为离散时间,wk-1∈(0,Qk-1)和vk∈(0,Rk)为离散后的高斯白噪声;Qk-1表示系统噪声协方差矩阵,Rk表示测量噪声协方差矩阵;
步骤4:经过以上步骤,实现多旋翼无人机的自主稳定。
进一步地,所述高增益加速度反馈具体如下:
首先利用欧拉-拉格朗日方程建立通用的动力学方程:
其中为广义位置向量,包括位置和角度;/>为惯性矩阵,/>为哥氏力项,/>为重力项,/>为摩擦阻力项,/>为驱动力或力矩项,/>为扰动力或力矩;
考虑无人机动力学模型中的任意一个自由度:
其中Jii是自由度的惯性系数;τi是控制输入;τui由各种耦合力以及扰动不确定项组成;表示哥氏力项矩阵的第i行第j元素,Gii(q)表示重力项矩阵中第i个对角线元素,/>表示摩擦阻力项矩阵中第i个对角线元素;qj表示状态分量中第j个元素;
高增益加速度反馈设计如下:
其中ka是正常数,v是上一层控制器的输出;qi表示表示状态分量中第i个元素;
联立以上两式得到:
若ka足够大,即ka>>1且ka>>Jii,则上式近似为:
此时,得到了一个加速度跟踪器,直接跟踪给定加速度信号,同时消除了扰动项τui的影响;
引入前置滤波器,得到:
此时引入前置滤波器得到:
无人机的任何一个自由度重新表示如下:
其中,A(s)、B(s)分别表示拉普拉斯变换后的多项式表达;
令ka→+∞得:
通过上式知不确定项τui经过一个滤波器Q(s)之后会对系统产生影响:
因为多旋翼无人机受到的扰动处于低频范围,因此选取Q(s)为高通滤波器的形式从而抑制低频扰动,选取:
其中a是正常数,系统表示为:
此时,扰动中的低频部分会被抑制,而高通滤波器的截止频率正好是a。
进一步地,所述自适应扩展卡尔曼滤波算法步骤如下:
第一步:状态预测:
xk=xk-1+ΔTf(xk-1)
其中,ΔT表示采样时间,f(xk-1)表示k-1时刻的状态方程;
第二步:协方差预测:
式中,In为单位矩阵;Pk-1表示表示k-1时刻的协方差矩阵,Qk-1表示k-1时刻的系统噪声方差矩阵,表示k-1时刻的预测状态;
第三步:计算卡尔曼滤波增益:
其中,h表示测量函数,表示k时刻的预测状态;
第四步:更新状态变量,即计算状态变量的估计值:
第五步:更新状态估计误差协方差矩阵:
第六步:噪声协方差矩阵自适应调整策略:
新息定义为测量变量的实际值与预测值之间的误差,即:
新息的理论协方差矩阵:
Ck=E(vkvk T)=HkPkHk T+Rk
新息的协方差矩阵计算方法如下:
式中,M为滑动窗口的长度;
当时减小Rk;当/>时,保持Rk不变;
定义的Rk调整因子为:
采用自适应协方差调整策略不更新Q和R,只在计算卡尔曼滤波增益和状态估计误差协方差矩阵给出Q和R修正量;
在更新滤波器增益时,自适应调整方法如下:
状态估计误差协方差矩阵的自适应调整方法为:
式中,ak表示波器增益自适应调整系数阵,κ表示协方差矩阵的自适应调整系数。
本发明的有益效果如下:
(1)本发明仅使用低成本GPS传感器和机载姿态确定装置实现无人机定位,无需成本高昂的高精度视觉传感器;
(2)本发明通过限制无人机桥梁检测过程中的飞行方向与速度,给出一定时间内的无人机位置伪测量,并结合动力学方程可实现无人机GPS信号阶段性失锁条件下的位置估计,方法具有创新性;
(3)本发明适于实际应用,具有较强的自主性。
附图说明
图1为六旋翼无人机坐标系示意图
图2为无人机在GPS拒止环境下的自主控制-估计框架。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
本发明的目的是提出一种阶段性GPS信号失锁条件下的桥梁检测无人机自主稳定新方法。采用六旋翼无人机作为飞行平台;在采用低成本GPS传感器和机载姿态确定装置实现无人机定位,设计状态高精度估计算法,提出旋翼无人机抗风扰方法,实现复杂强风场干扰的无人机的高精度稳定控制。
一种复杂风场下的桥梁检测特种无人机自主稳定方法,包括如下步骤:
步骤1:桥梁检测特种无人机采用多旋翼无人机,对多旋翼无人机进行动力学建模;
步骤1-1:先进行如下假设:
(5)假设飞行区域的地面是一个平面,忽略地球自转,重力加速度设为常数;
(6)将多旋翼无人机机体结构和旋翼都视为刚体,忽略多旋翼无人机机体的弹性形变及振动;
(7)多旋翼无人机六个电机及旋翼均对称安装,且除正反极性外其他参数均相同;
(8)多旋翼无人机机体的质量分布均匀且质心与外形中心重合;
无人机在桥梁检测过程中受力包括:多旋翼无人机本体的重力、旋翼产生的升力、空气阻力等;力矩包括:旋翼升力产生的空气动力矩、旋转产生的反扭力、空气阻力以及摩擦力产生的阻力矩等;
步骤1-2:建立机体坐标系和地面坐标系;
为确定无人机飞行中的位置,建立地面坐标系E-XYZ,地面坐标系原点是无人机起飞时的质心,X轴的正方向是起飞时无人机头的航向正方向,Y轴的正方向是起飞时无人机水平向左的方向,Z轴的正方向是起飞时无人机垂直向上的方向;
为确定无人机的姿态,建立惯性坐标系B-XbYbZb,机体坐标系原点是无人机的质心,Xb轴的正方向是机头的方向,Zb轴的正方向是垂直于机体平面向上,Yb轴的正方向由右手准则确定;
步骤1-3:如附图1所示,φ为无人机绕Xb轴转动的横滚角;θ为无人机绕Yb轴转动的俯仰角;ψ为旋翼无人机绕Zb轴转动的偏航角,则无人机姿态表示为Θ=[φ θ ψ]T;假设每个旋翼的升力和无人机平面垂直并指向上,此时每个旋翼的升力fi近似为:
其中,Ωi是旋翼转速;
此时六旋翼整体的推力表达为方向沿着-bz的方向;总升力在惯性坐标系中表示为-f Re3∈R3,其中e3=[0,0,1]T;
步骤1-4:为了保证力矩平衡,在六旋翼中3个旋翼顺时针旋转,另外3个逆时针旋转;假设旋翼产生的扭矩与其升力成正比,此时得到每个旋翼的扭矩:
τi=±cτfi (2)
其中,cτ是常数,表示升力和扭矩的近似关系;
得到六旋翼总的升力f和总力矩M=[M1 M2 M3]T与各旋翼升力之间的关系:
其中d表示从螺旋桨中心到机体坐标系的距离;
步骤1-5:最终得到多旋翼无人机的动力学模型表示为:
无人机位置动力学模型为:F1:
无人机姿态动力学模型:F2:其中,x是无人机相对于惯性坐标系的位置,m为无人机质量,g为重力加速度,df为扰动矢量,J为转动惯量,ω为角速度,dτ为扰动力矩矢量;
步骤2:设计多旋翼无人机抗风扰控制算法;
首先描述传统的高增益加速度反馈方法,通过引入带前置滤波器的加速度反馈方法,避免实际应用中高增益无法实现的问题。加速度反馈方法是一个通用的方法,适用于多自由度刚体系统本文的旋翼无人机系统可以利用欧拉–拉格朗日方程建立通用的动力学方程:
其中为广义位置向量,包括位置和角度;/>为惯性矩阵,/>为哥氏力项,/>为重力项,/>为摩擦阻力项,/>为驱动力或力矩项,/>为扰动力或力矩;高增益加速度反馈是一种适用于单输入单输出系统的增强型控制方法,并已经在许多系统中得到验证。
考虑无人机动力学模型中的任意一个自由度:
其中Jii是自由度的惯性系数(质量或转动惯量);τi是控制输入;τui由各种耦合力(力矩)以及扰动不确定项组成,将该项整体考虑,高增益反馈的核心思想是将上式中的力(力矩)驱动转换成加速度(角/线加速度驱动),从而达到抑制扰动的目的。;
高增益加速度反馈设计如下:
其中ka是正常数,v是上一层控制器的输出;
联立以上两式得到:
若ka足够大,即ka>>1且ka>>Jii,则上式近似为:
此时,得到了一个加速度跟踪器,直接跟踪给定加速度信号,同时消除了扰动项τui的影响;
引入前置滤波器,得到:
此时引入前置滤波器得到:
无人机的任何一个自由度重新表示如下:
令ka→+∞得:
通过上式知不确定项τui经过一个滤波器Q(s)之后会对系统产生影响:
因为多旋翼无人机受到的扰动处于低频范围,因此选取Q(s)为高通滤波器的形式从而抑制低频扰动,选取:
其中a是正常数,系统表示为:
此时,扰动中的低频部分会被抑制,而高通滤波器的截止频率正好是a。
步骤2-1:采用基于几何控制理论的级联PID控制器作为无人机的控制器,由外控制环和内控制环构成,外控制环包括位置PID控制器和速度PID控制器,内控制环包括姿态PID控制器和角速度PID控制器,具体如下:
为了实现无人机在GPS信号阶段失锁条件下的稳定控制-估计一体化,要求桥梁检测无人机在跟踪桥梁快速初检期望轨迹时满足飞行方向与飞行速度阶段性保持不变。无人机的轨迹包括期望位置xd和期望航向ψd,位置跟踪误差定义为ex=xd-x;位置PID控制器设计为:
其中,vd表示期望速度,kPx、kDx和kIx分别表示位置PID控制器的比例、微分和积分参数;
给定期望速度后,定义速度跟踪误差ev=vd-v,速度PID控制器设计如下:
其中,Fd表示期望三轴力,kPv、kDv和kIv表示速度PID控制器的比例、微分和积分参数;
步骤2-2:引入高增益加速度反馈算法,包括线加速度反馈和角加速度反馈;
在没有干扰的理想情况下无人机位置动力学模型中的力学方程表示为:
其中是期望加速度,而考虑实际系统中存在扰动矢量df与设计的线加速度反馈项vf,则式(6)表示为:
式中,是无人机真实的线加速度;df和vf满足:
其中,Q(s)为滤波器;
结合以上式(7)和(8)得到设计的线加速度反馈项:
/>
此时,Fd与vf的和作为内控制环的输入,根据几何控制思想,Fd与vf被分解为模长f=||Fd+vf||和方向结合期望轨迹中的航向信息,计算出期望旋转矩阵Rd,根据几何控制,姿态跟踪误差表示为:
其中,映射将SO(3)映射成为一个3维向量,表示向量的反对称运算的逆运算;则姿态PID控制器为:
其中,ωd表示期望加速度,kPR、kDR和KIR为姿态PID控制器的比例、微分和积分参数;
根据期望角速度ωd定义为角速度误差eω=RTRdωd-ω,角加速度PID控制器为:
其中,为期望角加速度,kPω、kDω和kIω为角加速度PID控制器的比例、微分和积分参数;
得到如下形式的角加速度反馈控制器:
其中,Md是期望力矩,是无人机机体角加速度矢量;
步骤3:设计GPS信号阶段性失锁条件下的状态估计算法;
步骤3-1:系统状态的实时获取是反馈控制的前提。为满足系统外控制环和内控制环的状态反馈需要,采用低成本GPS传感器和机载姿态确定装置获取无人机位姿信息,采用自适应卡尔曼滤波算法估计无人机的实时位置和姿态;
将无人机动力学模型统一转化为:
式中,f(x)=[F1 F2]T,w为系统高斯白噪声;
步骤3-2:考虑到无人机作业时GPS信号阶段性失锁的情况,系统的观测方程分以下两种情况:
第一种情况:当GPS传感器输出信息可用时,此时无人机系统的测量方程为:
考虑测量噪声,无人机系统的状态空间方程为:
式中,υ为测量传感器高斯白噪声;
第二种情况:当GPS信号失锁时:
由于桥梁检测无人机的飞行航向和速度阶段性保持不变,因此无人机的速度是先验已知的,即无人机飞行一定时间内的位移是先验已知的,因此,得到如下状态约束伪测量方程:
式中,vu为无人机飞行速度,tu为飞行时间,为k+1时刻无人机位置状态的期望值;
步骤3-2:令将无人机的位置动力学模型变形为:
其中,
式(18)作为无人机另一个伪测量位置状态,即:
联立以上式(18)和(19)得到系统的观测方程为:
由于桥梁宽度有限,无人机桥梁检测作业时GPS信号处于阶段性失锁状态而且失锁时间较短,利用系统的动力学方程进行系统状态伪测量并结合无人机飞行的位移约束信息,在一定时间内可实现无人机位置状态的估计。
经离散后,系统的动力学模型为:
式中,ΔT为离散时间,wk-1∈(0,Qk-1)和vk∈(0,Rk)为离散后的高斯白噪声;
自适应扩展卡尔曼滤波算法步骤如下:
第一步:状态预测:
xk=xk-1+ΔTf(xk-1)
第二步:协方差预测:
式中,In为单位矩阵;
第三步:计算卡尔曼滤波增益:
/>
第四步:更新状态变量,即计算状态变量的估计值:
第五步:更新状态估计误差协方差矩阵:
第六步:噪声协方差矩阵自适应调整策略:
新息定义为测量变量的实际值与预测值之间的误差,即:
新息的理论协方差矩阵:
Ck=E(vkvk T)=HkPkHk T+Rk
新息的协方差矩阵计算方法如下:
式中,M为滑动窗口的长度;
当时减小Rk;当/>时,保持Rk不变;
定义的Rk调整因子为:
采用自适应协方差调整策略不更新Q和R,只在计算卡尔曼滤波增益和状态估计误差协方差矩阵给出Q和R修正量;
在更新滤波器增益时,自适应调整方法如下:
状态估计误差协方差矩阵的自适应调整方法为:
步骤4:经过以上步骤,实现多旋翼无人机的自主稳定。
Claims (3)
1.一种复杂风场下的桥梁检测特种无人机自主稳定方法,其特征在于,包括如下步骤:
步骤1:桥梁检测特种无人机采用多旋翼无人机,对多旋翼无人机进行动力学建模;
步骤1-1:先进行如下假设:
(1)假设飞行区域的地面是一个平面,忽略地球自转,重力加速度设为常数;
(2)将多旋翼无人机机体结构和旋翼都视为刚体,忽略多旋翼无人机机体的弹性形变及振动;
(3)多旋翼无人机六个电机及旋翼均对称安装,且除正反极性外其他参数均相同;
(4)多旋翼无人机机体的质量分布均匀且质心与外形中心重合;
无人机在桥梁检测过程中受力包括:多旋翼无人机本体的重力、旋翼产生的升力、空气阻力;力矩包括:旋翼升力产生的空气动力矩、旋转产生的反扭力、空气阻力以及摩擦力产生的阻力矩;
步骤1-2:建立机体坐标系和地面坐标系;
为确定无人机飞行中的位置,建立惯性坐标系E-XYZ,地面坐标系原点是无人机起飞时的质心,X轴的正方向是起飞时无人机头的航向正方向,Y轴的正方向是起飞时无人机水平向左的方向,Z轴的正方向是起飞时无人机垂直向上的方向;
为确定无人机的姿态,建立机体坐标系B-XbYbZb,机体坐标系原点是无人机的质心,Xb轴的正方向是机头的方向,Zb轴的正方向是垂直于机体平面向上,Yb轴的正方向由右手准则确定;
步骤1-3:定义φ为无人机绕Xb轴转动的横滚角;θ为无人机绕Yb轴转动的俯仰角;ψ为旋翼无人机绕Zb轴转动的偏航角,则无人机姿态表示为Θ=[φ θ ψ]T;假设每个旋翼的升力和无人机平面垂直并指向上,此时每个旋翼的升力fi近似为:
其中,Ωi是旋翼转速;
此时六旋翼整体的推力表达为方向沿着-bZ的方向;总升力在惯性坐标系中表示为-fRe3∈R3,其中e3=[0,0,1]T,R表示本体坐标系到惯性坐标系的旋转矩阵;
步骤1-4:为了保证力矩平衡,在六旋翼中3个旋翼顺时针旋转,另外3个逆时针旋转;假设旋翼产生的扭矩与其升力成正比,此时得到每个旋翼的扭矩:
τi=±cτfi (2)
其中,cτ是常数,表示升力和扭矩的近似关系;
得到六旋翼总的升力f和总力矩M=[M1 M2 M3]T与各旋翼升力之间的关系:
其中d表示从螺旋桨中心到机体坐标系的距离;
步骤1-5:最终得到多旋翼无人机的动力学模型表示为:
无人机位置动力学模型为:F1:
无人机姿态动力学模型:F2:
其中,x是无人机相对于惯性坐标系的位置,m为无人机质量,g为重力加速度,df为扰动矢量,J为转动惯量,ω为角速度,dτ为扰动力矩矢量;
步骤2:设计多旋翼无人机抗风扰控制算法;
步骤2-1:采用基于几何控制理论的级联PID控制器作为无人机的控制器,由外控制环和内控制环构成,外控制环包括位置PID控制器和速度PID控制器,内控制环包括姿态PID控制器和角速度PID控制器,具体如下:
无人机的轨迹包括期望位置xd和期望航向ψd,位置跟踪误差定义为ex=xd-x;位置PID控制器设计为:
其中,vd表示期望速度,kPx、kDx和kIx分别表示位置PID控制器的比例、微分和积分参数;x表示实际位置;
给定期望速度后,定义速度跟踪误差ev=vd-v,速度PID控制器设计如下:
其中,Fd表示期望三轴力,kPv、kDv和kIv表示速度PID控制器的比例、微分和积分参数;v表示实际速度;
步骤2-2:引入高增益加速度反馈算法,包括线加速度反馈和角加速度反馈;
在没有干扰的理想情况下无人机位置动力学模型中的力学方程表示为:
其中是期望加速度,而考虑实际系统中存在扰动矢量df与设计的线加速度反馈项vf,则式(6)表示为:
式中,是无人机真实的线加速度;df和vf满足:
其中,Q(s)为滤波器;s为拉普拉斯算子;
结合以上式(7)和(8)得到设计的线加速度反馈项:
其中,a表示正常数;
此时,Fd与vf的和作为内控制环的输入,根据几何控制思想,Fd与vf被分解为模长f=||Fd+vf||和方向结合期望轨迹中的航向信息,计算出期望旋转矩阵Rd,根据几何控制,姿态跟踪误差表示为:
其中,映射V:将SO(3)映射成为一个3维向量,表示向量的反对称运算的逆运算,SO(3)表示一种特殊正交群;则姿态PID控制器为:
其中,ωd表示期望加速度,kPR、kDR和kIR为姿态PID控制器的比例、微分和积分参数;
根据期望角速度ωd定义为角速度误差eω=RTRdωd-ω,角加速度PID控制器为:
其中,为期望角加速度,kPω、kDω和kIω为角加速度PID控制器的比例、微分和积分参数;ω表示实际角速度;
得到如下形式的角加速度反馈控制器:
其中,Md是期望力矩,是无人机机体角加速度矢量;vτ(t)表示角加速度反馈控制器;b为常数;
步骤3:设计GPS信号阶段性失锁条件下的状态估计算法;
步骤3-1:采用GPS传感器和机载姿态确定装置获取无人机位姿信息,采用自适应卡尔曼滤波算法估计无人机的实时位置和姿态;
将无人机动力学模型统一转化为:
式中,f(x)=[F1 F2]T,w为系统高斯白噪声;
步骤3-2:考虑到无人机作业时GPS信号阶段性失锁的情况,系统的观测方程分以下两种情况:
第一种情况:当GPS传感器输出信息可用时,此时无人机系统的测量方程为:
其中,xk,yk,zk,θk,ψk分别表示k时刻的各项值;/>表示测量值;
考虑测量噪声,无人机系统的状态空间方程为:
式中,υ为测量传感器高斯白噪声,u表示控制输入,hk(.)表示厅时刻测量方程,f(.)表示系统状态函数,zk表示k时刻的测量函数;
第二种情况:当GPS信号失锁时:
由于桥梁检测无人机的飞行航向和速度阶段性保持不变,因此无人机的速度是先验已知的,即无人机飞行一定时间内的位移是先验已知的,因此,得到如下状态约束伪测量方程:
式中,vu为无人机飞行速度,tu为飞行时间,为k+1时刻无人机位置状态的期望值;xk-1、yk-1、zk-1分别表示k-1时刻的位置坐标;
步骤3-2:令将无人机的位置动力学模型变形为:
其中,
式(18)作为无人机另一个伪测量位置状态,即:
其中,wk表示k时刻的系统噪声;表示k+1时刻无人机位置状态的期望值;
联立以上式(18)和(19)得到系统的观测方程为:
经离散后,系统的动力学模型为:
式中,ΔT为离散时间,wk-1∈(0,Qk-1)和vk∈(0,Rk)为离散后的高斯白噪声;Qk-1表示系统噪声协方差矩阵,Rk表示测量噪声协方差矩阵;
步骤4:经过以上步骤,实现多旋翼无人机的自主稳定。
2.根据权利要求1所述的一种复杂风场下的桥梁检测特种无人机自主稳定方法,其特征在于,所述高增益加速度反馈具体如下:
首先利用欧拉-拉格朗日方程建立通用的动力学方程:
其中为广义位置向量,包括位置和角度;/>为惯性矩阵,/>为哥氏力项,/>为重力项,/>为摩擦阻力项,/>为驱动力或力矩项,为扰动力或力矩;
考虑无人机动力学模型中的任意一个自由度:
其中Jii是自由度的惯性系数;τi是控制输入;τui由各种耦合力以及扰动不确定项组成;表示哥氏力项矩阵的第i行第j元素,Gii(q)表示重力项矩阵中第i个对角线元素,表示摩擦阻力项矩阵中第i个对角线元素;qj表示状态分量中第j个元素;
高增益加速度反馈设计如下:
其中ka是正常数,v是上一层控制器的输出;qi表示表示状态分量中第i个元素;
联立以上两式得到:
若ka足够大,即ka>>1且ka>>Jii,则上式近似为:
此时,得到了一个加速度跟踪器,直接跟踪给定加速度信号,同时消除了扰动项τui的影响;
引入前置滤波器,得到:
此时引入前置滤波器得到:
无人机的任何一个自由度重新表示如下:
其中,A(s)、B(s)分别表示拉普拉斯变换后的多项式表达;
令ka→+∞得:
通过上式知不确定项τui经过一个滤波器Q(s)之后会对系统产生影响:
因为多旋翼无人机受到的扰动处于低频范围,因此选取Q(s)为高通滤波器的形式从而抑制低频扰动,选取:
其中a是正常数,系统表示为:
此时,扰动中的低频部分会被抑制,而高通滤波器的截止频率正好是a。
3.根据权利要求2所述的一种复杂风场下的桥梁检测特种无人机自主稳定方法,其特征在于,所述自适应扩展卡尔曼滤波算法步骤如下:
第一步:状态预测:
xk=xk-1+ΔTf(xk-1)
其中,ΔT表示采样时间,f(xk-1)表示k-1时刻的状态方程;
第二步:协方差预测:
式中,In为单位矩阵;Pk-1表示表示k-1时刻的协方差矩阵,Qk-1表示k-1时刻的系统噪声方差矩阵,表示k-1时刻的预测状态;
第三步:计算卡尔曼滤波增益:
其中,h表示测量函数,表示k时刻的预测状态;
第四步:更新状态变量,即计算状态变量的估计值:
第五步:更新状态估计误差协方差矩阵:
第六步:噪声协方差矩阵自适应调整策略:
新息定义为测量变量的实际值与预测值之间的误差,即:
新息的理论协方差矩阵:
Ck=E(vkvk T)=HkPkHk T+Rk
新息的协方差矩阵计算方法如下:
式中,M为滑动窗口的长度;
当时减小Rk;当/>时,保持Rk不变;
定义的Rk调整因子为:
采用自适应协方差调整策略不更新Q和R,只在计算卡尔曼滤波增益和状态估计误差协方差矩阵给出Q和R修正量;
在更新滤波器增益时,自适应调整方法如下:
状态估计误差协方差矩阵的自适应调整方法为:
式中,ak表示波器增益自适应调整系数阵,κ表示协方差矩阵的自适应调整系数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210439977.8A CN114942648B (zh) | 2022-04-25 | 2022-04-25 | 一种复杂风场下的桥梁检测特种无人机自主稳定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210439977.8A CN114942648B (zh) | 2022-04-25 | 2022-04-25 | 一种复杂风场下的桥梁检测特种无人机自主稳定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114942648A CN114942648A (zh) | 2022-08-26 |
CN114942648B true CN114942648B (zh) | 2024-05-03 |
Family
ID=82907588
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210439977.8A Active CN114942648B (zh) | 2022-04-25 | 2022-04-25 | 一种复杂风场下的桥梁检测特种无人机自主稳定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114942648B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117193346B (zh) * | 2023-10-26 | 2024-04-30 | 浙江大学 | 一种可进行自主导航的迷你共轴双旋翼无人机 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101264797A (zh) * | 2008-03-20 | 2008-09-17 | 北京航空航天大学 | 一种无人机滚转改平控制方法 |
WO2018028711A1 (zh) * | 2016-08-12 | 2018-02-15 | 贵州火星探索科技有限公司 | 一种对无人机的噪声协方差进行估算的方法 |
-
2022
- 2022-04-25 CN CN202210439977.8A patent/CN114942648B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101264797A (zh) * | 2008-03-20 | 2008-09-17 | 北京航空航天大学 | 一种无人机滚转改平控制方法 |
WO2018028711A1 (zh) * | 2016-08-12 | 2018-02-15 | 贵州火星探索科技有限公司 | 一种对无人机的噪声协方差进行估算的方法 |
Non-Patent Citations (1)
Title |
---|
多旋翼无人机强风环境优化控制仿真;李诚龙;潘卫军;;计算机仿真;20180315(03);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114942648A (zh) | 2022-08-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
Verling et al. | Full attitude control of a VTOL tailsitter UAV | |
Rodić et al. | The modeling and simulation of an autonomous quad-rotor microcopter in a virtual outdoor scenario | |
CN106249745A (zh) | 四轴无人机的控制方法 | |
CN105094138A (zh) | 一种用于旋翼无人机的低空自主导航系统 | |
CN104460685A (zh) | 一种四旋翼飞行器的控制系统及其控制方法 | |
Mahony et al. | A non-linear observer for attitude estimation of a fixed-wing unmanned aerial vehicle without GPS measurements | |
CN112346470A (zh) | 一种基于改进自抗扰控制的四旋翼姿态控制方法 | |
CN104635746A (zh) | 一种两轴四框架光电吊舱的伺服控制方法及系统 | |
CN108592911B (zh) | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 | |
Demitrit et al. | Model-based wind estimation for a hovering VTOL tailsitter UAV | |
CN103837151A (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
CN206050075U (zh) | 一种油动多旋翼无人机定桨距变转速系统 | |
CN107491083A (zh) | 一种基于饱和自适应滑模控制的四旋翼自主着船方法 | |
CN103217981A (zh) | 一种基于积分变结构控制的四旋翼飞行器速度控制方法 | |
Samar et al. | Autonomous terrain-following for unmanned air vehicles | |
JP2015024705A (ja) | 小型電動ヘリコプタの自動離着陸制御方法 | |
CN114942648B (zh) | 一种复杂风场下的桥梁检测特种无人机自主稳定方法 | |
CN113821044A (zh) | 基于强化学习的桥梁检测无人机自主导航与稳定控制方法 | |
Park | Wind and airspeed error estimation with GPS and pitot-static system for small UAV | |
CN106043695A (zh) | 一种油动多旋翼无人机定桨距变转速系统及控制技术 | |
Ahmadinejad et al. | Autonomous flight of quadcopters in the presence of ground effect | |
CN114489094A (zh) | 一种基于加速度反馈增强的旋翼无人机抗风扰控制方法 | |
Wu et al. | Model-free online motion adaptation for energy-efficient flight of multicopters | |
Park | Autonomous crabbing by estimating wind using only GPS velocity |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |