基于Octomap的仿人機器人局部環(huán)境與能力圖模型算法

易康 趙玉婷 齊新社

摘 要：基于3D點云數(shù)據(jù)的機器人三維空間能力圖模型算法存在體素網(wǎng)格搜索計算量大的問題，由于OcTree在三維空間細分時的層次化優(yōu)勢，提出一種基于Octomap的局部環(huán)境與能力圖模型算法。首先，根據(jù)NAO機器人的關(guān)節(jié)組成、正向運動學(xué)、逆向運動學(xué)和剛體坐標變換，對NAO仿人機器人構(gòu)建全身二叉樹狀運動學(xué)模型;其次在此基礎(chǔ)上使用前向運動學(xué)在笛卡兒空間計算離散的三維可達點云，并將其作為機器人終端效應(yīng)器的基礎(chǔ)工作空間;然后重點描述將點云空間表示轉(zhuǎn)化為Octomap空間節(jié)點表示的方法，尤其是空間節(jié)點的概率更新方法;最后提出根據(jù)節(jié)點幾何關(guān)系進行空間節(jié)點更新順序選擇的優(yōu)化方法，從而高效地實現(xiàn)了仿人機器人能力圖的空間優(yōu)化表示。實驗結(jié)果表明，相對于之前的原始Octomap更新方法，優(yōu)化后的算法能降低近30%空間節(jié)點數(shù)，提高計算效率。

關(guān)鍵詞：仿人機器人; 運動學(xué)模型; 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?引言

機器人作為一種最典型的應(yīng)用范圍廣、技術(shù)附加值高的數(shù)字控制裝備，在現(xiàn)代先進生產(chǎn)制造業(yè)中發(fā)揮越來越重要的作用。除工業(yè)機器人外，仿人機器人是模仿人的形態(tài)和行為而設(shè)計制造的機器人，一般由模仿人頭部、四肢和軀干的機器人連桿和大量的傳感器組成。搬運工業(yè)機器人大都是通過示教或預(yù)編程對其進行操作的，機器人只是完成點到點的動作，不能適應(yīng)真實的復(fù)雜場景中對于物體感知識別定位和進行人機交互。如今人們對機器人技術(shù)提出了更高的要求，希望機器人具有更高的智能和更強的環(huán)境適應(yīng)能力。標定的彩色和深度圖像[1]是機器人感知環(huán)境的重要信息來源，抓取是機器人最基本的功能也是最重要的功能之一[2]，結(jié)合視覺信息對仿人機器人的抓取控制反饋，是仿人機器人具備更加智能化的感知能力的前提，也是如今研究的重點。

要構(gòu)建仿人機器人局部環(huán)境與能力圖，需要對機器人進行運動學(xué)建模，在這方面國內(nèi)外有大量的文獻支撐。針對NAO的運動學(xué)建模在文獻[3]中有詳細描述，Kofinas等[4]將NAO機器人分解成五個獨立的部分，得出了NAO機器人完整的正逆運動學(xué)的解析解。文獻[5]最早提出了機器人能力圖的表示方法和構(gòu)建方法，文獻[6]則進一步提出了NAO機器人的能力圖構(gòu)建和表示方法。本文在此基礎(chǔ)上提出了一種基于Octomap的機器人局部環(huán)境與能力圖模型算法，并提出根據(jù)節(jié)點幾何關(guān)系進行空間節(jié)點更新順序選擇的優(yōu)化方法。

1?仿人機器人的運動學(xué)建模

1.1?NAO仿人機器人建模

機器人的運動學(xué)主要研究機器人連桿或終端效應(yīng)器相對于某個特定參考坐標系的運動幾何學(xué)關(guān)系，為控制機器人運動提供手段和方法。本節(jié)分別介紹對仿人機器人NAO的運動學(xué)抽象與建模和正逆問題[3，7]求解。

本文系統(tǒng)使用的是日本軟銀公司的NAO仿人機器人平臺。NAO機器人有25個自由度，下半身包括腿和骨盆共有11個自由度，上半身的腦袋、手臂和軀干共14個自由度。每個手臂在肩膀上有2個自由度，肘部有2個自由度，手腕1個自由度，還有1個自由度用來控制手的操作[8]。

對機器人的連桿和關(guān)節(jié)角進行分離，每個連桿都有兩個分支，以二叉樹的形式表示連桿之間的關(guān)系：左子樹表示“子”連桿;右子樹表示“兄弟”連桿，如圖1所示。

為了從二叉樹結(jié)構(gòu)的連桿關(guān)系中獲取相鄰連桿的關(guān)系，從樹根開始記為1，依次給每個連桿一個編號，這樣對于機器人的任意連桿都可以以二叉樹遍歷的方式進行訪問。

1.2?正向運動學(xué)計算

正向運動學(xué)（Forward Kinematics， FK）通過計算連桿關(guān)節(jié)角配置解決終端效應(yīng)器的六自由度（6 Degree of Freedom，6DoF）位姿問題。根據(jù)機器人鏈式法則，計算得到目標連桿的位置與姿態(tài)矩陣[3]：

