CN105180934A

CN105180934A - 一种agv惯性导航方法

Info

Publication number: CN105180934A
Application number: CN201510591888.5A
Authority: CN
Inventors: 包壁祯; 徐兵; 林欢庆; 肖骥; 张巨鹏
Original assignee: Chengdu Siwi High Tech Industrial Park Co Ltd
Current assignee: Chengdu Siwi High Tech Industrial Park Co Ltd
Priority date: 2015-09-16
Filing date: 2015-09-16
Publication date: 2015-12-23

Abstract

本发明公开了一种AGV惯性导航方法，它包括如下子步骤：S1：导航系统搭建；S2：数据采集；S3：采集陀螺仪数据；S4：固定漂移处理；S5：卡尔曼滤波处理；S6：角度获取；S7：磁钉校准；S8：航迹推算；S9：PID调节；S10：运动控制。该方法通过查询方式获取陀螺仪采集的数据，同时能够有效滤除陀螺仪采集的数据的固定漂移和随机误差，能够获得陀螺仪准确的角度数据，精度为±0.1度，可准确推算出所有采样时刻的位置和姿态，精度达到±5mm，导航精度可达±10mm。

Description

一种AGV惯性导航方法

技术领域

本发明属于自动控制领域，属于一种AGV惯性导航方法。

背景技术

目前AGV自动引导小车已经成为智能制造、先进物流以及数字化工厂中的重要设备，作为方便工厂运输、提高生产效率具有非常重要的作用。而所谓自动引导,即是沿着指定轨迹运行,目前常见的巡线导航有光电传感器、视觉、激光和磁导航传感器等等，对于光电传感器，配合黑白胶带整体便宜、简单，但是信号很容易因为地面不平导致不稳、胶带易受灰尘、垃圾影响等，导致巡线不能很稳定；视觉可视范围大，对线的要求不高，但是很容易受光线影响，且视频采集处理卡价格不菲，采样频率不高；而目前国内外市面上最常见的AGV产品大多采用磁条导航。而磁条导航AGV虽然相对价格便宜，且运行也较为可靠，但是每次运行AGV需要铺设磁条，特别路径较长时，铺设工作量较大，路径过长时，磁条的成本会比较高，同时更换路线后，由于磁条底部粘性变弱，不能重复使用，磁条后期在水、碾压、磨损等条件下，维护较麻烦，而随着工业4.0和智能制造需求，因此市场上对一种使用更灵活、方便的AGV导航的研发是迫在眉睫。

发明内容

本发明的目的在于克服现有技术的不足，提供一种AGV惯性导航方法，该方法通过查询方式获取陀螺仪采集的数据，同时能够有效滤除陀螺仪采集的数据的固定漂移和随机误差，能够获得陀螺仪准确的角度数据，精度为±0.1度，可准确推算出所有采样时刻的位置和姿态，精度达到±5mm，导航精度可达±10mm。

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

S1：导航系统搭建，将陀螺仪设置在AGV小车上，磁传感器设置在小车车头底部中线上，磁钉铺设在AGV航道上，编码器、数据处理单元和运动控制单元设置在控制盒内；

S2：数据采集，陀螺仪采集车轮的角速度，磁传感器采集航道上磁钉的位置，编码器采集单元根据外部中断计数编码器的脉冲，结合电机的极对数得到轮子的转动周数，根据轮子直径和减速比可以计算出里程；

S3：采集陀螺仪数据，数据处理单元中的陀螺仪数据采集模块通过SPI接口定时发送查询协议到陀螺仪，获取陀螺仪测量的原始数据；

S4：固定漂移处理，数据处理单元中的固定漂移处理模块将陀螺仪采集的原始数据，通过离线数据分析得到期望做补偿常量，使得数据成标准高斯分布；

S5：卡尔曼滤波处理，将去除固定漂移的数据进行卡尔曼滤波，使数据变得平滑；

S6：角度获取，将卡尔曼滤波后的数据进行积分得到角度信息；

S7：磁钉校准，磁钉校准单元根据磁传感器采集的磁钉轨迹得到AGV当前运行方向和实际轨迹方向的偏差角度，如果偏差角度较小，则将陀螺仪采集角度积分初始点校准为校准角度，否则不校准；如果偏差角度太大，则认为AGV脱轨运行；

