CN113295169B - 一种自动行驶装置的路径规划方法 - Google Patents

Abstract

本发明提供了一种自动行驶装置的路径规划方法，其步骤包括：S1规划前往目标点所途径的前置点及其行驶路径并移动；S2判断实际到达点与前置点之间的偏差是否达标，若达标则执行步骤S3，若不达标则跳转步骤S1；S3自当前位置直线规划行驶到目标点。籍此解决现有自动行驶装置去往目的地的精度，受到起点或者终点位置变化影响的问题。

Description

一种自动行驶装置的路径规划方法

技术领域

本发明涉及自动驾驶领域，尤其涉及一种自动行驶装置的路径规划方法。

背景技术

目前无人驾驶装置在工厂中被广泛应用，例如AGV，无人叉车等设备，然而随着对于自动化生产要求的提高，企业对于无人驾驶装置的导航精度，提出了越来越高的要求。

传统无人驾驶装置去往一个终点时，往往通过多段直线或者多条曲线直接行驶过去。但由于无人驾驶装置起点与目标点的相对位置发生变化时，规划的路径会不同，周围的环境不同。且在行驶过程中的路径曲率不同，速度不同，这些因素都会对到点精度产生影响。

而且此类现有技术的无人驾驶装置运行时，其在不同角度或者距离下，识别的目的地位置也会不同。因此，无人驾驶装置存在在不同起点或者去往不同终点的到点精度不一致的问题。而目前，现有技术文献中还没有很好的方案去解决这个精度受到起点和终点位置影响的问题。因此本领域亟待一种能够解决此类，无人驾驶装置去往目的地精度受到起点或者终点位置变化的问题的方案。

发明内容

本发明的主要目的在于提供一种自动行驶装置的路径规划方法，以解决现有自动行驶装置去往目的地的精度，受到起点或者终点位置变化影响的问题。

为了实现上述目的，根据本发明的第一个方面，提供了一种自动行驶装置的路径规划方法，其步骤包括：S1 规划前往目标点所途径的前置点及其行驶路径并移动；S2判断实际到达点与前置点之间的偏差是否达标，若达标则执行步骤S3，若不达标则跳转步骤S1；S3自当前位置直线规划行驶到目标点。

为了实现上述目的，根据本发明的第二个方面，提供了一种自动行驶装置的路径规划方法，其步骤包括：S1输入目标点坐标并配置第一预设距离，以进行第一路径处理计算得出前置点坐标；规划当前位置至前置点的行驶路径并移动；S2获取实际到达点坐标，并计算实际到达点与前置点的偏差，若偏差小于阈值δ则执行步骤S3，否则跳转至步骤S1；S3以最短距离规划实际到达点至目标点的行驶路径并移动。

在可能的优选实施方式中，目标点坐标包括：（x₁,y₁,θ），其中θ为目标点的朝向角度；第一预设距离Z≥自动行驶装置的长度；第一路径处理计算步骤包括：计算(x₁ + Z *cos(θ)，y₁ + Z * sin(θ), θ）获取前置点坐标(x₂,y₂,θ)。

在可能的优选实施方式中，实际到达点坐标包括：（x₃,y₃,θ₃），前置点坐标包括：(x₂,y₂,θ)，实际到达点与前置点的偏差计算步骤包括：计算世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)|。

在可能的优选实施方式中，阈值δ的计算步骤包括：S1设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；S2让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ。

为了实现上述目的，根据本发明的第三个方面，提供了一种自动行驶装置的路径规划方法，其步骤包括：S1识别目标点坐标（x₁,y₁,θ）,配置第一预设距离Z，以计算(x₁ + Z* cos(θ)，y₁ + Z * sin(θ), θ）获取前置点坐标(x₂,y₂,θ)；规划当前位置坐标至前置点的行驶路径并移动；S2在实际到达点（x₃,y₃,θ₃）再次识别目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new}, y_{2_new},θ_new）；判断实际到达点与新前置点在世界坐标系下横向偏差|-(x_{2_new}-x3)*sinθ₃+(y_{2_new}-y3)*cos(θ3)| ，若偏差小于阈值δ则执行步骤S3，否则跳转步骤S1；S3自当前位置直线规划行驶到目标点。

在可能的优选实施方式中，θ为目标点的朝向角度，θ₀ 为自动行驶装置的朝向角度；第一预设距离Z≥自动行驶装置的长度。

在可能的优选实施方式中，阈值δ的计算步骤包括：S1设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；S2让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ。

为了实现上述目的，根据本发明的第四个方面，提供了一种自动行驶装置的路径规划方法，其步骤包括：S1自起点至终点间设定途径目标点，以分解出里程段，并获取各里程段的目标点坐标（x₁,y₁,θ）；S2，配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ +Z * cos(θ)，y₁ + Z * sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置坐标至当前里程段前置点的行驶路径并移动；S3判断实际到达点（x₃,y₃,θ₃）与当前里程段前置点坐标(x₂,y₂,θ)，在世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)| ，若偏差小于阈值δ则执行步骤S4，否则执行步骤S2；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；S4自当前位置直线规划行驶到当前里程段的目标点；S5循环步骤S2至S4直至到达终点。

为了实现上述目的，根据本发明的第五个方面，提供了一种自动行驶装置的路径规划方法，其步骤包括：S1识别终点并自起点至终点间设定途径目标点，以分解出里程段；S2识别当前里程段的目标点坐标（x₁,y₁,θ）；配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ + Z * cos(θ)，y₁ + Z * sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置至当前里程段前置点的行驶路径并移动；S3在实际到达点（x₃,y₃,θ₃）再次识别当前里程段目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new}, y_{2_new},θ_new）；判断实际到达点与当前里程段新前置点在世界坐标系下横向偏差|-(x_{2_new}-x3)*sinθ₃+(y_{2_new}-y3)*cos(θ3)| ，若偏差小于阈值δ则执行步骤S4，否则跳转步骤S2；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；S4自当前位置直线规划行驶到当前里程段的目标点；S5循环步骤S2至S4直至到达终点。

本发明提供的该自动行驶装置的路径规划方法，通过在路径导航中引入前置点方案，来分段执行行驶任务，先让自动行驶装置规划路径，不断调整，高精度行驶到达前置点后，再通过直线行驶到目标点，从而解决了因起点或者终点不同，导致的到点精度低的问题。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明的自动行驶装置的路径规划方法中即有目标点数据的流程示意图；

图2为本发明的自动行驶装置的路径规划方法中需要识别目标点数据的流程示意图；

图3为本发明的自动行驶装置的路径规划方法中示例的几种行驶路径规划示意图；

图4为本发明的自动行驶装置的路径规划方法中示例前置点至目标点的行驶路径规划示意图。

具体实施方式

下面对本发明的具体实施方式进行详细地说明。以下示例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

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

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。

为解决现有自动行驶装置去往目的地的精度，受到起点或者终点位置变化影响的问题，本发明以无人驾驶叉车为例进行说明，其中本实施中该自动行驶装置的路径规划方法步骤包括：S1 规划前往目标点所途径的前置点及其行驶路径并移动，其中该路径可以为一条或者多段折线或者一条或者多条曲线；S2判断实际到达点与前置点之间的偏差是否达标，若达标则执行步骤S3，若不达标则跳转步骤S1；S3自当前位置直线规划行驶到目标点。籍此通过不断的修正移动位置，以提高去往目的地的精度

具体来说，如图1所示，若面对即有目标点数据的场景中，该步骤S1中优选包括：输入目标点坐标并配置第一预设距离，以进行第一路径处理计算得出前置点坐标；规划当前位置至前置点的行驶路径并移动，其中如图3所示，该路径可以为一条或者多段折线或者一条或者多条曲线。

