CN112230192A - 基于磁传感与定位***的雷达航向角测量方法及装置 - Google Patents

基于磁传感与定位***的雷达航向角测量方法及装置 Download PDF

Info

Publication number
CN112230192A
CN112230192A CN202011520842.1A CN202011520842A CN112230192A CN 112230192 A CN112230192 A CN 112230192A CN 202011520842 A CN202011520842 A CN 202011520842A CN 112230192 A CN112230192 A CN 112230192A
Authority
CN
China
Prior art keywords
data
radar
course angle
angle
weight
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.)
Pending
Application number
CN202011520842.1A
Other languages
English (en)
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.)
Dfwee Technology Ltd
Original Assignee
Dfwee Technology Ltd
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 Dfwee Technology Ltd filed Critical Dfwee Technology Ltd
Priority to CN202011520842.1A priority Critical patent/CN112230192A/zh
Publication of CN112230192A publication Critical patent/CN112230192A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种基于磁传感与定位***的雷达航向角测量方法及装置，方法包括：采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据，并进行互补滤波计算，得到雷达的第一航向角；采用雷达定位***测量雷达的第二航向角；确定第一航向角的权重和第二航向角的权重，第一航向角的权重和第二航向角的权重之和为1；根据第一航向角和第二航向角以及各自的权重，计算得到雷达的最终航向角。在外界存在不同干扰的环境下，电子磁罗盘无法正常工作或者雷达定位***无法正常工作，根据电子磁罗盘测量的雷达航向角和雷达定位***测量的航向角，综合进行计算，准确得到雷达的航向角，可准确测量在不同的外界环境下雷达的航向角。

Description

