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

摘要

本发明公开了一种移动双臂机器人运动规划方法、系统、设备及介质，用于移动高冗余双臂机器人的智能动态运动规划，通过引入移动底座与左右手的相对逆可达性图，实现移动底座与左右手的协同运动规划；同时，根据移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，能够确定既满足右手又满足左手操作性能需求的移动底座位姿与上臂操作位姿；本发明实现了移动双臂机器人在动态、非结构化环境中的智能仿人运动规划，过程简单，运动规划效果较好，规划效率较高。

权利要求

1.一种移动双臂机器人运动规划方法，其特征在于，包括以下步骤：构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；

对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；

根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；

动态检测环境中是否有新的障碍物；

若有，则更新移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；

设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；

利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；

根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；

判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

2.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，动态检测环境中是否有新的障碍物过程，若未检测到新的障碍物，则根据移动双臂机器人的移动底座相对右手的RiDRM图及移动双臂机器人的移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。

3.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，设计移动双臂机器人的最优位姿，并将移动双臂机器人的最优位姿加入至移动双臂机器人的可选位姿集的过程，采用改进的MaxiMin NSGA‑II算法设计得到移动双臂机器人的最优位姿。

4.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，根据移动机器人的最优位姿信息，进行轨迹规划及轨迹跟踪控制的过程，具体如下：根据移动机器人的最优位姿，采用直接双向RRT和梯度下降算法进行轨迹规划，并使用自适应RBF神经网络进行轨迹跟踪控制。

5.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，构建移动双臂机器人的移动底座相对右手的RiDRM图的过程，具体如下：构建移动双臂机器人的移动底座相对右手的RiDRM图的过程包括离线RiRM构建过程和在线RiDRM构建过程；

其中，离线RiRM构建过程，具体包括以下步骤：

在3D空间内，随机给定移动双臂机器人的右手目标位置，并均匀生成移动双臂机器人的右手目标位姿；

设定移动双臂机器人的右手目标位姿为参考坐标系；

根据移动双臂机器人的右手目标位姿，进行IK求解，得到右手候选位姿qkr；

判断右手候选位姿qkr是否为可行解；

若是，则求解右手候选位姿qkr的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

若否，则返回重新获取右手候选位姿；

构建与右手候选位姿qkr对应的RiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkr对应的RiRM空间像素vi的移动底座占用列表Oi；

判断右手候选位姿qkr的生成数量是否达到预设值N，若未达到，则返回继续获取右手候选位姿，否则，完成离线RiRM的构建；

在线RiDRM的构建过程，具体包括以下步骤：

将RiDRM空间初始化为RiRM；

动态检测环境中是否有新的障碍物，若无，则完成在线RiDRM的构建；

若有，则设置与新的障碍物存在碰撞的右手候选位姿qkr的可选使能为否；同步更新RiDRM空间对应的像素vi的到达列表Rei，以及RiDRM空间对应的像素vi的占用列表Oi。

6.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，构建移动双臂机器人的移动底座相对左手LiDRM图的过程，具体如下：构建移动双臂机器人的移动底座相对左手LiDRM图的过程包括离线LiRM构建过程和在线LiDRM构建过程；

其中，离线LiRM构建过程，具体包括以下步骤：

在3D空间内，随机给定移动双臂机器人的左手目标位置，并均匀生成移动双臂机器人的左手目标位姿；

设定移动双臂机器人的左手目标位姿为参考坐标系；

根据移动双臂机器人的左手目标位姿，进行IK求解，得到左手候选位姿qkl；

判断左手候选位姿qkl是否为可行解；

若是，则求解左手候选位姿qkl的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

若否，则返回重新获取左手候选位姿；

构建与左手候选位姿qkl对应的LiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkl对应的LiRM空间像素vi的移动底座占用列表Oi；

判断左手候选位姿qkl的生成数量是否达到预设值N，若未达到，则返回继续获取左手候选位姿，否则，完成离线LiRM的构建；

在线LiDRM的构建过程，具体包括以下步骤：

将LiDRM空间初始化为LiRM；

动态检测环境中是否有新的障碍物，若无，则完成在线LiDRM的构建；