此外如在本实施例下，该目标点坐标可包括：（x₁,y₁,θ），其中θ为目标点的朝向角度；第一预设距离Z≥自动行驶装置的长度；而该第一路径处理计算步骤包括：计算(x₁ + Z* cos(θ)，y₁ + Z * sin(θ), θ）获取前置点坐标(x₂,y₂,θ)。

而该步骤S2中包括：获取实际到达点坐标，并计算实际到达点与前置点的偏差，若偏差小于阈值δ则执行步骤S3，否则跳转至步骤S1，其中该实际到达点坐标包括：（x₃,y₃,θ₃），前置点坐标包括：(x₂,y₂,θ)，实际到达点与前置点的偏差计算步骤包括：计算世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)|。

值得一提的是，为了更好的确定该阈值δ，在优选实施方式中，阈值δ的计算步骤包括：S1设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；S2让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ。

此外步骤S3中，该直线规划的行驶路线仅为示例，而并未进行限制，本领域技术人员也可以选择其它符合当前场景最优的，最短距离规划实际到达点至目标点的行驶路径并移动。如图3所示，该路径可以为一条或者多段折线或者一条或者多条曲线。

另一方面，若面对的目标点非即有数据，或会进行变更的场景中，则如图2所示，该步骤S1包括：识别目标点坐标（x₁,y₁,θ）,配置第一预设距离Z，以计算(x₁ + Z * cos(θ)，y₁+ Z * sin(θ), θ）获取前置点坐标(x₂,y₂,θ)；规划当前位置坐标至前置点的行驶路径并移动；

而该步骤S2中包括：在实际到达点（x₃,y₃,θ₃）再次识别目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new},y_{2_new},θ_new）；判断实际到达点与新前置点在世界坐标系下横向偏差|-(x_{2_new}-x3)*sinθ₃+(y_{2_new}-y3)*cos(θ3)| ，若偏差小于阈值δ则执行步骤S3，否则跳转步骤S1；

而该步骤S3则如图4所示自当前位置直线规划行驶到目标点。

此外本实施例中该θ为目标点的朝向角度，θ₀ 为自动行驶装置的朝向角度；第一预设距离Z≥自动行驶装置的长度。

另一方面，为了能够更好的规划自动行驶装置自起点至终点的行驶计划，以便于适用于复数自动行驶装置调度场景，本实施例下，该自动行驶装置的路径规划方法的步骤包括：

步骤S1：自起点至终点间设定途径目标点，以分解出里程段，并获取各里程段的目标点坐标（x₁,y₁,θ），从而规划出该自动行驶装置的预设行驶轨迹；

步骤S2：配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ + Z * cos(θ)，y₁+ Z * sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置坐标至当前里程段前置点的行驶路径并移动；

步骤S3：判断实际到达点（x₃,y₃,θ₃）与当前里程段前置点坐标(x₂,y₂,θ)，在世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)| ，若偏差小于阈值δ则执行步骤S4，否则执行步骤S2；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

步骤S4：自当前位置直线规划行驶到当前里程段的目标点；

步骤S5：循环步骤S2至S4直至到达终点。

另一方面，为了能够在面对目标点非即有数据，或会进行变更的场景中，更好的规划自动行驶装置自起点至终点的行驶计划，以便于适用于复数自动行驶装置调度场景，本实施例下，该自动行驶装置的路径规划方法的步骤包括：

步骤S1：识别终点并自起点至终点间设定途径目标点，以分解出里程段；

步骤S2：识别当前里程段的目标点坐标（x₁,y₁,θ）；配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ + Z * cos(θ)，y₁ + Z * sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置至当前里程段前置点的行驶路径并移动；

步骤S3：在实际到达点（x₃,y₃,θ₃）再次识别当前里程段目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new},y_{2_new},θ_new）；判断实际到达点与当前里程段新前置点在世界坐标系下横向偏差|-(x_{2_new}-x3)*sinθ₃+(y_{2_new}-y3)*cos(θ3)| ，若偏差小于阈值δ则执行步骤S4，否则跳转步骤S2；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L,s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

步骤S4：自当前位置直线规划行驶到当前里程段的目标点；

步骤S5：循环步骤S2至S4直至到达终点。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的***、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的***、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (4)

1.一种自动行驶装置的路径规划方法，其特征在于，步骤包括：

