CN118655893A - 一种定位方法及其系统 - Google Patents

Abstract

本申请涉及一种定位方法及其系统，其包括：基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，第一参考线段为第一反光柱与第二反光柱的连线，第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；基于车体与目标停靠点的垂直距离，获取车体目标线速度，目标停靠点位于第一反光柱和第二反光柱之间；基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

Description

一种定位方法及其系统

技术领域

本申请涉及物流车定位领域，特别涉及一种定位方法及其系统。

背景技术

目前物流车导航方式有磁导航、二维码导航、激光导航、视觉导航。磁导航分磁体导航和电磁导航，两者方法相似，磁体导航比电磁导航路径改变相对容易，但磁带易被污染，可靠性较差。应用比较普遍的是二维码加惯导导航方式，定位停靠精度较高，但灵活性不够。激光导航应用也较多，灵活度虽然较高，但受周边物体特征、激光测距精度、光照噪声影响，使得定位精度限制在厘米级(2-5cm)，无法满足毫米级的精细操作。

目前为了满足停靠任务高精度的需求，停靠系统中的定位方法采用的都是基于人工路标的定位。虽然二维码辅助激光导航同样可以实现高精度定位，但二维码和磁条一样，粘贴在地面上，也存在易磨损、破裂、脱落、污染等问题。

发明内容

本申请提供一种定位方法及其系统，可以解决相关技术中物流车停靠时无法精准定位，定位不便的问题。

第一方面，本申请实施例提供一种定位方法，其包括：

基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；

基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；

基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；

驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

一些实施例中，基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，具体包括：

基于第一角度θ1、第二角度θ2、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取第三角度θA和第四角度θB，所述第三角度θA为第一参考线段与第二参考线段之间的夹角，第四角度θB为第一参考线段与第三参考线段之间的夹角；

基于第一角度θ1、第二角度θ2、第三角度θA和第四角度θB，获取车体姿态角θ；

基于车体姿态角θ、车体与目标停靠点的直线距离L和车体与目标停靠点横向上的偏差YC，获取车体目标转弯半径R。

一些实施例中，

一些实施例中，所述方法还包括获取车体与目标停靠点横向上的偏差YC的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点横向上的偏差YC。

一些实施例中，Y_C＝(BC*cosθ_B-AC*cosθ_A)/2；其中，BC为第三参考线段的长度，AC为第二参考线段的长度。

一些实施例中，所述方法还包括获取车体与目标停靠点的垂直距离的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点的垂直距离XC。

一些实施例中，X_C＝(BC*sinθ_B+AC*sinθ_A)/2；其中，BC为第三参考线段的长度，AC为第二参考线段的长度。

一些实施例中，所述车体上设置有激光雷达；所述第一角度和第二角度为车体在设定范围内采集的，所述车体在设定范围内时，激光雷达检测出第一反光柱和第二反光柱。

一些实施例中，所述第一反光柱和第二反光柱均包括反射膜，所述反射膜为圆柱形。

第二方面，本申请实施例提供了一种定位系统，其包括：第一模块、第二模块、第三模块和第四模块，第一模块用于基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；第二模块用于基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；第三模块用于基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；第四模块用于驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

本申请实施例提供的技术方案带来的有益效果包括：

本申请实施例提供了一种定位方法及其系统，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。

附图说明

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

图1为本申请实施例提供的定位方法流程图；

图2为本申请实施例提供的反光柱局部定位原理图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请实施例提供了一种定位方法及其系统，其能解决相关技术中物流车停靠时无法精准定位，定位不便的问题。

第一方面，本申请实施例提供了一种定位方法，其包括：

101：基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，第一参考线段为第一反光柱与第二反光柱的连线，第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；

102：基于车体与目标停靠点的垂直距离，获取车体目标线速度，目标停靠点位于第一反光柱和第二反光柱之间；

103：基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；

104：驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

本申请中，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。

需要说明的是，本申请中在车体的目标停靠点附近设置了反光柱，用于辅助车体定位。本实施例中，设置了第一反光柱和第二反光柱，并使目标停靠点位于第一反光柱和第二反光柱之间。

在车体上安装了激光雷达或激光传感器，在本实例中，将激光雷达安装于车体的中心位置处，当车体移动至设定范围内时，激光雷达检测出第一反光柱和第二反光柱。

参见图2所示，图中A点表示第一反光柱，B点表示第二反光柱；O点为目标停靠点，优选的，目标停靠点可以设置于第一反光柱和第二反光柱之间的中心位置处；C点表示激光雷达；因此，此时通过激光雷达能够获取到第一角度、第二角度、第二参考线段的长度和第三参考线段的长度。

其中，第一参考线段为第一反光柱与第二反光柱的连线，也就是A点、B点的距离，第一参考线段已知；第二参考线段为第一反光柱与车体连线，也就是A点、C点的距离；第三参考线段为第二反光柱与车体连线，也就是B点、C点的距离。

