CN114195045B - 一种无人叉车的自动叉取方法 - Google Patents

Abstract

本发明公开了一种无人叉车的自动叉取方法，具体涉及无人叉车技术领域，包括步骤：基于目标坐标系和叉车坐标系，通过视觉传感器获取目标托盘相对于叉车的叉取位姿；根据叉取位姿，基于位姿镇定算法控制叉车移动至目标托盘正前方的调校点；根据叉车实时的速度信息构建叉车的运动学模型；根据运动学模型构建叉车行进并叉取过程中，当前时刻叉车位姿与叉取位姿的误差方程；获取误差方程微分后的微分方程并带入轨迹跟踪控制律公式，获取下一时刻的速度信息；根据下一时刻的速度信息控制叉车进行速度状态调整直至叉车位姿到达叉取位姿。本发明将叉车的托盘叉取过程分为两个阶段，通过不同的算法实现了无人叉车高效、精准的目标托盘叉取。

Description

一种无人叉车的自动叉取方法

技术领域

本发明涉及无人叉车技术领域，具体涉及一种无人叉车的自动叉取方法。

背景技术

目前的无人叉车主要是基于激光/视觉系统进行导航定位，实现叉车自主搬运货物。其具体实现方式为：将激光/视觉装置安装在叉车主体的顶部，使激光/视觉装置具有较高处的视野，以用于识别周边环境或预先设置的靶标，进而得到叉车的位置信息，实现叉车自主搬运货物。然而现有无人仓储应用中的托盘自动搬运任务，托盘在经过上游操作后的放置位置(因人工或其它因素)往往存在较大偏移(如图2所示)。因此，仅仅是根据叉车位置进行简单的位姿调整使其对准，，是难以很好的根据标准的靶标-插孔方位关系进行目标托盘插孔的对齐的，从而导致无人叉车托盘叉取的失败，亦有可能对无人叉车或托盘及所载货物造成损坏。

发明内容

为了满足托盘没有完全摆正情况下，无人叉车对于托盘的安全叉取，本发明提出了一种无人叉车的自动叉取方法，以目标托盘的惯性原点为目标原点构建目标坐标系，以叉车轮轴中心点为叉车原点构建叉车坐标系，包括步骤：

S1：基于目标坐标系和叉车坐标系，通过视觉传感器获取目标托盘相对于叉车的叉取位姿；

S2：根据叉取位姿，基于位姿镇定算法控制叉车移动至目标托盘正前方的调校点；

S3：根据叉车实时的速度信息构建叉车的运动学模型；

S4：根据运动学模型构建叉车行进并叉取过程中，当前时刻叉车位姿与叉取位姿的误差方程；

S5：获取误差方程微分后的微分方程并带入轨迹跟踪控制律公式，获取下一时刻的速度信息；

S6：根据下一时刻的速度信息控制叉车进行速度状态调整；

S7：判断叉车位姿是否到达叉取位姿，若否，返回S3步骤，若是，完成叉取动作；

所述目标托盘的叉取方向为目标坐标系的第一轴向，所述叉车轮轴的中垂线为叉车坐标系的第一轴向。

进一步地，所述S2步骤中，位姿镇定算法可表述为如下公式，

式中，r为目标原点与叉车原点之间的直线距离，α为目标原点与叉车原点连线与叉车坐标系第一轴向的角度，β为目标原点与叉车原点连线与目标坐标系第一轴向的角度，θ为β与α之间的夹角，v为叉车的线速度，ω为叉车的角速度。

进一步地，所述位姿镇定算法，需满足如下控制律公式，

v＝k_ρrcosθ

ω＝k_αα+k_ββ

式中，k_ρ、k_α和k_β为根据电机响应速度设置的速度比例控制系数。

进一步地，所述S3步骤中，运动学模型可表示为如下公式，

式中，q＝(x,y,θ)^T为叉车移动过程中的广义坐标向量,T为时间，v为时刻T时叉车的线速度，ω为时刻T时叉车的角速度；叉车以线速度v和角速度ω的速度信息运行，且叉车以调校点处的叉车原点构建叉车坐标系的情况下，x为时刻T时叉车在第一轴向上的水平偏移距离，y为时刻T时叉车与第一轴向的垂直偏移距离，θ为时刻T时叉车原点与调校点连线与第一轴向的夹角。

