動(dòng)作捕捉機(jī)器人協(xié)同柔性姿態(tài)控制的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種動(dòng)作捕捉機(jī)器人協(xié)同柔性姿態(tài)控制的方法，屬于人機(jī)交互領(lǐng)域和 人工智能領(lǐng)域。
【背景技術(shù)】
[0002] 人機(jī)交互（HCI，Human-ComputerInteraction)是一門關(guān)于設(shè)計(jì)、評(píng)價(jià)和實(shí)現(xiàn)供人 們使用的交互式計(jì)算機(jī)系統(tǒng)，以及研究由此而發(fā)生的相關(guān)現(xiàn)象的學(xué)科。人機(jī)交互技術(shù)主要 是研究人與計(jì)算機(jī)之間的信息交換，體現(xiàn)在人到計(jì)算機(jī)和計(jì)算機(jī)到人的信息交互兩部分， 前者是指人們可以借助鍵盤、鼠標(biāo)、操作桿、位置跟蹤器、數(shù)據(jù)手套等設(shè)備，用手、腳、聲音、 姿態(tài)和身體的動(dòng)作、視線甚至腦電波等向計(jì)算機(jī)傳遞信息；后者是計(jì)算機(jī)通過(guò)打印機(jī)、繪圖 儀、顯示器、頭盔式顯示器、音箱等輸出設(shè)備或顯示設(shè)備給人提供信息。
[0003] Kinect是微軟在2009年6月2日的E3大展上，正式公布的XB0X360體感周邊外 設(shè)。Kinect徹底顛覆了游戲的單一操作，使人機(jī)互動(dòng)的理念更加徹底的展現(xiàn)出來(lái)。它是一 種3D體感攝影機(jī)，同時(shí)它導(dǎo)入了即時(shí)動(dòng)態(tài)捕捉、影像辨識(shí)、麥克風(fēng)輸入、語(yǔ)音辨識(shí)、社群互 動(dòng)等功能。體感，即軀體感覺(jué)，是觸覺(jué)、壓覺(jué)、溫覺(jué)、痛覺(jué)和本體感覺(jué)（關(guān)于肌肉和關(guān)節(jié)位置 和運(yùn)動(dòng)、軀體姿勢(shì)和運(yùn)動(dòng)以及面部表情的感覺(jué)）的總稱。采用LightCoding和光源標(biāo)定技 術(shù)的Kinect利用紅外線發(fā)射器發(fā)出雷射光，通過(guò)紅外線CMOS攝像機(jī)記錄空間中的每個(gè)散 斑，結(jié)合原始散斑圖案，通過(guò)晶片計(jì)算出具有3D深度的圖像，進(jìn)而轉(zhuǎn)換到骨架追蹤系統(tǒng)，使 得Kinect可以應(yīng)用到許多領(lǐng)域。
[0004] ΝΑ0是由AldebaranRobotics公司研制的一個(gè)身高58厘米的可編程仿人機(jī)器 人。其身體具有25個(gè)自由度，主要元件為電機(jī)和電動(dòng)致動(dòng)器。其運(yùn)動(dòng)模炔基于廣義逆運(yùn)動(dòng) 學(xué)，可處理笛卡爾坐標(biāo)系、關(guān)節(jié)控制、平衡、冗余性和任務(wù)優(yōu)先性。ΝΑ0頭部?jī)?nèi)嵌英特爾ATOM 1. 6GHz處理器，運(yùn)行LINUX內(nèi)核，擁有NAOqi操作系統(tǒng)，是一個(gè)開(kāi)源的軟件平臺(tái)，可用C++或 Python語(yǔ)言進(jìn)行編程?；谝陨咸攸c(diǎn)，機(jī)器人ΝΑ0可以最逼真地實(shí)現(xiàn)動(dòng)作跟蹤，是本發(fā)明的 重要組成部分。

【發(fā)明內(nèi)容】

