CN115157249A - 一种动态目标跟踪方法、装置和机器人 - Google Patents

一种动态目标跟踪方法、装置和机器人 Download PDF

Info

Publication number
CN115157249A
CN115157249A CN202210804647.4A CN202210804647A CN115157249A CN 115157249 A CN115157249 A CN 115157249A CN 202210804647 A CN202210804647 A CN 202210804647A CN 115157249 A CN115157249 A CN 115157249A
Authority
CN
China
Prior art keywords
dynamic target
predicted position
robot
motion
motion state
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
Application number
CN202210804647.4A
Other languages
English (en)
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.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN202210804647.4A priority Critical patent/CN115157249A/zh
Publication of CN115157249A publication Critical patent/CN115157249A/zh
Priority to US18/217,662 priority patent/US20240009841A1/en
Pending legal-status Critical Current

Links

Images

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
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40519—Motion, trajectory planning

Abstract

本申请提供一种动态目标跟踪方法、装置和机器人，该方法包括：实时获取跟踪的动态目标的运动状态；根据当前时刻的运动状态进行运动预测，得到动态目标的预测位置；对预测位置进行滞后补偿，得到补偿后的预测位置；根据补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；根据多步优化轨迹，控制机器人关节进行相应运动。该方法可以有效减小机器人在跟踪动态目标时的滞后问题等。

Description

