CN113687659B - 一种基于数字孪生的最优轨迹生成方法及系统 - Google Patents

Abstract

本发明公开了一种基于数字孪生的最优轨迹生成方法及系统，系统包括：数据传输接口、数据交互接口、轨迹规划模块和数字映射模块；所述轨迹规划模块包括：规划器组件和机器人物理模型组件；所述数字映射模块包括：环境模型和代理模型；轨迹规划模块的主要功能是根据规划任务生成对应的最优控制问题，并通过数值算法求解该问题；数字映射模块的主要功能之一是快速生成无碰撞且满足机器人动力学约束的控制量轨迹和状态量轨迹；功能之二是快速实现目标轨迹的偏差评估与碰撞检测。有益效果是：能够在提高工作精度、效率的同时兼顾能耗；能够应用于运动速度更高、控制精度要求更严苛的工作场景；能够应付控制实时性要求较高的工况，且具有持续优化性。

Description

一种基于数字孪生的最优轨迹生成方法及系统

技术领域

本发明涉及最优控制与数字孪生领域，尤其涉及一种基于数字孪生的最优轨迹生成方法及系统。

背景技术

运动规划可以分为路径规划和轨迹规划，其中路径规划是指计算出从初始状态运动到目标状态的整个无碰撞的状态转移过程；而轨迹规划则是在路径规划的基础上为每个中间状态添加了到达时间信息，即计算出所有状态变化的时间历程。路径规划的结果需要由控制器的路径跟踪方法实现，而轨迹规划通常可以直接或间接计算出控制律。

目前比较热门的路径规划方法包括以A*、D*及其变种为代表的启发式搜索算法，该算法是基于网格的搜索方法，适用于低维度场景如二维无人车规划任务；以及以随机路线图（PRM）、快速拓展随机树算法(RRT)及其变种为代表的随机采样法，该算法是基于采样的方法，适用于高维场景的规划任务如多自由度工业机器人抓取任务。上述规划算法的设计主要考虑了完备性，即如果存在可行解，是否一定能将其返回，而对于规划结果的最优性考虑不足。此外，上述算法通常只考虑被控系统的运动学特性而忽略了动力学约束，使得其规划结果难以适用于高速以及精度要求高的使用场景。最后，上述算法在搜索之前需要构建搜索图，为了保证算法的完备性，必须构建出足够细粒度的搜索图，这也成为制约上述算法适用性的重要因素。

为了突出或兼顾机器人在工作精度、效率以及能耗等方面的性能表现，可以将运动规划问题表示为最优控制问题，如时间最短或能量损耗最少。最优控制的目标是从可行解集中获得问题的最优解，其求解结果是状态量、控制量以及其它变量的时间历程，即目标运动轨迹以及控制量轨迹；因此，最优控制方法与传统轨迹规划方法的一大区别在于，最优控制方法能够直接获得控制量轨迹。最优控制在问题描述中包含了被控系统的物理模型，因此由该方法得到的控制律符合动力学约束，从而能够满足更高的控制精度和控制速度。尽管基于最优控制方法的轨迹规划具有诸多优点，然而最优控制方法的数值求解过程通常涉及大规模非线性规划问题求解，因此具有较大的计算量，难以应用于规划周期严格受限的快速路径规划场景。由此可知，提高最优控制方法的求解效率是提升该方法实用性的必要途径。

工业或轮式机器人的运动规划不仅是求解出任务相关的控制量轨迹和状态量轨迹，还需要保证其运动过程满足碰撞检测要求。传统的运动规划方法依次执行路径规划程序、碰撞检测程序；又或是引入“剪枝”操作交替的执行两道程序，属于一种“在线”的碰撞检测方式，造成了一定的时间开销。最优控制方法则需要预知碰撞可能发生的位置或时刻，然后通过给目标函数添加惩罚项或分阶段的方式以避开碰撞点；然而针对不同的规划任务，预知碰撞是不现实的，也就无法将其显式地集成到最优控制的问题描述中，只能通过额外的方式保证机器人与障碍物的间距；因此目前基于最优控制方法的轨迹规划难以适用于障碍物密集的工作场景。

发明内容

本发明提供一种适用于工业或轮式机器人的轨迹规划系统，该系统基于最优控制方法，可生成满足机器人动力学约束的无碰撞状态量轨迹与控制量轨迹，能够应用于运动速度更高、控制精度要求更严苛的工作场景；