若有，则设置与新的障碍物存在碰撞的左手候选位姿qkl的可选使能为否；同步更新LiDRM空间对应的像素vi的到达列表Rei，以及LiDRM空间对应的像素vi的占用列表Oi。

7.一种移动双臂机器人运动规划系统，其特征在于，包括：

构建模块，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

任务规划分解模块，用于根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；

执行模块，用于根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；动态检测环境中是否有新的障碍物；若有，则更新移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

优化模块，用于根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；

结束模块，用于判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

8.一种移动双臂机器人运动规划设备，其特征在于，包括：

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

处理器，用于执行所述计算机程序时实现如权利要求1‑6任一项所述一种移动双臂机器人运动规划方法的步骤。

9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1‑6任一项所述一种移动双臂机器人运动规划方法的步骤。

说明书

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

技术领域

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

背景技术

[0002] 机器人被定义为“具有感知、规划和行动能力的目标导向机器”，即具有环境和自我感知、运动规划和跟踪控制能力；一般情况下，安装在一个移动平台上或有两条腿的人形双臂机器人由于其外观和性能与人类相似，因此被设计为智能服务机器人；智能运动规划能力使机器人具有自主能力，对于需要高质量人机互动的智能服务机器人和智能制造机器人来说意义非凡。

[0003] 智能服务机器人能够为人类日常起居提供家居服务，智能制造机器人能够实现个性化智能生产并与人类进行协同作业；然而，由于在感知、机械关节、运动规划和跟踪控制以及多种技能的整合和协调等领域存在很多问题，实现灵巧的通用智能服务与智能制造机器人的实际应用还有很多挑战；其另一个明显的优势是可以在不改变现有基础设施的情况下与人类一起自然地工作。很显然，上述智能服务机器人需要类似人类的运动学结构，例如：具有一个可移动的底座和两条灵活的手臂；其与单臂机器人或移动小车相比，具有应用范围广、协作能力强、在复杂任务和动态环境下可靠性高的优点，也可广泛应用于工业制造、社会服务、航空航天等领域。但是为了得到类人的性能，机器人需要能够规划复杂的运动策略，包括双臂与底座的协同合作，这是非常具有挑战性的。

[0004] 为了使规划和操作切实可行，需要对移动双臂机器人的移动底座和上肢的运动进行预计算；而如果要求一个类人移动机器人具有人一样行为能力，几乎需要设计无穷多个期望的轨迹；针对上述问题，国内外的研究学者已经做了大量的工作。现有方法有基于层次规划策略(HPS)和贝叶斯决策，分别反映人体手臂运动的一般规律和选择合适的运动模式，用于机器人手臂规划算法来生成类人运动。基于CoM运动基元的概念，实现仿人机器人的全身运动规划，通过实现一连串的CoM移动，在构型空间中构建一棵运动树。然而，上述方法都需要手动给出预抓取移动底座的位置和抓取位姿(终位姿)，或者简单地假设它们是已知的。

[0005] 事实上，机器人能够自主设计合适的终位姿来实现更好的自主性是至关重要的；为此，很多运动规划方法引入了机器人工作空间的概念。例如，在SE(2)中相对于期望的抓取姿态确定人形机器人脚的最佳姿态位置的方法。可达的工作空间表示可以预先计算，并通过求逆获得逆可达图(iRM)。通过控制机器人Nao的单臂对所提出的方法进行了评估；也可采用逆曲面可达性图来确定机器人的性能较好的姿态，并将基于点的问题推广到平面多边形表面，从而设计出好的抓取姿态。在已有基于可达性图运动规划方法中，可达性图是通过对构型空间随机采样构建的，每个3D像素(x,y,z)的评价函数值是通过对所有具有任意姿态的6D像素(x,y,z,α,β,γ)的评价函数值进行累积获得的。利用仿人机器人单臂的逆可达性来生成适合抓取的底座位置的另一方法中，为有效的IK求解器建立了定向可达性图，从而生成有效的底座位姿和正确的关节构型，但是，它会在动态环境中崩溃。为了解决这个问题，新的方法中引入了动态逆可达映射(iDRM)用于浮动底座系统。通过离线构建腿型仿人机器人的iDRM并在检测到新障碍物时进行在线更新iDRM。对应于每个采样位姿的每个像素构建了两个列表，其中每个采样位姿是通过一个全身IK求解器得到。但是，该方法只考虑了单臂的约束。另外一个已有方法使用一对正‑逆动态可达性图来确定腿型放任机器人单手和双手的终位姿，实现在不平坦地形上完成任务；然而，并未考虑机器人的终位姿的操纵能力或位姿的类人性。现有的改进的MaxiMin NSGA‑II的冗余运动规划策略用于非完整约束仿人移动机器人‑给定末端执行器的位置，确定最优的移动底座位置和和机械臂构型；但是，由于对底座的搜索区域没有约束，在实时应用中动态智能运动规划的效率较低。

