移动双臂机器人的运动规划方法、系统、设备及介质

技术领域

本发明属于移动双臂机器人运动规划技术领域，特别涉及一种移动双臂机器人的运动规划方法、系统、设备及介质。

背景技术

人机交互与协作是协作移动双臂机器人发展的主要目标之一，与传统的工业移动双臂机器人不同，现代的协作移动双臂机器人需要在动态非结构化的环境中与人类近距离工作，甚至与人类合作；受人类能够使用移动的下肢和冗余双臂灵活地执行任务的启发，诸多移动双臂机器人研究机构与高校设计了许多移动双臂机器人；其中，针对冗余和多约束的移动双臂机器人的运动规划面临巨大的挑战；首先，移动双臂机器人的移动底座必须能够在动态非结构化环境中自主导航，并能够协调自身相对于上肢机械臂的运动；其次，上肢机械臂应该能够与人类合作，同时执行具有较高社会接受度的双手操作任务；因此，动态非结构化环境中的可预测和自然(或类人)运动的自主规划对于移动双臂机器人至关重要。

目前，在基于左右末端执行器的动态逆可达性图的移动双臂机器人的移动和双臂操作的运动规划研究中，使用IK求解器来构建动态逆可达性图，引入改进的最大最小NSGA-II算法作为补充设计，在确定了移动底座位姿范围后的最佳操作位姿；但由于算法过于复杂，在非结构化的动态环境中，无法保证在线规划的高效性、设计动作的可预测性和自然性。

发明内容

针对现有技术中存在的技术问题，本发明提供了一种移动双臂机器人的运动规划方法、系统、设备及介质，以解决现有的运动规划方法，无法在动态非结构化环境中快速地进行移动双臂机器人的移动底座位置确定与机械臂智能类人运动规划的技术问题，以实现在线规划的高效性、设计动作的可预测性和自然性。

为达到上述目的，本发明采用的技术方案为：

本发明提供了一种移动双臂机器人的运动规划方法，包括：

步骤1、构建末端执行器的离线动态逆可达性图；

步骤2、对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿；

步骤3、获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；

步骤4、判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图；

步骤5、根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；

步骤6、从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；

步骤7、根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；

步骤8、判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

进一步的，步骤1中，构建末端执行器的离线动态逆可达性图的过程，具体如下：

步骤11、在世界坐标系中，随机采样一个目标位置；根据所述目标位置，生成末端执行器的目标姿态；

步骤12、将所述末端执行器的目标姿态作为参考位姿，基于改进的最大最小NSGA-II算法生成移动双臂机器人的可行候选终位姿；

步骤13、判断所述移动双臂机器人的可行候选终位姿是否存在碰撞；若是，则计算所述移动双臂机器人的可行候选终位姿对应的评价函数，并确定移动底座的位姿；

步骤14、为所述移动双臂机器人的可行候选终位姿对应的工作空间中的每个体素构造到达列表和占用列表；

步骤15、判断移动双臂机器人的可行候选终位姿的个数是否大于预设阈值；若否，则末端执行器的离线动态逆可达性图构建完成；若否，则返回步骤11。

进一步的，步骤13中，若所述移动双臂机器人的可行候选终位姿存在碰撞，则返回步骤12中。

进一步的，步骤4中，若当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上无有新的障碍物，则执行以下步骤：

根据所述变换后的末端执行器的离线动态逆可达性图，确定移动底座的可选位姿集；

从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；

根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；

判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

进一步的，步骤4中，对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图的过程，具体如下：

根据所述新的障碍物，置与障碍物存在碰撞的位姿可选使能为否，同步更新所述变换后的末端执行器的离线动态逆可达性图空间与障碍物发生碰撞像素的到达列表与位姿占用列表。

进一步的，步骤6中，从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿的过程，具体如下；

利用预设的评价函数，对到达列表中的可选终位姿进行评价，得到位姿评价结果；其中，所述到达列表为移动底座的可选位姿集对应的体素中的到达列表；