为了进一步提高上述系统的计算效率，该系统引入了一个数字映射模块以实现最优控制问题的高效求解，以及实现“离线”式碰撞检测；本发明还提供一种“敏捷”优化机制，支持对目标轨迹多轮优化的同时，快速响应外部输出请求。

本发明提供的一种基于数字孪生的最优轨迹生成方法及系统，系统包括：数据传输接口、数据交互接口、轨迹规划模块和数字映射模块；

进一步地，所述轨迹规划模块包括：规划器组件和机器人物理模型组件；所述数字映射模块包括：环境模型和代理模型。

进一步地，所述规划器组件将轨迹规划任务转换为最优控制问题，并集成最优控制器求解该最优控制问题。

进一步地，所述机器人物理模型组件具体指通过物理建模手段获得的机器人动力学方程，且该动力学方程兼容所述最优控制器。

进一步地，所述环境模型具体指机器人真实工作环境的数字化映射。

所述代理模型，由深度神经网络表示，且所述深度神经网络通过强化学习算法离线训练，将机器人状态映射为动作。

所述一种基于数字孪生的最优轨迹生成方法采用敏捷轨迹优化，其实现原理具体为：

S101：外围设备通过所述数据交互接口向所述轨迹规划模块下发轨迹规划任务；

S102：轨迹规划模块根据所述轨迹规划任务生成最优控制问题，并进一步将所述轨迹规划任务下发至所述数字映射模块；

S103：在指定规划周期T内，所述数字映射模块根据所述轨迹规划任务生成机器人第一控制量轨迹和第一状态量轨迹；所述第一控制量轨迹和所述第一状态量轨迹构成初始无碰撞轨迹；

S104：所述轨迹规划模块基于所述初始无碰撞轨迹设置所述最优控制问题中机器人控制变量和状态变量的初始值，并求解所述最优控制问题，生成第二控制量轨迹和第二状态量轨迹；

S105：所述轨迹规划模块将第二控制量轨迹和第二状态量轨迹下发至所述数字映射模块；

S106：所述数字映射模块对第二控制量轨迹和第二状态量轨迹进行误差评估以及碰撞检测；若第二控制量轨迹和第二状态量轨迹同时通过误差评估及碰撞检测，则将第二控制量轨迹和第二状态量轨迹作为第一可接收轨迹；

S107：若规划周期T未结束，则将第一可接收轨迹作为最优控制问题中机器人控制变量和状态变量新的初始值，重新配置新的最优控制问题，重复步骤S104~S106，生成第二、第三、...、第N可接收轨迹，直至规划周期T结束，或外界请求到达时，最后的一条可接收轨迹，即为最终机器人规划轨迹；

S108：所述轨迹规划模块将所述最终机器人规划轨迹通过所述数据传输接口下发至机器人本体，机器人本体根据所述最终机器人规划轨迹执行动作。

本发明提供的有益效果是：不仅能提供机器人运动过程中的状态转移历程，还能直接计算出控制量轨迹，因此可作为一种机器人的前馈控制。采用最优控制方法规划的控制量轨迹与状态量轨迹具有一定的最优性质，能够在提高工作精度、效率的同时兼顾能耗；并且该系统提供的轨迹规划模块与数字映射模块相互搭配，实现了一种“敏捷”轨迹优化方案，该方案一方面能够满足系统对实时性的要求；另一方面持续改进生成轨迹，有效提高性能表现。

附图说明

图1是本发明系统结构示意图；

图2为本系统中强化学习算法训练示意图；

图3是本发明方法的原理实现流程示意图；

图4是敏捷轨迹优化的流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。

请参考图1，图1是本发明应用于工业机器人的系统结构图；本发明提供一种基于数字孪生的最优轨迹生成系统，包括：数据传输接口、数据交互接口、轨迹规划模块和数字映射模块；

所述轨迹规划模块包括：规划器组件和机器人物理模型组件；所述数字映射模块包括：环境模型和代理模型；

其中，数据传输接口的主要功能之一是获取机器人当前状态信息，如关节角位移、角速度、角加速度等，这些状态信息将用于标定机器人物理模型组件和环境模型；功能之二是将轨迹规划结果传输至机器人驱动控制系统。