[0006] 综上，移动双臂机器人运动规划是机器人技术领域的关键问题之一，而自主移动底座与双臂的位姿规划是实现移动双臂机器人智能化的关键；而目前关于机器人运动规划的研究主要集中在移动小车(UGV)、无人机(UAV)及简单机械臂；关于移动机械手和仿人机器人的运动规划也开展了很多研究。特别地，双臂机器人的运动规划主要方法有RRTs等直接搜索法、基于IK求解法、几何法、推理法、启发式探索法等；已有技术方法中虽然涉及冗余或双臂的运动规划，但是两臂之间的协作以及机器人底座的确定，以及机器人底座与上身机械臂之间的协调都没有得到很好的研究，更不用说类人的行为的规划。

发明内容

[0007] 针对现有技术中存在的技术问题，本发明提供了一种移动双臂机器人运动规划方法、系统、设备及介质，以解决现有的机器人运动规划方法，无法实现移动双臂机器人在动态、非结构化环境中智能仿人运动规划的技术问题。

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

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

[0010] 构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

[0011] 根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；

[0012] 对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；

[0013] 根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；

[0014] 动态检测环境中是否有新的障碍物；

[0015] 若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

[0016] 根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；

[0017] 设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；

[0018] 利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；

[0019] 根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；

[0020] 判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

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

[0022] 构建模块，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

[0023] 任务规划分解模块，用于根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；

[0024] 执行模块，用于根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；动态检测环境中是否有新的障碍物；若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；

[0025] 优化模块，用于根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；

[0026] 结束模块，用于判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

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

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

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

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

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

[0032] 本发明提供了一种移动双臂机器人运动规划方法，用于移动高冗余双臂机器人的智能动态运动规划，通过引入移动底座与左右手的相对逆可达性图，实现移动底座与左右手的协同运动规划；同时，根据移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，能够确定既满足右手又满足左手操作性能需求的移动底座位姿与上臂操作位姿，实现了移动双臂机器人在动态、非结构化环境中的智能仿人运动规划，过程简单，运动规划效果较好，规划效率较高。

[0033] 进一步的，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图的过程采用离线及在线过程的结合，不影响在线使用效率，实际在线搜索速率可以很高；如果在线检测到障碍物，RiDRM图与LiDRM图的在线更新只需要舍弃有碰撞的采样位姿，提高了更新效率；并且，通过引入移动底盘和双臂的相对逆可达性图来实现移动底盘和双臂的协同运动规划，此外定义了移动双臂机器人的类人运动评价指标，从而实现智能类人运动规划。

[0034] 本发明所述的移动双臂机器人运动规划方法，可用于静态/动态、结构化/非结构化、单臂/双臂、底座‑上臂协同等多种移动双臂机器人任务情景：在静态、结构化环境中，针对单臂任务/双臂协作任务，可实现移动双臂机器人移动底座位姿的智能自主规划；在静态、结构化环境中，针对单臂任务/双臂协作任务，可实现移动双臂机器人移动底座与末端执行器的协同智能自主规划；在静态、结构化环境中，针对单臂任务/双臂协作任务，可实现机器人的仿人位姿规划；在动态、非结构化环境中，针对单臂任务/双臂协作任务，可实现移动双臂机器人移动底座位姿的快速动态智能自主规划；在动态、非结构化环境中，针对单臂任务/双臂协作任务，可实现移动双臂机器人移动底座与末端执行器的协同运动的快速动态智能自主规划；在动态、非结构化环境中，针对单臂任务/双臂协作任务，可实现机器人的仿人位姿的快速智能规划；对于需要与人类协同或与人类进行互动的动态、非结构化场景中，实现人类可预见的移动双臂机器人动态智能仿人运动规划。

