CN110736457A - 一种基于北斗、gps和sins的组合导航方法 - Google Patents

一种基于北斗、gps和sins的组合导航方法 Download PDF

Info

Publication number
CN110736457A
CN110736457A CN201911103352.9A CN201911103352A CN110736457A CN 110736457 A CN110736457 A CN 110736457A CN 201911103352 A CN201911103352 A CN 201911103352A CN 110736457 A CN110736457 A CN 110736457A
Authority
CN
China
Prior art keywords
beidou
coordinate system
aerial vehicle
unmanned aerial
gps
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
CN201911103352.9A
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.)
Suzhou Vocational Institute of Industrial Technology
Original Assignee
Suzhou Vocational Institute of Industrial Technology
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 Suzhou Vocational Institute of Industrial Technology filed Critical Suzhou Vocational Institute of Industrial Technology
Priority to CN201911103352.9A priority Critical patent/CN110736457A/zh
Publication of CN110736457A publication Critical patent/CN110736457A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Abstract

本发明公开了一种基于北斗、GPS和SINS的组合导航方法，包括以下步骤：步骤1：利用扩展卡尔曼滤波器将SINS与北斗组合，对无人机的位置和运动速度进行有效估计；步骤2：当检测到无人机处于静止或匀速运动时，将陀螺仪与加速度计、GPS进行组合，利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正，以此对无人机的姿态进行估计；步骤3：当检测到无人机处于进行加速、减速或高速转动时，就不再进行姿态修正，而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算，直到无人机恢复静止或匀速状态。本发明组合导航方法有效的减少了惯性单元在导航时的时间累积误差，同时具有小型化、轻型化、低成本等优点。

Description

