轨迹跟踪控制方法、移动机器人、控制设备及存储介质

摘要

本发明提供一种轨迹跟踪控制方法、移动机器人、控制设备及存储介质，该轨迹跟踪控制方法包括：获取预设轨迹和运动目标的当前位置；在预设轨迹上确定基于当前位置出发的目标位置；根据预设轨迹在目标位置的曲率，确定目标位置对应的参考速度；根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数。根据预设轨迹在目标位置的曲率确定目标位置对应的参考速度；通过参考速度和运动目标的性能参数预估运动目标在目标位置的运动参数。通过上述方式，能够使得运动目标经过大曲率路径的跟踪更加精确。

说明书

技术领域

本发明涉及移动目标的控制技术领域，特别是涉及一种轨迹跟踪控制方法、移动机器人、控制设备及存储介质。

背景技术

在移动机器人的应用中，轨迹跟踪是其应用的关键技术，机器人在已知定位的情况下，如何快速，精确的跟踪目标轨迹是轨迹跟踪应用的核心。无论是在服务机器人领域跟踪一条实时变化的路径，还是在工业机器人领域，跟踪一条既定的路径，轨迹跟踪在其中都起到了至关重要的作用。因此，可以应用在巡检机器人、搬运AGV（Automated GuidedVehicle，自动搬运车）、智能叉车等等。

目前，移动机器人的控制大都与路径规划相关，可是现有的路径规划算法无法预观到轨迹后面的信息，造成移动机器人的跟踪精度不佳。使得移动机器人在一些曲率较大的路径点附近，往往会将原本应该到达的路径点略过，而且在这些路径点附近也会偶发运动目标脱轨无法恢复的情况。

发明内容

本发明主要解决的技术问题是提供一种轨迹跟踪控制方法、移动机器人、控制设备及存储介质，解决现有技术中轨迹跟踪精度不佳的问题。

为解决上述技术问题，本发明采用的第一个技术方案是：提供一种轨迹跟踪控制方法，该轨迹跟踪控制方法包括：获取预设轨迹和运动目标的当前位置；在预设轨迹上确定基于当前位置出发的目标位置；根据预设轨迹在目标位置的曲率，确定目标位置对应的参考速度；根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数。

其中，在预设轨迹上确定基于当前位置出发的目标位置的步骤包括：对预设轨迹进行采样，以得到多个采样点；计算多个采样点与当前位置之间的运行距离；将运行距离最短者对应的采样点，确定为目标位置。

其中，在预设轨迹上确定基于当前位置出发的目标位置的步骤包括：对预设轨迹进行采样，以得到多个采样点；计算运动目标从当前位置到达多个采样点的运行时间；将运行时间最短者对应的采样点，确定为目标位置。

其中，根据预设轨迹在目标位置的曲率，确定目标位置对应的参考速度的步骤包括：根据目标位置以及与目标位置相邻的后一采样点，计算目标位置的路径曲率；根据路径曲率，得到目标位置的参考速度。

其中，运动目标的性能参数包括运动目标的最大运动参数；根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数的步骤包括：根据运动目标的当前位姿信息，预测运动目标在目标位置的运动参数；判断预测的运动参数是否超过运动目标的最大运动参数；若是，则以运动目标的最大运动参数作为运动目标在目标位置的运动参数。

其中，运动目标的运动参数包括运行线速度和运行角速度；运动目标的最大运动参数包括运动目标运行的最大线速度和最大角速度；判断预测的运动参数是否超过运动目标的最大运动参数的步骤包括：判断运动目标的运行线速度是否超过最大线速度，以及判断运动目标的运行角速度是否超过最大角速度；如果预测的运动参数超过运动目标的最大运动参数，则以运动目标的最大运动参数作为运动目标的运动参数的步骤包括：如果运动目标的运行线速度超过最大线速度，则将最大线速度作为运动目标的运行线速度，以及如果运动目标的运行角速度超过最大角速度，则将最大角速度作为运动目标的运行角速度。

