CN114862301A - 基于二维码辅助定位的托盘叉车agv自动装载方法 - Google Patents

Abstract

本发明公开了一种基于二维码辅助定位的托盘叉车AGV自动装载方法，该方法利用相机获取布设有二维码标签的仓储托盘图像，对图像进行二维码标签检测；基于PnP方法解算标签中心在相机坐标系下的位姿，通过坐标转换得到仓储托盘参考点在相机坐标系下的位姿；根据仓储托盘局部位姿和托盘叉车AGV全局位姿计算仓储托盘全局位姿；进一步获得从托盘叉车AGV当前点到仓储托盘参考点的取货路径；控制托盘叉车AGV跟踪取货路径，实现托盘叉车AGV对仓储托盘的全自动精准装载。本发明不需要指定托盘颜色和尺寸，与传统技术相比，具有位姿估计准确性高、路径跟踪效果好的特点，显著提高了托盘自动装载成功率，在保证自动搬运安全可靠的前提下，有效提升搬运效率。

Description

基于二维码辅助定位的托盘叉车AGV自动装载方法

技术领域

本发明涉及移动机器人自主导航控制，仓储托盘检测技术领域，尤其涉及一种基于二维码辅助定位的托盘叉车AGV自动装载方法。

背景技术

互联网经济的发展，推动着传统物流配送模式不断变革升级，如何提高物流效率、降低物流成本已经成为焦点问题。自智慧物流概念被首次全方位提起以来，物流在多个环节正在日益实现自动化、智能化，一些重复度高、强度大的工作逐步被智能机器人完成。同时，人口老龄化加剧使得高度自动化的智能仓储成为迫切需求，以托盘叉车AGV为代表自主移动机器人自动搬运在物流业的应用越来越广泛，智慧物流也已经变成行业的风向标。智慧物流能够在装载、搬运、分拣等环节上提升效率并降低失误率，提高社会生产力水平。

在实际应用中，物流仓储环境复杂多变，存在静态特征物稀疏、光照强度不均、全局导航累计误差等影响因素，托盘叉车AGV依靠自主定位导航能力到达库位点附近准备取货，由于缺乏对装载目标的自动识别与定位能力，托盘叉车AGV无法获取仓储托盘的局部位姿信息，装载时极易发生碰撞、叉取不充分甚至损坏货物等问题，存在很大的安全隐患。为防止事故发生，需要人工监督和调整，虽然一定程度上减少了人工负担，但无法最大化发挥托盘叉车AGV搬运效率高、载重量大的特点。可见，仓储托盘检测与自适应位姿镇定功能是托盘叉车AGV不可或缺的关键技术，是实现物流仓储无人化作业至关重要的环节。

研究仓储托盘检测与位姿估计算法，以及自适应位姿镇定算法，对于提升托盘叉车AGV智能化水平，在物流装载、搬运、分拣等环节上降低出错率、提高效率，最终实现全自动搬运具有重大意义，也是智慧物流研究的核心问题之一。

发明内容

本发明的目的在于克服实现叉车自动搬运存在的种种困难以及现有仓储托盘检测方法的不足，提供一种基于二维码辅助定位的托盘叉车AGV自动装载方法，该方法涉及仓储托盘检测和移动机器人自适应位姿镇定：首先检测出托盘上的二维码标签；然后基于PnP方法解算标签中心的位姿，实现对托盘的位姿估计，并结合托盘叉车AGV的全局位姿计算并更新托盘状态。AGV自动装载部分首先使用局部路径规划算法制定最优取货路径并使用贝塞尔曲线平滑处理；然后提出用于路径跟踪的核心导航控制算法；最后在托盘叉车AGV实验平台上算法实现。

本发明的目的是通过以下技术方案实现的：一种基于二维码辅助定位的托盘叉车AGV自动装载方法，该方法包括以下步骤：

步骤1、利用安装在托盘叉车AGV的相机获取布设有二维码标签的仓储托盘的原始RGB图像；

