导航原理基础实验指导书

导航原理基础实验导航原理基础实验指导书指导书 承担单位：承担单位：航空航天学院航空航天学院 负责教师：负责教师：李李 滚滚/杜杜 爽爽 2016 年 4 月 I 目 录 实验一实验一 惯性测量单元误差标定与分析实验惯性测量单元误差标定与分析实验 . 1 一、实验目的 . 1 二、实验内容 . 1 三、实验原理 . 1 四、实验器材 . 2 五、实验步骤 . 3 六.实验注意事项. 3 七.实验报告要求. 3 实验二实验二 惯性导航静态实验惯性导航静态实验 . 5 一、实验目的 . 5 二、实验内容 . 5 三、实验原理 . 5 四、实验器材 . 8 五、实验步骤 . 8 六、实验注意事项. 8 七、实验报告要求. 8 实验实验三三 导航解算（接收机位置、时间）及结果分析导航解算（接收机位置、时间）及结果分析 . 9 一、实验目的 . 9 二、实验内容 . 9 三、实验原理 . 9 四、实验器材 . 13 五、实验步骤 . 13 六、实验注意事项. 14 七、实验报告要求. 14 1 实验实验一一 惯性测量单元误差标定惯性测量单元误差标定与分析实验与分析实验 一、一、实验实验目的目的 1、通过在速率转台上的测试实验，增强动手能力和对惯性测试设备的感性 认识； 2、通过对加速度计数据的分析，对加速度计误差参数的物理意义有清晰的 认识； 3、通过对陀螺仪数据的分析，对陀螺仪误差参数的物理意义有清晰的认识； 4、通过惯性测量单元在速率转台上的数据，完成对惯性测量单元的标定， 对惯性测量单元的误差模型有清晰的认识 5、为在实际工程中应用惯性测量单元和对惯性测量单元进行误差建模与补 偿奠定基础。 二、二、实验内容实验内容 1、根据与惯性测量单元的误差模型，确定利用单轴速率转台上能进行标定 的惯性测量单元误差参数； 2、利用惯性测量单元在单轴速率转台上的数据，进行加速度计和陀螺仪的 误差标定； 3、进行零偏重复性测试； 4、进行加速度计和陀螺仪误差补偿实验。 三、三、实验实验原理原理 1、加速度计的误差模型、加速度计的误差模型 惯性测量单元中的加速度计可以测量非引力加速度。 加速度计的误差包括偏 差，标度因数，以及噪声。其中偏差又可分为常值部分与随机部分。单轴加速度 计的误差模型可表示为： fxfxfxxx Sbff （1-1） 式中 x f 表示加速度计的实际输出， x f表示加速度计真实输入， fx b表示偏差， fx S表示标度因数，而 fx 表示噪声。 2、陀螺仪的误差模型 惯性测量单元中的陀螺仪可以测量载体运动的角速度。 陀螺仪的误差包括偏 差，标度因数，以及噪声。与加速度计类似，其偏差也可分为常值部分和随机部 分。单轴陀螺仪的误差模型可表示为： xxxxx Sb （1-2） 2 式中 x 表示陀螺仪的实际输出， x 表示陀螺仪真实输入， x b表示偏差， x S 表示标度因数，而 x 表示噪声。 3、惯性测量单元的误差模型 惯性测量单元由三轴相互垂直的加速度计和陀螺仪组成。 在加速度计误差和 陀螺仪误差的基础上，惯性测量单元还包括由于三轴不完全垂直所引入的误差， 称为安装误差。 惯性测量单元中三轴加速度计和三轴陀螺仪的误差模型可分别表 示为： fffff NSbfI （1-3） NSbI （1-4） 式中 IIf,分别表示三轴加速计和陀螺仪的实际输出，, f分别表示三轴加速 度计和陀螺仪真实输入， bbf,分别表示三轴加速度计和陀螺仪偏差， SSf ,分别 表示三轴加速度计和陀螺仪标度因数， NNf,分别表示三轴加速度计和陀螺仪的 安装误差，而 , f 分别表示三轴加速度计和陀螺仪噪声。 4、惯性测量单元误差标定 本实验利用速率转台对惯性测量单元进行标定。 利用速率转台可以给惯性测 量单元提供一个与当地水平面（地理坐标系中北向和东向确定的平面）和稳定的 已知旋转速率。式(1-3)和（1-4）可表示为矩阵的形式: fz fy fx z y x zyfzxf yzfyxf xzfxyf z y x fz fy fx fz fy fx z y x z y x f f f f f f S S S b b b f f f f f f 0 0 0 00 00 00 , , , （1-5） z y x z y x zyzx yzyx xzxy z y x z y x z y x z y x z y x S S S b b b 0 0 0 00 00 00 , , , （1-6） 式中 ii f , 分别表示i轴加速度计和陀螺仪的实际输出， ii f,分别表示i轴的 加速度计和陀螺仪输入， ifi bb ,分别表示i轴加速度计和陀螺仪偏差， ifi SS ,分别 表示i轴加速度计和陀螺仪标度因数， ijfij ,分别表示ji,轴安装误差对i轴加速 度计和陀螺仪的影响， ifi ,分别表示i轴加速度计和陀螺仪的噪声误差。对于 不同的理论输入值，我们可以获得不同的观测量方程，从而对式（1-5）和（1-6） 中的误差参数进行计算，已实现对惯性测量单元的标定。 四、四、实验实验器材器材 单轴速率转台、MEMS 陀螺仪（或光纤陀螺仪） 、稳压电源、数据采集系统与 分析系统。 3 五、五、实验步骤实验步骤 标度因数和零偏测试实验 a. 将惯性测量单元安装在转台上 （使 Z 轴和转台旋转轴重合， 如下图所示） ， 接通电源，预热一定时间； b. 陀螺工作稳定后，测量静止情况下三轴加速度计和陀螺仪陀螺输出并保 存数据； c. 转台以 70deg/s 的速度正转，测试并记录三轴加速度计陀螺仪输出，保 存数据； d. 转台以 80deg/s 的速度正转，测试并记录三轴加速度计陀螺仪输出，保 存数据； e. 转台以 90deg/s 的速度正转，测试并记录三轴加速度计陀螺仪输出，保 存数据； f. 转台以 100deg/s 的速度正转，测试并记录三轴加速度计陀螺仪输出，保 存数据； f.利用最小二乘法计算惯性测量单元中可以被标定的误差参数； g.关掉惯性测量单元电源，重复步骤 a-f 5 次，可以获得另外惯性测量单元 的 5 组标定值； h.分析惯性测量单元的 6 组标定值。 图 1 惯性测量单元在单轴转台的安装框图 六六.实验注意事项实验注意事项 无。 七七.实验报告实验报告要求要求 1.根据标准实验报告要求完整说明实验原理、实验目的、实验内容、实验步 骤，实验器材等相关内容； 4 2.根据设计要求简要说明所设计的 Matlab 程序，并附程序框图； 3.分析程序调试过程，得到并说明程序运行结果，得到实验结论 4.根据得到的 6 组惯性测量单元标定值，回答下列问题： 1) 在本实验中，惯性测量单元中哪儿误差参数是可以标定的，哪些误差 参数是不可以标定的？ 2) 比较惯性测量单元的 6 组标定值，陀螺仪的偏差是否一致，如果不一 致，原因是什么? 3) 比较惯性测量单元的 6 组标定值，陀螺仪的标度因数是否一致，如果 不一致，原因是什么? 4) 比较惯性测量单元的 6 组标定值，陀螺仪的安装误差是否一致，如果 不一致，原因是什么? 5 实验二实验二 惯性导航静态实验惯性导航静态实验 一、实验目的一、实验目的 1、掌握捷联惯导系统基本工作原理； 2、掌握捷联惯导系统捷联解算方法； 3、了解捷联惯导系统误差传递规律和方程。 二、实验内容二、实验内容 1中低精度惯性导航系统导航仿真； 2对导航仿真结果进行分析。 三、实验原理三、实验原理 捷联惯性导航系统 （SINS） 的导航解算流程如图 1 所示。 在程序初始化部分， 主要是获得 SINS 的初始姿态阵 b n C、初始位置,速度以及初值四元数q；并读取 SINS 数据更新频率等 SINS 的工作参数。 图 1 惯性导航原理 这里，L、分别为当地纬度和经度、分别为载体航向、俯仰、横 滚角。地理坐标系为东-北-天坐标系。 1姿态计算姿态计算 姿态矩阵为： 6 111213 212223 313233 n b TTT CTTT TTT (1) 11 12 13 21 22 23 31 32 33 cos cossinsinsin cos sinsinsincos sincos cos sin coscos sin sincoscos sinsin sinsincos sincos cos cos T T T T T T T T T (2) 使用姿态四元数来更新姿态。四元数微分方程为： 00 11 22 33 0 0 1 02 0 bbb nbxnbynbz bbb nbxnbznby bbb nbynbznbx bbb nbznbynbx qq qq qq qq (3) 简写为： 1 2 qMq (4) 其中： bbbbbn nbibinibnin C cossintan T n NEE inieie ytxtxt VVV LLL RhRhRh 2 11 (1sin) xte eL RR 2 11 (1 23 sin) yte eeL RR =6378245m e R =1/298.3e 解四元数微分方程： 11234 1 ()( )() 6 kk q tq tkkkk (5) 7 式中： 1 1 2 2 3 43 ( ) ( ) 2 () ( ) 222 () ( ) 222 () ( ) 2 b nbkk b nbkk b nbkk b nbkk T ktq t kTT ktq t kTT ktq t T ktTq tk (6) 其中 T 为导航解算周期。 归一化四元数，有更新后的姿态矩阵： 2222 012312031302 2222 120301232301 2222 130223010123 2()2() 2()2() 2()2() n b qqqqq qq qq qq q Cq qq qqqqqq qq q q qq qq qq qqqqq (7) 提取姿态角： 23 1333 2122 arcsin() arctan(/) arctan(/) T TT TT (8) 2速度计算速度计算 由下列速度方程进行速度的更新 nnnnnnn xibxiezenzieyenyx nnnnnnn yibyiezenziexenxy nnnnnnn zibzieyenyiexenxz 02(2)0 (2)020 2(2)0 VaV VaV VaVg （9） 式中 13 23 33 e ie nnee ieeieie e ie C CC C ， tan N yt n E en xt E xt V Rh V Rh V L Rh (10) 在忽略高度通道误差的情况下，速度方程可简写为： n y n x n enz n iez n enz n iez n iby n ibx n y n x V V a a V V 0)2( 20 （11） 速度更新 8 TVtVtV n k n k n )()( 1 （12） 由式（11）求出加速度V，则()( )t TtTVVV。在实际程序中，为了进一 步提高解算的精度，也可以在姿态阵更新前后分别计算两次加速度V，用梯形法 求得速度的更新值，如下所示： TtVtVtVtV k n k n k n k n )()(5 . 0)()( 11 （13） 3位置计算位置计算 T LhR tVtV tt T hR tVtV tLtL N kEkE kk M kNkN kk cos)( )()( 5 . 0)()( )()( 5 . 0)()( 1 1 1 1 （14） 四、实验器材四、实验器材 捷联惯性导航实验系统一套、监控计算机一台。 五、实验步骤五、实验步骤 1、中低精度惯性导航系统导航仿真、中低精度惯性导航系统导航仿真 由实验老师给定一组中低精度 IMU 的静态 IMU 采样数据，包括初始姿 态，初始位置及速度； 利用捷联解算方法计算纯惯性导航误差。 2、对导航仿真结果进行分析、对导航仿真结果进行分析 六、实验注意事项六、实验注意事项 无。 七、实验报告要求七、实验报告要求 1.根据标准实验报告要求完整说明实验原理、实验目的、实验内容、实验步 骤，实验器材等相关内容； 2.根据设计要求简要说明所设计的 Matlab 程序，并附程序框图； 3.分析程序调试过程，得到并说明程序运行结果，得到实验结论 4.根据得到的静态惯性导航结果，回答下列问题： 1) 姿态误差是线性误差，还是振荡误差，振荡周期是多少？ 2) 根据导航结果，分析导致速度误差的误差源? 3) 位置误差是线性误差，还是振荡误差? 9 实验实验三三 导航导航解算解算（接收机位置、时间）及结果分析（接收机位置、时间）及结果分析 一一、实验目的实验目的 1、理解接收机位置导航解算原理及基本公式中各个参量的含义； 2、理解将本地接收机钟差作为一个参量进行导航解算的原因及目的； 3、理解接收机钟差的特性； 4、能够根据实验数据编写验证接收机钟差特性的相关程序。 二二、实验实验内容内容 1、理解最小二乘法的思想； 2、接收机定位解算：根据卫星坐标以及伪距观测值进行定位解算，获得接 收机位置坐标(直角坐标)。 三三、实验原理实验原理 1、基本概念基本概念 GPS 接收机位置的导航解算即解出本地接收机的纬度、经度、高度的三维位 置，这是 GPS 接收机的核心部分。 GPS 接收机位置求解的过程如下：导航电文与测距码（C/A 码）共同调制 L1 载频后，由卫星发出。卫星上的时钟控制着测距信号广播的定时。本地接收机也 包含有一个时钟，假定它与卫星上的时钟同步，接收机接收到一颗卫星发送的数 据后，将导航电文解码得到导航数据。定时信息就包含在导航数据中，它使接收 机能够计算出信号离开卫星的时刻。同时接收机记下接收到卫星信号的时刻，便 可以算出卫星至接收机的传播时间。 将其乘以光速便可求得卫星至接收机的距离 R，这样就把接收机定位于以卫星为球心的球面的某一个地方。如果同时用第二 颗卫星进行同样方法的测距， 又可将接收机定位于以第二颗卫星为球心的第二个 球面上。因此接收机就处在两个球的相交平面的圆周上。当然也可能在两球相切 的一点上，但这种情况只发生在接收机与两颗卫星处于一条直线时，并不典型。 于是， 我们需要同时对第三颗卫星进行测距，这样就可将接收机定位于第三个球 面上和上述圆周上。第三个球面和圆周交于两个点，通过辅助信息可以舍弃其中 一点，比如对于地球表面上的用户而言，较低的一点就是真实位置，这样就得到 了接收机的正确位置。 在上述求解过程中，我们假定本地接收机与卫星时钟同步，但在实际测量中 这种情况是不可能的。GPS 星座内每一颗卫星上的时钟都与一个叫做世界协调时 （UTC，即格林尼至时间）的内在系统时间标度同步。卫星钟差可根据导航电文 中给出的有关钟差参数加以修正，其基准频率的频率稳定度为 10-13 左右。而本 10 地接收机时钟的频率稳定度只有 10-5 左右，而且其钟差一般难以预料。由于卫 星时钟和接收机时钟的频率稳定度没有可比性，这样，就会在卫星至接收机的传 播时间上增加一个很大的时间误差，严重影响定位精度。为解决这一问题，我们 通常将接收机的钟差也作为一个未知参数，与本地接收机的 ECEF 坐标（ECEF 坐 标系的定义在前序实验中已经给出）一起求解。这样，由于有 4 个未知量，我们 就需要同时观测到 4 颗卫星，由 4 个方程将其解出。解出的接收机钟差可以用来 校正本地接收机的时钟，这使得 GPS 接收机同时具有授时功能。 卫星实时位置的解算需要已知这颗卫星的星历和 GPS 时间。 由于 GPS 卫星属 于中轨道卫星，卫星信号在传输过程中又会产生诸如对流层误差、电离层误差、 相对论效应误差等各种实时传输误差，因此，由上述方法得出的卫星至接收机的 传输时间并不准确， 而由其乘以光速得出的距离也不是卫星到达接收机的真实距 离（Range） ，只能叫做伪距（Pseudorange） 。其含义就是“假的距离” ，因为其 中包含有各种误差。直接由伪距求解出的接收机位置会出现很大的误差，因此在 求解前首先要把各种误差从伪距中消去。 2、伪距定位原理伪距定位原理 根据 GPS 伪距测量原理可知，GPS 伪距测量方程为： tctcttR s r s r s r （1） 其中： 上标 s 表示卫星(Satellite)； 下标 r 表示接收机(Receiver)； tRs r 表示接收机与卫星之间的伪距观测值； tc r 表示接收机钟差等效距离，待求解； t s 表示卫星钟差，可由导航电文中相关参数计算得出； 令 tZtYtX sss , 表示 t 时刻卫星坐标，而 rrr ZYX, 表示接收机坐标， 则 t s r 可表示为： 222 r s r s r ss r ZtZYtYXtXt （2） 此时，将式（2）代入式（1） ，可得： tctcZtZYtYXtXtR s rr s r s r ss r 222 （3） 在式（3）中，包含接收机三维坐标 rrr ZYX, 与接收机钟差 tc r 共 4 个未 知参数，因此至少需要 4 颗卫星才能进行解算。 在式（3）中，仅有 t s r 包含非线性项，令 rrrr s r s r ss r ZYXfZtZYtYXtXt, 222 （4） 11 假设接收机坐标 rrrr ZYXX, 近似值为 0000 , rrrr ZYXX ，则近似距离 可以根据式（5）计算得到： 000 2 0 2 0 2 00 , rrrr s r s r ss r ZYXfZtZYtYXtXt （5） 根据 GPS 接收机坐标近似值，可以将 rrr ZYX, 分解为： rrr rrr rrr ZZZ YYY XXX 0 0 0 （6） 在式（6）中， rrr ZYX,是新的未知参数，而 000 , rrr XYZ 是已知的。 这 样 分 解 的 优 点 是 函 数 rrr ZYXf,可 以 被 替 换 为 等 价 函 数 rrrrrr ZZYYXXf 000 ,，而后者可以在近似位置展开成泰勒级数，即： 0 0 0 000 000 , , ,| , | , | rr rr rr rrrrrrrrr rrr rrrXXr r rrr XXr r rrr XXr r fX Y ZfXX YY ZZ fX Y Z fXYZX X fX Y Z Y Y fX Y Z Z Z （7） 在展开式（7）中仅包含一阶线性项，二阶及以上高阶项很微小可以忽略不 计。 式（7）中，在近似点 0000 , rrrr ZYXX 处，各项偏导数可以根据式（8） 计算得出： t ZtZ Z ZYXf t YtY Y ZYXf t XtX X ZYXf s r r s XX r rrr s r r s XX r rrr s r r s XX r rrr rr rr rr 0 0 0 0 0 0 0 0 0 | , | , | , （8） 将式（5）与式（8）代入式（7） ，得到： r s r r s r s r r s r s r r s s r s r Z t ZtZ Y t YtY X t XtX tt 0 0 0 0 0 0 0 （9） 在式（9）中， t s r 与 rrr ZYXf, 等价。 将式（9）代入式（3） ，得到接收机伪距定位方程为： 12 000 0 000 sss rrrss rrrrr sss rrr s r XtXYtYZtZ RttXYZ ttt ctct （10） 将式(10)重新安排，令已知量在左边，未知量在右边，得到： 00 0 00 0 0 ss rrsss rrrr ss rr s r rr s r XtXYtY RttctXY tt ZtZ Zct t （11） 式中：卫星钟差 s t可由卫星导航电文中相关参数计算得到，是已知量。 因此，式（11）仅包含 r X ， r Y ， r Z 与 r ct共 4 个未知参数需要计算。 在式（11）中，引入新变量： 0 0 0 0 0 0 0 r r r ssss rr s sr X s r s sr Y s r s sr Z s r yRttct XtX h t YtY h t ZtZ h t （12） 以简化式（11） ，在式（12）中， s y以及 s Xr h ， s Yr h ， s Zr h 均与时间有关，但为 简化，均省去了时间标志。 当观测到 n 颗卫星时，则有方程组： tcZhYhXhy tcZhYhXhy tcZhYhXhy rr n Zr n Yr n X n rrZrYrX rrZrYrX rrr rrr rrr 2222 1111 （13） 式中，上标1,2,n表示卫星标号。引入向量 tc Z Y X x hhh hhh hhh H y y y y r r r r n Z n Y n X ZYX ZYX n rrr rrr rrr , 1 1 1 , 222 111 2 1 （14） 将式（14）代入式（13） ，则后者写成矩阵形式为： Hxy