[发明专利]AGV小车的定位方法及定位系统

权利要求书

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₃。