基于磁传感与定位***的雷达航向角测量方法及装置
技术领域
本发明涉及雷达航天领域，更具体地，涉及一种基于磁传感与定位***的雷达航向角测量方法及装置。
背景技术
在航空、航天、雷达、枪瞄***等场合，比如，对雷达***的航向角的测量对雷达的航向定位非常重要。
目前，对雷达***的航向角的测量主要基于电子磁罗盘，但是外界环境比较复杂的情况下，电子磁罗盘容易受环境影响而导致测量航向角精准度出现错误偏差，不能满足多种不同环境条件下的使用。
发明内容
本发明实施例提供一种克服上述问题或者至少部分地解决上述问题的一种基于磁传感与定位***的雷达航向角测量方法及装置。
根据本发明实施例的第一方面，提供了一种基于磁传感与定位***的雷达航向角测量方法，包括：采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据；对所述磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；采用雷达定位***测量雷达的第二航向角；确定所述第一航向角的权重和所述第二航向角的权重，所述第一航向角的权重和第二航向角的权重之和为1；根据所述第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角。
根据本发明实施例的第二方面，提供了一种雷达航向角测量装置，包括电子磁罗盘、雷达定位***和处理器，所述电子磁罗盘包括加速度计、磁力计和陀螺仪；加速度计，用于采集雷达的加速度数据；磁力计，用于采集雷达的磁力计数据；陀螺仪，用于采集雷达的陀螺仪数据；雷达定位***，用于测量雷达的第二航向角；处理器，用于对所述磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；还用于确定所述第一航向角的权重和所述第二航向角的权重，所述第一航向角的权重和第二航向角的权重之和为1；以及根据所述第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角。
本发明实施例提供的一种基于磁传感和定位***的雷达航向角测量方法及装置，在外界不同环境，雷达定位***无法正常工作或者电子磁罗盘无法正常工作时，根据电子磁罗盘测量的雷达航向角和雷达定位***测量的航向角，综合进行计算，准确得到雷达的航向角，可准确测量在不同的外界环境下雷达的航向角。
附图说明
图1为本发明实施例提供的一种基于磁传感与定位***的雷达航向角测量方法流程图；
图2为本发明实施例提供的一种基于磁传感与定位***的雷达航向角测量方法的整体流程图；
图3是本发明实施例提供的一种雷达航向角测量装置结构图。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
图1为本发明实施例提供的一种基于磁传感与定位***的雷达航向角测量方法流程图，如图1所示，方法包括：101、采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据；102、对磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；103、采用雷达定位***测量雷达的第二航向角；104、确定第一航向角的权重和所述第二航向角的权重，所述第一航向角的权重和第二航向角的权重之和为1；105、根据第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角。
可以理解的是，在外界环境受到不同的干扰时，电子磁罗盘或者定位***都可能会受到影响，单纯使用电子磁罗盘或者定位***测量雷达的航向角不能够满足多种不同环境下的测量。
基于此，本发明实施例提出了一种能够满足不同环境下对雷达航向角的测量。主要步骤为，采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据，并对其进行处理，计算得到雷达的第一航向角；同样的，采用定位***测量雷达的第二航向角。根据外界不同的环境，分配第一航向角和第二航向角的权重，随后根据第一航向角、第二航向角以及各自的权重，计算出雷达最终的航向角。
本发明实施例在外界不同环境，雷达定位***可正常工作或者无法正常工作时，通过电子磁罗盘对航向角的测量，根据电子磁罗盘测量的雷达航向角和雷达定位***测量的航向角，综合进行计算，准确得到雷达的航向角，可准确测量在不同的外界环境下雷达的航向角。
在一种可能的实施例方式中，采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据之后还包括：采用二阶巴特沃斯滤波器分别对磁力计数据、加速度数据和陀螺仪数据进行滤波。
其中，二阶巴特沃斯滤波器为：
u(n)=x(n)-a(1)*u(n-1)-a(2)*u(n-2)；
y(n)=b(0)*u(n)+b(1)*u(n-1)+b(2)*u(n-2)；
其中，x(n)为实时采样的第n个磁力计数据或加速度数据或陀螺仪数据，y(n)为x(n)经过滤波后的输出结果，b(0)、b(1)、b(2)，a(1)、a(2)为系数。
可以理解的是，电子磁罗盘包括磁力计、加速度计和陀螺仪，采用磁力计、加速度计和陀螺仪采集得到雷达的磁力计原始数据rawmagx、rawmagy和rawmagz,加速度原始数据rawaccx、rawaccy和rawaccz,陀螺仪原始数据rawgyrox、rawgyroy和rawgyroz。
对采集的磁力计原始数据、加速度原始数据和陀螺仪原始数据进行滤波处理，去除其中的干扰信号。其中，本发明实施例采用二阶巴特沃斯滤波器对其进行滤波,二阶巴特沃斯滤波器的计算公式参见上述计算公式，其中，可通过如下方式求得系数b(0)、b(1)、b(2)，a(1)和a(2)。
其中，电子磁罗盘的采样频率为sample_freq，截止频率为cutoff_freq，M_PI_F为常数π。
fr=sample_freq/cutoff_freq，ohm=tanf(M_PI_F/fr)，
c=1.0+2.0*cosf(M_PI_F/4.0)*ohm + ohm*ohm，b(0)=ohm*ohm/c，
b(1)=2.0* b(0)，b(2)=b(0)，a(1)=2.0*(ohm*ohm-1.0)/c，
a(2)=(1.0-2.0*cosf(M_PI_F/4.0)*ohm+ohm*ohm)/c。
经过滤波处理后的磁力计数据为magx,magy,magz,加速度数据为accx,accy,accz,陀螺仪数据为gyrox,gyroy,gyroz。
在一种可能的实施例方式中，对磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角包括:计算加速度数据的误差值以及计算磁力计数据的误差值；根据所述加速度数据的误差值和磁力计数据的误差值，对所述陀螺仪数据进行修正；根据修正后的陀螺仪数据，计算四元数；根据四元数计算雷达的第一航向角。
可以理解的是，对于滤波处理后的磁力计数据、加速度数据和陀螺仪数据，分别计算加速度数据的误差值和磁力计数据的误差值，根据加速度数据的误差值和磁力计数据的误差值，对陀螺仪数据进行修正。然后利用修正后的陀螺仪数据，计算四元数，并根据四元数，计算雷达的第一航向角。
在一种可能的实施例方式中，计算加速度数据的误差值的过程为，将机体坐标系下的加速度数据进行归一化：
Figure 991013DEST_PATH_IMAGE001
, accx’= accx * accnorm，
accy’= accy * accnorm，accz’= accz * accnorm。
其中，accx、accy和accz为归一化前的加速度数据，accx’、accy’和accz’为归一化后的加速度数据。
计算地理坐标系的重力向量转换到机体坐标系的转换矩阵：
Figure 702617DEST_PATH_IMAGE002
其中，地理坐标系下的重力向量为
Figure 646433DEST_PATH_IMAGE003
，q0、q1、q2和q3为四元数，为未知数，其初始值q0 =q1=q2=q3=0。
将地理坐标系转到机体坐标系下的重力向量和机体坐标系测量的重力向量进行叉积，得到加速度数据的误差值：
Figure 191815DEST_PATH_IMAGE004
其中，ex、ey和ez即为加速度值的误差值。
计算磁力计数据的误差值的过程为，将机体坐标系下的磁力计数据进行归一化：
Figure 103139DEST_PATH_IMAGE005
magx’= magx *magnorm；
magy’= magy * magnorm；
magz’= magz * magnorm。
其中，magx、magy和magz为通过二阶巴特沃斯滤波器处理后的磁力计数据，magx’、magy’和magz’为归一化处理后的数据。将归一化后的磁力计数据转换到地理坐标系下：
Figure 697063DEST_PATH_IMAGE006
将地理坐标系下的磁力计数据转换到导航坐标系下：
Figure 557703DEST_PATH_IMAGE007
将导航坐标系下的磁力计数据再次转换到机体坐标系：
Figure 273986DEST_PATH_IMAGE008
其中，mx、my和mz为磁力计数据的误差值。
由上，求出了加速度数据的误差值和磁力计数据的误差值，陀螺仪的误差是导致机体坐标系误差的重要原因，因此，利用加速度数据的误差值和磁力计数据的误差值对陀螺仪数据进行修正。具体的，通过如下公式对陀螺仪数据进行修正：
Figure 157759DEST_PATH_IMAGE009
Figure 273483DEST_PATH_IMAGE010
其中，gyrox、gyroy和gyroz为修正前的陀螺仪数据，gyrox’、gyroy’和gyroz’为根据加速度数据的误差值修正后的陀螺仪数据，gyrox”、gyroy”和gyroz”为根据磁力计数据误差值修正后的陀螺仪数据，KPa、KPm为修正系数。
在一种可能的实施例方式中，根据修正后的陀螺仪数据，计算四元数包括：对四元数进行微分方程的一阶毕卡算法，得到四元数方程组：
Figure 660733DEST_PATH_IMAGE011
求解四元数方程组，得到四元数q0、q1、q2和q3，其中，Δt为算法运行时间差。
相应的，根据求解得到的四元数计算雷达的第一航向角包括以下步骤，首先，对四元数进行归一化：
Figure 875814DEST_PATH_IMAGE012
Figure 964993DEST_PATH_IMAGE013
雷达航向角为：
Figure 635139DEST_PATH_IMAGE014
雷达的横滚角为：
Figure 188481DEST_PATH_IMAGE015
雷达的俯仰角为：
Figure 387512DEST_PATH_IMAGE016
至此，根据电子磁罗盘采集的数据，计算出了雷达的航向角，称为雷达的第一航向角。
在一种可能的实施例方式中，采用雷达定位***测量雷达的第二航向角还包括：采用雷达定位***获取卫星颗数N和位置精度P；相应的，确定第一航向角和第二航向角的权重包括：当卫星颗数N<n并且P>p时，第一航向角的权重为k=1，第二航向角的权重为0；当卫星颗数N≥n且P≤p时，根据加速度数据的均方根误差、磁力计数据的均方根误差和陀螺仪数据的均方根误差，确定第一航向角的权重和第二航向角的权重；其中，n为卫星颗数阈值，p为位置精度阈值。
可以理解的是，雷达定位***采用如GPS双天线、北斗双天线方式获取准确航向角和卫星颗数N，位置精度P。
对获取的航向角进行低通滤波，一阶数字低通滤波器：
y(n) = q*x(n) + (1-q)*y(n-1)；
其中，y(n)表示一阶数字低通滤波器的输出，x(n)表示当前的输入，y(n-1)表示上一次的输出，经过滤波得出滤波后的雷达航向角yaw2，称为第二航向角，q为滤波权重系数，本发明实施例中q设置为0.25。
随后，根据第一航向角和第二航向角，选择加权方式进行航向角求解，out=k*yaw1 + (1-k)* yaw2，其中，out为计算的雷达的最终航向角，yaw1为第一航向角，k为第一航向角的权重，yaw2为第二航向角，（1-k）为第二航向角的权重，第一航向角的权重和第二航向角的权重之和为1。
在一种可能的实施例方式中，当卫星颗数N≥n且P≤p时，根据加速度数据的均方根误差、磁力计数据的均方根误差和陀螺仪数据的均方根误差，确定第一航向角的权重和第二航向角的权重包括：
分别计算加速度数据的均方根误差S1、磁力计数据的均方根误差S2和陀螺仪数据的均方根误差S3，计算S=S1+S2+S3；
根据S和第一航向角的权重之间的对应关系表，查找与S对应的第一航向角的权重。
可以理解的是，在确定第一航向角的权重和第二航向角的权重时，对于通过电子磁罗盘计算得到的航向角、横滚角和俯仰角，分别计算该航向角的均方根误差S1、横滚角的均方根误差S2和俯仰角的均方根误差S3，记S=S1+S2+S3。
对于第一航向角的权重和第二航向角的权重的确定，分为两种情况进行处理，第一种情况，卫星颗数N<n并且P>p时，认为定位***没有达到正常状态，此时k=1；本发明实施例的定位***选择n=13,p=0.3，其中，n,p具体可按照不同定位***进行调整。
第二种情况，卫星颗数N和位置精度P满足条件时，即N≥n且P≤p时，按照如下表1查找与S对应的第一航向角的权重，表1为S的取值范围与第一航向角的权重之间的关系表，其中，表1中S范围端点值和权重k取值可根据实际***进行重新设置。
表1
方差S 权重k 方差和S 权重k
0<s<0.1 0.9 0.5<s<0.6 0.4
0.1<s<0.2 0.8 0.6<s<0.7 0.3
0.2<s<0.3 0.7 0.7<s<0.8 0.2
0.3<s<0.4 0.6 0.8<s<0.9 0.1
0.4<s<0.5 0.5 0.9<s 0.05
根据表1可根据S值查找对应的第一航向角的权重，即可得到第一航向角的权重和第二航向角的权重，进而计算得到雷达的最终航向角。
当外界环境可满足室外能够正常接收定位卫星的情况下，定位***测量的航向角基于S动态自适应调整权重，可解决周围磁场环境较差时磁力计无法正常工作情况。在室内卫星信号差等环境下，定位***无法正常工作时，可通过加速度计、磁力计和陀螺仪组合计算的航向角来作为输出，解决定位***无法正常工作的情况；同时通过滤波和修正算法动态对采集的数据进行调整修正，可比一般磁罗盘输出航向角更加稳定。
在定位***精度和周围磁场较差的环境下，通过动态修正算法，结合磁罗盘和定位***来综合计算雷达的航向角，可比只使用定位***或只使用磁罗盘的效果更准确些。
参见图2，为本发明实施例提供的基于磁传感与定位***的雷达航向角测量方法的整体流程图，其中，分别采用电子磁罗盘的磁力计采集磁力计原始数据、加速度计采集加速度原始数据和采用陀螺仪采集陀螺仪原始数据，并分别对采集的磁力计原始数据、加速度原始数据和陀螺仪原始数据进行滤波。
随后，对滤波处理后的磁力计数据、加速度数据和陀螺仪数据进行互补滤波处理，利用加速度数据的误差值和磁力计数据的误差值对陀螺仪数据进行修正，得到修正后的陀螺仪数据。利用修正后的陀螺仪数据求解四元数，根据四元数，计算电子磁罗盘测量的雷达第一航向角。采用定位***测量雷达的第二航向角，对测量的第二航向角进行低通滤波。
最后，通过计算根据电子磁罗盘计算出的雷达的航向角、横滚角和俯仰角，分别计算三者均方根误差S1、S2和S3，并将三者累加，得到S=S1+S2+S3。根据定位***是否能够正常工作的情况，以及S的取值等，确定第一航向角和第二航向角的权重，最终计算得到雷达的航向角。
图3为本发明实施例提供的一种雷达航向角测量***结构图，如图3所示，测量***包括电子磁罗盘31、雷达定位***32和处理器33，电子磁罗盘31包括磁力计311、加速度计312和陀螺仪313。
其中，磁力计311，用于采集雷达的磁力计数据；加速度计312，用于采集雷达的加速度数据；陀螺仪313，用于采集雷达的陀螺仪数据；
雷达定位***32，用于测量雷达的第二航向角。处理器33，用于对磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；还用于确定第一航向角的权重和第二航向角的权重，第一航向角的权重和第二航向角的权重之和为1；以及根据第一航向角和第二航向角以及各自的权重，计算得到雷达的最终航向角。
本发明实施例提供的雷达航向角测量***与前述各实施例提供的雷达航向角测量方法相对应，雷达航向角测量***的相关技术特征可参考前述各实施例提供的雷达航向角测量方法的相关技术特征，在此不再重复说明。
尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包括这些改动和变型在内。