附图说明

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

[0036] 图2为实施例中离线RiRM构建的流程图；

[0037] 图3为实施例中在线RiDRM构建的流程图；

[0038] 图4为实施例中离线LiRM构建的流程图；

[0039] 图5为实施例中在线LiDRM构建的流程图；

[0040] 图6为实施例中根据右手的抓取位姿与RiDRM图确定的移动底座位姿区域图；

[0041] 图7为实施例中根据RiDRM图确定的移动底座位姿可选区域图；

[0042] 图8为实施例中根据右手的抓取位姿与RiDRM图设计的机器人抓取位姿图；

[0043] 图9为实施例中根据左手的抓取位姿与LiDRM图确定的移动底座位姿区域图；

[0044] 图10为实施例中根据左手LiDRM图确定的移动底座位姿可选区域图；

[0045] 图11为实施例中根据左手的抓取位姿与LiDRM图设计的机器人抓取位姿图；

[0046] 图12为实施例中有障碍物时的RiDRM图与移动底座位姿图；

[0047] 图13为实施例中有障碍物时的LiDRM图与移动底座位姿图；

[0048] 图14为实施例中根据右手的抓取位姿进行有障碍物时的位姿规划结果图；

[0049] 图15为实施例中根据左手的抓取位姿进行有障碍物时的位姿规划结果图；

[0050] 图16为实施例中根据左右手的抓取位姿确定移动底座的可选位姿图。

具体实施方式

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

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

[0053] 步骤1、构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。

[0054] 其中，构建移动双臂机器人的移动底座相对右手的RiDRM图的过程，具体如下：

[0055] 构建移动双臂机器人的移动底座相对右手的RiDRM图的过程包括离线RiRM构建过程和在线RiDRM构建过程；

[0056] 其中，离线RiRM构建过程，具体包括以下步骤：

[0057] 在3D空间内，随机给定移动双臂机器人的右手目标位置，并均匀生成移动双臂机器人的右手目标位姿；

[0058] 设定移动双臂机器人的右手目标位姿为参考坐标系；

[0059] 根据移动双臂机器人的右手目标位姿，进行IK求解，得到右手候选位姿qkr；

[0060] 判断右手候选位姿qkr是否为可行解；

[0061] 若是，则求解右手候选位姿qkr的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

[0062] 若否，则返回重新获取右手候选位姿；

[0063] 构建与右手候选位姿qkr对应的RiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkr对应的RiRM空间像素vi的移动底座占用列表Oi；

[0064] 判断右手候选位姿qkr的生成数量是否达到预设值N，若未达到，则返回继续获取右手候选位姿，否则，完成离线RiRM的构建；

[0065] 在线RiDRM的构建过程，具体包括以下步骤：

[0066] 将RiDRM空间初始化为RiRM；

[0067] 动态检测环境中是否有新的障碍物，若无，则完成在线RiDRM的构建；

[0068] 若有，则设置与新的障碍物存在碰撞的右手候选位姿qkr的可选使能为否；同步更新RiDRM空间对应的像素vi的到达列表Rei，以及RiDRM空间对应的像素vi的占用列表Oi。

[0069] 构建移动双臂机器人的移动底座相对左手LiDRM图的过程，具体如下：

[0070] 构建移动双臂机器人的移动底座相对左手LiDRM图的过程包括离线LiRM构建过程和在线LiDRM构建过程；

[0071] 其中，离线LiRM构建过程，具体包括以下步骤：

[0072] 在3D空间内，随机给定移动双臂机器人的左手目标位置，并均匀生成移动双臂机器人的左手目标位姿；

[0073] 设定移动双臂机器人的左手目标位姿为参考坐标系；

[0074] 根据移动双臂机器人的左手目标位姿，进行IK求解，得到左手候选位姿qkl；

[0075] 判断左手候选位姿qkl是否为可行解；

[0076] 若是，则求解左手候选位姿qkl的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

[0077] 若否，则返回重新获取左手候选位姿；

[0078] 构建与左手候选位姿qkl对应的LiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkl对应的LiRM空间像素vi的移动底座占用列表Oi；

[0079] 判断左手候选位姿qkl的生成数量是否达到预设值N，若未达到，则返回继续获取左手候选位姿，否则，完成离线LiRM的构建；

