一种基于库位特征识别的AGV无标记定位停靠方法及系统与流程

本发明涉及自动导向车（agv）控制，具体涉及一种基于库位特征识别的agv无标记定位停靠方法及系统，尤其适用于无需在地面或货架预设二维码、磁条等人工标记的自动化仓储场景。

背景技术：

1、在自动化仓储物流系统中，agv在行驶至目标库位（如货架库位、接驳台、充电桩等）进行货物对接时，需要极高的最终停靠精度。目前主流的解决方案是在每个库位对应的地面或货架立柱上粘贴二维码，agv通过车载相机识别二维码，获取自身相对于库位的精确位姿，从而实现精准停靠。

2、然而，这种依赖二维码的定位方式存在以下固有缺陷：

3、前期部署成本高、周期长：需要在成千上万个库位的地面逐一粘贴二维码，并精确测量每个二维码的世界坐标录入系统，工程量大。

4、后期维护困难：二维码易被叉车轮胎碾压、被灰尘油污覆盖、或因货物搬运而磨损、脱落，导致定位失效，需要频繁的人工巡检和更换。

5、环境适应性差：在冷库、潮湿、多尘等恶劣环境下，二维码的寿命和识别率会大幅下降。

6、灵活性不足：库位布局调整时，需要重新粘贴二维码并更新地图数据，不够灵活。

7、现有技术中虽有视觉定位方案，但大多仍依赖人工设置的特殊标记（如圆形色块、编码点等），未能实现真正意义上的无标记定位。

8、因此，亟需一种不依赖人工标记、能够直接利用库位本身结构特征进行高精度定位停靠的方法。

技术实现思路

1、针对现有技术中存在的上述问题，本发明的目的在于提供一种基于库位特征识别的agv无标记定位停靠方法及系统，通过3d相机直接感知库位的物理结构，计算库位中心点，实现无需人工标记的高精度停靠。

2、本发明通过以下技术方案来实现上述目的：

3、一种基于库位特征识别的agv无标记定位停靠方法，包括：

4、步骤s1：agv接收到调度系统下发的对接任务，任务包含目标库位的标识信息及其在世界坐标系下的粗略位置；

5、步骤s2：agv根据粗略位置导航行驶至目标库位的预设识别范围内，启动安装在agv上的3d视觉采集模块；

6、步骤s3：通过3d视觉采集模块采集包含目标库位的三维点云数据或深度图像数据；

7、步骤s4：对采集到的三维点云数据进行处理，识别出目标库位的固有结构特征；固有结构特征包括库位两侧立柱、库位底部边角、库位后挡板、或货架层板的边缘；

8、步骤s5：根据识别出的固有结构特征，通过几何拟合或计算，解算出目标库位的中心点坐标，并进一步计算agv当前位姿相对于该库位中心点的偏差量，当前位姿包括位置和航向角；

9、步骤s6：根据计算得到的偏差量，控制agv进行姿态调整并精确停靠在目标库位处。

10、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，在步骤s1中，调度系统接收上层管理系统下发的搬运任务，搬运任务中包含目标物料的标识信息；调度系统根据目标物料的标识信息查询库位管理数据库，获取物料当前所在的目标库位的标识信息及其在仓库地图中的库位坐标；调度系统将库位坐标映射至世界坐标系下，得到目标库位在世界坐标系下的粗略位置；调度系统将包含目标库位标识信息和粗略位置的对接任务通过无线通信方式下发给agv。

11、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，在步骤s2中，agv根据自身当前位姿与目标库位的粗略位置，通过路径规划算法规划从当前位置到目标库位所在区域的导航路径；

12、agv沿导航路径行驶，行驶过程中通过agv自身的导航系统实时获取agv在世界坐标系下的当前位姿，并根据当前位姿与目标库位的粗略位置实时计算agv与目标库位之间的剩余距离；

