车体位姿校正方法及装置与流程

本发明涉及机电控制技术领域，特别涉及一种车体位姿校正方法及装置。

背景技术：

agv(automatedguidedvehicle,自动导引运输车)是指装备有视觉、激光雷达等传感器，能够沿设定的路径自动行驶并具有自主避障、承载货物等功能的轮式移动机器人设备。近年，随着国际、国内物流、电商行业的迅猛发展，agv在智能无人仓储、无人配送等应用场景中需求巨大。

典型的agv设备底盘为长方形结构，配备两套车轮系统，一套为具备导向和承重功能的承重轮系统，主要用于支撑负载并在agv行进过程中起到运动导向的作用，其布置于底盘的四个角上，且四个车轮在同一平面上；另一套为驱动轮系统，两轮分布于底盘中部且关于车体中轴线左右对称。通过调整左右驱动轮的速度实现agv设备的直行、转向、原地掉头等动作。车体顶面居中位置安装举升装置，用于在运动过程中托起货架。货架采用四角支撑，货架底部可容纳agv进行通过和转向运动。车体底面居中位置安装高速摄像头，用于快速识别张贴于地面的二维码导向地标并实时计算出当前agv的位置和朝向。

技术实现要素：

发明人研究发现，在现有技术中，agv在到达设定的货架下方后执行以下三个步骤：第一步，原地转向将车头方向转到下一个指令运动方向；第二步，升起举升装置将货架托起；第三步，agv拖带货架运动到目标位置完成货物运送。在以上作业流程的第一步中，由于运动误差的存在，大部分情况下agv在到达货架下方后并没有严格停在货架的中心位置，在经过转向运动后，如果这一误差仍然没有消除，将导致在第二步货架举升过程中出现货架顶偏的情况，从而导致agv拖带货架运行过程中与其它货架发生碰撞，严重时可能导致货架倾覆造成损失。

本发明解决的一个技术问题是，如何实现车体在旋转过程中同步校正车体位姿。

根据本发明实施例的一个方面，提供了一种车体位姿校正方法，包括：将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数，车体旋转角为车体目标前进方向的法线方向与车体当前前进方向之间的夹角；根据车体旋转角、车体中心目标位置以及车体中心当前位置的数值，确定车体的左驱动轮与右驱动轮的速度比值，并根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数包括：将车体旋转半径确定为车体旋转角的线性函数，其中，车体旋转半径为旋转中心与车体中心之间的距离，车体中心位于车体的左驱动轮与右驱动轮的中点；根据线性函数、车体的左驱动轮与右驱动轮之间的距离，将车体的左驱动轮与右驱动轮的速度比值确定为车体旋转角的函数。

在一个实施例中，将车体旋转半径确定为车体旋转角的线性函数包括：将车体旋转半径用车体旋转角、车体旋转角系数以及常数项表示；将车体中心的位置随时间的变化率确定为车体旋转角的函数；根据车体中心的位置随时间变化率与车体旋转角的函数关系，确定车体中心目标位置、车体中心当前位置以及车体旋转角的函数关系；利用车体中心目标位置、车体中心当前位置，表示车体旋转角系数以及常数项。

在一个实施例中，所述车体中心的当前位置以及所述车体当前前进方向通过车体的摄像头探测确定。

在一个实施例中，根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度包括：将车体的旋转角速度确定为随时间变化的函数；根据车体旋转角以及车体在各个时刻的旋转角速度，确定车体旋转的总时长；根据车体旋转的总时长以及车体旋转的工作周期，确定车体旋转的工作周期数；在每个工作周期内，根据速度比值以及车体在各个时刻的旋转角速度，为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，将车体的旋转角速度确定为随时间变化的函数包括：根据车体的属性，设定车体匀变速旋转的角加速度以及最大旋转角速度；根据车体匀变速旋转的角加速度以及最大旋转角速度，确定车体在各个时刻的旋转角速度。