根据所述位姿评价结果，获取移动双臂机器人的最优位姿。

进一步的，所述末端执行器包括移动双臂机器人的左末端执行器或右末端执行器。

本发明还提供了一种移动双臂机器人的运动规划系统，包括：

第一模块，用于构建末端执行器的离线动态逆可达性图；

第二模块，用于对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿；

第三模块，获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；

第四模块，用于判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图；

第五模块，用于根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；

第六模块，用于从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；

第七模块，根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；

第八模块，用于判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回第三模块。

本发明还提供了一种移动双臂机器人的运动规划设备，包括：

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现所述的移动双臂机器人的运动规划方法的步骤。

本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的移动双臂机器人的运动规划方法的步骤。

与现有技术相比，本发明的有益效果为：

本发明提供了移动双臂机器人的运动规划方法及系统，通过构建末端执行器的离线动态逆可达性图，并根据当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上的障碍物，对位姿变换后的末端执行器的离线动态逆可达性图进行在线更新，之后根据末端执行器的在线动态逆可达性图进行轨迹规划与跟踪控制；末端执行器的离线动态逆可达性图采用离线构建的，不影响在线使用效率，而且在线搜索快；如果在线检测到障碍物，仅需对末端执行器的离线动态逆可达性图进行在线更新，即只需要舍弃有碰撞的采样位姿，更新效率快，实现动态非结构化环境中移动底座类人位姿的自主设计，有效提高了在线规划的高效性、设计动作的可预测性和自然性；针对需要单臂或双臂协同的操作任务，可自主进行动态非结构化环境中单臂与双臂协同的运动规划；对于复杂任务，可自主进行动态非结构化环境中移动底座与双臂的协同合作运动规划；对于需要与人类协同或与人类进行互动的场景，可自主实现动态非结构化环境中人类可预测的、自然的类人运动规划；对于三维浮动机械臂可实现动态非结构化环境中的可预测的、自然的运动规划。

附图说明

图1为实施例所述的移动双臂机器人的运动规划方法的流程图；

图2为实施例中构建末端执行器的离线动态逆可达性图的流程图；

图3为实施例中构建末端执行器的在线动态逆可达性图的流程图；

图4为实施例中静态结构化环境中构建的右末端执行器的动态逆可达性图；

图5为实施例中静态结构化环境中构建的左末端执行器的动态逆可达性图；

图6为实施例中动态非结构化环境中构建的右末端执行器的动态逆可达性图；

图7为实施例中动态非结构化环境中构建的左末端执行器的动态逆可达性图；

图8为实施例中静态结构化环境中移动底座位姿自主设计结果图；

图9为实施例中静态结构化环境中移动底座位姿自主设计结果图；

图10为实施例中动态非结构化环境中移动底座位姿自主设计结果图；

图11为实施例中动态非结构化环境中移动底座位姿自主设计结果图；

图12为实施例中静态结构化环境中类人终位姿设计结果图；

图13为实施例中动态非结构化环境中类人终位姿设计结果图；

图14为实施例中双臂协同运动规划移动底座可选区域设计结果图。

具体实施方式

为了使本发明所解决的技术问题，技术方案及有益效果更加清楚明白，以下具体实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明提供了一种移动双臂机器人的运动规划方法，包括以下步骤：

步骤1、构建末端执行器的离线动态逆可达性图；其中，所述末端执行器包括移动双臂机器人的左末端执行器或右末端执行器。

其中，构建末端执行器的离线动态逆可达性图的过程，具体如下：

步骤11、在世界坐标系中，随机采样一个目标位置；根据所述目标位置，生成末端执行器的目标姿态。

步骤12、将所述末端执行器的目标姿态作为参考位姿，基于改进的最大最小NSGA-II算法生成移动双臂机器人的可行候选终位姿。

步骤13、判断所述移动双臂机器人的可行候选终位姿是否存在碰撞；若否，则计算所述移动双臂机器人的可行候选终位姿对应的评价函数，并确定移动底座的位姿；若是，则返回步骤12。

