CN105180932A - 一种适合仓储agv的惯性导航方法 - Google Patents

Abstract

本发明公开了一种适合仓储AGV的惯性导航方法，它包括如下子步骤：S1：导航系统搭建，S2：读取磁钉数据，S3：获取小车偏移数据，S4：规划路径，S5：获取陀螺仪角度，S6：角度校准，S7：读取RFID，S8：确定运行模式，S9：运动控制，S10：重复步骤S2-S9。该方法采用磁钉代替磁条和二维码，有效地节约成本的同时能解决二维码受多尘环境影响较严重的缺陷，实现无轨导航方式，该系统测量位置准确，位置精度可达±5mm，测量角度准确，角度精度为±0.1度，其导航精度高，导航精度为±10mm，导航速度可达1m/s，有效的节约资源。

Description

一种适合仓储AGV的惯性导航方法

技术领域

本发明属于自动控制领域，特别是一种适合仓储AGV的惯性导航方法。

背景技术

目前随着工业4.0和中国制造2015，智能物流和智能制造已经成为制造业和工厂发展趋势必然。而AGV(自动引导小车)在物流系统中的自动搬运作用，在生产效率和管理具有非常重要的作用，特别是在随着工厂产品种类和数量增加，自动仓储的作用将越来越明显。而所谓自动引导,即是沿着指定轨迹运行,目前常见的导航方式有光电传感器、视觉导航、激光导航和磁导航传感器等等。一般仓库中，有放置整齐的货架，每个单元货架有上下几格，AGV需要把相应货物放在对应的单元处，因此由于不同存放点，使得AGV路径将会多样化和复杂化。目前AGV行业内常采用磁条导航，但是这样磁条铺设将会显得密密麻麻，不仅不美观，由于磁条复杂交错将会影响到AGV正常运行，甚至由于磁条有一定高度，会导致AGV运行振动甚至走偏；目前美国亚马逊采用KIVASYSTEM产品，其导航方式基于二维码，尽管二维码铺设简单，但是仓库内(特别国内库房)难免没有灰尘堆积、人迹、清洁水渍等等，这些因素不仅使得视觉识别二维码出错，甚至还可能导致二维码标签脱掉。磁条导航和二维码视觉导航方式皆不能满足库房自动仓储要求，因此市场上急需一种满足自动仓储的AGV导航系统。

发明内容

本发明的目的在于克服现有技术的不足，提供一种适合仓储AGV的惯性导航方法，该方法采用磁钉代替磁条和二维码，有效地节约成本的同时能解决二维码受多尘环境影响较严重的缺陷，实现无轨导航方式，该系统测量位置准确，位置精度可达±5mm，测量角度准确，角度精度为±0.1度，其导航精度高，导航精度为±10mm，导航速度可达1m/s，有效的节约资源。

本发明的目的是通过以下技术方案来实现的：一种适合仓储AGV的惯性导航方法，它包括如下子步骤：

S1：导航系统搭建，将陀螺仪设置在AGV小车上，磁传感器设置在小车车头底部中线上，磁钉和RFID卡设置在AGV航道上，编码器、控制单元和RID读卡器设置在控制盒内；

S2：读取磁钉数据，磁钉读取模块通过控制单元的串口读取磁传感器采集的磁钉数据；

S3：获取小车偏移数据，偏移数据获取模块根据磁传感器获得的磁钉状态获得小车偏移运行轨迹的角度和距离；

S4：规划路径，根据小车偏移运行轨迹的角度和距离规划出AGV转弯半径，同时控制左右差速实现以该半径转弯；

S5：获取陀螺仪角度，将陀螺仪的原始数据进行处理后，并积分得到AGV该时刻的角度；

S6：角度校准，通过磁钉校准的角度对陀螺仪获取的角度进行校准；

S7：读取RFID，在AGV小车运行到路径的交叉路口磁钉处时读取RFID地标的ID号；

