基于超声波雷达传感器的AGV导航参数选择*

张 文， 孙作雷， 曾连荪， 张 波

(1.上海海事大学 信息工程学院，上海 201306；2.中国科学院 上海高等研究院，上海 201210)

基于超声波雷达传感器的AGV导航参数选择*

张 文1， 孙作雷1， 曾连荪1， 张 波2

(1.上海海事大学 信息工程学院，上海 201306；2.中国科学院 上海高等研究院，上海 201210)

针对自主导引车(AGV)的已有导引方式灵活性差和精度低等问题，提出使用超声波雷达传感器实现AGV自主导航并设置合理的导航模型参数。分别建立AGV的运动学模型和传感器的观测模型，为了表征实际的系统误差，基于一致性检测原理为过程模型和观测模型添加乘性或加性噪声。仿真结果表明：设置适当的噪声参数大小，可以实现AGV沿基准路线的自主导航运动。

超声波雷达传感器； 自主导引车； 一致性检测； 正则化新息平方

0 引 言

传统的港口集装箱搬运车均采用人工操作的平板牵引车、跨运车、吊运车和集装箱叉车等，属于机械化、半自动化的方式，具有人工劳动强度大、准确性低、人工安全性差等缺点。智能集装箱搬运车也称作自主导引车(autonomous guided vehicle,AGV)[1]，属于移动机器人的一种，作为受车载微电脑控制的新型搬运设备，AGV可以实现自主行驶，不需要人工看管，可以广泛应用于港口集装箱码头环境。

铺轨式是最早的AGV导引方式，车身在固定的轨道上行驶，可以通过特定的设备控制其运动或停止。随着传感器技术的发展和广泛应用，AGV的导引方式也发生了很大变化。使用电磁感应传感器的电磁导引方式[2]是较成熟且应用较多的一种方式，但该方式的车辆线路固定，不易改变，并且码头环境下埋设电缆线困难，导致车辆的灵活性差，运动范围受到限制。视觉导引方式主要使用固定在AGV上的电荷耦合装置[3](charge coupled device,CCD)获得周围的图像信息，经实时处理后确定AGV的运动方式，不过，固定路线视觉导引方式同样需要埋设导引线，不方便改道，且光照条件的强弱会严重影响视觉传感器的效果。激光导引方式[4]由固定在AGV高处的激光器发射激光束，并被环境中设置的反光镜等定位标志反射，从而建立模型实现车辆的定位和定向。虽然激光导引客服了视觉导引对光线条件的依赖，但集装箱林立的复杂码头环境会影响激光束的扫描视场，进而影响定位精确。惯性导引方式[5]则利用全球定位系统(GPS)和惯性测量单元(IMU)协同工作，二者相互辅助可以实现AGV的精确定位。该方法灵活性强，精度高，但是工程实现难度大，价格昂贵，不适于在工业中大规模使用。

由于超声波方向性强、反射面积大，特别适合摆放规律的集装箱码头，而且超声波雷达[6]价格相对低廉，因此，本文选择超声波雷达去实现智能集装箱搬运车的自主导航，并参考激光导引方式建立AGV导航系统的过程模型和观测模型。如何选择合适的噪声参数是模型建立过程中非常重要的环节，为了验证所选噪声参数的合理性，本文进一步使用一致性检测方法校验不同参数对AGV的影响。仿真结果表明:只要设置合适的噪声参数大小，超声波雷达导引方式可以较好地实现AGV的自主导航运动，与其他常用的导引方式相比，该方法具有更好的灵活性和精确度。

1 导航模型建立

1.1 运动模型

过程模型主要是建立AGV理论上的运动学模型，为了讨论方便，本文将四轮AGV运动模型简化为一个基本的二轮自行车运动模型[7]，该模型主要包括一个用于导向的前轮和一个被动转动的后轮。如图1所示，AGV前轮和后轮的等效虚拟中心分别位于前轴和后轴中心的位置，即Of和Or。系统中主要涉及到两个重要的坐标系，即全局坐标系(或世界坐标系)OXY和以车辆为中心的坐标系OfXvYv。坐标系OfXvYv的原点等效在前轮的虚拟中心，水平方向与车辆的运动方向重合，故该坐标系随着车辆的运动而不断变化。

[x(t),y(t)]表示前轴中心Of在全局坐标系中的位置，φ(t)表示t时刻车辆速度方向与全局坐标系水平方向之间的夹角。设R(t),ω(t) ,v(t)分别表示前轮的半径、旋转角速度以及地面线速度，并且满足v(t)=R(t)ω(t)。车辆不断运动的过程中，前轮和后轮分别进行圆周运动，且拥有相同的圆心P。为了维持该圆周运动的连续进行，车辆的前轮需要不断调整方向，γ(t)就是车辆需要转动的角度，称为转向角，vf(t)表示前轮的运动速度。