步骤14、为所述移动双臂机器人的可行候选终位姿对应的工作空间中的每个体素构造到达列表和占用列表。

步骤15、判断移动双臂机器人的可行候选终位姿的个数是否大于预设阈值；若是，则末端执行器的离线动态逆可达性图构建完成；若否，则返回步骤11。

步骤2、对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿。

步骤3、获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图。

步骤4、判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图，并转至步骤5；若无，则执行以下步骤：

根据所述变换后的末端执行器的离线动态逆可达性图，确定移动底座的可选位姿集；从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

其中，对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图的过程，具体如下：

根据所述新的障碍物，置与障碍物存在碰撞的位姿可选使能为否，同步更新所述变换后的末端执行器的离线动态逆可达性图空间与障碍物发生碰撞像素的到达列表与位姿占用列表。

步骤5、根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；

步骤6、从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；具体为：利用预设的评价函数，对到达列表中的可选终位姿进行评价，得到位姿评价结果；其中，所述到达列表为移动底座的可选位姿集对应的体素中的到达列表；根据所述位姿评价结果，获取移动双臂机器人的最优位姿。

步骤7、根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；

步骤8、判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

本发明所述的移动双臂机器人的运动规划方法，实现动态逆可达性图的构建和移动双臂机器人类人运动规划；相比于现有方法：可实现动态逆可达性图的构建以及实现动态非结构化环境中移动底座类人位姿的自主设计；针对需要单臂或双臂协同的操作任务，可自主进行动态非结构化环境中单臂与双臂协同的运动规划；对于复杂任务，可自主进行动态非结构化环境中移动底座与双臂的协同合作运动规划；对于需要与人类协同或与人类进行互动的场景，可自主实现动态非结构化环境中人类可预见的类人运动规划；所述方法可以扩展到各种移动和浮动机械臂系统；例如：包裹分拣、零件装卸、个人服务和协助、无人机操作及水底勘测。

本发明还提供了一种移动双臂机器人的运动规划系统，包括第一模块、第二模块、第三模块、第四模块、第五模块、第六模块、第七模块及第八模块；第一模块，用于构建末端执行器的离线动态逆可达性图；第二模块，用于对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿；第三模块，获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；第四模块，用于判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图；第五模块，用于根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；第六模块，用于从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；第七模块，根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；第八模块，用于判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回第三模块。

本发明还提供了一种移动双臂机器人的运动规划设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现移动双臂机器人的运动规划方法的步骤。

所述处理器执行所述计算机程序时实现上述移动双臂机器人的运动规划方法的步骤，例如：步骤1、构建末端执行器的离线动态逆可达性图；步骤2、对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿；步骤3、获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；步骤4、判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图；步骤5、根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；步骤6、从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；步骤7、根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；步骤8、判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

或者，所述处理器执行所述计算机程序时实现上述系统中各模块的功能，例如：第一模块，用于构建末端执行器的离线动态逆可达性图；第二模块，用于对目标任务进行分解，得到若干子任务；根据若干子任务，规划末端执行器的期望位姿，得到每个子任务对应的末端执行器的期望位姿；第三模块，获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；第四模块，用于判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图；第五模块，用于根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；第六模块，用于从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；第七模块，根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；第八模块，用于判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回第三模块。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成预设功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序在所述移动双臂机器人的运动规划设备中的执行过程。

所述移动双臂机器人的运动规划设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述移动双臂机器人的运动规划设备可包括，但不仅限于处理器、存储器。本领域技术人员可以理解，上述是移动双臂机器人的运动规划设备的示例，并不构成对移动双臂机器人的运动规划设备的限定，可以包括比上述更多的部件，或者组合某些部件，或者不同的部件，例如所述移动双臂机器人的运动规划设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器是所述移动双臂机器人的运动规划设备的控制中心，利用各种接口和线路连接整个移动双臂机器人的运动规划设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述移动双臂机器人的运动规划设备的各种功能。

所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种移动双臂机器人的运动规划方法的步骤。