[0005] 針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明涉及一種動(dòng)作捕捉機(jī)器人協(xié)同柔性姿態(tài)控制的方 法。本發(fā)明的目的在于為人機(jī)交互提出一種新模式。
[0006] 本發(fā)明的技術(shù)方案如下：
[0007] -種動(dòng)作捕捉機(jī)器人協(xié)同柔性姿態(tài)控制的方法，包括步驟如下：
[0008] (1)利用體感設(shè)備Kinect實(shí)時(shí)捕獲不同時(shí)刻目標(biāo)人體對(duì)應(yīng)目標(biāo)關(guān)節(jié)位移數(shù)據(jù)；目 標(biāo)人體站在體感設(shè)備Kinect視距范圍內(nèi)，為保證最佳識(shí)別效果，目標(biāo)人體應(yīng)處在體感設(shè)備 Kinect的鏡頭前1. 2-3. 5m，水平視角±57°以內(nèi)；
[0009] (2)將體感設(shè)備Kinect捕獲到的目標(biāo)關(guān)節(jié)位移數(shù)據(jù)發(fā)送到PC端；
[0010] (3)PC端接收所有目標(biāo)關(guān)節(jié)位移數(shù)據(jù)，并根據(jù)其實(shí)時(shí)繪制出人體骨骼框架；
[0011] (4)PC端將人體骨骼框架動(dòng)態(tài)顯示在屏幕上，并提供報(bào)錯(cuò)反饋；
[0012] (5)PC端對(duì)接收到的所有目標(biāo)關(guān)節(jié)位移數(shù)據(jù)進(jìn)行處理：包括相對(duì)閾值距離比較濾 波計(jì)算、空間向量計(jì)算，得到人形機(jī)器人ΝΑ0的姿態(tài)控制數(shù)據(jù)，即關(guān)節(jié)角度數(shù)據(jù)；本發(fā)明由 于環(huán)境因素和傳感器自身抖動(dòng)等原因，導(dǎo)致得到的原始數(shù)據(jù)里存在干擾數(shù)據(jù)，因此有必要 對(duì)原始數(shù)據(jù)進(jìn)行相對(duì)閾值距離比較濾波計(jì)算，使得機(jī)器人動(dòng)作追蹤更加準(zhǔn)確、可靠；
[0013] (6)調(diào)用人形機(jī)器人ΝΑ0的NAOqi操作系統(tǒng)中的JointControl函數(shù)，根據(jù)傳送來(lái) 的關(guān)節(jié)角度數(shù)據(jù)，對(duì)人形機(jī)器人ΝΑ0的舵機(jī)進(jìn)行控制，使人形機(jī)器人ΝΑ0實(shí)時(shí)跟蹤目標(biāo)人體 動(dòng)作。
[0014] 根據(jù)本發(fā)明優(yōu)選的，所述步驟（4)還包括，所述PC端動(dòng)態(tài)顯示人體骨骼框架：如 果PC端顯示的人體骨骼框架與目標(biāo)人體的動(dòng)作一致，則將關(guān)節(jié)角度數(shù)據(jù)發(fā)送至人形機(jī)器 人ΝΑ0;如果PC端顯示的人體骨骼框架與目標(biāo)人體的動(dòng)作不匹配，則重新初始化程序，以保 證人形機(jī)器人ΝΑ0收到可靠姿態(tài)控制數(shù)據(jù)。
[0015] 根據(jù)本發(fā)明優(yōu)選的，所述步驟（5)中，所述相對(duì)閾值距離比較濾波計(jì)算包括：
[0016] 通過(guò)觀察位移關(guān)節(jié)點(diǎn)的空間坐標(biāo)變化，計(jì)算固定時(shí)間段（例如：0.ls-0. 5s)內(nèi)同 一關(guān)節(jié)在開(kāi)始時(shí)間和結(jié)束時(shí)間組成的波動(dòng)向量，觀察該波動(dòng)向量的模以及在空間坐標(biāo)系各 個(gè)方向的波動(dòng)，通過(guò)設(shè)定波動(dòng)閾值篩選關(guān)節(jié)波動(dòng)值。可以看出關(guān)節(jié)點(diǎn)識(shí)別位置的抖動(dòng)主要 是沿坐標(biāo)軸方向的快速抖動(dòng)，且出現(xiàn)抖動(dòng)時(shí)波動(dòng)向量的模會(huì)大幅增加。因此，應(yīng)對(duì)波動(dòng)較大 的關(guān)節(jié)點(diǎn)做相應(yīng)處理，對(duì)小幅度波動(dòng)的關(guān)節(jié)點(diǎn)保持上一狀態(tài)不變，對(duì)于不同的關(guān)節(jié)處使用 不同的閾值比較，保證每次濾波后的結(jié)果都是最優(yōu)解，以保證機(jī)器人姿態(tài)變換的連續(xù)性。
[0017] 根據(jù)本發(fā)明優(yōu)選的，所述步驟（5)中，所述空間向量計(jì)算包括：
[0018]Kinect所使用的空間坐標(biāo)系不同于常見(jiàn)的空間坐標(biāo)系，其X軸與y軸的零點(diǎn)與傳 統(tǒng)空間坐標(biāo)系相同，但其z軸坐標(biāo)零點(diǎn)為Kinect傳感器，正方向?yàn)镵inect指向的正前方。 Kinect空間坐標(biāo)系如圖6所示：
[0019] 由Kinect空間坐標(biāo)系中，得出Kinect坐標(biāo)系中任意兩個(gè)不重合的坐標(biāo)點(diǎn) PI(xl，yl，zl)，P2 (x2,y2,z2)，對(duì)其組成的向量P1P2:
[0020]
[0021] 若存在另一點(diǎn)P3且該點(diǎn)不在P1P2所在的直線上，則存在以下關(guān)系式：
[0022]
[0023] 根據(jù)上述性質(zhì)，將人體關(guān)節(jié)角度計(jì)算簡(jiǎn)化為對(duì)空間向量夾角的計(jì)算，下面將分別 說(shuō)明上肢關(guān)節(jié)角度的計(jì)算方法：
[0024] 1)關(guān)節(jié)角度：LeftElbowRoll
[0025] 如圖7所示：計(jì)算LeftElbowRoll角度只需構(gòu)造該空間夾角兩邊所在的一組向 量，
[0026]并根據(jù)上文中提到的關(guān)節(jié)角度計(jì)算公式得：
[0027]
[0028]2)關(guān)節(jié)角度：LeftElbowYaw
[0029] 如圖8所示：
[0030]LeftElbowYaw關(guān)節(jié)的角度為肘部繞上臂旋轉(zhuǎn)時(shí)產(chǎn)生的角度，此角度通常情況下 為ABC和BCD兩相交平面的夾角，即圖中平面S1和S2的夾角；根據(jù)空間平面夾角計(jì)算公式 得出角度LeftElbowRoll的計(jì)算方法如下。
[0031] 首先需要明確計(jì)算兩非共線向量所在平面的法向量的公式：
[0032]
[0033] 因此SI、S2平面的法向量表示為：
[0034]
[0035]
[0036]LeftElbowYaw關(guān)節(jié)的角度即等于Ml、M2兩法向量的夾角：
[0037]
[0038] 3)關(guān)節(jié)角度：LeftShoulder.Roll
[0039] 加圖9所元？
[0040]
[0041] 4)關(guān)節(jié)角度：LeftShoulderPitch
[0042] 如圖10所示：
[0043]LeftShoulderPitch關(guān)節(jié)的角度為上臂與雙肩軸線組成的平面與雙肩軸線與脊 柱點(diǎn)組成的平面之間的夾角，此角度通常情況下為ABC和BCD兩相交平面的夾角，類比Left ElbowYaw關(guān)節(jié)角的計(jì)算方法，根據(jù)空間平面夾角計(jì)算公式可得出角度LeftShoulder Pitch的計(jì)算方法如下：
[0044] 平面ABC的法向量：
[0045]
[0046] 平面B⑶的法向量：
[0047]
[0048]關(guān)節(jié)角LeftShoulderPitch計(jì)算：
[0049]
[0050] 5)手部張合角度：LeftHand
[0051] 由于KINECT對(duì)于手部信息的讀取無(wú)法精確到手指的狀態(tài)，因此對(duì)于手部張合角 度計(jì)算，通過(guò)計(jì)算Lefthand和LeftWrist之間的距離來(lái)估算手部張合的角度，計(jì)算方法 如下：
[0052] 如圖11所示：
[0053]
[0054] 至此，左臂所有關(guān)節(jié)的角度全部計(jì)算完畢，同理可以計(jì)算右臂關(guān)節(jié)角度；
[0055]6)關(guān)節(jié)角度：HeadPitch
[0056] 頭部?jī)蓚€(gè)關(guān)節(jié)中，與低頭和抬頭有關(guān)的角度名稱HeadPitch
[0057] 如圖12所示：
[0058] 頭部關(guān)節(jié)HeadPitch的角度選取肩中指向頭部的向量與兩肩與脊柱組成的平面 的法向量之間的夾角，具體計(jì)算公式如下：
[0059]
[0060]
[0061] 7〇 關(guān)節(jié)角：HeadYaw
[0062] 如圖13所示：
[0063] 頭部另外一個(gè)重要的關(guān)節(jié)角HeadYaw負(fù)責(zé)實(shí)現(xiàn)頭部左右轉(zhuǎn)動(dòng)，由于KINECT骨骼 識(shí)別結(jié)果中無(wú)法給出面部朝向的數(shù)據(jù)信息，因此HeadYaw角度無(wú)法進(jìn)行直接計(jì)算，然而通 過(guò)觀察骨骼繪制動(dòng)畫(huà)可以看出，頭部的空間位置明顯位于肩中的前方（即正面面對(duì)KINECT 時(shí)，Head點(diǎn)和Shouldercenter點(diǎn)在z軸上存在一定距離），因此關(guān)于此關(guān)節(jié)角本文將轉(zhuǎn)化 為平面夾角的計(jì)算：
[0064] 平面ABC的法向量
[0065]
[0066] 平面BCD法向量
[0067]
[0068] 關(guān)節(jié)角度
[0069]
[0070] 在機(jī)器人的運(yùn)動(dòng)過(guò)程中下肢關(guān)節(jié)活動(dòng)將直接影響到機(jī)器人整體的平穩(wěn)性，為了簡(jiǎn) 化控制難度，本文對(duì)于下肢控制采用相對(duì)位置法，通過(guò)計(jì)算下肢末端在相對(duì)坐標(biāo)系中