Claims (10)

1.一种基于磁传感与定位***的雷达航向角测量方法，其特征在于，包括：
采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据；
对所述磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；
采用雷达定位***测量雷达的第二航向角；
确定所述第一航向角的权重和所述第二航向角的权重，所述第一航向角的权重和第二航向角的权重之和为1；
根据所述第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角。
2.根据权利要求1所述的测量方法，其特征在于，所述采用电子磁罗盘测量雷达的磁力计数据、加速度数据和陀螺仪数据之后还包括：
采用二阶巴特沃斯滤波器分别对所述磁力计数据、加速度数据和陀螺仪数据进行滤波；
所述二阶巴特沃斯滤波器为：
u(n)=x(n)-a(1)*u(n-1)-a(2)*u(n-2);
y(n)=b(0)*u(n)+b(1)*u(n-1)+b(2)*u(n-2);
其中，x(n)为实时采样的第n个磁力计数据或加速度数据或陀螺仪数据，y(n)为x(n)经过滤波后的输出结果，b(0)、b(1)、b(2)，a(1)、a(2)为系数。
3.根据权利要求2所述的测量方法，其特征在于，通过如下方式求得系数b(0)、b(1)、b(2)，a(1)和a(2)：
fr=sample_freq/cutoff_freq;
ohm=tanf(M_PI_F/fr);
c=1.0+2.0*cosf(M_PI_F/4.0)*ohm+ohm*ohm;
b(0)=ohm*ohm/c;
b(1)=2.0* b(0);
b(2)=b(0);
a(1)=2.0*(ohm*ohm-1.0)/c;
a(2)=(1.0-2.0*cosf(M_PI_F/4.0)*ohm+ohm*ohm)/c;
其中，sample_freq为采样频率，cutoff_freq为截止频率，M_PI_F为常数π。
4.根据权利要求1-3任一项所述的测量方法，其特征在于，所述对所述磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角包括:
计算加速度数据的误差值以及计算磁力计数据的误差值；
根据所述加速度数据的误差值和磁力计数据的误差值，对所述陀螺仪数据进行修正；
根据修正后的陀螺仪数据，计算四元数；
根据四元数计算雷达的第一航向角。
5.根据权利要求4所述的测量方法，其特征在于，所述计算加速度数据的误差值包括：
将机体坐标系下的加速度数据进行归一化：
Figure 518368DEST_PATH_IMAGE001
accx’= accx * accnorm；
accy’= accy * accnorm；
accz’= accz * accnorm；
其中，accx、accy和accz为归一化前的加速度数据，accx’、accy’和accz’为归一化后的加速度数据；
计算地理坐标系的重力向量转换到机体坐标系的转换矩阵：
Figure 966667DEST_PATH_IMAGE002
其中，地理坐标系下的重力向量为
Figure 418508DEST_PATH_IMAGE003
，q0、q1、q2和q3为四元数，初始值q0=q1=q2=q3=0；
将地理坐标系转到机体坐标系下的重力向量和机体坐标系测量的重力向量进行叉积，得到加速度数据的误差值：
Figure 751400DEST_PATH_IMAGE004
所述计算磁力计数据的误差值包括：
将机体坐标系下的磁力计数据进行归一化：
Figure 237876DEST_PATH_IMAGE005
magx’=magx *magnorm；
magy’=magy * magnorm；
magz’=magz * magnorm；
其中，magx、magy和magz为归一化前的磁力计数据，magx’、magy’和magz’为归一化后的磁力计数据；
将归一化后的磁力计数据转换到地理坐标系下：
Figure 591497DEST_PATH_IMAGE006
Figure 530634DEST_PATH_IMAGE007
将地理坐标系下的磁力计数据转换到导航坐标系下：
Figure 136059DEST_PATH_IMAGE008
将导航坐标系下的磁力计数据再次转换到机体坐标系，得到磁力计数据的误差值：
Figure 601676DEST_PATH_IMAGE009
其中，mx、my和mz为磁力计数据的误差值；
相应的，根据加速度数据的误差值和磁力计数据的误差值，对陀螺仪数据进行修正包括：
Figure 267143DEST_PATH_IMAGE010
Figure 427997DEST_PATH_IMAGE011
其中，gyrox、gyroy和gyroz为修正前的陀螺仪数据，gyrox’、gyroy’和gyroz’为根据加速度数据的误差值修正后的陀螺仪数据，gyrox”、gyroy”和gyroz”为根据磁力计数据误差值修正后的陀螺仪数据，KPa、KPm为修正系数。
6.根据权利要求5所述的测量方法，其特征在于，所述根据修正后的陀螺仪数据，计算四元数包括：
对四元数进行微分方程的一阶毕卡算法，得到四元数方程组：
Figure 961747DEST_PATH_IMAGE012
求解四元数方程组，得到四元数q0、q1、q2和q3，其中，Δt为算法运行时间差；
相应的，所述根据四元数计算雷达的第一航向角包括：
对四元数进行归一化：
Figure 891657DEST_PATH_IMAGE013
Figure 993605DEST_PATH_IMAGE014
雷达航向角为：
Figure 31968DEST_PATH_IMAGE015
雷达的横滚角为：
Figure 979196DEST_PATH_IMAGE016
雷达的俯仰角为：
Figure 29191DEST_PATH_IMAGE017
。
7.根据权利要求6所述的测量方法，其特征在于，所述采用雷达定位***测量雷达的第二航向角还包括：
采用雷达定位***获取卫星颗数N和位置精度P；
相应的，所述确定所述第一航向角和所述第二航向角的权重包括：
当卫星颗数N<n并且P>p时，第一航向角的权重为k=1，第二航向角的权重为0；
当卫星颗数N≥n且P≤p时，根据加速度数据的均方根误差、磁力计数据的均方根误差和陀螺仪数据的均方根误差，确定第一航向角的权重和第二航向角的权重；
其中，n为卫星颗数阈值，p为位置精度阈值。
8.根据权利要求7所述的测量方法，其特征在于，所述当卫星颗数N≥n且P≤p时，根据加速度数据的均方根误差、磁力计数据的均方根误差和陀螺仪数据的均方根误差，确定第一航向角的权重和第二航向角的权重包括：
分别计算加速度数据的均方根误差S1、磁力计数据的均方根误差S2和陀螺仪数据的均方根误差S3，计算S=S1+S2+S3；
根据S和第一航向角的权重之间的对应关系表，查找与S对应的第一航向角的权重。
9.根据权利1或8所述的测量方法，其特征在于，所述根据所述第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角包括：
out = k* yaw1 + (1-k)* yaw2；
其中，out为雷达的最终航向角，yaw1为第一航向角，k为第一航向角的权重，yaw2为第二航向角，（1-k）为第二航向角的权重。
10.一种雷达航向角测量装置，其特征在于，包括电子磁罗盘、雷达定位***和处理器，所述电子磁罗盘包括加速度计、磁力计和陀螺仪；
所述加速度计，用于采集雷达的加速度数据；
所述磁力计，用于采集雷达的磁力计数据；
所述陀螺仪，用于采集雷达的陀螺仪数据；
所述雷达定位***，用于测量雷达的第二航向角；
所述处理器，用于对所述磁力计数据、加速度数据和陀螺仪数据进行互补滤波计算，得到雷达的第一航向角；还用于确定所述第一航向角的权重和所述第二航向角的权重，所述第一航向角的权重和第二航向角的权重之和为1；以及根据所述第一航向角和所述第二航向角以及各自的权重，计算得到雷达的最终航向角。
CN202011520842.1A 2020-12-21 2020-12-21 基于磁传感与定位***的雷达航向角测量方法及装置 Pending CN112230192A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011520842.1A CN112230192A (zh) 2020-12-21 2020-12-21 基于磁传感与定位***的雷达航向角测量方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011520842.1A CN112230192A (zh) 2020-12-21 2020-12-21 基于磁传感与定位***的雷达航向角测量方法及装置