一种基于北斗、GPS和SINS的组合导航方法
技术领域
本发明涉及导航技术领域，特别涉及一种基于北斗、GPS和SINS的组合导航方法。
背景技术
当今世界上一共存在四大全球卫星导航系统，其中包括美国的GPS，俄罗斯的GLONASS，欧盟的Galileo和中国的北斗卫星导航系统。随着北斗卫星导航系统的“三步走”部署开始实施，北斗卫星导航这个新兴技术越来越受到各行各业的青睐。
近年来，国内外的一些研究人员受到昆虫利用视觉进行导航的启发，使用北斗法进行小型无人机的导航。国外方面，Nils Gageilk、Dominilk Honegger等人利用北斗完成了无人机的定点悬停、自主驾驶以及速度估计，D.A.Mercado、Jinling Wang等人进行了北斗/GPS/SINS的组合导航研究，可以对无人机的导航信息进行实时的估计。国内方面，吕强、张洪涛等人也成功利用北斗法进行了悬停实验，宋宇利用两个北斗完成了对室内小型无人机速度、位置和姿态信息的获取，但缺点是需要较强的光源，同时北斗获取姿态信息时对无人机高度有一定要求。
在使用单个北斗进行导航时，原理上可以得到无人机的速度和位置信息，但是无法获取无人机的姿态信息。本专利申请针对这一问题，在上述研究的基础上，提出了一种基于北斗/GPS/SINS的组合导航方案。
发明内容
为了克服现有技术中的不足，本发明提供一种基于北斗、GPS和SINS的组合导航方法，利用扩展卡尔曼滤波器，将SINS与北斗组合，对无人机的位置和速度进行估计；当无人机静止或匀速运动时，将陀螺仪与加速度计、GPS组合，对无人机的姿态进行估计，该方案有效的减少了惯性单元在导航时的时间累积误差，同时具有小型化、轻型化、低成本等优点。
为了达到上述发明目的，解决其技术问题所采用的技术方案如下：
一种基于北斗、GPS和SINS的组合导航方法，包括以下步骤：
步骤1：利用扩展卡尔曼滤波器将SINS与北斗组合，对无人机的位置和运动速度进行有效估计；
步骤2：当检测到无人机处于静止或匀速运动时，将陀螺仪与加速度计、GPS进行组合，利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正，以此对无人机的姿态进行估计；
步骤3：当检测到无人机处于进行加速、减速或高速转动时，就不再进行姿态修正，而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算，直到无人机恢复静止或匀速状态。
进一步的，步骤1中，选择基于最小误差绝对值和SAD的块匹配BMA算法进行北斗的计算：
设xp(i，j)和xc(i+Δi，j+Δj)分别表示前一帧中选取的大小为n×n目标块的灰度值和当前帧搜索区域中待匹配目标块的灰度值，其中1≤i，j≤n；-d≤Δi，Δj≤d，基于最小绝对误差和SAD匹配准则的块匹配算法就是在搜索区域里寻找满足下式的Δi，Δj，从而获得北斗矢量V＝r(Δi，Δj)T，V的单位为像素/秒，r为摄像头的采集频率，单位为帧/秒，而U为绝对值误差和的最小值，有如下的关系表达式：
U＝min(Δi，Δj){SAD(Δi，Δj)} (公式2)
V＝r(Δi，Δj)T|U (公式3)
在初始状态时，在成像平面的原点处选取一个目标块，当无人机运动时，目标块会在后一帧图像中移动，在后一帧的搜索区域内，该搜索区域限定了两个方向的最大位移d，使当前帧的目标块和前一帧的目标块内像素灰度差的SAD最小，以此得到目标块的北斗矢量；
采集垂直于摄像头方向的图像，并使用8位的数据来表征每个像素的灰度，采集后的数据通过DMA存储在系统内存中，在整个采集过程中，选择8×8像素的数据块来作为块匹配的对象，搜索区域为±4个像素，所以对于每一帧图像，一共有64个像素点，同时有81个候选的向量方向，获得每一帧图像后，计算出各个候选向量的误差绝对值和，并选择其中的最小值作为北斗失量。
进一步的，步骤1中，使用针孔模型，对无人机在地理坐标系下的运动进行估计：
针孔模型中，设Pc＝[Xc，Yc，Zc]T是在摄像头坐标系下的一个点，f代表了摄像头的焦距，于是这一点在摄像头成像平面下表示为p＝[x，y，f]T，并有关系：
Figure BDA0002269697620000031
Figure BDA0002269697620000032
上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量p＝[x，y，f]T的第三项为常数并等于焦距。考虑地面上的任意一点P，在摄像头坐标系下P相对于无人机有如下的运动关系：
Vc＝-Tc-ω×Pc (公式7)
将公式7在三个维度上展开，可以得到：
Figure BDA0002269697620000034
Figure BDA0002269697620000035
Figure BDA0002269697620000036
其中，ω＝[ωx，ωy，ωz]T是无人机的角速度，
Figure BDA0002269697620000041
是无人机在摄像头坐标系的平动速度；
对公式4两边求导，可以得到Pc在摄像头坐标下的速度与p在成像平面下的速度的关系：
其中，v＝[vx，vy，vz]T，将上述公式11在三个维度上展开，可以得到：
Figure BDA0002269697620000043
Figure BDA0002269697620000044
vz＝0 (公式14)
将公式8、公式9、公式10代入公式12、公式13，可以得到：
Figure BDA0002269697620000045
公式15和公式16中的vx、vy是北斗矢量在x方向和y方向上的分量，可以通过块匹配最小误差绝对和的方法计算得出；式中的Zc可以由北斗传感器板载的超声波传感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺仪得出；式中的x，y用公式5和公式6代入，估算出飞行器在摄像头坐标系下的平动速度Tc，再通过坐标系变换矩阵
Figure BDA0002269697620000047
得到无人机在地理坐标系下的速度，积分后得到无人机在地理坐标系下的位置信息。
进一步的，步骤2中，加速度计通过敏感重力场，在载体不存在自身加速度的情况下，可以确定无人机的横滚角和俯仰角；GPS借助于加速度计得到的载体姿态信息，可以解算得到无人机航向角；两者相组合，就可以得到不随时间积累误差的全姿态信息。
进一步的，利用加速度计获取载体的横滚角和俯仰角重力矢量在地理坐标系下的分量为[0 0 -g]T，当载体处于静止态，相对导航坐标系无加速度时，安装在载体坐标系下的加速度计测量值为
Figure BDA0002269697620000051
由于重力加速度垂直于水平面，故载体的航向角不影响x向和y向加速度计的输出，于是有关系：
Figure BDA0002269697620000052
联立可以解得俯仰角和横滚角为：
Figure BDA0002269697620000054
利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息，故只有在载体无加速运动的情况下上述算式才成立。
进一步的，利用GPS获取载体的航向角，设地磁强度在地理坐标系三轴上的分量为
Figure BDA0002269697620000055
GPS沿体坐标系Fb三轴方向安装，地磁强度在载体坐标系三轴上的分量分别为
Figure BDA0002269697620000056
地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示：
式中，
Figure BDA0002269697620000061
的值通过查表给出，假设在苏州地区，北纬37.8度，东经112.5度，其值如公式21所示，而的数值由机载的GPS给出：
Figure BDA0002269697620000063
假定无人机在飞行过程中地磁场保持恒定，联立公式20和公式21，再结合由加速度计确定的俯仰角和横滚角，就可以解算出无人机在地理坐标系下的航向角。
进一步的，步骤1中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在导航坐标系下的速度和位置信息作为状态量，北斗传感器和超声波传感器的输出作为观测量，联合估计无人机的位置和速度信息：
考虑非线性系统的状态方程和观测方程：
Xk+1＝f[Xk，k]+BkUk+ΓkWk (方程1)
Zk+1＝h[Xk+1，k+1]+Vk+1 (方程2)
其中，
Figure BDA0002269697620000064
是状态向量，包括无人机的速度信息和位置信息；Zk＝[x y Zc]T是观测向量，包括北斗传感器输出的x方向北斗和y方向北斗和超声波传感器的读数Zc；
Figure BDA0002269697620000065
是系统的控制向量，由加速度计的数值经过坐标系矩阵变换得出；Bk是控制分配矩阵；Γk是噪声分配矩阵；Wk是过程噪声矩阵；Vk是测量噪声；f代表系统状态函数；h代表观测函数；
将状态方程和观测方程代入扩展卡尔曼滤波器，可以得到：
Figure BDA0002269697620000066
Figure BDA0002269697620000071
Figure BDA0002269697620000072
Figure BDA0002269697620000074
其中，
Figure BDA0002269697620000075
通过上述扩展卡尔曼滤波过程，可以得到在地理坐标系下对无人机的速度和位置信息。
进一步的，步骤2中，利用扩展卡尔曼滤波器去融合加速度计、磁力计和陀螺仪中的数据，系统状态向量可表示为Xk＝[q0 q1 q2 q3 ωx ωy ωz]T，其中，[q0 q1 q2 q3]T为系统的状态四元数，可以通过四阶龙格库塔法求出，[ωx ωy ωz]T为陀螺仪的输出值，系统的观测向量可以表示为Zk＝[q0 q1 q2 q3]T，其中，四个四元数利用加速度计和GPS的读数结合通过高斯牛顿法求出。
本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：
本发明提供一种基于北斗、GPS和SINS的组合导航方法，利用扩展卡尔曼滤波器，将SINS与北斗组合，对无人机的位置和速度进行估计；当无人机静止或匀速运动时，将陀螺仪与加速度计、GPS组合，对无人机的姿态进行估计，该方案有效的减少了惯性单元在导航时的时间累积误差，同时具有小型化、功耗低、轻型化、低成本、集成度高、工作稳定性、可靠性高、维护方便简单等优点。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：
图1是本发明一种基于北斗、GPS和SINS的组合导航方法的流程示意图；
图2是本发明一种基于北斗、GPS和SINS的组合导航方法中块匹配算法示意图；
图3是本发明一种基于北斗、GPS和SINS的组合导航方法中针孔(Pin-hole)模型图；
图4是本发明一种基于北斗、GPS和SINS的组合导航方法中IMU组合北斗传感器估计速度和位置示意图；
图5是本发明一种基于北斗、GPS和SINS的组合导航方法中加速度计组合GPS确定姿态示意图。
具体实施方式
以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
如图1所示，本实施例公开了一种基于北斗、GPS和SINS的组合导航方法，包括以下步骤：
步骤1：利用扩展卡尔曼滤波器将SINS与北斗组合，无论是在静止状态还是运动状态下，都可以对无人机的位置和运动速度进行有效估计；
步骤2：根据陀螺仪和加速度计/GPS测量姿态的特点，当检测到无人机处于静止或匀速运动时，将陀螺仪与加速度计、GPS进行组合，利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正，以此对无人机的姿态进行估计；
步骤3：当检测到无人机处于进行加速、减速或高速转动时，就不再进行姿态修正，而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算，直到无人机恢复静止或匀速状态。
进一步的，步骤1中，考虑到硬件平台和计算的复杂程度，选择基于最小误差绝对值和SAD的块匹配BMA算法进行北斗的计算：
如图2所示，设xp(i，j)和xc(i+Δi，j+Δj)分别表示前一帧中选取的大小为n×n目标块的灰度值和当前帧搜索区域中待匹配目标块的灰度值，其中1≤i，j≤n；-d≤Δi，Δj≤d，基于最小绝对误差和SAD匹配准则的块匹配算法就是在搜索区域里寻找满足下式的Δi，Δj，从而获得北斗矢量V＝r(Δi，Δj)T，V的单位为像素/秒，r为摄像头的采集频率，单位为帧/秒，而U为绝对值误差和的最小值，有如下的关系表达式：
Figure BDA0002269697620000091
U＝min(Δi，Δj){SAD(Δi，Δj)} (公式2)
V＝r(Δi，Δj)T|U (公式3)
在初始状态时，在成像平面的原点处选取一个目标块，当无人机运动时，目标块会在后一帧图像中移动，在后一帧的搜索区域内，该搜索区域限定了两个方向的最大位移d，使当前帧的目标块和前一帧的目标块内像素灰度差的SAD最小，以此得到目标块的北斗矢量；
在实验中，采集垂直于摄像头方向的图像，并使用8位的数据来表征每个像素的灰度，采集后的数据通过DMA存储在系统内存中，在整个采集过程中，选择8×8像素的数据块来作为块匹配的对象，搜索区域为±4个像素，所以对于每一帧图像，一共有64个像素点，同时有81个候选的向量方向，获得每一帧图像后，计算出各个候选向量的误差绝对值和，并选择其中的最小值作为北斗失量。
进一步的，步骤1中，使用针孔模型(Pin-hole image plane approach)，对无人机在地理坐标系下的运动进行估计：
针孔模型的示意图，如图3所示，设Pc＝[Xc，Yc，Zc]T是在摄像头坐标系下的一个点，f代表了摄像头的焦距，于是这一点在摄像头成像平面下表示为p＝[x，y，f]T，并有关系：
Figure BDA0002269697620000101
上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量p＝[x，y，f]T的第三项为常数并等于焦距。考虑地面上的任意一点P，在摄像头坐标系下P相对于无人机有如下的运动关系：
Vc＝-Tc-ω×Pc (公式7)
将公式7在三个维度上展开，可以得到：
Figure BDA0002269697620000103
Figure BDA0002269697620000104
Figure BDA0002269697620000105
其中，ω＝[ωx，ωy，ωz]T是无人机的角速度，
Figure BDA0002269697620000106
是无人机在摄像头坐标系的平动速度；
对公式4两边求导，可以得到Pc在摄像头坐标下的速度与p在成像平面下的速度的关系：
Figure BDA0002269697620000107
其中，v＝[vx，vy，vz]T，将上述公式11在三个维度上展开，可以得到：
Figure BDA0002269697620000109
vz＝0 (公式14)
将公式8、公式9、公式10代入公式12、公式13，可以得到：
Figure BDA0002269697620000112
公式15和公式16中的vx、vy是北斗矢量在x方向和y方向上的分量，可以通过块匹配最小误差绝对和的方法计算得出；式中的Zc可以由北斗传感器板载的超声波传感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺仪得出；式中的x，y用公式5和公式6代入，估算出飞行器在摄像头坐标系下的平动速度Tc，再通过坐标系变换矩阵
Figure BDA0002269697620000113
得到无人机在地理坐标系下的速度，积分后得到无人机在地理坐标系下的位置信息。
进一步的，步骤2中，加速度计通过敏感重力场，在载体不存在自身加速度的情况下，可以确定无人机的横滚角和俯仰角；GPS借助于加速度计得到的载体姿态信息，可以解算得到无人机航向角；两者相组合，就可以得到不随时间积累误差的全姿态信息。
进一步的，利用加速度计获取载体的横滚角和俯仰角重力矢量在地理坐标系下的分量为[0 0 -g]T，当载体处于静止态，相对导航坐标系无加速度时，安装在载体坐标系下的加速度计测量值为
Figure BDA0002269697620000114
由于重力加速度垂直于水平面，故载体的航向角不影响x向和y向加速度计的输出，于是有关系：
Figure BDA0002269697620000115
联立可以解得俯仰角和横滚角为：
Figure BDA0002269697620000121
利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息，故只有在载体无加速运动的情况下上述算式才成立。而在实际系统中，载体并不是始终保持静止或匀速运动的，一旦载体有了加速运动，加速度计的测量值就不再与重力加速度在载体系三轴上的分量相等。所以该方案只能用于静态姿态测量，动态时的姿态测量还要另寻方案。
进一步的，利用GPS获取载体的航向角，设地磁强度在地理坐标系三轴上的分量为GPS沿体坐标系Fb三轴方向安装，地磁强度在载体坐标系三轴上的分量分别为
Figure BDA0002269697620000123
地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示：
Figure BDA0002269697620000124
式中，
Figure BDA0002269697620000125
的值通过查表给出，假设在苏州地区，北纬37.8度，东经112.5度，其值如公式21所示，而的数值由机载的GPS给出：
Figure BDA0002269697620000127
假定无人机在飞行过程中地磁场保持恒定，联立公式20和公式21，再结合由加速度计确定的俯仰角和横滚角，就可以解算出无人机在地理坐标系下的航向角。
进一步的，步骤1中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在导航坐标系下的速度和位置信息作为状态量，北斗传感器和超声波传感器的输出作为观测量，联合估计无人机的位置和速度信息，如图4所示：
考虑非线性系统的状态方程和观测方程：
Xk+1＝f[Xk，k]+BkUk+ΓkWk (方程1)
Zk+1＝h[Xk+1，k+1]+Vk+1 (方程2)
其中，
Figure BDA0002269697620000131
是状态向量，包括无人机的速度信息和位置信息；Zk＝[x y Zc]T是观测向量，包括北斗传感器输出的x方向北斗和y方向北斗和超声波传感器的读数Zc；
Figure BDA0002269697620000132
是系统的控制向量，由加速度计的数值经过坐标系矩阵变换得出；Bk是控制分配矩阵；Γk是噪声分配矩阵；Wk是过程噪声矩阵；Vk是测量噪声；f代表系统状态函数；h代表观测函数；
将状态方程和观测方程代入扩展卡尔曼滤波器，可以得到：
Figure BDA0002269697620000133
Figure BDA0002269697620000136
Figure BDA0002269697620000137
其中，
Figure BDA0002269697620000138
通过上述扩展卡尔曼滤波过程，可以得到在地理坐标系下对无人机的速度和位置信息。
通过陀螺输出角速率信号积分可以获取无人机的姿态，但是MEMS陀螺仪存在较为严重的漂移问题，而加速度计/磁力计组合能提供不同噪声、无漂移的姿态，所以在步骤2中，利用扩展卡尔曼滤波器去融合加速度计、磁力计和陀螺仪中的数据，滤波过程如图5所示，系统状态向量可表示为Xk＝[q0 q1 q2 q3 ωx ωy ωz]T，其中，[q0 q1 q2 q3]T为系统的状态四元数，可以通过四阶龙格库塔法求出，[ωx ωy ωz]T为陀螺仪的输出值，系统的观测向量可以表示为Zk＝[q0 q1 q2 q3]T，其中，四个四元数利用加速度计和GPS的读数结合通过高斯牛顿法求出。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (8)

