CN114526756B - 一种无人机机载多传感器校正方法、装置及存储介质 - Google Patents

一种无人机机载多传感器校正方法、装置及存储介质 Download PDF

Info

Publication number
CN114526756B
CN114526756B CN202210005058.XA CN202210005058A CN114526756B CN 114526756 B CN114526756 B CN 114526756B CN 202210005058 A CN202210005058 A CN 202210005058A CN 114526756 B CN114526756 B CN 114526756B
Authority
CN
China
Prior art keywords
triaxial
error model
correction
error
data
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
Application number
CN202210005058.XA
Other languages
English (en)
Other versions
CN114526756A (zh
Inventor
胡青春
肖舜仁
方展杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202210005058.XA priority Critical patent/CN114526756B/zh
Publication of CN114526756A publication Critical patent/CN114526756A/zh
Application granted granted Critical
Publication of CN114526756B publication Critical patent/CN114526756B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00Road transport of goods or passengers
    • Y02T10/10Internal combustion engine [ICE] based vehicles
    • Y02T10/40Engine management systems

Abstract

本发明公开了一种无人机机载多传感器校正方法、装置及存储介质,其中方法包括:建立三轴加速度计、三轴陀螺仪和三轴磁力计的误差模型;对三轴加速度计和三轴磁力计的误差进行优化校正;结合已校正的三轴加速度计和三轴磁力计,提出三轴陀螺仪联合校正方法,将陀螺仪误差模型参数估计问题转化为非线性函数优化问题;采集多传感器的数据,运用L‑M算法并基于四元数姿态解算方法对三轴陀螺仪进行联合校正,解得陀螺仪误差模型中待求参数的最优估计值。本发明不依赖于精密的实验设备,针对无人机机载传感器校正的快速性和准确性要求,克服了校正过程繁琐及联合校正局限性的问题,具有良好的通用性和适应性。本发明可广泛应用于无人机技术领域。

Description

一种无人机机载多传感器校正方法、装置及存储介质
技术领域
本发明涉及无人机技术领域,尤其涉及一种无人机机载多传感器校正方法、装置及存储介质。
背景技术
四旋翼无人机依靠自身搭载的低成本MEMS传感器实现姿态的实时获取是其在诸多应用领域中正常控制作业的先行条件,而基于MEMS的惯性传感器如三轴加速度计、三轴陀螺仪在使用过程中易受安装、测量、信号传输等误差的影响,导致无人姿态信息获取的精度降低。因此,在无人机投入使用前,需对其搭载的MEMS传感器进行误差校正补偿,以获得更为精确的传感器数据,为后续进行多源传感器信息融合与姿态解算奠定基础。
目前国内外学者对MEMS传感器的校正补偿问题已进行了大量的研究。较为传统的方法是借助高精度转台对三轴加速度计进行校正,该方法成本高且局限性大;不借助转台的校正方法又大多存在精度不足或校正繁琐的问题。现有的技术方法还使用改进的粒子群优化算法对磁力计误差模型中的12个未知参数进行了最优估计,该方法校正补偿精度较高,但算法实现较为繁琐且仅能进行参数离线估计。
另外,还有方法基于校正后的加速度计以及动态旋转完成陀螺仪误差参数的快速估计,但该方法未考虑磁力计与其他两种传感器之间的相互影响,且仅通过加速度计无法得出偏航角,联合校正存在局限性。
发明内容
为至少一定程度上解决现有技术中存在的技术问题之一,本发明的目的在于提供一种无人机机载多传感器校正方法、装置及存储介质。
本发明所采用的技术方案是:
一种无人机机载多传感器校正方法,包括以下步骤:
构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化;
构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化;
构建三轴陀螺仪的第三误差模型,将所述第三误差模型的参数估计问题转化为非线性函数优化问题,并构建第一优化函数;
获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据;
采用优化后的第一误差模型对所述第一数据进行校正,采用优化后的第二误差模型对所述第二数据进行校正;
结合所述第一优化函数、所述第三数据以及校正后的所述第一数据、所述第二数据,对所述第三误差模型的参数进行优化;
其中,误差模型用于对传感器的测量数据进行校正,以使测量数据趋近于理想输出值。
进一步地,所述构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化,包括:
结合所述三轴加速度计的非正交误差、尺度因子和零偏误差,建立所述第一误差模型;
根据L-M算法的原理,将所述第一误差模型的参数估计问题转化为非线性函数优化问题,并构建第二优化函数;
采集所述三轴加速度计在静态下的数据,根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值。
进一步地,所述第一误差模型的表达式为:
Figure BDA0003455203610000021
其中,
Figure BDA0003455203610000022
为校正前三轴加速度计测得的比力;/>
Figure BDA0003455203610000023
为校正后三轴加速度计在机体正交坐标系下的理想比力;bx、by、bz为传感器的三轴零偏;kx、ky、kz为传感器的三轴尺度因子;
所述第二优化函数的表达式为:
Figure BDA0003455203610000024
其中,
Figure BDA0003455203610000025
是校正后的理想加速度比力值;/>
Figure BDA0003455203610000026
为加速度计误差模型中待求参数的最优估计值;M为最大采样个数;g为地重力加速度。
进一步地,所述根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值,包括:
采用L-M算法对所述第二优化函数进行迭代求解,其中,定义误差函数f(ξ)和指标函数F(ξ),有:
Figure BDA0003455203610000031
L-M算法的迭代步长为:
Δx=-[J(x)TJ(x)+μI]-1[J(x)Tf(x)],μ≥0,用于解决Jacobian矩阵非满秩导致的奇异问题;
其中,J(x)为f(x)中待求参数的Jacobian矩阵;μ为阻尼系数;
使用L-M算法进行迭代前,给定待求参数ξ的初始值ξ0,首先求出f(ξ)中待求参数的Jacobian矩阵J(ξ0),并根据经验设置阻尼系数μ的初值μ0为:
Figure BDA0003455203610000032
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数;
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ,增益系数ρ的计算如下:
Figure BDA0003455203610000033
其中,h=Δξ,
Figure BDA0003455203610000034
若ρ>0,表明当前指标函数拥有下降趋势,减小μ的值,使下一次迭代更接近于高斯牛顿迭代;当ρ≤0时,增大μ的值,减小步长并使得下一次迭代更接近于最速下降迭代。
进一步地,所述构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化,包括:
结合所述三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,构建三轴磁力计的第二误差模型;
根据L-M算法的原理,将所述第二误差模型的参数估计问题转化为非线性函数优化问题,并构建第三优化函数;
其中,所述三轴磁力计的校正原理是任意姿态下的三轴磁力计,三轴磁力计理想的磁感应强度测量值的模等于当地的磁场强度M。
进一步地,所述第三优化函数的表达式为:
Figure BDA0003455203610000041
其中,
Figure BDA0003455203610000042
是当地磁场强度最优估计值;/>
Figure BDA0003455203610000043
是校正后的理想磁感应强度;/>
Figure BDA0003455203610000044
是三轴磁力计的误差模型中待求参数的最优估计值;N为最大采样个数。
进一步地,所述三轴陀螺仪的校正原理表示为:期望估计值q'k应尽可能与校正后的比力和磁场强度转换而成的四元数qk相接近;
根据所述三轴陀螺仪的校正原理,建立获得的所述第一优化函数的表达式如下:
Figure BDA0003455203610000045
其中,
Figure BDA0003455203610000046
为三轴陀螺仪的误差模型中待求参数的最优估计值;/>
Figure BDA0003455203610000047
为第k次与第k+1次三轴加速度计或三轴磁力计数据测量时刻之间采集到的N个角速度数据;q'k+1表示根据理想的角速度值/>
Figure BDA0003455203610000048
和qk计算得到的第k+1次四元数的估计值;
其中,qk通过以下方式得到:
令三轴加速度计和三轴磁力计第k次测量校正后得到的比力
Figure BDA0003455203610000049
和磁感应强度值
Figure BDA00034552036100000410
分别为:
Figure BDA00034552036100000411
则第k次测量校正得到的欧拉角为:
Figure BDA00034552036100000412
其中,
Figure BDA00034552036100000413
将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4
定义操作Ψ如下:
Figure BDA0003455203610000051
其中,γ为待求参数。
进一步地,所述获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据,包括:
将无人机分别绕机身x,y,z轴正方向匀速旋转,每次旋转角度不同,每旋转一次都需保持t时间段的静止,采集时间段t内的比力、磁感应强度均值和两个连续静态间的N个动态角速度值。
本发明所采用的另一技术方案是:
一种无人机机载多传感器校正装置,包括:
至少一个处理器;
至少一个存储器,用于存储至少一个程序;
当所述至少一个程序被所述至少一个处理器执行,使得所述至少一个处理器实现上所述方法。
本发明所采用的另一技术方案是:
一种计算机可读存储介质,其中存储有处理器可执行的程序,所述处理器可执行的程序在由处理器执行时用于执行如上所述方法。
本发明的有益效果是:本发明不依赖于精密的实验设备,针对无人机机载传感器校正的快速性和准确性要求,克服了校正过程繁琐及联合校正局限性的问题,可快速应对上述误差模型的更改,得到所述误差模型待求参数的最优估计,具有良好的通用性和适应性。
附图说明
为了更清楚地说明本发明实施例或者现有技术中的技术方案,下面对本发明实施例或者现有技术中的相关技术方案附图作以下介绍,应当理解的是,下面介绍中的附图仅仅为了方便清晰表述本发明的技术方案中的部分实施例,对于本领域的技术人员而言,在无需付出创造性劳动的前提下,还可以根据这些附图获取到其他附图。
图1是本发明实施例中三轴传感器坐标系与机体正交坐标系间的偏差图;
图2是本发明实施例中基于L-M算法的三轴加速度计迭代校正流程图;
图3是本发明实施例中校正前后比力模值与重力加速度的偏差图;
图4是本发明实施例中磁力计初始值与L-M算法校正后的数据三维分布图;
图5是本发明实施例中校正前后四元数解算值与估计值之差的模值对比图;
图6是本实施例中一种基于L-M算法的无人机机载多传感器校正方法的步骤流程图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。对于以下实施例中的步骤编号,其仅为了便于阐述说明而设置,对步骤之间的顺序不做任何限定,实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。
在本发明的描述中,需要理解的是,涉及到方位描述,例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
在本发明的描述中,若干的含义是一个或者多个,多个的含义是两个以上,大于、小于、超过等理解为不包括本数,以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。
本发明的描述中,除非另有明确的限定,设置、安装、连接等词语应做广义理解,所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
如图6所示,本实施例提供一种基于L-M算法的无人机机载多传感器校正方法,包括以下步骤:
步骤一、分析机载多传感器误差来源及误差建模。
定义三轴传感器的坐标系为SF,而理想的传感器正交坐标系SOF,两者之间的联系是:(1)SOF的x轴与SF的x轴重合;(2)SOF的y轴在SF的x轴与y轴构成的平面上。假设无人机机体坐标系为严格正交的BF,则非正交坐标系SF与正交坐标系BF的偏差如图1所示。
对于小角度而言,非正交坐标系下的测量值在机体正交坐标系下的变换关系如下式:
sB=TsS
其中,
Figure BDA0003455203610000061
sS与sB分别代表传感器坐标系与机体系下传感器测得的三轴数据。
本实施例所提出的传感器校正方法,需假设机体正交坐标系BF与理想坐标系SOF重合,即θxz、θxy、θyx都为0,则三轴传感器的非正交矩阵可简化为:
Figure BDA0003455203610000071
尺度因子误差矩阵为:
Figure BDA0003455203610000072
其中,kx、ky、kz即为传感器的三轴尺度因子。
零偏误差的矩阵表示为:
b=[bx by bz]T
其中,bx、by、bz指传感器的三轴零偏。
结合三轴加速度计的非正交误差、尺度因子和零偏误差,建立三轴加速度计的误差模型为:
Figure BDA0003455203610000073
其中,
Figure BDA0003455203610000074
为校正前加速度计测得的比力;/>
Figure BDA0003455203610000075
为校正后加速度计在机体正交坐标系下的理想比力。
结合三轴陀螺仪的非正交误差、尺度因子和零偏误差,建立三轴陀螺仪的误差模型为:
Figure BDA0003455203610000076
其中,
Figure BDA0003455203610000077
为校正前陀螺仪测得的角速度;/>
Figure BDA0003455203610000078
为校正后陀螺仪在机体正交坐标系下的理想角速度。
结合三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,可建立磁力计误差模型。为简化矩阵运算,将非正交误差矩阵、尺度因子矩阵与软磁误差矩阵相结合,又对零偏、硬磁误差一并校正,则磁力计误差模型可简化为:
mB=H(mS+B)
其中,H为比例矩阵,B为三轴偏置矩阵,误差模型展开形式如下:
Figure BDA0003455203610000081
其中,
Figure BDA0003455203610000082
为校正前磁力计测得的磁感应强度;/>
Figure BDA0003455203610000083
为校正后磁力计在机体正交坐标系下的理想磁感应强度。
步骤二、使用L-M算法对三轴加速度计和三轴磁力计的优化函数进行迭代求解。
1)以三轴加速度计为例。
所述三轴加速度计校正原理为:静止状态下任意姿态摆放的三轴加速度计,其理想比力测量值的模值等于当地重力加速度。根据其校正原理,建立以下优化函数来估计待求参数:
Figure BDA0003455203610000084
使用L-M算法对上述优化函数进行迭代求解,定义误差函数f(ξ)和指标函数F(ξ),有:
Figure BDA0003455203610000085
L-M算法的迭代步长为:
Δx=-[J(x)TJ(x)+μI]-1[J(x)Tf(x)],μ≥0
该步长可用于解决Jacobian矩阵非满秩导致的奇异问题。其中,J(x)为f(x)中9个待求参数的Jacobian矩阵;μ为阻尼系数。
使用L-M算法进行迭代前,需给定待求参数ξ的初始值ξ0,首先求出f(ξ)中9个待求参数的Jacobian矩阵J(ξ0),并根据经验设置阻尼系数μ的初值μ0为:
Figure BDA0003455203610000086
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数,本实施例中取τ为10-5
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ。增益系数ρ的计算如下:
Figure BDA0003455203610000091
其中,h=Δξ,
Figure BDA0003455203610000092
若ρ>0,表明当前指标函数拥有下降趋势,此时可减小μ的值,使下一次迭代更接近于高斯牛顿迭代;当ρ≤0时,应增大μ的值,减小步长并使得下一次迭代更接近于最速下降迭代。
综上,所述基于L-M算法进行三轴加速度计校正的迭代校正流程如图2,其中迭代终止常数ε1=ε2=10-8,其他个别参数采用默认或经验值,在迭代过程中,当变量满足一定收敛条件时,迭代停止,判定算法迭代收敛,可将此时待求参数ξ的估计作为最优估计输出。
2)对上述三轴磁力计的优化函数进行迭代求解。
三轴磁力计误差模型的待求参数的迭代求解过程与三轴加速度计类似,其校正原理为:任意姿态下的三轴磁力计,其理想的磁感应强度测量值的模等于当地的磁场强度。但一般情况下,不同区域的磁场强度并不一致,因此应先对当地磁场强度M进行最优估计,再以M的最优值构建误差函数,借助L-M算法完成磁力计的迭代校正,得出误差模型待求参数的最优估计值,具体地:
第一步:运用L-M算法对当地磁场强度M进行最优估计,只对偏置误差(零偏、硬磁误差)进行修正,误差模型为
Figure BDA0003455203610000093
此时磁力计数据分布在空间坐标系内与原点偏置的规整球面上,球面半径即当地磁场强度M。可建立以下优化函数:
Figure BDA0003455203610000094
给定初始参数
Figure BDA0003455203610000095
从而采用L-M算法进行迭代校正得到磁场强度最优估计/>
Figure BDA0003455203610000096
和三轴磁力计的偏置误差B。
第二步:运用L-M算法对比例误差和偏置误差矩阵参数进行最优估计,此时磁力计的误差模型为
Figure BDA0003455203610000097
以第一步所述的磁场强度最优估计值/>
Figure BDA0003455203610000098
构建误差函数
Figure BDA0003455203610000101
和指标函数/>
Figure BDA0003455203610000102
建立以下优化函数:
Figure BDA0003455203610000103
以第一步所述的偏置误差B的估计值作为初始值B0,另给定比例误差参数的初始值H0,从而采用L-M算法进行迭代校正得到磁力计待求参数的最优估计
Figure BDA0003455203610000104
步骤三、采集三轴加速度计和三轴磁力计数据,使用上述L-M算法的迭代求解方法得误差模型的待求参数估计值。
本发明实施例是通过装载MPU9250型MEMS传感器的小型无人机实现的。
1)分别采集无人机18方向静止时的姿态信息。以机体轴X-Y-Z进行描述,该18个静止姿态分别为:X轴垂直向上、X轴垂直向下、Y轴垂直向上、Y轴垂直向下、Z轴垂直向上、Z轴垂直向下;X轴与水平方向呈±30°、±45°、±60°夹角;Y轴与水平方向呈±30°、±45°、±60°夹角,将采集到的加速度计比力数据通过串口发送到上位机中进行记录保存。筛取上述各静止姿态比力数据并取均值,使用L-M算法对加速度计误差模型中的待求参数进行最优估计,非正交误差、尺度因子、零偏误差的最优迭代估计结果如下:
Figure BDA0003455203610000105
Figure BDA0003455203610000106
Figure BDA0003455203610000107
由上述迭代估计参数对加速度计测量值进行误差校正,图3为校正前后18位置的加速度计比力模值与重力加速度的差值随迭代采样的变化。可知校正后的加速度计比力数据模值与重力加速度的差值在0附近小范围波动,远小于校正前。
2)分别采集无人机绕机身X、Y、Z轴和两条机臂轴旋转数周的磁感应强度数据,使用L-M算法对磁力计误差模型中的待求参数进行最优估计,当地磁场强度、比例误差和偏置误差的最优迭代估计结果如下:
Figure BDA0003455203610000111
Figure BDA0003455203610000112
Figure BDA0003455203610000113
由上述迭代估计参数对磁力计测量值进行误差校正,图4为磁力计采集的数据初始值和L-M算法校正后磁力计数据的理想值在三维空间的分布状况,可知校正前磁力计采集的数据与坐标原点偏差较大,且样本点分布呈近似椭球型,校正后的磁感应强度理想值分布在球心为坐标原点,半径为地磁强度的球面上。
步骤四、使用L-M算法对三轴陀螺仪联合校正的优化函数进行迭代求解。
在已校正的加速度计和磁力计的基础上,本发明实施例提出了陀螺仪联合校正方法。
令加速度计和磁力计第k次测量校正后得到的比力
Figure BDA0003455203610000114
和磁感应强度值/>
Figure BDA0003455203610000115
分别为:
Figure BDA0003455203610000116
则第k次测量校正得到的欧拉角为:
Figure BDA0003455203610000117
其中,
Figure BDA0003455203610000118
为避免奇异问题,使用四元数进行姿态更新,将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4
定义操作Ψ如下:
Figure BDA0003455203610000119
其中,γ为待求参数;
Figure BDA0003455203610000121
为第k次与第k+1次加速度计或磁力计数据测量时刻之间采集到的N个角速度数据。具体地,将无人机分别绕机身x,y,z轴正方向匀速旋转,每次旋转角度不一,每旋转一次都需保持t时间段的静止,采集时间段t内的比力、磁感应强度均值和两个连续静态间的N个动态角速度值。qk校正后的比力和磁感应强度转换而成的四元数;q'k+1表示根据理想的角速度值/>
Figure BDA0003455203610000122
和qk通过RK4方法计算得到的第k+1次四元数的估计值。
综上,可建立以下优化函数来估计三轴陀螺仪误差模型中的待求参数:
Figure BDA0003455203610000123
步骤五、同时采集机载加速度计、磁力计和陀螺仪数据,求解非线性函数优化问题,得到所述陀螺仪误差模型中待求参数的最优估计值。
将无人机分别绕机身x,y,z轴正方向匀速旋转三次,每次旋转的初始位置和角度不一,采集旋转前后静态区间内的加速度计、磁力计和两静态区间内的陀螺仪数据,使用L-M算法对陀螺仪误差模型中的待求参数进行最优估计,非正交误差、尺度因子、零偏误差的最优迭代估计结果如下:
Figure BDA0003455203610000124
Figure BDA0003455203610000125
Figure BDA0003455203610000126
由上述迭代估计参数进行陀螺仪校正,图5为校正前后无人机匀速旋转后静态测量的比力和磁感应强度数据解算而成的四元数,与根据RK4方法计算的四元数估计值之差的模值随迭代采样的变化,可知基于L-M算法对陀螺仪进行联合校正后,四元数解算值与估计值之差的模值显著减小。
本实施例还提供一种无人机机载多传感器校正装置,包括:
至少一个处理器;
至少一个存储器,用于存储至少一个程序;
当所述至少一个程序被所述至少一个处理器执行,使得所述至少一个处理器实现图6所述的方法。
本实施例的一种无人机机载多传感器校正装置,可执行本发明方法实施例所提供的一种无人机机载多传感器校正方法,可执行方法实施例的任意组合实施步骤,具备该方法相应的功能和有益效果。
本申请实施例还公开了一种计算机程序产品或计算机程序,该计算机程序产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存介质中。计算机设备的处理器可以从计算机可读存储介质读取该计算机指令,处理器执行该计算机指令,使得该计算机设备执行图6所示的方法。
本实施例还提供了一种存储介质,存储有可执行本发明方法实施例所提供的一种无人机机载多传感器校正方法的指令或程序,当运行该指令或程序时,可执行方法实施例的任意组合实施步骤,具备该方法相应的功能和有益效果。
在一些可选择的实施例中,在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如,取决于所涉及的功能/操作,连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外,在本发明的流程图中所呈现和描述的实施例以示例的方式被提供,目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的,其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。
此外,虽然在功能性模块的背景下描述了本发明,但应当理解的是,除非另有相反说明,所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中,或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是,有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说,考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下,在工程师的常规技术内将会了解该模块的实际实现。因此,本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是,所公开的特定概念仅仅是说明性的,并不意在限制本发明的范围,本发明的范围由所附权利要求书及其等同方案的全部范围来决定。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
在本说明书的上述描述中,参考术语“一个实施方式/实施例”、“另一实施方式/实施例”或“某些实施方式/实施例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施方式,本领域的普通技术人员可以理解:在不脱离本发明的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型,本发明的范围由权利要求及其等同物限定。
以上是对本发明的较佳实施进行了具体说明,但本发明并不限于上述实施例,熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换,这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (7)