[0080] 在线LiDRM的构建过程，具体包括以下步骤：

[0081] 将LiDRM空间初始化为LiRM；

[0082] 动态检测环境中是否有新的障碍物，若无，则完成在线LiDRM的构建；

[0083] 若有，则设置与新的障碍物存在碰撞的左手候选位姿qkl的可选使能为否；同步更新LiDRM空间对应的像素vi的到达列表Rei，以及LiDRM空间对应的像素vi的占用列表Oi。

[0084] 步骤2、根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿。

[0085] 步骤3、对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿。

[0086] 步骤4、根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置。

[0087] 步骤5、动态检测环境中是否有新的障碍物。

[0088] 步骤6、若有检测到新的障碍物，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。

[0089] 步骤7、根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。

[0090] 步骤8、若未检测到新的障碍物，则根据移动双臂机器人的移动底座相对右手的RiDRM图及移动双臂机器人的移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。

[0091] 步骤9、设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；其中，采用改进的MaxiMin NSGA‑II算法设计得到移动双臂机器人的最优位姿集。

[0092] 步骤10、根据更新后的移动双臂机器人的可选位姿集，确定移动机器人的最优位姿。

[0093] 步骤11、根据移动机器人的最优位姿，采用直接双向RRT和梯度下降算法进行轨迹规划，并使用自适应RBF神经网络进行轨迹跟踪控制。

[0094] 步骤12、判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

[0095] 本发明还提供了一种移动双臂机器人运动规划系统，包括构建模块、任务规划分解模块、执行模块、优化模块及结束模块。构建模块，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。任务规划分解模块，用于根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿。执行模块，用于根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；动态检测环境中是否有新的障碍物；若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。优化模块，用于根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制。结束模块，用于判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

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

[0097] 所述处理器执行所述计算机程序时实现上述移动双臂机器人运动规划方法的步骤，或者，所述处理器执行所述计算机程序时实现上述系统中各模块的功能。

[0098] 示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序在所述移动双臂机器人运动规划设备中的执行过程。例如，所述计算机程序可以被分割成包括构建模块、任务规划分解模块、执行模块、优化模块及结束模块，各模块具体功能如下：构建模块，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。任务规划分解模块，用于根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿。执行模块，用于根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；动态检测环境中是否有新的障碍物；若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图。优化模块，用于根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿加入至移动双臂机器人的可选位姿集；利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制。结束模块，用于判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。

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

[0100] 所称处理器可以是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field‑ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器是所述移动双臂机器人运动规划设备的控制中心，利用各种接口和线路连接整个移动双臂机器人运动规划设备的各个部分。

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

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

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

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

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

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

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

[0108] 实施例

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

[0110] 步骤1、给定预设的高冗余移动双臂机器人，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对于左手的LiDRM图。

[0111] 步骤2、预设某一需要抓取的目标物体，利用所述移动双臂机器人的检测系统，确定待抓取目标物体；根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿。

[0112] 步骤3、利用所述移动双臂机器人的任务规划系统，对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿。

[0113] 步骤4、根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图平移变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图平移变换至对应子任务的左手期望位置。

[0114] 步骤5、动态检测环境中是否有新的障碍物。如果检测到新的障碍物，转至步骤6；如未检测到新的障碍物，则转至步骤7。

[0115] 步骤6、根据检测到的新的障碍物，更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。

[0116] 步骤7、根据移动双臂机器人的移动底座相对右手的RiDRM图及移动双臂机器人的移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。

[0117] 步骤8、采用改进的MaxiMin NSGA‑II算法，设计机器人最优位姿加入可选位姿集，根据右手BiDRM与左手LiDRM选择左右臂的可选位姿加入可选位姿集，得到更新后的移动双臂机器人的可选位姿集。

[0118] 步骤9、利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿。

[0119] 步骤10、根据移动机器人的最优位姿，采用直接双向RRT和梯度下降算法进行轨迹规划，并使用自适应RBF神经网络进行轨迹跟踪控制。

[0120] 步骤11、判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回步骤4执行下一个子任务。

[0121] 本实施例中，构建移动双臂机器人的移动底座相对右手的RiDRM图的过程包括离线RiRM构建过程和在线RiDRM构建过程，具体如下：

[0122] 其中，如附图2所示，离线RiRM构建过程，具体包括以下步骤：

[0123] S11、在3D空间内，随机给定移动双臂机器人的右手目标位置，并均匀生成移动双臂机器人的右手目标位姿；

[0124] S12、设定移动双臂机器人的右手目标位姿为参考坐标系；

[0125] S13、根据移动双臂机器人的右手目标位姿，进行IK求解，得到右手候选位姿qkr；

[0126] S14、判断右手候选位姿qkr是否为可行解；

[0127] S15、若是，则求解右手候选位姿qkr的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

[0128] S16、若否，则返回步骤S13，重新获取右手候选位姿；

[0129] S17、构建与右手候选位姿qkr对应的RiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkr对应的RiRM空间像素vi的移动底座占用列表Oi；

[0130] S18、判断右手候选位姿qkr的生成数量是否达到预设值N，若未达到，则返回步骤S13继续获取右手候选位姿，否则，完成离线RiRM的构建。

[0131] 如附图3所示，在线RiDRM的构建过程，具体包括以下步骤：

[0132] S19、将RiDRM空间初始化为RiRM；

[0133] S110、动态检测环境中是否有新的障碍物，若无，则完成在线RiDRM的构建；

[0134] S111、若有，则设置与新的障碍物存在碰撞的右手候选位姿qkr的可选使能为否；同步更新RiDRM空间对应的像素vi的到达列表Rei，以及RiDRM空间对应的像素vi的占用列表Oi。

[0135] 本实施例中，构建移动双臂机器人的移动底座相对左手LiDRM图的过程包括离线LiRM构建过程和在线LiDRM构建过程，具体如下：

[0136] 如附图4所示，离线LiRM构建过程，具体包括以下步骤：

[0137] S21、在3D空间内，随机给定移动双臂机器人的左手目标位置，并均匀生成移动双臂机器人的左手目标位姿；

[0138] S22、设定移动双臂机器人的左手目标位姿为参考坐标系；

[0139] S23、根据移动双臂机器人的左手目标位姿，进行IK求解，得到左手候选位姿qkl；

[0140] S24、判断左手候选位姿qkl是否为可行解；

[0141] S25、若是，则求解左手候选位姿qkl的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；

[0142] S26、若否，则返回步骤S23重新获取左手候选位姿；

[0143] S27、构建与左手候选位姿qkl对应的LiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkl对应的LiRM空间像素vi的移动底座占用列表Oi；

[0144] S28、判断左手候选位姿qkl的生成数量是否达到预设值N，若未达到，则返回步骤S23继续获取左手候选位姿，否则，完成离线LiRM的构建。

[0145] 如附图5所示，在线LiDRM的构建过程，具体包括以下步骤：

[0146] S29、将LiDRM空间初始化为LiRM；

[0147] S210、动态检测环境中是否有新的障碍物，若无，则完成在线LiDRM的构建；

[0148] S211、若有，则设置与新的障碍物存在碰撞的左手候选位姿qkl的可选使能为否；同步更新LiDRM空间对应的像素vi的到达列表Rei，以及LiDRM空间对应的像素vi的占用列表Oi。

[0149] 本实施例中，为了实现所述移动双臂机器人的智能仿人运动，在构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图的过程中，引入了基于以下五个评价指标的评价函数，下述中末端执行器即为移动双臂机器人的左手或右手；具体如下：

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

[0151] f1(θ)＝||xR(θ)‑xRd||+||xL(θ)‑xLd||

[0152] 其中，f1(θ)为末端执行器的定位精度评价函数；xR(θ)为右手的实际位置；xRd为右手的期望位置；xL(θ)为左手的实际位置；xLd为左手的期望位置。(分别说明上述字母的中文物理含义)

[0153] (2)末端执行器的方向跟踪精度评价函数：

[0154] f2(θ)＝||eqR(θ)+||eqL(θ)||

[0155] 其中，其中，f2(θ)为末端执行器的方向跟踪精度评价函数；eqR(θ)为右手的姿态误差；eqL(θ)为左手的姿态误差；其中，右手的姿态误差eqR(θ)及左手的姿态误差eqL(θ)均采用四元数表示。

