随着工业自动化、智能化进程的推进，自动引导车(automated guided vehicle, AGV)逐渐成为物流行业、智能工厂及柔性制造车间“物料流”的重要支撑装备，可代替人工完成对各类货物的搬运工作，节约人力成本，提升工作效率^[1-3]。路径跟踪是AGV叉车运动控制的关键技术，其精度对于AGV叉车的工作范围扩展和效率提升具有重要作用^[4]，高精度路径跟踪的前提是AGV叉车定位的高准确性。

目前，AGV叉车的导航定位有惯性导航、磁导航、视觉导航及激光雷达导航等多种方法^[5]。惯性导航是在AGV叉车上安装陀螺仪，获取其三轴角速度和加速度，通过积分运算对AGV叉车进行导航定位。该方法成本低，短时间内精度高，但因累积误差的存在，导致长时间定位的精度较差。通常将该方法与其他导航方法融合^[6]，以获得较好的定位效果。磁导航包括电磁导航和磁条导航等，应用时间较早、技术成熟且定位精确^[7-8]，但需要在其行驶路径上铺设金属线、磁条等，对地面或环境进行较大的改造，难以满足柔性运输要求。视觉导航可分为通过视觉识别色带、二维码等标志物来实现^[9-11]，以及不依赖标志物而采用视觉同时定位与地图构建(simultaneous localization and mapping, SLAM)技术实现^[12-13]两类，相比磁导航路径设置简单且改变或扩充路径也较容易，灵活性较好，成本较低。但视觉导航的定位精度较低，且对光干扰较为敏感，仍然需要进一步深入研究。

相比视觉导航，激光导航成本高，但定位精度也高，行驶路径可灵活改变，并可在无光环境下运行，因而受到了广泛关注。基于反光柱的激光雷达定位方法适用于已知环境中AGV叉车的定位，通过激光雷达发射激光束扫描周围环境，并收集反光柱反射的激光束信息，基于匹配算法进一步求解AGV叉车的位置^[14]。针对错误检测反光柱的问题，Ronzoni等^[15]提出了一种基于地标识别的AGV自定位算法，即使存在异常值也能在0.3 s内得出正确定位结果。Luo等^[16]针对全局特征图中反光柱数量多的问题，提出了一种减少反光柱数量的AGV定位算法，既提高了定位精度，又将反光柱数量从3个及以上减少到2个。为进一步提高激光雷达定位算法的灵活性和适应性，文[17-19]采用SLAM技术实现AGV叉车在基于自然环境、无反光柱条件下的导航定位，但该方法的定位精度比基于反光柱的定位方法低^[20]。激光雷达一般安装在AGV叉车顶部，但由于AGV叉车运行过程中频繁加减速或路面不平整，会导致激光雷达在测量过程中出现较为明显的测量噪声。为了解决该问题，文[21-22]开展了激光雷达与其他传感器的融合定位方法研究，以提高定位精度。

在取得AGV叉车在全局坐标系的位置及航向以后，通过合理地控制AGV叉车的运动速度和方向减小路径跟踪误差，可以确保其精确地沿着目标路径行驶^[23]。由于AGV叉车属于欠驱动系统，对路径跟踪算法的效率和鲁棒性的要求较高。常见的路径跟踪控制方法有纯跟踪法、比例积分微分(PID)控制算法、模糊控制算法、模型预测控制(model predictive control, MPC)算法和线性二次型调节器(linear quadratic regulator, LQR)等^[24]。刘一帆等^[25]利用模糊控制算法确定预瞄距离，对纯跟踪法进行改进，提高了对路径的适应能力。李航等^[26]提出一种基于模糊PID的控制方法，优化了不同载荷与速度条件下的路径跟踪控制。Mohammadi等^[27]采用基于MPC算法的跟踪算法，集成了时间最优运动规划与控制，实现了AGV叉车的路径跟踪。Kokot等^[28]提出了一种统一的MPC设计方法，可以统一用于包括三轮车、差速器和双转向驱动在内的不同类型的AGV运动学装置，并实现了较好的路径跟踪精度。而Gu等^[29]在LQR控制器中引入AGV横向误差积分，并利用遗传算法实现LQR控制器的优化，降低了系统超调量并提高了AGV路径跟踪精度。相比纯跟踪法和PID控制算法，MPC算法和LQR计算量较大，对硬件要求高，因此在实际应用中还存在诸多限制。纯跟踪法的跟踪精度主要依赖预瞄距离，难以获得最优值^[30]，而PID控制算法操作简单，通过对路径跟踪误差进行反馈控制就能实现稳定的路径跟踪。