进一步地，所述S4步骤中，误差方程可表示为如下公式，

式中，q_e＝(x_e,y_e,θ_e)为时刻T时，在叉车以T时刻的叉车原点构建叉车坐标系的情况下，假使叉车以当前角速度和线速度继续运行，叉车移动至目标原点后的坐标偏移量；(x_r,y_r,θ_r)为目标原点在叉车以调校点处的叉车原点构建叉车坐标系下的坐标量。

进一步地，所述S5步骤中，微分方程可表示为如下公式，

式中，ω_c为下一时刻的角速度，v_c为下一时刻的线速度，v_r为叉车的预设线速度，ω_r为叉车的预设角速度。

进一步地，所述S5步骤中，轨迹跟踪控制律公式可表示为如下公式，

式中，k1、k2、k3为速度比例控制项，a，b为防止除0运算的约束量。

进一步地，所述约束量需满足如下约束公式，

k₂<(v_max-v_rmax)

k₁v_rmax+k₃<(ω_max-ω_rmax)

k₁,k₂,k₃,a,b>0

式中，v_max为叉车的最大线速度，v_rmax为叉车的预设最大线速度,ω_max为叉车的最大角速度，ω_rmax为叉车的预设最大角速度。

与现有技术相比，本发明至少含有以下有益效果：

(1)本发明所述的一种无人叉车的自动叉取方法，其通过将叉车的托盘叉取过程分为两个阶段，也即是初就位阶段和最终调校阶段，在初就位阶段通过位姿镇定法控制叉车移动至托盘正前方的调校点，此过程中无需过多考虑精准度，因此在该阶段可以极快得实现就位；

(2)在最终调校阶段，采用轨迹跟踪算法，对每一时刻的轨迹误差进行跟踪，从而使得最终的叉取位姿能够完全符合要求，避免叉取过程中对目标托盘的擦碰导致的货物以及叉车损坏；

(3)通过轨迹跟踪控制律的应用，使得对于初就位阶段任意的偏差，都能够是误差经过调整后能够趋于0，大大提高了叉取精准度。

附图说明

图1为一种无人叉车的自动叉取方法的方法步骤图；

图2为叉车叉取偏移托盘示意图；

图3为位姿镇定算法示意图；

图4为轨迹跟踪示意图。

具体实施方式

以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。

实施例一

为了使无人叉车在面对放置不规范的托盘时，能够更好的对齐托盘插孔，避免因叉车碰撞导致的叉车以及货物的损坏，如图1所示，本发明提出了一种无人叉车的自动叉取方法，其以目标托盘的惯性原点为目标原点构建目标坐标系，以叉车轮轴中心点为叉车原点构建叉车坐标系，包括步骤：

S1：基于目标坐标系和叉车坐标系，通过视觉传感器获取目标托盘相对于叉车的叉取位姿；

S2：根据叉取位姿，基于位姿镇定算法控制叉车移动至目标托盘正前方的调校点；

S3：根据叉车实时的速度信息构建叉车的运动学模型；(速度信息包括线速度和角速度)

S4：根据运动学模型构建叉车行进并叉取过程中，当前时刻叉车位姿与叉取位姿的误差方程；

S5：获取误差方程微分后的微分方程并带入轨迹跟踪控制律公式，获取下一时刻的速度信息；

S6：根据下一时刻的速度信息控制叉车进行速度状态调整；

S7：判断叉车位姿是否到达叉取位姿，若否，返回S3步骤，若是，完成叉取动作。

通过视觉传感器，可以获得目标托盘相对于叉车的位姿，为了方便托盘的高效、精准叉取，本发明将整个叉取过程分为两个阶段，第一阶段基于位姿镇定算法将叉车在一定误差范围内高速移动至目标托盘正前方适当位置处(也即是调校点)，而后通过第二阶段，采用轨迹跟踪的方法，利用轨迹跟踪控制律在一定条件限制下将叉车的角度偏差进行修正，从而使得叉车能够精确的对齐目标托盘的插孔，从而完成托盘的叉取。