1.一种基于北斗、GPS和SINS的组合导航方法，其特征在于，包括以下步骤：
步骤1：利用扩展卡尔曼滤波器将SINS与北斗组合，对无人机的位置和运动速度进行有效估计；
步骤2：当检测到无人机处于静止或匀速运动时，将陀螺仪与加速度计、GPS进行组合，利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正，以此对无人机的姿态进行估计；
步骤3：当检测到无人机处于进行加速、减速或高速转动时，就不再进行姿态修正，而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算，直到无人机恢复静止或匀速状态。
2.根据权利要求1所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，步骤1中，选择基于最小误差绝对值和SAD的块匹配BMA算法进行北斗的计算：
设xp(i，j)和xc(i+Δi，j+Δj)分别表示前一帧中选取的大小为n×n目标块的灰度值和当前帧搜索区域中待匹配目标块的灰度值，其中1≤i，j≤n；-d≤Δi，Δj≤d，基于最小绝对误差和SAD匹配准则的块匹配算法就是在搜索区域里寻找满足下式的Δi，Δj，从而获得北斗矢量V＝r(Δi，Δj)T，V的单位为像素/秒，r为摄像头的采集频率，单位为帧/秒，而U为绝对值误差和的最小值，有如下的关系表达式：
U＝min(Δi，Δj){SAD(Δi，Δj)} (公式2)
V＝r(Δi，Δj)T|U (公式3)
在初始状态时，在成像平面的原点处选取一个目标块，当无人机运动时，目标块会在后一帧图像中移动，在后一帧的搜索区域内，该搜索区域限定了两个方向的最大位移d，使当前帧的目标块和前一帧的目标块内像素灰度差的SAD最小，以此得到目标块的北斗矢量；
采集垂直于摄像头方向的图像，并使用8位的数据来表征每个像素的灰度，采集后的数据通过DMA存储在系统内存中，在整个采集过程中，选择8×8像素的数据块来作为块匹配的对象，搜索区域为±4个像素，所以对于每一帧图像，一共有64个像素点，同时有81个候选的向量方向，获得每一帧图像后，计算出各个候选向量的误差绝对值和，并选择其中的最小值作为北斗失量。
3.根据权利要求2所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，步骤1中，使用针孔模型，对无人机在地理坐标系下的运动进行估计：
针孔模型中，设Pc＝[Xc，Yc，Zc]T是在摄像头坐标系下的一个点，f代表了摄像头的焦距，于是这一点在摄像头成像平面下表示为p＝[x，y，f]T，并有关系：
Figure FDA0002269697610000021
Figure FDA0002269697610000022
Figure FDA0002269697610000023
上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量p＝[x，y，f]T的第三项为常数并等于焦距。考虑地面上的任意一点P，在摄像头坐标系下P相对于无人机有如下的运动关系：
Vc＝-Tc-ω×Pc (公式7)
将公式7在三个维度上展开，可以得到：
Figure FDA0002269697610000024
Figure FDA0002269697610000025
Figure FDA0002269697610000026
其中，ω＝[ωx，ωy，ωz]T是无人机的角速度，是无人机在摄像头坐标系的平动速度；
对公式4两边求导，可以得到Pc在摄像头坐标下的速度与p在成像平面下的速度的关系：
Figure FDA0002269697610000031
其中，v＝[vx，vy，vz]T，将上述公式11在三个维度上展开，可以得到：
Figure FDA0002269697610000032
vz＝0 (公式14)
将公式8、公式9、公式10代入公式12、公式13，可以得到：
Figure FDA0002269697610000034
Figure FDA0002269697610000035
公式15和公式16中的vx、vy是北斗矢量在x方向和y方向上的分量，可以通过块匹配最小误差绝对和的方法计算得出；式中的Zc可以由北斗传感器板载的超声波传感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺仪得出；式中的x，y用公式5和公式6代入，估算出飞行器在摄像头坐标系下的平动速度Tc，再通过坐标系变换矩阵
Figure FDA0002269697610000036
得到无人机在地理坐标系下的速度，积分后得到无人机在地理坐标系下的位置信息。
4.根据权利要求1所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，步骤2中，加速度计通过敏感重力场，在载体不存在自身加速度的情况下，可以确定无人机的横滚角和俯仰角；GPS借助于加速度计得到的载体姿态信息，可以解算得到无人机航向角；两者相组合，就可以得到不随时间积累误差的全姿态信息。
5.根据权利要求4所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，利用加速度计获取载体的横滚角和俯仰角重力矢量在地理坐标系下的分量为[0 0 -g]T，当载体处于静止态，相对导航坐标系无加速度时，安装在载体坐标系下的加速度计测量值为
Figure FDA0002269697610000041
由于重力加速度垂直于水平面，故载体的航向角不影响x向和y向加速度计的输出，于是有关系：
Figure FDA0002269697610000042
联立可以解得俯仰角和横滚角为：
Figure FDA0002269697610000044
利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息，故只有在载体无加速运动的情况下上述算式才成立。
6.根据权利要求5所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，利用GPS获取载体的航向角，设地磁强度在地理坐标系三轴上的分量为
Figure FDA0002269697610000045
GPS沿体坐标系Fb三轴方向安装，地磁强度在载体坐标系三轴上的分量分别为
Figure FDA0002269697610000046
地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示：
Figure FDA0002269697610000051
式中，
Figure FDA0002269697610000052
的值通过查表给出，假设在苏州地区，北纬37.8度，东经112.5度，其值如公式21所示，而
Figure FDA0002269697610000053
的数值由机载的GPS给出：
Figure FDA0002269697610000054
假定无人机在飞行过程中地磁场保持恒定，联立公式20和公式21，再结合由加速度计确定的俯仰角和横滚角，就可以解算出无人机在地理坐标系下的航向角。
7.根据权利要求1所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，步骤1中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在导航坐标系下的速度和位置信息作为状态量，北斗传感器和超声波传感器的输出作为观测量，联合估计无人机的位置和速度信息：
考虑非线性系统的状态方程和观测方程：
Xk+1＝f[Xk，k]+BkUk+ΓkWk (方程1)
Zk+1＝h[Xk+1，k+1]+Vk+1 (方程2)
其中，
Figure FDA0002269697610000055
是状态向量，包括无人机的速度信息和位置信息；Zk＝[x y Zc]T是观测向量，包括北斗传感器输出的x方向北斗和y方向北斗和超声波传感器的读数Zc；
Figure FDA0002269697610000056
是系统的控制向量，由加速度计的数值经过坐标系矩阵变换得出；Bk是控制分配矩阵；Γk是噪声分配矩阵；Wk是过程噪声矩阵；Vk是测量噪声；f代表系统状态函数；h代表观测函数；
将状态方程和观测方程代入扩展卡尔曼滤波器，可以得到：
Figure FDA0002269697610000062
Figure FDA0002269697610000064
其中，
Figure FDA0002269697610000066
通过上述扩展卡尔曼滤波过程，可以得到在地理坐标系下对无人机的速度和位置信息。
8.根据权利要求1所述的一种基于北斗、GPS和SINS的组合导航方法，其特征在于，步骤2中，利用扩展卡尔曼滤波器去融合加速度计、磁力计和陀螺仪中的数据，系统状态向量可表示为Xk＝[q0 q1 q2 q3 ωx ωy ωz]T，其中，[q0 q1 q2 q3]T为系统的状态四元数，可以通过四阶龙格库塔法求出，[ωx ωy ωz]T为陀螺仪的输出值，系统的观测向量可以表示为Zk＝[q0 q1 q2 q3]T，其中，四个四元数利用加速度计和GPS的读数结合通过高斯牛顿法求出。
CN201911103352.9A 2019-11-12 2019-11-12 一种基于北斗、gps和sins的组合导航方法 Pending CN110736457A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911103352.9A CN110736457A (zh) 2019-11-12 2019-11-12 一种基于北斗、gps和sins的组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911103352.9A CN110736457A (zh) 2019-11-12 2019-11-12 一种基于北斗、gps和sins的组合导航方法

Publications (1)

Publication Number Publication Date
CN110736457A true CN110736457A (zh) 2020-01-31

Family

ID=69272671

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911103352.9A Pending CN110736457A (zh) 2019-11-12 2019-11-12 一种基于北斗、gps和sins的组合导航方法

Country Status (1)

Country Link
CN (1) CN110736457A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558133A (zh) * 2020-11-23 2021-03-26 南京航空航天大学 一种基于四阶累积量music算法的飞行器姿态估计方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN113273511A (zh) * 2021-05-14 2021-08-20 深圳德技创新实业有限公司 动物监测装置及方法
CN114199239A (zh) * 2022-01-05 2022-03-18 中国兵器工业计算机应用技术研究所 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统
CN112630813B (zh) * 2020-11-24 2024-05-03 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
化雪荟等: "INS/光流/磁强计组合导航在小型无人机中的应用", 《电子器件》 *
李涛等: "INS/光流/磁强计/气压计组合导航系统在无人机中的应用", 《激光与光电子学进展》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558133A (zh) * 2020-11-23 2021-03-26 南京航空航天大学 一种基于四阶累积量music算法的飞行器姿态估计方法
CN112558133B (zh) * 2020-11-23 2024-04-16 南京航空航天大学 一种基于四阶累积量music算法的飞行器姿态估计方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112630813B (zh) * 2020-11-24 2024-05-03 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN113273511A (zh) * 2021-05-14 2021-08-20 深圳德技创新实业有限公司 动物监测装置及方法
CN114199239A (zh) * 2022-01-05 2022-03-18 中国兵器工业计算机应用技术研究所 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统
CN114199239B (zh) * 2022-01-05 2024-04-16 中国兵器工业计算机应用技术研究所 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统