13、当agv行驶至距离目标库位的粗略位置小于或等于第一预设距离阈值时，agv减速至第一预设速度并进入接近模式；当agv继续行驶至距离目标库位的粗略位置小于或等于第二预设距离阈值时，agv减速至第二预设速度；

14、当agv行驶至距离目标库位的粗略位置小于或等于第三预设距离阈值时，判定agv已进入目标库位的预设识别范围内，agv减速至停止，并启动安装在agv上的3d视觉采集模块，开始执行步骤s3。

15、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，在步骤s3中，在agv减速至停止并判定已进入目标库位的预设识别范围后，向3d视觉采集模块发送采集触发信号；3d视觉采集模块响应于采集触发信号，按照预设的采集参数对目标库位所在区域进行一次或多次扫描采集，生成包含目标库位的原始三维点云数据或深度图像数据；采集参数包括采集帧率、分辨率及曝光时间；

16、对采集到的原始三维点云数据或深度图像数据进行格式转换，将其从相机坐标系下的原始数据格式转换为所需的标准化数据格式；

17、将格式转换后的三维点云数据或深度图像数据输出至库位特征识别模块，进行目标库位固有结构特征的识别。

18、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，在步骤s4中，对采集到的三维点云数据进行处理，包括：采用体素下采样算法降低点云密度以减少数据量，并采用统计滤波算法剔除离群噪声点，得到降噪后的三维点云数据；

19、对降噪后的三维点云数据，采用欧几里得聚类算法将点云分割为若干个独立的点云簇，每个点云簇对应于一个独立的物理目标；

20、根据库位立柱几何约束条件，从分割得到的若干独立点云簇中筛选出分别对应于目标库位左侧立柱和右侧立柱的两个点云簇；库位立柱几何约束条件包括立柱的高度范围和宽度范围；

21、将筛选出的左侧立柱点云簇和右侧立柱点云簇分别投影至水平面，对投影后的点云分别采用最小二乘法拟合直线方程 x·sinθ - y·cosθ+ d= 0，求解得到左侧立柱中心轴线参数(θl, dl)和右侧立柱中心轴线参数(θr, dr)，左侧立柱中心轴线与右侧立柱中心轴线共同构成目标库位的固有结构特征识别结果。

22、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，对于分割得到的m个独立点云簇，分别计算每个点云簇的几何特征参数：

23、高度：，其中，和分别为点云簇在竖直方向上的最大高度值和最小高度值；

24、宽度：，其中，(,)为点云簇中第个点的水平坐标，(,)为点云簇的水平质心坐标；

25、体积：，其中，为点云簇所包含的点数，为体素下采样后单个体素的体积；

26、根据预设的立柱高度范围[,]、立柱宽度范围[,]和立柱体积范围[,]，对

m个独立点云簇进行初筛，剔除不满足以下任一条件的点云簇：

27、

28、将满足全部条件的点云簇保留，记初筛后的点云簇集合为：

29、

30、其中，

k为初筛后保留的点云簇数量。

31、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，对初筛后的点云簇集合ω进行两两组合配对，对于任意两个点云簇和，计算其组合特征参数：

32、水平间距：

33、其中，(,)和(,)分别为点云簇和的水平质心坐标；

34、高度一致性：

35、宽度一致性：

36、根据预设的库位标准宽度范围[,]、最大允许高度偏差和最大允许宽度偏差，从所有配对组合中筛选满足以下全部条件的候选配对集合ψ：

37、

38、对候选配对集合ψ进行最优配对选择，选取使水平间距与库位标准宽度之差的绝对值最小的配对(,)作为最优配对：

39、

40、其中，为库位标准宽度；

41、根据最优配对中两个点云簇的水平质心坐标(,)和(,)的左右位置关系，确定左侧立柱点云簇和右侧立柱点云簇：

42、

43、若候选配对集合ψ为空，则判定库位特征识别失败，终止当前识别流程并向上位系统发送识别失败报警信号。