S8：航迹推算，根据编码器的里程数据、校准后的陀螺仪角度数据形成极坐标关系，推算出所有采样时刻的位置和姿态，形成航迹；

S9：PID调节，将当前时刻的位置点和给定位置点作为PID调节器的输入，得出增量角度作为输出；

S10：运动控制，以增量角度转为运动输入，调整AGV的姿态。

所述的步骤S5卡尔曼滤波处理采集多组原始数据样本，分析其固定漂移量、高斯的期望和方差，并建立陀螺仪的数据模型，建立卡尔曼滤波的参数，

x(k)＝x(k-1)(1)

观测方程就是传感器的感知值，主要误差就是来自于测量误差，所以简化为

z(k)＝x(k)+v(k)(2)

z(k)为实际测量，x(k)真实值，v(k)为具有整体便宜的高斯噪声，这样就得到了其状态更新方程：

{\hat{x}}_{k}^{-} = {\hat{x}}_{k - 1} - - - (3)

P_{k}^{-} = P_{k - 1} - - - (4)

同样得到测量更新方程：

K_{k} = P_{k}^{-} / (P_{k}^{-} + R) - - - (5)

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - {\hat{x}}_{k}^{-}) - - - (6)

P_{k} = (1 - K_{k}) P_{k}^{-} - - - (7)

虽然卡尔曼滤波为无偏估计，但是由于原始的噪声使得增量变化不连续，会影响到控制算法的控制增量，同时也会影响最后角度控制效果，因此需要进行卡尔曼滤波。

所述的步骤S6中的角度获取将卡尔曼滤波后的陀螺仪数据进行积分在处理中离散为累计求和方式这样求出陀螺仪的角速度。

所述的步骤S7磁钉校准原理为：在运行轨迹直线处放置相距L的一对磁钉，磁钉校准单元，根据磁传感器分别检测M1磁钉、M2磁钉的位置，计算出AGV当前运行方向和实际轨迹方向的偏差角度θ＝arctan((b-a)/L)，如果该角度在可接受范围内则将陀螺仪的角度测量值进行校准，否则则认为AGV脱轨。

所述的步骤S8航迹推算根据编码器的里程数据、校准后的陀螺仪数据形成极坐标关系，假设k时刻AGV的位置和姿态为(x(k),y(k),θ(k))，则k+1时刻姿态为：

x(k+1)＝x(k)+ΔscosΔθ(k)

y(k+1)＝y(k)+ΔssinΔθ(k)

θ(k+1)＝θ(k)+Δθ(k)

其中Δs(k)为编码器获得增量里程；Δθ(k)采用陀螺仪测量得的增量角度，这样通过迭代推算出所有采样时刻的位置和姿态，从而形成航迹。

所述的步骤S9PID调节中如果在k时刻给定点坐标为(x_d(k),y_d(k))，而实际位置和姿态为(x(k),y(k))，此时输出偏差采用极坐标方式表达

Δ ρ = \sqrt{{(x (k) - x_{d} (k))}^{2} + {(y (k) - y_{d} (k))}^{2}}

Δθ＝atan(y(k)-y_d(k)/x(k)-x_d(k))

无偏差输入则指AGV实际轨迹和给定轨迹重合，也即Δθ；反之，只要Δθ不为零，则存在偏差，纠偏算法仍采用PID控制算法，如果调整后AGV的角度一致，但坐标不一致，即x不一致，这样AGV运行一段时间必然和给定坐标重合；但是如果AGV在当前周期调整还未达到上一时刻的给定值时，AGV偏差输入以当前时刻的给定值为准。

所述的步骤S10运动控制需根据AGV运动模型控制，若AGV为差动模型则根据角度偏差控制左右电机差速；若AGV为舵轮则直接控制舵轮角度。

本发明的有益效果是：本发明提供了一种AGV惯性导航方法，该方法通过查询方式获取陀螺仪采集的数据，同时能够有效滤除陀螺仪采集的数据的固定漂移和随机误差，能够获得陀螺仪准确的角度数据，精度为±0.1度，可准确推算出所有采样时刻的位置和姿态，精度达到±5mm，导航精度可达±10mm。

附图说明

图1为惯性导航方法流程图；

图2为磁钉校准原理图A；

图3为磁钉校准原理图B；

图4为轨迹推算示意图。

具体实施方式

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

如图1所示，一种AGV惯性导航方法，它包括如下子步骤：

S10：运动控制，以增量角度转为运动输入，调整AGV的姿态。

