CN109579838A - Agv小车的定位方法及定位系统 - Google Patents

Abstract

本发明提供一种AGV小车的定位方法，包括如下步骤：系统启动、前端采集、数据同步、姿态解算、数据融合、判定AGV小车速度、零速修正。本发明还提供一种AGV小车的定位系统。本发明解决了AGV的高精度定位问题；采用视觉与惯性导航融合的方式来对惯性导航和视觉的误差进行互相修正，来提高定位的精度，采用零速修正的方式来对累计误差进行消除，达到高精度定位的目的。

Description

AGV小车的定位方法及定位系统

技术领域

本发明属于定位系统技术领域，涉及一种AGV小车的定位方法及定位系统。

背景技术

AGV小车在室内进行自主导航、智能搬运，需要准确的位置信息和障碍物的信息。为了解决这个问题，现有的方案包括有：磁条导航、二维码导航、视觉导航、惯性导航等。磁条导航、二维码导航的主要缺点为：容易被碾压或者腐蚀而被损坏，不容易维护；视觉导航：在室内光线较暗，变化较大，导致采集的图像数据质量较差，影响导航；惯性导航：采用航迹推演的方式进行姿态解算，具有较大累计误差。

(1)磁条、二维码类导航方案：在室内空间当中，特别是机器人、AGV小车运动的过程当中，容易将磁条和二维码进行碾压，导致损坏从而导致无法进行导航，这就要求这里定位方式具有较好的维护，从而带来较高的维护成本。并且磁条导航还需要要求小车在磁条上进行运动，对运动进行了限制，不利于完全自主式的导航方式。

(2)传统的视觉惯导融合的方案：这种方案对场景具有一定的要求，需要曾在闭环情况下定位精度才会得到提升，在没有闭环信息的情况下，无法进行全局后端优化，无法消除累计误差，使得精度下降。

(3)传统的视觉导航方案：视觉导航在室内环境下，容易受到环境的影响，如：光线、粉尘等，在光线不稳定下，所找到的特征点和特征点数将会不一样，导致整个导航系统的鲁棒性降低；而且视觉导航比较依赖于闭环修正，需要在一个有闭环的环境下进行导航，因此对场景具有一定的限制。

(4)传统惯性导航中的零速修正方案:惯性导航的不足在于容易发散，不适合应用于长时间的导航，累计误差较大，加入零速修正后的惯性导航，虽能够在一定程度上起到消除累计误差的作用，但仍然曾在停车较频繁，停车时间较长的问题。

发明内容

本发明的目的是提供一种AGV小车的定位方法，解决了AGV的高精度定位问题；采用视觉与惯性导航融合的方式来对惯性导航和视觉的误差进行互相修正，来提高定位的精度，采用零速修正的方式来对累计误差进行消除，达到高精度定位的目的。

本发明的技术方案为：

本发明提供一种AGV小车的定位方法，包括如下步骤：

(1)系统启动；

(2)前端采集：工业相机采集图像数据，IMU采集数据信息，并将图形数据和IMU数据输入导航计算机；

(3)数据同步：在图像数据传输进导航计算机后，通过一个标志位，将图像信息传输给IMU，将图像数据与IMU数据进行同步；

(4)姿态解算；视觉姿态解算和IMU姿态计算；

(6)数据融合：将IMU姿态解算后的结果作为运动量，构建运动方程，将视觉解算出来的姿态信息作为观测量，来构建观测方程，构建Kalman滤波方程，进行数据融合，得到最优结果；

(7)判定AGV小车速度：根据判定方法，判定AGV小车处于绝对零速的时候，进入步骤(8)；反之，得到导航相关的定位信息，导航结束；

(8)零速修正：构建速度误差方程，运用Kalman滤波，对姿态信息进行修正，并将修正后的状态进行零速更新，达到消除累计误差的目的，导航结束。

优选的，所述步骤(2)中，工业相机将采集的图像数据通过USB端口传输进入导航计算机，IMU通过串口，并运用串口转换成USB端口，将IMU数据传输进入导航计算机；

优选的，所述视觉姿态解算为：将图像数据依次经过视觉跟踪、图像通过配置算法、PnP姿态解算算法，得到视觉姿态信息。

优选的，所述IMU姿态解算为：IMU数据通过航机推演算法进行姿态解算，并进行姿态更新，得到IMU姿态信息。

优选的，所述零速修正的判定方法为：

根据IMU中的加速度计和陀螺仪的输出来判定零速区间：通过第i时刻加速度计输出的比力信息f_i＝[f_xi f_yi f_zi]^T，陀螺仪输出的角速度信息ω_i＝[ω_xi ω_yi ω_zi]^T进行零速检测。