1.3?逆向運動學(xué)計算

在機器人技術(shù)中，逆向運動學(xué)（Inverse Kinematics， IK）[7]利用運動學(xué)方程來確定為機器人末端執(zhí)行器提供期望位置的關(guān)節(jié)參數(shù)，將機器人的運動規(guī)劃轉(zhuǎn)化為關(guān)節(jié)運動向量從而指導(dǎo)與關(guān)節(jié)級聯(lián)的電機的運動。本文采用的是通過迭代求解正運動學(xué)問題而得到最優(yōu)近似解的算法[3]，其核心思想就是對正運動學(xué)方程進行建模，在一定的誤差范圍內(nèi)停止迭代計算。

1.4?剛體坐標轉(zhuǎn)換

一個物體在空間的表示可以這樣實現(xiàn)[3，9]：通過在它上面固連的一個局部坐標系，由于坐標系一直固連在該物體上，因此只要這個坐標系在空間表示出來，它相對于機器人坐標系的變換矩陣求出來，那么這個物體相對于固定坐標系的位姿也就已知了。

其中：RobotTlocal表示從物體局部坐標系到機器人坐標系的齊次坐標變換，根據(jù)固定在NAO機器人頭上立體傳感器位置所確定。在本文中，NAO機器人使用左手坐標系，而立體傳感器使用右手坐標系，從右手坐標系轉(zhuǎn)到左手坐標系需要調(diào)整軸的順序，因此在齊次變換之前對三軸進行變換，[x， y，z]=[x，z， y]，之后計算相機到頭的平移量得到RobotTlocal齊次變換矩陣：

2?仿人機器人能力圖

機器人的工作空間是機器人使用末端效應(yīng)器影響環(huán)境的一部分，因此，仿人機器人NAO的工作空間是它的手、腳或頭能夠到達環(huán)境的一部分。機器人的這些部分位于各自的運動連桿的末端，是機器人與周圍世界互動的主要方式。眾所周知，每個終端效應(yīng)器能夠工作的工作空間非常重要，因為它保證機器人得以繼續(xù)完成之后的任務(wù)。仿人機器人試圖抓取能力空間以外的物體，將導(dǎo)致電機發(fā)熱，甚至失衡等嚴重后果。

2.1?能力圖運動學(xué)表示

當確定機器人能力內(nèi)的工作空間時，不僅僅是確定空間中三維點的坐標，應(yīng)該還包括點上對應(yīng)的旋轉(zhuǎn)位姿，這樣可以稱作機器人終端效應(yīng)器的靈巧空間，本節(jié)將集中在機器人手操作的能力圖[5]中，并通過機器人運動學(xué)的方式求解，這對于成功完成抓取任務(wù)是非常重要的。

為機器人的運動鏈構(gòu)建可達圖，需要從關(guān)節(jié)電機空間進行采樣。通過計算每個樣本的正向運動學(xué)，可以確定末端執(zhí)行器的空間工作空間，算法如下。

1）初始化機器人連桿位姿及關(guān)節(jié)向量。

這種基于運動學(xué)的方法需要知道機器人的前臂和上臂的恒定長度。對于仿人機器人NAO來說，通過肩關(guān)節(jié)和肘關(guān)節(jié)角運動范圍和連桿長度確定工作空間。如表1和表2所示，這里以機器人左臂為例。

圖2表明，機械手的工作空間是一個近似球形的形狀，集中在機器人的肩部[5]。形狀向仿人機器人的背面變形，后面的某些點是不可到達的，為了繪圖的目的，旋轉(zhuǎn)的結(jié)果就是丟棄這部分，只表征了空間的三維點。

2.2?能力圖Octomap表示

2.2.1?Octomap表示的必要性

從上文可知，給定目標物體的6DoF位姿RobotFobject，根據(jù)上述算法得到離散空間點的計算取決于采樣精度。判斷目標物體是否在工作空間之內(nèi)，將遭受暴力搜索帶來的時間效率上的延遲，并且，在基于采樣空間是仿人機器人實際的靈巧空間的子集的前提下：CSCR，推導(dǎo)出：p∈CR， pCS，因此，為了降低維度和進行高效地體素網(wǎng)格搜索，更新空間表示的數(shù)據(jù)結(jié)構(gòu)是必不可少的[6，10]。本節(jié)試圖通過Octomap這種新的表示方法來構(gòu)建能力圖和局部環(huán)境，并提出通過空節(jié)點優(yōu)化提升構(gòu)建效率的方法。

2.2.2?使用Octomap對三維空間建模

