基于Octomap的仿人机器人局部环境与能力图模型算法

易康 赵玉婷 齐新社

摘 要：基于3D点云数据的机器人三维空间能力图模型算法存在体素网格搜索计算量大的问题，由于OcTree在三维空间细分时的层次化优势，提出一种基于Octomap的局部环境与能力图模型算法。首先，根据NAO机器人的关节组成、正向运动学、逆向运动学和刚体坐标变换，对NAO仿人机器人构建全身二叉树状运动学模型;其次在此基础上使用前向运动学在笛卡儿空间计算离散的三维可达点云，并将其作为机器人终端效应器的基础工作空间;然后重点描述将点云空间表示转化为Octomap空间节点表示的方法，尤其是空间节点的概率更新方法;最后提出根据节点几何关系进行空间节点更新顺序选择的优化方法，从而高效地实现了仿人机器人能力图的空间优化表示。实验结果表明，相对于之前的原始Octomap更新方法，优化后的算法能降低近30%空间节点数，提高计算效率。

关键词：仿人机器人; 运动学模型; Octomap; 抓取; 能力图

中图分类号：TP242

文献标志码：A

文章编号：1001-9081（2019）04-1220-04

Abstract： The 3D capability map model of humanoid robot based on 3D point cloud data has the disadvantage of large voxel mesh searching computation. Considering the hierarchical advantage of OcTree in 3D space subdivision， a local environment and capability map model based on Octomap was proposed. Firstly， a binary-tree-like kinematics model of NAO humanoid robot was constructed according to the joint composition， forward kinematics， inverse kinematics and rigid body coordinate transformation of NAO robot. Secondly， the forward kinematics was used to calculate the 3D discrete reachable point clouds in Cartesian space， which were used as the basic workspace of the robot terminal effector. Thirdly， the methods of transforming the point cloud space representation into Octomap space node representation， especially the probability updating method of space node， were described emphatically. Finally， an optimization method of space node updating order selection was proposed according to the geometric relationship of nodes. With this optimization method， the space optimization representation of the humanoid robot's capability map was realized efficiently. Experimental results show that compared with the original Octomap updating method， the proposed algorithm can reduce the number of space nodes by nearly 30% and improve the computional efficiency.

Key words： humanoid robot; kinematics model; Octomap; grasp; capability map

0 引言

機器人作为一种最典型的应用范围广、技术附加值高的数字控制装备，在现代先进生产制造业中发挥越来越重要的作用。除工业机器人外，仿人机器人是模仿人的形态和行为而设计制造的机器人，一般由模仿人头部、四肢和躯干的机器人连杆和大量的传感器组成。搬运工业机器人大都是通过示教或预编程对其进行操作的，机器人只是完成点到点的动作，不能适应真实的复杂场景中对于物体感知识别定位和进行人机交互。如今人们对机器人技术提出了更高的要求，希望机器人具有更高的智能和更强的环境适应能力。标定的彩色和深度图像[1]是机器人感知环境的重要信息来源，抓取是机器人最基本的功能也是最重要的功能之一[2]，结合视觉信息对仿人机器人的抓取控制反馈，是仿人机器人具备更加智能化的感知能力的前提，也是如今研究的重点。

要构建仿人机器人局部环境与能力图，需要对机器人进行运动学建模，在这方面国内外有大量的文献支撑。针对NAO的运动学建模在文献[3]中有详细描述，Kofinas等[4]将NAO机器人分解成五个独立的部分，得出了NAO机器人完整的正逆运动学的解析解。文献[5]最早提出了机器人能力图的表示方法和构建方法，文献[6]则进一步提出了NAO机器人的能力图构建和表示方法。本文在此基础上提出了一种基于Octomap的机器人局部环境与能力图模型算法，并提出根据节点几何关系进行空间节点更新顺序选择的优化方法。

1 仿人机器人的运动学建模

1.1 NAO仿人机器人建模

机器人的运动学主要研究机器人连杆或终端效应器相对于某个特定参考坐标系的运动几何学关系，为控制机器人运动提供手段和方法。本节分别介绍对仿人机器人NAO的运动学抽象与建模和正逆问题[3，7]求解。

本文系统使用的是日本软银公司的NAO仿人机器人平台。NAO机器人有25个自由度，下半身包括腿和骨盆共有11个自由度，上半身的脑袋、手臂和躯干共14个自由度。每个手臂在肩膀上有2个自由度，肘部有2个自由度，手腕1个自由度，还有1个自由度用来控制手的操作[8]。

对机器人的连杆和关节角进行分离，每个连杆都有两个分支，以二叉树的形式表示连杆之间的关系：左子树表示“子”连杆;右子树表示“兄弟”连杆，如图1所示。

为了从二叉树结构的连杆关系中获取相邻连杆的关系，从树根开始记为1，依次给每个连杆一个编号，这样对于机器人的任意连杆都可以以二叉树遍历的方式进行访问。

1.2 正向运动学计算

正向运动学（Forward Kinematics， FK）通过计算连杆关节角配置解决终端效应器的六自由度（6 Degree of Freedom，6DoF）位姿问题。根据机器人链式法则，计算得到目标连杆的位置与姿态矩阵[3]：

1.3 逆向运动学计算

在机器人技术中，逆向運动学（Inverse Kinematics， IK）[7]利用运动学方程来确定为机器人末端执行器提供期望位置的关节参数，将机器人的运动规划转化为关节运动向量从而指导与关节级联的电机的运动。本文采用的是通过迭代求解正运动学问题而得到最优近似解的算法[3]，其核心思想就是对正运动学方程进行建模，在一定的误差范围内停止迭代计算。

1.4 刚体坐标转换

一个物体在空间的表示可以这样实现[3，9]：通过在它上面固连的一个局部坐标系，由于坐标系一直固连在该物体上，因此只要这个坐标系在空间表示出来，它相对于机器人坐标系的变换矩阵求出来，那么这个物体相对于固定坐标系的位姿也就已知了。

其中：RobotTlocal表示从物体局部坐标系到机器人坐标系的齐次坐标变换，根据固定在NAO机器人头上立体传感器位置所确定。在本文中，NAO机器人使用左手坐标系，而立体传感器使用右手坐标系，从右手坐标系转到左手坐标系需要调整轴的顺序，因此在齐次变换之前对三轴进行变换，[x， y，z]=[x，z， y]，之后计算相机到头的平移量得到RobotTlocal齐次变换矩阵：

2 仿人机器人能力图

机器人的工作空间是机器人使用末端效应器影响环境的一部分，因此，仿人机器人NAO的工作空间是它的手、脚或头能够到达环境的一部分。机器人的这些部分位于各自的运动连杆的末端，是机器人与周围世界互动的主要方式。众所周知，每个终端效应器能够工作的工作空间非常重要，因为它保证机器人得以继续完成之后的任务。仿人机器人试图抓取能力空间以外的物体，将导致电机发热，甚至失衡等严重后果。

2.1 能力图运动学表示

当确定机器人能力内的工作空间时，不仅仅是确定空间中三维点的坐标，应该还包括点上对应的旋转位姿，这样可以称作机器人终端效应器的灵巧空间，本节将集中在机器人手操作的能力图[5]中，并通过机器人运动学的方式求解，这对于成功完成抓取任务是非常重要的。

为机器人的运动链构建可达图，需要从关节电机空间进行采样。通过计算每个样本的正向运动学，可以确定末端执行器的空间工作空间，算法如下。

1）初始化机器人连杆位姿及关节向量。

这种基于运动学的方法需要知道机器人的前臂和上臂的恒定长度。对于仿人机器人NAO来说，通过肩关节和肘关节角运动范围和连杆长度确定工作空间。如表1和表2所示，这里以机器人左臂为例。