数据交互模块的主要功能之一是接收外围设备设定的规划任务；所述外围设备为示教器、上位机或上游决策系统中的一种或者两种。功能之二是辅助示教器或上位机读取以及配置数字孪生系统中的功能参数，如非线性规划算法、最大迭代次数、相对误差、神经网络学习率、时间分辨率等。

作为一种实施例，所述规划器是规划任务的求解工具，它将规划任务转换为最优控制问题，并集成最优控制求解器以求解该问题。

如前所述，规划器获取规划任务的方式包括示教器、上位机以及上游决策系统。示教器与上位机可以设置规划器生成最优控制的形式，如设定当前规划任务为时间最优或能量最优，同时也能设置最优控制求解算法的可调参数，如优化算法、非线性规划求解算法、最大迭代次数、误差精度等等。

作为一种实施例，机器人物理模型是通过物理建模手段得到的机器人的动力学方程，具体地说，可以采用任意方式对机器人建模，但最后得到的物理模型的数学形式必须能够兼容规划器中实现的最优控制求解器。

作为一种实施例，机器人物理模型用于生成最优控制问题中的动力学等式约束，其数学形式如下式：

（1）

其中，x表示状态变量，f()表示状态方程式,u表示控制变量,t表示时间；

表示状 态变量导数；进一步地，图1中物理模型中公式y=g(x,u,t)，其中g()表示输出方程式；y表 示输出变量；

作为一种实施例，环境模型是真实工作环境的数字化映射，模拟了当前机器人的工作环境，包括当前机器人本体模型、障碍物模型、工件模型以及其它与工作相关的模型。环境模型基于某种物理引擎如Gazebo、Unity3D、Bullet等构建，而其中的机器人本体、障碍物等模型则通过urdf文件或sdf文件进行描述。

代理模型由深度神经网络表示，可实现将机器人的状态映射为控制动作。根据机器人类型以及工作内容的不同，深度神经网络的结构需要进行针对性地设计，并通过强化学习算法离线训练。

作为一种实施例，本系统采用强化学习算法训练代理模型。机器人控制变量通常为连续实型，因此采用的强化学习算法要求能生成连续动作策略，可选的算法包括但不限于DDPG、SAC、PPO、TRPO等等。

强化学习算法的训练采用虚实结合的方式，图2为本系统中强化学习算法训练示意图。

根据规划任务，代理模型将当前的状态映射为下一次执行的控制动作，该控制动作驱动环境模型中的机器人本体模型；机器人本体模型随后发生状态转移，需要注意的一点是，状态转移的时间间隔由用户指定；状态转移后得到的新状态以及对该过程的评价回报被发送到代理模型；完成一定循环后强化学习算法更新代理模型。

当上述过程在代理模型和环境模型之间转换时，可充分利用计算设备的计算能力加速仿真，减少训练时间。由于环境模型是虚拟环境，其产生的数据与真实数据存在一定差异，为了提高代理模型的置信度，本系统将真实机器人引入训练过程，当虚拟训练进行一定次数后，将环境模型切换成真实机器人实现真实的交互数据。

关于“离线”式碰撞检测，需要补充说明的是，采用数字映射模块规划轨迹，计算代价小而且一定是无碰撞的，然后再将这个轨迹作为初值给规划器；由于数字映射模块中的强化学习算法在训练代理模型时综合考虑了碰撞，而且是在离线训练时完成的，规划器求解时不再需要考虑碰撞问题，因此这个方法就不再受限于障碍物密集的场景。

请参考图3，图3是本发明方法的原理实现流程示意图；一种基于数字孪生的最优轨迹生成方法采用敏捷轨迹优化，其实现原理具体为：

S101：外围设备通过所述数据交互接口向所述轨迹规划模块下发轨迹规划任务；

具体来说，轨迹规划任务来源包括示教器、上位机以及上层决策系统；数字孪生系统读取当前机器人状态信息。

S102：轨迹规划模块根据所述轨迹规划任务生成最优控制问题，并进一步将所述轨迹规划任务下发至所述数字映射模块；

具体来说，轨迹规划模块首先基于当前机器人状态信息更新机器人物理模型；其次基于规划任务生成最优控制问题；作为一些实施例，最优控制问题的一般元素包括目标函数、动力学约束、路径约束、始末端约束等。目标函数通常可构造成三种数学形式，如Mayer型、Bolza型以及Lagrange型；动力学约束即是机器人物理模型。

