基于Hermite曲线的智能叉车过渡轨迹规划和跟踪

经过 十-五”期间的大力发展，我国石化工业重大装备的研制能力和创新水平大大提高。

在石化工业的生产能力进-步提高的同时，石化成品的储运成为石化效益链上不可忽视的重要环节。随着 自动化技术的应用和发展，仓储物流系统正向着柔性化和智能化方向发展。图 1所示的燃油叉车已不能满足石化产品储运系统 Et益发展的需要，也不能满足石化和化学工业 十二五”发展规划中的绿色发展、清洁生产的基本原则 。

智能输送设备的运用将是石化行业仓储物流系统图 1 成品仓库《起重运输机械》 2013(3)向着柔性化和智能化方向发展的必然产物。智能输送设备不仅作业迅速、准确，可缩短作业时间，而且可以节省劳动力，节约 占地，有助于提高储运系统管理水平和商品物流的运作。许多专家对物流系统中智能输送设备的应用和发展进行了论述 ，肯定了智能输送设备在仓储物流系统中的作用 -3]。图2所示的智能叉车就是-种智能输送设备 ，它由动平台和升降装置组成，其动平台又常被称为轮式 AGV。由于智能叉车不需要在地面铺设导航用的磁条和铁轨，因此其运动轨迹是柔性的，这就大大提高了仓储物流系统的适应性，降低了生产成本，成为各大高校和研究所研究的热点4 。文献 [7]对智能叉车的运动控制进行了仔细的研究 ，其研发的动平台运动轨迹由直线和圆弧组成，在直线和圆弧的切点会出现曲率的突变，导致智能叉车驱动轮速度不连续，并且在跟踪轨迹时会出现 比较大的位姿波动，严重影响了智能叉车的运动平稳性。本文就智能叉车运动轨迹段的过渡问题进行探讨和研究，首先介绍轨迹- 85 - 跟踪的运动控制算法，然后进行过渡段轨迹 的规划，并通过试验验证了过渡轨迹有助于提高智能叉车的运动平稳性。

图2 智能叉车1 过渡轨迹的规划与跟踪算法1.1 轨迹跟踪的运动控制算法智能叉车为了完成作业任务，需要跟踪 已经规划好的轨迹。差速式智能叉车通过左右驱动轮的差速控制实现轨迹跟踪，轨迹跟踪的控制算法必须紧密结合运动控制器展开，本文结合 PMAC运动控制卡的特点讨论物流搬运机器人的运动控制算法。

如图3所示，在智能叉车的工作环境中建立大地坐标系∑G，在轨迹段的起始点建立轨迹段坐标系∑P，在智能叉车上建立车体坐标系∑A。用向量 [ Y ]来描述∑A在某-坐标系下的位姿，、 y表示车体坐标系∑4原点在某坐标系下的坐标，0表示智能叉车的方向，即车体坐标系∑A的轴与某坐标系 轴的夹角。设智能叉车在某-段时间内跟踪已规划好的轨迹 Y厂(X)，轨迹段坐标系∑P在大地坐标系∑G下的位姿为 [ 。y口]T，在某-时刻t，智能叉车坐标系∑A在大地坐标系∑G下的位姿为 [ ，0r，在轨迹段坐标系∑P下的位姿为 [ Ya ]T o为 了跟踪设定轨迹，需要智能叉车在 t1时刻运动到 ( )点，控制过程中采取在大地坐标系∑G下进行跟踪和在智能叉车坐标系∑A下控制的原则。

为了控制方便，对于多段运动轨迹情况，需要将智能叉车位姿由大地坐标系∑G变换到当前跟踪的轨迹段坐标系∑P下：FPXA1 r ]I l 。 I (1) l1 j j口 -OL (2)- 86 - oc； X， XA xG图3 轨迹跟踪原理图fCOSOL -sina ]； - I sin c。s l槲 ㈩Ar：T～l si co YA IL 0 0 1 J (4)v， ro[ R-K (Ay暗s)㈩ R 2I y I式 (4)和式 (5)是智能叉车在轨迹跟踪时的理论控制量，在实际控制时，受 电机、运动控制器等能力的限制，需要对左右驱动轮的速度进行修正，修正后的回转半径㈤ 式中： 、 分别为左右驱动轮修正后 的速度， 为两驱动轮间距。

经修正速度控制的智能叉车在 t1时刻的位姿为XB，R sinoYB，K( ).R E 1c0s ] (7)ATm将 t1时刻智能叉车在 点的位姿变换到路径坐标系∑P下，然后采用模型式 (3)-式 (7)的算法，继续循环控制智能叉车跟踪已规划的轨迹。轨迹跟踪的算法流程见图4。