图2表明，机械手的工作空间是一个近似球形的形状，集中在机器人的肩部[5]。形状向仿人机器人的背面变形，后面的某些点是不可到达的，为了绘图的目的，旋转的结果就是丢弃这部分，只表征了空间的三维点。

2.2 能力图Octomap表示

2.2.1 Octomap表示的必要性

从上文可知，给定目标物体的6DoF位姿RobotFobject，根据上述算法得到离散空间点的计算取决于采样精度。判断目标物体是否在工作空间之内，将遭受暴力搜索带来的时间效率上的延迟，并且，在基于采样空间是仿人机器人实际的灵巧空间的子集的前提下：CSCR，推导出：p∈CR， pCS，因此，为了降低维度和进行高效地体素网格搜索，更新空间表示的数据结构是必不可少的[6，10]。本节试图通过Octomap这种新的表示方法来构建能力图和局部环境，并提出通过空节点优化提升构建效率的方法。

2.2.2 使用Octomap对三维空间建模

基于OcTree的三维空间地图Octomap可完成三维空间地图构建[11-12]。Octomap是一个开放源码且抽象封装的C++库。它基于伯克利软件发行版（Berkeley Software Distribution， BSD）协议许可证，并在GitHub上发布，体现了完整的开放源码理念和思想，为智能移动机器人三维空间地图的研究创造了新起点。OcTree是一种用于三维空间细分的层次化数据结构，八叉树中的每个节点表示包含在立方体的空间，通常称为体素。这些体素通常递归地被八分割直到达到Octomap的最小分辨率，图3展示八分割的原理。

最小体素大小决定了八叉树的分辨率。在Octomap的实现中，八叉树的层次最大到16层以表征足够大的真实空间，其中三轴的尺度由式（6）表示：

将向量中的值转化成二进制的表示形式，根据三轴每个层次的二进制值组合，可以得到空间点在Octomap中每个层次的状态，如表3所示。

从上到下、从左往右，得到向量V=[5，2，2，2，2，2，2，2，2，5，5，2，2，7，0，0]表示空间点p=（1，-1，1）在Octomap中每一层节点的序号。

在实时局部环境中，往往会受到噪点和移动物体的影响，在更新Octomap节点时候，使用概率的方式表征某个节点占有、未知、空的三个状态。叶子节点n在传感器测量时间范围z1：t被占有的概率P（n|z1：t）可表示根据式（11）确定：

因此，对空间点状态的概率问题在最大值和最小值限定的前提下转化成了实数空间的加和问题。

2.2.3 空节点优化

构建和更新Octomap的过程中，需要当完成节点V1构建之后，在以某一轴为基准的前提下需要确认在另外两轴构建节点的优先级，如图4所示。

3 实验与结果分析

为验证本文提出的空间建模方法，实验中参考利用NAO机器人仿真平台进行其自身能力图的仿真测试，同时动态更新地图中采用Kinect V1传感器作为视觉输入，进行环境数据采集，Kinect V1可以同时获取环境的色彩数据和深度数据，并形成点云数据作为动态环境地图的数据输入。

动态更新的三维地图和静态构建的能力图为仿人机器人在未知环境中提供了重要信息，如图5所示的例子中，仿人机器人需要至少移动T=b-|r|b|b|抓取到目标物体。在不考虑旋转的前提下，能力图对最小移动距离进行了限定。

本文使用0.01的分辨率在可达离散空间的基础上构建了静态能力图，如图6所示。注意，本文为了展示效果忽略了旋转。

通过上文提到空节点优化，在更新节点时，能够大量减少空节点数目从而提升效率、降低冗余率，如图7所示，图中浅色点为空间中的空节点，深色点为可达到的最远节点。

从图7中可以明显看出，图（b）中空节点的数量明显低于图（a）。统计结果表示优化后的Octomap相对于优化之前，空间节点节省近30%。