所述移动双臂机器人的运动规划系统集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。

基于这样的理解，本发明实现上述移动双臂机器人的运动规划方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，所述计算机程序在被处理器执行时，可实现上述移动双臂机器人的运动规划方法的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或预设中间形式等。

所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-OnlyMemory，ROM)、随机存取存储器(RandomAccessMemory，RAM)、电载波信号、电信信号以及软件分发介质等。

需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。

实施例

本实施例以对某移动双臂机器人的运动规划过程为例，其中，所述移动双臂机器人包括左末端执行器、右末端执行器及移动底座。

如附图1所示，本实施例提供了一种移动双臂机器人的运动规划方法，包括以下步骤：

步骤1、针对待规划的移动双臂机器人，采用离线方式，构建末端执行器的离线动态逆可达性图(inverse dynamic reachability maps，iDRMs)。

如附图2所示，采用离线方式，构建末端执行器的离线动态逆可达性图的过程，具体包括以下步骤：

步骤11、在世界坐标系中，为末端执行器随机采样一个目标位置；根据所述目标位置，生成末端执行器的目标姿态；其中，所述末端执行器的目标姿态，包括左末端执行器的目标姿态X

步骤12、将所述末端执行器的目标姿态作为参考位姿，即将所述末端执行器的目标位置及姿态作为参考坐标系。

步骤13、根据所述参考位姿，基于改进的最大最小NSGA-II算法，生成移动双臂机器人的可行候选终位姿q

步骤14、判断所述移动双臂机器人的可行候选终位姿q

步骤15、为所述移动双臂机器人的可行候选终位姿q

步骤16、判断移动双臂机器人的可行候选终位姿的个数k是否大于预设阈值N；若否，则末端执行器的离线动态逆可达性图构建完成；若是，则返回步骤11。

步骤2、对目标任务进行分解，得到若干子任务；其中，对目标任务进行分解时，利用任务规划起将目标任务分解为一系列移动双臂机器人能够理解和执行的若干子任务；根据若干子任务，规划末端执行器的期望位姿；根据所述末端执行器的期望位姿，得到每个子任务n

步骤3、获取当前子任务开始时末端执行器的初始位姿；利用所述当前子任务开始时末端执行器的初始位姿，对所述末端执行器的离线动态逆可达性图进行位姿变换，得到变换后的末端执行器的离线动态逆可达性图；其中，对所述末端执行器的离线动态逆可达性图进行位姿变换时，将所述末端执行器的离线动态逆可达性图的原点，采用所述当前子任务开始时末端执行器的初始位姿进行修改，以将所述末端执行器的离线动态逆可达性图转移到左末端执行器及右末端执行器，得到所述变换后的末端执行器的离线动态逆可达性图。

步骤4、判断当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；其中，判断是否有障碍物的过程，具体为：利用移动双臂机器人的视觉、激光或雷达检测手段，对当前子任务执行环境中的障碍物进行检测；若有，则对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图，并跳转至步骤5；若否，则根据所述变换后的末端执行器的离线动态逆可达性图，确定移动底座的可选位姿集；从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

如附图3所示，对所述变换后的末端执行器的离线动态逆可达性图进行在线更新，得到末端执行器的在线动态逆可达性图的过程，具体如下：

初始化末端执行器的离线动态逆可达性图；

检测当前子任务开始时末端执行器的初始位姿到当前子任务对应的末端执行器的期望位姿的路径上是否有新的障碍物；若是，则根据所述新的障碍物，置与障碍物存在碰撞的位姿可选使能为否，同步更新所述变换后的末端执行器的离线动态逆可达性图空间与障碍物发生碰撞像素的到达列表与位姿占用列表，即得到所述末端执行器的在线动态逆可达性图。

步骤5、根据所述末端执行器的在线动态逆可达性图，确定移动底座的可选位姿集；具体的，综合评价所述末端执行器的在线动态逆可达性图的各体素中，各可选位姿的评价函数，根据所述评价函数的结果，确定移动底座的可选位姿集；其中，针对在地面移动的移动双臂机器人，关注地面附近的体素；针对三维浮动的移动双臂机器人，关注空间的所有体素。

