2 AGV 导航系统定位精度研究AGV 在GPS 信号良好的情况下可实现精度为10 cm 的定位导航，满足初步定位导航工况需求，本文对GPS 信号完全丢失情况下采用惯性导航定位和ArUco 标签定位的组合定位方案实现AGV 粗定位的精度进行分析。

2.1 AGV 惯性导航系统精度AGV 惯性导航系统的惯性坐标系为xi、yi、zi，运载体的坐标系为xb、yb、zb，如图1 所示，运载体的坐标系与惯性导航系统的惯性坐标系沿z 轴的转角为θ，惯性导航系统包括两个加速度计和一个陀螺仪，分别测量x 轴、y 轴的加速度和z 轴方向的角速度。

AGV 在惯性坐标系中的运动方程[5] 为

式中: vix 是xi 轴速度分量，viy 是yi 轴速度分量，abx是xb 轴加速度计测量值，aby 是yb 轴加速度计测量值，ωbz 是陀螺仪的角速度测量值。对式(1) 进行一阶变分得到5 个状态变量的误差方程[5]

式中: Δθ 是角度误差，Δvix 是xi 轴速度误差，Δviy是yi 轴速度误差，Δxi 是xi 轴位置误差，Δyi 是yi 轴位置误差，Δωbz 是陀螺仪的角速度偏差，Δabx 是xb 轴加速度计偏差，Δayb 是yb 轴加速度计偏差。将式（2）简化后得到误差方程[5]

由式（3）可知位置误差与速度误差相关，速度误差与2 个加速度计误差和1 个陀螺仪误差都相关，精确分析较困难，对其进行近似分析。本文中AGV 的满载速度为3 m/s，加速度为0.3 m/s2。目前，中等陀螺仪的偏差是20°/ h ，中等加速度计的偏差是10-2g，有

由于Δabx 远大于aiyΔθ，Δvix 的主项为Δabxcosθ-Δabysinθ，aiyΔθ 可忽略不计，则简化误差方程为

为不失一般性，设Δabx=Δaby，式（4）简化为

对式（5）中加速度误差进行两次积分，得到位置误差方程

式（6）中，t 为ArUco 标签的数据更新时间。基于视觉ArUco 标签技术的AGV 组合导航系统定位精度由3 个因素决定: 加速度计偏差Δabx、AGV 运行方向θ和ArUco 标签数据更新时间t。

AGV 的定位精度随ArUco 标签数据更新时间t 的增大而增大，当AGV 检测到下一个ArUco 标签时，定位误差恢复到基于视觉的ArUco 标签定位误差， ArUco标签间距越短，AGV 组合导航系统的更新时间越短，AGV 的定位精度越高。

铁路物流园堆场内AGV 转运集装箱沿着xi 轴方向满载启动行驶，运行100 m 才能检测到下一个ArUco标签，运行时间为38.3 s，加速计的精度为4×10-3g。将上述数据代入式（6）得

AGV 空载以6 m/s 的速度恒速运行时位置误差为

AGV 距离ArUco 标签3 m 时可检测识别出前摄像头可视区的ArUco 标签，根据相机内参数、ArUco 标签的特征点、相机成像模型计算AGV 与ArUco 标签之间的相对位置关系，ArUco 标签的经纬度已知，经过坐标转换得出AGV 的经纬度。针孔相机模型见图2。

图2 针孔相机模型

如图2 所示，Ow-Xw Yw Zw 为世界坐标系，Oc-Xc YcZc为摄像机坐标系，O-xy 为图像物理坐标系，Oi-uv 为图像像素坐标系， Oc 为摄像机的光学中心，Oc 在图像平面的投影点坐标为(u0，v0)，Oc 到图像平面的距离为焦距f，点P 在世界坐标系的坐标为(Xw ，Yw ，Zw)，点P 在图像像素坐标系的坐标为(u，v)，根据透视投影几何关系点P 在图像物理坐标系的坐标为[6]

将点P 在图像物理坐标系的坐标转换为图像像素坐标系坐标，并用齐次坐标与矩阵表示为[6]

世界坐标系在摄像机坐标系的姿态和位置可以用旋转矩阵和平移向量来描述，则点在世界坐标系的坐标和摄像机坐标系的坐标转换为[6]

将式（9）代入式（8）中，得到空间点的图像坐标与实际三维坐标之间的关系为[6]

式（10）中，使用相机标定获取相机内参数(f，u0 ，v0 )，AGV 前摄像头检测出ArUco 标签后，根据ArUco 标签特征点在世界坐标系的实际三维坐标和对应特征点在图像像素坐标系的坐标，可以求出旋转矩阵R 和平移向量T，即AGV 前摄像头与ArUco 标签的位姿关系。相机标定内参数有一定误差和相机镜头存在畸变等因素影响摄像头测量精度，测量X 轴方向AGV 前摄像头与ArUco标签不同距离时的摄像头的测量误差如图3 所示，AGV前摄像头检测到ArUco 标签时的误差小于15 mm。