一种动态目标跟踪方法、装置和机器人
技术领域
本申请涉及机器人运动规划技术领域，尤其涉及一种动态目标跟踪方法、装置和机器人。
背景技术
机器人进行动态目标的跟踪时是利用在线轨迹规划来实现的，目前主要是根据对跟踪目标的误差信息来驱动在线规划器产生在线轨迹。但在动态目标的跟踪时，由于机器人上所安装的视觉在采集及计算等会存在一定的延迟，即存在较低的视觉反馈频率，使得在线规划轨迹相对于动态目标具有明显的滞后误差。
发明内容
有鉴于此，本申请实施例提供一种动态目标跟踪方法、装置和机器人，可以有效减小机器人在跟踪动态目标时的滞后问题等。
第一方面，本申请实施例提供一种动态目标跟踪方法，包括：
实时获取跟踪的动态目标的运动状态；
根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置；
对所述预测位置进行滞后补偿，得到补偿后的预测位置；
根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；
根据所述多步优化轨迹，控制机器人关节进行相应运动。
在一些实施例中，所述运动状态包括所述动态目标在当前时刻的实际位置、实际速度及实际加速度，所述根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置，包括：
将所述动态目标在预设短时间内的运动视为一个匀加速运动，以分别计算所述动态目标以所述实际速度运动预设短时间所产生的第一位移、以及以所述实际加速度运动预设短时间所产生的第二位移；
根据所述动态目标在当前时刻的所述实际位置、所述第一位移和所述第二位移，计算得到所述动态目标的预测位置。
在一些实施例中，所述对所述预测位置进行滞后补偿，包括：
根据已规划的当前时刻的运动位置与所述实际位置，计算当前时刻对应的滞后误差，并将当前时刻的所述滞后误差和上一时刻的滞后误差进行累积，得到累积滞后误差；将所述累积滞后误差与所述预测位置叠加。
在一些实施例中，所述根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态规划量，包括：
基于所述补偿后的预测位置，计算得到当前时刻的关节运动状态量；
根据所述当前时刻的关节运动状态量和基于当前时刻的状态量与下一时刻的状态量之间的关系所构建的轨迹预测模型，计算得到对未来连续时刻进行轨迹规划的多步关节运动状态的规划量。
在一些实施例中，所述多步优化目标函数的构建包括：
以将机器人关节的多步位置和多步速度带回原点为第一目标，以将跟踪动态目标时的多步位置误差最小为第二目标，构建多步优化目标函数，其中，所述多步优化目标函数的约束条件包括机器人关节的位置、速度、加速度、以及位置误差满足的相关约束。
在一些实施例中，所述多步优化目标函数的表达式如下：
Figure BDA0003736330480000031
式中，Pk为k步位置规划量构成的位置矩阵，Vk为k步速度规划量构成的速度矩阵，Ak为k步加速度规划量构成的加速度矩阵，
Figure BDA0003736330480000032
为k步补偿后的预测位置构成的；ω1、ω2和ω3分别为三个预设权重。
在一些实施例中，所述动态目标的预测位置的计算公式如下：
Figure BDA0003736330480000033
式中，ppro为所述动态目标的预测位置，pfb、vfb和afb分别为所述动态目标在当前时刻的实际位置、实际速度及实际加速度，dt表示预设短时间。
第二方面，本申请实施例还提供一种动态目标跟踪装置，包括：
获取模块，用于实时获取跟踪的动态目标的运动状态；
运动预测模块，用于根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置；
空间补偿模块，用于对所述预测位置进行滞后补偿，得到补偿后的预测位置；
轨迹规划模块，用于根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；
关节控制模块，用于根据所述多步优化轨迹，控制机器人关节进行相应运动。
第三方面，本申请实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的动态目标跟踪方法。
第四方面，本申请实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的机器人跟踪动态目标的控制方法。
本申请的实施例具有如下有益效果：
本申请提出的动态目标跟踪方法通过实时获取跟踪的动态目标的运动状态，并对动态目标进行短时间的运动预测，得到动态目标的预测位置；再利用产生的滞后对预测位置进行滞后补偿，得到补偿后的预测位置；接着根据补偿后的预测位置利用模型预测思路进行轨迹在线规划，确定多步关节优化轨迹；最后根据多步优化轨迹来控制机器人的关节运动，以实现动态目标的实时跟踪。该方法通过对动态目标进行时间上的运动预测，再进行空间上的滞后补偿，同时结合模型预测进行在线轨迹优化，可以有效减小机器人在跟踪动态目标时的滞后问题，还得到顺滑的运动轨迹，从而提高机器人的稳定性等。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本申请实施例的机器人的一种结构示意图；
图2示出了本申请实施例的动态目标跟踪方法的流程图；
图3示出了本申请实施例的动态目标跟踪方法的运动预测的流程图；
图4示出了本申请实施例的动态目标跟踪方法的滞后补偿的流程图；
图5示出了本申请实施例的动态目标跟踪装置的结构示意图。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
除非另有限定，否则这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。
请参照图1，为本申请实施例提出的机器人的结构示意图。示范性地，机器人包括存储器11、处理器12和感知单元13，其中，存储器11和感知单元13均与处理器12连接，存储器11存储有相应的计算机程序，处理器12用于执行该计算机程序以实施本申请实施例中的动态目标跟踪方法，可以有效解决现有技术中机器人在进行动态目标跟踪时的滞后问题等。
其中，存储器11可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-OnlyMemory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存储器11用于存储计算机程序，处理器12在接收到执行指令后，可相应地执行所述计算机程序。
其中，处理器12可以是一种具有信号的处理能力的集成电路芯片。处理器12可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、图形处理器(GraphicsProcessing Unit，GPU)及网络处理器(Network Processor，NP)、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件中的至少一种。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。
其中，感知单元13主要指在机器人身上设置的传感器，这些传感器用于将必要的外部信息以及自身状态信息传递给机器人的控制系统，从而为机器人的决策提供必要的条件。在一些情况下，这些传感器可分为内部传感器和外部传感器，其中，内部传感器用于测量机器人自身状态的功能元件，其功能是测量运动学量和力学量，用于机器人感知自身的运动状态，使得机器人可以按照规定的位置、轨迹和速度等参数运动。而外部传感器主要是感知机器人自身和环境之间的相互信息，如包括视觉等信息，具体可包括但不限于接近传感器、视觉传感器等。例如，在本实施例中，通过视觉反馈得到机器人跟踪的动态目标的实时位置、速度、加速度等运动状态。应当明白的是，上述列举的这些传感器可根据机器人的实际应用场景而选择性设置。
可以理解，本实施例中，该机器人可以但不限于为，工业机械臂、外科用手术机械臂、移动机器人等，以移动机器人为例，可以是具有滚动轮的轮式机器人，如巡检机器人等，也可以是具有双足、三足及以上的足式机器人等，即这里的机器人是广义机器人，其存在形式并不作限定。而对于本实施例的方法主要运用于各种需要进行动态目标跟踪的应用场景，例如，可包括但不限于，移动机器人进行移动目标的实时跟随，工业机械臂进行动态打磨操作、动态轨迹跟踪操作等，又或是手术机械臂进行动态切割操作等。可以理解的是，作为被跟踪的对象，本实施例中的被跟踪的目标的状态是运动变化的。而且本实施例的方法还可以运用如车辆等跟踪动态目标的场景等。
基于上述的机器人结构，本申请实施例提出一种动态目标跟踪方法，该方法通过对动态目标进行时间上的运动预测，再进行空间上的滞后补偿，同时结合模型预测进行在线轨迹优化，不仅可以有效减小机器人在跟踪动态目标时的滞后问题，还可以得到顺滑的运动轨迹，从而提高系统稳定性等。
请参照图2，示范性地，该动态目标跟踪方法包括步骤S110～S150：
S110，实时获取跟踪的动态目标的运动状态。
其中，动态目标的运动状态可包括但不限于，每个运动时刻的实际位置和姿态(即位姿)、实际速度、实际加速度、实际加加速度等。例如，机器人可以通过如视觉反馈、距离感知等方式来直接或间接计算得到该动态目标的运动状态，进而根据这些运动状态信息来进行动态目标跟踪。可以理解，在理想情况下，机器人的运动状态应与动态目标的运动状态保持一致，从而实现实时跟踪。
以视觉反馈为例，在进行动态目标的跟踪时，由于机器人获得的视觉反馈频率较低，使得视觉反馈的位姿往往不能及时等于动态目标的实时位姿，导致基于视觉反馈的位姿得到的在线规划轨迹相对于真实的动态目标存在一定的滞后误差，为此，本实施例提出先对动态目标进行位置预测，这样可以解决因视觉反馈等不及时引起的时间上的误差等。
S120，根据当前时刻的运动状态进行运动预测，得到动态目标的预测位置。
其中，运动预测是指将动态目标在短时间内的运动视为一个匀加速运动，进而预测得到其将来的运动位置，即上述的预测位置。示范性地，在获得该动态目标的实际位置后，可通过对实时位置进行一次求导、二次求导，或者利用相邻几帧实际位置离散计算等，以获得当前时刻该动态目标的速度、加速度等状态量。然后，利用这些状态量结合运动预测原理，以计算得到该动态目标在下一时刻的预测位置。
在一种实施方式中，如图3所示，步骤S120包括以下子步骤：
S210，将动态目标在预设短时间内的运动视为一个匀加速运动，以分别计算动态目标以实际速度运动预设短时间所产生的第一位移、以及以实际加速度运动预设短时间所产生的第二位移。S220，根据动态目标在当前时刻的实际位置、第一位移和第二位移，计算得到动态目标的预测位置。
若以表达式描述，则该动态目标的预测位置的计算公式如下：
Figure BDA0003736330480000091
式中，ppro为动态目标的预测位置，pfb、vfb和afb分别为动态目标在当前时刻的实际位置、实际速度及实际加速度，dt表示预设短时间，例如，该短时间可以是指一个运动时刻或一个控制指令周期等。可以理解，上述公式中，等号右边第一项即为当前时刻的实际位置，第二项和第三项分别为在预设的短时间内产生的第一位移和第二位移。
考虑到若将该预测位置直接进行在线规划，由于机器人自身等结构或其他因素等，导致其规划的运动位置相对于反馈的实际位置还会存在一定的误差，为此，本实施例还将提出在机器人运动空间中进行滞后补偿。
S130，对预测位置进行滞后补偿，得到补偿后的预测位置。
其中，这里的滞后补偿主是指利用已规划的运动位置与机器人的实际位置之间产生的位置滞后误差对上述预测位置进行补偿。
在一种实施方式中，如图4所示，步骤S130包括以下子步骤：
S310，根据已规划的当前时刻的运动位置与实际位置，计算当前时刻对应的滞后误差；S320，将当前时刻的滞后误差和上一时刻的滞后误差进行累积，得到累积滞后误差；S330，将累积滞后误差与预测位置叠加。
例如，若在线规划器已规划输出的当前时刻t的运动位置为pexp，视觉反馈得到的实际位置为pfb，则当前时刻t对应的滞后误差et为两者之间的偏差，其中et＝pexp-pfb。此外，在当前时刻t之前还存在累计的历史滞后误差，于是，可采用如下公式来计算该累积滞后误差：
Figure BDA0003736330480000101
式中，
Figure BDA0003736330480000102
为当前时刻的累积滞后误差，
Figure BDA0003736330480000103
为上一时刻的滞后误差，et为当前时刻的滞后误差，w为预设的权重值。
于是，将已产生的累积滞后误差
Figure BDA0003736330480000104
补偿到对动态目标的预测位置ppro上，可以得到补偿后的预测位置。然后，将补偿后的预测位置作为输入，带入到在线规划器中进行在线规划，以得到机器人跟踪动态目标的规划轨迹。
S140，根据补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹。
本实施例中，将利用模型预测控制思想来实现在线轨迹规划，具体而言，采用多步规划加多步优化的方式来实现，其中，多步规划主要是通过轨迹预测模型预测出未来多个时刻机器人关节的运动状态，也即规划量；多步优化则在于通过优化函数求解出未来一组状态控制量，以使在未来多个时刻的实际输出尽可能地接近由参考轨迹所确定的期望输出。
示范性地，在进行在线规划之前，可先基于补偿后的预测位置，计算得到当前时刻的关节运动状态量；然后，根据当前时刻的关节运动状态量和预先构建的轨迹预测模型，计算得到对未来连续时刻进行轨迹规划的多步关节运动状态的规划量。其中，该轨迹预测模型可基于当前时刻的状态量与下一时刻的状态量之间的关系构建得到，例如，轨迹预测模型的表达式为：
Figure BDA0003736330480000111
Figure BDA0003736330480000112
ai+1＝ai+jidt
式中，ai、vi、pi均为第i个时刻(当前时刻)的状态量，依次表示加速度、速度、位置；ji为当前时刻的加加速度，其可通过对加速度求导得到；同理，ai+1、vi+1、pi+1依次表示第i+1个时刻(下一时刻)的加速度、速度、位置。
可以理解，通过上述轨迹预测模型进行多次迭代，可以实现多步位置、多步速度和多步加速度的预测，这里也称为规划量。假设需要预测未来k个时刻的关节运动状态，则可将预测输出的k步的运动状态通过矩阵形式表示，具体如下：
Pk＝TpJk+Bp；
Vk＝TvJk+Bv；
Ak＝TaJk+Ba；
其中，多步位置矩阵Pk＝[pi pi+1 pi+2···pi+k]T，多步速度矩阵Vk＝[vi vi+1vi+2···vi+k]T，多步加速度矩阵Ak＝[ai ai+1 ai+2···ai+k]T，多步加加速度矩阵Jk＝[JiJi+1 Ji+2···Ji+k]T，Tp、Bp、Tv、Bv、Ta、Ba分别为对应的参数矩阵，具体可由轨迹预测模型中的相应参数得到，这里不展开描述。值得注意的是，本实施例是将补偿后的预测位置(记为pobj)输入到轨迹预测模型中，故上述的多步位置矩阵又表示为
Figure BDA0003736330480000121
本实施例中，通过利用轨迹预测模型进行多步规划，然后将得到的多步规划量作为输入，带入到预先构建的多步优化目标函数中进行最优求解，从而确定一组最优轨迹参数，用于机器人关节控制。
其中，该多步优化目标函数的构建取决于设置的优化目标，当优化目标的数量或对象不同时，则构建的目标函数也会存在区别。在一种实施方式中，以两个优化目标为例，其中，以将机器人关节的多步位置和多步速度带回原点为第一目标，以将跟踪动态目标时的多步位置误差最小为第二目标，构建该多步优化目标函数。此时，多步优化目标函数的表达式可为：
Figure BDA0003736330480000122
式中，Pk为k步位置规划量构成的位置矩阵，Vk为k步速度规划量构成的速度矩阵，Ak为k步加速度规划量构成的加速度矩阵，
Figure BDA0003736330480000123
为k步补偿后的预测位置构成的；ω1、ω2和ω3分别为三个预设权重，例如，各预设权重可从0～1内进行取值，这里不作限定。
可以理解，基于上述两个优化目标所构建的多步优化目标函数所求解出的状态控制量，能够实现兼容跟踪时的较小滞后及轨迹平滑等。值得注意的是，该多步优化目标函数并不仅限于上述一种表达式，在另一种实施方式中，该多步优化目标函数还可以根据其他任务或其他函数类型来构建。例如，以任务为例，优化目标还可以是以跟踪动态目标时的能量损耗最小等；又例如，以函数类型为例，还可以采用各状态量与相应控制量之间的差值的平方等函数来构建上述多步优化目标函数，这里不作限定。
进而，为求解一组最优控制量，还需要设置该多步优化目标函数的约束条件，例如，可包括但不限于包括，机器人各关节的运动位置、速度、加速度、加加速度等的物理约束(如不能达到的上下限)等，此外还可以根据实际需求添加更多的约束条件，这里不作限定。
S150，根据该多步优化轨迹，控制机器人关节进行相应运动。
示范性地，当通过多步优化目标函数求解得到一组最优的控制量后(也就是上述未来k步的控制输出)，机器人将其作为在线规划的轨迹参数，来计算关节控制指令，然后进行关节电机控制，从而使机器人按照规划的轨迹进行运动。此外，机器人继续以设定的频率反馈当前动态目标的运动状态，而这个运动状态又会被重新用于执行本实施例的步骤S120～S150，如此循环，直到完成或停止当前的动态目标跟踪任务。可以理解，本实施例进行轨迹在线规划时，是每执行完一步都会按照上述步骤进行一次计算，由此得到兼顾较小滞后和轨迹光顺的轨迹。
本实施例的动态目标跟踪方法通过实时获取跟踪的动态目标的运动状态，并对动态目标进行短时间的运动预测，得到动态目标的预测位置；再利用产生的滞后对预测位置进行空间滞后补偿，得到补偿后的预测位置；接着根据补偿后的预测位置利用模型预测思路进行轨迹在线规划，确定多步关节优化轨迹；最后根据多步优化轨迹来控制机器人的关节运动，以实现动态目标的实时跟踪，不仅可以有效减小机器人在跟踪动态目标时的滞后问题，还可以得到顺滑的运动轨迹，从而提高机器人的稳定性等。
请参照图5，基于上述实施例的方法，本实施例提出一种动态目标跟踪装置100，示范性地，动态目标跟踪装置100包括：
获取模块110，用于实时获取跟踪的动态目标的运动状态。例如，该所述运动状态可包括但不限于包括，动态目标在当前时刻的实际位置、实际速度及实际加速度等。
运动预测模块120，用于根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置。
空间补偿模块130，用于对所述预测位置进行滞后补偿，得到补偿后的预测位置；
轨迹规划模块140，用于根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；
关节控制模块150，用于根据所述多步优化轨迹，控制机器人关节进行相应运动。
在一种实施方式中，运动预测模块120包括位移计算子模块和预测计算子模块，位移计算子模块用于将动态目标在预设短时间内的运动视为一个匀加速运动，以分别计算动态目标以实际速度运动预设短时间所产生的第一位移、以及以实际加速度运动预设短时间所产生的第二位移；而预测计算子模块用于根据动态目标在当前时刻的实际位置、第一位移和第二位移，计算得到动态目标的预测位置。
在一种实施方式中，空间补偿模块130包括误差计算子模块和补偿叠加子模块，其中，误差计算子模块用于根据已规划的当前时刻的运动位置与实际位置，计算当前时刻对应的滞后误差，并将当前时刻的滞后误差和上一时刻的滞后误差进行累积，得到累积滞后误差；补偿叠加子模块用于将累积滞后误差与预测位置叠加。
可以理解，本实施例的装置对应于上述实施例的方法，上述实施例中的可选项同样适用于本实施例，故在此不再重复描述。
此外，本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (10)