S103：在指定规划周期T内，所述数字映射模块根据所述轨迹规划任务生成机器人第一控制量轨迹和第一状态量轨迹；所述第一控制量轨迹和所述第一状态量轨迹构成初始无碰撞轨迹；

具体来说，数字映射模块基于轨迹规划任务重置环境模型；驱动由强化学习算法离线训练获得的代理模型，根据规划任务交互式地生成控制量轨迹，通过环境模型生成状态量轨迹。这里为区别后文称呼，分别表示为：第一控制量轨迹、第一状态量轨迹。需要强调的是，通过数字映射模块生成的第一控制量轨迹、第一状态量轨迹，其组合形成的一定是一条无碰撞的轨迹；因为第一控制量轨迹、第一状态量轨迹是在数字化虚拟环境中生成的；

S104：所述轨迹规划模块基于所述初始无碰撞轨迹设置所述最优控制问题中机器人控制变量和状态变量的初始值，并求解所述最优控制问题，生成第二控制量轨迹和第二状态量轨迹；

具体来说，这里为阐释清楚起见，将轨迹规划模块生成的轨迹分别表示为：第二控制量轨迹和第二状态量轨迹；需要强调的是，第二控制量轨迹和第二状态量轨迹组合形成的，可能为有碰撞轨迹，也可能为无碰撞轨迹，需要经过评估。

S105：所述轨迹规划模块将第二控制量轨迹和第二状态量轨迹下发至所述数字映射模块；

S106：所述数字映射模块对第二控制量轨迹和第二状态量轨迹进行误差评估以及碰撞检测；若第二控制量轨迹和第二状态量轨迹同时通过误差评估及碰撞检测，则将第二控制量轨迹和第二状态量轨迹作为第一可接收轨迹；

S107：若规划周期T未结束，则将第一可接收轨迹作为最优控制问题中机器人控制变量和状态变量新的初始值，重新配置新的最优控制问题，重复步骤S104~S106，生成第二、第三、...、第N可接收轨迹，直至规划周期T结束，或外界请求到达时，最后的一条可接收轨迹，即为最终机器人规划轨迹；根据该步骤，可以看出本申请的“敏捷”在于任何时候只要外界请求输出，系统都准备好了一条可用轨迹；

S108：所述轨迹规划模块将所述最终机器人规划轨迹通过所述数据传输接口下发至机器人本体，机器人本体根据所述最终机器人规划轨迹执行动作。

本方法中，步骤S107调整规划器配置的目的是将规划任务改造成新的最优控制问题，并且该最优控制问题相较于上一轮的表述变得更加严苛。随着优化次数的增加，目标轨迹可能逐渐收敛，也可能出现求解失败的情况；针对前者，本系统采用随机扰动的方式调整状态变量和控制变量的初始值，帮助其跳出局部极值位置；针对后者，本系统将依次采用切换初始轨迹和松弛约束条件的方式重启计算。

为了更好的阐述本发明，这里再次对“敏捷轨迹优化”的概念进一步说明。

请参考图4，图4是敏捷轨迹优化的流程示意图；“敏捷轨迹优化”在一个规划周期内的一般流程，该流程交替式地实现最优轨迹生成以及轨迹评估和碰撞检测。最优轨迹是最优控制问题的求解结果，而最优控制问题将根据上一轮优化情况进行调整，通常的手段是逐渐限缩容许条件，如压缩不等式约束的上、下界，调整目标函数中的权重参数等。

新的最优轨迹将通过环境模型进行验证，一方面是评价该控制量轨迹的驱动结果与规划任务的偏差；另一方面是检验状态转移过程是否发生碰撞。

如果控制量轨迹通过检验，那么该控制量轨迹及其状态量轨迹被标记为接受状态，当需要输出规划结果时，最后一个接受的轨迹将作为输出结果。

需要说明的一点是，如果计算资源足够，在“敏捷”轨迹优化过程中，规划器中的优化求解与环境模型中的轨迹验证可并行计算，即由规划器规划出一条目标轨迹并将其下发至环境模型后，随即更改配置生成新的问题并求解；同时，环境模型同步验证接收到的轨迹。

本发明的有益效果是：

1、本发明将运动规划问题表示成最优控制问题，相较于路径规划方法，本发明提供的数字孪生系统实现轨迹规划，不仅能提供机器人运动过程中的状态转移历程，还能直接计算出控制量轨迹，因此可作为一种机器人的前馈控制。采用最优控制方法规划的控制量轨迹与状态量轨迹具有一定的最优性质，能够在提高工作精度、效率的同时兼顾能耗；并且该系统提供的轨迹规划模块与数字映射模块相互搭配，实现了一种高效的最优控制问题求解方案。