综上所述，本文针对已知室内结构化环境下的单舵轮AGV叉车，开展激光雷达定位与路径跟踪方法研究。对于定位问题，采用基于反光柱的激光雷达定位方法，利用Kalman滤波多传感器融合定位算法，提高AGV叉车的定位精度。对于路径跟踪问题，采用比例积分(PI)控制方法对AGV叉车的直线和圆弧路径展开研究，考虑直线路径和圆弧路径衔接处曲率突变问题对跟踪精度的影响^[31]，使用Bézier曲线对圆弧路径进行优化。通过实验验证了所提出的定位和路径跟踪方法的有效性。

1 单舵轮AGV叉车运动学建模 1.1 单舵轮AGV叉车运动学方程

本文研究的对象是单舵轮AGV叉车，其运动系统主要由一个主动舵轮(简称舵轮)和多个从动轮构成，典型结构如图 1所示。舵轮负责驱动和转向，一般位于AGV叉车的中心轴线上；而从动轮均布在中心轴线两侧，主要起支撑和提高运动稳定性的作用^[31]。

以AGV叉车的运动平面为空间Decartes坐标系的XOY平面，Z轴垂直于XOY平面，正方向竖直向上。令P_h为舵轮摆动轴线与AGV叉车中心轴线的交点，P_b为从动轮轴线与AGV叉车中心轴线的交点，P_h和P_b之间的距离即为舵轮与从动轮轴线中心点的距离l，AGV叉车中心轴线与X轴之间的夹角φ∈[－π, π]，同时φ也是AGV叉车的航向角。将P_b作为单舵轮AGV叉车路径跟踪的控制点，此时AGV叉车的位姿坐标记为(x, y, φ)，则AGV叉车的运动速度为