根据本发明实施例的另一个方面，提供了一种车体位姿校正装置，包括：函数确定模块，配置为将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数，车体旋转角为车体目标前进方向的法线方向与车体当前前进方向之间的夹角；速度设定模块，配置为根据车体旋转角、车体中心目标位置以及车体中心当前位置的数值，确定车体的左驱动轮与右驱动轮的速度比值，并根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，函数确定模块配置为：将车体旋转半径确定为车体旋转角的线性函数，其中，车体旋转半径为旋转中心与车体中心之间的距离，车体中心位于车体的左驱动轮与右驱动轮的中点；根据线性函数、车体的左驱动轮与右驱动轮之间的距离，将车体的左驱动轮与右驱动轮的速度比值确定为车体旋转角的函数。

在一个实施例中，函数确定模块配置为：将车体旋转半径用车体旋转角、车体旋转角系数以及常数项表示；将车体中心的位置随时间的变化率确定为车体旋转角的函数；根据车体中心的位置随时间变化率与车体旋转角的函数关系，确定车体中心目标位置、车体中心当前位置以及车体旋转角的函数关系；利用车体中心目标位置、车体中心当前位置，表示车体旋转角系数以及常数项。

在一个实施例中，所述车体中心的当前位置以及所述车体当前前进方向通过车体的摄像头探测确定。

在一个实施例中，速度设定模块配置为：将车体的旋转角速度确定为随时间变化的函数；根据车体旋转角以及车体在各个时刻的旋转角速度，确定车体旋转的总时长；根据车体旋转的总时长以及车体旋转的工作周期，确定车体旋转的工作周期数；在每个工作周期内，根据速度比值以及车体在各个时刻的旋转角速度，为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，速度设定模块配置为：根据车体的属性，设定车体匀变速旋转的角加速度以及最大旋转角速度；根据车体匀变速旋转的角加速度以及最大旋转角速度，确定车体在各个时刻的旋转角速度。

根据本发明实施例的又一个方面，提供了一种车体位姿校正装置，包括：存储器；以及耦接至存储器的处理器，处理器被配置为基于存储在存储器中的指令，执行前述的车体位姿校正方法。

根据本发明实施例的再一个方面，提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机指令，指令被处理器执行时实现前述的车体位姿校正方法。

本发明提供的车体位姿校正方法，将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数；根据车体旋转角、车体中心目标位置以及车体中心当前位置的数值，确定车体的左驱动轮与右驱动轮的速度比值，并根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度。从而提供了一种车体位姿校正方法，实现车体在旋转过程中同步校正车体位姿。

通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1示出了车体的结构示意图。

图2a示出了现有技术中车体原地旋转的一个示意图。

图2b示出了现有技术中车体原地旋转的另一个示意图。

图3a示出了现有技术中车体进行90度旋转后的位姿校正情况。

图3b示出了现有技术中车体进行180度旋转后的位姿校正情况。

图4示出本发明车体位姿校正方法的一个实施例的流程示意图。

图5a示出本发明车体角度轨迹的一个实施例的示意图。

图5b示出本发明车体角速度轨迹的一个实施例的示意图。

图6a示出本发明车体位姿校正过程的一个实施例的示意图。

图6b示出本发明车体旋转过程中的一个实施例的轨迹示意图。

图6c示出本发明车体旋转过程中的左右驱动轮的速度变化的一个实施例的示意图。

图7示出本发明车体位姿校正装置的一个实施例的结构示意图。

图8示出本发明车体位姿校正装置的另一个实施例的结构示意图。

图9示出本发明车体位姿校正装置的又一个实施例的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

发明人对agv转向的原理进行了分析。图1示出了车体的结构示意图。如果为agv的左右驱动轮设定大小相同方向相反的转动速度，在旋转的任意时刻，agv中心的运动速度恒为0，即agv中心在旋转过程中实时保持原位。因此不能在旋转过程中实现位姿校正。如果agv在旋转之前存在位置误差，旋转后这一误差仍然存在且近似保持不变。图2a以及图2b示出了现有技术中车体原地旋转的示意图。

为改善这一状况，如果将旋转中心配置在非车体中心位置，调整旋转过程中车体中心的位置，在agv旋转过程中为左右驱动轮设置大小不同的速度，可以在一定程度上实现agv位姿校正。具体实施方法为：以agv目标中心为原点建立坐标系，则agv满足如下的运动学关系：

