一种基于北斗、GPS和SINS的组合导航方法与流程

本发明涉及导航技术领域，特别涉及一种基于北斗、gps和sins的组合导航方法。

背景技术：

当今世界上一共存在四大全球卫星导航系统，其中包括美国的gps，俄罗斯的glonass，欧盟的galileo和中国的北斗卫星导航系统。随着北斗卫星导航系统的“三步走”部署开始实施，北斗卫星导航这个新兴技术越来越受到各行各业的青睐。

近年来，国内外的一些研究人员受到昆虫利用视觉进行导航的启发，使用北斗法进行小型无人机的导航。国外方面，nilsgageilk、dominilkhonegger等人利用北斗完成了无人机的定点悬停、自主驾驶以及速度估计，d.a.mercado、jinlingwang等人进行了北斗/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，并有关系：

上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量p＝[x，y，f]^t的第三项为常数并等于焦距。考虑地面上的任意一点p，在摄像头坐标系下p相对于无人机有如下的运动关系：

vc＝-tc-ω×pc(公式7)

将公式7在三个维度上展开，可以得到：

其中，ω＝[ωx，ωy，ωz]^t是无人机的角速度，是无人机在摄像头坐标系的平动速度；

对公式4两边求导，可以得到pc在摄像头坐标下的速度与p在成像平面下的速度的关系：

其中，v＝[vx，vy，vz]^t，将上述公式11在三个维度上展开，可以得到：

vz＝0(公式14)

将公式8、公式9、公式10代入公式12、公式13，可以得到：

公式15和公式16中的vx、vy是北斗矢量在x方向和y方向上的分量，可以通过块匹配最小误差绝对和的方法计算得出；式中的zc可以由北斗传感器板载的超声波传感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺仪得出；式中的x，y用公式5和公式6代入，估算出飞行器在摄像头坐标系下的平动速度tc，再通过坐标系变换矩阵得到无人机在地理坐标系下的速度，积分后得到无人机在地理坐标系下的位置信息。

进一步的，步骤2中，加速度计通过敏感重力场，在载体不存在自身加速度的情况下，可以确定无人机的横滚角和俯仰角；gps借助于加速度计得到的载体姿态信息，可以解算得到无人机航向角；两者相组合，就可以得到不随时间积累误差的全姿态信息。

进一步的，利用加速度计获取载体的横滚角和俯仰角重力矢量在地理坐标系下的分量为[00-g]^t，当载体处于静止态，相对导航坐标系无加速度时，安装在载体坐标系下的加速度计测量值为由于重力加速度垂直于水平面，故载体的航向角不影响x向和y向加速度计的输出，于是有关系：

联立可以解得俯仰角和横滚角为：

利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息，故只有在载体无加速运动的情况下上述算式才成立。

进一步的，利用gps获取载体的航向角，设地磁强度在地理坐标系三轴上的分量为gps沿体坐标系fb三轴方向安装，地磁强度在载体坐标系三轴上的分量分别为地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示：

式中，的值通过查表给出，假设在苏州地区，北纬37.8度，东经112.5度，其值如公式21所示，而的数值由机载的gps给出：

假定无人机在飞行过程中地磁场保持恒定，联立公式20和公式21，再结合由加速度计确定的俯仰角和横滚角，就可以解算出无人机在地理坐标系下的航向角。

进一步的，步骤1中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在导航坐标系下的速度和位置信息作为状态量，北斗传感器和超声波传感器的输出作为观测量，联合估计无人机的位置和速度信息：

考虑非线性系统的状态方程和观测方程：

xk+1＝f[xk，k]+bkuk+γkwk(方程1)

zk+1＝h[xk+1，k+1]+vk+1(方程2)

其中，是状态向量，包括无人机的速度信息和位置信息；zk＝[xyzc]^t是观测向量，包括北斗传感器输出的x方向北斗和y方向北斗和超声波传感器的读数zc；是系统的控制向量，由加速度计的数值经过坐标系矩阵变换得出；bk是控制分配矩阵；γk是噪声分配矩阵；wk是过程噪声矩阵；vk是测量噪声；f代表系统状态函数；h代表观测函数；

将状态方程和观测方程代入扩展卡尔曼滤波器，可以得到：

其中，

通过上述扩展卡尔曼滤波过程，可以得到在地理坐标系下对无人机的速度和位置信息。

进一步的，步骤2中，利用扩展卡尔曼滤波器去融合加速度计、磁力计和陀螺仪中的数据，系统状态向量可表示为xk＝[q0q1q2q3ωxωyωz]^t，其中，[q0q1q2q3]^t为系统的状态四元数，可以通过四阶龙格库塔法求出，[ωxωyωz]^t为陀螺仪的输出值，系统的观测向量可以表示为zk＝[q0q1q2q3]^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为绝对值误差和的最小值，有如下的关系表达式：

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-holeimageplaneapproach)，对无人机在地理坐标系下的运动进行估计：