$ \left\{\begin{array}{l} \dot{x}=v_x, \\ \dot{y}=v_y, \\ \dot{\varphi}=w . \end{array}\right. $

(1)

其中：v_x和v_y分别为AGV叉车沿X和Y轴方向的移动速度，w为AGV叉车绕Z轴的角速度。

将AGV叉车舵轮驱动速度记为v_c，舵轮转角记为θ，转角方向定义为左转为正、右转为负，则AGV叉车沿X和Y轴方向的移动速度为

$ \left\{\begin{array}{l} v_x=v_c \cos (\varphi+\theta), \\ v_y=v_c \sin (\varphi+\theta) . \end{array}\right. $

(2)

AGV叉车的加速度a沿X轴和Y轴方向的分量a_x和a_y为

$ \left\{\begin{array}{l} a_x=\dot{v}_x, \\ a_x=\dot{v}_y . \end{array}\right. $

(3)

设O_c为AGV叉车的转动圆心，R_c为AGV叉车的瞬时转弯半径，则可得

$ R_{\mathrm{c}}=\frac{l}{\tan \theta} \text {. } $

(4)

1.2 单舵轮AGV叉车状态空间方程

以P_b为参考点，单舵轮AGV叉车的状态可用位置(x, y)、速度(v_x, v_y)以及航向角φ表示，则k时刻的AGV叉车的状态q_k可表示为

$ \boldsymbol{q}_k=\left[x_k, y_k, v_{x, k}, v_{y, k}, \varphi_k\right]^{\mathrm{T}} . $

(5)

其中，x_k和y_k分别为k时刻AGV叉车在X和Y轴的坐标值，v_{x, k}和v_{y, k}分别为k时刻AGV叉车沿X和Y轴行驶的速度，φ_k为k时刻AGV叉车的航向角。

将单舵轮AGV叉车的连续运动学方程离散化，可得

$ \left\{\begin{array}{l} v_{x, k}=v_{x, k-1}+a_{x, k} \Delta t, \\ v_{y, k}=v_{y, k-1}+a_{y, k} \Delta t . \end{array}\right. $

(6)

其中：a_{x, k}和a_{y, k}分别为k时刻AGV叉车沿X和Y轴方向的加速度分量，Δt为k与(k-1)时刻的时间间隔。

$ \varphi_k=\varphi_{k-1}+w_k \Delta t . $

(7)

其中w_k为k时刻AGV叉车绕Z轴的角速度。

$ \left\{\begin{array}{l} x_k=x_{k-1}+v_{x, k-1} \Delta t+\frac{1}{2} a_{x, k} \Delta t^2, \\ y_k=y_{k-1}+v_{y, k-1} \Delta t+\frac{1}{2} a_{y, k} \Delta t^2 . \end{array}\right. $

(8)

联立式(6)、(7)和(8)，即可得单舵轮AGV叉车的状态空间方程：

$ \boldsymbol{q}_k=\boldsymbol{A} \boldsymbol{q}_{k-1}+\boldsymbol{B} \boldsymbol{u}_k, $

$ \boldsymbol{q}_k=\left[\begin{array}{c} x_k \\ y_k \\ v_{x, k} \\ v_{y, k} \\ \varphi_k \end{array}\right], $

$\begin{aligned} \boldsymbol{A} & =\left[\begin{array}{ccccc} 1 & 0 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & \Delta t & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right], \\ \boldsymbol{B} & =\left[\begin{array}{ccc} \frac{1}{2} \Delta t^2 & 0 & 0 \\ 0 & \frac{1}{2} \Delta t^2 & 0 \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \end{array}\right], \end{aligned} $

$\boldsymbol{u}_k=\left[\begin{array}{c} a_{x, k} \\ a_{y, k} \\ w_k \end{array}\right] . $

(9)

其中：参数矩阵 $\boldsymbol{A} \in \mathbb{R}^{5 \times 5}, \boldsymbol{B} \in \mathbb{R}^{5 \times 3} $；AGV叉车在k时刻的控制向量 $ \boldsymbol{u}_k \in \mathbb{R}^{3 \times 1}$。

2 AGV叉车室内精准定位

针对工厂等室内结构化环境，在AGV叉车的工作范围内按一定原则布设一定数量的反光柱，并利用全站仪测量每一个反光柱相对全局坐标系的坐标。将激光雷达安装于AGV叉车顶部，且位于舵轮的正上方。通过设置合适的阈值对数据进行预处理，以过滤非反光柱反射的激光雷达点云数据。

在经过预处理后，基于密度的有噪声空间聚类(density-based spatial clustering of applications with noise，DBSCAN)算法，对激光雷达点云数据进行聚类得到点云聚类中心。之后，利用快速迭代最近点(fast iterative closest point, FICP)算法将其与原有地图中的反光柱位置进行匹配，从而获得点云数据的刚体变换矩阵。为解决异常点干扰点云匹配过程造成的定位精度下降问题，本文提出了基于距离的异常点剔除规则，检查经过刚体变换后的聚类中心数据。该规则是：如果某聚类中心离最近反光柱的距离超过一定阈值，就判定该中心点为异常点，将异常点删除后，重新对点云聚类中心进行FICP匹配，最终取得AGV叉车的定位结果。整个AGV叉车定位算法流程如图 2所示。

将通过激光雷达定位方法实时测得的(x, y)和利用单轴角度传感器测得的φ，作为AGV叉车部分状态的观测量，则系统的观测方程为：

$ \begin{gathered} \boldsymbol{z}_k=\boldsymbol{H q _ { k }}, \\ \boldsymbol{z}_k=\left[\begin{array}{c} z_{x, k} \\ z_{y, k} \\ z_{\varphi, k} \end{array}\right], \quad \boldsymbol{H}=\left[\begin{array}{lllll} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] . \end{gathered} $

(10)

其中：z_{x, k}、z_{y, k}和z_{φ, k}分别为系统在k时刻x、y和φ的观测量，参数矩阵 $\boldsymbol{H} \in \mathbb{R}^{3 \times 5}, \boldsymbol{z}_k $为系统在k时刻的观测量。

惯性测量单元(inertial measurement unit, IMU)采集的(a_x, a_y)和沿Z轴的角速度w可作为(9)中的u_k=[a_{x, k}, a_{y, k}, w_k]^T。因此，可利用IMU推算AGV叉车航迹。但由于IMU定位误差存在时间累积效应，长时间定位结果的误差将会超出可接受范围，故IMU仅可用于AGV叉车的短时间定位。

由于噪声的存在，通过式(9)和(10)推算所得的AGV叉车状态并不准确。假设这些噪声均服从Gauss分布，在状态空间方程和观测方程中分别引入过程噪声ε_k~N(0, Q₁)和测量噪声δ_k~N(0, Q₂)，可得

$\begin{gathered} \boldsymbol{q}_k=\boldsymbol{A} \boldsymbol{q}_{k-1}+\boldsymbol{B} \boldsymbol{u}_k+\boldsymbol{\varepsilon}_k, \\ \boldsymbol{z}_k=\boldsymbol{H} \boldsymbol{q}_k+\boldsymbol{\delta}_k . \end{gathered} $

(11)

要实现对AGV叉车的实时定位，则要求算法能实时滤除测量噪声，从而得到较为准确的定位结果。常规的滤波算法难以保证实时性，而Kalman滤波算法计算速度较快。应用式(12)Kalman滤波基本方程，融合激光雷达、角度传感器和IMU的定位结果，对AGV叉车进行精准定位：

$ \left\{\begin{array}{l} \hat{\boldsymbol{q}}_k^{-}=\boldsymbol{A} \hat{\boldsymbol{q}}_{k-1}+\boldsymbol{B} \boldsymbol{u}_k, \\ \boldsymbol{J}_k^{-}=\boldsymbol{A} \boldsymbol{J}_{k-1} \boldsymbol{A}^{\mathrm{T}}+\boldsymbol{Q}_1, \\ \boldsymbol{K}_{\mathrm{G}, k}=\boldsymbol{J}_k^{-} \boldsymbol{H}^{\mathrm{T}}\left(\boldsymbol{H} \boldsymbol{J}_k^{-} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{Q}_2\right)^{-1}, \\ \hat{\boldsymbol{q}}_k=\hat{\boldsymbol{q}}_k^{-}+\boldsymbol{K}_{\mathrm{G}, k}\left(\boldsymbol{z}_k-\boldsymbol{H} \hat{\boldsymbol{q}}_k^{-}\right), \\ \boldsymbol{J}_k=\left(\boldsymbol{E}-\boldsymbol{K}_{\mathrm{G}, k} \boldsymbol{H}\right) \boldsymbol{J}_k^{-} . \end{array}\right. $

(12)

其中: $ \hat{\boldsymbol{q}}_k$和 $ \hat{\boldsymbol{q}}_k^{-}$分别为k时刻系统状态的估计值和预测值，J_k和J_k^－分别为k时刻状态估计协方差的估计值和预测值，K_{G, k}为k时刻的Kalman增益，E为单位矩阵。

3 AGV叉车路径跟踪控制 3.1 比例积分控制器设计

PID控制器不依赖于精确的数学模型，具有控制简单、可靠性高等优点，但为了不放大噪声的影响，本文仅采用PI控制实现AGV叉车的高精度路径跟踪。AGV叉车路径跟踪的核心是控制其位姿(x, y, φ)，因而在路径跟踪过程中会产生2种误差：位置误差e_d和姿态误差e_eh。路径的起点为M，终点为N。

3.1.1 直线运动路径跟踪误差计算

对于直线运动，设AGV叉车在当前位置对应于直线路径上的理想点位为控制点P_b到直线路径的垂点P_ideal，那么此时P_ideal为P_b到预定直线路径的最近点。因此，直线运动的位置误差为|P_idealP_b|，姿态误差为AGV叉车中心轴线P_hP_b与直线路径的夹角，如图 3所示。

1) 位置误差计算。

定义P_b位于有向直线β_MN的左侧时，直线运动的位置误差e_d为正；P_b位于β_MN的右侧时，e_d为负。

图 4中，直线运动位置误差的正负可通过△MNP_b的矢量面积S_△MNPb判断：

$\boldsymbol{S}_{\triangle M N P_{\mathrm{b}}}=\frac{1}{2} \boldsymbol{\beta}_{M P_{\mathrm{b}}} \boldsymbol{\beta}_{N P_{\mathrm{b}}} . $

(13)

当P_b在β_MN的左侧时，点M、N、P_b的顺序呈逆时针，此时S_△MNPb大于0；当P_b在β_MN的右侧时，点M、N、P_b的顺序呈顺时针，此时S_△MNPb小于0；当P_b在β_MN上时，S_△MNPb为0。则e_d为

$ e_{\mathrm{d}}= \begin{cases}\left|P_{\text {ideal }} P_{\mathrm{b}}\right| \frac{\boldsymbol{S}_{\triangle M N P_{\mathrm{b}}}}{\left|\boldsymbol{S}_{\triangle M N P_{\mathrm{b}}}\right|}, & \left|\boldsymbol{S}_{\triangle M N P_{\mathrm{b}}}\right| \neq 0 ; \\ 0, & \left|\boldsymbol{S}_{\triangle M N P_{\mathrm{b}}}\right|=0 .\end{cases} $

(14)

2) 姿态误差计算。

一般将AGV叉车的叉齿向后的运动定义为正向运动，向前的运动定义为反向运动。用K_isForward表示AGV叉车的运动方向，其值为1和-1时分别表示正向运动和反向运动。

当AGV叉车正向运动和反向运动时，其车身姿态分别用向量 γ_PbPh和γ_PhPb表示。则φ为

$\varphi= \begin{cases}\left\langle\boldsymbol{\gamma}_{P_{\mathrm{b}} P_{\mathrm{h}}}, \boldsymbol{\beta}_X\right\rangle, & K_{\text {isForward }}=1 ; \\ \left\langle\boldsymbol{\gamma}_{P_{\mathrm{h}} P_{\mathrm{b}}}, \boldsymbol{\beta}_X\right\rangle, & K_{\text {isForward }}=-1 .\end{cases} $

(15)

其中β_X表示正向X轴。

β_MN与β_X之间的夹角为

$ \alpha=\left\langle\boldsymbol{\beta}_{M N}, \boldsymbol{\beta}_X\right\rangle, \quad-\pi \leqslant \alpha \leqslant \pi . $

(16)

定义当AGV车身轴线P_hP_b与β_MN之间夹角位于前进方向左侧时，姿态误差e_eh为正，反之为负。则e_eh为

$e_{\mathrm{eh}}=\varphi-\alpha . $

(17)

3.1.2 圆弧运动路径跟踪误差计算

设AGV叉车圆弧运动的标准圆弧路径的圆心为O_r，则AGV叉车在当前位置对应于圆弧路径上的理想点位为直线O_rP_b与圆弧路径的交点P_ideal，圆弧路径在P_ideal处的切线与AGV叉车中心轴线的交点为P′，如图 5所示。因此，圆弧运动的位置误差为控制点P_b与P_ideal之间的距离|P_idealP_b|，姿态误差为P_hP_b与P_ideal处切线的夹角。

1) 位置误差计算。

定义当P_b位于圆弧路径的外侧时，即当|O_rP_b|>|O_rP_ideal|时，位置误差为正，反之为负。则圆弧运动的位置误差为

$ e_{\mathrm{d}}=\left|O_{\mathrm{r}} P_{\mathrm{b}}\right|-\left|O_{\mathrm{r}} P_{\text {ideal }}\right| \text {. } $

(18)

2) 姿态误差计算。

定义当P_hP_b与P_ideal处切线之间夹角位于切线方向左侧时，姿态误差e_ed为正，反之为负。φ可由式(15)给出，设P_ideal处切线与β_X之间的夹角为α，那么圆弧运动的姿态误差e_eh为

$e_{\mathrm{eh}}=\varphi-\alpha . $

(19)

3.1.3 比例积分控制

令位置误差的比例参数为K_dp，姿态误差的比例参数为K_po，位置误差的积分参数为K_di。则在k时刻AGV叉车舵轮控制量Δθ_k为

$ \Delta \theta_k=\left(K_{\mathrm{dp}} e_{\mathrm{d}, k}+K_{\mathrm{po}} e_{\mathrm{eh}, k}+K_{\mathrm{di}} \sum\limits_{t=0}^k e_{\mathrm{d}, t}\right). $

(20)

其中：e_{eh, k}为k时刻的姿态误差，e_{d, t}为t时刻的位置误差。

1) 直线运动控制。

对于直线运动，其舵轮的理想转角θ_ideal为0。由于直线运动存在高速运动阶段，因此还需要根据AGV叉车的运行速度调整舵轮的控制量。设速度调整因子为K_s，那么在k时刻舵轮转角为

$\theta_k=-K_{\mathrm{s}} \Delta \theta_k K_{\text {isForward }} \cdot $

(21)

2) 圆弧运动控制。