1.一种无人机机载多传感器校正方法,其特征在于,包括以下步骤:
构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化;
构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化;
构建三轴陀螺仪的第三误差模型,将所述第三误差模型的参数估计问题转化为非线性函数优化问题,并构建第一优化函数;
获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据;
采用优化后的第一误差模型对所述第一数据进行校正,采用优化后的第二误差模型对所述第二数据进行校正;
结合所述第一优化函数、所述第三数据以及校正后的所述第一数据、所述第二数据,对所述第三误差模型的参数进行优化;
其中,误差模型用于对传感器的测量数据进行校正,以使测量数据趋近于理想输出值;
所述构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化,包括:
结合所述三轴加速度计的非正交误差、尺度因子和零偏误差,建立所述第一误差模型;
根据L-M算法的原理,将所述第一误差模型的参数估计问题转化为非线性函数优化问题,并构建第二优化函数;
采集所述三轴加速度计在静态下的数据,根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值;
所述构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化,包括:
结合所述三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,构建三轴磁力计的第二误差模型;
根据L-M算法的原理,将所述第二误差模型的参数估计问题转化为非线性函数优化问题,并构建第三优化函数;
其中,所述三轴磁力计的校正原理是任意姿态下的三轴磁力计,三轴磁力计理想的磁感应强度测量值的模等于当地的磁场强度M;
所述三轴陀螺仪的校正原理表示为:期望估计值q'k应尽可能与校正后的比力和磁场强度转换而成的四元数qk相接近;
根据所述三轴陀螺仪的校正原理,建立获得的所述第一优化函数的表达式如下:
Figure FDA0004188038650000021
其中,
Figure FDA0004188038650000022
为三轴陀螺仪的误差模型中待求参数的最优估计值;/>
Figure FDA0004188038650000023
为第k次与第k+1次三轴加速度计或三轴磁力计数据测量时刻之间采集到的N个角速度数据;q'k+1表示根据理想的角速度值/>
Figure FDA0004188038650000024
和qk计算得到的第k+1次四元数的估计值;
其中,qk通过以下方式得到:
令三轴加速度计和三轴磁力计第k次测量校正后得到的比力
Figure FDA0004188038650000025
和磁感应强度值/>
Figure FDA0004188038650000026
分别为:
Figure FDA0004188038650000027
则第k次测量校正得到的欧拉角为:
Figure FDA0004188038650000028
其中,
Figure FDA0004188038650000029
将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4
定义操作Ψ如下:
Figure FDA0004188038650000031
其中,γ为待求参数,g为地重力加速度。
2.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述第一误差模型的表达式为:
Figure FDA0004188038650000032
其中,
Figure FDA0004188038650000033
为校正前三轴加速度计测得的比力;/>
Figure FDA0004188038650000034
为校正后三轴加速度计在机体正交坐标系下的理想比力;/>
Figure FDA0004188038650000035
为传感器的三轴零偏;/>
Figure FDA0004188038650000036
为传感器的三轴尺度因子;
所述第二优化函数的表达式为:
Figure FDA0004188038650000037
其中,
Figure FDA0004188038650000038
是校正后的理想加速度比力值;/>
Figure FDA0004188038650000039
为加速度计误差模型中待求参数的最优估计值;M为最大采样个数;g为地重力加速度。
3.根据权利要求2所述的一种无人机机载多传感器校正方法,其特征在于,所述根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值,包括:
采用L-M算法对所述第二优化函数进行迭代求解,其中,定义误差函数f(ξ)和指标函数F(ξ),有:
Figure FDA00041880386500000310
L-M算法的迭代步长为:
Δx=-[J(x)TJ(x)+μI]-1[J(x)Tf(x)],μ≥0,用于解决Jacobian矩阵非满秩导致的奇异问题;
其中,J(x)为f(x)中待求参数的Jacobian矩阵;μ为阻尼系数;
使用L-M算法进行迭代前,给定待求参数ξ的初始值ξ0,首先求出f(ξ)中待求参数的Jacobian矩阵J(ξ0),并根据经验设置阻尼系数μ的初值μ0为:
Figure FDA0004188038650000041
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数;
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ,增益系数ρ的计算如下:
Figure FDA0004188038650000042
其中,h=Δξ,
Figure FDA0004188038650000043
若ρ>0,表明当前指标函数拥有下降趋势,减小μ的值,使下一次迭代更接近于高斯牛顿迭代;当ρ≤0时,增大μ的值,减小步长并使得下一次迭代更接近于最速下降迭代。
4.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述第三优化函数的表达式为:
Figure FDA0004188038650000044
其中,
Figure FDA0004188038650000045
是当地磁场强度最优估计值;/>
Figure FDA0004188038650000046
是校正后的理想磁感应强度;/>
Figure FDA0004188038650000047
是三轴磁力计的误差模型中待求参数的最优估计值;N为最大采样个数。
5.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据,包括:
将无人机分别绕机身x,y,z轴正方向匀速旋转,每次旋转角度不同,每旋转一次都需保持t时间段的静止,采集时间段t内的比力、磁感应强度均值和两个连续静态间的N个动态角速度值。
6.一种无人机机载多传感器校正装置,其特征在于,包括:
至少一个处理器;
至少一个存储器,用于存储至少一个程序;
当所述至少一个程序被所述至少一个处理器执行,使得所述至少一个处理器实现权利要求1-5任一项所述方法。
7.一种计算机可读存储介质,其中存储有处理器可执行的程序,其特征在于,所述处理器可执行的程序在由处理器执行时用于执行如权利要求1-5任一项所述方法。
CN202210005058.XA 2022-01-04 2022-01-04 一种无人机机载多传感器校正方法、装置及存储介质 Active CN114526756B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210005058.XA CN114526756B (zh) 2022-01-04 2022-01-04 一种无人机机载多传感器校正方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210005058.XA CN114526756B (zh) 2022-01-04 2022-01-04 一种无人机机载多传感器校正方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN114526756A CN114526756A (zh) 2022-05-24
CN114526756B true CN114526756B (zh) 2023-06-16

