[发明专利]基于二维码和惯性传感器的AGV复合导航方法

权利要求书

1.一种基于二维码和惯性传感器的AGV复合导航方法，其特征是按如下步骤进行：

步骤1、在AGV工作区域内等间距设置有n个二维码标签；

步骤2、AGV在行驶路径中识别到任意第i个二维码标签并启动二维码导航模式，从而利用计算得到的纵向偏差ΔL_QR、侧向偏差ΔD_QR、方向偏差Δφ_QR来实现AGV在第i个二维码标签上的姿态调整；

步骤2.1、在所述二维码导航模式下，利用车载摄像头获取AGV任意第i个二维码标签所对应的图像，得到长为L，宽为W的第i个二维码图像；

对所述第i个二维码图像中的三个定位角点进行识别，得到三个定位角点在第i个二维码图像中的像素行位置和像素列位置，分别记为和j表示定位角点的个数；

步骤2.2、利用式(1)得到AGV在第i个二维码上的纵向偏差

式(1)中，K表示摄像头的标定系数；

步骤2.3、利用式(2)得到AGV在所述第i个二维码上的侧向偏差

步骤2.4、利用式(3)得到AGV在所述第i个二维码上的方向偏差

式(3)中，j＝2；

步骤2.5、利用式(4)得到惯性导航模式下的初始纵向偏差ΔL′_IN、初始侧向偏差ΔD′_IN、初始方向偏差Δφ′_IN：

步骤3、AGV在行驶路径中无法识别到任意第i个二维码标签，则启动惯性导航模式，并利用计算得到的纵向偏差ΔL_IN、侧向偏差ΔD_IN、方向偏差Δφ_IN来实现AGV在行驶路径中第i个二维码标签和第i+1个二维码标签之间的导航；

步骤3.1、利用车载编码器估算AGV的车轮速度，利用惯性传感器采集AGV的横摆角速度，从而利用利用式(5)和式(6)分别建立系统状态空间方程和系统测量方程：

Z_(k)＝X_(k)+V_(k) (6)

式(5)和式(6)中：X_(k)表示k时刻的系统状态，Z_(k)表示k时刻的系统测量值；W_(k)表示k时刻的过程噪音；V_(k)表示k时刻的测试噪音；n_l,k表示k时刻AGV的左轮转速；n_l,k-1表示k-1时刻AGV的左轮转速；n_r,k表示k时刻AGV的右轮转速；n_r,k-1表示k-1时刻AGV的右轮转速；Δn_l,k表示k时刻左轮轮速的变化量；Δn_r,k表示k时刻右轮轮速的变化量；ω_k表示k时刻AGV的横摆角速度；ω_k-1表示k-1时刻AGV的横摆角速度；C表示左右轮速转换为横摆角速度的换算系数，并有：C＝πd/D，其中，d为车轮半径，D为轮距；

步骤3.2、根据所述系统状态空间方程和系统测量方程，对AGVk时刻的横摆角速度ω_k、左轮转速n_l,k和右轮转速n_r,k进行线性卡尔曼滤波，得到滤波后的横摆角速度ω′_k、左轮转速n′_l,k和右轮转速n′_r,k；

步骤3.3、利用式(7)得到滤波后的AGV的质心速度v_k：

步骤3.4、利用式(8)得到AGV在行驶过程中的位姿矩阵：

式(8)中：ΔT表示间隔的采样时间；L_IN,k表示AGV在k时刻相对于第i个二维码标签的纵向距离；D_IN,k表示AGV在k时刻相对于第i个二维码标签与第i+1个二维码标签连线的侧向距离；φ_IN,k表示AGV在k时刻相对于第i个二维码标签与第i+1个二维码标签连线的方向角度；

步骤3.5、利用式(9)得到AGV在行驶过程中的纵向偏差ΔL_IN、侧向偏差ΔD_IN、方向偏差Δφ_IN：