设预定P_ideal处的曲率为ρ_ideal。根据式(4)，当AGV叉车按预定路径运动(无位置误差和姿态误差)时，在圆弧运动中舵轮理想转角为

$ \theta_{\text {ideal }}=\arctan \left(l \rho_{\text {ideal }}\right) . $

(22)

用K_isLeft表示AGV叉车圆弧运动的方向，其值为1时表示左转，为-1时表示右转。圆弧运动一般为速度恒定的低速运动，不需要根据速度调节舵轮控制量，则在k时刻舵轮转角为

$ \theta_k=\left(\theta_{\text {ideal }, k}+\Delta \theta_k\right) K_{\text {isLeft }} K_{\text {isForward }}. $

(23)

其中θ_{ideal, k}为k时刻舵舱的理想转角。

3.2 圆弧路径规划

在工厂等实际应用环境中，AGV叉车的运动路径通常由一系列直线和圆弧构成。通常圆弧运动的起点和终点都衔接着直线运动，如果采用等曲率的标准圆弧，那么直线和圆弧的衔接点处就会产生曲率突变，进而要求舵轮转角发生阶跃变化，导致AGV叉车路径跟踪的平稳性和精度下降。为避免规划路径的曲率不连续导致AGV叉车的路径跟踪误差激增，同时也为了防止原地转向对轮胎的磨损，需要对AGV圆弧路径进行规划。

