CN117519212A - 路径跟踪控制方法、装置、终端及计算机可读存储介质 - Google Patents
路径跟踪控制方法、装置、终端及计算机可读存储介质 Download PDFInfo
- Publication number
- CN117519212A CN117519212A CN202410005646.2A CN202410005646A CN117519212A CN 117519212 A CN117519212 A CN 117519212A CN 202410005646 A CN202410005646 A CN 202410005646A CN 117519212 A CN117519212 A CN 117519212A
- Authority
- CN
- China
- Prior art keywords
- path
- robot
- information
- point
- determining
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000003860 storage Methods 0.000 title claims abstract description 15
- 230000004044 response Effects 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 8
- 238000010408 sweeping Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000011065 in-situ storage Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明提供一种路径跟踪控制方法、装置、终端及计算机可读存储介质，路径跟踪控制方法包括：获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数；基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点；基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息；基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。本申请中基于起点、目标点和机器人在局部坐标系中的更新位姿信息可以得到准确的运行信息，进行控制机器人以合理的最大速度从起点运行至目标点，节约跟踪时间，使得机器人可以实现及时准确的跟踪。
Description
技术领域
本发明涉及机器人技术领域，特别是涉及一种路径跟踪控制方法、装置、终端及计算机可读存储介质。
背景技术
扫地机器人作为一种新型家用设备，在日常生活中逐渐被越来越多的家庭使用。为了能够在家庭场景中自如的移动，需要扫地机器人的能够准确流畅地移动到目标位置。但是扫地机器人的处理能力有限，要求路径跟踪控制方法尽量简单高效。且主控单元运行非实时操作系统，在具体执行路径跟踪的过程中，可能存在控制延迟、获取定位时机波动等因素的影响。
发明内容
本发明主要解决的技术问题是提供一种路径跟踪控制方法、装置、终端及计算机可读存储介质，解决现有技术中机器人的跟踪效率不佳的问题。
为解决上述技术问题，本发明采用的第一个技术方案是：提供一种路径跟踪控制方法，路径跟踪控制方法包括：
获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数；
基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点；目标点和起点之间的路径点的方向信息与起点的方向信息一致；
基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息；
基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。
其中，路径点信息包括路径点对应的标志信息、编号和位姿信息；标志信息包括第一标识符和第二标识符；第一标识符表示机器人未经过路径点；第二标识符表示机器人已经过路径点；位姿信息包括位置信息和方向信息；
基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点，包括：
基于第一标识符对应的路径点中具有最小编号的路径点，确定起点；
基于起点的方向信息与第一标识符对应的其他路径点的方向信息，确定目标点；其中，第i个路径点的方向信息为第i个路径点指向第i+1个路径点的方向；第n个路径点的方向信息与第n-1个路径点的方向信息一致；0＜i＜n。
其中，当前位姿包括当前位置和当前方向；
基于第一标识符对应的路径点中具有最小编号的路径点，确定起点，包括：
响应于待跟踪路径中各路径点的标志信息均为第一标识符，则将机器人的当前位置作为起点；起点的当前方向为起点指向最小编号对应的路径点；
响应于第一标识符对应的路径点中最小编号为m，且0＜m＜n，则将编号为m-1对应的路径点作为起点。
其中，基于起点的方向信息与第一标识符对应的其他路径点的方向信息，确定目标点，包括：
将起点的方向信息与第一标识符对应的其他路径点的方向信息依次进行比较；
选取第一标识符对应的路径点中第一个与起点的方向信息不一致的路径点作为目标点；目标点的编号大于起点的编号。
其中，基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息，包括：
以目标点作为局部坐标系的原点，以起点指向目标点的方向作为局部坐标系中第一坐标轴的正方向；
将起点和机器人分别投影至局部坐标系，得到起点和机器人分别对应的更新位姿信息。
其中，运行信息包括角速度和线速度；
基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪，包括：
基于机器人在局部坐标系的更新位姿信息，确定机器人与第一坐标轴之间的第一最短距离以及与第二坐标轴之间的第二最短距离；第二坐标轴的正方向为第一坐标轴的正方向逆时针旋转90°所指的方向；
响应于第一最短距离小于第一阈值，则将第二最短距离与第二阈值进行比较；
响应于第二最短距离不小于第二阈值，则基于机器人在局部坐标系的更新位姿信息确定角速度和线速度；
控制机器人基于角速度和线速度进行路径跟踪。
其中，基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪，还包括：
响应于第一最短距离小于第一阈值，且第二最短距离小于第二阈值，则确定机器人已运行至目标点；
将起点至目标点中各路径点的标志位赋值为第二标识符。
其中，基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪，还包括：
响应于第一最短距离不小于第一阈值，则基于机器人的所在位置和机器人在局部坐标系的第一坐标轴上的投影位置构建更新局部坐标系；机器人在局部坐标系的第一坐标轴上的投影位置作为更新局部坐标系的原点，机器人的所在位置指向机器人在更新局部坐标系的第一坐标轴上的投影位置的方向作为更新局部坐标系的第一坐标轴的正方向；
将机器人的更新位姿信息投影至更新局部坐标系，则确定机器人在更新局部坐标系的更新位姿信息；
基于机器人在更新局部坐标系的更新位姿信息确定角速度和线速度；
控制机器人基于角速度和线速度进行路径跟踪。
其中，更新位姿信息包括更新位置信息和更新方向信息；更新位置信息包括第一方向坐标和第二方向坐标；第一方向坐标为更新位置信息在第一坐标轴上的分量，第二方向坐标为更新位置信息在第二坐标轴上的分量；
确定角速度和线速度的步骤，包括：
基于机器人的第一方向坐标、机器人的更新方向信息以及目标点的更新方向信息，确定线速度；
基于机器人的第二方向坐标、机器人的更新方向信息与目标点的更新方向信息之间的偏差值，确定角速度。
其中，基于机器人的第一方向坐标、机器人的更新方向信息以及目标点的更新方向信息，确定线速度，包括：
基于机器人的更新方向信息与目标点的更新方向信息，确定约束系数；
基于第一方向坐标以及约束系数，确定线速度。
其中，基于机器人的更新方向信息与目标点的更新方向信息，确定约束系数，包括：
基于机器人的更新方向信息与目标点的更新方向信息之间的差值，确定角度差值；
响应于角度差值满足预设条件，则将第一系数作为约束系数；第一系数的取值范围为1；
响应于角度差值不满足预设条件，则将第二系数作为约束系数；第二系数的取值范围为大于等于0且小于1。
其中，基于第一方向坐标以及约束系数，确定线速度，包括：
基于第一方向坐标，确定机器人与目标点之间的第一运行距离；
根据第一运行距离在预设数据表中查找，确定第一运行距离对应的第一速度信息；
将约束系数与第一速度信息的乘积作为线速度。
其中，基于机器人的第二方向坐标、机器人的更新方向信息与目标点的更新方向信息之间的偏差值，确定角速度，包括：
响应于第二方向坐标的绝对值不大于第三阈值，则将机器人的第二方向坐标、机器人的更新方向信息与目标点的更新方向信息之间的偏差值进行加权融合得到角速度；第三阈值小于第一阈值；
响应于第二方向坐标的绝对值大于第三阈值，则基于第二方向坐标确定角速度。
其中，基于第二方向坐标确定角速度，包括：
基于机器人的第二方向坐标，确定机器人与目标点之间的第二运行距离；
根据第二运行距离在预设数据表中查找，确定角速度。
为解决上述技术问题，本发明采用的第二个技术方案是：提供一种路径跟踪控制装置，路径跟踪控制装置包括：
获取模块，用于获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数；
确定模块，用于基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点；目标点和起点之间的路径点的方向信息与起点的方向信息一致；
处理模块，用于基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息；
跟踪模块，用于基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。
为解决上述技术问题，本发明采用的第三个技术方案是：提供一种终端，终端包括存储器、处理器以及存储于存储器中并在处理器上运行的计算机程序，处理器用于执行程序数据以实现如上述的路径跟踪控制方法中的步骤。
为解决上述技术问题，本发明采用的第四个技术方案是：提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如上述的路径跟踪控制方法中的步骤。
本发明的有益效果是：区别于现有技术的情况，提供的一种路径跟踪控制方法、装置、终端及计算机可读存储介质，路径跟踪控制方法包括：获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数；基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点；基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息；基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。本申请中通过实时获取机器人的当前位姿，以便于根据机器人的当前位姿以及待跟踪路径中各路径点的位姿信息确定当前跟踪的起点和目标点，基于起点、目标点和机器人在局部坐标系中的更新位姿信息可以得到准确的运行信息，进行控制机器人以合理的最大速度从起点运行至目标点，节约跟踪时间，使得机器人可以实现及时准确的跟踪，提高跟踪效率。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明提供的路径跟踪控制方法的流程示意图；
图2是图1提供的路径跟踪控制方法中步骤S2一具体实施例的流程示意图；
图3是图1提供的路径跟踪控制方法中步骤S4一具体实施例的流程示意图；
图4是本发明提供的预设数据库中距离与速度之间的对应曲线示意图；
图5是本发明提供的角度差值与系数之间的对应关系图；
图6是图3提供的路径跟踪控制方法中步骤S41一具体实施例的流程示意图；
图7是图3提供的路径跟踪控制方法中步骤S42一具体实施例的流程示意图；
图8是本发明提供的路径跟踪控制装置一实施例的框架示意图；
图9是本发明提供的终端一实施例的框架示意图；
图10是本发明提供的计算机可读存储介质一实施例的框架示意图。
具体实施方式
下面结合说明书附图，对本申请实施例的方案进行详细说明。
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、接口、技术之类的具体细节，以便透彻理解本申请。
本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。此外，本文中的“多”表示两个或者多于两个。
为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本发明所提供的一种路径跟踪控制方法做进一步详细描述。
扫地机器人为了更加准确地跟随目标路径，一般需要仔细区分设备的运动状态。以直线运动为例，需要设备从静止开始加速到最大速度，再减速到零，通过这一操作保证设备停止时尽量靠近目标点。
请参阅图1，图1是本发明提供的路径跟踪控制方法的流程示意图。
本实施例中提供一种路径跟踪控制方法，本申请实施例中所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。该路径跟踪控制方法包括如下步骤。
S1：获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数。
S2：基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点。
S3：基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息。
S4：基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。
本实施例中提供一种路径跟踪控制方法中，通过实时获取机器人的当前位姿，以便于根据机器人的当前位姿以及待跟踪路径中各路径点的位姿信息确定当前跟踪的起点和目标点，基于起点、目标点和机器人在局部坐标系中的更新位姿信息可以得到准确的运行信息，进行控制机器人以合理的最大速度从起点运行至目标点，节约跟踪时间，使得机器人可以实现及时准确的跟踪。
在一实施例中，步骤S1中获取机器人的当前位姿和待跟踪路径的具体实施方式如下所示。
本实施例中提供的机器人的运动机构为两轮差动式。两轮差动式的机器人是一种非完整约束结构，不能进行横移，想要到达平面中的任意位姿，往往不能一次完成移动，需要使用一些组合运动。
通过扫地机器人本身的环境感知传感器检测机器人的当前位置和当前朝向。其中，机器人的当前位置是在全局坐标系中的坐标位置。
同时获取机器人的待跟踪路径。待跟踪路径为机器人需要跟踪的完整路径或部分路径。待跟踪路径包括多个路径点信息。路径点信息包括路径点对应的标志信息、编号和位姿信息。
路径点的标志信息用于表示机器人是否到达该路径点。标志信息包括第一标识符和第二标识符；第一标识符表示机器人未经过该路径点；第二标识符表示机器人已经过该路径点。
路径点的位姿信息包括路径点的全局坐标位置和方向信息。例如，待跟踪路径中包括n个路径点。其中，第i个路径点的方向信息为第i个路径点指向第i+1个路径点的方向；第n个路径点的方向信息与第n-1个路径点的方向信息一致；0＜i＜n。当n为1时，路径点的方向信息为机器人指向路径点的方向。
待跟踪路径为按照路径点的编号依次连线得到的路径。
待跟踪路径在每个执行周期过程中都会获取，需要对待跟踪路径是否发生变化进行判断。如果当前时刻和上一时刻获取的待跟踪路径一致，则继续之前的跟踪过程。若待跟踪路径不同，则更新当前的待跟踪路径，并重置已经经过编号路径点的标志信息。为了保证所有路径点要按顺序依次到达，路径点上不能有障碍物，即保证为可以到达的全局坐标。在路径跟踪过程中，只向一个未到过的目标路径点移动。通过判断机器人和目标路径点之间的距离是否小于设定范围，作为机器人是否到达该目标路径点的依据。
在一具体实施例中，获取机器人在当前时刻的当前位姿和待跟踪路径。判断当前时刻的相邻前一时刻是否获取到待跟踪路径。当前位姿包括当前位置和当前方向。
如果相邻前一时刻获取到待跟踪路径，则将当前时刻的待跟踪路径与相邻前一时刻的待跟踪路径进行比对，基于相邻前一时刻的待跟踪路径中各路径点的标志信息作为当前时刻的待跟踪路径中各路径点的起始标志信息。选取待跟踪路径中第一个第一标识符对应的路径点以及待跟踪路径中最后一个第二标识符对应的路径点。
如果最后一个第二标识符对应的路径点的编号与待跟踪路径对应的路径点总数一致，则表明机器人已经完成了待跟踪路径的跟踪，控制机器人保持原地不动。
如果待跟踪路径中未包含路径点，则控制机器人保持原地不动。
如果相邻前一时刻未获取到待跟踪路径，则将当前时刻的待跟踪路径中的各路径点的标志信息赋值为第一标识符。
在一实施例中，步骤S2中基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点的具体实施方式如下所示。
请参阅图2，图2是图1提供的路径跟踪控制方法中步骤S2一具体实施例的流程示意图。
S21：基于第一标识符对应的路径点中具有最小编号的路径点，确定起点。
在一具体实施例中，响应于待跟踪路径中各路径点的标志信息均为第一标识符，则将机器人的当前位置作为起点，起点的当前方向为起点指向最小编号对应的路径点的方向。
在一具体实施例中，响应于第一标识符对应的路径点中最小编号为m，且0＜m＜n，则将编号为m-1对应的路径点作为起点。具体地，待跟踪路径中编号小于m的各路径点的标志信息为第二标识符。
S22：基于起点的方向信息与第一标识符对应的其他路径点的方向信息，确定目标点。
具体地，将起点的方向信息与第一标识符对应的其他路径点的方向信息依次进行比较。
选取第一标识符对应的路径点中第一个与起点的方向信息不一致的路径点作为目标点；目标点的编号大于起点的编号。其中，目标点和起点之间的路径点的方向信息与起点的方向信息一致，以便于机器人从起点到达目标点的过程中，不需要在中间路径点进行减速/加速，以节约时间，提高跟踪速度。
选取指向相同路径点中最远点，可以保持机器人按照最大速度前进，避免频繁加减速。
在一实施例中，步骤S3中基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息的具体实施方式如下所示。
具体地，根据起点和目标点建立右手坐标系，即局部坐标系。以目标点作为局部坐标系的原点，以起点指向目标点的方向作为局部坐标系中第一坐标轴的正方向，第二坐标轴正方向为第一坐标轴正方向逆时针转动九十度的方向。本实施例中，局部坐标系中第一坐标轴的正方向为局部坐标系中X轴的正方向。局部坐标系还包括第二坐标轴，局部坐标系中第二坐标轴的正方向为局部坐标系中Y轴的正方向。局部坐标系中第一坐标轴和第二坐标轴相互垂直且相交于局部坐标系的原点。
将起点和机器人的当前位姿分别投影至局部坐标系，得到起点和机器人分别对应的更新位姿信息。
在一实施例中，步骤S4中基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪的具体实施方式如下所示。
其中，运行信息包括角速度和线速度。
在一具体实施例中，基于机器人在局部坐标系的更新位姿信息，确定机器人与第一坐标轴之间的第一最短距离以及与第二坐标轴之间的第二最短距离；第二坐标轴与第一坐标轴之间相互垂直且相交于局部坐标系的原点。其中，机器人在局部坐标系的更新位姿信息包括更新位置信息（x1，y1）和更新角度信息。机器人与第一坐标轴之间的第一最短距离为y1，机器人与第二坐标轴之间的第二最短距离为x1。
响应于第一最短距离小于第一阈值，则将第二最短距离与第二阈值进行比较；响应于第二最短距离不小于第二阈值，则基于机器人在局部坐标系的更新位姿信息确定角速度和线速度。
控制机器人在当前位姿状态基于角速度和线速度进行路径跟踪。
在一具体实施例中，响应于第一最短距离小于第一阈值，且第二最短距离小于第二阈值，则确定机器人已运行至目标点；将起点至目标点中各路径点的标志位赋值为第二标识符。
在一具体实施例中，响应于第一最短距离不小于第一阈值，则基于机器人的所在位置和机器人在局部坐标系的第一坐标轴上的投影位置构建更新局部坐标系；机器人在局部坐标系的第一坐标轴上的投影位置作为更新局部坐标系的原点，机器人的所在位置指向机器人在更新局部坐标系的第一坐标轴上的投影位置的方向作为更新局部坐标系的第一坐标轴的正方向；将机器人的更新位姿信息投影至更新局部坐标系，则确定机器人在更新局部坐标系的更新位姿信息；基于机器人在更新局部坐标系的更新位姿信息确定角速度和线速度；控制机器人基于角速度和线速度进行路径跟踪。
具体地，上述实施例中的更新位姿信息包括更新位置信息和更新方向信息；更新位置信息包括第一方向坐标和第二方向坐标；第一方向坐标为更新位置信息在第一坐标轴上的分量，第二方向坐标为更新位置信息在第二坐标轴上的分量。
在一具体实施例中，确定角速度和线速度的具体步骤如下所示。
请参阅图3，图3是图1提供的路径跟踪控制方法中步骤S4一具体实施例的流程示意图。
S41：基于机器人的第一方向坐标、机器人的更新方向信息以及目标点的更新方向信息，确定线速度。
请参阅图4至图6，图4是本发明提供的预设数据库中距离与速度之间的对应曲线示意图；图5是本发明提供的角度差值与系数之间的对应关系图；图6是图3提供的路径跟踪控制方法中步骤S41一具体实施例的流程示意图。
S411：基于机器人的更新方向信息与目标点的更新方向信息，确定约束系数。
具体地，基于机器人的更新方向信息与目标点的更新方向信息之间的差值，确定角度差值。响应于角度差值满足预设条件，则将第一系数作为约束系数；第一系数的取值范围为1。响应于角度差值不满足预设条件，则将第二系数作为约束系数；第二系数的取值范围为大于等于0且小于1。
其中，预设条件是角度差值在[-θ，0°）或[0°，θ]的角度范围内。当角度差值的绝对值在θ至90°时，第二系数逐渐减小。当角度差值的绝对值为90°时，约束系数为0，如图5。例如，θ可以取值为20°、30°、40°、45°、50°等，具体根据实际情况进行设定。
S412：基于第一方向坐标以及约束系数，确定线速度。
具体地，基于第一方向坐标，确定机器人与目标点之间的第一运行距离；根据第一运行距离在预设数据表中查找，确定第一运行距离对应的第一速度信息，如图4。其中，线速度与第一运行距离成比例关系。
将约束系数与第一速度信息的乘积作为线速度。
S42：基于机器人的第二方向坐标、机器人的更新方向信息与目标点的更新方向信息之间的偏差值，确定角速度。
请参阅图7，图7是图3提供的路径跟踪控制方法中步骤S42一具体实施例的流程示意图。
S421：响应于第二方向坐标的绝对值不大于第三阈值，则将机器人的第二方向坐标、机器人的更新方向信息与目标点的更新方向信息之间的偏差值进行归一化并加权融合得到角速度。
其中，第三阈值小于第一阈值。
在一具体实施例中，将机器人的第二方向坐标与第一加权系数相乘，得到第一角速度；将偏差值与第二加权系数相乘，得到第二角速度。基于第一角速度和第二角速度的加和得到角速度。
响应于角速度为正数，则确定机器人的转动方向为顺时针方向；响应于角速度为负数，则确定机器人的转动方向为逆时针方向。
S422：响应于第二方向坐标的绝对值大于第三阈值，则基于第二方向坐标确定角速度。
具体地，将机器人的第二方向坐标与第一加权系数相乘，得到角速度。
基于上述的角速度和线速度控制机器人从起点运行至目标点。
本申请中基于机器人的当前位姿进行运行速度调节，可以对控制频率的扰动有更强的鲁棒性。将待跟踪路径的全局坐标位置转换为局部坐标位置，更加适合非完整约束的机器人进行速度控制调整。进一步通过使用查表插值法生成机器人的期望线速度，以及通过角度差值调整角速度，保证机器人按照合理的最大速移动到目标点。整个路径跟踪算法在保证跟踪精度的前提下，做到了以最大速度进行前进。
请参阅图8，图8是本发明提供的路径跟踪控制装置一实施例的框架示意图。
本实施例提供一种路径跟踪控制装置60，路径跟踪控制装置60包括获取模块61、确定模块62、处理模块63和跟踪模块64。
获取模块61用于获取机器人的当前位姿和待跟踪路径；待跟踪路径包括n个顺次连接的路径点信息；n取正整数。
确定模块62用于基于当前位姿和待跟踪路径中各路径点信息，确定起点和目标点；目标点和起点之间的路径点的方向信息与起点的方向信息一致。
处理模块63用于基于起点和目标点构建局部坐标系，并确定起点、目标点和机器人在局部坐标系中的更新位姿信息。
跟踪模块64用于基于起点、目标点和机器人在局部坐标系中的更新位姿信息，确定运行信息并控制机器人进行路径跟踪。
本实施例提供的路径跟踪控制装置，通过实时获取机器人的当前位姿，以便于根据机器人的当前位姿以及待跟踪路径中各路径点的位姿信息确定当前跟踪的起点和目标点，基于起点、目标点和机器人在局部坐标系中的更新位姿信息可以得到准确的运行信息，进行控制机器人以合理的最大速度从起点运行至目标点，节约跟踪时间，使得机器人可以实现及时准确的跟踪。
请参阅图9，图9是本发明提供的终端一实施例的框架示意图。
终端80包括相互耦接的存储器81和处理器82，处理器82用于执行存储器81中存储的程序指令，以实现上述任一路径跟踪控制方法实施例的步骤。在一个具体的实施场景中，终端80可以包括但不限于：微型计算机、服务器，此外，终端80还可以包括笔记本电脑、平板电脑等移动设备，在此不做限定。
具体而言，处理器82用于控制其自身以及存储器81以实现上述任一路径跟踪控制方法实施例的步骤。处理器82还可以称为CPU（Central Processing Unit，中央处理单元）。处理器82可能是一种集成电路芯片，具有信号的处理能力。处理器82还可以是通用处理器、数字信号处理器（Digital Signal Processor，DSP）、专用集成电路（ApplicationSpecific Integrated Circuit, ASIC）、现场可编程门阵列（Field-Programmable GateArray，FPGA）或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。另外，处理器82可以由集成电路芯片共同实现。
请参阅图10，图10是本发明提供的计算机可读存储介质一实施例的框架示意图。
计算机可读存储介质90存储有能够被处理器运行的程序指令901，程序指令901用于实现上述任一路径跟踪控制方法实施例的步骤。
在一些实施例中，本公开实施例提供的装置具有的功能或包含的模块可以用于执行上文方法实施例描述的方法，其具体实现可以参照上文方法实施例的描述，为了简洁，这里不再赘述。
上文对各个实施例的描述倾向于强调各个实施例之间的不同之处，其相同或相似之处可以互相参考，为了简洁，本文不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性、机械或其它的形式。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）或处理器（processor）执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
以上仅为本发明的实施方式，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
Claims (16)
1.一种路径跟踪控制方法，其特征在于，所述路径跟踪控制方法包括：
获取机器人的当前位姿和待跟踪路径；所述待跟踪路径包括n个顺次连接的路径点信息；n取正整数；
基于所述当前位姿和所述待跟踪路径中各所述路径点信息，确定起点和目标点；所述目标点和所述起点之间的路径点的方向信息与所述起点的方向信息一致；
基于所述起点和所述目标点构建局部坐标系，并确定所述起点、所述目标点和所述机器人在所述局部坐标系中的更新位姿信息；
基于所述起点、所述目标点和所述机器人在所述局部坐标系中的所述更新位姿信息，确定运行信息并控制所述机器人进行路径跟踪。
2.根据权利要求1所述的路径跟踪控制方法，其特征在于，所述路径点信息包括所述路径点对应的标志信息、编号和位姿信息；所述标志信息包括第一标识符和第二标识符；所述第一标识符表示所述机器人未经过所述路径点；所述第二标识符表示所述机器人已经过所述路径点；所述位姿信息包括位置信息和所述方向信息；
所述基于所述当前位姿和所述待跟踪路径中各所述路径点信息，确定起点和目标点，包括：
基于所述第一标识符对应的所述路径点中具有最小编号的所述路径点，确定所述起点；
基于所述起点的方向信息与所述第一标识符对应的其他所述路径点的方向信息，确定所述目标点；其中，第i个所述路径点的方向信息为第i个所述路径点指向第i+1个所述路径点的方向；第n个所述路径点的方向信息与第n-1个所述路径点的方向信息一致；0＜i＜n。
3.根据权利要求2所述的路径跟踪控制方法，其特征在于，所述当前位姿包括当前位置和当前方向；
所述基于所述第一标识符对应的所述路径点中具有最小编号的所述路径点，确定所述起点，包括：
响应于所述待跟踪路径中各所述路径点的标志信息均为所述第一标识符，则将所述机器人的当前位置作为所述起点；所述起点的当前方向为所述起点指向最小编号对应的所述路径点的方向；
响应于所述第一标识符对应的所述路径点中最小编号为m，且0＜m＜n，则将所述编号为m-1对应的所述路径点作为所述起点。
4.根据权利要求2所述的路径跟踪控制方法，其特征在于，
所述基于所述起点的方向信息与所述第一标识符对应的其他所述路径点的方向信息，确定所述目标点，包括：
将所述起点的方向信息与所述第一标识符对应的其他所述路径点的方向信息依次进行比较；
选取所述第一标识符对应的所述路径点中第一个与所述起点的方向信息不一致的所述路径点作为所述目标点；所述目标点的编号大于所述起点的编号。
5.根据权利要求1~4任一项所述的路径跟踪控制方法，其特征在于，
所述基于所述起点和所述目标点构建局部坐标系，并确定所述起点、所述目标点和所述机器人在所述局部坐标系中的更新位姿信息，包括：
以所述目标点作为所述局部坐标系的原点，以所述起点指向所述目标点的方向作为所述局部坐标系中第一坐标轴的正方向；
将所述起点和所述机器人分别投影至所述局部坐标系，得到所述起点和所述机器人分别对应的所述更新位姿信息。
6.根据权利要求5所述的路径跟踪控制方法，其特征在于，所述运行信息包括角速度和线速度；
所述基于所述起点、所述目标点和所述机器人在所述局部坐标系中的所述更新位姿信息，确定运行信息并控制所述机器人进行路径跟踪，包括：
基于所述机器人在所述局部坐标系的更新位姿信息，确定所述机器人与所述第一坐标轴之间的第一最短距离以及与第二坐标轴之间的第二最短距离；所述第二坐标轴的正方向为所述第一坐标轴的正方向逆时针旋转90°所指的方向；
响应于所述第一最短距离小于第一阈值，则将所述第二最短距离与第二阈值进行比较；
响应于所述第二最短距离不小于所述第二阈值，则基于所述机器人在所述局部坐标系的更新位姿信息确定所述角速度和所述线速度；
控制所述机器人基于所述角速度和所述线速度进行路径跟踪。
7.根据权利要求6所述的路径跟踪控制方法，其特征在于，
所述基于所述起点、所述目标点和所述机器人在所述局部坐标系中的所述更新位姿信息，确定运行信息并控制所述机器人进行路径跟踪，还包括：
响应于所述第一最短距离小于所述第一阈值，且所述第二最短距离小于所述第二阈值，则确定所述机器人已运行至所述目标点；
将所述起点至所述目标点中各所述路径点的标志位赋值为第二标识符。
8.根据权利要求6所述的路径跟踪控制方法，其特征在于，
所述基于所述起点、所述目标点和所述机器人在所述局部坐标系中的所述更新位姿信息，确定运行信息并控制所述机器人进行路径跟踪，还包括：
响应于所述第一最短距离不小于所述第一阈值，则基于所述机器人的所在位置和所述机器人在所述局部坐标系的第一坐标轴上的投影位置构建更新局部坐标系；所述机器人在所述局部坐标系的第一坐标轴上的投影位置作为所述更新局部坐标系的原点，所述机器人的所在位置指向所述机器人在所述更新局部坐标系的第一坐标轴上的投影位置的方向作为所述更新局部坐标系的第一坐标轴的正方向；
将所述机器人的更新位姿信息投影至所述更新局部坐标系，则确定所述机器人在所述更新局部坐标系的更新位姿信息；
基于所述机器人在所述更新局部坐标系的更新位姿信息确定所述角速度和所述线速度；
控制所述机器人基于所述角速度和所述线速度进行路径跟踪。
9.根据权利要求6或8所述的路径跟踪控制方法，其特征在于，所述更新位姿信息包括更新位置信息和更新方向信息；所述更新位置信息包括第一方向坐标和第二方向坐标；所述第一方向坐标为所述更新位置信息在所述第一坐标轴上的分量，所述第二方向坐标为所述更新位置信息在所述第二坐标轴上的分量；
所述确定所述角速度和所述线速度的步骤，包括：
基于所述机器人的第一方向坐标、所述机器人的更新方向信息以及所述目标点的更新方向信息，确定所述线速度；
基于所述机器人的第二方向坐标、所述机器人的更新方向信息与所述目标点的更新方向信息之间的偏差值，确定所述角速度。
10.根据权利要求9所述的路径跟踪控制方法，其特征在于，
所述基于所述机器人的第一方向坐标、所述机器人的更新方向信息以及所述目标点的更新方向信息，确定所述线速度，包括：
基于所述机器人的更新方向信息与所述目标点的更新方向信息，确定约束系数；
基于所述第一方向坐标以及所述约束系数，确定所述线速度。
11.根据权利要求10所述的路径跟踪控制方法，其特征在于，
所述基于所述机器人的更新方向信息与所述目标点的更新方向信息，确定约束系数，包括：
基于所述机器人的更新方向信息与所述目标点的更新方向信息之间的差值，确定角度差值；
响应于所述角度差值满足预设条件，则将第一系数作为所述约束系数；所述第一系数的取值范围为1；
响应于所述角度差值不满足所述预设条件，则将第二系数作为所述约束系数；所述第二系数的取值范围为大于等于0且小于1。
12.根据权利要求10所述的路径跟踪控制方法，其特征在于，
所述基于所述第一方向坐标以及所述约束系数，确定所述线速度，包括：
基于所述第一方向坐标，确定所述机器人与所述目标点之间的第一运行距离；
根据所述第一运行距离在预设数据表中查找，确定所述第一运行距离对应的第一速度信息；
将所述约束系数与所述第一速度信息的乘积作为所述线速度。
13.根据权利要求9所述的路径跟踪控制方法，其特征在于，
所述基于所述机器人的第二方向坐标、所述机器人的更新方向信息与所述目标点的更新方向信息之间的偏差值，确定所述角速度，包括：
响应于所述第二方向坐标的绝对值不大于第三阈值，则将所述机器人的第二方向坐标、所述机器人的更新方向信息与所述目标点的更新方向信息之间的偏差值进行归一化并加权融合得到所述角速度；所述第三阈值小于所述第一阈值；
响应于所述第二方向坐标的绝对值大于所述第三阈值，则基于所述第二方向坐标确定所述角速度。
14.一种路径跟踪控制装置，其特征在于，所述路径跟踪控制装置包括：
获取模块，用于获取机器人的当前位姿和待跟踪路径；所述待跟踪路径包括n个顺次连接的路径点信息；n取正整数；
确定模块，用于基于所述当前位姿和所述待跟踪路径中各所述路径点信息，确定起点和目标点；所述目标点和所述起点之间的路径点的方向信息与所述起点的方向信息一致；
处理模块，用于基于所述起点和所述目标点构建局部坐标系，并确定所述起点、所述目标点和所述机器人在所述局部坐标系中的更新位姿信息；
跟踪模块，用于基于所述起点、所述目标点和所述机器人在所述局部坐标系中的所述更新位姿信息，确定运行信息并控制所述机器人进行路径跟踪。
15.一种终端，其特征在于，所述终端包括存储器、处理器以及存储于所述存储器中并在所述处理器上运行的计算机程序，所述处理器用于执行程序数据以实现如权利要求1~13任一项所述的路径跟踪控制方法中的步骤。
16.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1~13任一项所述的路径跟踪控制方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410005646.2A CN117519212B (zh) | 2024-01-03 | 2024-01-03 | 路径跟踪控制方法、装置、终端及计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410005646.2A CN117519212B (zh) | 2024-01-03 | 2024-01-03 | 路径跟踪控制方法、装置、终端及计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117519212A true CN117519212A (zh) | 2024-02-06 |
| CN117519212B CN117519212B (zh) | 2024-04-12 |
Family
ID=89766774
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410005646.2A Active CN117519212B (zh) | 2024-01-03 | 2024-01-03 | 路径跟踪控制方法、装置、终端及计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117519212B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180281191A1 (en) * | 2017-03-30 | 2018-10-04 | Brain Corporation | Systems and methods for robotic path planning |
| CN110955245A (zh) * | 2019-12-09 | 2020-04-03 | 深圳市银星智能科技股份有限公司 | 路径追踪方法、装置、移动机器人及非暂态计算机可读存储介质 |
| US20200133289A1 (en) * | 2018-10-31 | 2020-04-30 | Ubtech Robotics Corp Ltd | Path tracking method and mobile robot using the same |
| CN111805535A (zh) * | 2020-06-11 | 2020-10-23 | 浙江大华技术股份有限公司 | 一种定位导航方法、装置以及计算机存储介质 |
| CN112783144A (zh) * | 2019-10-22 | 2021-05-11 | 舜宇光学（浙江）研究院有限公司 | 路径生成方法和路径规划方法及其系统和设备 |
| CN114756021A (zh) * | 2022-03-01 | 2022-07-15 | 深圳市正浩创新科技股份有限公司 | 路径跟踪方法、装置与路径跟踪设备 |
| CN116184654A (zh) * | 2022-12-27 | 2023-05-30 | 同济大学 | 坐标系构建方法、棱镜扫描器多目标探测扫描方法及系统 |
| CN116466589A (zh) * | 2023-04-26 | 2023-07-21 | 长春理工大学 | 一种基于自抗扰模型的路径跟踪控制方法、装置、存储介质及电子设备 |
| CN116481538A (zh) * | 2023-04-06 | 2023-07-25 | 杭州华橙软件技术有限公司 | 一种轨迹优化方法、装置、设备和存储介质 |
| CN116520847A (zh) * | 2023-05-19 | 2023-08-01 | 杭州萤石软件有限公司 | 路径规划方法、装置、移动机器人、电子设备及存储介质 |
-
2024
- 2024-01-03 CN CN202410005646.2A patent/CN117519212B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180281191A1 (en) * | 2017-03-30 | 2018-10-04 | Brain Corporation | Systems and methods for robotic path planning |
| US20200133289A1 (en) * | 2018-10-31 | 2020-04-30 | Ubtech Robotics Corp Ltd | Path tracking method and mobile robot using the same |
| CN112783144A (zh) * | 2019-10-22 | 2021-05-11 | 舜宇光学（浙江）研究院有限公司 | 路径生成方法和路径规划方法及其系统和设备 |
| CN110955245A (zh) * | 2019-12-09 | 2020-04-03 | 深圳市银星智能科技股份有限公司 | 路径追踪方法、装置、移动机器人及非暂态计算机可读存储介质 |
| CN111805535A (zh) * | 2020-06-11 | 2020-10-23 | 浙江大华技术股份有限公司 | 一种定位导航方法、装置以及计算机存储介质 |
| CN114756021A (zh) * | 2022-03-01 | 2022-07-15 | 深圳市正浩创新科技股份有限公司 | 路径跟踪方法、装置与路径跟踪设备 |
| CN116184654A (zh) * | 2022-12-27 | 2023-05-30 | 同济大学 | 坐标系构建方法、棱镜扫描器多目标探测扫描方法及系统 |
| CN116481538A (zh) * | 2023-04-06 | 2023-07-25 | 杭州华橙软件技术有限公司 | 一种轨迹优化方法、装置、设备和存储介质 |
| CN116466589A (zh) * | 2023-04-26 | 2023-07-21 | 长春理工大学 | 一种基于自抗扰模型的路径跟踪控制方法、装置、存储介质及电子设备 |
| CN116520847A (zh) * | 2023-05-19 | 2023-08-01 | 杭州萤石软件有限公司 | 路径规划方法、装置、移动机器人、电子设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117519212B (zh) | 2024-04-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Kwok et al. | Map-based multiple model tracking of a moving object | |
| WO2023087681A1 (zh) | 定位初始化方法、装置、计算机可读存储介质和计算机程序产品 | |
| CA3086559C (en) | Method for predicting a motion of an object, method for calibrating a motion model, method for deriving a predefined quantity and method for generating a virtual reality view | |
| Bodhale et al. | Path planning for a mobile robot in a dynamic environment | |
| CN110702093B (zh) | 基于粒子滤波的定位方法、装置、存储介质及机器人 | |
| CN117519212B (zh) | 路径跟踪控制方法、装置、终端及计算机可读存储介质 | |
| Yuen et al. | A comparison between extended Kalman filtering and sequential Monte Carlo techniques for simultaneous localisation and map-building | |
| CN117870702A (zh) | 基于机器人的地图构建方法、设备以及存储介质 | |
| Tiar et al. | ICP-SLAM methods implementation on a bi-steerable mobile robot | |
| CN112729349B (zh) | 里程计在线标定的方法、装置、电子设备及存储介质 | |
| CN112904855B (zh) | 基于改进动态窗口的跟随机器人局部路径规划方法 | |
| Ekmanis | Self-calibration in differential drive dynamics/kinematics model | |
| CN111708283B (zh) | 一种机器人仿真方法、设备以及计算机可读存储介质 | |
| Scherer et al. | DCTAM: Drift-corrected tracking and mapping for autonomous micro aerial vehicles | |
| KR20220037212A (ko) | 강인한 스테레오 비주얼 관성 항법 장치 및 방법 | |
| Tungadi et al. | Multiple laser polar scan matching with application to SLAM | |
| CN112518744A (zh) | 机器人轨迹规划方法、装置、机器人 | |
| Quack et al. | Infrastructure-based tracking of road users in urban intersections for partially available, variable-delay sensor data | |
| CN115267725B (zh) | 一种基于单线激光雷达的建图方法及装置、存储介质 | |
| Farrokhsiar et al. | A higher order Rao-Blackwellized particle filter for monocular vSLAM | |
| Carvalho et al. | Particle swarm localization for mobile robots using a 2D laser sensor | |
| Zhang et al. | Research on dynamic obstacle avoidance method of manipulator based on binocular vision | |
| CN117348577B (zh) | 一种生产工艺仿真检测方法、装置、设备以及介质 | |
| Uppunda et al. | Autonomous Vehicle Navigation with LIDAR using Path Planning | |
| Ginsberg | Optimizing multi-robot localization with Extended Kalman Filter feedback and collaborative laser scan matching |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |