综合卡尔曼滤波的多传感器协同室外定位研究.pdf

第 卷 第 期 年 月南京工程学院学报(自然科学版)().:./.投稿网址:/.综合卡尔曼滤波的多传感器协同室外定位研究汪 涛黄家才汤文俊高芳征(南京工程学院工业中心、创新创业学院 江苏 南京)摘 要:为了拓展移动机器人应用场景、满足室外定位的高精度需求提出一种基于综合卡尔曼滤波的协同室外定位算法可解决室外复杂环境下独立传感器失灵、机器人实时定位漂移的问题.首先构建 和超宽频非线性定位系统模型测试不同滤波算法对该模型的预测效果分析对比解算速度和均方根误差从而确定适合定位系统的最优算法然后针对 信号受环境遮挡导致丢失或失准的情况构建超宽频和惯性测量单元非线性定位补偿系统利用基于误差的卡尔曼滤波算法预测机器人位置姿态通过融合两种非线性系统下估计得到的不同状态向量确定机器人室外真实位置姿态进一步提高机器人室外定位精度保证定位系统的稳定性.试验验证表明本文算法室外定位误差小于 在 信号微弱的环境下能实时估计目标位置姿态大幅度降低障碍物干扰的影响准确预测机器人位置.关键词:协同定位多传感器融合综合卡尔曼滤波状态估计中图分类号:收稿日期:修回日期:基金项目:国家自然科学基金面上项目()江苏省重点研发计划课题()江苏省自然科学基金面上项目()作者简介:汪涛硕士研究生研究方向为机器视觉及自动化.:.引文格式:汪涛黄家才汤文俊等.综合卡尔曼滤波的多传感器协同室外定位研究.南京工程学院学报(自然科学版)():./.南京工程学院学报(自然科学版)年 月 .随着机器人技术的飞速发展机器人的应用场景逐渐由室内向室外拓展其自主性、智能性不断提高.精准的室外定位研究是机器人室外安全作业的重要保障.现在主流的室外定位传感器包括全球定位系统()、相机、激光雷达等.国内外研究人员通过借助传感器的环境感知能力在提升室外定位精度方面取得了不少成果.文献基于多光源检测以及高帧频的 遥感红外相机介绍一种信号调制技术使其在极端晴朗的条件下能够准确探测到红外信标而实现定位文献针对单目直接法难以构建低纹理区地图问题将惯导数据融入到图像跟踪过程搭建视觉惯性跟踪模型提出双投影匹配算法实现稠密点云地图的构建从而进行室外定位.为了提高定位精度上述方法在室外定位过程中系统需处理大量数据以获得完整的地图信息对硬件性能的要求较高.不依赖于复杂的算法可以直接输出目标姿态信息对控制器硬件性能的要求较低被广泛应用于自动驾驶技术.民用 误差较大(高达米级)卫星信号受限于搜星数的条件受外界影响严重致使单纯的 数据作为室外定位主要手段不能满足实际应用需求.为了解决上述问题人们开始探索使用其他传感器来弥补 的缺陷.文献针对 信号易受干扰的情况融合惯性导航系统()数据构建紧耦合定位系统降低环境因素的影响文献在可测量影像的基础上融合 和惯性测量单元()数据解决了车辆导航系统卫星信号失锁的问题.但上述算法中所使用的辅助传感器本身具有一定的局限性 本身极易受到地球磁场的影响导致数据失真并且只能提供角度数据的配准 误差会随时间增加逐渐增大最终导致数据漂移无法对 数据进行长时间的稳定校正.不同 于 传 统 连 续 波 的 通 信 方 式 超 宽 频()无载波通信技术使用周期为纳秒级的脉冲传输数据.由于其穿透能力和抗干扰能力强 无载波通信技术逐渐被应用于定位领域一般定位精度在 以内.作为测距手段时非视距信号()是影响信号测量准确性的主要原因之一.为了弥补该误差文献提出一种室内定位滤波器借助 提供的先验信息抑制 的观测误差从而提高定位精度.受限于 的使用范围机器人室外作业活动范围具有局限性.面对复杂的室外情况单个传感器的适用场景受到了一定限制从而影响室外定位系统的定位精度和定位范围.为此本文提出一种基于综合卡尔曼滤波()的多传感器室外协同定位算法.首先构建基于 与 的非线性数据融合系统然后分析各种滤波器的性能并选出最优滤波算法考虑到 大范围信号丢失的情况构建基于 数据与 数据的非线性系统利用状态误差卡尔曼滤波()算法解算目标的位姿状态最后融合两种非线性状态观测系统的状态估计数据可大幅度提高室外复杂环境下的多传感器协同定位精度.技术框架本系统室外定位流程框架由数据解析层、滤波算法层和姿态估计层三部分组成.对、以及 三种传感器的解析为后续融合层提供数据第 卷第 期汪 涛等:综合卡尔曼滤波的多传感器协同室外定位研究基础.利用解析得到的数据搭建非线性系统方程采用合适的滤波算法提高位姿解算的精确度.如图 所示数据解析层解析 输出的()报文获取目标的速度以及方位角信息 模块采用 的测距方法构建方程组通过 算法得到目标的位置信息.滤波算法层分析不同滤波算法框架下模型的解算速度和均方根误差()选取实时性和稳定性最优的算法.姿态估计层判断 信号受到的扰动大小选择合适的传感器定位组合当 数据出现连续失真(即当 和 非线性系统产生的卡尔曼增益超出限制)时及时切换组合算法实现室外协同定位.本系统硬件连接方式如图 所示.图 本文算法流程框架图图 本文系统硬件连接方式本文设计的室外定位系统除了移动机器人以及嵌入式处理器以外还需要连接、以及 传感器模块.模块通过多路卫星通道解算出目标的位姿数据 模块依靠多个基站的数据解算出目标的定位信息 作为刚体嵌入目标可直接输出目标的运动数据.综合卡尔曼滤波算法由于室外单一传感器会受到复杂环境的各种影响本文设计滤波算法层在获取到各个传感器的数据后构建 和 的非线性数据融合系统弥补 定位误差大、定位范围局限的缺陷.本 文 基 于 传 统 扩 展 卡 尔 曼 滤 波()算法分析判别各种滤波器的优劣(滤波时间以及效果)选取一种相对符合实际应用场景的最优滤波器参与系统运算.为了解决在恶劣环境下 信号丢失问题本文设计一种基于 和 数据的非线性数据融合系统利用 算法求解滤波结果.在实际场景下通过对 数据质量的判断选择性地添加该系统的结果作为实际定位数据.扩展卡尔曼滤波算法卡尔曼滤波算法作为状态最优估计算法被广泛应用于传感器、机器人和自动驾驶技术该算法通过预测和更新两个步骤实现对状态信息的准确估计.假设以正东和正北方向为坐标轴正方向建立空间坐标系系统所受的噪声均为符合高斯分布的白噪声则令状态向量为 其中:、分别为目标在空间坐标系下的坐标、分别为目标在对应方向下的分速度.目标状态方程为:()()式中为 时刻下的状态 为状态转移矩阵 为 时刻下的状态 为噪声驱动矩阵 为观测噪声.假设其协方差矩阵 则令:.()南京工程学院学报(自然科学版)年 月式中 为采样周期为方便计算假设 .由位置与速度的关系可得:()构建状态方程后以解析 数据得到的速度 和方位角 信息及解算 数据得到的目标坐标数据构建观测方程假设观测向量为:()()式中:为观测向量 为状态向量 与 观 测 向 量 之 间 的 函 数 关 系()其中、分别为目标当前时刻在对应方向上的速度分量为观测噪声假设其满足高斯分布特征其协方差矩阵 .对构建的非线性系统方程采用 处理提高定位精度.滤波过程为:)预测过程构建状态和概率密度预测方程:()式中:为 时刻的先验状态估计 为 时刻的后验状态估计为 时刻先验误差的协方差矩阵(该矩阵表明预测质量的优劣)为 时刻后验误差的协方差矩阵.通过状态转移矩阵 和噪声驱动矩阵 对状态向量和误差协方差矩阵进行先验估计.)更新过程根据预测阶段得到的误差协方差矩阵计算卡尔曼增益:/()()式中 为观测噪声误差.通过计算式()可以得到预测误差和观测误差的比重大小后验估计方程为:()()式中为后验状态估计即滤波后的状态数据.误差协方差矩阵更新为:()().集合卡尔曼滤波()算法传统扩展卡尔曼滤波算法是利用 公式将非线性方程转化为一阶线性方程在转换过程中会不可避免地产生不同程度的误差同时新增的雅可比矩阵会给算法增加计算难度不利于实时性和准确性指标.采用改进的扩展卡尔曼滤波器对系统状态进行重新估计将显著降低计算的复杂度、提高数据融合的精确度.本文分析不同卡尔曼滤波器对同一个非线性系统的滤波效果最终选取集合卡尔曼滤波算法作为改进的滤波算法.不同于传统卡尔曼滤波器算法用样本协方差代替原本的协方差矩阵即采用多个小量级的矩阵运算模拟传统的协方差矩阵.现实生活中 的维度要远远大于样本协方差的数量 算法只需要对样本协方差进行运算不需要显式地计算 的值.以 和 数据作为观测量构建非线性系统方程:()()()式中:为状态转移函数 为状态向量到观测向量的函数关系、为高斯白噪声.算法分为预测和更新两个阶段进行数据融合计算.在预测阶段不需要求出协方差矩阵只需要计算先验状态估计向量以及由多个样本协方差模拟的传统协方差矩阵:()()()()()选取多个样本协方差计算先验状态估计向量并取样本平均值求解 计算卡尔曼增益:/()()数据融合后的状态向量为:()()第 卷第 期汪 涛等:综合卡尔曼滤波的多传感器协同室外定位研究.状态误差卡尔曼滤波算法 算法在旋转问题的处理上可以将状态向量使用最小化的参数表示甚至可以用单位矩阵表示在大多数的 系统中往往采用 算法代替其他滤波算法.和传统 本质的区别在于 将系统真实状态分为标准状态和误差状态其解决的是误差状态的估计问题.实际使用只在于状态向量的区别.算法在进行预测之前由于 输出目标姿态(解决的状态必须是向量)需要对系统模型变量进行相应的转换.根据 中值积分模型可得系统真实状态 的一阶导数为:()()式中:为 中值积分函数关系为 的测量 值 为 各 种 噪 声 .的状态量从 和 组合系统中分别选取位置量、速度量、偏转量、加速度偏置、角速度偏置 以及重力加速度.由物理关系建立误差状态运动学方程为:()()()式中:为的采样时间、为输入高斯白噪声.由此可以构建离散时间下的系统递推方程为:()()()将式()的误差状态方程线性化后建立观测矩阵:()()()式中.至此将 与 数据融合系统中位姿信息转换为状态向量建立状态系统方程后续对系统的更新和预测过程与 算法步骤相同最终可以获取高精度的状态向量.试验分析.和 滤波算法为了提高多传感器协同室外定位系统的精度本文运用 语言设计几个改进卡尔曼滤波器对相同的非线性系统进行状态估计.比较数据结果选取最合适的滤波算法.各算法滤波效果如图 所示.试验设定真实轨迹为一个半径为 的圆周运动采样周期为.分别运用、算法进行滤波处理.从图 中可以看出:四种算法在高斯噪声的干扰下均能有效跟踪实际轨迹相比于其他三种算法 算法的预测轨迹最平滑更加符合实际应用场景.图 为采样时间的均方根误差折线图在相同条件下各种滤波算法在白噪声的影响下 算法的均方根误差折线图保持在.左右 算法的均方根误差曲线浮动较大平均误差达到.试验设计了不同里程下各算法处理时间的对比结果如表 所示.由表 可见实际轨迹经过三圈数据处理所用时间中 算法的最短(不到 )和 算法的较长 算法的最长(达到 ).试验表明 算法在均方根误差指标和实时性上均有较好表现.因此在 与 融合室外定位的问题上选用 算法.和 滤波算法由于 数据为目标姿态信息室外定位系统采用 算法对 数据和 数据进行融合处理.图 为实际场景下采集的 数据和 数据融合轨迹图.南京工程学院学报(自然科学版)年 月()算法()算法()算法()算法图 各算法滤波效果对比图 均方根误差折线图表 各算法处理时间结果里程 算法 算法 算法 算法一圈.两圈.三圈.试验小车按照半径为 的圆形轨迹行进采样时间为.在复杂环境的影响下融合轨迹接近实际行进轨迹.在未融合 数据集的情况下尽管 数据的稠密点分布在实际轨迹上但仍存在较大的噪声影响定位精度采用 算法降低受图 融合轨迹示意图影响较大的观测值的权重使预测轨迹符合实际运行轨迹.图 中 的长时间使用会造成数据漂移现象融合轨迹会逐渐漂移.多传感器室外协同定位考虑到实际环境中综合场景的活动范围及作业安全性定位系统需要兼具覆盖范围广以及定位精度高的特点.因此本文的姿态估计层采用多传感器协同定位的策略构建室外定位系统以减少由于树木、高楼及各种障碍物的遮挡使定位传感器的定位精度受到影响.为防止 模块受搜星数量第 卷第 期汪 涛等:综合卡尔曼滤波的多传感器协同室外定位研究和质量影响扰乱定位系统的稳定性采用 算法计算得到的卡尔曼增益(卡尔曼增益描述数据融合后的不确定性变化程度当不确定性升高时表明该融合数据不利于系统预测准确性)作为 数据判断的标准当增益小于一定阈值时认定 数据失真此时选择 的输出代替 数据参与轨迹预测.试验借助高德地图的信息显示 的经纬度数据以及定位系统融合状态信息后预测的轨迹.图 中点表示 的数据其它传感器之后的位置点虚线表示 与 数据融合之后的轨迹估计实线是融合 数据之后的轨迹估计.在室外导航路径的后半段由于障碍物的遮挡导致 数据逐渐发散最终使估计精度大幅下降在加入 数据后估计位置点逐渐聚集解决了在障碍物遮挡环境下精度有所欠缺、数据失真严重的情况.图 室外定位轨迹 结语在复杂环境中精确定位是移动机器人在室外完成各种任务的基础.本文针对单一传感器无法长时间稳定提供精准定位数据的问题提出了一种基于 的多传感器室外协同定位算法.该算法融合了、以及 三种传感器以提高室外定位准确度.首先采用 与 数据作为观测量在 信号良好的情况下运用 算法解算出准确的定位信息当室外有大范围遮挡或信号干扰严重、置信度指标小于阈值时由 与 数据融合获得预测位置.试验仿真表明该算法在复杂干扰的情况下可以实现比较精确的定位在实际应用中该算法在复杂道路环境中的定位坐标与真实坐标拟合度高可以达到厘米级精度.与传统的室外定位算法作对比有以下结论:)基于多传感器协同的室外定位系统在满足实时性和稳定性要求的同时不占用过多内存资源针对复杂场景变化所导致的误差问题采用两套融合算法自适应切换有效降低单一传感器数据失真对系统稳定性造成的不利影响同时减小了复杂环境变化所带来的不可估计的误差)采用 作为 与 数据融合的算法在不牺牲实时性的基础上精度大幅提升.)当 数据失真时以 数据替代 数据运用 算法对目标状态进行重新估计纠正 错误数据带来的影响使定位系统维持高精度定位工作状态.参考文献:.():.():.():.于建均王洋左国玉等.融合惯性信息的单目直接法定位与稠密地图构建.控制工程():.武曌晗荣学文范永.导盲机器人研究现状综述.计算机工程与应用():.杨奇峰曲道奎徐方.基于 的移动机器人定位算法研究.控制工程():./.:.高扬高逦乌萌等./组合导航方法研究.西北工业大学学报():./.():.南京工程学院学报(自然科学版)年 月().():.():.():./.():./():.应保胜周晓帅方海龙等.基于 的 和 融合定位算法.计算机系统应用():.:.():.华智宋吉来杜振军等.基于滤波与图优化的定位与建图系统.电子测量技术():.:.