[发明专利]一种磁导航AGV的高精度定位方法

权利要求书

1.一种磁导航AGV的高精度定位方法，其特征在于，包括以下步骤：

步骤一：建立磁导航小车的运动学模型：首先安装圆柱形小磁铁，然后在磁导航小车上安装霍尔效应传感器，传感器通过测量磁通量密度以及磁偶极电位，测量小磁铁与传感器之间的相对位置；

步骤二：建立隶属度函数和模糊控制规则：模糊控制的两个输入参数为d_e和θ_e，分别是小车当前位置与期望路径的横向偏差以及角度偏差；

步骤三：基于扩展卡尔曼滤波算法的误差纠正：扩展卡尔曼滤波有预测和更新两个过程，根据上一时刻的状态量x_k-1，控制输入量u_k-1，噪声干扰量w_k-1，来计算出下一时刻的状态量，此步骤包括以下四个过程：

过程一：预测磁导航小车下一时刻位置；

过程二：计算错误协方差矩阵；

过程三：观测和匹配过程；

过程四：评估过程。

2.根据权利要求1所述的一种磁导航AGV的高精度定位方法，其特征在于：所述步骤一中，磁导航小车质心的线速度以及角速度计算公式如下所示：

通过时间迭代的方法计算磁导航小车的位移公式：

Δx_c＝v_c·cosθ_c·T

Δy_c＝v_c·sinθ_c·T

Δθ_c＝ω_c·T

总计算公式如下：

3.根据权利要求1所述的一种磁导航AGV的高精度定位方法，其特征在于：所述步骤一中，测量小磁铁与传感器之间的相对位置所用公式如下：

其中，U,B_y,B_x,B_z，由霍尔效应传感器测得，然后计算出磁导航小车相对于磁铁的位置坐标：x,y,z。

4.根据权利要求1所述的一种磁导航AGV的高精度定位方法，其特征在于：所述步骤二的模糊控制规则如下所示：

(1)如果d_e和θ_e都为零，则导航速度设置为高速模式，即：

如果θ_e＝Z,d_e＝Z，则θ＝Z,v_s＝Z；

(2)如果θ_e为左小，d_e为左近，则θ为右小，v_s为右低速，即：

如果θ_e＝LS,d_e＝LN，则θ＝RS,v_s＝RS；

(3)如果θ_e为又大，d_e为右远，则θ为左大，v_s为左高速，即：

如果θ_e＝RL,d_e＝RF，则θ＝LB,v_s＝LF。

5.根据权利要求1所述的一种磁导航AGV的高精度定位方法，其特征在于：所述步骤三中，计算出下一时刻的状态量所用公式如下：

f(x_k)＝f(x_k-1,u_k-1,w_k-1)，

对于磁导航小车，控制输入量为v_c，ω_c，则：

通过小磁块计算小车的当前位置，其中η_θ为霍尔效应传感器的测量噪声，则：

z_k＝h(x_k,η_θ)。

6.根据权利要求1所述的一种磁导航AGV的高精度定位方法，其特征在于：所述步骤三中，过程一所用公式为：