第一角度、第二角度、第二参考线段的长度和第三参考线段的长度均为车体在设定范围内激光雷达采集的。激光雷达接收到的数据中除了激光束的距离(本申请中指第二参考线段的长度、第三参考线段的长度和下文中提及的车体与目标停靠点的直线距离和车体与目标停靠点的垂直距离)和夹角(本申请中指第一角度、第二角度和下文中提及的第三角度、第四角度)外，还包括了第一反光柱和第二反光柱反射回来的激光束的光强信息。

车体工作的环境中大部分物体的表面都发生漫反射，漫反射反射回的激光光强一般都比较小；一些表面比较光滑的物体会发生镜面反射，镜面反射反射回的激光光强相对较大。因此，本申请使用钻石级的反射膜(一种反光性能强的材料)部署在环境中，也就是第一反光柱和第二反光柱均包括反射膜，反射膜包裹在柱体外周表面，形成第一反光柱和第二反光柱。保证激光照射反射膜得到的信号强度远大于照射一般物体得到的信号强度，通过信号强度可以从激光雷达数据中识别出跟反射膜相关的数据。为了保证反射膜可以360°范围内均可以被扫描到，反射膜被设计成圆柱形。

在上述实施例的基础上，本实施例中，基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，具体包括步骤1011至步骤1013：

步骤1011：基于第一角度θ₁、第二角度θ₂、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取第三角度θ_A和第四角度θ_B。

具体的，参见图2所示，第三角度θ_A为第一参考线段与第二参考线段之间的夹角，第四角度θ_B为第一参考线段与第三参考线段之间的夹角。

通过激光雷达采集的数据可以得到第二参考线段的长度和第三参考线段的长度以及第一角度θ₁、第二角度θ₂，第一参考线段的长度是已知的，所以利用余弦定理可以计算出第三角度θ_A和第四角度θ_B：

其中，BC为第三参考线段的长度，AC为第二参考线段的长度；AB为第一参考线段的长度。

步骤1012：基于第一角度θ₁、第二角度θ₂、第三角度θ_A和第四角度θ_B，获取车体姿态角θ。

具体的，第一角度θ₁、第二角度θ₂、第三角度θ_A和第四角度θ_B已知，可以计算车体姿态角θ：

θ＝(θ₂-θ_B+θ_A-θ₁)/2。

步骤1013：基于车体姿态角θ、车体与目标停靠点的直线距离L和车体与目标停靠点横向上的偏差Y_C，获取车体目标转弯半径R。

具体的，车体由C点运行到第一反光柱和第二反光柱中心点O点稳定精确停靠是通过反光柱定位法和纯跟踪算法融合实现的。纯跟踪算法中转弯半径公式如下：

式中，L为C点到O点的直线距离，也即车体与目标停靠点的直线距离L，也可以称为前视距离，在实际应用中可以根据实际情况将设置L为固定值。θ为车体姿态角。Y_C为C点和O点横向上的偏差，可以通过反光柱局部定位原理得到。

因此，该方法还包括获取车体与目标停靠点横向上的偏差Y_C的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θ_A和第四角度θ_B，获取车体与目标停靠点横向上的偏差Y_C。

利用公式：Y_C＝(BC*cosθ_B-AC*cosθ_A)/2；可以获得车体与目标停靠点横向上的偏差Y_C，车体与目标停靠点横向上的偏差Y_C获得后，将其代入转弯半径公式获得车体目标转弯半径R。

在本申请中，会根据车体与目标停靠点的垂直距离，也即C点距离O点的垂直距离，作为离目标位置的剩余距离X_C，通过车体速度和剩余距离成正比的关系算出线速度V。

因此，在上述实施例的基础上，本实施例中，该方法还包括获取车体与目标停靠点的垂直距离的步骤：

基于第二参考线段的长度、第三参考线段的长度、第三角度θ_A和第四角度θ_B，获取车体与目标停靠点的垂直距离X_C。

具体的，车体与目标停靠点的垂直距离X_C获得公式为：

X_C＝(BC*sinθ_B+AC*sinθ_A)/2；

通过第三参考线段的长度BC，第二参考线段的长度AC、第三角度θ_A和第四角度θ_B，计算车体与目标停靠点的垂直距离X_C。

获得车体与目标停靠点的垂直距离X_C后，通过公式V＝K×X_C，算出车体目标线速度V。其中，式中K为关系系数，可以根据实际情况调节。

在上述实施例的基础上，本实施例中，基于车体目标转弯半径和车体目标线速度，获取车体目标角速度中：

具体的，根据车体目标转弯半径R和车体目标线速度V算出车体目标角速度ω：ω＝V/R。

在上述实施例的基础上，本实施例中，驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点中：

具体的，车体上还设置有控制器，将算出的车体目标转弯半径、车体目标线速度和车体目标角速度作为指令发给控制器控制车体完成相应的移动和转弯动作，最后实现C点和O点重合。在实验中可以达到了±5mm、±1°的停靠精度。

下面用具体的实施例进行说明：