44、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，步骤s5中解算库位中心点坐标的方法包括以下步骤：

45、获取拟合得到的左侧立柱中心轴线参数(θl, dl)和右侧立柱中心轴线参数(θr,dr)，分别计算左侧立柱中心轴线和右侧立柱中心轴线在水平面上的直线方程：

46、左侧立柱中心轴线直线方程：x·sinθl- y·cosθl+ dl= 0；

47、右侧立柱中心轴线直线方程：x·sinθr- y·cosθr+ dr= 0；

48、计算左侧立柱中心轴线与右侧立柱中心轴线的中点连线，形成库位中线，具体包括：

49、计算左侧立柱中心轴线上任意一点pl的坐标，以及右侧立柱中心轴线上与pl处于同一水平高度yh（即y坐标值相同）的对应点pr的坐标，按下式计算pl和pr的中点pm：

50、pm= (pl+ pr) / 2；

51、令y坐标值从ymin到ymax以预设步长δy连续变化，计算每个水平高度下对应的中点pm(y)，将所有中点pm(y)连接形成库位中线的直线方程；库位中线为左侧立柱中心轴线与右侧立柱中心轴线的对称中心线，其方向向量vmid= vl+ vr，其中vl和vr分别为左侧立柱中心轴线和右侧立柱中心轴线的方向单位向量。

52、根据本发明提供的一种基于库位特征识别的agv无标记定位停靠方法，采用ransac随机抽样一致性算法，对降噪后的三维点云数据中位于目标库位后部区域的点云进行平面拟合，识别出目标库位的后挡板平面方程：

53、af·x + bf·y + cf·z + df= 0；

54、同时，采用ransac随机抽样一致性算法，对降噪后的三维点云数据中位于目标库位底部区域的点云进行平面拟合，识别出目标库位的底部平面方程：

55、ag·x + bg·y + cg·z + dg= 0；

56、确定库位中心点pcenter的三维坐标，其中：

57、水平坐标确定方式为：计算库位中线与后挡板平面的交点pintersect，取库位中线上沿库位中线方向向量vmid距交点pintersect预设深度偏移量loffset处的点作为库位中心点的水平坐标pcenterhorizontal，即：

58、pcenterhorizontal= pintersect+ loffset· vmid；

59、其中，预设深度偏移量 loffset根据agv货叉插入库位所需的深度预先设定；

60、高度坐标确定方式为：取底部平面方程的高度值zfloor，叠加预设举升偏移量hlift，得到库位中心点的高度坐标：

61、zcenter= zfloor+ hlift；

62、其中，zfloor= -dg/ cg，预设举升偏移量hlift根据agv货叉叉取货物所需的举升高度预先设定；

63、最终得到库位中心点pcenter的三维坐标为：

64、pcenter= (xcenter, ycenter, zcenter)；

65、其中，xcenter和ycenter为pcenterhorizontal的x坐标和y坐标，zcenter= zcenter；

66、获取agv当前在世界坐标系下的位姿信息，包括agv中心点坐标pagv= (xagv, yagv,zagv)和agv航向角θagv，计算agv相对于库位中心点的偏差量：

67、横向偏移：δx = xagv- xcenter；

68、纵向偏移：δy = yagv- ycenter；

69、角度偏差：δθ = θagv- θtarget；

70、其中，θtarget为库位中心线法向在世界坐标系下的目标航向角，根据库位中线的方向向量vmid计算得到：θtarget= atan2(vmidy, vmidx) + π/2。

71、一种基于库位特征识别的agv无标记定位停靠系统，包括：

72、3d视觉采集模块，安装于agv上，用于采集包含目标库位的三维点云数据或深度图像数据；

73、库位特征识别模块，用于采集到的三维点云数据进行处理，识别出目标库位的固有结构特征；

74、库位中心解算模块，用于根据识别出的固有结构特征，通过几何拟合或计算，解算出目标库位的中心点坐标，并进一步计算agv当前位姿相对于该库位中心点的偏差量；

