专利名称：基于航向角的wsn/ins组合导航系统的制作方法
技术领域：
本实用新型涉及一种基于航向角的WSN/INS组合导航系统，属于多传感器数据融合领域。
背景技术：
全球定位系统(Globalpositioning systems, GPS)和惯性导航系统(Inertialnavigation system, INS)均为目前应用最广泛的导航系统之一。其中GPS能够提供精确地，具有持续稳定导航精度的导航信息，但是在室内、高层建筑密集的市区、矿井、隧道等环境下，GPS信号失锁，不能进行定位。INS虽然具有全自主、运动信息且全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度严重 下降。因此，INS对GPS导航信息的补偿只能是短期补偿，而目前最为常用的GPS/INS组合导航系统的导航精度依赖于GPS的导航精度，在GPS长时间失锁的情况下，组合导航系统无法提供高精度的导航信息。近年来,无线传感器网络(Wireless Sensors Network, WSN)以其低成本、低功耗和低系统复杂度的特点在短距离定位领域表现出很大的潜力。WSN为在无GPS信号地区，即所谓的“盲区”时，如室内、高层建筑密集的市区、矿井、隧道等环境下未知节点定位提供了可能。但由于WSN采用的通信技术通常为短距离无线通信技术(如ZigBee、WIFI等)，因此若想完成长距离的目标跟踪定位，需要大量的网络节点共同完成，这增加了 WSN的网络负担。除此之外，WSN只能提供位置和速度信息，不能提供全面的运动信息。为了在GPS长失锁环境下得到长时间的稳定的导航信息，许多学者提出将WSN定位技术引入到低成本的INS系统中，构建WSN/INS组合导航系统，如东南大学Y. Xu,虽然这一组合方式很好的解决了地下密闭环境下的长距离目标跟踪和导航设备成本高的问题，但是这种方法只是运用了载体的速度、未知节点与参考节点之间的距离信息来对载体的导航信息进行预估，并没有将INS得到的载体的姿态信息(如航向角，加速度计等)考虑进去，没有有效的利用周围的环境信息。
实用新型内容为了解决上述问题，本实用新型提出了一种基于航向角的WSN/INS组合导航系统。本实用新型为解决其技术问题采用如下技术方案一种基于航向角的WSN/INS组合导航系统，包括参考节点部分和未知节点部分，参考节点部分包括参考节点无线网络接收�？椤⒊饩嗄？楹褪奔渫侥？椋渲校慰冀诘悴糠种械牟慰冀诘阄尴咄缃邮漳？楦涸鹜瓿稍冻堂畹氖辗ⅲ诮拥皆冻堂钪笫紫瓤刂剖奔渫侥？橥瓿刹饩嗟氖奔渫剑幼糯蚩饩嗄？椴饬肯嘤Φ木嗬胄畔ⅲ詈螅饩嗄？椴饬康玫降木嗬胄畔⑼ü慰冀诘阄尴咄缃邮漳？榉祷馗粗诘悖晃粗诘悴糠职ㄎ粗诘阄尴咄缃邮漳？椤NS导航�？楹椭醒胧荽砟？椋渲校粗诘悴糠种械奈粗诘阄尴咄缃邮漳？楦涸鹜瓿晌粗诘阌氩慰冀诘阒涞木嗬胄畔⒌牟杉⒔杉玫降氖荽透醒胧荽砟？椋籌NS导航�？橥瓿傻氖峭勇菁颖硇畔⒌牟杉⒔杉玫降氖荽透醒胧荽淼ピ恢醒胧荽淼ピ涸鸾粗诘阄尴咄缃邮漳？楹虸NS导航�？椴杉降氖萁惺萑诤希佣玫降己叫畔�。所述的基于航向角的WSN/INS组合导航系统的控制方法，包括下列步骤(I)将导航过程分为培训过程和自适应补偿过程两部分，将有WSN信号的导航过程称为培训过程，而只有INS信号的导航过程称之为自适应补偿过程；(2)在培训过程中，在本地相对坐标系中将INS和WSN进行集成，通过扩展卡尔曼滤波对得到的同步导航数据进行数据融合；(3)构建扩展卡尔曼滤波器的系统方程，该系统方程以INS每一时刻两个方向的位直"]^差(^雄，气V占)、速度"^差)、加速度计"^差)和航向角性作为状
态变量，滤波器的系统方程如式(I)所示，其中r为采样周期w力系统噪声^力载体的·航向角，k为系统的采样周期计数；
「 ， I 0 T 0 cos --JeSinai 0「 ]「 刁
2 2
e^M o I o r siii^ I i^cosf> o 气v’k m^k
eVjrfM22em^k %,*■
0 — 0 0 I 0 rcosffi -Tsmm 0 0 ,(I)
p0 0 0 1 rsina Tcosm 0 p m
cOxMlk
0 0 0 0 I0 0 . m ,
^+1警,
0 0 0 0 0I Offl
興+i」L興」L
00 0 000IL J
%fi LJ xk
A(4)通过INS测量每个时刻未知节点在x和y方向的位置，将得到的位置信息与参考节点的位置信息通过距离公式计算出INS测量得到的未知节点和第I个参考节点之间的
距离如式⑵所示Lf) = I x_ - Xi f + (y_ - )2:1 = 1，2,…,N(2)其中，为INS测量得到的未知节点的坐标，(Ii5y.)为第i个参考节点的坐标N力参考节点的数目；通过超声测距�？椴饬砍鑫粗诘愫偷趇个参考节点之间的距离将Lf)的平方与I^sa)的平方作差，差值定义为，在此基础上，以
Ml-AL1G 22)作为滤波器的观测量，与此同时，将INS测量的每个时刻的两个方向的速度与WSN中包含的测速传感器测量得到的速度作差，将差值也作为滤波器的观测量，同时，将INS测量得到的航向角也作为观测量，通过上述的观测量构建观测方程，滤波器观测方程如式⑶所示
权利要求1.一种基于航向角的WSN/INS组合导航系统，其特征在于包括参考节点部分和未知节点部分，参考节点部分包括参考节点无线网络接收�？�、超声测距�？楹褪奔渫侥？椋渲校慰冀诘悴糠种械牟慰冀诘阄尴咄缃邮漳？楦涸鹜瓿稍冻堂畹氖辗ⅲ诮拥皆冻堂钪笫紫瓤刂剖奔渫侥？橥瓿刹饩嗟氖奔渫剑幼糯蚩饩嗄？椴饬肯嘤Φ木嗬胄畔ⅲ詈螅饩嗄？椴饬康玫降木嗬胄畔⑼ü慰冀诘阄尴咄缃邮漳？榉祷馗粗诘悖晃粗诘悴糠职ㄎ粗诘阄尴咄缃邮漳？�、INS导航模块和中央数据处理�？椋渲校粗诘悴糠种械奈粗诘阄尴咄缃邮漳？楦涸鹜瓿晌粗诘阌氩慰冀诘阒涞木嗬胄畔⒌牟杉⒔杉玫降氖荽透醒胧荽砟？椋籌NS导航�？橥瓿傻氖峭勇菁颖硇畔⒌牟杉⒔杉玫降氖荽透醒胧荽淼ピ恢醒胧荽淼ピ涸鸾粗诘阄尴咄缃邮漳？楹虸NS导航模块采集到的数据进行数据融合，从而得到导航信息。
专利摘要一种基于航向角的WSN/INS组合导航系统，属于多传感器数据融合领域。该组合导航系统包括参考节点部分和未知节点部分，参考节点部分包括参考节点无线网络接收�？�、超声测距�？楹褪奔渫侥？椋晃粗诘悴糠职ㄎ粗诘阄尴咄缃邮漳？�、INS导航�？楹椭醒胧荽砟？�。利用扩展卡尔曼滤波得到系统中INS测量的位置误差和速度误差最优预估值，将INS本身测出的导航信息与最优预估值作差，得出最优的导航信息。本实用新型通过运用WSN/INS组合导航系统，克服了在地下巷道、狭长隧道等GPS信号长期失锁的环境下INS导航的误差随时间漂移的问题。能满足地面城市交通、狭长隧道、小型智能机器人等中低精度的定位和定向的要求。
文档编号G01C21/20GK202562486SQ20122019039
公开日2012年11月28日 申请日期2012年5月2日 优先权日2012年5月2日
发明者陈熙源, 徐元, 李庆华, 黄浩乾, 申冲 申请人:东南大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
