一种基于UWB与惯导融合的车载导航定位方法技术

本发明专利技术提供了一种基于UWB（Ultra‑Wide‑Band）与惯导融合的车载导航定位方法，属于室内外定位技术领域。该方法针对当UWB信号遮挡严重、大量存在多径效应时，会出现粗大误差问题，及当接收信号的基站数目小于3个时，标签无法完成定位的问题。结合惯导不受外界环境影响，能实现自主连续定位特点。提出了当接收的基站个数大于1个时，使用扩展卡尔曼滤波实现车辆的最优估计，当接收基站个数为1个时，使用余弦定理的方法估算车辆位置。该方法能提供能够长时间连续可靠的定位，具有很强的工程应用价值。

A vehicle navigation and positioning method based on the fusion of UWB and ins

【技术实现步骤摘要】
一种基于UWB与惯导融合的车载导航定位方法
本专利技术属于室内外导航定位领域，特别涉及了一种基于UWB(Ultra-Wide-Band)与惯导融合的车载导航定位方法。
技术介绍
UWB是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据，与传统的窄带系统相比，具有穿透力强、功耗低、抗干扰强等优点，同时UWB凭借着精度高，适用性广等特点，在工厂、仓储、机器人等定位领域得到广泛应用。但当UWB信号遮挡严重、存在大量多径或该区域定位基站较少时，UWB将会出现粗大误差或不能定位的问题。惯性导航系统具有不受外界影响，自主性、定位连续的特点。本专利技术针对UWBTDOA（TimeDifferenceOfArrival）技术中标签只发送信号不接收信号特点，将融合算法分为两部分：标签侧负责解算车辆的姿态、行驶状态等信息，平台侧负责根据接收到的UWB数据及车辆的姿态、行驶状态等信息对车辆的位置、速度、姿态进行估计。
技术实现思路
本专利技术提出一种基于UWB与惯导融合的车载导航定位方法。该方法可以使得在UWB信号遮挡严重、或存在大量多径、或接收UWB信号的基站个数较少如小于3个时仍能持续高精度的完成定位。在定位过程中，当接收UWB信号的基站个数大于1个时，使用扩展卡尔曼滤波实现车辆的最优估计，当接收基站个数为1个时，使用余弦定理方法估算车辆位置。本专利技术提出的一种基于UWB与惯导融合的车载导航定位方法，包括以下步骤。步骤01：将含有惯性传感器的UWB标签，安装在车辆上，要求惯性传感器的Y轴或X轴与车辆的行驶方向一致。读取惯性传感器中的加速度计计陀螺仪数据。步骤02：根据读取的加速度数据，判断当前的车辆是否静止。若静止则执行步骤03，否则执行步骤05。步骤03：根据读取的加速度数据，计算俯仰角和横滚角。步骤04：对一段Tw时间内的三轴陀螺仪数据求平均，可以解算出陀螺仪的三轴零偏w_bias。步骤05：把初始航向角为0°及步骤03解算的俯仰角及横滚角当作四元数计算的初始值，然后使用计算出的陀螺仪零偏值实时校正输出的角速度数据，实时更新四元数，进而跟踪车辆的行驶姿态角。步骤06：根据一段Tw时间内的加速度数据及陀螺仪数据，判断车辆的行驶状态，即是向前行驶或倒退行驶。车辆静止启动时的行驶状态判断，求一段Tw时间内加速度计x轴的最大值max_acc_x，最小值min_acc_x；加速度计y轴的最大值max_acc_y，最小值min_acc_y，则（1）若f大于设定阈值static_thr，则车辆开始运动，此时若Tw时间内Y轴加速度均值大于0，则车辆向前行驶，否则车辆倒退行驶；若f小于设定阈值static_thr，则保持上一时刻的静止状态。当车辆非静止状态时，需要针对一段Tw时间内加速度计和陀螺仪数据联合进行判断叉车的行驶状态。若加速度计数据与陀螺仪数据在Tw时间段内的上升或下降趋势相同且最后一个数据的正负号相同，则车辆是倒退行驶；否则为向前行驶。步骤07：根据四元数解算的航向角及判断的车辆行驶状态，即可判断车辆的行驶方向。当车辆为倒退行驶时，则车辆的行驶方向为+180°，否则保持不变。步骤08：获取标签的ID（IdentityDocument）、发送数据的包序号、时间戳信息，然后与惯导解算模块计算的车辆行驶方向一起发送至平台（包括端平台或云平台）。步骤09：平台侧解析UWB基站发送过来的数据，然后分析当前接收到UWB信号的基站个数，当接收的到UWB信号的基站个数小于2个时，执行步骤14，否则直行步骤10。步骤10：当接收到UWB信号的基站为3个及3个以上时，则使用TDOA算法计算车辆的位置，否则UWB无法独立计算出车辆的位置。步骤11：求一段Tw时间窗内的行驶方向最大值max_angle与最小值min_angle，若(max_angle-min_angle)除以360°的余数小于阈值angle_thr，则车辆直线行驶，执行步骤12，否则曲线行驶，执行步骤13。步骤12：当车辆行驶的直线距离L大于设定的阈值dist_thr时，则根据Tw时间段内的UWB定位轨迹，利用最小二乘方法进行直线拟合，可求得直线与X轴之间的夹角为（[-90°，90°]），则根据公式（2）（2）（3）可求得求车辆运动方向与Y轴（导航系为右手直角坐标系）正向之间的夹角，否则不做处理。式中detax为在X轴上行驶趋势，大于0为沿着X轴的正方向，小于0为沿着X轴的负方向；detay为在Y轴上的行驶趋势，大于0为沿着Y轴的正方向，小于0为沿着Y轴的负方向。步骤13：以车辆的二维位置x，y及车辆的行驶速度v建立状态向量，对应的状态方程如下：（4）式中为n+1时刻的车辆的平面位置坐标，为n+1时刻车辆的速度，为车辆相邻两次方法的时间间隔，为车辆n时刻的航向角，W为状态方程的系统噪声矩阵。观测量为相对于时基基站的测距差，那么对应的观测方程为：（5）式中为第i个UWB基站的平面坐标，H为UWB基站距离地面的高度，为车辆位置距离i号基站的距离与到1号基站的距离的差值，V为UWB观测量的量测噪声。然后根据扩展卡尔曼滤波公式对车辆的位置、速度进行最优估计。步骤14：此时由于接收到UWB信号只有一个基站，即UWB独立无法完成定位。本专利技术提供一种基于余弦定理的方法对车辆的位置进行估计，使得该系统仍能保持高精度定位。该方法可根据定位标签发送相邻两次数据的时间间隔、基站接收该标签两次相邻数据的时间间隔、车辆前一时刻的位置，记为a点、接收标签信号的基站位置，记为b点、车辆行驶方向等数据信息，估计车辆的最新位置信息，记为c点（此方法也可以是在多个基站接收到标签信号时，估计车辆位置）。求车辆前一时刻的位置到接收到标签信号的基站之间的距离Lab；求直线ab与直线ac之间的夹角；求车辆最新位置到接收标签信号的基站之间的距离为Lbc；已知Lbc、Lab、角度，可根据三角余弦定理得到另外一条边Lac最优解，即车辆行驶的距离。根据车辆的行驶方向角，车辆的行驶距离Lac，根据（6）即可推算车辆当前时刻的位置。步骤15：输出步骤13或步骤14解算的车辆位置及行驶方向。附图说明图1一种基于UWB与惯导融合的车载导航定位方法流程图。图2余弦定理估计位置示意图。图3标签到基站的测距示意图。具体实施方式以下结合附图对本专利技术的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本专利技术，并不用于限制本专利技术。将标签安装在车辆顶部，将惯性传感器的Y轴或X轴与车辆行驶方向保持一致。加速度计及陀螺仪的采样频率为200Hz，使用到的阈值参数设定：缓存的加速度及陀螺仪数据的时间窗Tw=[0.5s,1s]，判断车辆是否静止状态的阈值static_thr=[10,20]；判断Tw时间窗本文档来自技高网...

