【基于惯性导航的无人车导航技术探究8000字（论文）】

基于惯性导航的无人车导航技术研究摘要随着科学技术的发展，无人系统已进入无人驾驶汽车、救护车、无人仓库管理系统等越来越多的领域。在无人系统中，保证精确定位和导航非常重要。在户外，全球导航卫星系统越来越先进，能够提供精确导航。房间里很难玩，因为很难得到卫星信号。随着室内布置和导航需求的增长，多种室内定位软件应运而生。然而，在实践中，单一定位技术不能满足精度要求。本论文针对无人机，研究了基于卡尔曼滤波器扩展的超宽带/惯性测量组合定位算法。为了解决测量物体运动噪声的协方差矩阵突变影响滤波器扩展卡尔曼算法位置估计的问题，本文提出了一种基于简化Sage-Husa对卡尔曼滤波器自适应的UWB/IMU组合确定算法。最后对算法的综合定位进行了仿真，并在所建立的试验现场对这里开发的UWB/IMU综合定位系统进行了试验。关键词：惯性导航；无人车导航；UWB；卡尔曼滤波；组合定位目录TOC\o"1-3"\h\u165741引言 引言1.1研究背景随着科技的进步，无人系统开始渗透到越来越多的社区，如无人汽车、高速无人系统、无人仓库管理系统等。在无人系统中，保证精确定位和导航非常重要。基于惯性导航的室内定位方法不需要在场景中放置任何其他设施。它是一种通过陀螺仪、加速计和其他惯性元件测量搬运工运动的自主导航设备。它具有完全自主、高度不确定性、高度免疫和信息连续性的特点。上述定位技术在实践中得到应用，但也有其自身的局限性。例如，一些无线定位技术需要在定位环境中预先安装定位装置，而惯性导航定位系统在惯性装置中要求精度很高，因此这些系统仅在某些环境中效果相对较好。总之，需要研究一种高精度、应用广泛的室内定位系统。1.2研究意义惯性导航可以在短时间内实现高精度自主导航。但是，随着时间的推移，惯性导航会积累导航误差，使导航结果不准确。提高导航精度的一种方法是将惯性导航系统与其他定位方法结合起来，以避免累积误差。超宽带接入具有高传输速率、低带宽和定位精度.本课题将利用超宽带定位和惯性导航（IMU）技术进行集成，利用其优势，弥补其不足，实现更精确的室内定位和导航。2惯性导航行为算法2.1坐标系及其转换2.1.1常用坐标系（1）地球坐标系在地球坐标系中，原点定义为地心，x轴、y轴和z轴均符合法律规律。x轴位于地球赤道平面和正午主平面的交点，z轴指向地球北极。地球坐标系如图2-1所示。图2-1地球坐标系（2）地理坐标系如图2-2所示，地理坐标系的起点是载体的位置，x轴为东-地理位置，y轴为地理位置的北方向，z轴为垂直垂直垂直水平位置，x轴、y轴、z轴为对应的规律（z=x*y），根据这一点，东北物体的常规坐标系（ENU）诞生了。图2-2地理坐标系（3）载体坐标系载体坐标系的起点定义为载体的重心，x轴指向载流子的右侧，y轴指向载体的前端，z轴在上面。三轴匹配规律，载体坐标系如图所示。图2-3载体坐标系（4）导航坐标系本篇文章中选取动捕系统坐标系作为导航坐标系。2.1.2坐标系转换导航系统需要坐标系的转换，可以通过移动和旋转来实现。由于水平位移，两个坐标系的初始点相交，然后通过有限的单轴坐标系相交，得到坐标系的变化。在短连接惯性系中，测得的加速度是承载坐标系中的一个值。在实际应用中，将其转换为导航坐标系是可行的。因此，由于所选择的导航坐标系为地理坐标系，且两个坐标系的初始点重合，本文旨在完成从载体坐标系向导航坐标系的过渡，因此，如果需要旋转坐标系，可以进行坐标系转换。坐标变换可以是四元、OWL角等形式.待定。欧拉角虽然有一般的阻塞问题，但这个问题在测试环境中并不存在，而且与四点比较，欧拉角更容易掌握形状，因此本文采用欧拉角来表示坐标系的变换。2.2捷联惯导系统速度和位置更新在载体坐标系下加速度计的量测值为ab如下式所示：(2-1)载体坐标系中的加速度值BA通过旋转矩阵转换为导航坐标系的加速度值an1中：(2-2)再将导航坐标系下的加速度值an1减去重力矢量，得到加速度an。(2-3)2.3定位结果评价方法2.3.1圆概率误差在以前的应用中，使用圆概率误差来估计炮弹或导弹的精度。在二维平面中，圆概率误差定义为以误差平均值为中心的圆半径，包括50%的定位计算，这是定位精度的简单表达式。如果x和y不同，且标准偏差不同，则圆概率误差可表示为：(2-4)或者圆误差概率可以表示为：(2-5)σX是水平误差的标准差，σY是纵向误差的标准偏差。该公式通常是军方使用的计算公式。如果两个标准差之间的差异相对较大，例如σ1/σ2∈[0.2,1]，则公式2的近似程度相对较高。2.3.2误差累积分布函数累积误差分布函数（CDF）是指误差值x小于或等于x的概率，即：(2-6)在实际应用中，计算定位误差需要进行多种实验，然后利用分布函数对系统的定位精度进行评价。2.3.3最大定位误差最大定位误差(MPE)是定位误差中的最大值，下式所示：(2-7)最大定位误差可以评价系统的鲁棒性。2.3.4均方根定位误差平均平方误差（RMSE）允许您评估系统的精度。均方误差是指目标真实位置与其位置估计之间的均方值。计算公式：(2-8)X和Y表示目标位置的真实值，X和Y表示定位系统的位置估计值。均方误差是对定位精度的简单而准确的描述.3组合定位算法3.1基于卡尔曼滤波的定位算法过滤是指从混合物中过滤不需要的信号以产生有用的信号。严格地说，卡尔曼滤波器应该被称为最优估计理论。这里提到的过滤器具有与传统过滤器完全不同的概念和效果。卡尔曼滤波器具有以下特点：（1）卡尔曼滤波器是在时区设计的，用于多维情况。（2）没有有用或无用的信号需要处理。（3）在计算中，系统噪声和过程噪声不是为了过滤对象而设计的，但它们的统计特性是估计卡尔曼滤波器所需的数据。卡尔曼滤波器由连续算法和离散算法两种算法组成.离散算法可以直接在数字计算机上实现.在实际应用中，离散卡尔曼滤波器更常用，因此我们在此建立离散系统模型。设系统的状态向量为Xk，在tk时刻受系统的噪声序列Wk−1驱动，建立离散系统模型为1-1所示(3-1)在这个模型中，第一个方程是系统的状态方程，第二个方程是系统的测量方程。Φ系统从Tk-1时间到Tk模式的转换矩阵；Γk-1是系统噪声驱动矩阵；Wk-1：Tk-1系统中的噪声矩阵；H为系统测量矩阵；Vk是tk中的噪声测量系统.在上述公式中，Wk−1和Vk也是相关的：(3-2)(3-3)其中Q为系统噪声的协方差矩阵，R为量测噪声的协方差矩阵。由于Wk-1和Vk相互独立，因此还有。对于上面的系统状态方程（1.1）来说，求取状态向量Xk的估计值可通过以下式子求得：状态一步预测：(3-4)协方差矩阵一步预测：(3-5)计算卡尔曼增益：(3-6)状态估计：(3-7)协方差矩阵预测：(3-8)对于上面的计算流程可以用下面的算法流程图表示：图3-1卡尔曼滤波算法流程图如图所示，计算过程分为滤波器计算部分和增益计算部分两部分，其中滤波器回路的一部分必须在增益测量单位上运行。在计算期间，卡尔曼滤波器会更新两个信息：更新时间和测量时间。在这五个方程中，状态评估的一个阶段是在前一个周期评估的基础上对前一个计算周期进行评估。卡尔曼的KK增益幅度决定了状态估计是否已接近先前状态估计或状态估计方程中的测量值，即状态估计是否已接近于测量值。从图表中可以看出，计算过程分为两部分：用于计算滤波器的滤波器回路部分和用于计算增益的回路部分，其中滤波器回路的这一部分必须在增益计算回路组件的基础上运行。在本报告所述期间，卡尔曼滤波器更新了两条信息。更新和测量时间。在上面的五个表达式中，一个阶段的状态估计是根据前一个计算周期的估计得出的，以获得当前计算周期的初步估计。另一方面，系统噪声的谐波Q矩阵和噪声测量R的协方差矩阵影响卡尔曼增益值KK，而卡尔曼增益值KK在状态估计方程中定义，以确定状态估计是接近预估计还是更接近测量，即估计值偏向于系统方程的估计还是测量结果。3.2基于扩展卡尔曼滤波的组合导航前一节讨论了卡尔曼滤波问题，提出系统方程的数学模型是线性的，但在技术应用的实际情况下，遇到的系统大多具有不同程度的非线性。例如，系统方程是非线性的或可测量的，或非线性的。在本课题下，基于排序的超宽带定位系统是一个非线性系统，因此有必要研究非线性卡尔曼滤波系统。在非线性卡尔曼滤波器中，扩展卡尔曼滤波器是一种简单有效的估计方法。卡尔曼滤波器的扩展基于线性卡尔曼滤波器，其基本思想是通过泰勒分解对非线性方程进行线性分布，并在估计点将非线性系统转换为近似线性系统。非线性系统可以用以下方程表示：(3-8)第一个方程是系统的状态方程，第二个方程是系统的测量方程。F（⋅）是系统状态方程的非线性函数；Γk-1是系统噪声驱动矩阵；Wk-1是系统在tk-1处的过程噪声矩阵；h（⋅）是测量方程中的非线性函数，Vk是测量噪声矩阵。假设过程噪声w和观测噪声V是均值为零、方差为Q和R的高斯白噪声，并且它们不相关。上述公式中系统状态方程的非线性函数f（⋅）通过泰勒一阶扩展到估计值，如以下公式所示：(3-9)(3-10)(3-11)则得到系统的状态方程如下式所示：(3-12)令(3-13)(3-14)可以得到系统的量测方程为：(3-15)根据线性卡尔曼滤波的基本原理推导出扩展卡尔曼滤波的具体算法如下：预测状态：(3-16)预测状态协方差状态：(3-17)计算卡尔曼增益：(3-18)更新状态：(3-19)更新协方差矩阵：(3-20)与上述计算过程中的线性卡尔曼滤波器不同，卡尔曼滤波器F中的扩展状态位移矩阵和观测矩阵H是f（⋅）和h（⋅）雅克比矩阵。设系统的状态变量为，雅克比矩阵的计算方式如下所示：(3-21)(3-22)3.3基于自适应卡尔曼滤波的组合定位卡尔曼滤波器作为最优估计理论得到了广泛的应用，但随着应用范围的扩大，许多场景都不能满足卡尔曼的基本假设。在某些情况下，通常很难建立准确的状态模型并获得相应的观测噪声特性，而且存在未知干扰，降低了估计精度，甚至导致滤波器散射。当系统环境发生变化时，传感器测量结果呈现异常特征，观察值发生变化，这也严重影响卡尔曼滤波器的估计。从上面的算法图可以看出Q矩阵和R矩阵对卡尔曼K-增益的大小有影响。如果Q矩阵小于实际噪声分布或R矩阵大于观测噪声分布，则K卡尔曼系数将太小，导致估计值太小；相反，如果Q矩阵大于实际噪声分布，或者R矩阵小于观测到的噪声分布，则K卡尔曼系数将过大，导致滤波器散射。因此，Q和R矩阵的选择非常重要.在传统的卡尔曼滤波器中，Q和R的值由经验值决定。如果计算过程中没有变化，就会出现以下问题。如果测量结果Z（k）有异常值且不改变噪声R的协方差矩，则测量结果影响估计，导致结果偏差甚至卡尔曼滤波器散射。如果系统的数学模型在过程中发生变化，或者如果系统噪声没有变化，那么Q矩阵噪声协调系统不能准确描述系统的状态，那么预测的一步也会影响估计结果。因此，对噪声协方差矩阵R和系统协方差矩阵Q进行自适应适应，可以减少异常对预期结果的影响。为了解决传统卡尔曼滤波器的缺点，有几种改进的滤波算法。常用的滤波算法包括衰减内存法、有限内存法、自适应卡尔曼法、防腐滤波器等.4定位导航试验4.1车载试验平台下图显示了车载测试平台的具体结构：图中红色方块为惯性传感器，绿色方块为UWB模块，黄色三角形为运动控制系统需要测试的对象。惯性传感器和超宽带模块的数据通过USB传输到上层计算机，运动控制系统的数据通过WiFi传输到上层计算机。图4-1车载平台（1）车载平台本次测试选择的车载测试平台是松陵生产的全方位UGVScout2.0。总体而言，Scout2.0采用模块化智能化设计理念，采用充气橡胶轮与模块动力相结合的独立悬挂方式，借助无刷直流伺服电机，采用差动转速。Scout2.0的最大载荷为50kg，以满足测试需求。图4-2SCOUT2.0试验车辆（2）惯性传感器模块本次测试中使用的惯性传感器模块为xsens的mti-g-710。Mti-g-710位置和位置传感器MTI-G-710是MTI-100系列基于卫星导航和惯性导航相结合的导航和稳定控制应用系统的解决方案，是一种集成传感器系统，它可以输出高频位置和位置数据。下表列出了具体参数。表4-1MTi-G-710具体参数（3）UWB模块本测试中使用的超宽带模块是来自Nooplop的linktrack-s模型超宽带模块。该超宽带模块具有以下优点。1）有三种模式：本地定位、分布式测距和数据传输；2）所有节点自动无线连接，无需拉线；3）定位、导航、定时和通信一体化（pntc）；4）它可以在没有服务器的情况下运行，并且该模块可以实时求解；5）相同的硬件可以设置为标签、基站和其他角色；6）高容量、高刷新率：40个标签/8个基站/1个控制台；7）距离和定位的刷新频率高达200Hz；8）1D和2D的典型定位精度为10cm，3D的典型定位准确度为30cm；9）独立的高速数据传输模式，低延迟，带宽3mbps；10）最远通信距离为80m；11）支持UART和USB通信；12）在3.5GHz到6.5GHz之间有6个射频频段；13）传输增益可调范围为0～33.5db。4.2实物试验4.2.1静态定位试验对于静态定位测试，测试站的布置如图所示。在四个基站的情况下，四个基站布置在矩形表面的四个角点上。在三个基站的情况下，三个基站布置在等腰三角形的三个角上。在测试中，测试车每短距离停车一次并收集数据。超宽带的数据更新频率为50Hz，运动检测系统的采样频率为100Hz。在每个数据采集点，采集时间超过10秒。通过求解来自UWB的距离信息，获得位置坐标，计算平均值作为估计值，并将移动采集系统收集的数据的平均值计算为真值。记录如下图所示。（a）四基站静态定位（b）三基站静态定位图4-3UWB静态定位效果图在上图中，黑色三角形是基站的坐标。在四个基站条件下，四个基站的坐标为BS1（-2.328，-1.894）、BS2（-2.337,2.358）、BS3（2.353,2.33七）、BS4（2.321，-1.885）。在三个基站条件下，三个基站的坐标为BS1（-2.328，-1.894）、BS2（-0.007,2.555）和BS3（2.321，-1.885）。红框是移动检测系统的定位结果，蓝星是根据UWB去除方案的定位结果。上图显示，如果标签离基站太近，超宽带解决方案的定位效果会恶化。对于四个基站，定位结果的最大误差（0.349，-0.448）和均方误差为RMSE（0.094.0.124）。在三个基站的情况下，定位结果的最大误差（0.305、0.405）和均方误差为均方根误差（RMSE）（0.109.0.1218）。就结果而言，在三个基站和四个基站的情况下，静态定位结果之间的差异很小，可以看出，如果标签和基站之间的距离太近，则标签位置分辨率结果的偏差很大。在以后的实验中，白天和基站之间的距离应尽可能远。4.2.2基于卡尔曼滤波的UWB定位系统在动态测试中，测试场景的布局与静态定位测试相对应。在四个基站的情况下，四个基站布置在矩形表面的四个角点上。在测试中，遥控车从图4-4的左下角开始（-1.896，-1.574），并围绕测试站顺时针移动。其中包括UWB50Hz、IMU100Hz和运动检测系统100Hz的数据更新频率。下图显示了汽车的轨迹。图4-4基于卡尔曼滤波的定位结果图4-5各方向定位结果在上图中，黑色曲线是用作真值的运动检测系统的数据。蓝色曲线是最小平方定位的结果，红色曲线是卡尔曼计算滤波的结果。图中显示卡尔曼滤波器对定位结果平滑效果良好。图4-6各方向定位误差从上图中的位置误差曲线可以明显看出，采样点800处的x轴误差中存在峰值毛刺，因为此时小车在基站附近运行，这与之前静态测试的结果一致。4.2.3组合定位在视距环境条件下，基于卡尔曼滤波器的宽带定位系统具有很好的定位效果，但当车辆处于那不勒斯视野时，会产生超宽带场值，导致定位精度下降。采用超宽带和惯性导航的组合导航可以有效地提高系统的定位精度和稳定性。在车辆运行期间的组合定位过程中增加了障碍物，以创造一个没有能见度的环境。测试过程是汽车从左下角开始，行驶轨迹从矩形开始。当机器移动到矩形的顶部时，在左下角的主站前面添加了一个障碍物，以创建一个不可见的环境。从下图的定位结果中还可以清楚地看到，当非物质环境出现时，UWB距离的结果表现为位置结果所反映的野值，即UWB距离的结果在非物质环境出现时表现为位置结果所反映的毛刺。图4-7组合导航定位结果图4-8组合导航定位结果细节图图4-9组合导航各定位效果上面的两个数字是定位在二维平面和x轴或x轴上的结果。在图中，黑色曲线是运动控制系统的轨迹，用于与定位效应进行比较。蓝色曲线是根据UWB距离设置二次解的最小值的结果；红色曲线是卡尔曼滤波器最小二乘定义的结果；绿色曲线是通过扩展卡尔曼滤波器对IMU和UWB数据进行定位的结果；浅蓝色曲线是利用自适应卡尔曼滤波器进行集成后IMU和UWB数据定位的结果。从上图可以看出，当存在没有视线的环境时，超宽带的测量结果存在异常值，导致最小二乘定位结果出现毛刺，如图4-9所示。为了减小最小平方的定位误差，使用卡尔曼滤波器对结果进行估计。从图中可以看出，在非视觉条件下，红色曲线的定位结果略高于最小正方形的定位结果，但仍存在毛刺。为了提高定位精度，使用先进的卡尔曼滤波器将IMU的转换加速度数据和可以从绿色曲线中看到的UWB的距离信息合并。在视线之外的环境中，从上述两个数字可以看出，组合定位使曲线平滑。但也可以看到，与非视觉线环境中的视觉环境相比，定位效果仍然相对较低。这是因为噪声测量矩阵R的值在非线性条件下没有发生相应的变化，而噪声测量矩阵在视觉条件下仍然使用。为了提高视觉条件下的定位精度，使用Sage-Husa自适应滤波器结合IMU数据进行加速度转换和UWB距离信息.根据蓝色曲线，超宽带测量结果在不可见条件下的异常值对卡尔曼滤波器的自调整结果影响不大。与高级卡尔曼滤波器相比，具有参数估计的卡尔曼滤波器能够更好地平滑效果。5结论本文首先分析了惯性导航系统的工作原理，介绍了一些常用的坐标系，分析了坐标系转换之间的关系，分析了惯性定位系统中速度和位置的变化。这里提出了一种局部化结果的估计方法，包括圆概率误差、最大定位误差和均方根误差。进一步，本文建立了基于卡尔曼滤波器的超宽带定位系统，与最小二乘法直接求解位置相比，定位精度明显提高；然后在扩展卡尔曼滤波器的基础上，构建了UWB/IMU组合定位导航系统，并进行了仿真.该组合导航定位系统基于Sage-Husa自适应卡尔曼滤波器.在对仿真结果进行分析后，提出了一种采用卡尔曼滤波器组合的改进简化导航定位系统。与基于卡尔曼萨格-胡斯自适应滤波器的综合定位导航系统相比，初始阶段的收敛速度明显提高。最后设计并构建了基于LinkTrack-S和MTI-G-710导航定位系统复合体的平台，采用基于距离信息的最小二乘法对定位结果进行了比较，该方法是基于卡尔曼滤波器的超宽带定位系统。基于卡尔曼滤波器扩展的UWB/IMU综合定位导航系统和基于简化Sage-Husa自适应卡尔曼滤波器方法的高级综合定位导航系统。基于卡尔曼滤波器扩展的组合定位系统和基于Sage-Husa简化变体的改进卡尔曼滤波器组合自适