图1 AGV的运动学模型分析Fig 1 Kinematics model analysis of AGV

因此，车辆的运动学模型可以表示为

(1)

其中，B为车辆前轴和后轴之间的距离。

定义t时刻车辆的状态向量和控制向量分别为：x(t)=[x(t),y(t),φ(t),R(t)]T，u(t)=[ω(t),γ(t)]T，ΔT为时间间隔，则上式的运动模型可以表示成如下的标准形式

x(t+1)=f(x(t),u(t))

(2)

1.2 观测模型

AGV在行驶过程中，利用放置在车身上的超声波雷达以固定频率扫描周围的环境，假设雷达固定在后轴的中心Or处，并在环境中设置固定的观测特征目标，如导航灯塔。超声波雷达可以提供观测量主要有2个：r(t)和θ(t)。r(t)表示雷达与导航灯塔之间的距离，θ(t)表示导航灯塔相对于雷达之间的夹角。如果用向量z(t)表示雷达的观测量，则观测量以笛卡尔坐标的形式可以表示为

(3)

由于车辆的状态向量均表示为世界坐标系下的形式，为了状态向量和观测向量的匹配，需要将上面的观测向量转换成世界坐标系下的形式，具体方法如下

(4)

2 基于一致性检测的模型参数选择

2.1 一致性检测原理

一致性检测是基于χ2假设检验[8]评估贝叶斯估计器性能的一种方法。该方法主要依据χ2假设检验的概率原理，从贝叶斯估计的过程模型和观测模型出发，选择新息序列Yt并构造正则化新息平方(normalized innovation square,NIS)[9]。在估计器满足一致性的假设条件下，新息序列Yt服从均值为零的高斯分布，故正则化新息平方序列可以表示为

(5)

则qt服从χ2分布，其自由度等于观测向量的维度m，∑t表示新息序列的协方差矩阵。

(6)

(7)

2.2AGV的噪声参数选定

AGV的自主运动是一个动态估计的过程，需要车辆不断估计自身位置并及时调整从而实现精确定位和导航。卡尔曼滤波(Kalman filtering，KF)是一种可以估计动态系统状态的高效递归滤波器，其改进形式之一的扩展卡尔曼滤波器(EKF)[10]广泛应用于各种非线性系统。一致性检测方法中的新息就是EKF算法推导和表述过程中非常重要的概念。因此，本文使用EKF算法处理AGV的运动模型和观测模型，不过选定合适的模型噪声参数是算法实现过程中的关键点和难点，而2.1节提到的一致性检测方法则可以清晰地判断噪声参数的选择和设置是否合理。

2.2.1 过程噪声参数的选择

AGV系统中引入的误差主要通过添加噪声参数的形式体现。系统的误差来源主要有三个方面：车辆旋转角速度误差、转向角误差和车轮半径改变引入的误差。角速度和转向角误差与车辆运动时的跳动和侧滑有关，这部分以乘性噪声的方式体现，噪声参数的具体设置如下

R(t)=(t)+ΔTδR(t).

(8)

2.2.2 观测噪声参数的选择

(9)

3 仿真与结果

为了验证上节中选定的噪声参数的合理性，本节首先对集装箱码头环境下的AGV搬运车的自主运动过程进行了仿真和模拟。如图2所示，首先通过随机设置路径基点，拟合出一条基准路径，并在基准路径的周围设置不同的导航灯塔。车辆在运动过程中，激光雷达传感器以固定频率不断地检测周围环境中的导航灯塔，进而得到不同的雷达观测点，并由编码器记录下相应的观测量。为了实现AGV沿着基准路径的自主运动，利用当前时刻车辆的真实位置和实时观测量，通过EKF算法估算下一时刻车辆应该到达的估计位置，最终得到了车辆的估计路径。估计路径与基准路径以及车辆应走的真实路径之间有一定的误差，该误差主要来源于建立并仿真模型的过程中添加的噪声。

图2 AGV的自主运动Fig 2 Autonomious movement of AGV

过程噪声和观测噪声的大小设置会严重影响AGV估计路径的精度。为了使噪声参数与实际情况下的噪声严格匹配，进而得到与基准路径之间误差最小的估计路径，使用一致性检测方法验证EKF滤波器的一致性性能，并根据一致性的结果分别调整过程噪声参数和观测噪声参数的大小。