步骤2、对仓储托盘原始RGB图像进行二维码标签区域检测，若未检测到二维码标签区域，则继续获取下一帧图像；

步骤3、基于PnP方法解算二维码标签中心在相机坐标系下的六自由度位姿，通过坐标转换得到仓储托盘参考点在相机坐标系下的位姿，实现对仓储托盘的局部位姿估计；

步骤4、根据步骤3得到的仓储托盘局部位姿和托盘叉车AGV全局位姿，计算仓储托盘全局位姿；

步骤5、根据仓储托盘当前全局位姿和托盘叉车AGV当前全局位姿，获得从托盘叉车AGV当前点到仓储托盘参考点的取货路径；

步骤6、控制托盘叉车AGV跟踪取货路径，实现托盘叉车AGV对仓储托盘的全自动精准装载。

进一步地，所述二维码标签布设在仓储托盘取货侧中心立柱的外侧面，将仓储托盘上与二维码标签中心对称的点作为仓储托盘参考点。

进一步地，所述步骤2中，对于仓储托盘原始RGB图像，通过图像灰度化、自适应阈值处理、连续边界分割、四边形拟合、解码匹配，检测出布设在仓储托盘上的二维码标签。

进一步地，所述连续边界分割具体为：基于自适应阈值处理得到的二值图的黑色和白色像素信息分割边缘；使用联合查找算法对明暗像素的连接像素簇进行分割，并用唯一的ID代表每个像素簇。

进一步地，所述四边形拟合具体为：先找到少量角点，然后遍历角点的所有组合方式来计算近似分组，输出一组或多组候选四边形；

所述解码匹配具体为：将四边形包含的四个方向的码值分别与标签簇中的每个标签代码进行异或比较，过滤错误的候选四边形。

进一步地，所述步骤3具体为：

3-1、通过步骤2检测到的目标二维码标签，结合相机内参矩阵、二维码标签物理尺寸和相机单应矩阵，求解二维码标签中心在相机坐标系下的六自由度位姿信息，即相机坐标系下三个方向的坐标：X_c、Z_c、Y_c和欧拉角：偏航角

俯仰角和横滚角；

3-2、将二维码标签中心点在相机坐标系下的位姿表示为O:

坐标转换之后，得到仓储托盘参考点在相机坐标系下的位姿P:

其参考点与标签中心存在横向和纵向偏差，托盘边长假设为a，则托盘参考点的位姿表示为：

进一步地，所述步骤4中，计算仓储托盘全局位姿具体为：

相机光心到托盘叉车AGV参考点存在位姿偏差，假设纵向偏差为x_c、横向偏差为y_c、角度偏差为θ；由相机坐标系到叉车AGV坐标系，旋转矩阵R表示为：

令托盘叉车AGV在世界坐标系下的位姿为(X_w,Y_w,α)，仓储托盘在相机坐标系下的位姿为

则仓储托盘在世界坐标系下的位姿计算公式为：

其中(X_wp,Y_wp)是仓储托盘在世界坐标系下的全局坐标，φ是仓储托盘在世界坐标系下的全局偏航角；仓储托盘参考点在全局坐标系下的状态表示为：(X_wp,Y_wp,φ)。

进一步地，所述步骤5中，根据仓储托盘当前全局位姿和托盘叉车AGV当前全局位姿，获得从托盘叉车AGV当前点到仓储托盘参考点的最优取货路径，并使用贝塞尔曲线平滑处理，取货路径F(t)的表达式为：

F(t)＝(1-t)³P₁+3t(1-t)²P₂+3t²(1-t)P₃+t³P₄ t∈[0,1]

上式为三阶贝塞尔曲线参数方程，比例系数t取值为0到1；P₁为起点，即托盘叉车AGV的当前位姿，P₄为终点，即仓储托盘参考点，P₂和P₃为控制点，人为设定；通过人为选取两个控制点进行路径拟合，拟合之后取货路径更加平滑。