1.一种动态目标跟踪方法，其特征在于，包括：
实时获取跟踪的动态目标的运动状态；
根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置；
对所述预测位置进行滞后补偿，得到补偿后的预测位置；
根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；
根据所述多步优化轨迹，控制机器人关节进行相应运动。
2.根据权利要求1所述的动态目标跟踪方法，其特征在于，所述运动状态包括所述动态目标在当前时刻的实际位置、实际速度及实际加速度，所述根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置，包括：
将所述动态目标在预设短时间内的运动视为一个匀加速运动，以分别计算所述动态目标以所述实际速度运动预设短时间所产生的第一位移、以及以所述实际加速度运动预设短时间所产生的第二位移；
根据所述动态目标在当前时刻的所述实际位置、所述第一位移和所述第二位移，计算得到所述动态目标的预测位置。
3.根据权利要求2所述的动态目标跟踪方法，其特征在于，所述对所述预测位置进行滞后补偿，包括：
根据已规划的当前时刻的运动位置与所述实际位置，计算当前时刻对应的滞后误差，并将当前时刻的所述滞后误差和上一时刻的滞后误差进行累积，得到累积滞后误差；将所述累积滞后误差与所述预测位置叠加。
4.根据权利要求1所述的动态目标跟踪方法，其特征在于，所述根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态规划量，包括：
基于所述补偿后的预测位置，计算得到当前时刻的关节运动状态量；
根据所述当前时刻的关节运动状态量和基于当前时刻的状态量与下一时刻的状态量之间的关系所构建的轨迹预测模型，计算得到对未来连续时刻进行轨迹规划的多步关节运动状态的规划量。
5.根据权利要求4所述的动态目标跟踪方法，其特征在于，所述多步优化目标函数的构建包括：
以将机器人关节的多步位置和多步速度带回原点为第一目标，以将跟踪动态目标时的多步位置误差最小为第二目标，构建多步优化目标函数，其中，所述多步优化目标函数的约束条件包括所述机器人关节的位置、速度、加速度满足的物理约束。
6.根据权利要求5所述的动态目标跟踪方法，其特征在于，所述多步优化目标函数的表达式如下：
Figure FDA0003736330470000021
式中，Pk为k步位置规划量构成的位置矩阵，Vk为k步速度规划量构成的速度矩阵，Ak为k步加速度规划量构成的加速度矩阵，
Figure FDA0003736330470000022
为k步补偿后的预测位置构成的；ω1、ω2和ω3分别为三个预设权重。
7.根据权利要求1或2所述的动态目标跟踪方法，其特征在于，所述动态目标的预测位置的计算公式如下：
Figure FDA0003736330470000031
式中，ppro为所述动态目标的预测位置，pfb、vfb和afb分别为所述动态目标在当前时刻的实际位置、实际速度及实际加速度，dt表示预设短时间。
8.一种动态目标跟踪装置，其特征在于，包括：
获取模块，用于实时获取跟踪的动态目标的运动状态；
运动预测模块，用于根据当前时刻的所述运动状态进行运动预测，得到所述动态目标的预测位置；
空间补偿模块，用于对所述预测位置进行滞后补偿，得到补偿后的预测位置；
轨迹规划模块，用于根据所述补偿后的预测位置进行轨迹在线规划，得到未来时刻的多步关节运动状态的规划量，并结合多步优化目标函数，确定多步优化轨迹；
关节控制模块，用于根据所述多步优化轨迹，控制机器人关节进行相应运动。
9.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的动态目标跟踪方法。
10.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-7中任一项所述的动态目标跟踪方法。
CN202210804647.4A 2022-07-08 2022-07-08 一种动态目标跟踪方法、装置和机器人 Pending CN115157249A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210804647.4A CN115157249A (zh) 2022-07-08 2022-07-08 一种动态目标跟踪方法、装置和机器人
US18/217,662 US20240009841A1 (en) 2022-07-08 2023-07-03 Dynamic target tracking method, robot and computer-readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210804647.4A CN115157249A (zh) 2022-07-08 2022-07-08 一种动态目标跟踪方法、装置和机器人