动态更新的三维地图考虑到机器人非质点的自碰撞，向外膨胀了0.06m，构建如图8所示。

4 结语

仿人机器人在空间中抓取目标物体是一项复杂的工程，在整个过程中，获取自身终端效应器能力图以及局部环境的信息，是完成抓取操作的前提之一。本文以仿人机器人NAO作为平台，对链式的连杆和关节进行抽象建模，并以此为基础构建了基于运动学的离散能力空间，随后使用Octomap作为数据结构，提出一种基于Octomap的局部环境与能力图模型算法，在此基础上提出了根据节点几何关系进行空间节点更新顺序选择的优化方法，降低了空节点的数量，从而提升了构建效率。但是，实现仿人机器人的抓取，还需要视觉、导航等多个模块的协同。

参考文献（References）

[1] KHOSHELHAM K. Accuracy analysis of Kinect depth data[EB/OL]. [2018-05-10]. https：//people.eng.unimelb.edu.au/kkhoshelham/research_files/kinectaccuracy/isprsarchives-XXXVIII-5-W12-133-2011.pdf.

[2] TAN M， WANG S. Research progress on robotics[J]. Acta Automatica Sinica， 2013， 39（7）： 963-972.

[3] 梶田秀司. 仿人機器人[M]. 北京： 清华大学出版社， 2007： 44-50. （SHUUJI K. Humanoid Robots[M]. Beijing： Tsinghua University Press， 2007： 44-50.）

[4] KOFINAS N， ORFANOUDAKIS E， LAGOUDAKIS M G. Complete analytical forward and inverse kinematics for the NAO humanoid robot[J]. Journal of Intelligent & Robotic Systems， 2015， 77（2）： 251-264.

[5] ZACHARIAS F， BORST C， HIRZINGER G. Capturing robot workspace structure： representing robot capabilities[C]// Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2007： 3229-3236.

[6] MELLMANN H， COTUGNO G. Dynamic Motion Control： Adaptive Bimanual Grasping for a Humanoid Robot[M]. Amsterdam： IOS Press， 2011： 89-101.

[7] BURGET F， BENNEWITZ M. Stance selection for humanoid grasping tasks by inverse reachability maps[C]// Proceedings of the 2015 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2015： 5669-5674.

[8] 温淑慧， 王同辉， 薛红香， 等. NAO机器人的手臂建模及模糊控制算法研究[J]. 控制工程， 2018， 25（4）： 559-564. （WEN S H， WANG T H， XUE H X， et al. Study of arm dynamics modeling and fuzzy control of humanoid robots[J]. Control Engineering of China， 2018， 25（4）： 559-564.）

[9] SAKAGAMI Y， WATANABE R， AOYAMA C， et al. The intelligent ASIMO： system overview and integration[C]// Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2002： 2478-2483.

[10] MELLMANN H， SCHEUNEMANN M， STADIE O. Adaptive grasping for a small humanoid robot utilizing force and electric current sensors[EB/OL]. [2018-05-10]. https：//www2.informatik.hu-berlin.de/～mellmann/content/publications/data/CSP-Mellma nnScheunemannEtAl-13.pdf.

[11] HORNUNG A， WURM K M， BENNEWITZ M， et al. OctoMap： an efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous Robots， 2013， 34（3）： 189-206.

[12] LEIDNER D， DIETRICH A， SCHMIDT F， et al. Object-centered hybrid reasoning for whole-body mobile manipulation[C]// Proceedings of the 2014 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2014： 1828-1835.

[13] 王秋玥， 方明， 張海涛. 基于多空间混合约束的NAO机器人抓取轨迹规划[J]. 长春理工大学学报（自然科学版）， 2014， 37（6）： 107-110. （WANG Q Y， FANG M， ZHANG H T. Grasp track planning with the NAO robot under mixed constraints of multi-space[J]. Journal of Changchun University of Science and Technology （Natural Science Edition）， 2014， 37（6）： 107-110.）