Bézier曲线是由法国工程师Pierre Bézier提出的一种参数曲线，通过给定的多个控制点按顺序连接构成控制多边形，以控制生成的Bézier曲线，改变控制点即可改变Bézier曲线的形状。Bézier曲线的优点是通过起点和终点，并与控制多边形的首末两边相切于起点和终点。n阶Bézier曲线需要(n+1)个控制点确定，求解公式为

$\begin{gathered} p_n(j)=\sum\limits_{i=0}^n C_n^i \cdot j^i \cdot(1-j)^{n-i} \cdot P_i=\\ \sum\limits_{i=0}^n B_{i, n}(j) \cdot P_i, \\ B_{i, n}(j)=C_n^i \cdot j^i \cdot(1-j)^{n-i} . \end{gathered} $

(24)

其中: p_n为n阶Bézier曲线点；参数变量j∈[0, 1.0]，当j为0、0.5和1.0时，分别代表曲线的起点、中间点和终点；C_nⁱ为组合计算；B_{i, n}(j)为Bernstein基函数；P_i为第i个控制点。

为保证规划路径二阶导连续，可利用三阶Bézier曲线生成曲率连续的圆弧路径，以满足AGV叉车圆弧运动的路径跟踪要求。图 6中，采用4个控制点P₀、P₁、P₂和P₃生成三阶Bézier曲线，求解公式如下：