S8：确定运行模式，将RFID的ID和任务路径中ID进行匹配得出相应动作确定AGV运行模式；

S9：运动控制，控制AGV执行直行、左转90度和右转90度的运动，如果AGV执行旋转动作，则差速电机以速度大小相等、方向相反原地运行，通过陀螺仪测量的角度判断AGV是都旋转到90度；

S10：重复步骤S2-S9。

该方法还包括一个脱轨检测步骤，通过电机编码器读取相邻磁钉AGV所运行里程判断，如果2m仍没有磁钉则AGV脱轨，停止AGV运行。

所述的步骤S2中读取磁钉数据通过控制单元的RS232串口读取磁导航数据，路过第2k个磁钉，记录其状态；路过2k+1记录状态，第2k和2k+1磁钉为一组磁钉。

所述的步骤S3获取小车偏移数据根据一组磁钉的状态和磁传感器的状态计算，磁传感器的状态：

则AGV偏心距离为：

$\mathrm{\Δ}d=\frac{\underset{i=1}{\overset{i=12}{\Σ}}{x}_i(k+1)-\underset{i=1}{\overset{i=12}{\Σ}}{x}_i\left(k\right)}{2}m---\left(2\right)$

则AGV偏移角度为：

$\mathrm{\θ}=atan\left(\frac{\mathrm{\Δ}d}{l}\right)---\left(3\right)$

其中l为磁钉间距，m为磁传感器监测点间距，x_i为磁传感器状态。

所述的步骤S4路径规划为AGV仓储惯性导航的关键单元，AGV运动半径r，AGV航向角θ，磁钉间距l，通过三角关系有：

$s=\sqrt{d^2+l^2}---\left(4\right)$

通过旋切角关系可以得出：

$r=\frac{s/2}{\cos \mathrm{\θ}}---\left(5\right)$

综合式(3)、式(4)、式(5)，可以得出AGV选择半径：

$s=\frac{\sqrt{d^2+l^2}}{2cos\left(atan\frac{\mathrm{\Δ}d}{l}\right)}---\left(6\right)$

然后，控制单元根据运行半径控制AGV左右差速。

所述的步骤S5中获取陀螺仪角度先去除固定漂移，再进行卡尔曼滤波，并进行积分后得到ω(k)，则在第k时刻，AGV的角度为

所述的步骤S6角度校准计算方式如下：AGV在2k时刻的角度为θ(2k)，在2k+1时刻角度为θ(2k+1)，陀螺仪所测量的角度为ρ(2k+1)，而 $\mathrm{\Δ}\mathrm{\θ}(2k+1)=\mathrm{\θ}(2k+1)-\mathrm{\θ}\left(2k\right)=2atan\left(\frac{\mathrm{\Δ}d}{l}\right),$

如果|ρ(2k+1)-Δθ(2k+1)|＜6°，则将陀螺仪的角度校准为

本发明的有益效果是：本发明提供了一种适合仓储AGV的惯性导航方法，该方法采用磁钉代替磁条和二维码，有效地节约成本的同时能解决二维码受多尘环境影响较严重的缺陷，实现无轨导航方式，该系统测量位置准确，位置精度可达±5mm，测量角度准确，角度精度为±0.1度，其导航精度高，导航精度为±10mm，导航速度可达1m/s，有效的节约资源。

附图说明

图1为AGV运行路径示意图；

图2为导航方法流程图；

图3为磁钉测量示意图；

图4为路径规划示意图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

如图1所示，仓储货架一般呈正方形规则排布，所以路线又直线段组成，直线段交叉成90度。

如图2所示，一种适合仓储AGV的惯性导航方法，它包括如下子步骤：

S1：导航系统搭建，将陀螺仪设置在AGV小车上，磁传感器设置在小车车头底部中线上，磁钉和RFID卡设置在AGV航道上，编码器、控制单元和RID读卡器设置在控制盒内；

