CN114526756A - 一种无人机机载多传感器校正方法、装置及存储介质 - Google Patents
一种无人机机载多传感器校正方法、装置及存储介质 Download PDFInfo
- Publication number
- CN114526756A CN114526756A CN202210005058.XA CN202210005058A CN114526756A CN 114526756 A CN114526756 A CN 114526756A CN 202210005058 A CN202210005058 A CN 202210005058A CN 114526756 A CN114526756 A CN 114526756A
- Authority
- CN
- China
- Prior art keywords
- error model
- axis
- error
- data
- value
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000012937 correction Methods 0.000 title claims abstract description 53
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 37
- 230000008569 process Effects 0.000 claims abstract description 9
- 230000006870 function Effects 0.000 claims description 55
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000005259 measurement Methods 0.000 claims description 20
- 230000006698 induction Effects 0.000 claims description 18
- 230000003068 static effect Effects 0.000 claims description 12
- 238000013016 damping Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 230000002829 reductive effect Effects 0.000 claims description 5
- 230000003247 decreasing effect Effects 0.000 claims description 3
- 238000013459 approach Methods 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000000670 limiting effect Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种无人机机载多传感器校正方法、装置及存储介质,其中方法包括:建立三轴加速度计、三轴陀螺仪和三轴磁力计的误差模型;对三轴加速度计和三轴磁力计的误差进行优化校正;结合已校正的三轴加速度计和三轴磁力计,提出三轴陀螺仪联合校正方法,将陀螺仪误差模型参数估计问题转化为非线性函数优化问题;采集多传感器的数据,运用L‑M算法并基于四元数姿态解算方法对三轴陀螺仪进行联合校正,解得陀螺仪误差模型中待求参数的最优估计值。本发明不依赖于精密的实验设备,针对无人机机载传感器校正的快速性和准确性要求,克服了校正过程繁琐及联合校正局限性的问题,具有良好的通用性和适应性。本发明可广泛应用于无人机技术领域。
Description
技术领域
本发明涉及无人机技术领域,尤其涉及一种无人机机载多传感器校正方法、装置及存储介质。
背景技术
四旋翼无人机依靠自身搭载的低成本MEMS传感器实现姿态的实时获取是其在诸多应用领域中正常控制作业的先行条件,而基于MEMS的惯性传感器如三轴加速度计、三轴陀螺仪在使用过程中易受安装、测量、信号传输等误差的影响,导致无人姿态信息获取的精度降低。因此,在无人机投入使用前,需对其搭载的MEMS传感器进行误差校正补偿,以获得更为精确的传感器数据,为后续进行多源传感器信息融合与姿态解算奠定基础。
目前国内外学者对MEMS传感器的校正补偿问题已进行了大量的研究。较为传统的方法是借助高精度转台对三轴加速度计进行校正,该方法成本高且局限性大;不借助转台的校正方法又大多存在精度不足或校正繁琐的问题。现有的技术方法还使用改进的粒子群优化算法对磁力计误差模型中的12个未知参数进行了最优估计,该方法校正补偿精度较高,但算法实现较为繁琐且仅能进行参数离线估计。
另外,还有方法基于校正后的加速度计以及动态旋转完成陀螺仪误差参数的快速估计,但该方法未考虑磁力计与其他两种传感器之间的相互影响,且仅通过加速度计无法得出偏航角,联合校正存在局限性。
发明内容
为至少一定程度上解决现有技术中存在的技术问题之一,本发明的目的在于提供一种无人机机载多传感器校正方法、装置及存储介质。
本发明所采用的技术方案是:
一种无人机机载多传感器校正方法,包括以下步骤:
构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化;
构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化;
构建三轴陀螺仪的第三误差模型,将所述第三误差模型的参数估计问题转化为非线性函数优化问题,并构建第一优化函数;
获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据;
采用优化后的第一误差模型对所述第一数据进行校正,采用优化后的第二误差模型对所述第二数据进行校正;
结合所述第一优化函数、所述第三数据以及校正后的所述第一数据、所述第二数据,对所述第三误差模型的参数进行优化;
其中,误差模型用于对传感器的测量数据进行校正,以使测量数据趋近于理想输出值。
进一步地,所述构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化,包括:
结合所述三轴加速度计的非正交误差、尺度因子和零偏误差,建立所述第一误差模型;
根据L-M算法的原理,将所述第一误差模型的参数估计问题转化为非线性函数优化问题,并构建第二优化函数;
采集所述三轴加速度计在静态下的数据,根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值。
进一步地,所述第一误差模型的表达式为:
所述第二优化函数的表达式为:
进一步地,所述根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值,包括:
采用L-M算法对所述第二优化函数进行迭代求解,其中,定义误差函数f(ξ)和指标函数F(ξ),有:
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为:
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数;
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ,增益系数ρ的计算如下:
若ρ>0,表明当前指标函数拥有下降趋势,减小μ的值,使下一次迭代更接近于高斯牛顿迭代;当ρ≤0时,增大μ的值,减小步长并使得下一次迭代更接近于最速下降迭代。
进一步地,所述构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化,包括:
结合所述三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,构建三轴磁力计的第二误差模型;
根据L-M算法的原理,将所述第二误差模型的参数估计问题转化为非线性函数优化问题,并构建第三优化函数;
其中,所述三轴磁力计的校正原理是任意姿态下的三轴磁力计,三轴磁力计理想的磁感应强度测量值的模等于当地的磁场强度M。
进一步地,所述第三优化函数的表达式为:
进一步地,所述三轴陀螺仪的校正原理表示为:期望估计值q'k应尽可能与校正后的比力和磁场强度转换而成的四元数qk相接近;
根据所述三轴陀螺仪的校正原理,建立获得的所述第一优化函数的表达式如下:
其中,为三轴陀螺仪的误差模型中待求参数的最优估计值;为第k次与第k+1次三轴加速度计或三轴磁力计数据测量时刻之间采集到的N个角速度数据;q'k+1表示根据理想的角速度值和qk计算得到的第k+1次四元数的估计值;
其中,qk通过以下方式得到:
则第k次测量校正得到的欧拉角为:
将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4;
定义操作Ψ如下:
其中,γ为待求参数。
进一步地,所述获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据,包括:
将无人机分别绕机身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
本实施例所提出的传感器校正方法,需假设机体正交坐标系BF与理想坐标系SOF重合,即θxzθxyθyx都为0,则三轴传感器的非正交矩阵可简化为:
尺度因子误差矩阵为:
其中,kx、ky、kz即为传感器的三轴尺度因子。
零偏误差的矩阵表示为:
b=[bx by bz]T
其中,bx、by、bz指传感器的三轴零偏。
结合三轴加速度计的非正交误差、尺度因子和零偏误差,建立三轴加速度计的误差模型为:
结合三轴陀螺仪的非正交误差、尺度因子和零偏误差,建立三轴陀螺仪的误差模型为:
结合三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,可建立磁力计误差模型。为简化矩阵运算,将非正交误差矩阵、尺度因子矩阵与软磁误差矩阵相结合,又对零偏、硬磁误差一并校正,则磁力计误差模型可简化为:
mB=H(mS+B)
其中,H为比例矩阵,B为三轴偏置矩阵,误差模型展开形式如下:
步骤二、使用L-M算法对三轴加速度计和三轴磁力计的优化函数进行迭代求解。
1)以三轴加速度计为例。
所述三轴加速度计校正原理为:静止状态下任意姿态摆放的三轴加速度计,其理想比力测量值的模值等于当地重力加速度。根据其校正原理,建立以下优化函数来估计待求参数:
使用L-M算法对上述优化函数进行迭代求解,定义误差函数f(ξ)和指标函数F(ξ),有:
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为:
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数,本实施例中取τ为10-5。
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ。增益系数ρ的计算如下:
综上,所述基于L-M算法进行三轴加速度计校正的迭代校正流程如图2,其中迭代终止常数ε1=ε2=10-8,其他个别参数采用默认或经验值,在迭代过程中,当变量满足一定收敛条件时,迭代停止,判定算法迭代收敛,可将此时待求参数ξ的估计作为最优估计输出。
2)对上述三轴磁力计的优化函数进行迭代求解。
三轴磁力计误差模型的待求参数的迭代求解过程与三轴加速度计类似,其校正原理为:任意姿态下的三轴磁力计,其理想的磁感应强度测量值的模等于当地的磁场强度。但一般情况下,不同区域的磁场强度并不一致,因此应先对当地磁场强度M进行最优估计,再以M的最优值构建误差函数,借助L-M算法完成磁力计的迭代校正,得出误差模型待求参数的最优估计值,具体地:
第一步:运用L-M算法对当地磁场强度M进行最优估计,只对偏置误差(零偏、硬磁误差)进行修正,误差模型为此时磁力计数据分布在空间坐标系内与原点偏置的规整球面上,球面半径即当地磁场强度M。可建立以下优化函数:
步骤三、采集三轴加速度计和三轴磁力计数据,使用上述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算法对加速度计误差模型中的待求参数进行最优估计,非正交误差、尺度因子、零偏误差的最优迭代估计结果如下:
由上述迭代估计参数对加速度计测量值进行误差校正,图3为校正前后18位置的加速度计比力模值与重力加速度的差值随迭代采样的变化。可知校正后的加速度计比力数据模值与重力加速度的差值在0附近小范围波动,远小于校正前。
2)分别采集无人机绕机身X、Y、Z轴和两条机臂轴旋转数周的磁感应强度数据,使用L-M算法对磁力计误差模型中的待求参数进行最优估计,当地磁场强度、比例误差和偏置误差的最优迭代估计结果如下:
由上述迭代估计参数对磁力计测量值进行误差校正,图4为磁力计采集的数据初始值和L-M算法校正后磁力计数据的理想值在三维空间的分布状况,可知校正前磁力计采集的数据与坐标原点偏差较大,且样本点分布呈近似椭球型,校正后的磁感应强度理想值分布在球心为坐标原点,半径为地磁强度的球面上。
步骤四、使用L-M算法对三轴陀螺仪联合校正的优化函数进行迭代求解。
在已校正的加速度计和磁力计的基础上,本发明实施例提出了陀螺仪联合校正方法。
则第k次测量校正得到的欧拉角为:
为避免奇异问题,使用四元数进行姿态更新,将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4。
定义操作Ψ如下:
其中,γ为待求参数;为第k次与第k+1次加速度计或磁力计数据测量时刻之间采集到的N个角速度数据。具体地,将无人机分别绕机身x,y,z轴正方向匀速旋转,每次旋转角度不一,每旋转一次都需保持t时间段的静止,采集时间段t内的比力、磁感应强度均值和两个连续静态间的N个动态角速度值。qk校正后的比力和磁感应强度转换而成的四元数;q'k+1表示根据理想的角速度值和qk通过RK4方法计算得到的第k+1次四元数的估计值。
综上,可建立以下优化函数来估计三轴陀螺仪误差模型中的待求参数:
步骤五、同时采集机载加速度计、磁力计和陀螺仪数据,求解非线性函数优化问题,得到所述陀螺仪误差模型中待求参数的最优估计值。
将无人机分别绕机身x,y,z轴正方向匀速旋转三次,每次旋转的初始位置和角度不一,采集旋转前后静态区间内的加速度计、磁力计和两静态区间内的陀螺仪数据,使用L-M算法对陀螺仪误差模型中的待求参数进行最优估计,非正交误差、尺度因子、零偏误差的最优迭代估计结果如下:
由上述迭代估计参数进行陀螺仪校正,图5为校正前后无人机匀速旋转后静态测量的比力和磁感应强度数据解算而成的四元数,与根据RK4方法计算的四元数估计值之差的模值随迭代采样的变化,可知基于L-M算法对陀螺仪进行联合校正后,四元数解算值与估计值之差的模值显著减小。
本实施例还提供一种无人机机载多传感器校正装置,包括:
至少一个处理器;
至少一个存储器,用于存储至少一个程序;
当所述至少一个程序被所述至少一个处理器执行,使得所述至少一个处理器实现图6所述的方法。
本实施例的一种无人机机载多传感器校正装置,可执行本发明方法实施例所提供的一种无人机机载多传感器校正方法,可执行方法实施例的任意组合实施步骤,具备该方法相应的功能和有益效果。
本申请实施例还公开了一种计算机程序产品或计算机程序,该计算机程序产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存介质中。计算机设备的处理器可以从计算机可读存储介质读取该计算机指令,处理器执行该计算机指令,使得该计算机设备执行图6所示的方法。
本实施例还提供了一种存储介质,存储有可执行本发明方法实施例所提供的一种无人机机载多传感器校正方法的指令或程序,当运行该指令或程序时,可执行方法实施例的任意组合实施步骤,具备该方法相应的功能和有益效果。
在一些可选择的实施例中,在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如,取决于所涉及的功能/操作,连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外,在本发明的流程图中所呈现和描述的实施例以示例的方式被提供,目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的,其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。
此外,虽然在功能性模块的背景下描述了本发明,但应当理解的是,除非另有相反说明,所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中,或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是,有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说,考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下,在工程师的常规技术内将会了解该模块的实际实现。因此,本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是,所公开的特定概念仅仅是说明性的,并不意在限制本发明的范围,本发明的范围由所附权利要求书及其等同方案的全部范围来决定。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用,或结合这些指令执行***、装置或设备而使用。就本说明书而言,“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
在本说明书的上述描述中,参考术语“一个实施方式/实施例”、“另一实施方式/实施例”或“某些实施方式/实施例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施方式,本领域的普通技术人员可以理解:在不脱离本发明的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型,本发明的范围由权利要求及其等同物限定。
以上是对本发明的较佳实施进行了具体说明,但本发明并不限于上述实施例,熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换,这些等同的变形或替换均包含在本申请权利要求所限定的范围内。
Claims (10)
1.一种无人机机载多传感器校正方法,其特征在于,包括以下步骤:
构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化;
构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化;
构建三轴陀螺仪的第三误差模型,将所述第三误差模型的参数估计问题转化为非线性函数优化问题,并构建第一优化函数;
获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据;
采用优化后的第一误差模型对所述第一数据进行校正,采用优化后的第二误差模型对所述第二数据进行校正;
结合所述第一优化函数、所述第三数据以及校正后的所述第一数据、所述第二数据,对所述第三误差模型的参数进行优化;
其中,误差模型用于对传感器的测量数据进行校正,以使测量数据趋近于理想输出值。
2.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述构建三轴加速度计的第一误差模型,对所述第一误差模型的参数进行优化,包括:
结合所述三轴加速度计的非正交误差、尺度因子和零偏误差,建立所述第一误差模型;
根据L-M算法的原理,将所述第一误差模型的参数估计问题转化为非线性函数优化问题,并构建第二优化函数;
采集所述三轴加速度计在静态下的数据,根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值。
4.根据权利要求3所述的一种无人机机载多传感器校正方法,其特征在于,所述根据采集到的数据和所述第二优化函数获取所述第一误差模型的参数的最优估计值,包括:
采用L-M算法对所述第二优化函数进行迭代求解,其中,定义误差函数f(ξ)和指标函数F(ξ),有:
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为:
其中,aii为矩阵A0的对角元素;τ为经验给定的正常数;
在L-M算法的迭代过程中,根据增益系数ρ的正负符号来判断当前迭代点是否准确有效,且可根据增益系数ρ来计算下一步迭代的阻尼系数μ,增益系数ρ的计算如下:
若ρ>0,表明当前指标函数拥有下降趋势,减小μ的值,使下一次迭代更接近于高斯牛顿迭代;当ρ≤0时,增大μ的值,减小步长并使得下一次迭代更接近于最速下降迭代。
5.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述构建三轴磁力计的第二误差模型,对所述第二误差模型的参数进行优化,包括:
结合所述三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差,构建三轴磁力计的第二误差模型;
根据L-M算法的原理,将所述第二误差模型的参数估计问题转化为非线性函数优化问题,并构建第三优化函数;
其中,所述三轴磁力计的校正原理是任意姿态下的三轴磁力计,三轴磁力计理想的磁感应强度测量值的模等于当地的磁场强度M。
7.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述三轴陀螺仪的校正原理表示为:期望估计值q'k应尽可能与校正后的比力和磁场强度转换而成的四元数qk相接近;
根据所述三轴陀螺仪的校正原理,建立获得的所述第一优化函数的表达式如下:
其中,为三轴陀螺仪的误差模型中待求参数的最优估计值;为第k次与第k+1次三轴加速度计或三轴磁力计数据测量时刻之间采集到的N个角速度数据;q'k+1表示根据理想的角速度值和qk计算得到的第k+1次四元数的估计值;
其中,qk通过以下方式得到:
则第k次测量校正得到的欧拉角为:
将上述第k次测量校正得到的欧拉角转换为相应的四元数qk∈R4;
定义操作Ψ如下:
其中,γ为待求参数。
8.根据权利要求1所述的一种无人机机载多传感器校正方法,其特征在于,所述获取同一时刻下,所述三轴加速度计测量的第一数据,所述三轴磁力计测量的第二数据,以及所述三轴陀螺仪测量的第三数据,包括:
将无人机分别绕机身x,y,z轴正方向匀速旋转,每次旋转角度不同,每旋转一次都需保持t时间段的静止,采集时间段t内的比力、磁感应强度均值和两个连续静态间的N个动态角速度值。
9.一种无人机机载多传感器校正装置,其特征在于,包括:
至少一个处理器;
至少一个存储器,用于存储至少一个程序;
当所述至少一个程序被所述至少一个处理器执行,使得所述至少一个处理器实现权利要求1-8任一项所述方法。
10.一种计算机可读存储介质,其中存储有处理器可执行的程序,其特征在于,所述处理器可执行的程序在由处理器执行时用于执行如权利要求1-8任一项所述方法。
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 true CN114526756A (zh) | 2022-05-24 |
CN114526756B 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 (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140336970A1 (en) * | 2013-05-13 | 2014-11-13 | Giancarlo Troni-Peralta | System and method for determining and correcting field sensors errors |
US20150285835A1 (en) * | 2013-12-23 | 2015-10-08 | InvenSense, Incorporated | Systems and methods for sensor calibration |
US20180080768A1 (en) * | 2016-02-01 | 2018-03-22 | Limited Liability Company "Topcon Positioning Systems" | A method and device for calibration of a three-axis magnetometer |
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自适应两级卡尔曼滤波的无人机姿态估计方法 |
-
2022
- 2022-01-04 CN CN202210005058.XA patent/CN114526756B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140336970A1 (en) * | 2013-05-13 | 2014-11-13 | Giancarlo Troni-Peralta | System and method for determining and correcting field sensors errors |
US20150285835A1 (en) * | 2013-12-23 | 2015-10-08 | InvenSense, Incorporated | Systems and methods for sensor calibration |
US20180080768A1 (en) * | 2016-02-01 | 2018-03-22 | Limited Liability Company "Topcon Positioning Systems" | A method and device for calibration of a three-axis magnetometer |
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)
Title |
---|
ARAS YURTMAN等: "Novel Noniterative Orientation Estimation for Wearable Motion Sensor Units Acquiring Accelerometer, Gyroscope, and Magnetometer Measurements", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT 》 * |
吴和龙: "多旋翼无人机的低成本Inertial/GNSS/Vision组合导航关键技术研究", 《中国博士学位论文全文数据库工程科技Ⅱ辑(月刊)》 * |
Also Published As
Publication number | Publication date |
---|---|
CN114526756B (zh) | 2023-06-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109813336B (zh) | 惯性测量单元标定方法 | |
CN109696183B (zh) | 惯性测量单元的标定方法及装置 | |
KR100533106B1 (ko) | 지자계 센서의 자세 오차 보상장치 및 방법 | |
CN106885566B (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
CN108225370B (zh) | 一种运动姿态传感器的数据融合与解算方法 | |
CN109459585B (zh) | 一种加速度计零位偏置修正方法 | |
CN109724602A (zh) | 一种基于硬件fpu的姿态解算***及其解算方法 | |
CN108061855A (zh) | 一种基于mems传感器的球形电机转子位置检测方法 | |
CN110174123B (zh) | 一种磁传感器实时标定方法 | |
CN103487011B (zh) | 一种数据手套的姿态角检测方法 | |
CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
CN105910593B (zh) | 一种校准终端的地磁传感器的方法及装置 | |
CN112665574B (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
CN113108790B (zh) | 机器人imu角度测量方法、装置、计算机设备及存储介质 | |
CN114280332B (zh) | 一种三轴加速度传感器校正方法 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
CN105607760A (zh) | 一种基于微惯性传感器的轨迹还原方法及*** | |
CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN111638386A (zh) | 一种基于重力场的加速度计标度因数非线性度测试方法 | |
CN116817896A (zh) | 一种基于扩展卡尔曼滤波的姿态解算方法 | |
Hoang et al. | Measurement optimization for orientation tracking based on no motion no integration technique | |
CN111521196A (zh) | 传感器校正方法、装置、虚拟现实设备、存储介质 | |
CN108088431B (zh) | 一种自校正电子罗盘及其校正方法 | |
CN107356786B (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 |