其中，agv旋转中心距agv中心的距离(即旋转半径)为r，agv中心的当前位置为x,y，agv当前前进方向与agv目标前进方向的法线方向(即x轴)之间的夹角θ。对公式(1)进行积分，可得公式(2)：

由于公式(2)有两个线性方程，却仅有一个未知变量r，因此在非特殊情况下，不存在已知的r使得以上两个方程同时满足。因此，本方案仅能保证在旋转过程中校正单个坐标值，无法同时实现x,y两个坐标值的旋转矫正。图3a示出了车体进行90度旋转后的位姿校正情况，图3b示出了车体进行180度旋转后的位姿校正情况。可以看出，该方案仅能实现某一轴向的位姿校正，无法同时实现x,y两个轴方向的位姿校正。因此在前述应用场景下，该方案仍然无法避免出现货架顶歪的情况。

发明人研究认为，在根本上需要解决如下数学问题：设agv在任意时刻的位姿由状态三元组(x,y,θ)表示。已知：agv的初始状态记为(x0,y0,θ0),期望达到的终止状态记为(xf,yf,θf)，这里一般有x0≠0,y0≠0,xf＝0,yf＝0。求：规划agv在转动过程中的左右轮速vl(t),vr(t),t∈[t0,tf),使agv由初始状态转移至期望的终止状态。由于agv的运动学模型具有典型的非线性微分方程形式，直观上很难获得问题的求解方法，于是需要对agv转向姿态矫正进行运动学分析与建模。

(一)agv转向姿态矫正的运动学分析

双轮差动式agv的运动学模型为

其中，v表示agv前进方向的线速度，ω表示agv自身旋转的角速度，θ表示当前车体前进方向与x轴正向的夹角，即车体旋转角。方程(3)是agv运动过程满足的基本运动学关系，在此方程中假定agv在运动过程中不存在横移、打滑等现象。根据方程(3),我们可以看出agv的运动学方程是非线性的(即，位移增量(δx,δy)与偏转角θ满足三角函数关系),然而位移增量(δx,δy)关于运动速度v是线性的，这就为实现本发明的控制需求提供了条件。

假定agv由状态(x0,y0,θ0)转移至状态(xf,yf,θf),问题实际上涉及到了x,y,θ三个状态轨迹的规划问题。这里偏转角θ的轨迹可通过事先规划获得，意味着任意时刻的cos(θ),sin(θ)是已知的，即进一步问题可以作为一个线性动态问题进行处理。本问题中，自由变量为(xf-x0,yf-y0)共2个，有效方程个数也为2个，因此问题的解是存在的，且存在一个最简单解。

(二)agv转向姿态矫正的运动学建模

以角度θ而非时间t作为自变量，则有如下关系成立。记则有

由旋转半径的定义

agv运动学方程(3)可进一步变为

即微分方程组(6)变为关于变量θ的函数。根据差动轮agv旋转运动的运动学关系，通过调整agv旋转过程中的旋转半径可以实现旋转归位的需求。其推导过程如下：令旋转半径为旋转角θ的线性函数，记为

r(θ)＝aθ+b,(7)

其中，a,b为待确定的未知常数。基于前述分析，方程(7)的线性函数形式是满足问题要求的最简单形式的解。对式(6)进行积分,即

上述问题进一步变为

根据分部积分定理,有如下关系成立

基于式(10),式(9)进一步变为

上式中仅常数(a,b)为未知量，由式(11)为二元一次方程组，因此(a,b)的解是唯一的，分别为

其中，

下面结合图4描述本发明提供的车体位姿校正方法的一个实施例。

图4示出本发明车体位姿校正方法的一个实施例的流程示意图。如图4所示，该实施例的车体位姿校正方法包括：

步骤s402，将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数。其中，车体旋转角为车体目标前进方向的法线方向与车体当前前进方向之间的夹角，左驱动轮速度为右驱动轮速度为