S2：读取磁钉数据，磁钉读取模块通过控制单元的串口读取磁传感器采集的磁钉数据；

S3：获取小车偏移数据，偏移数据获取模块根据磁传感器获得的磁钉状态获得小车偏移运行轨迹的角度和距离；

S4：规划路径，根据小车偏移运行轨迹的角度和距离规划出AGV转弯半径，同时控制左右差速实现以该半径转弯；

S5：获取陀螺仪角度，将陀螺仪的原始数据进行处理后，并积分得到AGV该时刻的角度；

S6：角度校准，通过磁钉校准的角度对陀螺仪获取的角度进行校准；

S7：读取RFID，在AGV小车运行到路径的交叉路口磁钉处时读取RFID地标的ID号；

S8：确定运行模式，将RFID的ID和任务路径中ID进行匹配得出相应动作确定AGV运行模式；

S9：运动控制，控制AGV执行直行、左转90度和右转90度的运动，如果AGV执行旋转动作，则差速电机以速度大小相等、方向相反原地运行，通过陀螺仪测量的角度判断AGV是都旋转到90度；

S10：重复步骤S2-S9。

该方法还包括一个脱轨检测步骤，通过电机编码器读取相邻磁钉AGV所运行里程判断，如果2m仍没有磁钉则AGV脱轨，停止AGV运行。

所述的步骤S2中读取磁钉数据通过控制单元的RS232串口读取磁导航数据，路过第2k个磁钉，记录其状态；路过2k+1记录状态，第2k和2k+1磁钉为一组磁钉。

所述的步骤S3获取小车偏移数据根据一组磁钉的状态和磁传感器的状态计算，磁传感器的状态：

结合图3可知AGV偏心距离为：

$\mathrm{\Δ}d=\frac{\underset{i=1}{\overset{i=12}{\Σ}}{x}_i(k+1)-\underset{i=1}{\overset{i=12}{\Σ}}{x}_i\left(k\right)}{2}m---\left(2\right)$

则AGV偏移角度为：

$\mathrm{\θ}=atan\left(\frac{\mathrm{\Δ}d}{l}\right)---\left(3\right)$

其中l为磁钉间距，m为磁传感器监测点间距，x_i为磁传感器状态。

所述的步骤S4路径规划为AGV仓储惯性导航的关键单元，AGV运动半径r，AGV航向角θ，磁钉间距l，根据图4和三角关系有：

$s=\sqrt{d^2+l^2}---\left(4\right)$

通过旋切角关系可以得出：

$r=\frac{s/2}{\cos \mathrm{\θ}}---\left(5\right)$

综合式(3)、式(4)、式(5)，可以得出AGV选择半径：

$s=\frac{\sqrt{d^2+l^2}}{2cos\left(atan\frac{\mathrm{\Δ}d}{l}\right)}---\left(6\right)$

然后，控制单元根据运行半径控制AGV左右差速。

所述的步骤S5中获取陀螺仪角度先由固定漂移处理模块将陀螺仪采集的原始数据，通过离线数据分析得到期望做补偿常量，使得数据成标准高斯分布。

再进行卡尔曼滤波，并进行积分后得到ω(k)，则在第k时刻，AGV的角度为

所述的步骤S6角度校准计算方式如下：AGV在2k时刻的角度为θ(2k)，在2k+1时刻角度为θ(2k+1)，陀螺仪所测量的角度为ρ(2k+1)，而 $\mathrm{\Δ}\mathrm{\θ}(2k+1)=\mathrm{\θ}(2k+1)-\mathrm{\θ}\left(2k\right)=2atan\left(\frac{\mathrm{\Δ}d}{l}\right),$

如果|ρ(2k+1)-Δθ(2k+1)|＜6°，则将陀螺仪的角度校准为

Claims (7)

1.一种适合仓储AGV的惯性导航方法，其特征在于：它包括如下子步骤：