2、本系统实现了一种“敏捷”轨迹优化，以满足规划任务求解的时效性。首先数字映射模块模拟了真实的工作环境，并通过由强化学习算法离线学习的代理模型快速生成无碰撞轨迹，该轨迹将作为初始轨迹再由规划器进行多轮优化；如果在规划周期内外界请求输出一条可行轨迹，规划器可以将最后一次的优化结果轨迹输出，因此称其为“敏捷”轨迹优化。

3、机器人的碰撞检测可能是在生成完整轨迹之后，也可能采用轨迹规划与碰撞检测交替进行的方式，两种方式适用于不同的工作场景。对于前者，由于轨迹已经生成，若其无法通过碰撞检测，则需要重新规划新的轨迹，造成不必要的计算代价。本系统采用的数字映射模块采用强化学习算法训练一个代理模型，用于实现状态量到控制量的映射；代理模型是通过离线方式训练的，并通过在训练过程中引入真实环境来增强代理模型的置信度；因此部署在实际系统中的代理模型能够迅速生成无碰撞的控制量轨迹。在强化学习算法中设定生成轨迹的评价方式,即偏好于远离障碍物的轨迹；因此当规划器对数字映射模块生成的初始轨迹进行优化时，新的目标轨迹通常保持无碰撞，避免了重新计算的问题。从某种程度上讲，该方法将碰撞检测以“离线”的方式实现，从而提高了规划任务的求解效率。

应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。此外，附图的结构图中各模块的组成部分只是示意框架，其可以包括多个子部分，分别承担独立的功能。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于数字孪生的最优轨迹生成方法，应用于一种基于数字孪生的最优轨迹生成系统，其特征在于：

所述系统，包括：数据传输接口、数据交互接口、轨迹规划模块和数字映射模块；

所述轨迹规划模块包括：规划器组件和机器人物理模型组件；所述数字映射模块包括：环境模型和代理模型；

所述规划器组件将轨迹规划任务转换为最优控制问题，并集成最优控制器求解该最优控制问题；

所述机器人物理模型组件具体指通过物理建模手段获得的机器人动力学方程，且该动力学方程兼容所述最优控制器；

所述环境模型具体指机器人真实工作环境的数字化映射；

所述代理模型，由深度神经网络表示，且所述深度神经网络通过强化学习算法离线训练，将机器人状态映射为动作

所述一种基于数字孪生的最优轨迹生成方法采用敏捷轨迹优化，其步骤具体为：

S101：外围设备通过所述数据交互接口向所述轨迹规划模块下发轨迹规划任务；

S102：轨迹规划模块根据所述轨迹规划任务生成最优控制问题，并进一步将所述轨迹规划任务下发至所述数字映射模块；

S103：在指定规划周期T内，所述数字映射模块根据所述轨迹规划任务生成机器人第一控制量轨迹和第一状态量轨迹；所述第一控制量轨迹和所述第一状态量轨迹构成初始无碰撞轨迹；

S104：所述轨迹规划模块基于所述初始无碰撞轨迹设置所述最优控制问题中机器人控制变量和状态变量的初始值，并求解所述最优控制问题，生成第二控制量轨迹和第二状态量轨迹；

S105：所述轨迹规划模块将第二控制量轨迹和第二状态量轨迹下发至所述数字映射模块；

S106：所述数字映射模块对第二控制量轨迹和第二状态量轨迹进行误差评估以及碰撞检测；若第二控制量轨迹和第二状态量轨迹同时通过误差评估及碰撞检测，则将第二控制量轨迹和第二状态量轨迹作为第一可接收轨迹；

S107：若规划周期T未结束，则将第一可接收轨迹作为最优控制问题中机器人控制变量和状态变量新的初始值，重新配置新的最优控制问题，重复步骤S104~S106，生成第二、第三、...、第N可接收轨迹，直至规划周期T结束，或外界请求到达时，最后的一条可接收轨迹，即为最终机器人规划轨迹；

S108：所述轨迹规划模块将所述最终机器人规划轨迹通过所述数据传输接口下发至机器人本体，机器人本体根据所述最终机器人规划轨迹执行动作。

Images