双舵轮泊车机器人运动学分析及定位系统研究

钟浩翔, 陈亚, 王殿君, 朱亚东, 焦向东, 高易佳

(北京石油化工学院 机械工程学院,北京 102600)

泊车机器人是一种重载移动机器人,是立体车库的关键执行设备[1-2]。在进行泊车作业时,若机器人位姿信息出现偏差会导致汽车无法被搬运到指定位置,甚至使用户财产安全受到威胁[3-4]。

目前移动机器人室内定位常用的方法有SLAM、视觉里程计、RFID和惯性单元等[5-8]。其中惯性单元因其对环境依赖性低、低成本等优点成为移动机器人室内定位较常采用的方法,但零点漂移是惯性单元的主要问题,导致无法长时间独立定位[9-10]。陈映秋等将惯性单元固定于车轮中央,利用惯性单元与车轮一起旋转的运动特性,提取出运动速度信息,从惯导安装方式和算法设计层面实现了惯性单元的独立导航[11],但惯导单元零点漂移误差无法消除,使得该定位方法无法保证定位系统精度。刘艳娇等将惯性单元与视觉传感器进行结合,将惯性单元数据紧密关联到图像跟踪过程,提供精确的短期运动约束和较好的初始梯度方向信息,有效地解决了零漂问题[12],但泊车机器人工作环境属于无人环境,光线不足问题使得视觉传感器并不适用。Wang等提出了一种利用RFID和惯性单元进行室内定位的方法[13],估计轨迹主要由RFID确定,在RFID信号不稳定时采用惯性单元进行定位,但由于基于RFID的定位方法定位误差较大,且车库中包括无线通讯设备在内的电子设备会严重影响基于RFID的定位系统精度。因此需要对泊车机器人的定位方法进行进一步研究。

首先对双舵轮泊车机器人进行了运动学分析,设计了一种融合惯导、里程计、运动学航位推算数据的定位算法,建立了泊车机器人定位系统,并进行了机器人定位试验。

1 泊车机器人运动学模型

泊车机器人轮系布局如图1所示。该轮系结构由对角布置的舵轮、两个从动万向轮以及中部两个弹簧减震轮组成,每个舵轮能够独立进行驱动和转向。

图1 泊车机器人轮系结构Fig.1 Wheel structure of a parking robot

以惯性坐标系N(X,O,Y)为基准,建立车体坐标系B(x,o,y)、舵轮坐标系D(xo,o,yo),如图2所示。

图2 曲线运动位姿状态Fig.2 Pose status of curved motion

设左前方舵轮的速度为Va,与xo轴正方向夹角为θa,右后方舵轮的速度为Vb,与xo轴正方向夹角为θb,车体中心速度为Vo,与xo轴正方向夹角为θ,Vox和Voy为Vo在xo、yo方向速度分量。φ为车体坐标系与惯性坐标系X轴夹角。L为两舵轮沿y方向的前后距离,W为两舵轮沿x方向的左右间距,α为定值,表达式为

α=arctan(W/L)

(1)

为了使机器人两舵轮间不产生内应力,双舵轮运动模型满足如下约束条件

Vasinθa=Vbsinθb

(2)

根据两舵轮实际速度及方向角可推导泊车机器人中心位置的速度大小为

(3)

机器人中心速度Vo与xo方向的夹角θ为

(4)

机器人在平面内绕某一点做圆周运动,根据舵轮的速度,可确定其旋转半径R及机器人角速度ω:

(5)

(6)

综合以上推导,设泊车机器人初始位姿为(x0,y0,φ0),单个控制周期时间为t,则机器人于惯性坐标系位姿可表示为:

(7)

2 多传感器组合定位模块

定位功能是泊车机器人的核心功能,是机器人完成存/取车任务的前提。泊车机器人要求定位系统对车库改造小,独立工作能力强,且定位精度达到±25 mm[14]。本文提出的定位模块将惯导与里程计组合进行位姿推算,并利用卡尔曼滤波技术[15],将其与运动学航位推算结果进行数据融合,从而提高定位精度。最终结合二维码的绝对位姿信息对定位结果进行定期校正。

(8)

式中:c=cos,s=sin。