步骤6、从所述移动底座的可选位姿集中，选取移动双臂机器人的最优位姿；具体的，利用预设的评价函数，对到达列表中的可选终位姿进行评价，得到位姿评价结果；其中，所述到达列表为移动底座的可选位姿集对应的体素中的到达列表；根据所述位姿评价结果，获取移动双臂机器人的最优位姿；即，综合评价底座可选位姿集对应的体素中，其到达列表中可选终位姿的评价函数，选择评价函数最优的可选终位姿。

步骤7、根据所述当前子任务开始时末端执行器的初始位姿以及所述移动双臂机器人的最优位姿，得到当前子任务的轨迹规划结果，并执行当前子任务的轨迹跟踪控制；

步骤8、判断是否已完成所有子任务的轨迹规划与跟踪控制；若是，则流程结束；若否，则返回步骤3。

本实施例中，为使所述移动双臂机器人能够实现可预测的、自然的类人运动，定义了以下评价函数，具体如下：

(1)末端执行器的定位精度评价函数f

f

其中，x

(2)末端执行器的方向跟踪精度评价函数f

f

其中，e

(3)末端执行器的可操作性评价函数f

f

或/>

其中，末端执行器的可操作性描述了移动双臂机器人位姿到奇异位姿的距离；J(θ)为末端执行器的雅可比矩阵；σ为J(θ)的奇异值。

(4)关节位移评价函数f

其中，n是广义变量θ的维度；θ

本实施例中，为了节能，需要移动双臂机器人有最小的关节位移；因此，定义了基于质量的关节位移目标函数作为关节位移评价函数；其中，关节位移评价函数f

(5)为了实现移动底座与操作臂之间的协同，因此，定义末端执行器相对底座的位移评价函数f

f

其中，x

试验结果：

本实施例所述的移动双臂机器人的运动规划方法，通过进行动态逆可达性图离线构建、在线更新以及移动双臂机器人的运动规划仿真；假定移动双臂机器人执行一次抓取任务，移动双臂机器人末端执行器的位姿已知，需要设计移动双臂机器人的移动底座与双臂的位姿；

其中，设计移动双臂机器人的移动底座与双臂的位姿的过程，具体如下：

首先，完成动态逆可达性图的构建：

假定移动双臂机器人在静态结构化环境中执行特定任务，依据末端执行器的期望抓取位姿，构建末端执行器的离线动态逆可达性图；如附图4-5所示，附图4中给出了静态结构化环境中构建的右末端执行器的动态逆可达性图，附图5中给出了静态结构化环境中构建的左末端执行器的动态逆可达性图；从附图4-5中可以看出，移动双臂机器人的移动底座位姿的可选区域为环绕右或左末端执行器的球环区域。

假定移动双臂机器人在动态非结构化环境中执行特定任务，依据末端执行器的期望抓取位姿，构建末端执行器的离线动态逆可达性图；如附图6-7所示，附图6中给出了动态非结构化环境中构建的右末端执行器的动态逆可达性图，附图7中给出了动态非结构化环境中构建的左末端执行器的动态逆可达性图；从附图6-7中可以看出，移动双臂机器人的移动底座位姿的可选区域为环绕右或左末端执行器的球环区域，与障碍物无碰撞。

其次，进行移动底座位姿的设计：

假定移动双臂机器人在静态结构化环境中执行特定任务，根据末端执行器的离线动态逆可达性图确定移动底座的位姿区域，其设计结果如附图8-9所示；假定移动双臂机器人在动态非结构化环境中执行特定任务，根据末端执行器的离线动态逆可达性图确定移动底座的位姿区域，其设计结果如附图10-11所示；从附图8-11中可以看出，移动底座与障碍物无碰撞。

最后，完成移动双臂机器人的终位姿的设计：

假定移动双臂机器人在静态结构化环境中执行特定任务，根据右末端执行器的iDRM图选择最优终位姿，其设计结果如附图12所示；从附图12中可以看出，所设计的终位姿是可预测的、自然的类人运动。

假定移动双臂机器人在动态非结构化环境中执行特定任务，根据右末端执行器的iDRM图选择最优终位姿，其设计结果如附图13所示；从附图13中可以看出，所设计的终位姿是可预测的、自然的类人运动，与障碍物无碰撞。

如果需要双臂完成协同操作任务，只需要将左右末端执行器的iDRM图合并，即可得到移动底座位姿的设计结果，其设计结果如附图14所示；从附图14中可以看出，所设计的终位姿是可预测的、自然的类人运动，与障碍物无碰撞。

本实施例中，所述末端执行器的离线动态逆可达性图中，包含的移动双臂机器人可选位姿是基于改进的最大最小NSGA-II算法，可实现多个目标函数的多方向优化，故而可保证所设计的移动双臂机器人位姿是可预测的、自然的，即类人的；末端执行器的离线动态逆可达性图采用离线构建的，不影响在线使用效率，而且在线搜索快；如果在线检测到障碍物，仅需对末端执行器的离线动态逆可达性图进行在线更新，即只需要舍弃有碰撞的采样位姿，更新效率快。

本实施例提供的一种移动双臂机器人的运动规划系统、设备及计算机可读存储介质中相关部分的说明可以参见本实施例所述的一种移动双臂机器人的运动规划方法中对应部分的详细说明，在此不再赘述。

本发明中，通过离线构建动态逆可达性图和规划移动双臂机器人的类人运动，可实现动态非结构化环境中移动底座类人位姿的自主设计；针对需要单臂或双臂协同的操作任务，可自主进行动态非结构化环境中单臂与双臂协同的运动规划；对于复杂任务，可自主进行动态非结构化环境中移动底座与双臂的协同合作运动规划；对于需要与人类协同或与人类进行互动的场景，可自主实现动态非结构化环境中人类可预见的类人运动规划；所述方法可以扩展到各种移动和浮动机械臂系统，例如：包裹分拣、零件装卸、个人服务和协助、无人机操作或水底勘测领域。

本发明所述的移动双臂机器人的运动规划方法及系统，可用于任何移动或浮动机械臂的动态逆可达性图构建；同时，可用于单手(右手/左手)操作、单手与底座系统操作、双臂协同操作或双臂-底座协同操作任务的运动规划，并能应用于动态和静态环境，移动双臂机器人的智能化程度得到很大的提高；本发明所述的方法可以快速延伸到更复杂的运动规划情况，例如：如涉及到需要控制左手相对于右手的情况，可以直接在已有算法的基础上，先确定底座与右臂(左臂)的最优位姿，然后进行左臂(右臂)的位姿规划。

本发明方法可用于家居助理移动双臂机器人的运动规划，例如：给定需要拿取的目标物体(如：杯子，书籍，遥控板等)的位置信息，移动双臂机器人能够智能规划出拿取该物体时移动双臂机器人底座的可选择区域，并规划出抓取物体时的最优位置和姿态；同时，也可用于个性化工业生产中操作任务的运动规划，例如：给定需要操作(焊接、装配，喷涂等)的目标物体(如：金属零件)的位置信息，移动双臂机器人能够智能规划出操作该物体时移动双臂机器人底座的可选择区域，并规划出对物体进行操作时移动双臂机器人的最优位置和姿态；并且还可用于海底、航空、航天等领域的浮动机械臂的操作任务规划，例如给出需要操作的目标物体的位姿信息，移动双臂机器人能够智能规划处浮动基座的位姿和移动双臂机器人的最优操作位姿。

上述实施例仅仅是能够实现本发明技术方案的实施方式之一，本发明所要求保护的范围并不仅仅受本实施例的限制，还包括在本发明所公开的技术范围内，任何熟悉本技术领域的技术人员所容易想到的变化、替换及其他实施方式。