进一步地，控制点P₂和P₃分别设定为从托盘叉车AGV当前世界坐标到二维码标签中心世界坐标连线的中点，以及二维码标签中心世界坐标到仓储托盘参考点连线的中点。

进一步地，所述步骤6中，通过计算角速度和速度下发给叉车底盘，控制其运动，实现托盘叉车AGV跟踪取货路径，具体为：

6-1、角速度计算公式表示为：

ω＝K₁Δy+K₂β+K₃γ

其中Δy为当前位姿点到取货路径上目标点的切线的距离，β表示叉车当前位姿的偏航角，γ表示叉车当前位姿点与目标点之间的角度偏差；输入参数K₁，K₂，K₃为固定参数，三个参数的选择会影响路径跟踪效果，输出结果为角速度ω；

6-2、线速度的计算具体为：

计算当前点到目标点的距离d：

其中a为加速度，v_cur和v_min分别为当前速度和速度下限；假设当前位姿点到目标点的投影距离是Δx；

如果d小于等于Δx，则线速度计算公式为：

如果d大于Δx，则线速度计算公式为：

v＝v+aT

此处v有最大速度v_max限制，即v≤v_max，其中T为控制周期。

本发明的有益结果是：本发明提供的一种基于二维码辅助定位的托盘叉车AGV自动装载方法，该方法分为两部分：首先通过阈值处理、连续边界分割、四边形拟合、解码匹配等步骤检测出托盘上的二维码标签；然后基于PnP方法解算标签中心的位姿，实现对托盘的位姿估计，并结合托盘叉车AGV的全局位姿计算并更新托盘状态。AGV自动装载部分首先使用局部路径规划算法制定最优取货路径并使用贝塞尔曲线平滑处理；然后提出用于路径跟踪的核心导航控制算法；最后在托盘叉车AGV实验平台上算法实现。本发明不需要指定托盘颜色和尺寸，与传统技术相比，具有位姿估计准确性高、路径跟踪效果好的特点，显著提高了托盘自动装载成功率，在保证自动搬运安全可靠的前提下，有效提升搬运效率。

附图说明

图1为本发明基于二维码辅助定位的托盘叉车AGV自动装载方法的流程图。

图2为仓储托盘检测与位姿估计结果图。

图3为仓储托盘检测与位姿估计俯视图。

图4为仓储托盘与叉车位姿关系示意图。

图5为路径跟踪算法原理图。

具体实施方式

下面结合附图进一步清楚、完整地描述本发明的技术方案，但所描述的实施例仅仅是本发明一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护范围。

如图1所示，本实施例提供的基于二维码辅助定位的托盘叉车AGV自动装载方法，包括以下步骤：

步骤1、利用安装在托盘叉车AGV的相机获取布设有二维码标签的仓储托盘的原始RGB图像；

在一个实施例中，步骤1的实现具体如下：

1-1、相机安装在托盘叉车AGV右叉腿末端，相机采用Realsense D435，RealsenseD435相机的RGB摄像头实时传输速度达到30fps，图像分辨率支持1920×1080，RealsenseD435相机的固件版本为Signed_Image_UVC_5_12_14_50.bin，相机与控制器之间使用USB3.1进行数据传输；

1-2、仓储托盘布设的二维码标签选取Apriltag基准库中的Tag36h11族，共有587个不同的标签，标签尺寸为60mm×60mm；

1-3、二维码标签布设在仓储托盘立柱外侧面，优选为取货侧面中心立柱的外侧面，如图2所示，将仓储托盘上与二维码标签中心对称的点作为仓储托盘参考点，如图3中P为参考点；

1-4、将D435相机的图像格式、分辨率、帧率等参数设置为统一格式，D435相机进行硬件初始化、软件初始化后，开启D435相机，开始获取含有仓储托盘的RGB图像。