[0156] 本实施例中，除了到达左右手期望的位置和位姿，要使移动双臂机器人达到仿人的运动行为，还需引入以下评价指标。

[0157] (3)末端执行器的操作性能评价函数：

[0158] 本实施例中，操作性能能够用于描述移动双臂机器人位姿到奇异位姿的距离；操作性能的表达式如下所示：

[0159] 或

[0160] 其中，Ω为操作性能；J(θ)为末端执行器的雅各比矩阵；JT(θ)为末端执行器的雅各比矩阵J(θ)的转置；σmax为末端执行器的雅各比矩阵J(θ)的最大奇异值；σmin为末端执行器的雅各比矩阵J(θ)的最小奇异值。

[0161] 因此，末端执行器的操作性能评价函数：

[0162] f3(θ)＝‑(ΩR(R)+ΩL(R))

[0163] 其中，f3(θ)为末端执行器的操作性能评价函数；ΩR(R)为右手的操作性能；ΩL(R)为左手的操作性能。

[0164] (4)关节位移评价函数：

[0165] 为了节能，需要移动双臂机器人有最小的关节位移，故而引入了基于质量的关节位移评价指标：

[0166]

[0167]

[0168]

[0169] 其中，f4(θ)为关节位移评价函数，用于反映移动双臂机器人最重部分具有最大的移动难度；n为广义变量θ的维度；θi为广义变量；θimin为广义变量θi的左边界；θimax为广义变量θi的右边界；Wi为基于质量分布的权重；Mi为连杆i到连杆k的总质量；Mj为连杆j到连杆k的总质量；ne为末端执行器的下标；mk为连杆k的质量。

[0170] (5)末端执行器相对底座的位移评价函数：

[0171] 为了实现移动底座与操作臂之间的协同；基于末端执行器在移动底座坐标系中的位移评价函数定义为：

[0172] f5(θ)＝||xRb(θ)‑xRbi||+||xLb(θ)‑xLbi||

[0173] 其中，f5(θ)为末端执行器相对底座的位移评价函数；xRb(θ)为右手在移动底座坐标系中的期望位置；xRbi为右手在移动底座坐标系中的初始位置；xLb(θ)为左手在移动底座坐标系中的期望位置；xLbi为左手在移动底座坐标系中的初始位置。

[0174] 本实施例所述的移动双臂机器人运动规划方法中，假定移动双臂机器人的左右手期望抓取位姿已知，需要确定移动双臂机器人的移动底座、上身和双臂的位姿。

[0175] 假定环境中没有障碍物，依据右或左手期望抓取位姿，根据移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动底座位姿区域；其设计结果如附图6‑7及附图9‑10所示，从附图6‑7及附图9‑10中可以看出，确定移动底座可选择区域为移动底座落在地面的区域，即z坐标为[‑δ,+δ]的区域，δ>0，进而确定移动底座的可选区域为环绕右及左手的环形区域。

[0176] 从可选位姿集中选择目标函数最优的位姿作为移动双臂机器人的期望抓取位姿，结果如附图8、11所示，从附图8、11中可以看出，移动双臂机器人的抓取位姿是可预见的、仿人的，并未出现奇异位姿。

[0177] 其次，假设环境中有一矩形障碍物，移动底座相对右手的RiDRM图的更新时间为0.49366s，移动底座相对左手的LiDRM图的更新时间为0.41696517s；则有障碍物时右手的RiDRM图与移动底座位姿的可选区域设计结果如附图12所示，有障碍是左手的LiDRM图与移动底座位姿的可选区域设计结果如附图13所示；从附图12及附图13中可以看出，可选位姿的数量由17个变化为6个或8个；根据右手的抓取位姿进行有障碍物时的位姿规划结果如附图14所示，从附图14中可以看出，根据右手的抓取位姿进行有障碍物时的位姿规划时间为
17.691014s；根据左手的抓取位姿进行有障碍物时的位姿规划如附图15所示，从附图15中可以看出，根据左手的抓取位姿进行有障碍物时的位姿规划时间为17.409708s；根据左右手的抓取位姿确定移动底座的可选位姿，如附图16所示，从附图16中可以看出，左右手同时操作时移动底座的可选位姿区域为一环形区域，且较集中分布于左右手期望位置的对称轴区域。

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

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

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

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