x(k)＝x(k-1)(1)

z(k)＝x(k)+v(k)(2)

{\hat{x}}_{k}^{-} = {\hat{x}}_{k - 1} - - - (3)

P_{k}^{-} = P_{k - 1} - - - (4)

同样得到测量更新方程：

K_{k} = P_{k}^{-} / (P_{k}^{-} + R) - - - (5)

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - {\hat{x}}_{k}^{-}) - - - (6)

P_{k} = (1 - K_{k}) P_{k}^{-} - - - (7)

步骤S7磁钉校准原理为：在运行轨迹直线处放置相距L的一对磁钉，如图2所示的如图M1、M2。磁钉校准单元根据磁传感器分别检测M1磁钉、M2磁钉的位置，如图3所示，计算出AGV当前运行方向和实际轨迹方向的偏差角度θ＝arctan((b-a)/L)，如果该角度在可接受范围内则将陀螺仪的角度测量值进行校准，否则则认为AGV脱轨。磁传感器的间距越短，磁钉间距越大，测量角度分辨率越小，磁钉间距不能太大，否则会出现不能同时读到两个磁钉现象，如果没有读到第二颗磁钉，则认为校准角度很大，即AGV脱轨。

如图4所示，所述的步骤S8航迹推算根据编码器的里程数据、校准后的陀螺仪数据形成极坐标关系，假设k时刻AGV的位置和姿态为(x(k),y(k),θ(k))，则k+1时刻姿态为：

x(k+1)＝x(k)+ΔscosΔθ(k)

y(k+1)＝y(k)+ΔssinΔθ(k)

θ(k+1)＝θ(k)+Δθ(k)

Δ ρ = \sqrt{{(x (k) - x_{d} (k))}^{2} + {(y (k) - y_{d} (k))}^{2}}

Δθ＝atan(y(k)-y_d(k)/x(k)-x_d(k))

Claims

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

S10：运动控制，以增量角度转为运动输入，调整AGV的姿态。

2.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S5卡尔曼滤波处理采集多组原始数据样本，分析其固定漂移量、高斯分布的期望和方差，并建立陀螺仪的数据模型，建立卡尔曼滤波的参数，

x(k)＝x(k-1)(1)

z(k)＝x(k)+v(k)(2)

{\hat{x}}_{k}^{-} = {\hat{x}}_{k - 1} - - - (3)

P_{k}^{-} = P_{k - 1} - - - (4)

同样得到测量更新方程：

K_{k} = P_{k}^{-} / (P_{k}^{-} + R) - - - (5)

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - {\hat{x}}_{k}^{-}) - - - (6)

P_{k} = (1 - K_{k}) P_{k}^{-} - - - (7)

3.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S6中的角度获取将卡尔曼滤波后的陀螺仪数据进行积分在处理中离散为累计求和方式这样求出陀螺仪的角速度。

4.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S7磁钉校准原理为：在运行轨迹直线处放置相距L的一对磁钉，磁钉校准单元，根据磁传感器分别检测M1磁钉、M2磁钉的位置，计算出AGV当前运行方向和实际轨迹方向的偏差角度θ＝arctan((b-a)/L)，如果该角度在可接受范围内则将陀螺仪的角度测量值进行校准，否则则认为AGV脱轨。

5.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S8航迹推算根据编码器的里程数据、校准后的陀螺仪数据形成极坐标关系，假设k时刻AGV的位置和姿态为(x(k),y(k),θ(k))，则k+1时刻姿态为：

x(k+1)＝x(k)+ΔscosΔθ(k)

y(k+1)＝y(k)+ΔssinΔθ(k)

θ(k+1)＝θ(k)+Δθ(k)

6.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S9PID调节中如果在k时刻给定点坐标为(x_d(k),y_d(k))，而实际位置和姿态为(x(k),y(k))，此时输出偏差采用极坐标方式表达

Δ ρ = \sqrt{{(x (k) - x_{d} (k))}^{2} + {(y (k) - y_{d} (k))}^{2}}

Δθ＝atan(y(k)-y_d(k)/x(k)-x_d(k))

7.根据权利要求1所述的一种AGV惯性导航方法，其特征在于：所述的步骤S10运动控制需根据AGV运动模型控制，若AGV为差动模型则根据角度偏差控制左右电机差速；若AGV为舵轮则直接控制舵轮角度。