在步骤s402中，首先将车体旋转半径r确定为车体旋转角θ的线性函数。其中，车体旋转半径r为旋转中心与车体中心之间的距离，车体中心位于车体的左驱动轮与右驱动轮的中点。具体包括：将车体旋转半径用车体旋转角、车体旋转角系数以及常数项表示，例如方程(7)所示；将车体中心的位置随时间的变化率确定为车体旋转角的函数，例如方程(6)所示；根据车体中心的位置随时间变化率与车体旋转角的函数关系，确定车体中心目标位置、车体中心当前位置以及车体旋转角的函数关系，例如方程(11)所示；利用车体中心目标位置、车体中心当前位置，表示车体旋转角系数以及常数项，例如方程(12)所示。

然后，根据线性函数(7)、车体的左驱动轮与右驱动轮之间的距离d/2，将车体的左驱动轮与右驱动轮的速度比值确定为车体旋转角的函数。

步骤s404，根据车体旋转角、车体中心目标位置以及车体中心当前位置的数值，确定车体的左驱动轮与右驱动轮的速度比值，并根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度。

本领域技术人员应理解，车体中心的当前位置以及车体当前前进方向可以通过车体的摄像头探测确定。

在步骤s404中，可以将车体的旋转角速度确定为随时间变化的函数，然后根据车体旋转角以及车体在各个时刻的旋转角速度，确定车体旋转的总时长，并根据车体旋转的总时长以及车体旋转的工作周期确定车体旋转的工作周期数，以便在每个工作周期内根据速度比值以及车体在各个时刻的旋转角速度为车体的左驱动轮与右驱动轮设定旋转速度。

例如，可以根据车体的属性设定车体匀变速旋转的角加速度以及最大旋转角速度，并根据车体匀变速旋转的角加速度以及最大旋转角速度，确定车体在各个时刻的旋转角速度。

下面给出车体先匀加速、然后匀速最后匀减速运动的应用例。

首先输入以下变量：当前agv姿态(x0,y0,θ0),期望的目标姿态(xf＝0,yf＝0,θf),最大转动角速度ωb,最大转动角加速度aωb,始末转动角速度为0，工作周期为ts。然后，根据agv的当前姿态和期望姿态，可以计算车体需要转动的角度θ；根据agv的运动性能及任务需求，确定匀变速转动的加减速性能参数。其中，匀变速转动的加速度不超过aωb，最大转动角速度不超过ωb。通过规划旋转角速度ω(t)可以求得的总转动时间记为t，转向所需的工作周期的个数得到角度轨迹θ(t)如图5a所示，角速度轨迹ω(t)如图5b所示。需要特别说明的是，由于agv实际运行并不是严格按规划的轨迹运动，因此为提高矫正精度，需在每一个工作周期重新计算新的r(θ)函数关系。即，将上一工作周期的状态(xi-1,yi-1,θi-1)和期望状态(xf,yf,θf)代入方程(7)，获得当前工作周期旋转半径所满足的关系式。

ri(θ)＝aiθ+bi.(14)

其中i表示当前工作周期数。然后，可以通过ccd摄像头扫描获得当前的状态(xi,yi,θi),或通过式运动学积分关系估计当前的状态，即

(xi,yi,θi)＝kinematics(xi-1,yi-1,θi-1,ts)(15)

由式(14)计算当前旋转半径ri(θi)，并计算当前规划的转动角速度ω(its)。设匀加速运动时间为0～tup，匀速运动时间为tup～tconst，匀减速运动时间为tconst～t。那么，在匀加速运动阶段(its)＜tup,ω(its)＝ω((i-1)ts)+awbts；在匀速运动阶段tup＜(its)＜tconst,ω(its)＝ω((i-1)ts)；在匀减速运动阶段(its)＞tconst,ω(its)＝ω((i-1)ts)-awbts。

规划当前工作周期内左右轮的驱动速度分别为

输出vl,vr至agv左右轮的伺服驱动器，等待ts时长后进行下一个工作周期。基于以上方法，可以精确计算从任意初始位姿pose(t0)到目标位姿pose(tf)的转移轨迹。图6a示出车体位姿校正过程的示意图，图6b示出车体转动过程中的轨迹示意图，图6c示出左右驱动轮速度变化的示意图。