基于OcTree的三維空間地圖Octomap可完成三維空間地圖構(gòu)建[11-12]。Octomap是一個開放源碼且抽象封裝的C++庫。它基于伯克利軟件發(fā)行版（Berkeley Software Distribution， BSD）協(xié)議許可證，并在GitHub上發(fā)布，體現(xiàn)了完整的開放源碼理念和思想，為智能移動機器人三維空間地圖的研究創(chuàng)造了新起點。OcTree是一種用于三維空間細分的層次化數(shù)據(jù)結(jié)構(gòu)，八叉樹中的每個節(jié)點表示包含在立方體的空間，通常稱為體素。這些體素通常遞歸地被八分割直到達到Octomap的最小分辨率，圖3展示八分割的原理。

最小體素大小決定了八叉樹的分辨率。在Octomap的實現(xiàn)中，八叉樹的層次最大到16層以表征足夠大的真實空間，其中三軸的尺度由式（6）表示：

將向量中的值轉(zhuǎn)化成二進制的表示形式，根據(jù)三軸每個層次的二進制值組合，可以得到空間點在Octomap中每個層次的狀態(tài)，如表3所示。

從上到下、從左往右，得到向量V=[5，2，2，2，2，2，2，2，2，5，5，2，2，7，0，0]表示空間點p=（1，-1，1）在Octomap中每一層節(jié)點的序號。

在實時局部環(huán)境中，往往會受到噪點和移動物體的影響，在更新Octomap節(jié)點時候，使用概率的方式表征某個節(jié)點占有、未知、空的三個狀態(tài)。葉子節(jié)點n在傳感器測量時間范圍z1：t被占有的概率P（n|z1：t）可表示根據(jù)式（11）確定：

因此，對空間點狀態(tài)的概率問題在最大值和最小值限定的前提下轉(zhuǎn)化成了實數(shù)空間的加和問題。

2.2.3?空節(jié)點優(yōu)化

構(gòu)建和更新Octomap的過程中，需要當完成節(jié)點V1構(gòu)建之后，在以某一軸為基準的前提下需要確認在另外兩軸構(gòu)建節(jié)點的優(yōu)先級，如圖4所示。

3?實驗與結(jié)果分析

為驗證本文提出的空間建模方法，實驗中參考利用NAO機器人仿真平臺進行其自身能力圖的仿真測試，同時動態(tài)更新地圖中采用Kinect V1傳感器作為視覺輸入，進行環(huán)境數(shù)據(jù)采集，Kinect V1可以同時獲取環(huán)境的色彩數(shù)據(jù)和深度數(shù)據(jù)，并形成點云數(shù)據(jù)作為動態(tài)環(huán)境地圖的數(shù)據(jù)輸入。

動態(tài)更新的三維地圖和靜態(tài)構(gòu)建的能力圖為仿人機器人在未知環(huán)境中提供了重要信息，如圖5所示的例子中，仿人機器人需要至少移動T=b-|r|b|b|抓取到目標物體。在不考慮旋轉(zhuǎn)的前提下，能力圖對最小移動距離進行了限定。

本文使用0.01的分辨率在可達離散空間的基礎(chǔ)上構(gòu)建了靜態(tài)能力圖，如圖6所示。注意，本文為了展示效果忽略了旋轉(zhuǎn)。

通過上文提到空節(jié)點優(yōu)化，在更新節(jié)點時，能夠大量減少空節(jié)點數(shù)目從而提升效率、降低冗余率，如圖7所示，圖中淺色點為空間中的空節(jié)點，深色點為可達到的最遠節(jié)點。

從圖7中可以明顯看出，圖（b）中空節(jié)點的數(shù)量明顯低于圖（a）。統(tǒng)計結(jié)果表示優(yōu)化后的Octomap相對于優(yōu)化之前，空間節(jié)點節(jié)省近30%。

動態(tài)更新的三維地圖考慮到機器人非質(zhì)點的自碰撞，向外膨脹了0.06m，構(gòu)建如圖8所示。

4?結(jié)語

仿人機器人在空間中抓取目標物體是一項復(fù)雜的工程，在整個過程中，獲取自身終端效應(yīng)器能力圖以及局部環(huán)境的信息，是完成抓取操作的前提之一。本文以仿人機器人NAO作為平臺，對鏈式的連桿和關(guān)節(jié)進行抽象建模，并以此為基礎(chǔ)構(gòu)建了基于運動學(xué)的離散能力空間，隨后使用Octomap作為數(shù)據(jù)結(jié)構(gòu)，提出一種基于Octomap的局部環(huán)境與能力圖模型算法，在此基礎(chǔ)上提出了根據(jù)節(jié)點幾何關(guān)系進行空間節(jié)點更新順序選擇的優(yōu)化方法，降低了空節(jié)點的數(shù)量，從而提升了構(gòu)建效率。但是，實現(xiàn)仿人機器人的抓取，還需要視覺、導(dǎo)航等多個模塊的協(xié)同。

參考文獻（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]. 北京： 清華大學(xué)出版社， 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機器人抓取軌跡規(guī)劃[J]. 長春理工大學(xué)學(xué)報（自然科學(xué)版）， 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.）