75、停靠控制模块，用于根据计算出得到的偏差量，控制agv进行姿态调整并精确停靠在目标库位处。

76、由此可见，与现有技术相比，本发明提出的基于库位特征识别的agv无标记定位停靠方法及系统，具有以下有益效果：

77、1、本发明直接利用库位固有的物理结构（两侧立柱、后挡板、底部边角等）作为定位参照，无需在地面或货架粘贴二维码、磁条等任何人工标记，亦无需在库位周边布置反光板、uwb基站等辅助定位设备，改变了现有agv精确定位停靠依赖于人工标记的技术路径。本发明仅需将库位自身的结构参数（立柱间距、高度范围、宽度范围等）录入系统即可完成部署。

78、2、本发明通过构建的几何特征参数体系以及两级筛选架构，能够自动从点云数据中识别库位立柱并完成定位，大幅降低了系统部署的时间成本和人力成本。在后期运维阶段，由于无需应对二维码磨损、污损、脱落等问题，人工巡检和更换的频次显著降低，仓储系统的综合运营成本得到有效控制。

79、3、本发明采用3d相机采集目标库位的三维点云数据，通过对库位立柱、后挡板、底部平面等物理结构的几何特征识别实现定位，这些结构特征由金属型材构成，具有足够的结构强度和稳定性，不易因环境因素而改变或损坏。同时，本发明采用ransac算法拟合后挡板平面和底部平面的方法，对点云数据中的离群噪声具有较强的鲁棒性，能够在点云存在一定比例干扰点的情况下依然稳定地拟合出平面参数，进一步增强了本发明在恶劣环境下的可靠性。

80、4、本发明中，库位定位参照是库位自身的物理结构，库位坐标信息仅与库位在世界坐标系下的位置有关。当库位布局调整时，只需在调度系统的库位管理数据库中更新目标库位的位置坐标，无需对库位物理环境进行任何改造。本发明的库位特征识别和中心点解算方法，完全基于3d相机对库位固有结构的实时感知，不依赖任何预先铺设的物理标记，因此库位位置的变更不会影响识别算法的有效性，使得仓储系统能够以极高的灵活性响应业务需求变化，实现仓储资源的动态优化配置。

81、5、本发明直接以库位的实际物理结构作为定位参照，基于3d相机实时感知的库位点云数据计算库位中心点，定位基准是现场实际物理位置而非数据库中记录的坐标，实现了真正意义上的所见即所得定位。本发明通过构造的库位中线，利用左右立柱的对称性计算出的虚拟中线，其精度取决于立柱点云拟合的精度而非人工测量的精度；通过中库位中线与后挡板平面的交点计算及预设深度偏移量的引入，本发明将定位基准锚定在库位的实际物理结构上，从根本上消除了因测量误差、标记位移或数据库坐标误差导致的定位偏差。同时，本发明利用ransac算法同时识别后挡板平面和底部平面的冗余设计，使得即使某一平面的点云数据因遮挡等原因不够完整，系统仍可依据另一平面的信息完成定位解算，进一步提升了定位的可靠性。

82、6、本发明构建了全局粗定位与局部精定位两级定位架构：全局层面，agv依靠调度系统下发的粗略位置和自身导航系统（如激光slam）行驶至目标库位附近，该阶段对导航精度的要求较低，agv可以保持较高的行驶速度以提升作业效率；局部层面，agv进入预设识别范围后减速停止，通过3d相机采集点云数据进行高精度位姿解算和微调停靠。

83、7、本发明将末端停靠环节的定位精度需求与行驶环节的导航效率需求进行了解耦设计，使agv在不同作业阶段采用不同精度等级和不同技术路径的定位手段，既保证了末端停靠的高精度，又兼顾了长距离行驶的高效率，同时降低了3d传感器和计算资源的持续占用。

84、下面结合附图和具体实施方式对本发明作进一步详细说明。