下面对位姿镇定算法进行相应说明。以目标托盘的惯性原点(由于目标托盘规格大多一致，因此根据视觉传感器以及目标托盘的规格参数即可获得惯性原点的位置)为目标原点构建目标坐标系，目标托盘的叉取方向为目标坐标系的第一轴向(X₀)，垂直于该轴向的方向为目标坐标系的第二轴向(Y₀)；以叉车轮轴中心点为叉车原点构建叉车坐标系，叉车轮轴的中垂线为叉车坐标系的第一轴向(X_r)，垂直于该轴向的方向为叉车坐标系的第二轴向(Y_r)。在该相对坐标系下，叉车与目标托盘的位姿坐标系如图3所示，r表示无人叉车轮轴中心点与目标托盘惯性原点之间的距离，α表示叉车原点与惯性原点连线间与叉车坐标系第一轴向(X_r)的夹角，β表示叉车原点与惯性原点连线间与目标坐标系第一轴向(X₀)的夹角，θ为β与α之间的夹角的大小，也即是无人叉车朝向与目标托盘插孔朝向之间的夹角。那么根据该位姿坐标系中的角度、距离关系，以及无人叉车位于叉车原点处的线速度v，角速度ω，可得到如下方程，

同时，由于叉车发动机的响应速度是有一定限制的，因此线速度和角速度存在一定的限制关系，也即是如下限制公式，

v＝k_ρrcosθ

ω＝k_αα+k_ββ

式中，k_ρ、k_α和k_β为根据电机响应速度设置的速度比例控制系数。

将上述限制公式作为控制律公式对方程进行限制即可完成第一阶段对无车叉车的运动控制。

在第二阶段中，由于需要考虑到误差偏差对叉取动作安全性的影响，因此，在此处本发明选用轨迹跟踪、加以误差控制的方式进行无人叉车的运动控制。首先，根据无人叉车的线速度v和角速度ω作为控制器的输入量，构建无人叉车的运动学模型如下，

式中，q＝(x,y,θ)^T为叉车移动过程中的广义坐标向量,T为时间，v为时刻T时叉车的线速度，ω为时刻T时叉车的角速度；叉车以线速度v和角速度ω的速度信息运行，且叉车以调校点处的叉车原点构建叉车坐标系的情况下，x为时刻T时叉车在第一轴向上的水平偏移距离，y为时刻T时叉车与第一轴向的垂直偏移距离，θ为时刻T时叉车原点与调校点连线与第一轴向的夹角。

如图4所示，假设第二阶段中，q＝(x,y,θ)^T为无人叉车某时刻的位姿，那么无人叉车要从该位姿移动至目标托盘惯性原点处的位姿q_r＝(x_r,y_r,θ_r)^T，根据坐标变换可得描述移动无人叉车过程中位姿变化的误差方程，如下式，

式中，q_e＝(x_e,y_e,θ_e)为时刻T时，在叉车以T时刻的叉车原点构建叉车坐标系的情况下，假使叉车以当前角速度和线速度继续运行，叉车移动至目标原点后的坐标偏移量；(x_r,y_r,θ_r)为目标原点在叉车以调校点处的叉车原点构建叉车坐标系下的坐标量。

对误差方程进行微分，可得到无人叉车位姿的微分方程，

式中，ω_c为下一时刻的角速度，v_c为下一时刻的线速度，v_r为叉车的预设线速度，ω_r为叉车的预设角速度。

由于无人叉车的运动轨迹跟踪控制可以看成寻找控制输入p＝(v_c,ω_c)使对调校点任意的初始误差，系统在该输入下都能使得q_e＝(x_e,y_e,θ_e)^T有界，且lim_t→∞||(x_e y_e θ_e)^T||＝0,也即是使微分方程得出的数据有界限，不是无穷态，误差趋于0。基于该限制条件，本发明选用轨迹跟踪控制律公式在一定约束下进行无人叉车的轨迹跟踪，其中，轨迹跟踪控制律公式如下，

式中，k1、k2、k3为速度比例控制项，a，b为防止除0运算的约束量。

其中，约束量需满足如下约束公式，

，₂<(v_max-v_rmax)

k₁v_rmax+k₃<(ω_max-ω_rmax)

k₁,k₂,k₃,a,b>0

式中，v_max为叉车的最大线速度，v_rmax为叉车的预设最大线速度,ω_max为叉车的最大角速度，ω_rmax为叉车的预设最大角速度。

