技术领域Technical Field
本申请涉及物流车定位领域，特别涉及一种定位方法及其系统。The present application relates to the field of logistics vehicle positioning, and in particular to a positioning method and system thereof.
背景技术Background Art
目前物流车导航方式有磁导航、二维码导航、激光导航、视觉导航。磁导航分磁体导航和电磁导航，两者方法相似，磁体导航比电磁导航路径改变相对容易，但磁带易被污染，可靠性较差。应用比较普遍的是二维码加惯导导航方式，定位停靠精度较高，但灵活性不够。激光导航应用也较多，灵活度虽然较高，但受周边物体特征、激光测距精度、光照噪声影响，使得定位精度限制在厘米级(2-5cm)，无法满足毫米级的精细操作。At present, the navigation methods for logistics vehicles include magnetic navigation, QR code navigation, laser navigation, and visual navigation. Magnetic navigation is divided into magnet navigation and electromagnetic navigation. The two methods are similar. Magnetic navigation is relatively easier to change the path than electromagnetic navigation, but the magnetic tape is easily contaminated and has poor reliability. The most commonly used is the QR code plus inertial navigation method, which has high positioning and docking accuracy, but lacks flexibility. Laser navigation is also widely used. Although it has high flexibility, it is affected by the characteristics of surrounding objects, laser ranging accuracy, and light noise, so that the positioning accuracy is limited to the centimeter level (2-5cm), which cannot meet the millimeter-level fine operations.
目前为了满足停靠任务高精度的需求，停靠系统中的定位方法采用的都是基于人工路标的定位。虽然二维码辅助激光导航同样可以实现高精度定位，但二维码和磁条一样，粘贴在地面上，也存在易磨损、破裂、脱落、污染等问题。At present, in order to meet the high-precision requirements of docking tasks, the positioning methods used in docking systems are all based on artificial landmarks. Although QR code-assisted laser navigation can also achieve high-precision positioning, QR codes, like magnetic strips, are easily worn, cracked, fallen off, and contaminated when pasted on the ground.
发明内容Summary of the invention
本申请提供一种定位方法及其系统，可以解决相关技术中物流车停靠时无法精准定位，定位不便的问题。The present application provides a positioning method and system thereof, which can solve the problem in the related art that logistics vehicles cannot be accurately positioned and the positioning is inconvenient when docked.
第一方面，本申请实施例提供一种定位方法，其包括：In a first aspect, an embodiment of the present application provides a positioning method, which includes:
基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；Obtaining a target turning radius of a vehicle body based on a first angle, a second angle, a length of a first reference line segment, a length of a second reference line segment, and a length of a third reference line segment, wherein the first angle is a first azimuth angle of a first reflective column relative to a vehicle body, the second angle is a second azimuth angle of a second reflective column relative to a vehicle body, the first reference line segment is a line connecting the first reflective column and the second reflective column, the second reference line segment is a line connecting the first reflective column and the vehicle body, and the third reference line segment is a line connecting the second reflective column and the vehicle body;
基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；Obtaining a target linear velocity of the vehicle body based on a vertical distance between the vehicle body and a target stop point, wherein the target stop point is located between the first reflective column and the second reflective column;
基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；Based on the target turning radius of the vehicle body and the target linear speed of the vehicle body, the target angular velocity of the vehicle body is obtained;
驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。The vehicle is driven to adjust its posture according to the target turning radius, target linear speed and target angular speed of the vehicle and move to the target stop point.
一些实施例中，基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，具体包括：In some embodiments, obtaining a target turning radius of a vehicle body based on the first angle, the second angle, the length of the first reference line segment, the length of the second reference line segment, and the length of the third reference line segment specifically includes:
基于第一角度θ1、第二角度θ2、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取第三角度θA和第四角度θB，所述第三角度θA为第一参考线段与第二参考线段之间的夹角，第四角度θB为第一参考线段与第三参考线段之间的夹角；Based on the first angle θ1, the second angle θ2, the length of the first reference line segment, the length of the second reference line segment and the length of the third reference line segment, obtaining a third angle θA and a fourth angle θB, wherein the third angle θA is the angle between the first reference line segment and the second reference line segment, and the fourth angle θB is the angle between the first reference line segment and the third reference line segment;
基于第一角度θ1、第二角度θ2、第三角度θA和第四角度θB，获取车体姿态角θ；Based on the first angle θ1, the second angle θ2, the third angle θA and the fourth angle θB, a vehicle body posture angle θ is obtained;
基于车体姿态角θ、车体与目标停靠点的直线距离L和车体与目标停靠点横向上的偏差YC，获取车体目标转弯半径R。The target turning radius R of the vehicle body is obtained based on the vehicle body posture angle θ, the straight-line distance L between the vehicle body and the target stop point, and the lateral deviation YC between the vehicle body and the target stop point.
一些实施例中，In some embodiments,
一些实施例中，所述方法还包括获取车体与目标停靠点横向上的偏差YC的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点横向上的偏差YC。In some embodiments, the method further includes the step of obtaining a lateral deviation YC between the vehicle body and the target stop point: based on the length of the second reference line segment, the length of the third reference line segment, the third angle θA and the fourth angle θB, obtaining the lateral deviation YC between the vehicle body and the target stop point.
一些实施例中，YC＝(BC*cosθB-AC*cosθA)/2；其中，BC为第三参考线段的长度，AC为第二参考线段的长度。In some embodiments, YC =(BC*cosθB -AC*cosθA )/2; wherein BC is the length of the third reference line segment, and AC is the length of the second reference line segment.
一些实施例中，所述方法还包括获取车体与目标停靠点的垂直距离的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点的垂直距离XC。In some embodiments, the method further includes the step of obtaining a vertical distance XC between the vehicle body and the target stop point: based on the length of the second reference line segment, the length of the third reference line segment, the third angle θA and the fourth angle θB, obtaining a vertical distance XC between the vehicle body and the target stop point.
一些实施例中，XC＝(BC*sinθB+AC*sinθA)/2；其中，BC为第三参考线段的长度，AC为第二参考线段的长度。In some embodiments, XC =(BC*sinθB +AC*sinθA )/2; wherein BC is the length of the third reference line segment, and AC is the length of the second reference line segment.
一些实施例中，所述车体上设置有激光雷达；所述第一角度和第二角度为车体在设定范围内采集的，所述车体在设定范围内时，激光雷达检测出第一反光柱和第二反光柱。In some embodiments, a laser radar is provided on the vehicle body; the first angle and the second angle are collected when the vehicle body is within a set range, and when the vehicle body is within the set range, the laser radar detects the first reflective column and the second reflective column.
一些实施例中，所述第一反光柱和第二反光柱均包括反射膜，所述反射膜为圆柱形。In some embodiments, the first reflective column and the second reflective column both include a reflective film, and the reflective film is cylindrical.
第二方面，本申请实施例提供了一种定位系统，其包括：第一模块、第二模块、第三模块和第四模块，第一模块用于基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，所述第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，所述第一参考线段为第一反光柱与第二反光柱的连线，所述第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；第二模块用于基于车体与目标停靠点的垂直距离，获取车体目标线速度，所述目标停靠点位于第一反光柱和第二反光柱之间；第三模块用于基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；第四模块用于驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。In a second aspect, an embodiment of the present application provides a positioning system, which includes: a first module, a second module, a third module and a fourth module, wherein the first module is used to obtain a target turning radius of a vehicle body based on a first angle, a second angle, a length of a first reference line segment, a length of a second reference line segment and a length of a third reference line segment, wherein the first angle is a first azimuth angle of a first reflective column relative to a vehicle body, the second angle is a second azimuth angle of a second reflective column relative to a vehicle body, the first reference line segment is a line connecting the first reflective column and the second reflective column, the second reference line segment is a line connecting the first reflective column and the vehicle body, and the third reference line segment is a line connecting the second reflective column and the vehicle body; the second module is used to obtain a target linear velocity of the vehicle body based on a vertical distance between the vehicle body and a target stop point, wherein the target stop point is located between the first reflective column and the second reflective column; the third module is used to obtain a target angular velocity of the vehicle body based on a target turning radius of the vehicle body and a target linear velocity of the vehicle body; and the fourth module is used to drive the vehicle body to adjust its posture and move to the target stop point according to the target turning radius of the vehicle body, the target linear velocity of the vehicle body and the target angular velocity of the vehicle body.
本申请实施例提供的技术方案带来的有益效果包括：The beneficial effects brought by the technical solution provided by the embodiments of the present application include:
本申请实施例提供了一种定位方法及其系统，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。The embodiment of the present application provides a positioning method and a system thereof, which adopts a first reflective column and a second reflective column to assist in navigation for ground navigation, and calculates the target turning radius, target linear speed and target angular speed of the vehicle body that need to be adjusted and controlled, to ensure that the vehicle body moves along the expected point and direction, and does not need to paste QR codes on the docking position and the nearby ground, thereby solving problems such as wear, cracking, falling off and contamination, ensuring that the vehicle body accurately enters the narrow cargo space, and solving the problem that pure laser SLAM navigation cannot meet millimeter-level fine positioning.
附图说明BRIEF DESCRIPTION OF THE DRAWINGS
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.
图1为本申请实施例提供的定位方法流程图；FIG1 is a flow chart of a positioning method provided in an embodiment of the present application;
图2为本申请实施例提供的反光柱局部定位原理图。FIG. 2 is a schematic diagram of the local positioning principle of a reflective column provided in an embodiment of the present application.
具体实施方式DETAILED DESCRIPTION
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
本申请实施例提供了一种定位方法及其系统，其能解决相关技术中物流车停靠时无法精准定位，定位不便的问题。The embodiments of the present application provide a positioning method and system thereof, which can solve the problem in the related art that logistics vehicles cannot be accurately positioned and the positioning is inconvenient when docked.
第一方面，本申请实施例提供了一种定位方法，其包括：In a first aspect, an embodiment of the present application provides a positioning method, which includes:
101：基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，第一参考线段为第一反光柱与第二反光柱的连线，第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；101: based on the first angle, the second angle, the length of the first reference line segment, the length of the second reference line segment and the length of the third reference line segment, obtain the target turning radius of the vehicle body, the first angle is the first azimuth angle of the first reflective column relative to the vehicle body, the second angle is the second azimuth angle of the second reflective column relative to the vehicle body, the first reference line segment is the line connecting the first reflective column and the second reflective column, the second reference line segment is the line connecting the first reflective column and the vehicle body, and the third reference line segment is the line connecting the second reflective column and the vehicle body;
102：基于车体与目标停靠点的垂直距离，获取车体目标线速度，目标停靠点位于第一反光柱和第二反光柱之间；102: Acquire a target linear velocity of the vehicle body based on a vertical distance between the vehicle body and a target stop point, where the target stop point is located between the first reflective column and the second reflective column;
103：基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；103: Obtain a target angular velocity of the vehicle body based on a target turning radius of the vehicle body and a target linear velocity of the vehicle body;
104：驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。104: Drive the vehicle body to adjust its posture according to the target turning radius of the vehicle body, the target linear velocity of the vehicle body, and the target angular velocity of the vehicle body and move to the target stop point.
本申请中，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。In the present application, ground navigation is performed by using a first reflective column and a second reflective column to assist in navigation, and the target turning radius, target linear velocity and target angular velocity of the vehicle body that need to be adjusted and controlled are calculated to ensure that the vehicle body moves along the expected point and direction. There is no need to stick QR codes on the docking position and the nearby ground, and problems such as wear, cracking, falling off and contamination are solved. This ensures that the vehicle body can accurately enter narrow cargo spaces, and solves the problem that pure laser SLAM navigation cannot meet millimeter-level fine positioning requirements.
需要说明的是，本申请中在车体的目标停靠点附近设置了反光柱，用于辅助车体定位。本实施例中，设置了第一反光柱和第二反光柱，并使目标停靠点位于第一反光柱和第二反光柱之间。It should be noted that in the present application, a reflective column is arranged near the target stop point of the vehicle body to assist in positioning the vehicle body. In the present embodiment, a first reflective column and a second reflective column are arranged, and the target stop point is located between the first reflective column and the second reflective column.
在车体上安装了激光雷达或激光传感器，在本实例中，将激光雷达安装于车体的中心位置处，当车体移动至设定范围内时，激光雷达检测出第一反光柱和第二反光柱。A laser radar or laser sensor is installed on the vehicle body. In this example, the laser radar is installed at the center of the vehicle body. When the vehicle body moves to a set range, the laser radar detects the first reflective column and the second reflective column.
参见图2所示，图中A点表示第一反光柱，B点表示第二反光柱；O点为目标停靠点，优选的，目标停靠点可以设置于第一反光柱和第二反光柱之间的中心位置处；C点表示激光雷达；因此，此时通过激光雷达能够获取到第一角度、第二角度、第二参考线段的长度和第三参考线段的长度。As shown in Figure 2, point A in the figure represents the first reflective column, point B represents the second reflective column; point O is the target stopping point, and preferably, the target stopping point can be set at the center position between the first reflective column and the second reflective column; point C represents the laser radar; therefore, at this time, the first angle, the second angle, the length of the second reference line segment and the length of the third reference line segment can be obtained through the laser radar.
其中，第一参考线段为第一反光柱与第二反光柱的连线，也就是A点、B点的距离，第一参考线段已知；第二参考线段为第一反光柱与车体连线，也就是A点、C点的距离；第三参考线段为第二反光柱与车体连线，也就是B点、C点的距离。Among them, the first reference line segment is the line connecting the first reflective column and the second reflective column, that is, the distance between point A and point B, and the first reference line segment is known; the second reference line segment is the line connecting the first reflective column and the vehicle body, that is, the distance between point A and point C; the third reference line segment is the line connecting the second reflective column and the vehicle body, that is, the distance between point B and point C.
第一角度、第二角度、第二参考线段的长度和第三参考线段的长度均为车体在设定范围内激光雷达采集的。激光雷达接收到的数据中除了激光束的距离(本申请中指第二参考线段的长度、第三参考线段的长度和下文中提及的车体与目标停靠点的直线距离和车体与目标停靠点的垂直距离)和夹角(本申请中指第一角度、第二角度和下文中提及的第三角度、第四角度)外，还包括了第一反光柱和第二反光柱反射回来的激光束的光强信息。The first angle, the second angle, the length of the second reference line segment and the length of the third reference line segment are all collected by the laser radar within the set range of the vehicle body. In addition to the distance of the laser beam (in this application, it refers to the length of the second reference line segment, the length of the third reference line segment, and the straight-line distance between the vehicle body and the target stop point and the vertical distance between the vehicle body and the target stop point mentioned below) and the angle (in this application, it refers to the first angle, the second angle and the third angle and the fourth angle mentioned below), the data received by the laser radar also includes the light intensity information of the laser beam reflected back by the first reflective column and the second reflective column.
车体工作的环境中大部分物体的表面都发生漫反射，漫反射反射回的激光光强一般都比较小；一些表面比较光滑的物体会发生镜面反射，镜面反射反射回的激光光强相对较大。因此，本申请使用钻石级的反射膜(一种反光性能强的材料)部署在环境中，也就是第一反光柱和第二反光柱均包括反射膜，反射膜包裹在柱体外周表面，形成第一反光柱和第二反光柱。保证激光照射反射膜得到的信号强度远大于照射一般物体得到的信号强度，通过信号强度可以从激光雷达数据中识别出跟反射膜相关的数据。为了保证反射膜可以360°范围内均可以被扫描到，反射膜被设计成圆柱形。In the working environment of the vehicle body, the surfaces of most objects will experience diffuse reflection, and the intensity of the laser light reflected back by diffuse reflection is generally relatively small; some objects with relatively smooth surfaces will experience specular reflection, and the intensity of the laser light reflected back by specular reflection is relatively large. Therefore, the present application uses a diamond-grade reflective film (a material with strong reflective properties) to be deployed in the environment, that is, the first reflective column and the second reflective column both include a reflective film, and the reflective film is wrapped around the outer peripheral surface of the column to form the first reflective column and the second reflective column. Ensure that the signal intensity obtained by laser irradiation of the reflective film is much greater than the signal intensity obtained by irradiating a general object, and the data related to the reflective film can be identified from the lidar data through the signal intensity. In order to ensure that the reflective film can be scanned within a 360° range, the reflective film is designed to be cylindrical.
在上述实施例的基础上，本实施例中，基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，具体包括步骤1011至步骤1013：On the basis of the above embodiment, in this embodiment, based on the first angle, the second angle, the length of the first reference line segment, the length of the second reference line segment and the length of the third reference line segment, the target turning radius of the vehicle body is obtained, which specifically includes steps 1011 to 1013:
步骤1011：基于第一角度θ1、第二角度θ2、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取第三角度θA和第四角度θB。Step 1011: Obtain a third angle θA and a fourth angle θB based on the first angle θ1 , the second angle θ2 , the length of the first reference line segment, the length of the second reference line segment, and the length of the third reference line segment.
具体的，参见图2所示，第三角度θA为第一参考线段与第二参考线段之间的夹角，第四角度θB为第一参考线段与第三参考线段之间的夹角。Specifically, referring to FIG. 2 , the third angle θA is the angle between the first reference line segment and the second reference line segment, and the fourth angle θB is the angle between the first reference line segment and the third reference line segment.
通过激光雷达采集的数据可以得到第二参考线段的长度和第三参考线段的长度以及第一角度θ1、第二角度θ2，第一参考线段的长度是已知的，所以利用余弦定理可以计算出第三角度θA和第四角度θB：The length of the second reference line segment and the length of the third reference line segment as well as the first angle θ1 and the second angle θ2 can be obtained from the data collected by the laser radar. The length of the first reference line segment is known, so the third angle θA and the fourth angle θB can be calculated using the cosine theorem:
其中，BC为第三参考线段的长度，AC为第二参考线段的长度；AB为第一参考线段的长度。Among them, BC is the length of the third reference line segment, AC is the length of the second reference line segment; AB is the length of the first reference line segment.
步骤1012：基于第一角度θ1、第二角度θ2、第三角度θA和第四角度θB，获取车体姿态角θ。Step 1012: Obtain the vehicle body posture angle θ based on the first angle θ1 , the second angle θ2 , the third angle θA and the fourth angle θB .
具体的，第一角度θ1、第二角度θ2、第三角度θA和第四角度θB已知，可以计算车体姿态角θ：Specifically, the first angle θ1 , the second angle θ2 , the third angle θA and the fourth angle θB are known, and the vehicle body posture angle θ can be calculated:
θ＝(θ2-θB+θA-θ1)/2。θ=(θ2 -θB +θA -θ1 )/2.
步骤1013：基于车体姿态角θ、车体与目标停靠点的直线距离L和车体与目标停靠点横向上的偏差YC，获取车体目标转弯半径R。Step 1013: Obtain a target turning radius R of the vehicle based on the vehicle attitude angle θ, the straight-line distance L between the vehicle and the target stop point, and the lateral deviation YC between the vehicle and the target stop point.
具体的，车体由C点运行到第一反光柱和第二反光柱中心点O点稳定精确停靠是通过反光柱定位法和纯跟踪算法融合实现的。纯跟踪算法中转弯半径公式如下：Specifically, the vehicle body runs from point C to the center point O of the first reflective column and the second reflective column and stops stably and accurately by integrating the reflective column positioning method and the pure tracking algorithm. The turning radius formula in the pure tracking algorithm is as follows:
式中，L为C点到O点的直线距离，也即车体与目标停靠点的直线距离L，也可以称为前视距离，在实际应用中可以根据实际情况将设置L为固定值。θ为车体姿态角。YC为C点和O点横向上的偏差，可以通过反光柱局部定位原理得到。Where L is the straight-line distance from point C to point O, that is, the straight-line distance L between the vehicle and the target stop point, which can also be called the foresight distance. In practical applications, L can be set to a fixed value according to actual conditions. θ is the vehicle attitude angle.Y C is the lateral deviation between point C and point O, which can be obtained through the principle of local positioning of reflective columns.
因此，该方法还包括获取车体与目标停靠点横向上的偏差YC的步骤：基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点横向上的偏差YC。Therefore, the method further includes the step of obtaining a lateral deviationYC between the vehicle body and the target stop point: obtaining a lateral deviationYC between the vehicle body and the target stop point based on the length of the second reference line segment, the length of the third reference line segment, the third angleθA and the fourth angleθB .
利用公式：YC＝(BC*cosθB-AC*cosθA)/2；可以获得车体与目标停靠点横向上的偏差YC，车体与目标停靠点横向上的偏差YC获得后，将其代入转弯半径公式获得车体目标转弯半径R。By using the formula: YC =(BC*cosθB -AC*cosθA )/2, the lateral deviation YC between the vehicle body and the target stop point can be obtained. After the lateral deviation YC between the vehicle body and the target stop point is obtained, it is substituted into the turning radius formula to obtain the target turning radius R of the vehicle body.
在本申请中，会根据车体与目标停靠点的垂直距离，也即C点距离O点的垂直距离，作为离目标位置的剩余距离XC，通过车体速度和剩余距离成正比的关系算出线速度V。In this application, the vertical distance between the vehicle body and the target stop point, that is, the vertical distance between point C and point O, is used as the remaining distance XC to the target position, and the linear speed V is calculated by the proportional relationship between the vehicle body speed and the remaining distance.
因此，在上述实施例的基础上，本实施例中，该方法还包括获取车体与目标停靠点的垂直距离的步骤：Therefore, based on the above embodiment, in this embodiment, the method further includes the step of obtaining the vertical distance between the vehicle body and the target stop point:
基于第二参考线段的长度、第三参考线段的长度、第三角度θA和第四角度θB，获取车体与目标停靠点的垂直距离XC。Based on the length of the second reference line segment, the length of the third reference line segment, the third angle θA and the fourth angle θB , a vertical distance XC between the vehicle body and the target stop point is acquired.
具体的，车体与目标停靠点的垂直距离XC获得公式为：Specifically, the vertical distanceXC between the vehicle body and the target stop point is obtained by:
XC＝(BC*sinθB+AC*sinθA)/2；XC =(BC*sinθB +AC*sinθA )/2;
通过第三参考线段的长度BC，第二参考线段的长度AC、第三角度θA和第四角度θB，计算车体与目标停靠点的垂直距离XC。The vertical distance XC between the vehicle body and the target stop point is calculated by the length BC of the third reference line segment, the length AC of the second reference line segment, the third angle θA and the fourth angle θB .
获得车体与目标停靠点的垂直距离XC后，通过公式V＝K×XC，算出车体目标线速度V。其中，式中K为关系系数，可以根据实际情况调节。After obtaining the vertical distance XC between the vehicle body and the target stop point, the vehicle body target linear velocity V is calculated by the formula V = K × XC , where K is the relationship coefficient and can be adjusted according to actual conditions.
在上述实施例的基础上，本实施例中，基于车体目标转弯半径和车体目标线速度，获取车体目标角速度中：On the basis of the above embodiment, in this embodiment, based on the target turning radius of the vehicle body and the target linear velocity of the vehicle body, the target angular velocity of the vehicle body is obtained:
具体的，根据车体目标转弯半径R和车体目标线速度V算出车体目标角速度ω：ω＝V/R。Specifically, the target angular velocity ω of the vehicle body is calculated according to the target turning radius R of the vehicle body and the target linear velocity V of the vehicle body: ω=V/R.
在上述实施例的基础上，本实施例中，驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点中：On the basis of the above embodiment, in this embodiment, the vehicle body is driven to adjust its posture according to the target turning radius of the vehicle body, the target linear velocity of the vehicle body and the target angular velocity of the vehicle body and move to the target stop point:
具体的，车体上还设置有控制器，将算出的车体目标转弯半径、车体目标线速度和车体目标角速度作为指令发给控制器控制车体完成相应的移动和转弯动作，最后实现C点和O点重合。在实验中可以达到了±5mm、±1°的停靠精度。Specifically, the vehicle body is also equipped with a controller, which sends the calculated target turning radius, target linear velocity and target angular velocity of the vehicle body as instructions to the controller to control the vehicle body to complete the corresponding movement and turning actions, and finally achieve the coincidence of point C and point O. In the experiment, the parking accuracy of ±5mm and ±1° can be achieved.
下面用具体的实施例进行说明：The following is described with specific embodiments:
在目标停靠点旁边立了两个反光柱，即第一反光柱和第二反光柱，第一反光柱和第二反光柱之间的距离比车身整长宽6cm。实验目标是使用基于纯跟踪算法利用反光柱局部定位技术实现车体精确地停靠在第一反光柱和第二反光柱之间的中心位置，也即目标停靠点。车体从起始点出发向设定范围内行走这个过程可以使用纯激光SLAM(Simultaneouslocalization and mapping同步定位与建图)导航，在本实施例中，设定范围为目标停靠点前方1m±0.1m内，保证此时激光雷达可以检测到第一反光柱和第二反光柱。然后将车体的导航方式切换到纯跟踪算法模式：基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径；然后基于车体与目标停靠点的垂直距离，获取车体目标线速度；接着基于车体目标转弯半径和车体目标线速度，获取车体目标角速度，最后实现车体精确地停靠在目标停靠点。需要说明的是，车体行驶过程中车体目标转弯半径、车体目标线速度、车体目标角速度是时刻变化的，因此需实时计算车体目标转弯半径、车体目标线速度、车体目标角速度。Two reflective columns are erected next to the target stop point, namely the first reflective column and the second reflective column. The distance between the first reflective column and the second reflective column is 6cm wider than the entire length of the vehicle body. The experimental goal is to use the reflective column local positioning technology based on a pure tracking algorithm to achieve the precise parking of the vehicle body at the center position between the first reflective column and the second reflective column, that is, the target stop point. The process of the vehicle body moving from the starting point to the set range can use pure laser SLAM (Simultaneous localization and mapping) navigation. In this embodiment, the set range is within 1m±0.1m in front of the target stop point to ensure that the laser radar can detect the first reflective column and the second reflective column at this time. Then switch the vehicle's navigation mode to pure tracking algorithm mode: based on the first angle, the second angle, the length of the first reference line segment, the length of the second reference line segment, and the length of the third reference line segment, obtain the vehicle's target turning radius; then based on the vertical distance between the vehicle and the target stop point, obtain the vehicle's target linear velocity; then based on the vehicle's target turning radius and the vehicle's target linear velocity, obtain the vehicle's target angular velocity, and finally achieve precise parking of the vehicle at the target stop point. It should be noted that the vehicle's target turning radius, vehicle's target linear velocity, and vehicle's target angular velocity are constantly changing during the vehicle's travel, so the vehicle's target turning radius, vehicle's target linear velocity, and vehicle's target angular velocity need to be calculated in real time.
由于激光SLAM导航重复定位精度可以满足在±5cm以内，所以基本可以保证车体可以停在设定范围内，然后通过局部定位修正位姿后顺利准确进入货位。Since the laser SLAM navigation repeatability accuracy can be within ±5cm, it can basically ensure that the vehicle can be parked within the set range, and then enter the cargo space smoothly and accurately after correcting the position through local positioning.
在实际应用中，可以对停靠精度要求比较高的特殊货位使用基于纯跟踪算法利用反光柱局部定位技术，在停靠的货位附近立柱上贴反光材料，即反光膜，并使用激光传感器、激光雷达等设备识别货架边缘，通过反光柱局部定位来精确计算车体相对于目标停靠点的位姿。In practical applications, for special cargo locations with relatively high requirements for docking accuracy, the reflective column local positioning technology based on pure tracking algorithm can be used. Reflective materials, i.e. reflective films, can be affixed to the columns near the docked cargo locations, and laser sensors, lidars and other equipment can be used to identify the edge of the shelf. The position of the vehicle relative to the target docking point can be accurately calculated through local positioning of reflective columns.
因此，本申请使用钻石级的反射膜(一种反光性能强的材料)部署在环境中，便于从激光雷达的测量数据中筛选出跟第一反光柱和第二反光柱相关的原始数据，然后利用中值滤波对这些原始数据进行处理，是为了防止跳变的异常数据影响车体运行；将反光柱局部定位法和纯跟踪算法结合，实现稳定停靠定位；地面取放货采用反光柱辅助激光导航，兼顾停靠定位的精度和导航的灵活性。Therefore, the present application uses a diamond-grade reflective film (a material with strong reflective properties) deployed in the environment to facilitate the screening of the original data related to the first reflective column and the second reflective column from the measurement data of the lidar, and then uses a median filter to process these original data in order to prevent abnormal data from jumping and affecting the operation of the vehicle; the local positioning method of the reflective column and the pure tracking algorithm are combined to achieve stable docking positioning; the ground pick-up and delivery of goods uses reflective columns to assist laser navigation, taking into account the accuracy of docking positioning and the flexibility of navigation.
第二方面，本申请实施例提供了一种定位系统，其包括：第一模块、第二模块、第三模块和第四模块；第一模块用于基于第一角度、第二角度、第一参考线段的长度、第二参考线段的长度和第三参考线段的长度，获取车体目标转弯半径，第一角度为第一反光柱相对于车体的第一方位角，第二角度为第二反光柱相对于车体的第二方位角，第一参考线段为第一反光柱与第二反光柱的连线，第二参考线段为第一反光柱与车体连线，第三参考线段为第二反光柱与车体连线；第二模块用于基于车体与目标停靠点的垂直距离，获取车体目标线速度，目标停靠点位于第一反光柱和第二反光柱之间；第三模块用于基于车体目标转弯半径和车体目标线速度，获取车体目标角速度；第四模块用于驱使车体根据车体目标转弯半径、车体目标线速度和车体目标角速度调整姿态并移动至目标停靠点。In a second aspect, an embodiment of the present application provides a positioning system, which includes: a first module, a second module, a third module and a fourth module; the first module is used to obtain a target turning radius of the vehicle body based on a first angle, a second angle, a length of a first reference line segment, a length of a second reference line segment and a length of a third reference line segment, the first angle being a first azimuth angle of a first reflective column relative to the vehicle body, the second angle being a second azimuth angle of a second reflective column relative to the vehicle body, the first reference line segment being a line connecting the first reflective column and the second reflective column, the second reference line segment being a line connecting the first reflective column and the vehicle body, and the third reference line segment being a line connecting the second reflective column and the vehicle body; the second module is used to obtain a target linear velocity of the vehicle body based on a vertical distance between the vehicle body and a target stop point, the target stop point being located between the first reflective column and the second reflective column; the third module is used to obtain a target angular velocity of the vehicle body based on a target turning radius of the vehicle body and a target linear velocity of the vehicle body; the fourth module is used to drive the vehicle body to adjust its posture according to the target turning radius of the vehicle body, the target linear velocity of the vehicle body and the target angular velocity of the vehicle body and move to the target stop point.
本申请中，采用第一反光柱和第二反光柱辅助导航的方式进行地面导航工作，并通过运算得出车体所需调整控制的车体目标转弯半径、车体目标线速度和车体目标角速度，保证车体沿着预期的点和方向运动，不需要在停靠位和附近地面上粘贴二维码，解决磨损、破裂、脱落、污染等问题，确保车体准确进入狭窄货位，解决纯激光SLAM导航无法满足毫米级精细定位的问题。In the present application, ground navigation is performed by using a first reflective column and a second reflective column to assist in navigation, and the target turning radius, target linear velocity and target angular velocity of the vehicle body that need to be adjusted and controlled are calculated to ensure that the vehicle body moves along the expected point and direction. There is no need to stick QR codes on the docking position and the nearby ground, and problems such as wear, cracking, falling off and contamination are solved. This ensures that the vehicle body can accurately enter narrow cargo spaces, and solves the problem that pure laser SLAM navigation cannot meet millimeter-level fine positioning requirements.
在本申请的描述中，需要说明的是，术语“上”、“下”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。In the description of the present application, it should be noted that the terms "upper", "lower", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application. Unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances.
需要说明的是，在本申请中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。It should be noted that, in this application, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.
以上所述仅是本申请的具体实施方式，使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。The above is only a specific implementation of the present application, so that those skilled in the art can understand or implement the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest range consistent with the principles and novel features applied for herein.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410681743.3ACN118655893A (en) | 2024-05-29 | 2024-05-29 | A positioning method and system thereof |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410681743.3ACN118655893A (en) | 2024-05-29 | 2024-05-29 | A positioning method and system thereof |
| Publication Number | Publication Date |
|---|---|
| CN118655893Atrue CN118655893A (en) | 2024-09-17 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410681743.3APendingCN118655893A (en) | 2024-05-29 | 2024-05-29 | A positioning method and system thereof |
| Country | Link |
|---|---|
| CN (1) | CN118655893A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112945249A (en)* | 2015-02-10 | 2021-06-11 | 御眼视觉技术有限公司 | System, vehicle and method for autonomously navigating a vehicle along a road segment |
| US20230116869A1 (en)* | 2021-10-08 | 2023-04-13 | Institute Of Intelligent Manufacturing, Gdas | Multi-sensor-fusion-based autonomous mobile robot indoor and outdoor navigation method and robot |
| CN116148868A (en)* | 2022-12-30 | 2023-05-23 | 苏州寻迹智行机器人技术有限公司 | A secondary positioning method of laser radar based on reflective parts |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112945249A (en)* | 2015-02-10 | 2021-06-11 | 御眼视觉技术有限公司 | System, vehicle and method for autonomously navigating a vehicle along a road segment |
| US20230116869A1 (en)* | 2021-10-08 | 2023-04-13 | Institute Of Intelligent Manufacturing, Gdas | Multi-sensor-fusion-based autonomous mobile robot indoor and outdoor navigation method and robot |
| CN116148868A (en)* | 2022-12-30 | 2023-05-23 | 苏州寻迹智行机器人技术有限公司 | A secondary positioning method of laser radar based on reflective parts |
| Title |
|---|
| 陈慧岩: "《无人驾驶车辆理论与设计》", 31 March 2018, 北京理工大学出版社, pages: 108 - 109* |
| Publication | Publication Date | Title |
|---|---|---|
| CN111796281B (en) | System and method for landing and takeoff guidance | |
| US11865708B2 (en) | Domestic robotic system | |
| CN105486311B (en) | Indoor Robot positioning navigation method and device | |
| CA2307206C (en) | Method and device for association of anonymous reflectors to detected angle positions | |
| CN105974922A (en) | Tunnel automatic guide vehicle, guide system, and operating method for guide system | |
| CN108829137A (en) | A kind of barrier-avoiding method and device of robot target tracking | |
| CN104102222A (en) | Accurately positioning method for AGV (Automatic Guided Vehicle) | |
| CN108287539A (en) | Self-position apparatus for predicting and self-position estimating method | |
| JPH07306042A (en) | Method and apparatus for creating peripheral map of cellular structure | |
| CN103019240B (en) | An AGV trolley plane positioning and navigation system and method | |
| CN110907891B (en) | A kind of AGV positioning method and device | |
| CN110477808A (en) | A kind of robot | |
| KR20100086589A (en) | A moving object tracking control system for a mobile robot using zigbee's rssi(received signal strength indication) | |
| CN109828569A (en) | A kind of intelligent AGV fork truck based on 2D-SLAM navigation | |
| CN111687851A (en) | Museum collection checking robot | |
| JP7609211B2 (en) | AUTONOMOUS MOBILE DEVICE AND METHOD FOR CONTROLLING MOBILITY OF AUTONOMOUS MOBILE DEVICE | |
| CN118655893A (en) | A positioning method and system thereof | |
| JP3149661B2 (en) | Automatic guided vehicle position identification method | |
| CN210210406U (en) | Tracking robot | |
| CN115229777A (en) | Autonomous mobile robot, logistics docking system and docking method | |
| JP7190629B2 (en) | mobile robot | |
| CN111367269B (en) | Navigation positioning method, device and system of laser radar | |
| JP2021056764A (en) | Movable body | |
| WO2022168377A1 (en) | Baggage transport system, and method and computer program used in baggage transport system | |
| Li et al. | Study of a transferring system for measurements in aircraft assembly |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |