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

易康 趙玉婷 齊新社

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

關(guān)鍵詞：仿人機(jī)器人; 運(yùn)動(dòng)學(xué)模型; Octomap; 抓取; 能力圖

中圖分類號(hào)：TP242

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：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?引言

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

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

1?仿人機(jī)器人的運(yùn)動(dòng)學(xué)建模

1.1?NAO仿人機(jī)器人建模

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

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

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

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

1.2?正向運(yùn)動(dòng)學(xué)計(jì)算

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

1.3?逆向運(yùn)動(dòng)學(xué)計(jì)算

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

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

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

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

2?仿人機(jī)器人能力圖

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

2.1?能力圖運(yùn)動(dòng)學(xué)表示

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

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

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

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

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

2.2?能力圖Octomap表示

2.2.1?Octomap表示的必要性

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

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

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

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

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

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

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

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

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

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

3?實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

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

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

4?結(jié)語

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

參考文獻(xiàn)（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] 梶田秀司. 仿人機(jī)器人[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ī)器人的手臂建模及模糊控制算法研究[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ī)器人抓取軌跡規(guī)劃[J]. 長春理工大學(xué)學(xué)報(bào)（自然科學(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.）