Family

ID=81620243

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210005058.XA Active CN114526756B (zh) 2022-01-04 2022-01-04 一种无人机机载多传感器校正方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN114526756B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN109459586A (zh) * 2018-12-05 2019-03-12 智灵飞(北京)科技有限公司 一种基于lm算法的无人机加速度计标定方法
CN110440746A (zh) * 2019-08-05 2019-11-12 桂林电子科技大学 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN110887480A (zh) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 基于mems传感器的飞行姿态估计方法及系统
CN111351482A (zh) * 2020-03-19 2020-06-30 南京理工大学 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN112577518A (zh) * 2020-11-19 2021-03-30 北京华捷艾米科技有限公司 一种惯性测量单元标定方法及装置
CN112945271A (zh) * 2021-01-27 2021-06-11 苏州大学 磁力计信息辅助的mems陀螺仪标定方法及标定系统
CN113670314A (zh) * 2021-08-20 2021-11-19 西南科技大学 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9417091B2 (en) * 2013-05-13 2016-08-16 The Johns Hopkins University System and method for determining and correcting field sensors errors
US20160178657A9 (en) * 2013-12-23 2016-06-23 InvenSense, Incorporated Systems and methods for sensor calibration
WO2017135836A1 (en) * 2016-02-01 2017-08-10 Limited Liability Company "Topcon Positioning Systems A method and device for calibration of a three-axis magnetometer

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN109459586A (zh) * 2018-12-05 2019-03-12 智灵飞(北京)科技有限公司 一种基于lm算法的无人机加速度计标定方法
CN110440746A (zh) * 2019-08-05 2019-11-12 桂林电子科技大学 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN110887480A (zh) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 基于mems传感器的飞行姿态估计方法及系统
CN111351482A (zh) * 2020-03-19 2020-06-30 南京理工大学 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN112577518A (zh) * 2020-11-19 2021-03-30 北京华捷艾米科技有限公司 一种惯性测量单元标定方法及装置
CN112945271A (zh) * 2021-01-27 2021-06-11 苏州大学 磁力计信息辅助的mems陀螺仪标定方法及标定系统
CN113670314A (zh) * 2021-08-20 2021-11-19 西南科技大学 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Novel Noniterative Orientation Estimation for Wearable Motion Sensor Units Acquiring Accelerometer, Gyroscope, and Magnetometer Measurements;Aras Yurtman等;《IEEE Transactions on Instrumentation and Measurement 》;第69卷(第6期);第3206-3215页 *
多旋翼无人机的低成本Inertial/GNSS/Vision组合导航关键技术研究;吴和龙;《中国博士学位论文全文数据库工程科技Ⅱ辑(月刊)》(第8期);第C031-13页 *