通过上述公式分别对无人叉车进行阶段划分式的目标托盘叉取，从而使得无人叉车能够在保证叉取效率的情况下，保证无人叉车以及货物的安全性。

综上所述，本发明所述的一种无人叉车的自动叉取方法，其通过将叉车的托盘叉取过程分为两个阶段，也即是初就位阶段和最终调校阶段，在初就位阶段通过位姿镇定法控制叉车移动至托盘正前方的调校点，此过程中无需过多考虑精准度，因此在该阶段可以极快得实现就位。

在最终调校阶段，采用轨迹跟踪算法，对每一时刻的轨迹误差进行跟踪，从而使得最终的叉取位姿能够完全符合要求，避免叉取过程中对目标托盘的擦碰导致的货物以及叉车损坏。同时，通过轨迹跟踪控制律的应用，使得对于初就位阶段任意的偏差，都能够是误差经过调整后能够趋于0，大大提高了叉取精准度。

需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本发明中如涉及“第一”、“第二”、“一”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

Claims (2)

1.一种无人叉车的自动叉取方法，其特征在于，以目标托盘的惯性原点为目标原点构建目标坐标系，以叉车轮轴中心点为叉车原点构建叉车坐标系，包括步骤：

S1：基于目标坐标系和叉车坐标系，通过视觉传感器获取目标托盘相对于叉车的叉取位姿；

S2：根据叉取位姿，基于位姿镇定算法控制叉车移动至目标托盘正前方的调校点；

S3：根据叉车实时的速度信息构建叉车的运动学模型；

S4：根据运动学模型构建叉车行进并叉取过程中，当前时刻叉车位姿与叉取位姿的误差方程；

S5：获取误差方程微分后的微分方程并带入轨迹跟踪控制律公式，获取下一时刻的速度信息；

S6：根据下一时刻的速度信息控制叉车进行速度状态调整；

S7：判断叉车位姿是否到达叉取位姿，若否，返回S3步骤，若是，完成叉取动作；

所述目标托盘的叉取方向为目标坐标系的第一轴向，所述叉车轮轴的中垂线为叉车坐标系的第一轴向；

所述S2步骤中，位姿镇定算法可表述为如下公式，

式中，r为目标原点与叉车原点之间的直线距离，α为目标原点与叉车原点连线与叉车坐标系第一轴向的角度，β为目标原点与叉车原点连线与目标坐标系第一轴向的角度，θ为β与α之间的夹角，v为叉车的线速度，ω为叉车的角速度；

所述S3步骤中，运动学模型可表示为如下公式，

式中，为叉车移动过程中的广义坐标向量,T为时间，v为时刻T时叉车的线速度，ω为时刻T时叉车的角速度；叉车以线速度v和角速度ω的速度信息运行，且叉车以调校点处的叉车原点构建叉车坐标系的情况下，x为时刻T时叉车在第一轴向上的水平偏移距离，y为时刻T时叉车与第一轴向的垂直偏移距离，θ为时刻T时叉车原点与调校点连线与第一轴向的夹角；

所述S4步骤中，误差方程可表示为如下公式，

式中，为时刻T时，在叉车以T时刻的叉车原点构建叉车坐标系的情况下，假使叉车以当前角速度和线速度继续运行，叉车移动至目标原点后的坐标偏移量；/>为目标原点在叉车以调校点处的叉车原点构建叉车坐标系下的坐标量；

所述S5步骤中，微分方程可表示为如下公式，

式中，ω_c为下一时刻的角速度，v_c为下一时刻的线速度，v_r为叉车的预设线速度，ω_r为叉车的预设角速度；

所述S5步骤中，轨迹跟踪控制律公式可表示为如下公式，

式中，k1、k2、k3为速度比例控制项，a，b为防止除0运算的约束量；

所述约束量需满足如下约束公式，

式中，v_max为叉车的最大线速度，v_rmax为叉车的预设最大线速度,ω_max为叉车的最大角速度，ω_rmax为叉车的预设最大角速度。

2.如权利要求1所述的一种无人叉车的自动叉取方法，其特征在于，所述位姿镇定算法，需满足如下控制律公式，

式中，k_ρ、k_α和k_β为根据电机响应速度设置的速度比例控制系数。