Publications (1)

Publication Number Publication Date
CN112230192A true CN112230192A (zh) 2021-01-15

Family

ID=74124938

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011520842.1A Pending CN112230192A (zh) 2020-12-21 2020-12-21 基于磁传感与定位***的雷达航向角测量方法及装置

Country Status (1)

Country Link
CN (1) CN112230192A (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102589552A (zh) * 2012-01-19 2012-07-18 北京华力创通科技股份有限公司 低成本组合导航***的数据融合方法和装置
CN103339473A (zh) * 2010-12-03 2013-10-02 高通股份有限公司 用于gnss车辆导航的受惯性传感器辅助的航向和定位
CN104819716A (zh) * 2015-04-21 2015-08-05 北京工业大学 一种基于mems的ins/gps组合的室内外个人导航算法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置
CN109443349A (zh) * 2018-11-14 2019-03-08 广州中海达定位技术有限公司 一种姿态航向测量***及其融合方法、存储介质
WO2019143838A1 (en) * 2018-01-17 2019-07-25 Cubic Corporation Cuboid inertial measurement unit
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111562603A (zh) * 2020-06-30 2020-08-21 高新兴物联科技有限公司 基于航位推算的导航定位方法、设备及存储介质
CN111982100A (zh) * 2020-07-07 2020-11-24 广东工业大学 一种无人机的航向角解算算法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103339473A (zh) * 2010-12-03 2013-10-02 高通股份有限公司 用于gnss车辆导航的受惯性传感器辅助的航向和定位
CN102589552A (zh) * 2012-01-19 2012-07-18 北京华力创通科技股份有限公司 低成本组合导航***的数据融合方法和装置
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置
CN104819716A (zh) * 2015-04-21 2015-08-05 北京工业大学 一种基于mems的ins/gps组合的室内外个人导航算法
WO2019143838A1 (en) * 2018-01-17 2019-07-25 Cubic Corporation Cuboid inertial measurement unit
CN109443349A (zh) * 2018-11-14 2019-03-08 广州中海达定位技术有限公司 一种姿态航向测量***及其融合方法、存储介质
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111562603A (zh) * 2020-06-30 2020-08-21 高新兴物联科技有限公司 基于航位推算的导航定位方法、设备及存储介质
CN111982100A (zh) * 2020-07-07 2020-11-24 广东工业大学 一种无人机的航向角解算算法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
CSDN: "姿态估计（1）——互补滤波（Complementary filter ）", 《CSDN HTTPS://BLOG.CSDN.NET/U012814946/ARTICLE/DETAILS/52850400/》 *
STACKOVERFLOW: "Calculate Coefficients of 2nd Order Butterworth Low Pass Filter", 《STACKOVERFLOW HTTPS://STACKOVERFLOW.COM/QUESTIONS/20924868/CALCULATE-COEFFICIENTS-OF-2ND-ORDER-BUTTERWORTH-LOW-PASS-FILTER》 *
蒋窍 等: "基于单片机的GPS/MEMS陀螺仪航向角实时测量***", 《传感技术学报》 *

Similar Documents

Publication Publication Date Title
CN109163721B (zh) 姿态测量方法及终端设备
CN109001787B (zh) 一种姿态角解算与定位的方法及其融合传感器
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN112066985B (zh) 一种组合导航***初始化方法、装置、介质及电子设备
CN108562305B (zh) 一种惯性/天文深组合导航***安装误差五位置快速粗标定方法
CN111289012B (zh) 一种传感器的姿态校准方法及装置
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN112304337B (zh) 基于陀螺仪和加速度计的运动角度估计方法和***
CN115435817B (zh) Mems惯导安装误差的标定方法、存储介质和控制计算机
CN113188505B (zh) 姿态角度的测量方法、装置、车辆及智能臂架
Cao et al. Anti-disturbance fault tolerant initial alignment for inertial navigation system subjected to multiple disturbances
CN112562077A (zh) 一种融合pdr和先验地图的行人室内定位方法
JP2019120587A (ja) 測位システム及び測位方法
CN111561930A (zh) 一种车载mems陀螺仪随机漂移误差的抑制方法
CN112985384B (zh) 一种抗干扰磁航向角优化***
CN110058324B (zh) 利用重力场模型的捷联式重力仪水平分量误差修正方法
CN112284388B (zh) 一种无人机多源信息融合导航方法
CN111679307B (zh) 一种卫星定位信号解算方法及装置
CN112230192A (zh) 基于磁传感与定位***的雷达航向角测量方法及装置
CN112240957A (zh) 一种卫星导航干扰测向中天线幅相特性校正方法
CN116718153A (zh) 一种基于gnss和ins的形变监测方法及***
CN103969671B (zh) 一种基于非线性跟踪微分器的杆臂误差解算方法
Candan et al. Estimation of attitude using robust adaptive Kalman filter
CN110733671B (zh) 一种小天体自旋角速度动力学修正方法
CN110186479B (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210115