Also Published As

Publication number Publication date
CN114526756A (zh) 2022-05-24

Similar Documents

Publication Publication Date Title
CN109696183B (zh) 惯性测量单元的标定方法及装置
CN109813336B (zh) 惯性测量单元标定方法
CN106885566B (zh) 一种可穿戴式运动传感器及其抗磁场干扰的方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN111590564A (zh) 一种六维力传感器重力补偿方法及系统
CN113008227B (zh) 一种基于三轴加速度计测姿的地磁二分量测量方法
CN114216456B (zh) 一种基于imu与机器人本体参数融合的姿态测量方法
CN111780786A (zh) 一种三轴tmr传感器在线校准方法
WO2014070399A1 (en) Estimating the gravity vector in a world coordinate system using an accelerometer in a mobile device
CN109724602A (zh) 一种基于硬件fpu的姿态解算系统及其解算方法
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN113916256B (zh) 三轴mems陀螺组合惯性测量单元标定方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN105910593A (zh) 一种校准终端的地磁传感器的方法及装置
CN108318027A (zh) 载体的姿态数据的确定方法和装置
CN106813679A (zh) 运动物体的姿态估计的方法及装置
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN106556384A (zh) 一种筒形摄像机中的电子罗盘的校准补偿方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN110567493B (zh) 一种磁力计校准数据采集方法、装置以及飞行器
CN110728716B (zh) 一种标定方法、装置及飞行器
CN112665574A (zh) 基于动量梯度下降法的水下机器人姿态采集方法
CN111521196A (zh) 传感器校正方法、装置、虚拟现实设备、存储介质
CN108225371A (zh) 一种惯导/相机安装误差标定方法
CN114526756B (zh) 一种无人机机载多传感器校正方法、装置及存储介质

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