[发明专利]一种基于磁钉的导引车导航方法及调度系统

权利要求书

1.一种基于磁钉的导引车导航方法，其特征在于：包括如下步骤：

A、将N个磁钉阵列铺设在导引车行驶的场地，以第一个磁钉为零点建立全局坐标系；

B、将磁传感器阵列安装在导引车上，该磁传感器阵列由至少三个磁传感器组成；将里程计设置在导引车中心，设置导引车的初始位置位于一磁钉正上方；

C、导引车行驶过程中，通过里程计测量的导引车的线速度和角速度，得到导引车当前时刻的第一位姿信息，该第一位姿信息包括导引车当前时刻在全局坐标系中的坐标和转向角；

D、磁传感器阵列检测磁场强度，当检测到的磁场强度大于预先设置的阈值时，进入步骤F；否则，进入步骤E；

E、根据步骤C所得的导引车当前的第一位姿信息进行导航；

F、建立磁偶极子模型，根据所检测到的磁场强度，利用毕奥-萨伐尔定律创建误差函数，并根据该误差函数得到距离引导车最近的磁钉的相对状态信息，并进入步骤G；

G、根据该相对状态信息、磁传感器阵列中心与导引车中心的距离以及步骤C所得到的导引车第一位姿信息，计算导引车当前时刻的第二位姿信息，该第二位姿信息包括导引车当前时刻在全局坐标系中的坐标和转向角，并进入步骤H；

H、采用最优的加权因子对步骤C所得的第一位姿信息和步骤G所得的第二位姿信息进行加权融合，根据融合后的最终位姿信息进行导航；

步骤H包括如下步骤：

H1、设里程计与磁传感器阵列的融合参数为：其中，d₁为步骤C所得的第一位姿信息，d₂为步骤G所得的第二位姿信息，设导引车实际的位置为d，则d₁、d₂为d的无偏估计，故融合参数的方差σ²为而根据多元函数求极值理论可确定在最小方差σ²下的最优加权因子w_i：为d₁的方差，为d₂的方差

H2、根据步骤H1中所得到的w_i得到当前时刻k的融合参数为：

2.根据权利要求1所述的一种基于磁钉的导引车导航方法，其特征在于：所述步骤C中，所述第一位姿信息包括导引车当前时刻在全局坐标系中的坐标(x_k,y_k)和转向角θ，其计算公式为：其中，v_k、w_k分别为里程计在第k时刻所测量的导引车的线速度和角速度，Δt表示采样时间间隔。

3.根据权利要求2所述的一种基于磁钉的导引车导航方法，其特征在于：所述步骤D中，预先检测铺设好后的各磁钉的磁场波动，所述阈值则设定为大于所有磁钉的磁场波动的值。

4.根据权利要求2或3所述的一种基于磁钉的导引车导航方法，其特征在于：所述步骤F包括如下步骤：

F1、所述误差函数为其中，B_mlx、B_mly和B_mlz分别为第l个磁传感器在三个正交方向上所测得的磁通密度，B_lx、B_ly和B_lz分别为第l个磁传感器在三个正交方向上的磁通密度理论值；

F2、通过LM算法，找出使误差函数取值最小的磁钉位置，作为磁传感器阵列中心与离导引车最近的磁钉之间的相对位置信息(x,y,h)，并根据该值得到磁传感器阵列中心与该磁钉之间的相对状态信息(m,n,p)，其中，h＝0,p＝0。

5.根据权利要求4所述的一种基于磁钉的导引车导航方法，其特征在于：所述步骤G中，导引车当前时刻的第二位姿信息包括导引车当前时刻在全局坐标系中的坐标(x_k1,y_k1)和转向角T为磁传感器阵列中心与导引车中心之间的距离。

6.根据权利要求5所述的一种基于磁钉的导引车导航方法，其特征在于：所述步骤H1中，d₁分别为第一位姿信息中的坐标(x_k,y_k)和转向角θ，d₂分别为第二位姿信息中的坐标(x_k1,y_k1)和转向角