S1：导航系统搭建，将陀螺仪设置在AGV小车上，磁传感器设置在小车车头底部中线上，磁钉和RFID卡设置在AGV航道上，编码器、控制单元和RID读卡器设置在控制盒内；

S2：读取磁钉数据，磁钉读取模块通过控制单元的串口读取磁传感器采集的磁钉数据；

S3：获取小车偏移数据，偏移数据获取模块根据磁传感器获得的磁钉状态获得小车偏移运行轨迹的角度和距离；

S4：规划路径，根据小车偏移运行轨迹的角度和距离规划出AGV转弯半径，同时控制左右差速实现以该半径转弯；

S5：获取陀螺仪角度，将陀螺仪的原始数据进行处理后，并积分得到AGV该时刻的角度；

S6：角度校准，通过磁钉校准的角度对陀螺仪获取的角度进行校准；

S7：读取RFID，在AGV小车运行到路径的交叉路口磁钉处时读取RFID地标的ID号；

S8：确定运行模式，将RFID的ID和任务路径中ID进行匹配得出相应动作确定AGV运行模式；

S9：运动控制，控制AGV执行直行、左转90度和右转90度的运动，如果AGV执行旋转动作，则差速电机以速度大小相等、方向相反原地运行，通过陀螺仪测量的角度判断AGV是都旋转到90度；

S10：重复步骤S2-S9。

2.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：该方法还包括一个脱轨检测步骤，通过电机编码器读取相邻磁钉AGV所运行里程判断，如果2m仍没有磁钉则AGV脱轨，停止AGV运行。

3.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：所述的步骤S2中读取磁钉数据通过控制单元的RS232串口读取磁导航数据，路过第2k个磁钉，记录其状态；路过2k+1记录状态，第2k和2k+1磁钉为一组磁钉。

4.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：所述的步骤S3获取小车偏移数据根据一组磁钉的状态和磁传感器的状态计算，磁传感器的状态：

则AGV偏心距离为：

$\mathrm{\Δ}d=\frac{\underset{i=1}{\overset{i=12}{\Σ}}{x}_i(k+1)-\underset{i=1}{\overset{i=12}{\Σ}}{x}_i\left(k\right)}{2}m---\left(2\right)$

则AGV偏移角度为：

$\mathrm{\θ}=atan\left(\frac{\mathrm{\Δ}d}{l}\right)---\left(3\right)$

其中l为磁钉间距，m为磁传感器监测点间距，x_i为磁传感器状态。

5.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：所述的步骤S4路径规划为AGV仓储惯性导航的关键单元，AGV运动半径r，AGV航向角θ，磁钉间距l，通过三角关系有：

$s=\sqrt{d^2+l^2}---\left(4\right)$

通过旋切角关系可以得出：

$r=\frac{s/2}{\cos \mathrm{\θ}}---\left(5\right)$

综合式(3)、式(4)、式(5)，可以得出AGV选择半径：

$s=\frac{\sqrt{d^2+l^2}}{2cos\left(atan\frac{\mathrm{\Δ}d}{l}\right)}---\left(6\right)$

然后，控制单元根据运行半径控制AGV左右差速。

6.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：所述的步骤S5中获取陀螺仪角度先去除固定漂移，再进行卡尔曼滤波，并进行积分后得到ω(k)，则在第k时刻，AGV的角度为

7.根据权利要求1所述的一种适合仓储AGV的惯性导航方法，其特征在于：所述的步骤S6角度校准计算方式如下：AGV在2k时刻的角度为θ(2k)，在2k+1时刻角度为θ(2k+1)，陀螺仪所测量的角度为ρ(2k+1)，而 $\mathrm{\Δ}\mathrm{\θ}(2k+1)=\mathrm{\θ}(2k+1)-\mathrm{\θ}\left(2k\right)=2atan\left(\frac{\mathrm{\Δ}d}{l}\right),$

如果|ρ(2k+1)-Δθ(2k+1)|＜6°，则将陀螺仪的角度校准为