其中，运动参数还包括运行的线性加速度和角加速度；最大运动参数还包括运动目标的最大线性加速度和最大角加速度；判断预测的运动参数是否超过运动目标的最大运动参数的步骤还包括：判断运行的线性加速度是否超过最大线性加速度，以及判断运行的角加速度是否超过最大角加速度；如果预测的运动参数超过运动目标的最大运动参数，则以运动目标的最大运动参数作为运动目标的运动参数的步骤还包括：如果运动目标的线性加速度超过最大线性加速度，则将最大线性加速度作为运动目标的线性加速度，以及如果运动目标的角加速度超过最大角加速度，则将最大角加速度作为运动目标的角加速度。

其中，运动参数至少包括运行速度；根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数的步骤还包括：判断运动目标在目标位置的运行速度是否超过目标位置对应的参考速度；若是，则调节参考速度作为运动目标在目标位置的运行速度。

其中，根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数的步骤还包括：当运动目标的运行角速度达到运动目标的最大角速度时，判断运动目标的运行线速度是否超过预设线速度；如果是，则将预设线速度作为运动目标的运行线速度；或当运动目标的运行线速度达到运动目标的最大线速度时，判断运动目标的运行角速度是否超过预设角速度；如果是，则将预设角速度作为运动目标的运行角速度。

其中，还包括：采用投影的方式判断运动目标到最后一个目标位置的距离是否超过预设值；如果运动目标到最后一个目标位置的距离未超过预设值，则确定运动目标到达预设轨迹中最后一个目标位置。

为解决上述技术问题，本发明采用的第二个技术方案是：提供一种移动机器人，该移动机器人包括存储器、处理器以及存储于存储器中并在处理器上运行的计算机程序，处理器用于实现上述轨迹跟踪控制方法中的步骤。

为解决上述技术问题，本发明采用的第三个技术方案是：提供一种机器人控制设备，该控制设备包括处理器以及与处理器连接的存储器和通信模组，通信模组用于与机器人进行数据交互，存储器用于存储程序数据，处理器用于执行程序数据以用于实现上述轨迹跟踪控制方法中的步骤。

为解决上述技术问题，本发明采用的第四个技术方案是：提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现上述轨迹跟踪控制方法中的步骤。

本发明的有益效果是：区别于现有技术的情况，提供的一种轨迹跟踪控制方法、移动机器人、控制设备及存储介质，本申请中的轨迹跟踪控制方法通过获取预设轨迹和当前位置确定运动目标出发的目标位置；根据预设轨迹在目标位置的曲率确定目标位置对应的参考速度；通过参考速度和运动目标的性能参数预估运动目标在目标位置的运动参数。本申请中运动目标在目标位置的运动参数受到通过曲率得到的参考速度以及运动目标自身性能参数的约束，相对于现有方法可以满足跟踪路径曲率过大的场景使用，使得运动目标经过大曲率路径的跟踪更加精确。

附图说明

图1是本发明提供的轨迹跟踪控制方法一实施例的流程示意图；

图2是图1提供的轨迹跟踪控制方法中步骤S2一具体实施例的流程示意图；

图3是图1提供的轨迹跟踪控制方法中步骤S2另一具体实施例的流程示意图；

图4是图1提供的轨迹跟踪控制方法中步骤S3一具体实施例的流程示意图；

图5是图1提供的轨迹跟踪控制方法中步骤S4一具体实施例的流程示意图；

图6是本发明提供的轨迹跟踪控制方法中运行线速度与曲率的关系图；

图7是本发明提供的实施事例一场景示意图；

图8是本发明提供的实施事例另一场景示意图；

图9是本发明提供的移动机器人一实施方式的示意框图；

图10是本发明提供的移动机器人控制设备一实施方式的示意框图；

图11是本发明提供的计算机可读存储介质一实施方式的示意框图。

具体实施方式

下面结合说明书附图，对本申请实施例的方案进行详细说明。

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、接口、技术之类的具体细节，以便透彻理解本申请。

本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。此外，本文中的“多”表示两个或者多于两个。

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本发明所提供的一种目标跟踪方法做进一步详细描述。

请参阅图1，图1是本发明提供的轨迹跟踪控制方法一实施例的流程示意图。本实施例中提供一种轨迹跟踪控制方法，该轨迹跟踪控制方法包括如下步骤。

S1：获取预设轨迹和运动目标的当前位置。