步骤2、对仓储托盘原始RGB图像，通过图像灰度化、自适应阈值处理、连续边界分割、四边形拟合、解码匹配等方法进行二维码标签区域检测，若未检测到二维码标签区域，则继续获取下一帧图像；图2为检测结果示例；在一个实施例中，步骤2的实现具体如下：

(1)将RGB图像灰度化；

(2)自适应阈值处理：采用自适应阈值方法将输入灰度图像处理为二值图像，取4×4像素块中的极大值max和极小值min计算阈值，根据阈值形成二值图，阈值计算公式为：

T＝(max+min)/2

(3)连续边界分割：为提高分割准确率，基于所产生的黑色和白色像素信息分割边缘。使用联合查找算法(Union-Find)对明暗像素的连接像素簇进行分割，并用唯一的ID代表每个像素簇；

(4)四边形拟合：先找到少量角点，然后遍历角点的所有组合方式来计算近似分组。四边形拟合步骤输出一组或几组候选四边形，环境中的许多四边形结构，包括开关、托盘网格、标签单个比特位等都能找到；

(5)快速解码匹配：将四边形包含的四个方向的码值分别与标签簇中的每个标签代码进行异或比较，过滤掉错误的候选四边形。

步骤3、基于PnP(Perspective-n-Point)方法解算二维码标签中心在相机坐标系下的六自由度位姿，通过坐标转换得到仓储托盘参考点在相机坐标系下的位姿，实现对仓储托盘的局部位姿估计，如图3所示；在一个实施例中，步骤3的实现具体如下：

(1)PnP是求解3D到2D点对运动的方法，可以根据空间点坐标及其投影位置解算相机的位姿。通过上述步骤2已经检测到目标二维码标签，接下去需要结合相机内参矩阵、二维码标签的物理尺寸和相机的单应矩阵求解二维码标签中心在相机坐标系下的6DOF位姿信息，即相机坐标系下三个方向的坐标：X_c、Z_c、Y_c和欧拉角：偏航角

俯仰角和横滚角。

(2)通过坐标转换，得到仓储托盘参考点在相机坐标系下的位姿，实现对仓储托盘的局部位姿估计。在实际物流应用场景中，相对位姿关系只需要考虑横向偏移X_c、纵向偏移Z_c以及偏航角

二维码标签中心点在相机坐标系下的位姿表示为O:

坐标转换之后，得到仓储托盘的参考点在相机坐标系下的位姿P:

其参考点与标签中心存在横向和纵向偏差，托盘边长假设为a，则托盘参考点P的位姿可以表示为：

步骤4、根据步骤3得到的仓储托盘局部位姿和托盘叉车AGV全局位姿，计算仓储托盘全局位姿，如图4所示；在一个实施例中，步骤4的实现具体如下：

相机光心O_C到托盘叉车AGV参考点O_F存在位姿偏差，假设纵向偏差为x_c、横向偏差为y_c、角度偏差为θ；由相机坐标系到叉车AGV坐标系，旋转矩阵R表示为：

托盘叉车AGV通常可基于激光雷达传感器实现全局地图下的定位，通过预留接口能够获取其全局的实时位姿。令托盘叉车AGV在世界坐标系下的位姿为(X_w,Y_w,α)，仓储托盘在相机坐标系下的位姿为

则仓储托盘在世界坐标系下的位姿计算公式为：

其中(X_wp,Y_wp)是仓储托盘在世界坐标系下的全局坐标，φ是仓储托盘在世界坐标系下的全局偏航角；仓储托盘参考点在全局坐标系下的状态表示为：(X_wp,Y_wp,φ)。

步骤5、根据仓储托盘当前全局位姿和托盘叉车AGV当前全局位姿，获得从托盘叉车AGV当前点到仓储托盘参考点的取货路径；在一个实施例中，使用局部路径跟踪算法获得从托盘叉车AGV当前点到仓储托盘参考点的最优取货路径，并使用贝塞尔曲线平滑处理，取货路径F(t)的表达式为：

F(t)＝(1-t)³P₁+3t(1-t)²P₂+3t²(1-t)P₃+t³P₄ t∈[0,1]

上式为三阶贝塞尔曲线参数方程，比例系数t取值为0到1；P₁为起点，即托盘叉车AGV的当前位姿，P₄为终点，即仓储托盘参考点，P₂和P₃为控制点，人为设定，优选为从托盘叉车AGV当前世界坐标到二维码标签中心世界坐标连线的中点，以及二维码标签中心世界坐标到仓储托盘参考点连线的中点；通过人为选取两个控制点进行路径拟合，拟合之后取货路径更加平滑。

步骤6、控制托盘叉车AGV跟踪取货路径，实现托盘叉车AGV对仓储托盘的全自动精准装载，具体为：如图5所示，通过计算角速度和速度下发给叉车底盘，控制其运动，实现托盘叉车AGV跟踪取货路径：

6-1、角速度计算公式表示为：

ω＝K₁Δy+K₂β+K₃γ

其中Δy为当前位姿点到取货路径上目标点的切线的距离，β表示叉车当前位姿的偏航角，γ表示叉车当前位姿点与目标点之间的角度偏差；输入参数K₁，K₂，K₃为固定参数，三个参数的选择会影响路径跟踪效果，输出结果为角速度ω；

6-2、线速度的计算具体为：

计算当前点到目标点的距离d：

其中a为加速度，v_cur和v_min分别为当前速度和速度下限；假设当前位姿点到目标点的投影距离是Δx；

如果d小于等于Δx，则线速度计算公式为：

如果d大于Δx，则线速度计算公式为：

v＝v+aT

此处v有最大速度v_max限制，即v≤v_max，其中T为控制周期。

本发明不需要指定托盘颜色和尺寸，与传统技术相比，具有位姿估计准确性高、路径跟踪效果好的特点，显著提高了托盘自动装载成功率，在保证自动搬运安全可靠的前提下，有效提升搬运效率。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

上述对本说明书特定实施例进行了描述。其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。

在本说明书一个或多个实施例使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本说明书一个或多个实施例。在本说明书一个或多个实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本说明书一个或多个实施例可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本说明书一个或多个实施例范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

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

Claims (10)

1.一种基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，包括：

步骤1、利用安装在托盘叉车AGV的相机获取布设有二维码标签的仓储托盘的原始RGB图像；

步骤2、对仓储托盘原始RGB图像进行二维码标签区域检测，若未检测到二维码标签区域，则继续获取下一帧图像；

步骤3、基于PnP方法解算二维码标签中心在相机坐标系下的六自由度位姿，通过坐标转换得到仓储托盘参考点在相机坐标系下的位姿，实现对仓储托盘的局部位姿估计；

步骤4、根据步骤3得到的仓储托盘局部位姿和托盘叉车AGV全局位姿，计算仓储托盘全局位姿；

步骤5、根据仓储托盘当前全局位姿和托盘叉车AGV当前全局位姿，获得从托盘叉车AGV当前点到仓储托盘参考点的取货路径；

步骤6、控制托盘叉车AGV跟踪取货路径，实现托盘叉车AGV对仓储托盘的全自动精准装载。

2.根据权利要求1所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述二维码标签布设在仓储托盘取货侧中心立柱的外侧面，将仓储托盘上与二维码标签中心对称的点作为仓储托盘参考点。

3.根据权利要求1所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述步骤2中，对于仓储托盘原始RGB图像，通过图像灰度化、自适应阈值处理、连续边界分割、四边形拟合、解码匹配，检测出布设在仓储托盘上的二维码标签。

4.根据权利要求3所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述连续边界分割具体为：基于自适应阈值处理得到的二值图的黑色和白色像素信息分割边缘；使用联合查找算法对明暗像素的连接像素簇进行分割，并用唯一的ID代表每个像素簇。

