一种基于北斗、GPS和SINS的组合导航方法技术

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

An integrated navigation method based on Beidou, GPS and sins

【技术实现步骤摘要】
一种基于北斗、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中，利用扩展卡尔曼滤波将加速度计数据和北斗传感器的数据进行融合，选取无人机在本文档来自技高网...

【技术保护点】
1.一种基于北斗、GPS和SINS的组合导航方法，其特征在于，包括以下步骤：/n步骤1：利用扩展卡尔曼滤波器将SINS与北斗组合，对无人机的位置和运动速度进行有效估计；/n步骤2：当检测到无人机处于静止或匀速运动时，将陀螺仪与加速度计、GPS进行组合，利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正，以此对无人机的姿态进行估计；/n步骤3：当检测到无人机处于进行加速、减速或高速转动时，就不再进行姿态修正，而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算，直到无人机恢复静止或匀速状态。/n

【技术特征摘要】
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，并有关系：









上式中，因为从摄像头到成相平面的距离的始终为焦距，所以向量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，可以由陀螺仪得出...

【专利技术属性】
技术研发人员：王栋，戴丽华，
申请(专利权)人：苏州工业职业技术学院，
类型：发明
国别省市：江苏;32