本实施例中，预设轨迹为运动目标行走所要跟踪的路径。例如，预设轨迹可以为制定的路径或经过路径规划得到的路径等，在此不做限制。可以获取预设轨迹的路径信息，例如，可以获取预设轨迹在指定坐标系中的路径信息。其中，坐标系可以是世界坐标系。

运动目标的当前位置为运动目标当前所在的位置。可以获取运动目标当前位置的坐标信息，例如，可以对运动目标进行定位，获取运动目标的当前位置在指定坐标系中的位置坐标。其中，运动目标的当前位置可能在预设轨迹上，也可以不在预设轨迹上。其中，本实施例中的运动目标可以为移动机器人。

在一可选实施例中，可以按照预设时间间隔、预设行走路程间隔或其它预设方式在运动目标行走过程中获取运动目标的所在位置，在此不作限定。

S2：在预设轨迹上确定基于当前位置出发的目标位置。

具体地，在运动目标行走过程中，可以在预设轨迹上选取一个在运动目标所在位置前方的采样点作为运动目标出发的目标位置。

在一可选实施例中，请参阅图2，图2是图1提供的轨迹跟踪控制方法中步骤S2一具体实施例的流程示意图。根据运动目标的当前位置与预设轨迹之间的距离确定运动目标出发的目标位置，具体包括如下子步骤。

S211：对预设轨迹进行采样，以得到多个采样点。

具体地，在运动目标行走过程中，对预设轨迹进行采样处理，以在预设轨迹上选取若干个采样点。在一可选实施例中，各采样点之间的距离可以相等，也可以是到达各采样点之间的时间相等。例如，各采样点组成的集合R={p

S212：计算多个采样点与当前位置之间的运行距离。

具体地，获取集合R中各采样点与运动目标在当前位置之间的距离。将各采样点与运动目标在当前位置之间的距离依次相互比对。

S213：将运行距离最短者对应的采样点，确定为目标位置。

具体地，判断上述步骤S212得到的运动目标与预设轨迹上各采样点之间的运行距离中的最短者，将运行距离最短者对应的采样点确定为预设目标出发的目标位置。

在一具体实施例中，如果运动目标的当前位置与采样点p

在一可选实施例中，请参阅图3，图3是图1提供的轨迹跟踪控制方法中步骤S2另一具体实施例的流程示意图。根据运动目标的当前位置以及姿态与预设轨迹之间的运行时间确定运动目标出发的目标位置。具体包括如下子步骤。

S221：对预设轨迹进行采样，以得到多个采样点。

具体地，在运动目标行走过程中，对预设轨迹进行采样处理，以在预设轨迹上选取若干个采样点。在一可选实施例中，各采样点之间的距离可以相等，也可以是到达各采样点之间的时间相等。例如各采样点组成的集合R={p

S222：计算运动目标从当前位置到达多个采样点的运行时间。

具体地，获取运动目标的当前位置和当前姿态，确定运动目标处于当前位置和当前姿态到达预设轨迹上各采样点的运行时间。也就是说，结合运动目标的当前位置和相对于预设轨迹的所处角度判断当前位置到达预设轨迹上各采样点的运行时间。

S223：将运行时间最短者对应的采样点，确定为目标位置。

在本实施例中，判断上述步骤S222得到的运动目标到达预设轨迹上各采样点之间的运行时间中的最短者，将运行时间最短者对应的采样点确定为预设目标出发的目标位置。

在一具体实施例中，如果运动目标的所处当前位置的角度使得运动目标到达采样点p

S3：根据预设轨迹在目标位置的曲率，确定目标位置对应的参考速度。

具体地，请参阅图4，图4是图1提供的轨迹跟踪控制方法中步骤S3一具体实施例的流程示意图。根据预设轨迹在目标位置的曲率，确定目标位置对应的参考速度具体包括如下子步骤。

S31：根据目标位置以及与目标位置相邻的后一采样点，计算目标位置的路径曲率。

具体地，将上述步骤S213或步骤S223中得到的采样点p

式中：x

S32：根据路径曲率，得到目标位置的参考速度。

在一可选实施例中，获取运动目标的性能参数。运动目标的性能参数包括运动目标的最大侧向加速度a

其中，目标位置的参考速度的计算公式如下公式2所示。

式中：v

如果运动目标在该采样点时的运行速度超过该最大可行速度，则运动目标在该采样点的运行会出现脱轨，会降低运动目标的轨迹跟踪精确度。

S4：根据参考速度和运动目标的性能参数，预估运动目标在目标位置的运动参数。

请参阅图5，图5是图1提供的轨迹跟踪控制方法中步骤S4一具体实施例的流程示意图。具体地，运动目标的性能参数包括运动目标的最大运动参数。其中，运动目标的最大运动参数包括运动目标运行的最大线速度和最大角速度。最大运动参数还包括运动目标的最大线性加速度和最大角加速度。

预估的运动参数至少包括运行速度。其中，运行速度包括运行线速度和运行角速度。在一可选实施例中，运动参数还包括运行的线性加速度和角加速度。

S41：根据运动目标的当前位姿信息，预测运动目标在目标位置的运动参数。

在本实施例中，获取运动目标的当前状态作为初始状态x

其中，所涉及的目标函数针对运动目标的性能指标J(x

J(u)=(x

式中：Q是半正定矩阵，R是正定矩阵，u=(u

目标函数针对运动目标的性能指标对运动参数进行优化的过程中，需要对运动参数进行相应的约束，以提高运动参数的精确度，进而提升运动目标的轨迹跟踪精确度。

在一可选实施例中，由于运动目标常规状态下需要至少两个轮子进行移动，因此需要对运动参数进行差动轮运动学约束，使得运动目标的左右轮子能够以适当的形式关联起来。

其中，差动轮运动学约束的公式如下公式4和公式5所示。

式中，x

S42：判断预测的运动参数是否超过运动目标的最大运动参数。

具体地，判断运动目标的运行线速度是否超过最大线速度，以及判断运动目标的运行角速度是否超过最大角速度。在另一可选实施例中，判断运行的线性加速度是否超过最大线性加速度，以及判断运行的角加速度是否超过最大角加速度，进而平滑运动参数，避免运动参数出现加速过量或减速过量的现象，影响运动目标的轨迹跟踪精确度。

S43：以运动目标的最大运动参数作为运动目标在目标位置的运动参数。

具体地，如果预测的运动参数超过运动目标的最大运动参数，则以运动目标的最大运动参数作为运动目标在目标位置的运动参数。

在一可选实施例中，如果运动目标的运行线速度超过最大线速度，则将最大线速度作为运动目标的运行线速度，以及如果运动目标的运行角速度超过最大角速度，则将最大角速度作为运动目标的运行角速度。如果运动目标的线性加速度超过最大线性加速度，则将最大线性加速度作为运动目标的线性加速度，以及如果运动目标的角加速度超过最大角加速度，则将最大角加速度作为运动目标的角加速度，进而约束运动目标的角加速度，提升运动目标的轨迹跟踪平稳性。

S44：以预测的运动参数作为运动目标在目标位置的运动参数。

具体地，如果预测的运动参数未超过运动目标的最大运动参数，则以预测的运动参数作为运动目标在目标位置的运动参数。

S45：判断运动目标在目标位置的运行速度是否超过目标位置对应的参考速度。

具体地，判断上述步骤S43或步骤S44得到的目标位置的运动参数中的运行速度是否超过目标位置对应的参考速度，进而进一步约束目标位置的运行速度。

S46：调节参考速度作为运动目标在目标位置的运行速度。

具体地，当运动目标在目标位置的运行速度超过目标位置对应的参考速度，则调节参考速度作为运动目标在目标位置的运行速度。

S47：以运动参数中的运行速度作为运动目标在目标位置的运行速度。

具体地，当运动目标在目标位置的运行速度未超过目标位置对应的参考速度，则以运动参数中的运行速度作为运动目标在目标位置的运行速度。

在本实施例中，还通过目标位置的线速度和角速度之间的联合关系对参考速度进行约束。

在一可选实施例中，当运动目标的运行角速度达到运动目标的最大角速度时，判断运动目标的运行线速度是否超过预设线速度；如果运动目标的运行线速度超过预设线速度，则将预设线速度作为运动目标的运行线速度。如果运动目标的运行线速度未超过预设线速度，则将预测的运动目标的运行线速度作为运动目标的运行线速度。

在另一可选实施例中，当运动目标的运行线速度达到运动目标的最大线速度时，判断运动目标的运行角速度是否超过预设角速度。如果运动目标的运行角速度超过预设角速度，则将预设角速度作为运动目标的运行角速度。如果运动目标的运行角速度未超过预设角速度，则将运动目标的运行角速度作为运动目标的运行角速度。

其中，根据运动目标处于x

由上面的公式6和公式7增加速度与角速度的联合约束如公式8所示。

其中k为运动目标角速度与速度的比值，为该点的曲率值，如公式9所示：

式中：w

其中，常数项β＞0和λ＞0是需要调节的参数，不同的参数对应的曲线趋势不同。请参阅图6，图6是本发明提供的轨迹跟踪控制方法中运行线速度与曲率的关系图。当目标位置的曲率值越大时，目标位置对应的运动线速度越小。

当通过上述步骤得出的目标位置的运行参数符合上述的条件，则将该运行参数作为运动目标在目标位置的运行参数。

在另一可选实施例中，当判断运动目标是否到达预设轨迹的终点时，可以采用投影的方式判断运动目标到最后一个目标位置的距离是否超过预设值。如果运动目标到最后一个目标位置的距离未超过预设值，则确定运动目标到达预设轨迹中最后一个目标位置。

具体地，假设最后一个目标位置为pf，则当前运动目标的位姿（x，y，θ）到终点的判断条件如公式10所示。

dist=(pf

当运动目标的当前位姿超过终点时，则运动目标与终点之间的坐标距离为负数，则可以认为运动目标到达终点。采用投影距离的方式可以减少运动目标最后到点无意义的动作，并且在定位精度比较差的情况下可以确保一定可以到点。

在一具体实施例中，请参阅图7，图7是本发明提供的实施事例一场景示意图。运动目标可以为移动机器人，获取移动机器人的当前位置信息，即获取移动机器人的坐标位置O（x

请参阅图8，图8是本发明提供的实施事例另一场景示意图.移动机器人在跟踪过程中，判断移动机器人是否到达预设路径的终点。当移动机器人到达O'时，根据预设路径的终点p

本实施例提供的一种轨迹跟踪控制方法通过获取预设轨迹和当前位置确定运动目标出发的目标位置；根据预设轨迹在目标位置的曲率确定目标位置对应的参考速度；通过参考速度和运动目标的性能参数预估运动目标在目标位置的运动参数。本申请中运动目标在目标位置的运动参数受到通过曲率得到的参考速度以及运动目标自身性能的约束，相对于现有方法可以满足跟踪路径曲率过大的场景使用，使得运动目标经过大曲率路径的跟踪更加精确。

参阅图9，图9是本发明提供的移动机器人一实施方式的示意框图。如图7所示，该实施方式中的移动机器人70包括：处理器71、存储器72以及存储在存储器72中并可在处理器71上运行的计算机程序，该计算机程序被处理器71执行时实现上述实施例中轨迹跟踪控制方法，为避免重复，此处不一一赘述。

参阅图10，图10是本发明提供的移动机器人控制设备一实施方式的示意框图。如图10所示，该实施方式中的移动机器人控制设备80包括处理器81以及与处理器81连接的存储器82和通信模组83，通信模组83用于与移动机器人进行数据交互，存储器82用于存储程序数据，所述处理器81用于执行所述程序数据以实现上述实施例中轨迹跟踪控制方法，为避免重复，此处不一一赘述。

参阅图11，图11是本发明提供的计算机可读存储介质一实施方式的示意框图。

本申请的实施方式中还提供一种计算机可读存储介质90，计算机可读存储介质90存储有计算机程序901，计算机程序901中包括程序指令，处理器执行程序指令，实现上述实施例中轨迹跟踪控制方法。

其中，计算机可读存储介质90可以是前述实施方式的计算机设备的内部存储单元，例如计算机设备的硬盘或内存。计算机可读存储介质90也可以是计算机设备的外部存储设备，例如计算机设备上配备的插接式硬盘，智能存储卡(SmartMedia Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。

以上仅为本发明的实施方式，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。