1.2 过渡轨迹的规划方法由于 Hermite插值多项式可以根据相邻两点的位姿规划出-条曲率连续的过渡轨迹。因此，本文采用 Hermite插值曲线实现两段轨迹的曲率连续性过渡，这样既发挥了直线和圆弧轨迹的优势，又保证了运动的平稳性。

假设在路径坐标系下，过渡段轨迹始末点的位姿分别为 [ rs ] 和 [ ，E ] ，采用三次Hermite插值法规划出-条过渡轨迹。

2H3( 2)ta E由Hermite多项式可以得到：《起重运输机械》 2013(3)跟踪下- - 轨迹段N开始变换到当前轨迹段坐标系计算 1时刻的目标点将目标点的信息变换到t时刻动平台坐标系中计算运动控制量> 二 L 而 。

图4 轨迹跟踪流程图 ㈤[ -2 (丢 ( x-x2)( )。 z(10)式中的轨迹点 ( ，Y)就是在运动控制过程中智能叉车不断跟踪的目标点，其中： 1ax(/3) (11)式中的 (卢)是-个关于智能叉车姿态角的函数。

根据式 (10)、式 (11)计算 目标点，采用式(3)-式 (7)的跟踪算法，进行智能叉车的过渡段轨迹规划和轨迹跟踪试验。

2 过渡轨迹规划及跟踪试验过渡轨迹的规划和跟踪是在样机 XAUTAGV-100上进行的，样机传感器的分布如图5所示。如图6，假定智能叉车根据作业任务需要从-段直线轨迹 cA运动到另-段直线轨迹 BD上，位置 A、B分别为两相交直线段轨迹的末点和始点。智能叉车先作直线运动，运动到直线段轨迹末点A处，开始进行轨迹过渡，在过渡段轨迹的末点 处开始另-段直线轨迹BD的运动。智能叉车在 点的位姿分别由侧向的2个模拟量超声波和2个驱动轮的编码器确定：- 87 - - N - 髫 ，模超图5 传感器的分布位置开关量超声波Ⅺ 、 B/- ] - ～ 皂 J2iCyG j图 6 试验示意图同理可以得到智能叉车在 B点的位姿描述。

下面根据大地坐标系下 点的测量位姿和 B点的设定位姿进行过渡段轨迹规划和跟踪。

试验开始前 ，根据左前、左后超声波的测量值确定物流搬运机器人开始运动时在大地坐标系∑G下的初始位姿，在运动程序中控制智能叉车动平台的运动速度为0.1 m/s、加速度为 0.25 m/s ，A点的位姿由式 (12)确定，过渡段末点B的位姿为 [5 000，-1 200，-3O] 。试验嘲如图7所示，试验过程中通过上位机采集左右驱动轮编码器的反馈值，取其中-组试验数据绘制图8，忽略采样过程中噪声干扰。从图8中可以看出，左右驱动轮速度变化平缓，没有阶跃现象，说明运动较平稳。

图7 过渡轨迹的规划与跟踪图8 过渡段驱动轮的速度3 结束语针对智能叉车轨迹跟踪的特点，讨论了智能叉车在实际应用中的过渡轨迹规划和跟踪问题。

就直线段和圆弧段轨鉴行平滑过渡的问题进行了研究，介绍了智能叉车的轨迹跟踪算法 ，对智能叉车相邻两轨稼点进行过渡段轨迹规划，并通过试验验证了该方法的可行性，对致力于智能叉车研究的同行有-定的借鉴作用。