S1输入目标点坐标并配置第一预设距离，以进行第一路径处理计算得出前置点坐标；规划当前位置至前置点的行驶路径并移动；

S2获取实际到达点坐标，并计算实际到达点与前置点的偏差，若偏差小于阈值δ则执行步骤S3，否则跳转至步骤S1；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

S3以最短距离规划实际到达点至目标点的行驶路径并移动；

其中目标点坐标包括：（x₁,y₁,θ），其中θ为目标点的朝向角度；第一预设距离Z≥自动行驶装置的长度；第一路径处理计算步骤包括：计算(x₁ + Z * cos(θ)，y₁ + Z * sin(θ), θ）获取前置点坐标(x₂,y₂,θ)；

其中实际到达点坐标包括：（x₃,y₃,θ₃），前置点坐标包括：(x₂,y₂,θ)，实际到达点与前置点的偏差计算步骤包括：计算世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)|。

2.一种自动行驶装置的路径规划方法，其特征在于，步骤包括：

S1识别目标点坐标（x₁,y₁,θ）,配置第一预设距离Z，以计算(x₁ + Z * cos(θ)，y₁ + Z *sin(θ), θ）获取前置点坐标(x₂,y₂,θ)；规划当前位置坐标至前置点的行驶路径并移动；

S2在实际到达点（x₃,y₃,θ₃）再次识别目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new}, y_{2_new},θ_new）；判断实际到达点与新前置点在世界坐标系下横向偏差|-(x_{2_new}-x₃)*sinθ₃+(y_{2_new}-y₃)*cos(θ₃)| ，若偏差小于阈值δ则执行步骤S3，否则跳转步骤S1；其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L, s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

S3自当前位置直线规划行驶到目标点。

3.一种自动行驶装置的路径规划方法，其特征在于，步骤包括：

S1自起点至终点间设定途径目标点，以分解出里程段，并获取各里程段的目标点坐标（x₁,y₁,θ）；

S2，配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ + Z * cos(θ)，y₁ + Z *sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置坐标至当前里程段前置点的行驶路径并移动；

S3判断实际到达点（x₃,y₃,θ₃）与当前里程段前置点坐标(x₂,y₂,θ)，在世界坐标系下横向偏差|-(x₂-x₃)*sinθ₃ + (y₂-y₃)*cos(θ₃)| ，若偏差小于阈值δ则执行步骤S4，否则执行步骤S2；

其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L,s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

S4自当前位置直线规划行驶到当前里程段的目标点；

S5循环步骤S2至S4直至到达终点。

4.一种自动行驶装置的路径规划方法，其特征在于，步骤包括：

S1识别终点并自起点至终点间设定途径目标点，以分解出里程段；

S2识别当前里程段的目标点坐标（x₁,y₁,θ）；配置第一预设距离Z≥自动行驶装置的长度，以计算(x₁ + Z * cos(θ)，y₁ + Z * sin(θ), θ），来动态获取当前里程段前置点坐标(x₂,y₂,θ)；规划当前位置至当前里程段前置点的行驶路径并移动；

S3在实际到达点（x₃,y₃,θ₃）再次识别当前里程段目标点坐标（x_{1_new},y_{1_new},θ_new），并计算(x_{1_new} + Z*cos(θ)，y_{1_new} + Z*sin(θ),θ_new）以获取新前置点坐标（x_{2_new}, y_{2_new},θ_new）；判断实际到达点与当前里程段新前置点在世界坐标系下横向偏差|-(x_{2_new}-x₃)*sinθ₃+(y_{2_new}-y₃)*cos(θ₃)| ，若偏差小于阈值δ则执行步骤S4，否则跳转步骤S2；

其中阈值δ的计算步骤包括：设定自动行驶装置在世界坐标系下朝向0度；分别在（L,s）和(-L, s)坐标下，后退/前进行驶到(0,0)点，测量到点精度，其中L为自动行驶装置的长度；让s逐渐变大，当到第i组s下，到点精度大于容许范围时，此时第i-1组的s即为δ；

S4自当前位置直线规划行驶到当前里程段的目标点；

S5循环步骤S2至S4直至到达终点。