优选的，采用IMU检测零速，其检测算法包括三个条件：加速度模值、加速度滑动方差和角速度模值，分别记为Q1，Q2，Q3；

各条件实现方式如下：

a.加速度模值检测，即当加速度模值大于等于最小阈值且小于等于最小阈值时，条件Q₁(i)＝1；

b.比力滑动方差检测，即当滑动窗口大小为N时比力方差小于等于给定阈值时，条件Q₂(i)＝1；

其中，为[i-n i+n]时间间隔内采样得到的2n+1个比力f的均值，即：

c.角速度模值检测，即当角速度模值小于等于给定阈值th_max时，条件Q₃(i)＝1；

IMU检测算法为多条件符合检测方法，即同时满足以上三个条件时为零速区间：

Q_IMU(i)＝Q₁∧Q₂∧Q₃。

优选的，所述零速更新为：由于AGV定位系统是非线性的，利用扩展卡尔曼滤波对非线性系统中的待估参数X进行线性化，得到线性化的模型，然后在利用标准Kalman滤波进行估计；

选取地理坐标中姿态误差角、速度误差、位置误差、陀螺常值漂移和加速度计常值漂移为状态量取系统速度误差δv为观测量，建立15维Kalman滤波器：

式中：F是根据误差模型和状态矢量构成的15*15维系统矩阵；W为15维系统随机过程噪声序列；V为三维系统随机观测噪声序列；H＝[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]是3×15维观测矩阵；

在零速阶段AGV小车的速度是绝对零，因此可以将零速作为外参考速度，应用Kalman滤波器对系统状态进行估计。

本发明的另一方面还提供一种AGV小车定位系统，所述定位系统采用上述所述的任意一种定位方法。

优选的，所述定位系统包括：惯性测量单元(IMU)，工业相机，导航计算机，AGV小车；

所述IMU中配备有陀螺仪和加速度计；

所述AVG小车搭载有工业相机、IMU和导航计算机，所述工业相机通过接口与导航计算机相连，所述IMU通过接口与导航计算机相连。

本发明的有益效果为：

1.本发明解决了AGV的高精度定位问题：采用视觉与惯性导航(本发明采用IMU)融合的方式来对惯性导航和视觉的误差进行互相修正，来提高定位的精度，采用零速修正的方式来对累计误差进行消除，达到高精度定位的目的。

2.本发明采用工业相机和惯性测量单元(IMU)相结合，不需要对场景进行特殊的处理和维护，也不需要AGV小车在特定的路线上进行运动，具备完全的自主性；本发明引入零速修正的策略，利用AGV小车停车中的绝对零速来对累计误差进行修正，不需要特定的场景，不需要有闭环的信息(场景识别)，依然能够消除累积误差，比传统的视觉惯性融合的方式更加的方便，具有更广泛的应用空间。本发明采用工业相机，加入了视觉部分，它很好的对IMU进行了修正，能够使得IMU的误差发散较慢，因此需要停车的时间更短，停车的频率更低。

3.本发明是典型的多源融合导航系统，由惯性导航(本发明采用的惯性导航系统为IMU)和视觉导航(工业相机)共同组成，同时引入零速修正的策略，在光线不稳定或者较差时，可以惯性导航为主，视觉导航为辅来进行导航；而惯性导航本身也不会受到外界的影响，因此本发明具有高效稳定的特点，不容易受到外在条件的影响，当视觉方面受到影响而不准的时候惯性导航(IMU)能够起到对应的作用。

4.在传统的视觉SLAM(即时定位与地图构建)和传统VISLAM(视觉slam)的基础上引入了零速修正的概念，即使在无闭环条件的环境中，依旧可以对整个系统进行全局优化，消除整个的累计误差，具有非常广泛的应用空间，而不需要局限在有闭环信息的空间当中，也不需要小车按照既定的轨迹和轨道进行运行(词条导航)。

5.在整个系统当中，采用工业相机，就可以测出任意时刻的速度值，不管是当场景较简单，特征较明显时，还是在光线较暗或者图像质量较差时，都可以得到准确的速度信息，进行误差修正，对累计误差进行消除，因此本发明在提供高精度定位信息的基础之上，能够做到停车少、时间短、甚至可以不停车。

附图说明

图1是本发明实施例提供的一种AGV小车的定位方法的流程示意图；

图2是本发明实施例提供的AGV小车的定位系统的结构示意图；

图3是本发明实施例提供的一种AGV小车的定位方法中视觉的解算位置误差仿真数据示意图；

图4是本发明实施例提供的一种AGV小车的定位方法中IMU的解算位置误差仿真数据示意图；

图5是本发明实施例提供的一种AGV小车的定位方法中视觉与IMU结合的解算位置误差仿真数据示意图；

图6是本发明实施例提供的一种AGV小车的定位方法中视觉、IMU与零速修正结合的解算位置误差仿真数据示意图。

具体实施方式

下面将根据附图以及具体实施方式对发明进行详细的说明。以下为本发明的优选实施例，本发明的实施例不限制本发明的保护范围，本发明的保护范围以其权利要求书为准。

实施例一

本发明的实施例一一种AGV小车的定位方法，包括如下步骤：

(1)系统启动；

(2)前端采集：工业相机采集图像数据，工业相机将采集的图像数据通过USB端口传输进入导航计算机；IMU采集数据信息，通过串口，并运用串口转换成USB端口，将IMU数据传输进入导航计算机；

(3)数据同步：在图像数据传输进导航计算机后，通过一个标志位，将图像信息传输给IMU，将图像数据与IMU数据进行同步；

(4)姿态解算：IMU数据通过航机推演算法进行姿态解算，并进行姿态更新，得到IMU姿态信息。将图像数据依次经过视觉跟踪、图像通过配置算法、PnP姿态解算算法，得到视觉姿态信息。

(6)数据融合：将IMU姿态解算后的结果作为运动量，构建运动方程，将视觉解算出来的姿态信息作为观测量，来构建观测方程，构建Kalman滤波方程，进行数据融合，得到最优结果；

(7)判定AGV小车速度：根据判定方法，判定AGV小车处于绝对零速的时候，进入步骤(8)；反之，得到导航相关的定位信息，导航结束；

(8)零速修正：构建速度误差方程，运用Kalman滤波，对姿态信息进行修正，并将修正后的状态进行零速更新，达到消除累计误差的目的，导航结束。

所述零速修正的判定方法为：

根据IMU中的加速度计和陀螺仪的输出来判定零速区间：通过第i时刻加速度计输出的比力信息f_i＝[f_xi f_yi f_zi]^T，陀螺仪输出的角速度信息ω_i＝[ω_xi ω_yi ω_zi]^T进行零速检测。

优选的，采用IMU检测零速，其检测算法包括三个条件：加速度模值、加速度滑动方差和角速度模值，分别记为Q1，Q2，Q3；

各条件实现方式如下：

a.加速度模值检测，即当加速度模值大于等于最小阈值且小于等于最小阈值时，条件Q₁(i)＝1；

b.比力滑动方差检测，即当滑动窗口大小为N时比力方差小于等于给定阈值时，条件Q₂(i)＝1；

其中，为[i-n i+n]时间间隔内采样得到的2n+1个比力f的均值，即：

c.角速度模值检测，即当角速度模值小于等于给定阈值th_max时，条件Q₃(i)＝1；

IMU检测算法为多条件符合检测方法，即同时满足以上三个条件时为零速区间：

Q_IMU(i)＝Q₁∧Q₂∧Q₃。

所述零速更新为：由于AGV定位系统是非线性的，利用扩展卡尔曼滤波对非线性系统中的待估参数X进行线性化，得到线性化的模型，然后在利用标准Kalman滤波进行估计；

选取地理坐标中姿态误差角、速度误差、位置误差、陀螺常值漂移和加速度计常值漂移为状态量取系统速度误差δv为观测量，建立15维Kalman滤波器：

式中：F是根据误差模型和状态矢量构成的15*15维系统矩阵；W为15维系统随机过程噪声序列；V为三维系统随机观测噪声序列；H＝[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]是3×15维观测矩阵；

在零速阶段AGV小车的速度是绝对零，因此可以将零速作为外参考速度，应用Kalman滤波器对系统状态进行估计。

实施例二

本发明的实施例二提供一种AGV小车定位系统，所述定位系统采用如上述所述的定位方法。

所述定位系统包括：惯性测量单元(IMU)，工业相机，导航计算机，AGV小车；

所述IMU中配备有陀螺仪和加速度计；

所述AVG小车搭载有工业相机、IMU和导航计算机，所述工业相机通过接口(可以采用USB端口)与导航计算机相连，所述IMU通过接口(可以采用串口)与导航计算机相连。

