CN108994838A - 机器人位置与规划路径的关系计算方法及系统 - Google Patents
机器人位置与规划路径的关系计算方法及系统 Download PDFInfo
- Publication number
- CN108994838A CN108994838A CN201810952483.3A CN201810952483A CN108994838A CN 108994838 A CN108994838 A CN 108994838A CN 201810952483 A CN201810952483 A CN 201810952483A CN 108994838 A CN108994838 A CN 108994838A
- Authority
- CN
- China
- Prior art keywords
- planning path
- straightway
- distance
- endpoint
- processed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
本发明实施例公开了一种机器人位置与规划路径的关系计算方法及系统。机器人位置与规划路径的关系计算方法包括：获取机器人的当前位置、规划路径和逼近规划路径的目标直线段；在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。本发明实施例解决了实时规划路径上投影点计算的问题，满足机器人控制的实时性、高精度要求。
Description
技术领域
本发明实施例涉及智能控制技术，尤其涉及一种机器人位置与规划路径的关系计算方法及系统。
背景技术
机器人循迹行驶指控制机器人按规划路径进行行驶的方法。循迹中常用样条曲线描述路径。如图1所示，对于给定样条曲线，控制系统通过实时计算机器人当前位置与规划路径间的关系(包括距离和垂足)，调整控制量(速度、方向)，达到循迹前进的目的。
现有技术中，机器人位置与规划路径的关系计算方法包括以下几步：一、上位机负责路线规划，将规划路径输出至负责控制的下位机；二、下位机根据机器人的当前位置和接收到的规划路径，计算机器人的当前位置与规划路径间的关系，即确定机器人的当前位置至样条曲线的距离、投影点(垂足)，如图1所示。下位机在计算上述关系时一般采用递归方法，即令规划路径由定义，其中，u为规划路径上的单位，x、y为空间坐标。令机器人的当前位置与规划路径接近，采用Raphson-Newton方法或其变形，递归，使得距离，即或最小，其中，(xp,yp)是机器人的当前位置。
在采用Raphson-Newton方法求最小值的过程中，如果初始值不佳，会导致计算时间较长，难以满足下位机控制机器人前进的实时性需求；而且，该递归方法计算精度较低，不能满足机器人控制的高精度需求。
发明内容
本发明实施例提供一种机器人位置与规划路径的关系计算方法及系统，此外还提供了一种规划路径的直线段逼近方法、装置、设备和可读存储介质，以满足机器人控制的实时性和高精度需求。
第一方面，本发明实施例提供了机器人位置与规划路径的关系计算方法，包括：
获取机器人的当前位置、规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；
在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
第二方面，本发明实施例还提供了一种规划路径的直线段逼近方法，包括：
从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算待处理直线段与其端点间的规划路径之间的距离；
比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；
如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
第三方面，本发明实施例还提供了一种机器人位置与规划路径的关系计算装置，包括：
获取模块，用于获取机器人的当前位置、规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；
第一确定模块，用于在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
第二确定模块，用于根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
计算模块，用于以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
第四方面，本发明实施例还提供了一种规划路径的直线段逼近装置，包括：
获取模块，用于从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算模块，用于计算待处理直线段与其端点间的规划路径之间的距离；
比较模块，用于比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
处理模块，用于如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；
删除模块，用于如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
选取模块，用于在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回模块，用于返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
第五方面，本发明实施例还提供了一种机器人位置与规划路径的关系计算系统，包括：相连接的机器人位置与规划路径的关系计算装置和规划路径的直线段逼近装置；
所述规划路径的直线段逼近装置，用于：从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；计算待处理直线段与其端点间的规划路径之间的距离；比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；返回执行新的待处理直线段的获取操作，直到全部直线段处理完成；将逼近所述规划路径的直线段和规划路径发送至机器人位置与规划路径的关系计算装置；
所述机器人位置与规划路径的关系计算装置，用于：获取机器人的当前位置、规划路径的直线段逼近装置发送的规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
第六方面，本发明实施例还提供了一种电子设备，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现任一实施例所述的机器人位置与规划路径的关系计算方法，或者任一实施例所述的规划路径的直线段逼近方法。
第七方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一实施例所述的机器人位置与规划路径的关系计算方法，或者任一实施例所述的规划路径的直线段逼近方法。
本实施例中，机器人位置与规划路径的关系计算装置直接获取逼近规划路径的目标直线段，而不需要执行直线段的计算操作，从而节省计算时间和计算量；通过确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点，从而根据目标直线段估计出规划路径上接近当前位置的初始坐标点，进而，以初始坐标点为初始值，能够较精确地、高速地计算当前位置与规划路径的距离和当前位置在规划路径上的投影点；而且，直线的表达式和几何计算方法均较简单，因此可以快速确定当前位置在目标直线段上的目标坐标点，进而快速确定初始坐标点。综上，本发明解决了实时规划路径上投影点计算的问题，实施例能够高效、高精度地计算机器人位置与规划路径的关系，满足机器人控制的实时性、高精度要求。
附图说明
图1是计算机器人的当前位置与规划路径间的关系的示意图；
图2是本发明实施例一提供的一种机器人位置与规划路径的关系计算方法的流程图；
图3a是本发明实施例二提供的一种机器人位置与规划路径的关系计算方法的流程图；
图3b是本发明实施例二提供的逼近规划路径的多条直线段的示意图；
图3c是本发明实施例二提供的一种当前位置到两条直线段的最短距离和最短距离处的坐标点的示意图；
图3d是本发明实施例二提供的另一种当前位置到两条直线段的最短距离和最短距离处的坐标点的示意图；
图4a是本发明实施例三提供的一种机器人位置与规划路径的关系计算方法的流程图；
图4b是本发明实施例三提供的初始坐标点的示意图；
图5是本发明实施例四提供的一种机器人位置与规划路径的关系计算方法的流程图；
图6是本发明实施例五提供的一种规划路径的直线段逼近方法的流程图；
图7是本发明实施例提供的直线段拆分的示意图；
图8是本发明实施例六提供的一种机器人位置与规划路径的关系计算装置的结构示意图；
图9是本发明实施例七提供的一种规划路径的直线段逼近装置的结构示意图；
图10是本发明实施例八提供的一种机器人位置与规划路径的关系计算系统的结构示意图；
图11是本发明实施例九提供的一种电子设备的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
实施例一
图2是本发明实施例一提供的一种机器人位置与规划路径的关系计算方法的流程图，本实施例可适用于在控制机器人循迹的过程中，计算机器人的当前位置与规划路径间的关系的情况，该方法可以由机器人位置与规划路径的关系计算装置来执行，该装置可以是STM32，作为下位机使用。该装置可以由硬件和/或软件构成，并集成在电子设备中。如图2所示，具体包括如下步骤：
S110、获取机器人的当前位置、规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上。
本实施例中，机器人的位置需要实时测量，在当前时刻测量到的机器人的位置(xa,ya)称为当前位置。
规划路径指机器人需要循迹的路径，该路径一般是一条曲线。可选地，用样条曲线表示规划路径，基于此，获取样条曲线的控制点和阶次即可得到完整的规划路径。
目标直线段的两个端点在规划路径上，其与规划路径的距离较近。可选地，获取目标直线段的两个端点，即可获取到目标直线段。
S120、在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点。
其中，第一预设距离要求指与当前位置的距离小于第一预设阈值，或者与当前位置的距离最小。
第一预设阈值可以根据机器人与规划路径的距离经验值获取，例如，根据经验，在机器人行进过程中与规划路径的距离一般保持在3cm以内，则第一预设阈值可以设置为3cm或者大于3cm。目标直线段上的坐标点与当前位置的距离可以通过两点之间的距离公式得到，如果与当前位置的距离小于第一预设阈值的坐标点有两个或者两个以上，则选取其中任一坐标点作为目标坐标点，或者选取其中与当前位置的距离最小的坐标点作为目标坐标点。
S130、根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点。
为了方便描述和区分，目标直线段的两个端点分别称为第一目标端点和第二目标端点。
目标坐标点在目标直线段上的位置指目标坐标点在第一目标端点和第二目标端点间的位置。根据目标端点在第一目标端点和第二目标端点间的位置，确定规划路径上由第一目标端点和第二目标端点构成的闭区间内，对应上述位置处的初始坐标点。
示例性地，如果目标坐标点在目标直线段的左半部分，则初始坐标点也位于上述闭区间的左半部分，可从上述闭区间的左半部分任选一点作为初始坐标点；如果目标坐标点在目标直线段的右半部分，则初始坐标点也位于上述闭区间的右半部分，可从上述闭区间的右半部分任选一点作为初始坐标点；如果目标坐标点在目标直线段的中间，则初始坐标点也位于上述闭区间的中间。
S140、以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
本实施例中，当前位置与规划路径的关系为当前位置与规划路径的距离和当前位置在规划路径上的投影点。
由于目标直线段逼近规划路径，可以估计当前位置在规划路径上的投影点在与目标直线段对应的规划路径上，且在初始坐标点附近。
基于上述分析，以初始坐标点为初始值，采用递归方法，更新规划路径上的坐标点，使得当前位置与更新后的坐标点的距离最小，得到当前位置与规划路径的最小距离和当前位置在规划路径上的投影点(即更新后的坐标点)。其中，当前位置在规划路径上的投影点指在当前位置与规划路径的距离最小处，规划路径上的坐标点。
本实施例中，机器人位置与规划路径的关系计算装置直接获取逼近规划路径的目标直线段，而不需要执行直线段的计算操作，从而节省计算时间和计算量；通过确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点，从而根据目标直线段估计出规划路径上接近当前位置的初始坐标点，进而，以初始坐标点为初始值，能够较精确地、高速地计算当前位置与规划路径的距离和当前位置在规划路径上的投影点；而且，直线的表达式和几何计算方法均较简单，因此可以快速确定当前位置在目标直线段上的目标坐标点，进而快速确定初始坐标点。综上，本发明实施例能够高效、高精度地计算机器人位置与规划路径的关系，满足机器人控制的实时性、高精度要求。
实施例二
在上述实施例中，当逼近规划路径的直线段仅有一条时，可直接获取该直线段。当逼近规划路径的直线段有多条时，需要从多条直线段中选取目标直线段。本实施例对上述实施例进行进一步优化，具体限定从多条直线段中获取目标直线段的过程。图3a是本发明实施例二提供的一种机器人位置与规划路径的关系计算方法的流程图，具体包括以下步骤：
S210、获取机器人的当前位置、规划路径和逼近规划路径的多条直线段，多条直线段中每条直线段的两个端点在规划路径上。
可选地，分别获取多条直线段的两个端点，即可获取到多条直线段，从而减少传输数据量。
如图3b所示，逼近规划路径的多条直线段中每条直线段的两个端点在规划路径上，多条直线段首尾相连，多条直线段相连得到的多段线与规划路径十分接近。图3b中，实线是规划路径，实线上方的虚线是多条直线段，实线下方的虚线由控制点构成。
S220、在多条直线段中，确定与当前位置的距离满足第二预设距离要求的目标直线段。
其中，第二预设距离要求指与当前位置的距离小于第二预设阈值，或者与当前位置的距离最小。与第一预设阈值的获取方式相同，第二预设阈值也可以根据机器人与规划路径的距离经验值获取，此处不再赘述。
多条直线段与当前位置的距离可通过计算当前位置到多条直线段的端点的距离或者垂线段的长度得到。如果与当前位置的距离小于第二预设阈值的直线段有两条或者两条以上，则选取其中任一条直线段作为目标直线段，或者选取其中与当前位置的距离最小的直线段作为目标直线段。
在一可选实施方式中，第二预设距离要求指与当前位置的距离最小。基于此，本步骤包括以下两步。
第一步：计算当前位置到每条直线段的最短距离。
可选地，计算当前位置到每条直线段的第一端点的距离、第二端点的距离、当前位置到每条直线段所在直线的距离和投影点。
设当前位置的坐标为(xa,ya)，直线方程为ax+by+z＝0，采用解析的方法计算第一步中的距离和投影点。
如果投影点位置在对应的直线段内，令当前位置到该直线段的最短距离为当前位置到该直线段所在直线的距离。如果投影点位置在对应的直线段外，获取当前位置到该直线段的第一端点的距离、第二端点的距离中的较短者，作为当前位置到该直线段的最短距离。进一步地，获取该直线段上与当前位置距离最短的坐标点。
图3c是本发明实施例二提供的一种当前位置到两条直线段的最短距离和最短距离处的坐标点的示意图。在图3c中，当前位置在直线上的投影点在对应的直线段l1内，则当前位置与直线段l1的最短距离是垂线段d1的长度，对应的距离最短的坐标点是垂足，即投影点。当前位置在直线上的投影点在对应的直线段l2外，即直线段l2的延长线上，则当前位置与直线段l2的最短距离是与第一端点的距离d2，对应的距离最短的坐标点是第一端点。
第二步：在当前位置到每条直线段的最短距离中，确定最短的最短距离对应的目标直线段。
例如，图3c中d1＜d2，则确定最短距离d1对应的目标直线段l1。
又例如，图3d中，d1＞d2，则确定最短距离d2对应的目标直线段l2。
S230、在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点。
例如，在图3c中，确定目标直线段l1上与当前位置距离最短的目标坐标点。此时，目标坐标点位于目标直线段的两个端点之间。
又例如，在图3d中，确定目标直线段l2上与当前位置距离最短目标坐标点。此时，目标坐标点与目标直线段的一个端点重合。
S240、根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点。
S250、以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
本实施例中，通过计算当前位置到每条直线段的最短距离；在当前位置到每条直线段的最短距离中，确定最短的最短距离对应的目标直线段，并获取目标直线段上与当前位置距离最短的目标坐标点，从而精确地计算与当前位置距离最近的目标坐标点，进而精确地计算初始坐标点，提高机器人位置与规划路径的关系的计算精确度和速度。
实施例三
本实施例对上述实施例进行进一步优化，图4a是本发明实施例三提供的一种机器人位置与规划路径的关系计算方法的流程图，具体包括以下步骤：
S310、获取机器人的当前位置、规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上。
S320、在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点。
S330、计算目标坐标点与第一目标端点us之间的第一线段长度Ls。
S340、计算目标坐标点与第二目标端点ue之间的第二线段长度Le。
S350、根据公式计算规划路径上由第一目标端点us和第二目标端点ue构成的闭区间内的初始坐标点u0。
本实施例中，目标坐标点在目标直线段上的位置采用目标坐标点与第一目标端点和第二目标端点之间的线段长度表示。
图4b是本发明实施例三提供的初始坐标点的示意图。假设规划路径是样条曲线，其表达式是f(u)，第一目标端点用us表示，第二目标端点用于ue表示。可选地，计算目标坐标点与第一目标端点us之间的第一线段长度Ls；计算目标坐标点与第二目标端点ue之间的第二线段长度Le；根据公式计算规划路径上第一目标端点us和第二目标端点ue之间的初始坐标点u0。
S360、以初始坐标点u0为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
本实施例中，通过根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，确定上述闭区间内的初始坐标点，从而提高了计算初始坐标点的精确度，进而提高了计算机器人位置与规划路径的关系的精确度和速度。
实施例四
本发明实施例对上述实施例进行进一步优化，图5是本发明实施例四提供的一种机器人位置与规划路径的关系计算方法的流程图，包括以下步骤：
S410、获取机器人的当前位置、规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上。
S420、在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点。
S430、根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点。
S410-S430详见上述实施例的相关描述，此处不再赘述。
S440、以初始坐标点为初始值，采用牛顿-拉弗森方法更新规划路径上的坐标点，以使当前位置到更新后的坐标点的距离是当前位置到规划路径的距离的最小值。
牛顿-拉弗森方法(Newton-Raphson method)用于寻找方程的近似根。本实施例中，将当前位置到规划路径的距离函数f1(u)进行求导，得到方程f′1(u)，求f′1(u)的近似根。
首先将初始坐标点u0带入f′1(u)中，得到f′1(u0)；作经过f′1(u0)的切线，该切线与u坐标轴交点为u1；继续将u1带入f′1(u)中，得到f′1(u1)，以此类推，不断更新坐标点。
S450、如果更新后的坐标点的递归方向与其前一坐标点的递归方向不同，将更新后的坐标点和前一坐标点中的较小者构成初始区域范围的左边界，较大者构成初始区域范围的右边界。
本实施例中，将当前坐标点的递归方向定义为指向后一坐标点的方向，亦即未来的递归方向。同理，前一坐标点的递归方向是指向当前坐标点的方向。例如，当前坐标点的后一坐标点是0.2，当前坐标点是0.1，当前坐标点的前一坐标点是0.5，则前一坐标点的递归方向是向左，当前坐标点的递归方向向右，二者递归方向不同，则将当前坐标点0.1和前一坐标点0.5构成初始区域范围的左边界和右边界。
可见，在坐标点向左递归时，其构成初始区域范围的右边界，在坐标点向右递归时，其构成初始区域范围的左边界。进一步地，继续采用牛顿-拉弗森方法更新规划路径上的坐标点，以使初始区域范围的左边界和右边界至少更新一次，形成新的初始区域范围。新的初始区域范围较之前的初始区域范围小，能够更快速、精确地计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
S460、在初始区域范围中，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
可选地，采用交叉递归方法得到当前位置与规划路径的距离和当前位置在规划路径上的投影点。
具体地，假设初始区域范围是[Ra,Rb]，其中，u∈[Ra,Rb]，Ra是左边界，Rb是右边界，是中点。
计算左边界Ra与当前位置的距离da，称为左侧距离。计算右边界Rb与当前位置的距离db，称为右侧距离。计算左边界和右边界的中点与当前位置的距离dh，称为中心距离。同时，计算当前位置到规划路径的距离函数f1(u)在左边界、右边界及中点处的梯度，分别记为：Da,Db,Dh。
如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均大于等于预设距离阈值，即min(da,db)-dh≥ε，判断Dh与Da,Db的符号，并更新与中点的梯度符号相同的梯度对应的边界，以缩小初始区域范围。
具体地，当sign(Dh)＝sign(Da)时，更新左边界，具体将左边界更新至中点。当sign(Dh)＝sign(Db)时，更新右边界，具体将右边界更新至中点。在更新左边界或者右边界之后，根据更新后的边界计算新的中点，并返回执行左侧距离、右侧距离和中心距离以及Da,Db,Dh的计算操作。
如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均小于预设距离阈值，即0＜min(da,db)-dh＜ε。将中心距离作为当前位置与规划路径的距离，并将中点作为当前位置在规划路径上的投影点，令
如果中心距离大于左侧距离，即da-dh＜0，将左侧距离作为当前位置与规划路径的距离，并将左边界作为当前位置在规划路径上的投影点，令u＝Ra。如果中心距离大于右侧距离，即db-dh＜0，将右侧距离作为当前位置与规划路径的距离，并将右边界作为当前位置在规划路径上的投影点，令u＝Rb。
进一步地，由规划路径的单位坐标u和控制点，计算得到投影点(垂足)对应坐标(x,y)。
采用本发明实施例提供的交叉递归方法可以在短时间内得到高精度的当前位置与规划路径的距离和投影点，在一实验中，求得u＝0.1837598019547777，投影点：[5.006279090.10130299]，距离：0.39874644721447783。比较单纯递归、交叉递归、给定步长递归方法，本实施例提供的方法在准确性和效率方面均有提高。
实施例五
图6是本发明实施例五提供的一种规划路径的直线段逼近方法的流程图，本实施例可适用于在计算机器人的位置与规划路径的关系之前，对规划路径进行直线段逼近的情况，该方法可以由规划路径的直线段逼近装置执行，该装置的运算能力较强，可作为上位机使用。该装置可以由硬件和/或软件构成，并集成在电子设备中。如图6所示，包括以下步骤：
S510、从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段。
规划路径指机器人需要循迹的路径，该路径一般是一条曲线。可选地，用样条曲线f(u)表示规划路径，令其曲线上单位为u∈[0,1]，对应坐标为x(u),y(u)。可选地，从端点位于规划路径上的至少一条直线段中，依次获取一条直线段作为待处理直线段，直到至少一条直线段均处理完成。
可选地，在S510之前还包括：选取规划路径上的至少两个坐标点；以至少两个坐标点中任意两个坐标点为端点构成直线段。
可选地，选取规划路径上首尾两个坐标点，以该首尾两个坐标点为端点构成直线段。当然，也可以获取规划路径上三个或者三个以上的坐标点，以任意两个坐标点为端点构成直线段族。
其中，规划路径上坐标点的选取与机器人和规划路径的位置均有关，可根据实际应用场景进行选取。例如，选取机器人沿路径规划行进方向上位于机器人前端和位于机器人后端预设距离范围内的任意两个坐标点。
对于样条曲线上任意两个坐标点，令其样条曲线坐标为us,ue，可做直线段l(t):(us,ue)，对应第一端点(初始点)、第二端点(终点)为(x(us),y(us)),(x(ue),y(ue))。直线段同时采用样条曲线形式，其上坐标为t∈[0,1]。令构成的至少一条直线段的直线段族表达式为{l0:(0,1)}。
S520、计算待处理直线段与其端点间的规划路径之间的距离。
可选地，计算待处理直线段上任意坐标点与其端点间的规划路径的距离，任意一点可以是中点、第一端点、第二端点或者其它坐标点。
示例性地，计算待处理直线段上的各坐标点到其端点间的规划路径的距离；从待处理直线段上的各坐标点到其端点间的规划路径的距离中，选取最大距离作为待处理直线段与其端点间的距离。
S530、比较待处理直线段与其端点间的规划路径之间的距离与预设阈值。如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，跳转到S540；如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，跳转到S550。
S540、将待处理直线段作为逼近规划路径的直线段，执行S570。
如果待处理直线段上各坐标点至其端点间的规划路径的距离的最大值小于预设阈值，即返回该直线段{lk:(0,1)}，将{lk:(0,1)}作为逼近规划路径的直线段。
S550、删除待处理直线段并保留待处理直线段的两个端点。继续执行S560。
S560、在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段。返回执行S510。
对于待处理直线段上各坐标点至其端点间的规划路径的距离的最大值大于等于预设阈值的直线段li:(us,ue)，对待处理直线段进行拆分。
可选地，可在待处理直线段的两个端点间的规划路径上的任意位置选取一个、两个或者多个坐标点。由于生成了新的直线段，新的直线段与其端点间的规划路径之间的距离是否小于预设阈值尚不得而知，因此，返回执行S510，继续执行新的待处理直线段的获取操作，直到全部直线段处理完成。
S570、判断直线段是否处理完成，如果直线段未处理完成，返回执行S510。如果直线段处理完成，执行S580。
S580、结束本次操作。
本实施例中，通过从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；计算待处理直线段与其端点间的规划路径之间的距离，如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段，返回执行新的待处理直线段的获取操作，直到全部直线段处理完成，从而得到规划路径的直线段逼近，而且，通过拆分直线段的方式能够快速得到与规划路径逼近的直线段。进一步，该装置将直线段和规划路径发送至机器人位置与规划路径的关系计算装置，机器人位置与规划路径的关系计算装置可根据直线段和规划路径快速、高效地计算机器人位置与规划路径的关系。
在上述实施例的各可选实施方式中，在待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值的情况下，如果该距离较远，需要将待处理直线段拆分成数量较多的直线段；如果该距离较近，可将待处理直线段拆分成数量较少的直线段。这样，能够尽快得到距离小于预设阈值的直线段。
基于上述分析，在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，包括：计算待处理直线段与其端点间的规划路径之间的距离减去预设阈值的差值；确定差值对应的差值分段等级；根据差值分段等级对应的坐标点数量，在待处理直线段的两个端点间的规划路径上选取对应数量的坐标点。
预先设置至少两个差值分段等级，每个差值分段等级对应预设的坐标点数量。例如，差值分段等级1～10对应坐标点数量1，差值分段等级：11～20对应坐标点数量2，差值分段等级：21～30对应坐标点数量3。差值分段等级越高，对应的坐标点数量越多，相应地，拆分成的直线段数量越多。
可选地，可以在待处理直线段的两个端点间的规划路径上均匀选取对应数量的坐标点，也可以不均匀选取对应数量的坐标点。
值得说明的是，在待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值的情况下，无论该距离如何，均可以在待处理直线的两个端点间的规划路径上以均匀或者不均匀的方式选取固定数量的坐标点。在一示例中，在待处理直线段的两个端点间的规划路径上选取中点。如图7所示，在待处理直线段li:(us,ue)的两个端点间的规划路径上插入中点:两个端点分别与中点构成第一直线段和第二直线段，并删除直线段li:(us,ue)。
实施例六
图8是本发明实施例六提供的一种机器人位置与规划路径的关系计算装置的结构示意图，包括获取模块61、第一确定模块62、第二确定模块63和计算模块64。
获取模块61，用于获取机器人的当前位置、规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上；
第一确定模块62，用于在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
第二确定模块63，用于根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
计算模块64，用于以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
本实施例中，机器人位置与规划路径的关系计算装置直接获取逼近规划路径的目标直线段，而不需要执行直线段的计算操作，从而节省计算时间和计算量；通过确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点，从而根据目标直线段估计出规划路径上接近当前位置的初始坐标点，进而，以初始坐标点为初始值，能够较精确地、高速地计算当前位置与规划路径的距离和当前位置在规划路径上的投影点；而且，直线的表达式和几何计算方法均较简单，因此可以快速确定当前位置在目标直线段上的目标坐标点，进而快速确定初始坐标点。综上，本发明实施例能够高效、高精度地计算机器人位置与规划路径的关系，满足机器人控制的实时性、高精度要求。
可选地，获取模块61在获取逼近规划路径的目标直线段时，具体用于：获取逼近规划路径的多条直线段，多条直线段中每条直线段的两个端点在规划路径上；在多条直线段中，确定与当前位置的距离满足第二预设距离要求的目标直线段。
可选地，获取模块61在在多条直线段中，确定与当前位置的距离满足第二预设距离要求的目标直线段时，具体用于：计算当前位置到每条直线段的最短距离；在当前位置到每条直线段的最短距离中，确定最短的最短距离对应的目标直线段。
可选地，第二确定模块63在根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点时，具体用于：计算目标坐标点与第一目标端点us之间的第一线段长度Ls；计算目标坐标点与第二目标端点ue之间的第二线段长度Le；根据公式计算规划路径上由第一目标端点us和第二目标端点ue构成的闭区间内的初始坐标点u0。
可选地，计算模块64在以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点时，具体用于：以初始坐标点为初始值，采用牛顿-拉弗森方法更新规划路径上的坐标点，以使当前位置到更新后的坐标点的距离是当前位置到规划路径的距离的最小值；如果更新后的坐标点的递归方向与其前一坐标点的递归方向不同，将更新后的坐标点和前一坐标点中的较小者构成初始区域范围的左边界，较大者构成初始区域范围的右边界；在初始区域范围中，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
可选地，计算模块64在在初始区域范围中，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点时，具体用于：计算左边界与当前位置的左侧距离，右边界与当前位置的右侧距离，以及左边界和右边界的中点与当前位置的中心距离；如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均大于等于预设距离阈值，更新左边界或右边界，以缩小初始区域范围，返回执行左侧距离、右侧距离和中心距离的计算操作；如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均小于预设距离阈值，将中心距离作为当前位置与规划路径的距离，并将中点作为当前位置在规划路径上的投影点；如果中心距离大于左侧距离，将左侧距离作为当前位置与规划路径的距离，并将左边界作为当前位置在规划路径上的投影点；如果中心距离大于右侧距离，将右侧距离作为当前位置与规划路径的距离，并将右边界作为当前位置在规划路径上的投影点。
本发明实施例所提供的机器人位置与规划路径的关系计算装置可执行本发明任意实施例所提供的机器人位置与规划路径的关系计算方法，具备执行方法相应的功能模块和有益效果。
实施例七
图9是本发明实施例七提供的一种规划路径的直线段逼近装置的结构示意图，该装置包括：获取模块71、计算模块72、比较模块73、处理模块74、删除模块75、选取模块76、返回模块77。
获取模块71，用于从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算模块72，用于计算待处理直线段与其端点间的规划路径之间的距离；
比较模块73，用于比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
处理模块74，用于如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；
删除模块75，用于如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
选取模块76，用于在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回模块77，用于返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
本实施例中，通过从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；计算待处理直线段与其端点间的规划路径之间的距离，如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段，返回执行新的待处理直线段的获取操作，直到全部直线段处理完成，从而得到规划路径的直线段逼近，而且，通过拆分直线段的方式能够快速得到与规划路径逼近的直线段。进一步，该装置将直线段和规划路径发送至机器人位置与规划路径的关系计算装置，机器人位置与规划路径的关系计算装置可根据直线段和规划路径快速、高效地计算机器人位置与规划路径的关系。
可选地，该装置还包括直线段构成模块，用于在从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段之前，选取规划路径上的至少两个坐标点；以至少两个坐标点中任意两个坐标点为端点构成直线段。
可选地，选取模块76在在待处理直线段的两个端点间的规划路径上选取至少一个坐标点时，具体用于：计算待处理直线段与其端点间的规划路径之间的距离减去预设阈值的差值；确定差值对应的差值分段等级；根据差值分段等级对应的坐标点数量，在待处理直线段的两个端点间的规划路径上选取对应数量的坐标点。
本发明实施例所提供的规划路径的直线段逼近装置可执行本发明任意实施例所提供的规划路径的直线段逼近方法，具备执行方法相应的功能模块和有益效果。
实施例八
图10是本发明实施例八提供的一种机器人位置与规划路径的关系计算系统的结构示意图，包括相连接的机器人位置与规划路径的关系计算装置81和规划路径的直线段逼近装置82；
规划路径的直线段逼近装置82，用于：从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；计算待处理直线段与其端点间的规划路径之间的距离；比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；返回执行新的待处理直线段的获取操作，直到全部直线段处理完成；将逼近规划路径的直线段和规划路径发送至机器人位置与规划路径的关系计算装置81；
机器人位置与规划路径的关系计算装置81，用于：获取机器人的当前位置、规划路径的直线段逼近装置82发送的规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上；在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
实施例九
图11是本发明实施例九提供的一种电子设备的结构示意图，如图11所示，该电子设备包括处理器90、存储器91；电子设备中处理器90的数量可以是一个或多个，图11中以一个处理器90为例；电子设备中的处理器90、存储器91可以通过总线或其他方式连接，图11中以通过总线连接为例。
存储器91作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明实施例中的机器人位置与规划路径的关系计算方法对应的程序指令/模块(例如，机器人位置与规划路径的关系计算装置中的获取模块61、第一确定模块62、第二确定模块63和计算模块64)。处理器90通过运行存储在存储器91中的软件程序、指令以及模块，从而执行电子设备的各种功能应用以及数据处理，即实现上述的机器人位置与规划路径的关系计算方法。
存储器91可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据终端的使用所创建的数据等。此外，存储器91可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器91可进一步包括相对于处理器90远程设置的存储器，这些远程存储器可以通过网络连接至电子设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
本发明实施例还提供了另一种电子设备，其包括：一个或多个处理器；存储器，用于存储一个或多个程序，当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现本发明实施例所提供的规划路径的直线段逼近方法，该方法包括：
从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算待处理直线段与其端点间的规划路径之间的距离；
比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；
如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
当然，本领域技术人员可以理解，处理器还可以实现本发明任意实施例所提供的规划路径的直线段逼近方法的技术方案。该电子设备的硬件结构以及功能可参见实施例九的内容解释。
实施例十
本发明实施例十还提供一种其上存储有计算机程序的计算机可读存储介质，计算机程序在由计算机处理器执行时用于执行一种机器人位置与规划路径的关系计算方法，该方法包括：
获取机器人的当前位置、规划路径和逼近规划路径的目标直线段，目标直线段的两个端点在规划路径上；
在目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
根据目标坐标点在目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
以初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
当然，本发明实施例所提供的一种其上存储有计算机程序的计算机可读存储介质，其计算机程序不限于如上的方法操作，还可以执行本发明任意实施例所提供的机器人位置与规划路径的关系计算方法中的相关操作.
本发明实施例还提供了另一种计算机可读存储介质，其上存储的计算机程序在由计算机处理器执行时用于执行规划路径的直线段逼近方法，该方法包括：
从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算待处理直线段与其端点间的规划路径之间的距离；
比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
如果待处理直线段与其端点间的规划路径之间的距离小于预设阈值，将待处理直线段作为逼近规划路径的直线段；
如果待处理直线段与其端点间的规划路径之间的距离大于等于预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
当然，本发明实施例所提供的一种计算机可读存储介质，其计算机程序不限于如上的方法操作，还可以执行本发明任意实施例所提供的应用于规划路径的直线段逼近方法中的相关操作。对存储介质的介绍可参见实施例九中的内容解释。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例的方法。
值得注意的是，上述规划路径的直线段逼近装置和机器人位置与规划路径的关系计算装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。
Claims (14)
1.一种机器人位置与规划路径的关系计算方法，其特征在于，包括：
获取机器人的当前位置、规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；
在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
2.根据权利要求1所述的方法，其特征在于，所述获取逼近所述规划路径的目标直线段，包括：
获取逼近所述规划路径的多条直线段，所述多条直线段中每条直线段的两个端点在所述规划路径上；
在所述多条直线段中，确定与当前位置的距离满足第二预设距离要求的目标直线段。
3.根据权利要求2所述的方法，其特征在于，所述在所述多条直线段中，确定与当前位置的距离满足第二预设距离要求的目标直线段，包括：
计算当前位置到每条直线段的最短距离；
在当前位置到每条直线段的最短距离中，确定最短的最短距离对应的目标直线段。
4.根据权利要求1所述的方法，其特征在于，所述根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点，包括：
计算所述目标坐标点与第一目标端点us之间的第一线段长度Ls；
计算所述目标坐标点与第二目标端点ue之间的第二线段长度Le；
根据公式计算规划路径上由第一目标端点us和第二目标端点ue构成的闭区间内的初始坐标点u0。
5.根据权利要求1所述的方法，其特征在于，所述以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点，包括：
以所述初始坐标点为初始值，采用牛顿-拉弗森方法更新规划路径上的坐标点，以使当前位置到更新后的坐标点的距离是当前位置到所述规划路径的距离的最小值；
如果更新后的坐标点的递归方向与其前一坐标点的递归方向不同，将更新后的坐标点和前一坐标点中的较小者构成初始区域范围的左边界，较大者构成初始区域范围的右边界；
在所述初始区域范围中，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
6.根据权利要求5所述的方法，其特征在于，所述在所述初始区域范围中，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点，包括：
计算左边界与当前位置的左侧距离，右边界与当前位置的右侧距离，以及左边界和右边界的中点与当前位置的中心距离；
如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均大于等于预设距离阈值，更新左边界或右边界，以缩小初始区域范围，返回执行左侧距离、右侧距离和中心距离的计算操作；
如果中心距离小于左侧距离和右侧距离，并且左侧距离和右侧距离减去中心距离得到的差值均小于预设距离阈值，将中心距离作为当前位置与规划路径的距离，并将所述中点作为当前位置在规划路径上的投影点；
如果中心距离大于左侧距离，将左侧距离作为当前位置与规划路径的距离，并将左边界作为当前位置在规划路径上的投影点；
如果中心距离大于右侧距离，将右侧距离作为当前位置与规划路径的距离，并将右边界作为当前位置在规划路径上的投影点。
7.一种规划路径的直线段逼近方法，其特征在于，包括：
从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算待处理直线段与其端点间的规划路径之间的距离；
比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；
如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
8.根据权利要求7所述的方法，其特征在于，在从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段之前，还包括：
选取所述规划路径上的至少两个坐标点；
以至少两个坐标点中任意两个坐标点为端点构成直线段。
9.根据权利要求7所述的方法，其特征在于，所述在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，包括：
计算待处理直线段与其端点间的规划路径之间的距离减去预设阈值的差值；
确定所述差值对应的差值分段等级；
根据所述差值分段等级对应的坐标点数量，在待处理直线段的两个端点间的规划路径上选取对应数量的坐标点。
10.一种机器人位置与规矩路径的关系计算装置，其特征在于，包括：
获取模块，用于获取机器人的当前位置、规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；
第一确定模块，用于在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；
第二确定模块，用于根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；
计算模块，用于以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
11.一种规划路径的直线段逼近装置，其特征在于，包括：
获取模块，用于从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；
计算模块，用于计算待处理直线段与其端点间的规划路径之间的距离；
比较模块，用于比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；
处理模块，用于如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；
删除模块，用于如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；
选取模块，用于在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；
返回模块，用于返回执行新的待处理直线段的获取操作，直到全部直线段处理完成。
12.一种机器人位置与规划路径的关系计算系统，其特征在于，包括：相连接的机器人位置与规划路径的关系计算装置和规划路径的直线段逼近装置；
所述规划路径的直线段逼近装置，用于：从端点位于规划路径上的至少一条直线段中，获取一条直线段作为待处理直线段；计算待处理直线段与其端点间的规划路径之间的距离；比较待处理直线段与其端点间的规划路径之间的距离与预设阈值；如果待处理直线段与其端点间的规划路径之间的距离小于所述预设阈值，将待处理直线段作为逼近所述规划路径的直线段；如果待处理直线段与其端点间的规划路径之间的距离大于等于所述预设阈值，删除待处理直线段并保留待处理直线段的两个端点；在待处理直线段的两个端点间的规划路径上选取至少一个坐标点，在待处理直线段的两个端点和至少一个坐标点中，以相邻坐标点为端点构成新的直线段；返回执行新的待处理直线段的获取操作，直到全部直线段处理完成；将逼近所述规划路径的直线段和规划路径发送至机器人位置与规划路径的关系计算装置；
所述机器人位置与规划路径的关系计算装置，用于：获取机器人的当前位置、规划路径的直线段逼近装置发送的规划路径和逼近所述规划路径的目标直线段，所述目标直线段的两个端点在所述规划路径上；在所述目标直线段上，确定与当前位置的距离满足第一预设距离要求的目标坐标点；根据所述目标坐标点在所述目标直线段上的位置、目标直线段的第一目标端点和第二目标端点，在规划路径上由第一目标端点和第二目标端点构成的闭区间内确定初始坐标点；以所述初始坐标点为初始值，计算当前位置与规划路径的距离和当前位置在规划路径上的投影点。
13.一种电子设备，其特征在于，所述电子设备包括：
一个或多个处理器；
存储器，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的机器人位置与规划路径的关系计算方法，或者权利要求7-9中任一所述的规划路径的直线段逼近方法。
14.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6中任一所述的机器人位置与规划路径的关系计算方法，或者权利要求7-9中任一所述的规划路径的直线段逼近方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810952483.3A CN108994838A (zh) | 2018-08-21 | 2018-08-21 | 机器人位置与规划路径的关系计算方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810952483.3A CN108994838A (zh) | 2018-08-21 | 2018-08-21 | 机器人位置与规划路径的关系计算方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108994838A true CN108994838A (zh) | 2018-12-14 |
Family
ID=64592929
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810952483.3A Pending CN108994838A (zh) | 2018-08-21 | 2018-08-21 | 机器人位置与规划路径的关系计算方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108994838A (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109731708A (zh) * | 2018-12-27 | 2019-05-10 | 上海理工大学 | 基于图像识别的汽车维修自动喷漆方法 |
| CN111190989A (zh) * | 2019-12-31 | 2020-05-22 | 深圳安智杰科技有限公司 | 离散轨迹分析方法、装置、电子设备及可读存储介质 |
| CN111736602A (zh) * | 2020-06-16 | 2020-10-02 | 国网上海市电力公司 | 一种改进的轮式机器人路径规划方法 |
| CN113711155A (zh) * | 2019-07-19 | 2021-11-26 | 西门子（中国）有限公司 | 机器人路径规划器、方法、装置和计算机可读存储介质 |
| CN113791627A (zh) * | 2021-11-16 | 2021-12-14 | 中国科学院自动化研究所 | 一种机器人导航方法、设备、介质和产品 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007105077A2 (en) * | 2006-03-14 | 2007-09-20 | Toyota Jidosha Kabushiki Kaisha | Trajectory tracking control system and method for mobile unit |
| CN102566500A (zh) * | 2011-11-24 | 2012-07-11 | 山东理工大学 | 基于直线段逼近节点的数控系统轮廓误差控制方法 |
| CN104848870A (zh) * | 2015-06-05 | 2015-08-19 | 深圳市凯立德科技股份有限公司 | 一种偏航后的导航路径规划方法及设备 |
| CN105867379A (zh) * | 2016-04-13 | 2016-08-17 | 上海物景智能科技有限公司 | 一种机器人的运动控制方法及控制系统 |
| CN106345857A (zh) * | 2016-09-29 | 2017-01-25 | 哈尔滨工业大学 | 一种平面分段逼近圆弧的方法 |
| CN106997355A (zh) * | 2016-01-25 | 2017-08-01 | 北京四维图新科技股份有限公司 | 基于Mongodb获取距离、确定距离最短线段的方法及装置 |
| CN107390691A (zh) * | 2017-07-28 | 2017-11-24 | 广东嘉腾机器人自动化有限公司 | 一种agv路径跟踪方法 |
-
2018
- 2018-08-21 CN CN201810952483.3A patent/CN108994838A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007105077A2 (en) * | 2006-03-14 | 2007-09-20 | Toyota Jidosha Kabushiki Kaisha | Trajectory tracking control system and method for mobile unit |
| CN102566500A (zh) * | 2011-11-24 | 2012-07-11 | 山东理工大学 | 基于直线段逼近节点的数控系统轮廓误差控制方法 |
| CN104848870A (zh) * | 2015-06-05 | 2015-08-19 | 深圳市凯立德科技股份有限公司 | 一种偏航后的导航路径规划方法及设备 |
| CN106997355A (zh) * | 2016-01-25 | 2017-08-01 | 北京四维图新科技股份有限公司 | 基于Mongodb获取距离、确定距离最短线段的方法及装置 |
| CN105867379A (zh) * | 2016-04-13 | 2016-08-17 | 上海物景智能科技有限公司 | 一种机器人的运动控制方法及控制系统 |
| CN106345857A (zh) * | 2016-09-29 | 2017-01-25 | 哈尔滨工业大学 | 一种平面分段逼近圆弧的方法 |
| CN107390691A (zh) * | 2017-07-28 | 2017-11-24 | 广东嘉腾机器人自动化有限公司 | 一种agv路径跟踪方法 |
Non-Patent Citations (3)
| Title |
|---|
| 冷强奎: "一种基于逐段比较的直线逼近曲线方法", 《渤海大学学报(自然科学版)》 * |
| 周晓铭: "基于二分法的曲线等误差圆弧拟合算法及其应用", 《南昌大学学报(理科版)》 * |
| 马怡然: "基于几何特征的点到空间参数曲线最小距离的简化算法", 《试验研究》 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109731708A (zh) * | 2018-12-27 | 2019-05-10 | 上海理工大学 | 基于图像识别的汽车维修自动喷漆方法 |
| CN113711155A (zh) * | 2019-07-19 | 2021-11-26 | 西门子（中国）有限公司 | 机器人路径规划器、方法、装置和计算机可读存储介质 |
| CN111190989A (zh) * | 2019-12-31 | 2020-05-22 | 深圳安智杰科技有限公司 | 离散轨迹分析方法、装置、电子设备及可读存储介质 |
| CN111190989B (zh) * | 2019-12-31 | 2023-03-14 | 深圳安智杰科技有限公司 | 离散轨迹分析方法、装置、电子设备及可读存储介质 |
| CN111736602A (zh) * | 2020-06-16 | 2020-10-02 | 国网上海市电力公司 | 一种改进的轮式机器人路径规划方法 |
| CN113791627A (zh) * | 2021-11-16 | 2021-12-14 | 中国科学院自动化研究所 | 一种机器人导航方法、设备、介质和产品 |
| CN113791627B (zh) * | 2021-11-16 | 2022-02-11 | 中国科学院自动化研究所 | 一种机器人导航方法、设备、介质和产品 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108994838A (zh) | 机器人位置与规划路径的关系计算方法及系统 | |
| CN110083165B (zh) | 一种机器人在复杂狭窄环境下路径规划方法 | |
| CN113219998B (zh) | 一种基于改进双向informed-RRT*的车辆路径规划方法 | |
| CN110221600B (zh) | 路径规划方法、装置、计算机设备和存储介质 | |
| CN108582073B (zh) | 一种基于改进的随机路标地图法的机械臂快速避障方法 | |
| CN111375205B (zh) | 游戏中寻路路径的处理方法、装置、电子设备及存储介质 | |
| CN112595337B (zh) | 避障路径规划方法、装置、电子装置、车辆及存储介质 | |
| CN111737389B (zh) | 曲线优化方法、装置、设备和介质 | |
| CN110648529A (zh) | 一种车辆行驶轨迹的渲染方法、装置、设备及存储介质 | |
| WO2023103692A1 (zh) | 自动驾驶的决策规划方法、电子设备及计算机存储介质 | |
| CN108225333B (zh) | 一种用于航线规划的最优路径生成方法 | |
| CN113312760B (zh) | 一种基于交通仿真的网联机动车右转轨迹规划方法和装置 | |
| CN112327826B (zh) | 一种路径规划方法、装置、设备和介质 | |
| CN109916421A (zh) | 路径规划方法及装置 | |
| Liu et al. | Laformer: Trajectory prediction for autonomous driving with lane-aware scene constraints | |
| JP2017146710A (ja) | 搬送計画生成装置および搬送計画生成方法 | |
| CN113971723B (zh) | 高精地图中三维地图的构建方法、装置、设备和存储介质 | |
| KR102568948B1 (ko) | 장애물 속도를 결정하는 방법, 장치, 전자 기기, 저장 매체 및 프로그램 | |
| CN110749332B (zh) | Rs曲线的曲率优化方法、装置、计算机设备及存储介质 | |
| CN117007066A (zh) | 多规划算法集成的无人驾驶轨迹规划方法及相关装置 | |
| CN116734877A (zh) | 基于改进a*算法与动态窗口法的机器人动态避障方法 | |
| CN110647162A (zh) | 一种导游无人机路径规划方法、终端设备及存储介质 | |
| Hallgarten et al. | Stay on Track: A Frenet Wrapper to Overcome Off-road Trajectories in Vehicle Motion Prediction | |
| CN113432618A (zh) | 轨迹生成方法、装置、计算设备及计算机可读存储介质 | |
| JP4535683B2 (ja) | 移動経路諸元推定方法、装置及びコンピュータプログラム |
Legal Events
| 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 | ||
| AD01 | Patent right deemed abandoned |
Effective date of abandoning: 20210319 |
|
| AD01 | Patent right deemed abandoned |