Publications (1)

Publication Number Publication Date
CN115157249A true CN115157249A (zh) 2022-10-11

Family

ID=83492640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210804647.4A Pending CN115157249A (zh) 2022-07-08 2022-07-08 一种动态目标跟踪方法、装置和机器人

Country Status (2)

Country Link
US (1) US20240009841A1 (zh)
CN (1) CN115157249A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116125819A (zh) * 2023-04-14 2023-05-16 智道网联科技(北京)有限公司 轨迹修正方法、装置、电子设备和计算机可读存储介质

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116125819A (zh) * 2023-04-14 2023-05-16 智道网联科技(北京)有限公司 轨迹修正方法、装置、电子设备和计算机可读存储介质

Also Published As

Publication number Publication date
US20240009841A1 (en) 2024-01-11

Similar Documents

Publication Publication Date Title
CN112775976B (zh) 任务执行控制方法、装置、控制设备及可读存储介质
JP6610915B2 (ja) 車両を制御するコントローラー及び方法並びに非一時的コンピューター可読メモリ
Do Formation tracking control of unicycle-type mobile robots with limited sensing ranges
US20170241790A1 (en) Path plan generating apparatus for mobile body
CN110546459A (zh) 具有数据融合的机器人跟踪导航
US11300663B2 (en) Method for predicting a motion of an object
CN105928514A (zh) 基于图像与惯性技术的agv复合导引系统
JP2018177121A (ja) 自動運転システム
JP5679121B2 (ja) ロボットの運動予測制御方法と装置
Vladareanu et al. The navigation of mobile robots in non-stationary and non-structured environments
CN115157249A (zh) 一种动态目标跟踪方法、装置和机器人
CN114274147B (zh) 目标跟踪控制方法及装置、机械臂控制设备和存储介质
US11106184B2 (en) Control system
CN114967465A (zh) 轨迹规划方法、装置、电子设备及存储介质
KR101981641B1 (ko) 다중 이동 로봇의 대형 제어 방법 및 시스템
CN114115295B (zh) 机器人运动规划方法及装置、机器人控制设备和存储介质
US20230191604A1 (en) Robot stability control method, robot and computer-readable storage medium
Wagner et al. Path planning and tracking for robots based on cubic hermite splines in real-time
CN113799116A (zh) 直接力反馈控制方法、装置、控制器和机器人
Izzo et al. Nonlinear model predictive control applied to vision-based spacecraft landing
Weckx et al. Open experimental AGV platform for dynamic obstacle avoidance in narrow corridors
Vladareanu et al. The petri nets and Markov chains approach for the walking robots dynamical stability control
Abichandani et al. Experimental multi-vehicle path coordination under communication connectivity constraints
Vladareanu et al. The navigation mobile robot systems using Bayesian approach through the virtual projection method
Schperberg et al. Risk-averse MPC via visual-inertial input and recurrent networks for online collision avoidance

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