$ \left\{\begin{array}{l} p_3(j)=(1-j)^3 P_0+3 j(1-j)^2 P_1+ \\ \quad 3 j^2(1-j) P_2+j^3 P_3, \\ \dot{p}_3(j)=3(1-j)^2\left(P_1-P_0\right)+ \\ \quad 6 j(1-j)\left(P_2-P_1\right)+3 j^2\left(P_3-P_2\right), \\ \ddot{p}_3(j)=6(1-j) \cdot\left(P_2-2 P_1+P_0\right)+ \\ \quad 6 j \cdot\left(P_3-2 P_2+P_1\right) . \end{array}\right. $

(25)

其中P₀和P₃分别为圆弧运动的起点M和终点N。

曲线上任意一点的曲率ρ为

$ \rho=\frac{\dot{x} \times \ddot{y}-\dot{y} \times \ddot{x}}{\left((\dot{x})^2+(\dot{y})^2\right)^{3 / 2}} . $

(26)

为使轨迹轴对称，本文使P₁和P₂的位置在MI和IN上的变化规律一致，以保证|IP₁|=|IP₂|=f，此时Bézier曲线的起点和终点处的曲率一致。不同的f生成的Bézier曲线不同，为满足AGV叉车圆弧运动的要求，所生成的Bézier曲线应符合2个条件：1) Bézier曲线的起点和终点处的曲率ρ₀应尽量接近0，以减小曲率突变的影响；2) 由于舵轮转角限制，AGV叉车的瞬时转弯半径存在一个最小值，即Bézier曲线的曲率存在一个最大值限制。设AGV叉车舵轮最大转角为θ_max，由式(4)可知，Bézier曲线的最大曲率ρ_max应该满足：