上述实施例提出的车体位姿校正方法，可以实现车体在转向过程中同步进行误差消除，使得车体在转向后能精确停在货架的中心位置，保证货架在准确位置顶起货架，从而避免仓储系统内部发生事故。与此同时，由于实现了车体在旋转过程中同步进行误差消除，无需在旋转过程中停止运动，节省了小车的总运动时长，提高了小车的工作效率。此外，本发明的方法易于实现，所建立的计算模型使得求解过程简单省时。

下面结合图7描述本发明一个实施例的车体位姿校正装置。

图7示出本发明车体位姿校正装置的一个实施例的结构示意图。如图7所示，该实施例的车体位姿校正装置70包括：

函数确定模块702，配置为将车体的左驱动轮与右驱动轮的速度比值确定为随车体旋转角、车体中心目标位置以及车体中心当前位置变化的函数，车体旋转角为车体目标前进方向的法线方向与车体当前前进方向之间的夹角。

速度设定模块704，配置为根据车体旋转角、车体中心目标位置以及车体中心当前位置的数值，确定车体的左驱动轮与右驱动轮的速度比值，并根据速度比值为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，函数确定模块702配置为：

将车体旋转半径确定为车体旋转角的线性函数，其中，车体旋转半径为旋转中心与车体中心之间的距离，车体中心位于车体的左驱动轮与右驱动轮的中点；

根据线性函数、车体的左驱动轮与右驱动轮之间的距离，将车体的左驱动轮与右驱动轮的速度比值确定为车体旋转角的函数。

在一个实施例中，函数确定模块702配置为：

将车体旋转半径用车体旋转角、车体旋转角系数以及常数项表示；

将车体中心的位置随时间的变化率确定为车体旋转角的函数；

根据车体中心的位置随时间变化率与车体旋转角的函数关系，确定车体中心目标位置、车体中心当前位置以及车体旋转角的函数关系；

利用车体中心目标位置、车体中心当前位置，表示车体旋转角系数以及常数项。

在一个实施例中，所述车体中心的当前位置以及所述车体当前前进方向通过车体的摄像头探测确定。

在一个实施例中，速度设定模块704配置为：

将车体的旋转角速度确定为随时间变化的函数；

根据车体旋转角以及车体在各个时刻的旋转角速度，确定车体旋转的总时长；

根据车体旋转的总时长以及车体旋转的工作周期，确定车体旋转的工作周期数；

在每个工作周期内，根据速度比值以及车体在各个时刻的旋转角速度，为车体的左驱动轮与右驱动轮设定旋转速度。

在一个实施例中，速度设定模块704配置为：

根据车体的属性，设定车体匀变速旋转的角加速度以及最大旋转角速度；

根据车体匀变速旋转的角加速度以及最大旋转角速度，确定车体在各个时刻的旋转角速度。

上述实施例提出的车体位姿校正装置，可以实现车体在转向过程中同步进行误差消除，使得车体在转向后能精确停在货架的中心位置，保证货架在准确位置顶起货架，从而避免仓储系统内部发生事故。与此同时，由于实现了车体在旋转过程中同步进行误差消除，无需在旋转过程中停止运动，节省了小车的总运动时长，提高了小车的工作效率。此外，本发明的方法易于实现，所建立的计算模型使得求解过程简单省时。

图8示出了本发明车体位姿校正装置的另一个实施例的结构图。如图8所示，该实施例的车体位姿校正装置80包括：存储器810以及耦接至该存储器810的处理器820，处理器820被配置为基于存储在存储器810中的指令，执行前述任意一个实施例中的车体位姿校正方法。

存储器810例如可以包括系统存储器、固定非易失性存储介质等。系统存储器例如存储有操作系统、应用程序、引导装载程序(bootloader)以及其他程序等。

图9示出了本发明车体位姿校正装置的又一个实施例的结构图。如图9所示，该实施例的装置90包括：存储器810以及处理器820，还可以包括输入输出接口930、网络接口940、存储接口950等。这些接口930，940，950以及存储器810和处理器820之间例如可以通过总线950连接。其中，输入输出接口930为显示器、鼠标、键盘、触摸屏等输入输出设备提供连接接口。网络接口940为各种联网设备提供连接接口。存储接口950为sd卡、u盘等外置存储设备提供连接接口。

本发明还包括一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现前述任意一个实施例中的车体位姿校正方法。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。