图3所示的结果中，正则化新息平方的均值都高于95%的置信区间，这说明噪声参数的设置太大，导致滤波器出现不一致，因而,要适当减小过程噪声或观测噪声的大小，使NIS的均值能落在置信区间内。与此相反，图4中的NIS均值则都低于95 %的置信区间，所以,需要增加模型中的噪声参数值。

图3 噪声设置太小Fig 3 Setting of noise is too small

图4 噪声设置太大Fig 4 Setting of noise is too large

当噪声参数的设置恰好匹配时，可以得到图5所示的结果，可以看出，随着时间的增加，NIS的均值逐渐收敛在置信区间限定的区域内，这说明此时的噪声设置不仅使滤波器具有良好的估计一致性，同时也说明此时AGV的估计路径误差最小，AGV可以实现沿着既定路径的自主导航运动。

图5 噪声设置合理Fig 5 Setting of noise is reasonable

4 结 论

本文主要实现了对搭载超声波雷达传感器的AGV的导航参数选择。在对AGV的动力学运动和激光雷达观测建立模型的基础上，选择合适的导航模型噪声参数反映系统中的各种误差，仿真实验中，通过一致性检测方法对滤波器一致性的校验设置噪声参数的大小。实验结果表明:选择的导航噪声参数可以满足AGV激光雷达导航方式下的自主导航运动，该方法可以适用于复杂的码头环境，且由于基准路线可以随意设置，具有很好的灵活性。

[1] Durrant-Whyte H F.An autonomous guided vehicle for cargo handling applications[J].The International Journal of Robotics Research,1996,15(5):407-440.

[2] 樊跃进,李重光,张智勇.韩国三星公司的电磁导引AGV[J].机器人技术与应用,1999(4):15-16.

[3] Lee J W,Choi S U,Lee C H,et al.A study for AGV steering control and identification using vision system[C]∥2001 Proceedings of IEEE International Symposium on Industrial Electronics,ISIE 2001,IEEE,2001:1575-1578.

[4] Nishide K,Hanawa M,Kondo T.Automatic position findings of vehicle by means of laser[C]∥Proceedings of 1986 IEEE International Conference on Robotics and Automation,IEEE,1986:1343-1348.

[5] Durrant-Whyte H,Nebot E,Scheding S,et al.The design of ultra-high integrity navigation systems for large autonomous vehicle-s[M].Berlin Heidelberg:Springer Experimental Robotics V,1998:252-261.

[6] Benedict R H,Nashif P J,Tracy S J.Hybrid ultrasonic and radar based backup aid:US 5,754,123[P].1998—05—19.

[7] Dissanayake M W M G,Newman P,Clark S,et al.A solution to the simultaneous localization and map building (SLAM) prob-lem[J].IEEE Transactions on Robotics and Automation,2001,17(3):229-241.

[8] 张智霞,刘瑞元.卡方分布与F分布判别的假设检验[J].高师理科学刊,2008,28(3):33-35.

[9] Ali-Löytty S,Sirola N,Piché R.Consistency of three Kalman filter extensions in hybrid navigation[C]∥Proceedings of The Euro-pean Navigation Conference on GNSS,2005:1049-1056.

[10] Durrant-Whyte H F.Introduction to estimation and the Kalman filter[J].Australian Centre for Field Robotics,2001,28(3):65-94.

Selection of AGV navigation parameters based on ultrasonic wave radar sensor*

ZHANG Wen1， SUN Zuo-lei1， ZENG Lian-sun1， ZHANG Bo2

(1.College of Information Engineering, Shanghai Maritime University,Shanghai 201306,China;2.Shanghai Advanced Research Institute,Chinese Academy of Sciences,Shanghai 201210,China)

Aiming at poor flexibility and low precision of navigation mode in autonomous guided vehicle(AGV)system,propose employing ultrasonic wave radar sensor to realize autonomous navigation of AGV is proposed,and also set up reasonable parameters of navigation models.Kinematics model of AGV and observation model of sensor are established respectively,by adding multiplicative or addictive noise to process and observation models based on principle of consistency test,in order to reflect real system errors.Simulation results demonstrate that autonomous navigation movement of AGV along standard routine can be achieved by setting reasonable noise parameters.

ultrasonic wave radar sensor; AGV; consistency test; normalized innovation square

10.13873/J.1000—9787(2014)10—0034—04

2014—04—08

国家自然科学基金资助项目(61105097，51279098)；上海市教育委员会科研创新项目(13YZ081)

TP 79

A

1000—9787(2014)12—0034—04

张 文(1989-)，女，山东菏泽人，硕士研究生，目前研究方向为多传感器数据融合。