一种机器人的运动控制方法及控制系统的制作方法
【专利摘要】本发明提供一种机器人的运动控制方法及控制系统，包括如下步骤：S20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在规划路径上，计算出机器人在规划路径上的目标位置信息；S30若机器人当前所处位置不在规划路径上，则根据实时位置信息和目标位置信息，使得机器人朝向目标位置；否则，直接跳转至步骤S40；S40根据实时位置信息和目标位置信息，计算出机器人的线速度、角速度和实时朝向后，控制机器人运动。本发明可通过计算规划路径上距离机器人最近的点作为目标位置，以及使得机器人朝向目标位置，不仅使得机器人在最短时间回到规划路径上，还能控制机器人按照任意规划路径快速、精准地行走。
【专利说明】
一种机器人的运动控制方法及控制系统
技术领域
[0001]本发明涉及机器人技术领域，尤指是机器人的运动控制方法及控制系统。
【背景技术】
[0002]随着科技不断进步，机器人在当今社会中扮演着重要角色，由于机器人在某些方面可以自主完成各种任务，例如清洁机器人、灭火机器人，从而达到节省人力资源、减小消防人员安危等目的。
[0003]其中，控制机器人按照规划路径行走是机器人自主完成各种任务的关键。目前，通常是采用非完整控制系统来控制机器人运动，其非完整控制系统是:通过某种控制输入的作用，使得机器人快速、精确地移动到运动空间的某一位置或规划路径上。
[0004]在现实中，由于系统物理特性的限制(例如机器人自身性能的限制)，或者安全因素(例如路况不平坦等安全因素)，实际系统的控制输入都会受到饱和约束。在非完整控制系统中利用两个输入量控制三个输出量，存在较大的难度;再考虑饱和约束，更难以控制机器人按照规划路径快速、精确地行走。
【发明内容】
[0005]本发明为了解决机器人快速、精确行走的问题，提供一种机器人的运动控制方法及控制装置。该控制方法及控制装置可控制机器人按照任意规划路径快速、精准地行走。
[0006]为了实现本发明以上发明目的，本发明是通过以下技术方案实现的:
[0007]本发明提供一种机器人的运动控制方法，包括如下步骤:S20根据输入的地图信息、规划路径及实时位置信息，判断所述机器人当前所处位置是否在所述规划路径上，计算出机器人在所述规划路径上的目标位置信息；S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置；否则，直接跳转至步骤S40; S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动。
[0008]进一步，所述步骤S30包括:S31根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上；S32获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角；S33判断所述当前夹角与所述目标夹角是否相同；S34若所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转后，跳转至步骤S33;否则，所述机器人保持当前朝向。
[0009]进一步，本机器人的运动控制方法，还包括如下步骤:S41根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息；S42根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值;S43判断所述当前曲率值与第一曲率阈值的大小;S44若所述当前曲率值大于所述第一曲率阈值，则进一步判断所述机器人的当前速度与第一速度阈值的大小；否则，跳转至步骤S46; S45若所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小后，跳转至步骤S44;否则，跳转至步骤S46; S46判断所述机器人是否到达所述目标位置;S47若所述机器人到达所述目标位置，则进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值；否贝1J，跳转至步骤S41;S48若所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，则所述机器人以预设角速度旋转后，跳转至步骤S47。
[0010]进一步，本机器人的运动控制方法，还包括如下步骤:S50判断所述目标位置是否为所述规划路径上的终点位置;S60若所述目标位置不是所述规划路径上的终点位置，则跳转至步骤S20;否则，所述机器人的当前速度以第二预设阈值进行减小。
[0011]进一步，本机器人的运动控制方法，还包括如下步骤:S70判断所述机器人是否到达所述终点位置;S80若所述机器人到达所述终点位置，则进一步判断所述机器人的当前朝向与所述终点位置的规划朝向是否相同；否则，跳转至步骤S60;S90若所述机器人的当前朝向与所述终点位置的规划朝向不同，所述机器人以预设角速度旋转后，跳转至步骤S80;否则，所述机器人保持当前朝向。
[0012]进一步，所述步骤S20中的地图呈栅格状，所述步骤S20中的目标位置信息是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点信息，或所述规划路径上距离所述机器人当前所处位置最近的点位置信息。
[0013]进一步，所述步骤S40是指:根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向。
[0014]本发明还提供一种机器人的运动控制系统，包括:判断模块，所述判断模块根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;计算模块，所述计算模块与所述判断模块电连接，用于计算出所述机器人在所述规划路径上的目标位置信息;朝向控制模块，所述朝向控制模块分别与所述判断模块、计算模块电连接，所述朝向控制模块接收所述判断模块输出的所述机器人当前所处位置不在所述规划路径上，所述朝向控制模块根据所述计算模块输出的所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;运动控制模块，所述运动控制模块分别与所述判断模块、朝向控制模块电连接，所述运动控制模块根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动。
[0015]进一步，本机器人的运动控制系统，还包括:所述计算模块还根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上;所述获取模块用于获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角；所述判断模块还分别与所述计算模块、获取模块电连接，所述判断模块接收所述计算模块输出的目标夹角，所述判断模块还接收所述获取模块输出的当前夹角，判断所述当前夹角与所述目标夹角是否相同；所述朝向控制模块与所述判断模块电连接，所述朝向控制模块接收所述判断模块输出的所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转；否则，所述机器人保持当前朝向。
[0016]进一步，本机器人的运动控制系统，还包括:所述计算模块还根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息;所述计算模块还根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值;所述判断模块接收所述计算模块输出的当前曲率值，判断所述当前曲率值与第一曲率阈值的大小;若所述当前曲率值大于所述第一曲率阈值，所述判断模块进一步判断所述机器人的当前速度与第一速度阈值的大小;速度控制模块，所述速度控制模块与所述判断模块电连接，所述速度控制模块接收所述判断模块输出的所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小;所述判断模块还判断所述机器人是否到达所述目标位置;若所述机器人到达所述目标位置，所述判断模块进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值;所述朝向控制模块接收所述判断模块输出的所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，所述机器人以预设角速度旋转。
[0017]本发明至少具有以下有益效果之一:
[0018]1、本发明中机器人在任意初始位置时，通过计算出规划路径上距离机器人最近的点作为目标位置，以及使得机器人朝向目标位置，可让机器人尽快移动到规划路径上，从而增大机器人的可移动范围。同时，机器人因不确定外界因素偏离路径后，可通过计算规划路径上距离机器人最近的点作为目标位置，以及使得机器人朝向目标位置，不仅使得机器人在最短时间回到规划路径上，还能控制机器人按照任意规划路径快速、精准地行走。
[0019]2、本发明中机器人由于一些不确定因素超过预设阈值后，可自动调节机器人的实时速度，不仅可以满足机器人的最大速度要求，还使得机器人结合实际路况，更为准确地按照规划路径行走，提高了机器人灵活性、工作效率。
[0020]3、本发明中还通过遍历机器人的目标位置前后一定范围路径的曲率，根据比较当前曲率与第一曲率阈值，来调整实时速度，使得机器人满足饱和输入条件下的路径跟踪控制，实现对任意路径的跟踪，降低了对路径规划的要求，极大的提高了路径跟踪的精确度，使机器人可在狭窄区域完成路径跟踪。
【附图说明】
[0021 ]下面结合附图和【具体实施方式】对本发明作进一步详细说明:
[0022]图1为一种机器人的运动控制方法的流程示意图；
[0023]图2为机器人的运动控制方法的一部分流程示意图；
[0024]图3为机器人的运动控制方法的另一部分流程示意图；
[0025]图4为另一种机器人的运动控制方法的流程示意图；
[0026]图5为一种机器人的运动控制系统的组成结构示意图；
[0027]图6为机器人的运动控制系统的一部分组成结构示意图；
[0028]图7为机器人的运动控制系统的另一部分组成结构示意图；
[0029]图8为再一种机器人的运动控制方法的流程示意图；
[0030]图9为机器人的运动控制方法的再一部分流程示意图；
[0031 ]图10为机器人的运动控制方法的又一部分流程示意图；
[0032]图11为机器人的运动控制方法的又又一部分流程示意图；
[0033]图12为机器人的运动控制方法的又又又一部分流程示意图；
[0034]图13a为机器人初始位置不在规划路径上的一种的运动示意图；
[0035]图13b为机器人初始位置不在规划路径上的另一种的运动示意图；
[0036]图14a为机器人偏离规划路径的一种的运动示意图；
[0037]图14b为机器人偏离规划路径的另一种的运动示意图；
[0038]图15a为机器人偏离规划路径后回到规划路径的一种的运动示意图；
[0039]图15b为机器人偏离规划路径后回到规划路径的另一种的运动示意图；
[0040]图16a为机器人转弯时的一种的运动不意图；
[0041]图16b为机器人转弯时的另一种的运动不意图；
[0042]图17a为规划路径上规划点的一种设置示意图；
[0043]图17b为规划路径上规划点的另一种设置示意图；
[0044]图中:
[0045]10-计算模块 n-获取模块20-判断模块
[0046]30-朝向控制模块40-运动控制模块 41-速度控制模块；
[0047]100-规划路径 100’、100”_实际行走路径
[0048]10、10 ’ -走到规划路径的实际行走路径
[0049]23、23’_偏离规划路径的实际行走路径
[0050]34、34’、35、35’_回归至规划路径的实际行走路径
[0051]101-初始位置 102、103、_规划路径上距离机器人最近的点[0052 ]104、106、1011、1015-规划路径上的规划点
[0053]105-偏离位置 A-机器人B-墙壁。
【具体实施方式】
[0054]为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，以下说明和附图对于本发明是示例性的，并且不应被理解为限制本发明。以下说明描述了众多具体细节以方便对本发明理解。然而，在某些实例中，熟知的或常规的细节并未说明，以满足说明书简洁的要求。
[0055]本发明机器人的运动控制方法可以通过java语言进行编写，同理也可以使用C#，VB，C++/C等其他编程语言进行实现。
[0056]如图1所述，根据本发明的一个实施例，一种机器人的运动控制方法，包括如下步骤:
[0057]SlO输入的地图信息、规划路径，以及机器人的实时位置信息，所述地图呈栅格状，每个栅格的大小可以根据具体情况进行设置，所述规划路径经过若干个栅格，所述规划点的选取:间隔选取设定距离的栅格为规划点，例如每隔五个栅格，设置一个规划点；或者每隔十个栅格，设置一个规划点；
[0058]S20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;此处，可以判断机器人的初始位置是否在规划路径上;也可以判断机器人在行走过程中，是否在规划路径上行走；
[0059]计算出所述机器人在所述规划路径上的目标位置信息；实时位置是指:机器人当前所处位置;当所述机器人在规划路径上时，目标位置是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点；当所述机器人不在规划路径上时，目标位置是指:所述规划路径上距离所述机器人当前所处位置最近的点；
[0060]S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;否则，直接跳转至步骤S40;
[0061 ] S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动;优选的，根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向。
[0062]具体的，机器人的初始位置101未在规划路径上时，如图13a、13b所示，100是规划路径，机器人从初始位置101行走到已知规划路径100上的最近点位置102时，方法中不包括步骤S30(根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置)时，机器人的行走路径为12;方法中包括步骤S30时，机器人的行走路径为12’。比较行走路径12和行走路径12’后，发现图13b中机器人的行走路径12’更短，并且机器人可以在狭窄区域完成行走。
[0063]如图1、图2所示，根据本发明的另一个实施例，一种机器人的运动控制方法，包括如下步骤:
[0064]SlO输入的地图信息、规划路径，以及机器人的实时位置信息，所述地图呈栅格状，每个栅格的大小可以根据具体情况进行设置，所述规划路径经过若干个栅格，所述规划点的选取:间隔选取设定距离的栅格为规划点，例如每隔五个栅格，设置一个规划点；或者每隔十个栅格，设置一个规划点；
[0065]S20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;此处，可以判断机器人的初始位置是否在规划路径上;也可以判断机器人在行走过程中，是否在规划路径上行走；
[0066]计算出所述机器人在所述规划路径上的目标位置信息；实时位置是指:机器人当前所处位置;当所述机器人在规划路径上时，目标位置是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点；当所述机器人不在规划路径上时，目标位置是指:所述规划路径上距离所述机器人当前所处位置最近的点；
[0067]S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;否则，直接跳转至步骤S40:
[0068]S31根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上；
[0069]S32获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角；
[0070]S33判断所述当前夹角与所述目标夹角是否相同；
[0071]S34若所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转后，跳转至步骤S33;
[0072]S35若所述当前夹角与所述目标夹角相同，则所述机器人保持当前朝向；跳转至步骤 S40;
[0073]这里的夹角计算仅用在算规划路径上距离机器人最近的点，一旦计算出最近点，把最近点作为第一个目标点，接着机器人运动到达第一个目标，然后机器人向下一个目标点运动，之后不用计算所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，直到机器人如果偏离已知规划路径一定范围，需要重新计算最近点，此时重复上述过程；
[0074]S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动;优选的，根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向。
[0075]具体的，机器人在行走过程中(在规划路径100上最近点102至规划点104这段距离时)，由于一些不确定因素(路面不平、轮子打滑等)，造成机器人偏离已知规划路径100，如图14a、14b所示。23是机器人非正常情况下的行走路径。求取已知规划路径上的最近点103作为目标位置，最近点是指:规划路径上距离机器人最近的点，最近点可以是规划路径所经过的一个栅格，也可以是规划点；使机器人最快趋近已知规划路径100。图14a为不计算最近点作为目标位置104时，机器人的行走路径34;图14b为计算最近点103作为目标位置时，机器人的行走路径34’。比较行走路径34和行走路径34’后，发现机器人按照行走路径34’行走，会最快回到规划路径上。
[0076]如图1、图3所示，根据本发明的再一个实施例，一种机器人的运动控制方法，包括如下步骤:
[0077]SlO输入的地图信息、规划路径，以及机器人的实时位置信息，所述地图呈栅格状，每个栅格的大小可以根据具体情况进行设置，所述规划路径经过若干个栅格，所述规划点的选取:间隔选取设定距离的栅格为规划点，例如每隔五个栅格，设置一个规划点；或者每隔十个栅格，设置一个规划点；
[0078]S20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;此处，可以判断机器人的初始位置是否在规划路径上;也可以判断机器人在行走过程中，是否在规划路径上行走；
[0079]计算出所述机器人在所述规划路径上的目标位置信息；实时位置是指:机器人当前所处位置;当所述机器人在规划路径上时，目标位置是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点；当所述机器人不在规划路径上时，目标位置是指:所述规划路径上距离所述机器人当前所处位置最近的点；
[0080]S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;否则，直接跳转至步骤S40;
[0081 ] S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动;优选的，根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向；
[0082]S41根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息;相邻位置是指:目标位置周侧的预设位置；
[0083]S42根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值；
[0084]S43判断所述当前曲率值与第一曲率阈值的大小，所述第一曲率阀值是根据机器人的性能，而得到的转弯路径的最大曲率值;判断机器人是否能够转过此弯；
[0085]S44若所述当前曲率值大于所述第一曲率阈值，则进一步判断所述机器人的当前速度与第一速度阈值的大小，所述第一速度阈值是根据机器人自身的性能，而得到的转弯时的最大速度值，判断机器人当前速度是否满足在转弯时的最大速度；否则，跳转至步骤S46；
[0086]S45若所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小后，跳转至步骤S44;否则，跳转至步骤S46;
[0087]S46判断所述机器人是否到达所述目标位置；
[0088]S47若所述机器人到达所述目标位置，则进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值;允许机器人转弯时，有适当误差;否贝1J，跳转至步骤S41;
[0089]S48若所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，则所述机器人以预设角速度旋转后，跳转至步骤S47。
[0090]具体的，在饱和输入条件下，即在有速度、加速度限制条件下，机器人从当前位置105行走到已知规划路径100上最近点103，如图15a、15b所示，如图16a、16b所示。不计算路径曲率时，机器人的行走路径35、100;计算路径曲率时，机器人的行走路径35’、100’。比较行走路径35、100和行走路径35 ’、100 ’后，发现计算路径曲率后，机器人行走路径更为精确，不会因速度、加速度的限制，而造成机器人速度不能到达控制输入值，使得机器人冲出目标点。
[0091]如图4所示，根据本发明的又一个实施例，一种机器人的运动控制方法，包括如下步骤:
[0092]SlO输入的地图信息、规划路径，以及机器人的实时位置信息，所述地图呈栅格状，每个栅格的大小可以根据具体情况进行设置，所述规划路径经过若干个栅格，所述规划点的选取:间隔选取设定距离的栅格为规划点，例如每隔五个栅格，设置一个规划点；或者每隔十个栅格，设置一个规划点；
[0093]S20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;此处，可以判断机器人的初始位置是否在规划路径上;也可以判断机器人在行走过程中，是否在规划路径上行走；
[0094]计算出所述机器人在所述规划路径上的目标位置信息；实时位置是指:机器人当前所处位置;当所述机器人在规划路径上时，目标位置是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点；当所述机器人不在规划路径上时，目标位置是指:所述规划路径上距离所述机器人当前所处位置最近的点；
[0095]S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;否则，直接跳转至步骤S40;
[0096]S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动;优选的，根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向；
[0097]S50判断所述目标位置是否为所述规划路径上的终点位置；
[0098]S60若所述目标位置不是所述规划路径上的终点位置，则跳转至步骤S20;否则，所述机器人的当前速度以第二预设阈值进行减小；
[0099]S70判断所述机器人是否到达所述终点位置；
[0100]S80若所述机器人到达所述终点位置，则进一步判断所述机器人的当前朝向与所述终点位置的规划朝向是否相同，若对机器人到达终点位置时，没有朝向要求，可省略此处机器人朝向判断及机器人后续朝向调整步骤;否则，跳转至步骤S60;
[0101]S90若所述机器人当前朝向与所述终点位置的规划朝向不同，所述机器人以预设角速度旋转后，跳转至步骤S80;
[0102]S91若所述机器人当前朝向与所述终点位置的规划朝向相同，则所述机器人保持当前朝向。
[0103]具体的，在选取机器人在规划路径上的规划点时，规划点之间的距离较大，其最大速度较大，如图17a所示；规划点之间的距离较小，其最大速度较小，如图17b所示；即图17a中机器人最大速度大于图17b中机器人最大速度，规划点的适当选取，可以满足对机器人最大速度的要求。
[0104]如图5、图6、图7所示，根据本发明的一个实施例，一种机器人的运动控制系统，包括:
[0105]判断模块20，所述判断模块20根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上；
[0106]计算模块10，所述计算模块10与所述判断模块20电连接，计算出所述机器人在所述规划路径上的目标位置信息；
[0107]朝向控制模块30，所述朝向控制模块30分别与所述判断模块20、计算模块10电连接，所述朝向控制模块30接收所述判断模块20输出的所述机器人当前所处位置不在所述规划路径上，所述朝向控制模块30根据所述计算模块10输出的所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置；
[0108]运动控制模块40，所述运动控制模块40分别与所述判断模块20、朝向控制模块30电连接，所述运动控制模块40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动。
[0109]优选的，所述计算模块10还根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上；
[0110]优选的，还包括获取模块11，所述获取模块11用于获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角；
[0111]优选的，所述判断模块20还分别与所述计算模块10、获取模块11电连接，所述判断模块20接收所述计算模块10输出的目标夹角，所述判断模块20还接收所述获取模块11输出的当前夹角，判断所述当前夹角与所述目标夹角是否相同；
[0112]优选的，所述朝向控制模块30与所述判断模块20电连接，所述朝向控制模块30接收所述判断模块20输出的所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转;否则，所述机器人保持当前朝向；
[0113]优选的，所述计算模块10还根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息；
[0114]优选的，所述计算模块10还根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值；
[0115]优选的，所述判断模块20接收所述计算模块10输出的当前曲率值，判断所述当前曲率值与第一曲率阈值的大小；
[0116]优选的，若所述当前曲率值大于所述第一曲率阈值，所述判断模块20进一步判断所述机器人的当前速度与第一速度阈值的大小；
[0117]优选的，还包括速度控制模块41，所述速度控制模块41与所述判断模块20电连接，所述速度控制模块41接收所述判断模块20输出的所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小；
[0118]优选的，所述判断模块20还判断所述机器人是否到达所述目标位置;若所述机器人到达所述目标位置，所述判断模块20进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值；
[0119]优选的，所述朝向控制模块30接收所述判断模块20输出的所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，所述机器人以预设角速度旋转。
[0120]如图5、图6、图7所示，根据本发明的另一个实施例，一种机器人的运动控制系统，包括:
[0121]优选的，还包括输入模块，用于输入的地图信息、规划路径，以及机器人的实时位置信息，所述地图呈栅格状，每个栅格的大小可以根据具体情况进行设置，所述规划路径经过若干个栅格，所述规划点的选取:间隔选取设定距离的栅格为规划点，例如每隔五个栅格，设置一个规划点;或者每隔十个栅格，设置一个规划点；
[0122]判断模块20，用于根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上;此处，可以判断机器人的初始位置是否在规划路径上；也可以判断机器人在行走过程中，是否在规划路径上行走；
[0123]计算模块10，用于计算出所述机器人在所述规划路径上的目标位置信息；实时位置是指:机器人当前所处位置；当所述机器人在规划路径上时，目标位置是指:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点；当所述机器人不在规划路径上时，目标位置是指:所述规划路径上距离所述机器人当前所处位置最近的点；；
[0124]朝向控制模块30，若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置；
[0125]优选的，所述计算模块10还根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上；
[0126]优选的，还包括获取模块11，用于获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角；
[0127]优选的，所述判断模块20还判断所述当前夹角与所述目标夹角是否相同；
[0128]优选的，若所述当前夹角与所述目标夹角不同，所述朝向控制模块30控制所述机器人以预设角速度旋转；
[0129]优选的，若所述当前夹角与所述目标夹角相同，所述朝向控制模块30控制所述机器人保持当前朝向；
[0130]所述运动控制模块40还根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动;优选的，根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向；
[0131 ]优选的，所述计算模块10还根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息;相邻位置是指:目标位置周侧的预设位置；
[0132]优选的，所述计算模块10还根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值；
[0133]优选的，所述判断模块20还判断所述当前曲率值与第一曲率阈值的大小，所述第一曲率阀值是根据机器人的性能，而得到的转弯路径的最大曲率值;判断机器人是否能够转过此弯；
[0134]优选的，若所述当前曲率值大于所述第一曲率阈值，所述判断模块20还进一步判断所述机器人的当前速度与第一速度阈值的大小，所述第一速度阈值是根据机器人自身的性能，而得到的转弯时的最大速度值，判断机器人当前速度是否满足在转弯时的最大速度；
[0135]优选的，还包括速度控制模块41，若所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小；
[0136]优选的，所述判断模块20还判断所述机器人是否到达所述目标位置；
[0137]优选的，若所述机器人到达所述目标位置，所述判断模块20还进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值;允许机器人转弯时，有适当误差；
[0138]优选的，若所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，所述朝向控制模块30还控制所述机器人以预设角速度旋转；
[0139]优选的，所述判断模块20还判断所述目标位置是否为所述规划路径上的终点位置；
[0140]优选的，若所述目标位置是所述规划路径上的终点位置，所述速度控制模块41还所述机器人的当前速度以第二预设阈值进行减小；
[0141 ]优选的，所述判断模块20还判断所述机器人是否到达所述终点位置；
[0142]优选的，若所述机器人到达所述终点位置，所述判断模块20还进一步判断所述机器人的当前朝向与所述终点位置的规划朝向是否相同，若对机器人到达终点位置时，没有朝向要求，可省略此处机器人朝向判断及机器人后续朝向调整步骤；
[0143]优选的，若所述机器人当前朝向与所述终点位置的规划朝向不同，所述朝向控制模块30还控制所述机器人以预设角速度旋转；否则，所述朝向控制模块30还控制所述机器人保持当前朝向。
[0144]如图8、图9、图10、图11和图12所不，根据本发明的另一个实施例，一种机器人的运动控制方法，包括:
[0145]S1:输入的信息包括栅格地图、规划好的路径、机器人的实时位置信息，将实际地图转换为栅格地图，每个栅格为5cm X 5cm;
[0146]S2:计算已知规划路径上距离机器人最近的点，遍历已知规划路径上所有目标点，计算与当前位置距离，距离最小的即为最近点，并将最近记为目标点，如图7所示；
[0147]S3:机器人姿态判断模块，如图9所示；
[0148]S3.1:连接机器人当前位置点和最近点，计算所得连线与全局坐标系横轴的夹角，判断机器人当前姿态是否与所得夹角相同；不同转S3.2，相同转S4;
[0149]S3.2:机器人以给定角速度原地旋转一定角度，转S3.1;
[0150]S4:根据机器人当前姿态信息、目标点姿态信息，把对路径的跟踪看作是机器人依次到达路径上的坐标点，在极坐标误差模型下，利用Lyapunov直接法设计跟踪控制器，计算出控制输入量:机器人线速度、角速度，并计算出机器人实时姿态，转S5;
[0151]S5:判断目标点是否为已知规划路径的终点，否则转S6，是则转S8;
[0152]S6:计算曲率模块，如图10所示；
[0153]S6.1:遍历机器人实时位置前后一定范围，计算出路径曲率转，S6.2;
[0154]S6.2:判断计算所得曲率值与阈值曲率的大小，小于阈值转S6.5，大于阈值转S6.3；
[0155]S6.3:判断机器人当前时刻速度与阈值速度大小，小于阈值转S6.5，大于阈值转S6.4；
[0156]S6.4:机器人速度以某一阈值减小，转S6.3;
[0157]S6.5:判断机器人是否到达目标点，否则转S6.1，是则转S6.6;
[0158]S6.6:判断机器人当前姿态是否满足阈值，否则转S6.7，是则转S7;
[0159]S6.7:机器人以给定角速度旋转一定角度，转S6.6;
[0160]S7:计算机器人下一个目标点，机器人目标点的选取可以满足对机器人最大速度的要求，转S4;
[0161 ] S8:判断机器人速度模块，如图11所示；
[0162]S8.1:判断机器人当前时刻速度与阈值速度大小，小于阈值转S9，大于阈值转S8.2；
[0163]S8.2:机器人速度以某一阈值减小，转S8.1 ；
[0164]S9:判断机器人是否到达终点，否则转Sl0.1，是则转SlO;
[0165]SlO:机器人终点姿态判断模块，如图12所示；
[0166]S10.1:判断机器人当前姿态是否与终点姿态相同，否则转S10.2，是则转Sll;
[0167]S10.2:机器人以给定角速度原地旋转一定角度，转Sl0.1;
[0168]S11:输出路径跟踪完毕信息，结束。
[0169]应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
【主权项】
1.一种机器人的运动控制方法，其特征在于，包括如下步骤: S20根据输入的地图信息、规划路径及实时位置信息，判断所述机器人当前所处位置是否在所述规划路径上，计算出机器人在所述规划路径上的目标位置信息； S30若所述机器人当前所处位置不在所述规划路径上，则根据所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置;否则，直接跳转至步骤S40; S40根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动。2.根据权利要求1所述的机器人的运动控制方法，其特征在于，所述步骤S30包括: S31根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上； S32获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角； S33判断所述当前夹角与所述目标夹角是否相同； S34若所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转后，跳转至步骤S33;否则，所述机器人保持当前朝向。3.根据权利要求1所述的机器人的运动控制方法，其特征在于，还包括如下步骤: S41根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息； S42根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值； S43判断所述当前曲率值与第一曲率阈值的大小； S44若所述当前曲率值大于所述第一曲率阈值，则进一步判断所述机器人的当前速度与第一速度阈值的大小;否则，跳转至步骤S46; S45若所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小后，跳转至步骤S44;否则，跳转至步骤S46; S46判断所述机器人是否到达所述目标位置； S47若所述机器人到达所述目标位置，则进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值;否则，跳转至步骤S41; S48若所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，则所述机器人以预设角速度旋转后，跳转至步骤S47。4.根据权利要求1所述的机器人的运动控制方法，其特征在于，还包括如下步骤: S50判断所述目标位置是否为所述规划路径上的终点位置； S60若所述目标位置不是所述规划路径上的终点位置，则跳转至步骤S20;否则，所述机器人的当前速度以第二预设阈值进行减小。5.根据权利要求4所述的机器人的运动控制方法，其特征在于，还包括如下步骤: S70判断所述机器人是否到达所述终点位置； S80若所述机器人到达所述终点位置，则进一步判断所述机器人的当前朝向与所述终点位置的规划朝向是否相同；否则，跳转至步骤S60; S90若所述机器人的当前朝向与所述终点位置的规划朝向不同，所述机器人以预设角速度旋转后，跳转至步骤S80;否则，所述机器人保持当前朝向。6.根据权利要求1?5中任意一项所述的机器人的运动控制方法，其特征在于: 所述步骤S20中的地图呈栅格状，所述步骤S20中的目标位置信息包括:所述规划路径上的所有规划点中距离所述机器人当前所处位置最近的规划点位置信息，或所述规划路径上距离所述机器人当前所处位置最近的点位置信息。7.根据权利要求1?5中任意一项所述的机器人的运动控制方法，其特征在于: 所述步骤S40是指:根据所述实时位置信息和目标位置信息，在极坐标模型下，利用Lyapunov函数，利用Lyapunov直接法设计跟踪控制器，计算出所述机器人的线速度、角速度和实时朝向。8.—种应用在如权利要求1?7中任意一项所述的机器人的运动控制方法的控制系统，其特征在于，包括: 判断模块，所述判断模块根据输入的地图信息、规划路径及实时位置信息，判断机器人当前所处位置是否在所述规划路径上； 计算模块，所述计算模块与所述判断模块电连接，用于计算出所述机器人在所述规划路径上的目标位置信息； 朝向控制模块，所述朝向控制模块分别与所述判断模块、计算模块电连接，所述朝向控制模块接收所述判断模块输出的所述机器人当前所处位置不在所述规划路径上，所述朝向控制模块根据所述计算模块输出的所述实时位置信息和目标位置信息，使得所述机器人朝向所述目标位置； 运动控制模块，所述运动控制模块分别与所述判断模块、朝向控制模块电连接，所述运动控制模块根据所述实时位置信息和目标位置信息，计算出所述机器人的线速度、角速度和实时朝向后，控制所述机器人运动。9.根据权利要求8所述的机器人的运动控制系统，其特征在于，还包括: 所述计算模块还根据所述机器人的实时位置信息、目标位置信息，计算出所述实时位置和目标位置所在的直线段与坐标系上横轴所形成的目标夹角，所述坐标系设置在所述地图上； 所述获取模块用于获取所述机器人的当前朝向与所述坐标系上横轴所形成的当前夹角； 所述判断模块还分别与所述计算模块、获取模块电连接，所述判断模块接收所述计算模块输出的目标夹角，所述判断模块还接收所述获取模块输出的当前夹角，判断所述当前夹角与所述目标夹角是否相同； 所述朝向控制模块与所述判断模块电连接，所述朝向控制模块接收所述判断模块输出的所述当前夹角与所述目标夹角不同，则所述机器人以预设角速度旋转;否则，所述机器人保持当前朝向。10.根据权利要求8所述的机器人的运动控制系统，其特征在于，还包括: 所述计算模块还根据所述机器人的目标位置信息，计算出所述目标位置的相邻位置信息; 所述计算模块还根据所述相邻位置信息，计算出所述机器人所在路径的当前曲率值； 所述判断模块接收所述计算模块输出的当前曲率值，判断所述当前曲率值与第一曲率阈值的大小； 若所述当前曲率值大于所述第一曲率阈值，所述判断模块进一步判断所述机器人的当前速度与第一速度阈值的大小； 速度控制模块，所述速度控制模块与所述判断模块电连接，所述速度控制模块接收所述判断模块输出的所述机器人的当前速度大于第一速度阈值，则所述机器人的当前速度以第一预设阈值进行减小； 所述判断模块还判断所述机器人是否到达所述目标位置； 若所述机器人到达所述目标位置，所述判断模块进一步判断所述机器人的当前朝向与所述目标位置的规划朝向之间的差值是否满足第一阈值； 所述朝向控制模块接收所述判断模块输出的所述机器人的当前朝向与所述目标位置的规划朝向之间的差值不满足第一阈值，所述机器人以预设角速度旋转。
【文档编号】G05D1/02GK105867379SQ201610244271
【公开日】2016年8月17日
【申请日】2016年4月13日
【发明人】左嘉欣, 张小*, 章征贵
【申请人】上海物景智能科技有限公司