基于超宽带和二维码的无迹卡尔曼滤波融合定位方法技术

本发明专利技术公开了一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，包括基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程；通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程；基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值；基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新；基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统；使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声。从而实现提高自动导引车辆定位的精度、稳定性、实时性，最终达到复杂工业环境的需求。

【技术实现步骤摘要】
基于超宽带和二维码的无迹卡尔曼滤波融合定位方法
本专利技术涉及自动导引车(AGV)导航定位
，尤其涉及一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法。
技术介绍
随着人工智能+互联网模式的不断渗透，机器人逐渐代替人类繁重的工作已是大势所趋，AGV作为智能移动机器人重要的分支，使其更安全稳定具有相当大的工程意义与研究价值。围绕AGV的定位问题，国内外学者做了大量的研究工作，比如激光定位、视觉定位、惯性定位等，这些定位方式针对不同的工作场景以及成本费用，均可以取得不错的定位效果。目前对于单传感器的定位依然存在较多的问题：激光定位被广泛的应用于室内定位，然而要达到复杂的工业环境下的高精度定位，其需要昂贵的成本。二维码定位具有定位精度高、维护方便等优点，然而只有在扫描到二维码时才能进行坐标计算，无法适用于AGV全程的导航。对于UWB而言，其在复杂的工业环境下，定位信号被遮挡后会出现发射和折射的现象，从而导致非视距误差。因此，急需提供一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，在提高AGV在复杂工业环境的定位精度、稳定性、实时性的同时减少成本。
技术实现思路
本专利技术的目的在于提供一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，以提高自动导引车辆的定位精度，同时最大化的减少布局费用，最终使车辆能达到工作要求。为实现上述目的，本专利技术提供了一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，包括：基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程；<br>通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程；基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值；基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新；基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统；使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声。其中，基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程，具体包括：建立车辆的角速度状态更新方程、位置状态更新方程；根据所述角速度状态更新方程、所述位置状态更新方程基于不同的舵轮横摆角速度建立车辆位姿的状态更新方程。其中，基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程，其中，状态更新方程为：其中，X(Δt)为车辆经过Δt时刻的位姿偏移量，g为车辆位姿状态更新方程，w为车辆的横摆角速度，wd为舵轮的横摆角速度，θ为车辆的偏航角，v为车辆的线速度，为x坐标上的偏移量，为y坐标上的偏移量，为偏航角的偏移量，x(t)，y(t)为t时刻的车辆的位置X和Y坐标。其中，通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程，具体包括：由超宽带测得车辆的位置信息数据，由相机扫描二维码获得车辆的位姿信息数据，由陀螺仪测得车辆的偏航角信息数据，由编码器测得舵轮的角速度和线速度信息数据；根据所述车辆的位置信息数据、所述车辆的位姿信息数据、所述车辆的偏航角信息数据和所述舵轮的角速度和线速度信息数据建立车辆位姿的测量信息方程。其中，通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程，其中，测量信息方程为：其中，在t时刻，θq是相机扫描二维码计算得到的偏航角；Xq，Yq分别是相机扫描二位计算得到的绝对坐标；Xu，Yu分别是超宽带标签计算得到的坐标；θe是陀螺仪获取的偏航角。其中，基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值，其中，UT变换具体包括：其中，和分别为估计状态通过UT变换后更新sigma点和sigma权值，和Pz分别为测量信息通过UT变换后更新sigma点和sigma权值，χk-1和wk-1分别为由状态量计算得到的sigma点和sigma权值，Q和R分别为估计噪声和测量噪声。其中，基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新，计算公式可表示为如下：其中，Txz为状态矩阵互相关函数，Kk为卡尔曼增益，为更新后的状态，Pk为更新后的状态协方差，T为矩阵转置符号。其中，基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统，其中，超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值可表示如下：QU_accuracy＝αUWB+αQR；其中，QU_accuracy为基于定位特性反馈的值，αUWB为超宽带标签反馈的值，αUWB的取值为0～30，αQR为相机反馈的值，difference表示观测值和估计值的差值，zx,zy表示传感器测量的X、Y坐标，表示通过状态更新方程得到的X、Y坐标。其中，使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声的方式如下：Qk＝Qk-1×αQ,Rk＝Rk-1×αR；其中，αQ，αR分别为模糊推理系统输出的过程噪声和测量噪声系数；Qk-1，Rk-1为上一时刻的过程噪声和测量噪声；Qk，Rk为当前时刻的过程噪声和测量噪声，且Qk的取值范围为0.5～3，Rk的取值范围为0.5～15。本专利技术的一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，通过基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程；通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程；基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值；基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新；基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统；使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声。从而实现提高自动导引车辆定位的精度、稳定性、实时性，最终达到复杂工业环境的需求。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本专利技术提供一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法的流程示意图；图2是本专利技术提供的融合算法流程图；图3是本专利技术提供的实车系统结构图；图4是本专利技术提供的AGV运行学模型；图5是本专利技术提供的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法的效果图；图6是本专利技术提供的车辆位姿状态与记录的车辆位姿状态进行对比图；图7是本专利技术提供的模糊控制系统隶属函数。具体实施方式下面详细描述本专利技术的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或本文档来自技高网...

【技术保护点】
1.一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，包括：/n基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程；/n通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程；/n基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值；/n基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新；/n基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统；/n使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声。/n

【技术特征摘要】
1.一种基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，包括：
基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程；
通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程；
基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值；
基于状态矩阵互相关函数计算卡尔曼增益，并进行状态更新与协方差更新；
基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统；
使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声。


2.如权利要求1所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程，具体包括：
建立车辆的角速度状态更新方程、位置状态更新方程；
根据所述角速度状态更新方程、所述位置状态更新方程基于不同的舵轮横摆角速度建立车辆位姿的状态更新方程。


3.如权利要求2所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，基于前叉式自动导引车辆的运动学方程得到车辆位姿的状态更新方程，其中，状态更新方程为：






其中，X(Δt)为车辆经过Δt时刻的位姿偏移量，g为车辆位姿状态更新方程，w为车辆的横摆角速度，wd为舵轮的横摆角速度，θ为车辆的偏航角，v为车辆的线速度，为x坐标上的偏移量，为y坐标上的偏移量，为偏航角的偏移量，x(t)，y(t)为t时刻的车辆的位置X和Y坐标。


4.如权利要求1所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程，具体包括：
由超宽带测得车辆的位置信息数据，由相机扫描二维码获得车辆的位姿信息数据，由陀螺仪测得车辆的偏航角信息数据，由编码器测得舵轮的角速度和线速度信息数据；
根据所述车辆的位置信息数据、所述车辆的位姿信息数据、所述车辆的偏航角信息数据和所述舵轮的角速度和线速度信息数据建立车辆位姿的测量信息方程。


5.如权利要求4所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法，其特征在于，通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程，其中，测量信息方程为：



其中...

【专利技术属性】
技术研发人员：景晖，赵世宠，匡兵，刘溯奇，刘飞，刘夫云，
申请(专利权)人：桂林电子科技大学，
类型：发明
国别省市：广西;45