从图3-6中可以看出，图3是视觉的解算位置误差仿真数据，有误匹配的情况，所以会出现误差较大的点。图4是IMU的解算位置误差仿真数据，它随时间误差累计，且发散较快。图5是视觉与惯性导航(本实施例采用IMU)结合的解算位置误差仿真数据，虽然误差发散的满，但依旧有累计误差，且无法消除。图6是视觉与惯性导航(本实施例采用IMU)以及零速修正结合的解算位置误差，发散慢的情况下，且能够消除累计误差。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (9)

1.一种AGV小车的定位方法，其特征在于，包括如下步骤：

(1)系统启动；

(2)前端采集：工业相机采集图像数据，IMU采集数据信息，并将图形数据和IMU数据输入导航计算机；

(3)数据同步：在图像数据传输进导航计算机后，通过一个标志位，将图像信息传输给IMU，将图像数据与IMU数据进行同步；

(4)姿态解算；视觉姿态解算和IMU姿态计算；

(6)数据融合：将IMU姿态解算后的结果作为运动量，构建运动方程，将视觉解算出来的姿态信息作为观测量，来构建观测方程，构建Kalman滤波方程，进行数据融合，得到最优结果；

(7)判定AGV小车速度：根据判定方法，判定AGV小车处于绝对零速的时候，进入步骤(8)；反之，得到导航相关的定位信息，导航结束；

(8)零速修正：构建速度误差方程，运用Kalman滤波，对姿态信息进行修正，并将修正后的状态进行零速更新，达到消除累计误差的目的，导航结束。

2.根据权利要求1所述的方法，其特征在于，所述步骤(2)中，工业相机将采集的图像数据通过USB端口传输进入导航计算机，IMU通过串口，并运用串口转换成USB端口，将IMU数据传输进入导航计算机。

3.根据权利要求1所述的方法，其特征在于，所述视觉姿态解算为：将图像数据依次经过视觉跟踪、图像通过配置算法、PnP姿态解算算法，得到视觉姿态信息。

4.根据权利要求1所述的方法，其特征在于，所述IMU姿态解算为：IMU数据通过航机推演算法进行姿态解算，并进行姿态更新，得到IMU姿态信息。

5.根据权利要求1所述的方法，其特征在于，所述零速修正的判定方法为：

根据IMU中的加速度计和陀螺仪的输出来判定零速区间：通过第i时刻加速度计输出的比力信息f_i＝[f_xi f_yi f_zi]^T，陀螺仪输出的角速度信息ω_i＝[ω_xi ω_yi ω_zi]^T进行零速检测。

6.根据权利要求5所述的方法，其特征在于，所述步骤(7)中采用IMU检测零速，其检测算法包括三个条件：加速度模值、加速度滑动方差和角速度模值，分别记为Q1，Q2，Q3；

各条件实现方式如下：

a.加速度模值检测，即当加速度模值大于等于最小阈值且小于等于最小阈值时，条件Q₁(i)＝1；

b.比力滑动方差检测，即当滑动窗口大小为N时比力方差小于等于给定阈值时，条件Q₂(i)＝1；

其中，为[i-n i+n]时间间隔内采样得到的2n+1个比力f的均值，即：

c.角速度模值检测，即当角速度模值小于等于给定阈值th_max时，条件Q₃(i)＝1；

IMU检测算法为多条件符合检测方法，即同时满足以上三个条件时为零速区间：

Q_IMU(i)＝Q₁∧Q₂∧Q₃。

7.根据权利要求1所述的方法，其特征在于，所述零速更新为：由于AGV定位系统是非线性的，利用扩展卡尔曼滤波对非线性系统中的待估参数X进行线性化，得到线性化的模型，然后在利用标准Kalman滤波进行估计；

选取地理坐标中姿态误差角、速度误差、位置误差、陀螺常值漂移和加速度计常值漂移为状态量取系统速度误差δv为观测量，建立15维Kalman滤波器：

式中：F是根据误差模型和状态矢量构成的15*15维系统矩阵；W为15维系统随机过程噪声序列；V为三维系统随机观测噪声序列；H＝[0_3×3I_3×30_3×30_3×30_3×3]是3×15维观测矩阵；

在零速阶段AGV小车的速度是绝对零，因此可以将零速作为外参考速度，应用Kalman滤波器对系统状态进行估计。

8.一种AGV小车定位系统，其特征在于，所述定位系统采用如权利要求1-6任意一项所述的定位方法。

9.根据权利要求8所述的定位系统，其特征在于，所述定位系统包括：惯性测量单元(IMU)，工业相机，导航计算机，AGV小车；

所述IMU中配备有陀螺仪和加速度计；

所述AVG小车搭载有工业相机、IMU和导航计算机，所述工业相机通过接口与导航计算机相连，所述IMU通过接口与导航计算机相连。