针孔模型的示意图，如图3所示，设pc＝[xc，yc，zc]^t是在摄像头坐标系下的一个点，f代表了摄像头的焦距，于是这一点在摄像头成像平面下表示为p＝[x，y，f]^t，并有关系：

上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量p＝[x，y，f]^t的第三项为常数并等于焦距。考虑地面上的任意一点p，在摄像头坐标系下p相对于无人机有如下的运动关系：

vc＝-tc-ω×pc(公式7)

将公式7在三个维度上展开，可以得到：

其中，ω＝[ωx，ωy，ωz]^t是无人机的角速度，是无人机在摄像头坐标系的平动速度；

对公式4两边求导，可以得到pc在摄像头坐标下的速度与p在成像平面下的速度的关系：

其中，v＝[vx，vy，vz]^t，将上述公式11在三个维度上展开，可以得到：

vz＝0(公式14)

将公式8、公式9、公式10代入公式12、公式13，可以得到：

公式15和公式16中的vx、vy是北斗矢量在x方向和y方向上的分量，可以通过块匹配最小误差绝对和的方法计算得出；式中的zc可以由北斗传感器板载的超声波传感器得出；式中的角速度值ωx、ωy、ωz，可以由陀螺仪得出；式中的x，y用公式5和公式6代入，估算出飞行器在摄像头坐标系下的平动速度tc，再通过坐标系变换矩阵得到无人机在地理坐标系下的速度，积分后得到无人机在地理坐标系下的位置信息。

进一步的，步骤2中，加速度计通过敏感重力场，在载体不存在自身加速度的情况下，可以确定无人机的横滚角和俯仰角；gps借助于加速度计得到的载体姿态信息，可以解算得到无人机航向角；两者相组合，就可以得到不随时间积累误差的全姿态信息。

进一步的，利用加速度计获取载体的横滚角和俯仰角重力矢量在地理坐标系下的分量为[00-g]^t，当载体处于静止态，相对导航坐标系无加速度时，安装在载体坐标系下的加速度计测量值为由于重力加速度垂直于水平面，故载体的航向角不影响x向和y向加速度计的输出，于是有关系：

联立可以解得俯仰角和横滚角为：

利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息，故只有在载体无加速运动的情况下上述算式才成立。而在实际系统中，载体并不是始终保持静止或匀速运动的，一旦载体有了加速运动，加速度计的测量值就不再与重力加速度在载体系三轴上的分量相等。所以该方案只能用于静态姿态测量，动态时的姿态测量还要另寻方案。

进一步的，利用gps获取载体的航向角，设地磁强度在地理坐标系三轴上的分量为gps沿体坐标系fb三轴方向安装，地磁强度在载体坐标系三轴上的分量分别为地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示：

式中，的值通过查表给出，假设在苏州地区，北纬37.8度，东经112.5度，其值如公式21所示，而的数值由机载的gps给出：

假定无人机在飞行过程中地磁场保持恒定，联立公式20和公式21，再结合由加速度计确定的俯仰角和横滚角，就可以解算出无人机在地理坐标系下的航向角。

进一步的，步骤1中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在导航坐标系下的速度和位置信息作为状态量，北斗传感器和超声波传感器的输出作为观测量，联合估计无人机的位置和速度信息，如图4所示：

考虑非线性系统的状态方程和观测方程：

xk+1＝f[xk，k]+bkuk+γkwk(方程1)

zk+1＝h[xk+1，k+1]+vk+1(方程2)

其中，是状态向量，包括无人机的速度信息和位置信息；zk＝[xyzc]^t是观测向量，包括北斗传感器输出的x方向北斗和y方向北斗和超声波传感器的读数zc；是系统的控制向量，由加速度计的数值经过坐标系矩阵变换得出；bk是控制分配矩阵；γk是噪声分配矩阵；wk是过程噪声矩阵；vk是测量噪声；f代表系统状态函数；h代表观测函数；

将状态方程和观测方程代入扩展卡尔曼滤波器，可以得到：

其中，

通过上述扩展卡尔曼滤波过程，可以得到在地理坐标系下对无人机的速度和位置信息。

通过陀螺输出角速率信号积分可以获取无人机的姿态，但是mems陀螺仪存在较为严重的漂移问题，而加速度计/磁力计组合能提供不同噪声、无漂移的姿态，所以在步骤2中，利用扩展卡尔曼滤波器去融合加速度计、磁力计和陀螺仪中的数据，滤波过程如图5所示，系统状态向量可表示为xk＝[q0q1q2q3ωxωyωz]^t，其中，[q0q1q2q3]^t为系统的状态四元数，可以通过四阶龙格库塔法求出，[ωxωyωz]^t为陀螺仪的输出值，系统的观测向量可以表示为zk＝[q0q1q2q3]^t，其中，四个四元数利用加速度计和gps的读数结合通过高斯牛顿法求出。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。