5.根据权利要求3所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述四边形拟合具体为：先找到少量角点，然后遍历角点的所有组合方式来计算近似分组，输出一组或多组候选四边形；

所述解码匹配具体为：将四边形包含的四个方向的码值分别与标签簇中的每个标签代码进行异或比较，过滤错误的候选四边形。

6.根据权利要求1-5中任一项所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述步骤3具体为：

3-1、通过步骤2检测到的目标二维码标签，结合相机内参矩阵、二维码标签物理尺寸和相机单应矩阵，求解二维码标签中心在相机坐标系下的六自由度位姿信息，即相机坐标系下三个方向的坐标：X_c、Z_c、Y_c和欧拉角：偏航角

俯仰角和横滚角；

3-2、将二维码标签中心点在相机坐标系下的位姿表示为

坐标转换之后，得到仓储托盘参考点在相机坐标系下的位姿

其参考点与标签中心存在横向和纵向偏差，托盘边长假设为a，则托盘参考点的位姿表示为：

7.根据权利要求1所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述步骤4中，计算仓储托盘全局位姿具体为：

相机光心到托盘叉车AGV参考点存在位姿偏差，假设纵向偏差为x_c、横向偏差为y_c、角度偏差为θ；由相机坐标系到叉车AGV坐标系，旋转矩阵R表示为：

令托盘叉车AGV在世界坐标系下的位姿为(X_w,Y_w,α)，仓储托盘在相机坐标系下的位姿为

则仓储托盘在世界坐标系下的位姿计算公式为：

其中(X_wp,Y_wp)是仓储托盘在世界坐标系下的全局坐标，φ是仓储托盘在世界坐标系下的全局偏航角；仓储托盘参考点在全局坐标系下的状态表示为：(X_wp,Y_wp,φ)。

8.根据权利要求1所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述步骤5中，根据仓储托盘当前全局位姿和托盘叉车AGV当前全局位姿，获得从托盘叉车AGV当前点到仓储托盘参考点的最优取货路径，并使用贝塞尔曲线平滑处理，取货路径F(t)的表达式为：

F(t)＝(1-t)³P₁+3t(1-t)²P₂+3t²(1-t)P₃+t³P₄ t∈[0,1]

上式为三阶贝塞尔曲线参数方程，比例系数t取值为0到1；P₁为起点，即托盘叉车AGV的当前位姿，P₄为终点，即仓储托盘参考点，P₂和P₃为控制点，人为设定；通过人为选取两个控制点进行路径拟合，拟合之后取货路径更加平滑。

9.根据权利要求8所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，控制点P₂和P₃分别设定为从托盘叉车AGV当前世界坐标到二维码标签中心世界坐标连线的中点，以及二维码标签中心世界坐标到仓储托盘参考点连线的中点。

10.据权利要求1所述的基于二维码辅助定位的托盘叉车AGV自动装载方法，其特征在于，所述步骤6中，通过计算角速度和速度下发给叉车底盘，控制其运动，实现托盘叉车AGV跟踪取货路径，具体为：

6-1、角速度计算公式表示为：

ω＝K₁Δy+K₂β+K₃γ

其中Δy为当前位姿点到取货路径上目标点的切线的距离，β表示叉车当前位姿的偏航角，γ表示叉车当前位姿点与目标点之间的角度偏差；输入参数K₁，K₂，K₃为固定参数，三个参数的选择会影响路径跟踪效果，输出结果为角速度ω；

6-2、线速度的计算具体为：

计算当前点到目标点的距离d：

其中a为加速度，v_cur和v_min分别为当前速度和速度下限；假设当前位姿点到目标点的投影距离是Δx；

如果d小于等于Δx，则线速度计算公式为：

如果d大于Δx，则线速度计算公式为：

v＝v+aT

此处v有最大速度v_max限制，即v≤v_max，其中T为控制周期。

Images