$ \rho_{\max } \leqslant \frac{\tan \theta_{\max }}{l} . $

(27)

以经过点M(0, 0)、I(0, 1.00)和N(1.00, 1.00)的圆弧运动为例，对f∈[0, 1.00)以0.01的间隔进行遍历，生成相应的Bézier曲线路径，如图 7所示。由图可知，f越大，所生成的Bézier曲线越接近M(P₀)和N(P₃)的连线。

图 8中，当f=0.50时，Bézier曲线在起点和终点处的曲率明显大于曲线中间点的曲率，且大于0，这不符合条件1；且当f继续增大时，起点和终点处的曲率越来越大，因此由f∈[0.50, 1.00)生成的Bézier曲线不满足圆弧运动要求。

对由f∈[0, 0.50)生成的Bézier曲线的曲率进行研究，结果如图 9所示。Bézier曲线最接近标准圆弧的路径时，f∈(0.40, 0.50)；当f在0.50附近时，出现曲线在起点和终点处的曲率大于中间点处的情况；而当f∈[0, 0.40)时，曲线在起点和终点处的曲率小于曲线中间点处的曲率。

图 10中，当f越接近0时，曲线起点和终点的曲率越接近0，但此时曲线对应的θ_max却越大。

因此，应该在Bézier曲线的曲率满足式(27)的情况下，尽量选择较小的f使ρ₀较小。综上所述，基于Bézier曲线生成的AGV叉车圆弧路径f值的选择应遵循：

$ \begin{array}{ll} & \min \rho_0, \\ \text { s.t. } & \rho_{\max } \leqslant \frac{\tan \theta_{\max }}{l} . \end{array} $

(28)

4 实验验证 4.1 实验场景

实验使用改造后的瑞博特单舵轮AGV叉车作为对象。主控制器采用汇川可编程逻辑控制器，驱动采用柯蒂斯的电机和驱动器，舵轮转向采用步科的电机及驱动器。

为实现AGV叉车的精准定位，在实验场地中布置多根反光柱，并在AGV叉车上加装倍加福R2000 UHD激光雷达，激光雷达的分辨率为1 mm，重复精度小于12 mm，能实现2.5×10⁵ Hz的测量频率。此外，AGV叉车加装维特智能HWT901B-232的IMU[加速度稳定性为0.01 g，陀螺仪稳定性为0.05°/s]以及维特智能HWT101CT-485的单轴角度传感器(Z轴角度测量精度为0.1°)，如图 11所示。

图 12中，在软件层面，AGV叉车车载工控机作为下位机，安装Debian4.19操作系统，并由QT应用程序开发框架开发下位机软件，实时监测AGV叉车的运行状态(位置、舵轮角度等)并设置PI控制参数。上位机的调度系统采用B/S(browser/server)架构，通过Socket通信以JSON字符串的格式与下位机进行通信，控制AGV叉车。AGV叉车也实时将自身状态反馈给上位机，供调度系统掌握运行情况，便于调度系统对下一步指令进行决策。