Similar Documents

Publication Publication Date Title
CN109540126B (zh) 一种基于光流法的惯性视觉组合导航方法
CA2502340C (en) Inertial navigation system error correction
CN109887057B (zh) 生成高精度地图的方法和装置
CN107727079B (zh) 一种微小型无人机全捷联下视相机的目标定位方法
EP2434256B1 (en) Camera and inertial measurement unit integration with navigation data feedback for feature tracking
EP2133662B1 (en) Methods and system of navigation using terrain features
CN110501024A (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
Shen et al. Optical flow sensor/INS/magnetometer integrated navigation system for MAV in GPS-denied environment
US20150234055A1 (en) Aerial and close-range photogrammetry
JP2008304260A (ja) 画像処理装置
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
CN110736457A (zh) 一种基于北斗、gps和sins的组合导航方法
JP2009501996A (ja) 映像の幾何補正方法およびその装置
CN110865650A (zh) 基于主动视觉的无人机位姿自适应估计方法
RU2762143C2 (ru) Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области
JP4986883B2 (ja) 標定装置、標定方法および標定プログラム
RU2592715C1 (ru) Астронавигационная система
Ding et al. Adding optical flow into the GPS/INS integration for UAV navigation
CN110779514B (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
US11037018B2 (en) Navigation augmentation system and method
Veth et al. Tightly-coupled ins, gps, and imaging sensors for precision geolocation
Chavez Armijos et al. Vision-based state estimation for asteroid exploration
Maggio et al. Vision-based terrain relative navigation on high altitude balloon and sub-orbital rocket
Indelman et al. Real-time mosaic-aided aerial navigation: II. Sensor fusion
Hurwitz et al. Relative Constraints and Their Contribution to Image Configurations

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

Application publication date: 20200131