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所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法,其特征在于,通过超宽带定位、二维码定位、陀螺仪以及编码器得到车辆位姿的测量信息方程,其中,测量信息方程为:其中,在t时刻,θq是相机扫描二维码计算得到的偏航角;Xq,Yq分别是相机扫描二位计算得到的绝对坐标;Xu,Yu分别是超宽带标签计算得到的坐标;θe是陀螺仪获取的偏航角。
6.如权利要求1所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法,其特征在于,基于所述状态更新方程和所述测量信息方程通过UT变换获得sigma点、sigma权值,其中,UT变换具体包括:其中, 和 分别为估计状态通过UT变换后更新sigma点和sigma权值, 和Pz分别为测量信息通过UT变换后更新sigma点和sigma权值,χk-1和wk-1分别为由状态量计算得到的sigma点和sigma权值,Q和R分别为估计噪声和测量噪声。
7.如权利要求6所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法,其特征在于,基于状态矩阵互相关函数计算卡尔曼增益,并进行状态更新与协方差更新,计算公式可表示为如下:其中,Txz为状态矩阵互相关函数,Kk为卡尔曼增益, 为更新后的状态,Pk为更新后的状态协方差,T为矩阵转置符号。
8.如权利要求1所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法,其特征在于,基于超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值设计模糊推理系统,其中,超宽带和二维码定位的特性与无迹卡尔曼滤波中的差值可表示如下:QU_accuracy=αUWB+αQR;
其中,QU_accuracy为基于定位特性反馈的值,αUWB为超宽带标签反馈的值,αUWB的取值为0~30,αQR为相机反馈的值,difference表示观测值和估计值的差值,zx,zy表示传感器测量的X、Y坐标, 表示通过状态更新方程得到的X、Y坐标。
9.如权利要求1所述的基于超宽带和二维码的无迹卡尔曼滤波融合定位方法,其特征在于,使用所述模糊推理系统自适应调节估计系统的系统噪声和测量噪声的方式如下:Qk=Qk-1×αQ,Rk=Rk-1×αR;
其中,αQ,αR分别为模糊推理系统输出的过程噪声和测量噪声系数;Qk-1,Rk-1为上一时刻的过程噪声和测量噪声;Qk,Rk为当前时刻的过程噪声和测量噪声,且Qk的取值范围为0.5~
3,Rk的取值范围为0.5~15。