本文设计了1条路径测试AGV叉车定位及路径跟踪的效果，如图 13所示。P₀点为停车点，其余的7个站点(P₁~ P₇)为工作站点，工作站点之间包括4条直线路径和3条圆弧路径。

4.2 定位结果

测试AGV叉车在静止状态下激光雷达的定位算法，X和Y轴的定位结果如图 14所示，可知，基于3σ准则，AGV叉车X和Y轴的位置定位结果绝大部分落在+3σ线和-3σ线之间即±0.003 m的误差带内，说明所提激光雷达的定位算法精度能够达到±3 mm。

利用Kalman滤波算法，将激光雷达、角度传感器和IMU的定位结果进行融合，可得AGV叉车运动状态的定位结果如图 15所示。可知，通过综合IMU和传感器数据推算所得的是已滤除了激光雷达和角度传感器测量噪声的结果，说明此方法可以提升AGV叉车的定位精度。

4.3 路径跟踪结果

让AGV叉车沿图 13中的测试路径 P₀→ P₁→ P₂→ P₃→ P₄→ P₅→ P₆→ P₇→ P₁分别采用叉齿向前和叉齿向后2种方式行驶，并测试路径跟踪控制算法，结果如图 16和17所示。

4.3.1 直线路径跟踪效果

计算直线运动段 P₁→ P₂、P₃→ P₄、P₅→ P₆、P₇→ P₁跟踪的误差平均值和均方根值，求取叉齿向后和向前2种运动方式下10次计算结果的均值，求得的直线路径跟踪误差分析结果如表 1所示。可以看出，直线路径跟踪的误差平均值为-0.006 m，在10 mm以内，说明AGV叉车直线运动路径跟踪的效果较好；而直线路径跟踪的误差均方根值为0.023 m，说明AGV叉车直线路径跟踪的精度能够控制在23 mm以内。

4.3.2 圆弧路径跟踪效果

计算圆弧运动段 P₂→ P₃、P₄→ P₅、P₆→ P₇跟踪的误差平均值和均方根值，求取叉齿向后和叉齿向前2种运动方式下10次计算结果的均值，求得基于Bézier曲线的圆弧路径跟踪误差分析结果如表 2所示。圆弧路径跟踪的误差平均值为0.014 m, 误差均方根值为0.025 m，说明AGV叉车圆弧路径跟踪的精度能够控制在25 mm以内。相同控制参数下，使用等曲率圆弧进行跟踪控制实验，所得结果如表 3所示，误差平均值为0.075 m，误差均方根值为0.091 m。对比表 2和3中的结果可知，相比等曲率圆弧路径，使用Bézier曲线改进后的圆弧路径跟踪误差均方根值下降约72%，跟踪精度得到明显提升。

4.3.3 工作点位重复定位精度

对于AGV叉车而言，路径跟踪控制的另外一个重要目的是保证在工作点位的定位精度，确保AGV叉车能准确地装卸货物。以图 13的测试路径的最后停车点P1为对象，研究在PI路径跟踪控制算法下AGV叉车的重复定位精度，分析结果如图 18所示。最小包络圆的半径为0.012 m，说明AGV叉车的重复定位精度可达±12 mm。

5 结论

本文开展了针对室内结构化环境下的单舵AGV叉车的激光雷达定位与路径跟踪研究。利用DBSCAN算法和FICP算法建立了基于反光柱的激光雷达定位方法，提出了基于距离的异常点剔除规则，并利用Kalman滤波算法融合IMU的测量数据，消除激光雷达和角度传感器测量噪声，最终实现了±3 mm的AGV叉车定位精度。面向工业实际应用中常见的直线和圆弧两类运动，基于几何关系定义了路径跟踪过程中产生的位置误差和姿态误差，设计了PI控制器。考虑到等曲率圆弧与直线路径衔接时的曲率突变问题，本文设计了三阶Bézier曲线进圆弧路径，通过优化分析将圆弧路径跟踪误差降低了约72%，避免了因曲率突变造成的路径跟踪精度下降问题。通过实验验证，基于PI控制的AGV叉车路径跟踪方法，可将跟踪精度控制在25 mm内，工作站点的重复定位精度可达±12 mm。