自动引导小车(automated guided vehicle,AGV)作为一种柔性智能搬运设备,在制造车间、快递物流等场所应用越来越广泛,对中国制造业带来重要影响^[1]。AGV常见的导航方式有视觉导航、激光导航、磁条导航、惯性导航等。近年由于机器视觉技术的发展,于亦奇等^[2-3]提出一种视觉导航,它可以完成粗略的导航定位,但很难满足现实需求。导航精度能满足实际需求的有车菲^[4]提出的激光导航和刘娟^[5]、李玉琪^[6]提出的磁条导航,但它们很难符合目前所提倡的柔性制造。在生产制造中我们既要导航精度也要轨迹柔性,虽然惯性导航兼顾了两者,但它存在无法消除的累积误差,杨玉明^[7]针对惯性导航存在的累积误差对AGV机械机构进行改进和导航算法优化,但惯性导航的定位精度在很大程度上取决于惯性元件。为了在短距离室内导航中能使AGV定位达到较高精度且成本较低,本文展开了基于卡尔曼滤波(Kalman filter)^[8]的惯性导航研究,将惯性元件解算得到AGV的姿势进行卡尔曼最优估计,得到最优姿势,再通过PID控制(proportional-integral-derivative control,比例-积分-微分控制)进行姿势调节,并且引入偏置量弥补系统存在的未知偏置误差。

图1 AGV轨迹误差仿真
Fig.1 AGV trajectory error simulationAGV

在直线行驶过程中存在累积误差、过程噪声和观测噪声,这些都对AGV的导航产生一定的影响。传统控制方式的AGV在直线运动过程中轨迹误差累积较缓慢,但转向处误差出现突变,由于传统控制方式无法消除累积产生的误差,因此,随着AGV继续运行,误差越来越大。AGV轨迹误差仿真结果如图1所示。

经过Δt时间后,AGV角度误差ω_e和轨迹误差d_e^[9]可表示为

{ω_e=∫^t₀Δωdt=∫^t₀(V_R-V_L)/Ldt;

d_e=∫^t₀Δddt=∫^t₀(V_R+V_L)/2sinω_edt。(1)

式(1)中:Δω为角度误差增量; V_L、V_R为左右轮线速度; L为两轮之间距离; Δd为轨迹误差增量。

根据图1和式(1)可知,当转角存在误差时,转角误差对后续轨迹运动影响较大,基于此,开展基于卡尔曼滤波定位及转角滤波研究,对AGV姿势进行实时最优估计并及时调整姿势。

图2 AGV运动模型
Fig.2 AGV motion model2 AGV

定位纠偏控制模型

AGV在行驶过程中,由卡尔曼滤波算法对AGV姿势做出最优估计,从而得出最优姿势,并发送给控制器,控制器通过脉冲宽度调制(pulse width modulation,PWM)控制电机驱动器,从而控制电机调整AGV姿势。本研究建立如图2所示的AGV运动模型。

AGV在Q点坐标为(x,y),转角为θ,因此Q点姿势Q可以表示为[x,y,θ]^T。AGV采用差速轮驱动,左右两轮线速度V_L、V_R可以由电机转速n_R、n_L表示为

{V_L=n_L×2πr;

V_R=n_R×2πr。(2)

AGV的线速度V和角速度ω定义如下:

{V=(V_L+V_R)/2;

ω=(V_L-V_R)/L。(3)

AGV姿势Q离散化后表示为

Q=[x(k+1)

y(k+1)

θ(k+1)]=[1 T 0 0 0 0

0 0 1 T 0 0

0 0 0 0 1 T][x(k)

V(k)cos(θ(k))

y(k)

V(k)sin(θ(k))

θ(k)

ω(k)]+[0.5T ² 0 0

0 0.5T ² 0

0 0 0.5T ²][u_x(k)

u_y(k)

u_θ(k)]。(4)

式(2)～(4)中:r为轮子半径; T为采用周期; u_x(k)、u_y(k)、u_θ(k)分别为x、y、θ的控制量。

由于惯性元件的误差会随时间变化,严重影响惯性导航在AGV上的应用发展,因此,我们需要分析、估计误差。惯性元件主要由陀螺仪和加速度计组成,陀螺仪是一种角运动元件,它的准确性对AGV的转角产生直接的影响。陀螺仪的误差主要由刻度系数误差和漂移组成,刻度系数是一个随机常数,可通过标定消除,因此,陀螺仪误差^[10]333、加速度计误差^[10]334(主要包括偏置误差和随机误差)可以表示为

{ε_i(t)=ε_bi(t)+ε_ri(t)+ω_gi(t);

β_i=β_bi+ω_ai;

i=x,y,z。(5)

式(5)中:ε_bi(t)为逐次启动漂移; ε_ri(t)为慢变漂移(可忽略); ω_gi(t)为陀螺仪白噪音; β_bi为偏置误差; ω_ai为加速度计白噪音。为了简化误差模型可以将惯性元件误差看成白噪音和偏置误差组成。

为了消除惯性元件的偏置误差,在观测上引入偏置,AGV在动态过程用一般隐马尔科夫模型(hidden Markov model)表示出系统状态A和观测值Z:

{A(k+1)=FA(k)+Γ W(k);

Z(k)=HA(k)+V(k)+b(k)。(6)

式(6)中:F为状态转移矩阵; Γ为噪声驱动矩阵; W为过程噪声; H为观测矩阵; V(k)为观测噪声; b(k)为测量偏置。

在观测方程中引入测量偏置并利用逆威沙特(normal-inverse-Wishart)分布拟合估计出偏置值^[11]。卡尔曼滤波算法主要包括预测和更新两个过程,预测根据上一次状态和控制量进行预测,更新根据本次状态和传感器数据进行高斯混合(Gaussian mixture)得到最优数据。卡尔曼滤波式子^[12]36如下:

{A(k+1|k)=FA(k|k)+Bu;

P(k+1|k)=FP(k|k)F^T+Γ QΓ^T;

K(k+1)=P(k+1|k)H^T(HP(k+1|k)H^T+R)^-1;

A(k+1|k+1)=A(k+1|k)+K(k+1)(Z(k+1)-HA(k+1|k));

P(k+1|k+1)=P(k+1|k)-K(k+1)HP(k+1|k)。(7)

式(7)中:A(k+1|k)为状态预测; B为系数矩阵; u为偏置; Q为过程噪声方差; K为卡尔曼增益(Kalman gain); P(k+1|k)为协方差预测; R为测量噪声方差; A(k+1|k+1)为状态更新; P(k+1|k+1)为协方差更新。

最后将经过卡尔曼滤波的角速度进行四元数解算,可以将翻滚角φ、俯仰角θ、偏航角ψ^[13]用四元数表示如下:

{φ=arctan((2(q₂q₃+q₀q₁))/(q²₀-q²₁-q²₂+q²₃));

θ=arcsin(-2(q₁q₃-q₀q₂));

ψ=arctan((2(q₁q₂+q₀q₃))/(q²₀+q²₁-q²₂-q²₃))。(8)

通过一阶龙格库塔法(Runge-Kutta)迭代,可以将惯性元件测得的角速度来表示四元数,从而求得欧拉角(Euler angle):

{q₀=q₀+1/2Δt(-ω_xq₁-ω_yq₂-ω_zq₃);

q₁=q₁+1/2Δt(ω_xq₀-ω_yq₃+ω_zq₂);

q₂=q₂+1/2Δt(ω_xq₃+ω_yq₀-ω_zq₁);

q₃=q₃+1/2Δt(-ω_xq₂+ω_yq₁+ω_zq₀)。(9)

式(8)～(9)中:q₀、q₁、q₂、q₃为四元数; ω_x、ω_y、ω_z分别为三个方向的角速度。

为了便于观察AGV的定位误差,在MATLAB软件中进行直线行走和转弯的仿真。图3为卡尔曼位置观测及误差,图4为卡尔曼角度观测及误差。

图3 卡尔曼位置观测及误差
Fig.3 Kalman positioning observation and error

图4 卡尔曼角度观测及误差
Fig.4 Kalman angle observation and error chart

通过MATLAB中的Simulink建立AGV纠偏控制模型,如图5所示。模型主要由AGV控制器模块、直流无刷电机模块^[14]、姿势解算模块、卡尔曼滤波器模块^[12]160和其他模块组成,并且电机控制采用MATLAB中的power_brushlessDCmotor模块为基本单元。AGV在运行过程中,卡尔曼滤波器采集惯性元件的数据进行最优估计,然后将最优姿势与控制器的目标姿势进行比对得到偏差,最后通过PID控制器调整AGV姿势。

图5 AGV纠偏控制模型
Fig.5 Control model of AGV correction

AGV小车先匀速直线行驶,然后进行90°转弯。模拟直线行驶距离为10 m,转弯后再行驶3 m。AGV行驶速度、方向角初始值都为0。模拟运动局部轨迹如图6所示,其行驶轨迹误差如图7所示。

图6 模拟运动局部轨迹
Fig.6 Local trajectory of simulation motion

图7 AGV轨迹误差
Fig.7 AGV track error

通过两种控制方式的仿真比较,经过计算得出,在短距离内卡尔曼滤波定位惯性导航方式的轨迹均值误差为0.000 58 m,而传统推算法为0.004 69 m,前者导航精度比后者提高了8倍左右。

AGV在运动过程中由于两轮转速存在偏差,需要不断调整自身姿势。由于卡尔曼滤波算法具有较好的实时性,因此,对于控制量频繁改变的AGV具有较好的适应性。

卡尔曼滤波定位惯性导航方式控制简单、实用,在一般场所中能满足所需的精度和稳定性要求。本文通过在惯性导航中加入带偏置的卡尔曼滤波进行轨迹预测和角度滤波,提高了惯性导航的室内定位导航能力,通过Simulink仿真验证了这一方法的有效性,从而为AGV的定位导航问题提供了一定的理论参考。