在目标停靠点旁边立了两个反光柱，即第一反光柱和第二反光柱，第一反光柱和第二反光柱之间的距离比车身整长宽6cm。实验目标是使用基于纯跟踪算法利用反光柱局部定位技术实现车体精确地停靠在第一反光柱和第二反光柱之间的中心位置，也即目标停靠点。车体从起始点出发向设定范围内行走这个过程可以使用纯激光SLAM(Simultaneouslocalization and mapping同步定位与建图)导航，在本实施例中，设定范围为目标停靠点前方1m±0.1m内，保证此时激光雷达可以检测到第一反光柱和第二反光柱。然后将车体的导航方式切换到纯跟踪算法模式：基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径；然后基于车体与目标停靠点的垂直距离，获取车体目标线速度；接着基于车体目标转弯半径和车体目标线速度，获取车体目标角速度，最后实现车体精确地停靠在目标停靠点。需要说明的是，车体行驶过程中车体目标转弯半径、车体目标线速度、车体目标角速度是时刻变化的，因此需实时计算车体目标转弯半径、车体目标线速度、车体目标角速度。

由于激光SLAM导航重复定位精度可以满足在±5cm以内，所以基本可以保证车体可以停在设定范围内，然后通过局部定位修正位姿后顺利准确进入货位。

在实际应用中，可以对停靠精度要求比较高的特殊货位使用基于纯跟踪算法利用反光柱局部定位技术，在停靠的货位附近立柱上贴反光材料，即反光膜，并使用激光传感器、激光雷达等设备识别货架边缘，通过反光柱局部定位来精确计算车体相对于目标停靠点的位姿。

因此，本申请使用钻石级的反射膜(一种反光性能强的材料)部署在环境中，便于从激光雷达的测量数据中筛选出跟第一反光柱和第二反光柱相关的原始数据，然后利用中值滤波对这些原始数据进行处理，是为了防止跳变的异常数据影响车体运行；将反光柱局部定位法和纯跟踪算法结合，实现稳定停靠定位；地面取放货采用反光柱辅助激光导航，兼顾停靠定位的精度和导航的灵活性。

第二方面，本申请实施例提供了一种定位系统，其包括：第一模块、第二模块、第三模块和第四模块；第一模块用于基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，第一参考线段为第一反光柱与第二反光柱的连线，第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；第二模块用于基于车体与目标停靠点的垂直距离，获取车体目标线速度，目标停靠点位于第一反光柱和第二反光柱之间；第三模块用于基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；第四模块用于驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

本申请中，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。

在本申请的描述中，需要说明的是，术语“上”、“下”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

需要说明的是，在本申请中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅是本申请的具体实施方式，使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种定位方法，其特征在于，其包括：

基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；

基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；

基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；

驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。

2.如权利要求1所述的定位方法，其特征在于，基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，具体包括：

基于第一角度θ₁、第二角度θ₂、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取第三角度θ_A和第四角度θ_B，所述第三角度θ_A为第一参考线段与第二参考线段之间的夹角，第四角度θ_B为第一参考线段与第三参考线段之间的夹角；

基于第一角度θ₁、第二角度θ₂、第三角度θ_A和第四角度θ_B，获取车体姿态角θ；

基于车体姿态角θ、车体与目标停靠点的直线距离L和车体与目标停靠点横向上的偏差Y_C，获取车体目标转弯半径R。

3.如权利要求2所述的定位方法，其特征在于：

4.如权利要求2所述的定位方法，其特征在于，所述方法还包括获取车体与目标停靠点横向上的偏差Y_C的步骤：

基于第二参考线段的长度、第三参考线段的长度、第三角度θ_A和第四角度θ_B，获取车体与目标停靠点横向上的偏差Y_C。

5.如权利要求4所述的定位方法，其特征在于：

Y_C＝(BC*cosθ_B-AC*cosθ_A)/2；

其中，BC为第三参考线段的长度，AC为第二参考线段的长度。

6.如权利要求1所述的定位方法，其特征在于，所述方法还包括获取车体与目标停靠点的垂直距离的步骤：

基于第二参考线段的长度、第三参考线段的长度、第三角度θ_A和第四角度θ_B，获取车体与目标停靠点的垂直距离X_C。

7.如权利要求6所述的定位方法，其特征在于：

X_C＝(BC*sinθ_B+AC*sinθ_A)/2；

其中，BC为第三参考线段的长度，AC为第二参考线段的长度。

8.如权利要求1所述的定位方法，其特征在于：

所述车体上设置有激光雷达；

所述第一角度和第二角度为车体在设定范围内采集的，所述车体在设定范围内时，激光雷达检测出第一反光柱和第二反光柱。

9.如权利要求1所述的定位方法，其特征在于：

所述第一反光柱和第二反光柱均包括反射膜，所述反射膜为圆柱形。

10.一种定位系统，其特征在于，其包括：

第一模块：其用于基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；

第二模块：其用于基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；

第三模块：其用于基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；

第四模块：其用于驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。