【技术保护点】
1.一种基于UWB与惯导融合的车载导航定位方法，其特征在于，包括以下步骤：/n步骤1：根据安装在车辆上面定位标签里惯导数据，检测车辆的静止状态，并对陀螺仪传感器进行标定；/n步骤2：根据陀螺仪数据及加速度计数据，判断车辆的行驶状态，即向前或后退行驶；/n步骤3：根据计算的航向角信息，判断车辆是否是直线行驶；/n步骤4：当车辆直线行驶一段距离后，根据此时间段内的UWB定位结果拟合求车辆的行驶方向；/n步骤5：当接收的基站大于1个时，根据定位标签到基站的测距差及车辆的行驶方向，对车辆的位置、速度用扩展卡尔曼滤波进行最优估计；/n步骤6：当接收的基站个数为1个时，使用余弦定理方法估计车辆当前的位置；/n所述的车辆包括但不限于汽车、叉车等。/n

【技术特征摘要】
1.一种基于UWB与惯导融合的车载导航定位方法，其特征在于，包括以下步骤：
步骤1：根据安装在车辆上面定位标签里惯导数据，检测车辆的静止状态，并对陀螺仪传感器进行标定；
步骤2：根据陀螺仪数据及加速度计数据，判断车辆的行驶状态，即向前或后退行驶；
步骤3：根据计算的航向角信息，判断车辆是否是直线行驶；
步骤4：当车辆直线行驶一段距离后，根据此时间段内的UWB定位结果拟合求车辆的行驶方向；
步骤5：当接收的基站大于1个时，根据定位标签到基站的测距差及车辆的行驶方向，对车辆的位置、速度用扩展卡尔曼滤波进行最优估计；
步骤6：当接收的基站个数为1个时，使用余弦定理方法估计车辆当前的位置；
所述的车辆包括但不限于汽车、叉车等。


