CN107703973B - 轨迹跟踪方法、装置 - Google Patents

轨迹跟踪方法、装置 Download PDF

Info

Publication number
CN107703973B
CN107703973B CN201710812423.7A CN201710812423A CN107703973B CN 107703973 B CN107703973 B CN 107703973B CN 201710812423 A CN201710812423 A CN 201710812423A CN 107703973 B CN107703973 B CN 107703973B
Authority
CN
China
Prior art keywords
preset
target
current position
variable
tar
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.)
Active
Application number
CN201710812423.7A
Other languages
English (en)
Other versions
CN107703973A (zh
Inventor
罗汉杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201710812423.7A priority Critical patent/CN107703973B/zh
Priority to PCT/CN2017/116856 priority patent/WO2019047415A1/zh
Publication of CN107703973A publication Critical patent/CN107703973A/zh
Application granted granted Critical
Publication of CN107703973B publication Critical patent/CN107703973B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/12—Target-seeking control

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种轨迹跟踪方法、装置。其中，该方法包括：获取目标设备的当前位置；判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。本发明解决了现有技术中，无法控制机器人的运行轨迹与预定轨迹一致的技术问题。

Description

轨迹跟踪方法、装置
技术领域
本发明涉及机器人导航领域，具体而言，涉及一种轨迹跟踪方法、装置。
背景技术
机器人导航技术是机器人领域的核心技术之一，其研究目标是控制机器人在特定的场景中，通过传感器获取环境信息和判断自身状态，实现自我定位，并且规划在当前特定场景下面向目标的自主活动，以完成给定的任务。
在机器人导航技术中，轨迹跟踪是其中的一个经典问题，它要求机器人沿着一条给定的轨迹运行，轨迹跟踪问题的难点在于，如何根据机器人的当前状态和给定的轨迹，生成机器人的控制信号，使得机器人运行轨迹跟给定轨迹一致。
针对上述现有技术中，无法控制机器人的运行轨迹与预定轨迹一致的问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种轨迹跟踪方法、装置，以至少解决现有技术中，无法控制机器人的运行轨迹与预定轨迹一致的技术问题。
根据本发明实施例的一个方面，提供了一种轨迹跟踪方法，包括：获取目标设备的当前位置；判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
进一步地，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，方法还包括：若判断出当前位置与预定位置的距离小于等于预设距离阀值，则确定目标轨迹的下一预定位置，其中，下一预定位置与当前位置的距离大于预设距离阀值，下一预定位置为预定位置的下一个位置；根据目标设备的当前位置和下一预定位置生成控制指令，其中，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至下一预定位置。
进一步地，获取目标设备的当前位置，包括：获取目标设备的位置变量和方向变量；根据位置变量和方向变量，确定目标设备的当前位置。
进一步地，在预定位置为目标轨迹的初始轨迹点的情况下，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，方法还包括：将目标轨迹的初始轨迹点确定为预定位置；获取预定位置的位置变量和方向变量，其中，根据目标设备的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定当前位置与预先设定的目标轨迹中预定位置的距离。
进一步地，根据目标设备的当前位置和预定位置，生成控制指令，包括：根据当前位置的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定目标设备从当前位置移动至预定位置的导航向量；归一化处理导航向量，得到归一化处理后的导航向量；获取归一化处理后的导航向量的角度值；根据导航向量和归一化处理后的导航向量的角度值，得到控制指令。
进一步地，通过如下公式计算得到目标设备从当前位置移动至预定位置的导航向量：预设当前位置为st＝[xt，yt，θt]，预设预定位置为star＝[xtar，ytar，θtar]，预设预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure GDA0003096839030000021
其中，xt，yt为当前位置的位置变量，θt为当前位置的方向变量，xtar，ytar为预定位置的位置变量，θtar为预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
进一步地，通过如下方式归一化处理导航向量，得到归一化处理后的导航向量：
Figure GDA0003096839030000022
其中，Fn为归一化处理后的导航向量。
进一步地，通过如下公式计算得到归一化处理后的导航向量的角度值：
Figure GDA0003096839030000023
其中，φ为归一化处理后的导航向量的角度值。
进一步地，通过如下公式计算得到控制指令：
Figure GDA0003096839030000031
其中，at为目标设备在t时刻的控制指令，vlinear为目标设备移动的线速度，vangular为目标设备移动的角速度，xt，yt为目标设备的位置变量，θt为目标设备的方向变量，φ为归一化处理后的导航向量的角度值，
Figure GDA0003096839030000032
为φ的导数，Tanh为双曲正切函数，ku，kw为预先设定的控制参数，其中ku用于控制目标设备的线速度，kw用于控制目标设备的角速度。
进一步地，在根据目标设备的当前位置和预定位置，生成控制指令之后，方法还包括：将控制指令转换为控制信号；发送控制信号至目标设备，以控制目标设备根据控制信号移动。
根据本发明实施例的另一方面，还提供了一种轨迹跟踪装置，包括：获取模块，用于获取目标设备的当前位置；判断模块，用于判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；控制模块，用于若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
根据本发明实施例的另一方面，还提供了一种存储介质，其特征在于，存储介质包括存储的程序，其中，程序执行上述任意一项的轨迹跟踪方法。
根据本发明实施例的另一方面，还提供了一种处理器，其特征在于，处理器用于运行程序，其中，程序运行时执行上述任意一项的轨迹跟踪方法。
在本发明实施例中，采用循环控制的方式，通过获取目标设备的当前位置；判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置，达到了控制机器人一个轨迹点接着一个轨迹点移动至目标轨迹的最终位置的目的，从而实现了控制机器人的运行轨迹与预定轨迹一致的技术效果，进而解决了现有技术中，无法控制机器人的运行轨迹与预定轨迹一致的技术问题。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明实施例的一种轨迹跟踪方法的步骤流程图；
图2是根据本发明实施例的一种可选的目标轨迹的示意图；
图3是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图4是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图5是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图6是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图7是根据本发明实施例的一种可选的目标设备在导航向量导航下移动的示意图；
图8是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图9是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图；
图10是根据本发明实施例的一种轨迹跟踪装置的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
实施例1
根据本发明实施例，提供了一种轨迹跟踪方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
图1是根据本发明实施例的一种轨迹跟踪方法的步骤流程图，如图1所示，该方法包括如下步骤：
步骤S102，获取目标设备的当前位置。
具体的，在上述步骤S102中，目标设备可以为仿生设备，优选的，可以为机器人。
在一种可选的实施例中，可以通过机器人中设置的传感器或者定位装置来获取机器人当前所处的位置，其中，上述当前位置可以包括如下信息：机器人的位置变量和方向向量，其中，机器人的位置变量可以为世界坐标系下的位置变量，机器人的方向向量可以为机器人在所处位置的朝向夹角。
步骤S104，判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小。
具体的，在上述步骤S104中，可以预先设定或者给定一条目标轨迹，该目标轨迹为要求机器人沿着该轨迹运行的轨迹，并且可以在该目标轨迹上选择一个预定位置，图2是根据本发明实施例的一种可选的目标轨迹的示意图，如图2所示，该预定位置可以为目标轨迹上的任意一个轨迹点，例如，s0、s1、s2等；优选的，可以为目标轨迹上的初始轨迹点s0，以实现控制机器人沿着给定的目标轨迹从初始轨迹点，移动至目标轨迹上的最后一个轨迹点。
具体的，当前位置与预先设定的目标轨迹中预定位置的距离可以为欧拉距离，其中，欧拉距离为直角坐标系里的两点间的距离，可以根据欧拉距离公式进行计算。
具体的，上述预设距离阀值可以为根据实际情况确定的，例如，可以选择控制机器人在目标轨迹上移动，或者沿着目标轨迹，在偏离目标轨迹预设距离的一条轨迹上移动，优选的，上述预设距离阀值可以为0，也即，控制机器人在目标轨迹上移动。
在一种可选的实施例中，可以在世界坐标系下，根据机器人当前的位置变量和方向向量，与预先设定的目标轨迹中预定位置的位置变量和方向向量相比较，确定机器人当前位置与目标轨迹中预定位置的距离，进而可以判断机器人当前位置与目标轨迹中预定位置的距离与预设阈值大小。
步骤S106，若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
具体的，在上述步骤S106中，控制指令可以包括有以下信息：目标设备移动的角速度和线速度，用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
在一种可选的实施例中，在判断出当前位置与预定位置的距离大于预设距离阀值，则表明机器人当前位置已偏离给定的目标轨迹，因此，可以控制机器人从当前所处的位置移动至目标轨迹中的预定位置，可选的，控制机器人的实施主体可以但不限于内置在机器人中的控制器，其中，可以假定为是一个可以接受a＝[vlinear，vangular]T作为控制指令的控制器，其中，a为控制指令，vlinear为将上述角速度，vangular为上述线速度。作为一种可选的实施方式，控制器可以将控制指令转变为控制机器人中的电机的控制信号，并将该控制信号发送至与该控制器连接的电机，控制电机执行相对应的动作。
作为一种可选的实施例，可以根据机器人的当前位置和目标轨迹中的预定位置，计算出虚拟的导航向量，其中，该导航向量是从机器人当前位置指向预定位置的，并根据导航向量可以得出控制机器人移动的角速度和线速度，在生成控制机器人的控制指令之后，可以根据控制指令控制机器人根据给定的角速度和线速度移动至预定位置，而且机器人移动的路径与导航向量的指向一致。
基于上述步骤S102至步骤S108所提供的实施例，本申请还提供了如下可选或优选的技术方案：
在一种可选的实施例中，图3是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图3所示，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，方法还包括如下步骤：
步骤S202，若判断出当前位置与预定位置的距离小于等于预设距离阀值，则确定目标轨迹的下一预定位置，其中，下一预定位置与当前位置的距离大于预设距离阀值，下一预定位置为预定位置的下一个位置；
步骤S204，根据目标设备的当前位置和下一预定位置生成控制指令，其中，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至下一预定位置。
具体的，预设上述下一预定位置与当前位置的距离大于预设距离阀值，下一预定位置为预定位置的下一个位置，如图2所示，在预定位置为s0的情况下，下一预定位置可以为s1，可选的，预先设定下一预定位置与当前位置的欧拉距离大于预设距离阀值，可以控制机器人的移动距离。
在一种可选的实施例中，在判断出当前位置与预定位置的距离大于预设距离阀值，则表明机器人当前在目标轨迹上移动，或者沿着目标轨迹，在偏离目标轨迹预设距离的一条轨迹上移动，因此，根据机器人的当前位置和目标轨迹上的下一预定位置，生成控制机器人移动至下一预定位置的控制指令。
具体的，该控制指令可以包括有以下信息：目标设备移动的角速度和线速度，用于控制目标设备根据角速度和线速度，根据给定的角速度和线速度从当前位置移动至下一预定位置，而且机器人移动的路径与导航向量的指向一致。
在一种可选的实施例中，图4是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图4所示，获取目标设备的当前位置，包括如下步骤：
步骤S302，获取目标设备的位置变量和方向变量；
步骤S304，根据位置变量和方向变量，确定目标设备的当前位置。
在一种可选的实施例中，图5是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图5所示，在预定位置为目标轨迹的初始轨迹点的情况下，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，方法还包括如下步骤：
步骤S402，将目标轨迹的初始轨迹点确定为预定位置；
步骤S404，获取预定位置的位置变量和方向变量，其中，根据目标设备的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定当前位置与预先设定的目标轨迹中预定位置的距离。
在一种可选的实施例中，若设定上述目标轨迹为Sobj，将目标轨迹Sobj中的第初始轨迹点s0设为预定位置，则该预定位置为机器人移动的初始子目标star，在当前时刻目标轨迹中预定位置为初始轨迹点，也即，star＝s0。
其中，由于当前位置与预先设定的目标轨迹中预定位置的距离可以为欧拉距离，则可以根据目标设备的位置变量和方向变量，以及预定位置的位置变量和方向变量，通过欧拉距离公式计算得到当前位置与预先设定的目标轨迹中预定位置的欧拉距离。
在一种可选的实施例中，图6是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图6所示，根据目标设备的当前位置和预定位置，生成控制指令，包括如下步骤：
步骤S502，根据当前位置的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定目标设备从当前位置移动至预定位置的导航向量；
步骤S504，归一化处理导航向量，得到归一化处理后的导航向量；
步骤S506，获取归一化处理后的导航向量的角度值；
步骤S508，根据导航向量和归一化处理后的导航向量的角度值，得到控制指令。
具体的，上述导航向量可以为一种具有方向的向量，不断引导机器人移动至预定位置，而且可以使得机器人移动的方向与目标的朝向一致。
具体的，上述归一化是一种简单的无量纲处理计算方式，也即将有量纲的表达式变换为无量纲的表达式，成为标量。
作为一种可选的实施例，根据目标设备的当前位置和预定位置，生成控制指令可以通过以下步骤实现：首先，根据当前位置的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定目标设备从当前位置移动至预定位置的导航向量，并对导航向量进行归一化处理，得到归一化处理之后的导航向量，其次，可以通过公式计算获取归一化处理后的导航向量的角度值，最后，根据导航向量和归一化处理后的导航向量的角度值，得到控制指令。
在一种可选的实施例中，通过如下公式计算得到目标设备从当前位置移动至预定位置的导航向量：
预设当前位置为st＝[xt，yt，θt]，预设预定位置为star＝[xtar，ytar，θtar]，预设预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure GDA0003096839030000081
其中，xt，yt为当前位置的位置变量，θt为当前位置的方向变量，xtar，ytar为预定位置的位置变量，θtar为预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
在一种可选的实施例中，通过如下方式归一化处理导航向量，得到归一化处理后的导航向量：
Figure GDA0003096839030000082
其中，Fn为归一化处理后的导航向量。
在一种可选的实施例中，通过如下公式计算得到归一化处理后的导航向量的角度值：
Figure GDA0003096839030000091
其中，φ为归一化处理后的导航向量的角度值。
在一种可选的实施例中，通过如下公式计算得到控制指令：
Figure GDA0003096839030000092
其中，at为目标设备在t时刻的控制指令，vlinear为目标设备移动的线速度，vangular为目标设备移动的角速度，xt，yt为目标设备的位置变量，θt为目标设备的方向变量，φ为归一化处理后的导航向量的角度值，
Figure GDA0003096839030000093
为φ的导数，Tanh为双曲正切函数，ku，kw为预先设定的控制参数，具体数值要根据具体任务调整；其中，ku用于控制目标设备的线速度，kw用于控制目标设备的角速度。
在现有的很多导航算法中，只能确保机器人最终到达目标位置，但是不能保证机器人最后的朝向与目标一致。在一种可选的实施例中，可以通过具有方向的导航向量不断引导机器人驶向预定位置，并且使得机器人追踪的朝向与目标一致，如图7所示，在导航向量作用下，机器人最终会驶向如图7所示的中间位置，并且朝右。
在一种可选的实施例中，图8是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图8所示，在根据目标设备的当前位置和预定位置，生成控制指令之后，方法还包括：
步骤S602，将控制指令转换为控制信号；
步骤S604，发送控制信号至目标设备，以控制目标设备根据控制信号移动。
作为一种可选的实施例，可选的，控制机器人的实施主体可以但不限于控制器，其中，可以假定为是一个可以接受a＝[vlinear，vangular]T作为控制指令的控制器，其中，a为控制指令，vlinear为将上述角速度，vangular为上述线速度。作为一种可选的实施方式，控制器可以将控制指令转变为控制机器人中的电机的控制信号，并将该控制信号发送至与该控制器连接的电机，控制电机执行相对应的动作。
以下提供一种可选的实施方案对上述实施例进行说明，图9是根据本发明实施例的一种可选的轨迹跟踪方法的步骤流程图，如图9所示，上述轨迹跟踪方法可以通过如下步骤实现：
步骤S702，预先设定目标轨迹Sobj中的初始轨迹点s0为初始子目标star。
步骤S704，获取机器人的当前位置。
具体的，根据上述当前位置可以判断机器人在当前时刻t的位姿状态,可以设为：st＝[xt，yt，θt]，其中，s为机器人，xt，yt为位置变量，θt为方向变量。
步骤S706，计算当前位置与预先设定的目标轨迹中预定位置的欧拉距离d。
步骤S708，判断上述欧拉距离d与预设距离阀值dstep的大小。
具体的，如果判断出欧拉距离d大于预设距离阀值dstep，则进入步骤S712，如果判断出欧拉距离d小于等于预设距离阀值dstep，则进入步骤S710。
步骤S710，依照顺序在Sobj中选取下一个轨迹点作为新的子目标star，预设机器人当前位置与新的子目标之间的欧拉距离d＞dstep；如果star已经是Sobj的最后一个点，则star不变。
步骤S712，根据给定的控制算法输出控制指令at，机器人执行控制指令运行一步。
在一种可选的实施例中，在机器人到达终点的情况下，获取到的控制指令就是a＝[0，0]T，使得机器人自动停下来，否则循环执行至步骤S704，也即，本申请所提的轨迹跟踪方法中的算法是可以不断循环的，除非用户强制控制跳出。
或者还可以在获取当前时刻机器人的当前位置之后，判断机器人是否已经到达目标轨迹Sobj的最后一个轨迹点，如果是，则退出。
实施例2
本发明实施例还提供了一种轨迹跟踪装置，需要说明的是，上述实施例1所提供的方法步骤，可以但不限于在本发明实施例所提供的生成测试脚本的装置中执行。
图10是根据本发明实施例的一种轨迹跟踪装置的结构示意图，如图10所示，该装置包括以下模块：获取模块10、判断模块12以及控制模块14，其中，
获取模块10，用于获取目标设备的当前位置；判断模块12，用于判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；控制模块14，用于若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
需要说明的是，上述获取模块10、判断模块12以及控制模块14对应于实施例1中的步骤S102至步骤S106，上述模块与对应的步骤所实现的示例和应用场景相同，但不限于上述实施例1所公开的内容。需要说明的是，上述模块作为装置的一部分可以在诸如一组计算机可执行指令的计算机系统中执行。
仍需要说明的是，上述各个模块是可以通过软件或硬件的形式来实现的，对于后者，可以通过以下方式来实现，但不限于此：上述各个模块位于同一处理器中；或者，上述各个模块位以任意组合的形式于不同的处理器中。
实施例3
本发明实施例还提供了一种存储介质，存储介质包括存储的程序，其中，程序执行上述任意一项可选的或优选的轨迹跟踪方法。
本发明实施例提供的存储介质用于存储执行以下功能的程序：获取目标设备的当前位置；判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
本发明实施例提供的存储介质用于存储执行以下功能的程序：在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，方法还包括：若判断出当前位置与预定位置的距离小于等于预设距离阀值，则确定目标轨迹的下一预定位置，其中，下一预定位置与当前位置的距离大于预设距离阀值，下一预定位置为预定位置的下一个位置；根据目标设备的当前位置和下一预定位置生成控制指令，其中，控制指令用于控制目标设备根据角速度和线速度，从预定位置移动至下一预定位置。
本发明实施例提供的存储介质用于存储执行以下功能的程序：获取目标设备的当前位置，包括：获取目标设备的位置变量和方向变量；根据位置变量和方向变量，确定目标设备的当前位置。
本发明实施例提供的存储介质用于存储执行以下功能的程序：在预定位置为目标轨迹的初始轨迹点的情况下，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，方法还包括：将目标轨迹的初始轨迹点确定为预定位置；获取预定位置的位置变量和方向变量，其中，根据目标设备的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定当前位置与预先设定的目标轨迹中预定位置的距离。
本发明实施例提供的存储介质用于存储执行以下功能的程序：根据目标设备的当前位置和预定位置，生成控制指令，包括：根据当前位置的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定目标设备从当前位置移动至预定位置的导航向量；归一化处理导航向量，得到归一化处理后的导航向量；获取归一化处理后的导航向量的角度值；根据导航向量和归一化处理后的导航向量的角度值，得到控制指令。
本发明实施例提供的存储介质用于存储执行以下功能的程序：通过如下公式计算得到目标设备从当前位置移动至预定位置的导航向量：预设当前位置为st＝[xt，yt，θt]，预设预定位置为star＝[xtar，ytar，θtar]，预设预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure GDA0003096839030000121
其中，xt，yt为当前位置的位置变量，θt为当前位置的方向变量，xtar，ytar为预定位置的位置变量，θtar为预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
本发明实施例提供的存储介质用于存储执行以下功能的程序：通过如下方式归一化处理导航向量，得到归一化处理后的导航向量：
Figure GDA0003096839030000122
其中，Fn为归一化处理后的导航向量。
本发明实施例提供的存储介质用于存储执行以下功能的程序：通过如下公式计算得到归一化处理后的导航向量的角度值：
Figure GDA0003096839030000123
其中，φ为归一化处理后的导航向量的角度值。
本发明实施例提供的存储介质用于存储执行以下功能的程序：通过如下公式计算得到控制指令：
Figure GDA0003096839030000131
其中，at为目标设备在t时刻的控制指令，vlinear为目标设备移动的线速度，vangular为目标设备移动的角速度，xt，yt为目标设备的位置变量，θt为目标设备的方向变量，φ为归一化处理后的导航向量的角度值，
Figure GDA0003096839030000132
为φ的导数，Tanh为双曲正切函数，ku，kw为预先设定的控制参数，其中ku用于控制目标设备的线速度，kw用于控制目标设备的角速度。
本发明实施例提供的存储介质用于存储执行以下功能的程序：在根据目标设备的当前位置和预定位置，生成控制指令之后，方法还包括：将控制指令转换为控制信号；发送控制信号至目标设备，以控制目标设备根据控制信号移动。
实施例4
本发明实施例还提供了一种处理器，其特征在于，处理器用于运行程序，其中，程序运行时执行上述任意一项可选的或优选的轨迹跟踪方法。
本发明实施例提供的处理器用于运行执行以下功能的程序：获取目标设备的当前位置；判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；若判断出当前位置与预定位置的距离大于预设距离阀值，则根据目标设备的当前位置和预定位置，生成控制指令，其中，控制指令包括有以下信息：目标设备移动的角速度和线速度，控制指令用于控制目标设备根据角速度和线速度，从当前位置移动至预定位置。
本发明实施例提供的处理器用于运行执行以下功能的程序：在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，方法还包括：若判断出当前位置与预定位置的距离小于等于预设距离阀值，则确定目标轨迹的下一预定位置，其中，下一预定位置与当前位置的距离大于预设距离阀值，下一预定位置为预定位置的下一个位置；根据目标设备的当前位置和下一预定位置生成控制指令，其中，控制指令用于控制目标设备根据角速度和线速度，从预定位置移动至下一预定位置。
本发明实施例提供的处理器用于运行执行以下功能的程序：获取目标设备的当前位置，包括：获取目标设备的位置变量和方向变量；根据位置变量和方向变量，确定目标设备的当前位置。
本发明实施例提供的处理器用于运行执行以下功能的程序：在预定位置为目标轨迹的初始轨迹点的情况下，在判断当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，方法还包括：将目标轨迹的初始轨迹点确定为预定位置；获取预定位置的位置变量和方向变量，其中，根据目标设备的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定当前位置与预先设定的目标轨迹中预定位置的距离。
本发明实施例提供的处理器用于运行执行以下功能的程序：根据目标设备的当前位置和预定位置，生成控制指令，包括：根据当前位置的位置变量和方向变量，以及预定位置的位置变量和方向变量，确定目标设备从当前位置移动至预定位置的导航向量；归一化处理导航向量，得到归一化处理后的导航向量；获取归一化处理后的导航向量的角度值；根据导航向量和归一化处理后的导航向量的角度值，得到控制指令。
本发明实施例提供的处理器用于运行执行以下功能的程序：通过如下公式计算得到目标设备从当前位置移动至预定位置的导航向量：预设当前位置为st＝[xt，yt，θt]，预设预定位置为star＝[xtar，ytar,θtar]，预设预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure GDA0003096839030000141
其中，xt，yt为当前位置的位置变量，θt为当前位置的方向变量，xtar，ytar为预定位置的位置变量，θtar为预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
本发明实施例提供的处理器用于运行执行以下功能的程序：通过如下方式归一化处理导航向量，得到归一化处理后的导航向量：
Figure GDA0003096839030000142
其中，Fn为归一化处理后的导航向量。
本发明实施例提供的处理器用于运行执行以下功能的程序：通过如下公式计算得到归一化处理后的导航向量的角度值：
Figure GDA0003096839030000143
其中，φ为归一化处理后的导航向量的角度值。
本发明实施例提供的处理器用于运行执行以下功能的程序：通过如下公式计算得到控制指令：
Figure GDA0003096839030000151
其中，at为目标设备在t时刻的控制指令，vlinear为目标设备移动的线速度，vangular为目标设备移动的角速度，xt，yt为目标设备的位置变量，θt为目标设备的方向变量，φ为归一化处理后的导航向量的角度值，
Figure GDA0003096839030000152
为φ的导数，Tanh为双曲正切函数，ku，kw为预先设定的控制参数，其中ku用于控制目标设备的线速度，kw用于控制目标设备的角速度。
本发明实施例提供的处理器用于运行执行以下功能的程序：在根据目标设备的当前位置和预定位置，生成控制指令之后，方法还包括：将控制指令转换为控制信号；发送控制信号至目标设备，以控制目标设备根据控制信号移动。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (11)

1.一种轨迹跟踪方法，其特征在于，包括：
获取目标设备的当前位置；
判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；
若判断出所述当前位置与所述预定位置的距离大于所述预设距离阀值，则根据所述目标设备的当前位置和所述预定位置，生成控制指令，其中，所述控制指令包括有以下信息：所述目标设备移动的角速度和线速度，所述控制指令用于控制所述目标设备根据所述角速度和所述线速度，从所述当前位置移动至所述预定位置；
其中，根据所述目标设备的当前位置和所述预定位置，生成控制指令，包括：
根据所述当前位置的位置变量和方向变量，以及所述预定位置的位置变量和方向变量，确定所述目标设备从所述当前位置移动至所述预定位置的导航向量；
归一化处理所述导航向量，得到归一化处理后的导航向量；
获取所述归一化处理后的导航向量的角度值；
根据所述导航向量和所述归一化处理后的导航向量的角度值，得到所述控制指令；
其中，通过如下公式计算得到所述目标设备从所述当前位置移动至所述预定位置的导航向量：
预设所述当前位置为st＝[xt，yt，θt)，预设所述预定位置为star＝[xtar，ytar，θtar]，预设所述预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure FDA0003096839020000011
其中，xt，yt为所述当前位置的位置变量，θt为所述当前位置的方向变量，xtar，ytar为所述预定位置的位置变量，θtar为所述预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
2.根据权利要求1所述的方法，其特征在于，在判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，所述方法还包括：
若判断出所述当前位置与所述预定位置的距离小于等于所述预设距离阀值，则确定所述目标轨迹的下一预定位置，其中，所述下一预定位置与所述当前位置的距离大于所述预设距离阀值，所述下一预定位置为所述预定位置的下一个位置；
根据所述目标设备的当前位置和所述下一预定位置生成所述控制指令，其中，所述控制指令用于控制所述目标设备根据所述角速度和所述线速度，从所述当前位置移动至所述下一预定位置。
3.根据权利要求1所述的方法，其特征在于，获取目标设备的当前位置，包括：
获取所述目标设备的位置变量和方向变量；
根据所述位置变量和所述方向变量，确定所述目标设备的当前位置。
4.根据权利要求3所述的方法，其特征在于，在所述预定位置为所述目标轨迹的初始轨迹点的情况下，在判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，所述方法还包括：
将所述目标轨迹的初始轨迹点确定为所述预定位置；
获取所述预定位置的位置变量和方向变量，其中，根据所述目标设备的位置变量和方向变量，以及所述预定位置的位置变量和方向变量，确定所述当前位置与预先设定的目标轨迹中预定位置的距离。
5.根据权利要求4所述的方法，其特征在于，通过如下方式归一化处理所述导航向量，得到归一化处理后的导航向量：
Figure FDA0003096839020000021
其中，Fn为所述归一化处理后的导航向量。
6.根据权利要求5所述的方法，其特征在于，通过如下公式计算得到所述归一化处理后的导航向量的角度值：
Figure FDA0003096839020000022
其中，φ为所述归一化处理后的导航向量的角度值。
7.根据权利要求6所述的方法，其特征在于，通过如下公式计算得到所述控制指令：
Figure FDA0003096839020000023
其中，at为所述目标设备在t时刻的控制指令，vlinear为所述目标设备移动的线速度，vangular为所述目标设备移动的角速度，xt，yt为所述目标设备的位置变量，et为所述目标设备的方向变量，φ为所述归一化处理后的导航向量的角度值，
Figure FDA0003096839020000031
为所述φ的导数，Tanh为双曲正切函数，ku，kw为预先设定的控制参数，其中ku用于控制所述目标设备的线速度，kw用于控制所述目标设备的角速度。
8.根据权利要求1至7中任意一项所述的方法，其特征在于，在根据所述目标设备的当前位置和所述预定位置，生成控制指令之后，所述方法还包括：
将所述控制指令转换为控制信号；
发送所述控制信号至所述目标设备，以控制所述目标设备根据所述控制信号移动。
9.一种轨迹跟踪装置，其特征在于，包括：
获取模块，用于获取目标设备的当前位置；
判断模块，用于判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；
控制模块，用于若判断出所述当前位置与所述预定位置的距离大于所述预设距离阀值，则根据所述目标设备的当前位置和所述预定位置，生成控制指令，其中，所述控制指令包括有以下信息：所述目标设备移动的角速度和线速度，所述控制指令用于控制所述目标设备根据所述角速度和所述线速度，从所述当前位置移动至所述预定位置；
其中，所述装置还用于根据所述当前位置的位置变量和方向变量，以及所述预定位置的位置变量和方向变量，确定所述目标设备从所述当前位置移动至所述预定位置的导航向量；归一化处理所述导航向量，得到归一化处理后的导航向量；获取所述归一化处理后的导航向量的角度值；根据所述导航向量和所述归一化处理后的导航向量的角度值，得到所述控制指令；
其中，通过如下公式计算得到所述目标设备从所述当前位置移动至所述预定位置的导航向量：
预设所述当前位置为st＝[xt，yt，θt]，预设所述预定位置为star＝[xtar，ytar，θtar]，预设所述预定位置的目标姿态向量p＝[pn，pm]T＝[cos(θtar)，sin(θtar)]T；
则，
Figure FDA0003096839020000041
其中，xt，yt为所述当前位置的位置变量，θt为所述当前位置的方向变量，xtar，ytar为所述预定位置的位置变量，θtar为所述预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。
10.一种存储介质，其特征在于，所述存储介质包括存储的程序，其中，所述程序执行权利要求1至8中任意一项所述的轨迹跟踪方法。
11.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1至8中任意一项所述的轨迹跟踪方法。
CN201710812423.7A 2017-09-11 2017-09-11 轨迹跟踪方法、装置 Active CN107703973B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201710812423.7A CN107703973B (zh) 2017-09-11 2017-09-11 轨迹跟踪方法、装置
PCT/CN2017/116856 WO2019047415A1 (zh) 2017-09-11 2017-12-18 轨迹跟踪方法和装置、存储介质、处理器

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710812423.7A CN107703973B (zh) 2017-09-11 2017-09-11 轨迹跟踪方法、装置

Publications (2)

Publication Number Publication Date
CN107703973A CN107703973A (zh) 2018-02-16
CN107703973B true CN107703973B (zh) 2021-08-31

Family

ID=61172508

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710812423.7A Active CN107703973B (zh) 2017-09-11 2017-09-11 轨迹跟踪方法、装置

Country Status (2)

Country Link
CN (1) CN107703973B (zh)
WO (1) WO2019047415A1 (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108296687B (zh) * 2018-03-05 2019-08-27 北京华航唯实机器人科技股份有限公司 轨迹调整方法及装置
WO2020000127A1 (zh) * 2018-06-25 2020-01-02 深圳市大疆创新科技有限公司 一种导航路径跟踪控制方法、设备、移动机器人及系统
CN109189060B (zh) * 2018-07-25 2021-01-12 博众精工科技股份有限公司 移动机器人的点镇定控制方法及装置
CN110286672A (zh) * 2019-05-13 2019-09-27 深圳创动科技有限公司 机器人及其导航控制方法、导航控制装置以及存储介质
CN110653821B (zh) * 2019-10-10 2023-03-24 上海电气集团股份有限公司 用于机械臂的控制方法、系统、介质及设备
CN110764051B (zh) * 2019-12-19 2020-04-07 湖南数格信息科技有限公司 一种基于uwb的轨道交通车辆定位方法、服务器及系统
CN111176292B (zh) * 2020-01-13 2022-11-22 天津工业大学 一种基于侧边距的波浪滑翔器路径跟踪控制方法
CN111474930B (zh) * 2020-04-13 2023-07-18 北京欣奕华科技有限公司 一种基于视觉定位的循迹控制方法、装置、设备及介质
CN116300409B (zh) * 2023-05-17 2023-08-15 季华实验室 一种轨迹跟踪控制方法、装置、电子设备及存储介质

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0997112A (ja) * 1995-09-28 1997-04-08 Toyota Motor Corp 軌跡制御方法および装置
JP4266211B2 (ja) * 2005-03-23 2009-05-20 株式会社東芝 ロボット装置、ロボット装置の移動方法、および、プログラム
CN101508113B (zh) * 2009-03-11 2010-10-27 哈尔滨工业大学 一种基于余弦二阶的机器人轨迹规划方法
FR2993973B1 (fr) * 2012-07-27 2016-11-04 Thales Sa Procede de traitement d'un plan de vol
EP2975483B1 (en) * 2013-03-15 2020-02-26 NEC Corporation Target pointing apparatus, target pointing control method, target pointing control program
CN105204520B (zh) * 2015-09-17 2018-07-06 深圳市十方联智科技有限公司 一种无人飞行器的控制方法
CN105045284B (zh) * 2015-09-21 2019-03-19 北京天航华创科技股份有限公司 一种抗干扰无人飞行器路径跟踪控制方法
CN105867368B (zh) * 2016-03-31 2019-03-01 纳恩博（常州）科技有限公司 一种信息处理方法和移动装置
CN105867379B (zh) * 2016-04-13 2018-09-04 上海物景智能科技有限公司 一种机器人的运动控制方法及控制系统
CN105929849B (zh) * 2016-04-28 2018-10-23 东南大学 一种基于点镇定的轮式移动机器人目标跟踪控制方法
CN106647814B (zh) * 2016-12-01 2019-08-13 华中科技大学 一种基于二维码地标识别的无人机视觉辅助定位与飞控系统及方法

Also Published As

Publication number Publication date
CN107703973A (zh) 2018-02-16
WO2019047415A1 (zh) 2019-03-14

Similar Documents

Publication Publication Date Title
CN107703973B (zh) 轨迹跟踪方法、装置
KR101978967B1 (ko) 제스처의 방향에 기초하여 상기 제스처를 인식하는 제스처 인식 장치와 제스처 인식 방법
CN109917818B (zh) 基于地面机器人的协同搜索围堵方法
CN108986801B (zh) 一种人机交互方法、装置及人机交互终端
Qian et al. Developing a gesture based remote human-robot interaction system using kinect
CN110322500A (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
US20120039507A1 (en) Information Processing Device And Information Processing Method
US10659670B2 (en) Monitoring system and control method thereof
WO2012035707A1 (ja) 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置
US10191566B1 (en) Interactive input controls in a simulated three-dimensional (3D) environment
WO2013032192A2 (ko) 천장 임의 형상 특성 활용 이동 로봇 위치 인식 방법
CN109291055A (zh) 机器人运动控制方法、装置、计算机设备和存储介质
US20220156973A1 (en) Information processing apparatus, information processing method, and program
Fujii et al. Gesture recognition system for human-robot interaction and its application to robotic service task
EP3782119A1 (en) Detection, tracking and 3d modeling of objects with sparse rgb-d slam and interactive perception
EP2899706B1 (en) Method and system for analyzing human behavior in an intelligent surveillance system
CN110303499A (zh) 一种机械臂归位方法及装置
CN105509748B (zh) 机器人的导航方法及装置
CN112580582A (zh) 动作学习方法、装置、介质及电子设备
US9460368B2 (en) Method of learning a parameter to estimate posture of an articulated object and method of estimating posture of an articulated object
CN111452039B (zh) 动态系统下机器人姿态调整方法、装置、电子设备及介质
US10198084B2 (en) Gesture control device and method
CN109531578B (zh) 仿人机械手臂体感控制方法及装置
JP2015114933A (ja) 物体認識装置、ロボット及び物体認識方法
CN115862074A (zh) 人体指向确定、屏幕控制方法、装置及相关设备

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