设Δt时间内两个舵轮的转向角度保持不变,前后舵轮运动轨迹近似为短直线,则两舵轮移动里程分别为ΔS1、ΔS2,进而可求得车体中心移动里程ΔS为

(9)

车体中心于导航坐标系X方向、Y方向的移动里程ΔSX和ΔSY可表示为

(10)

设第i时刻,泊车机器人的位置为[X(i),Y(i),Z(i)],第i+1时刻,惯性单元测量的姿态角为[θ(i+1),φ(i+1),φ(i+1)],从第i时刻到第i+1时刻泊车机器人几何中心移动里程为ΔS(i+1),则第i+1时刻,泊车机器人的位置[X(i+1),Y(i+1),Z(i+1)]为

(11)

工控机可接收PMAC运动控制卡获取的相应电机编码器数据及惯导数据,换算后得出前、后舵轮移动里程ΔS1、ΔS2,舵轮方向θ1、θ2及机器人姿态角(θ,φ,φ),对数据进行上述计算,可得出基于惯导/里程计信息推算的机器人位姿。工控机同样可以通过提取的前、后舵轮的速度(V1,V2)和角度(θ1,θ2)进行基于运动学的航位推算,然后采用卡尔曼滤波的数据融合方法将两种定位结果进行融合,充分利用短时间内惯性单元的高精度航向角检测和里程计的准确位置推算的特点,结合运动学推算位姿实时性较强的优势,得到机器人的最优位姿。

车库中以一定距离间隔布置二维码标签,它们包含的信息包括自身ID号以及当前二维码传感器中心与标签中心的偏移量(ΔX,ΔY,Δθ),其中ID号与导航坐标系中坐标(X,Y)绑定,则扫描二维码能够提取到机器人的位姿(X-ΔX,Y-ΔY,Δθ),利用该数据对数据融合结果进行校正,达到定期清除累计误差的目的。定位模块软件流程如图3所示。

图3 导航定位模块软件流程Fig.3 Software flow of navigation and positioning module

3 定位实验

3.1 定位系统

利用团队自主研发的双舵轮泊车机器人样机对定位模块进行实验验证,机器人样机如图4所示。

图4 双舵轮泊车机器人样机Fig.4 Prototype of the double steering-wheel parking robot

样机采用研华PPC-6120作为上位机,泰道公司Clipper运动控制卡作为下位机,构成开放式控制器硬件系统,保证机器人对控制信号的实时响应。实验场地为平坦水泥路面,令泊车机器人直行6.65 m,横移2.5 m,倒退6.65 m,模拟从车厅取车后存入车位的运动过程。误差标定设备采用美国FARO公司的激光跟踪仪,将激光靶球置于泊车机器人中心,实时记录机器人在导航坐标系中的坐标位置。实验环境如图5所示。

3.2 定位结果

设定泊车机器人移动速度为0.3 m/s,分别应用运动学航位推算定位方法和多传感器组合定位方法进行了泊车机器人定位试验。采用运动学航位推算定位方法得到的定位结果如图6和表1所示,采用多传感器组合定位方法得到的定位结果如图7和表2所示。

表1 运动学航位推算定位方法实验数据Tab.1 Experimental data of kinematic dead reckoning positioning method

表2 多传感器组合定位方法实验数据Tab.2 Experimental data of multi-sensor combined positioning method

图6 运动学航位推算定位方法实验结果Fig.6 Experimental results on kinematic dead reckoning positioning method

图7 多传感器组合定位方法实验结果Fig.7 Experimental results on multi-sensor combined positioning method

3.3 对比分析

从表2可知,采用运动学航位推算定位方法的泊车机器人平均定位偏差为176.46 mm,从表3可知,同样实验条件下,采用多传感器组合定位方法的泊车机器人平均定位偏差为14.04 mm,相比前者降低了92%,定位精度有了较大提高,满足泊车机器人定位精度±25 mm的技术要求。

4 结论

1) 建立了双舵轮泊车机器人的运动学模型,提出了融合惯导、里程计、运动学航位推算的多传感器组合导航定位方法。

2) 定位试验结果表明,采用本文提出的多传感器组合定位方法的泊车机器人定位精度明显提高,能够满足泊车机器人技术要求。

3) 所构建的泊车机器人定位系统为机器人存取车功能研究奠定了基础。