2.根据权利要求1所述的定位标签，其特征在于，包含惯导计算模块、UWB数据模块；
定位标签的安装，需要将惯性传感器的Y轴或X轴与车辆行驶方向保持一致；
所述的惯导计算模块，是根据加速度计和陀螺仪的数据计算车辆的行驶方向；
所述的UWB数据模块，包括标签的ID（IdentityDocument），发送信息的包序号，时间戳等数据。


3.根据权利要求1所述的检测车辆的静止状态，并对陀螺仪传感器进行标定，其特征在于：
求一段时间Tw内加速度计x轴的最大值max_acc_x，最小值min_acc_x；加速度计y轴的最大值max_acc_y，最小值min_acc_y；若max_acc_x-min_acc_x<阈值且max_acc_y-min_acc_y<阈值，则当前车辆为静止状态，否则为运动状态；
所述的对陀螺仪传感器进行标定，是指当检测到车辆在Tw时间内静止时，对Tw时间内的陀螺仪三轴数据求平均，其值为陀螺仪三轴的零偏W_bias；然后使用该零偏值对原始的陀螺仪数据进行校正。


4.根据权利要求1所述的判断车辆行驶状态，其特征在于，包含车辆静止启动时行驶状态判断、车辆行驶过程中的状态判断；
所述的车辆静止启动时的行驶状态判断，其特征在于，求一段时间Tw内加速度计x轴的最大值max_acc_x，最小值min_acc_x；加速度计y轴的最大值max_acc_y，最小值min_acc_y；则


（1）
若f大于设定阈值static_thr，则车辆开始运动，此时若Tw时间内Y轴（与车辆行驶方向保持一致的轴）加速度均值大于0，则车辆向前行驶，否则车辆倒退行驶；若f小于设定阈值static_thr，则保持上一时刻的静止状态；
所述的行驶过程中的行驶状态判断，针对一段时间Tw内加速度计和陀螺仪数据进行分析；若加速度计数据与陀螺仪数据在Tw时间段内的上升或下降趋势相同，则车辆是倒退行驶；否则为向前行驶；
所述的上升或下降趋势，是指对Tw时间内的数据进行直线拟合，若拟合的直线的斜率大于0，则为上升趋势，否则为下降趋势。


5.根据权利要求1所述的判断车辆是否直线行驶，其特征在于：
求一段时间窗内的角度最大值max_angle与最小值min_angle...

【专利技术属性】
技术研发人员：李磊，王凡，吴杰，
申请(专利权)人：华清科盛北京信息技术有限公司，
类型：发明
国别省市：北京;11