一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法
【專利摘要】本發(fā)羽涉及一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，其特征在于，該方法包括：步驟S1：以支撐腳為坐標(biāo)原點(diǎn)，對(duì)身體基坐標(biāo)系引入浮體運(yùn)動(dòng)學(xué)，建立一個(gè)基于全身關(guān)節(jié)的關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型；步驟S2：基于關(guān)帶鏈和運(yùn)動(dòng)學(xué)模型，引入關(guān)節(jié)角范圍約束、穩(wěn)定性約束，利用蒙特卡洛法采樣并求解機(jī)器人末端執(zhí)行器的工作空間，獲得工作空間構(gòu)成的三維點(diǎn)云網(wǎng)格；步驟S3：在三維點(diǎn)云網(wǎng)格的基礎(chǔ)上，進(jìn)行平面截取，并通過極值分析，得到機(jī)器人在各個(gè)方向上的極值狀態(tài)與位姿圖。與現(xiàn)有技術(shù)相比，本發(fā)明以求解復(fù)雜全身運(yùn)動(dòng)下的機(jī)器人工作奈間為目標(biāo)，基于數(shù)值算法，求取滿足固有約束的復(fù)雜情形下末端軌跡工作空間的邊界范圍。
【專利說明】-種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人控制領(lǐng)域，尤其是涉及一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的 方法。

【背景技術(shù)】
[0002] 人形機(jī)器人在日常使用中的一個(gè)常見任務(wù)為利用手臂和手去抓取物體，其中一個(gè) 相關(guān)的問題為機(jī)器人可以碰到的范圍或者空間。在這些任務(wù)中，機(jī)器人的操作手是否可以 碰到目標(biāo)物體是必須解答的問題。如圖1中的判斷流程，如果物體在手的可到達(dá)范圍內(nèi)，機(jī) 器人可以直接執(zhí)行抓取行為來完成任務(wù)，否則的話，機(jī)器人需要通過移動(dòng)或者行走，使得機(jī) 器人的手臂可到達(dá)空間包含目標(biāo)物體。
[0003] 機(jī)器人工作空間為末端執(zhí)行器在約束下可以達(dá)到的笛卡爾空間，就是機(jī)器人末端 構(gòu)件上的一點(diǎn)所能達(dá)到的點(diǎn)的集合。機(jī)器人工作空間的大小代表了機(jī)器人的活動(dòng)范圍，它 是衡量機(jī)器人工作能力的一個(gè)重要運(yùn)動(dòng)學(xué)指標(biāo)。對(duì)于每個(gè)末端執(zhí)行器的工作空間的分析與 計(jì)算有助于機(jī)器人對(duì)任務(wù)進(jìn)行更為有效的規(guī)劃。如機(jī)器人對(duì)目標(biāo)物的抓取與釋放的任務(wù)前 提，就是目標(biāo)物在機(jī)器人的工作空間范圍內(nèi)。
[0004] 一般機(jī)器人抓取的定位精度不高，往往需要重復(fù)定位，影響其定位精度的一個(gè)主 要因素就是現(xiàn)有方法主要基于傳統(tǒng)的運(yùn)動(dòng)學(xué)，且身體基座標(biāo)為恒定靜止，從而計(jì)算出的機(jī) 器人運(yùn)動(dòng)空間構(gòu)建不精準(zhǔn)、不全面。


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

[0005] 本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種固有約束下構(gòu) 建機(jī)器人運(yùn)動(dòng)空間的方法，以求解復(fù)雜全身運(yùn)動(dòng)下的機(jī)器人工作空間為目標(biāo)，基于數(shù)值算 法，求取滿足固有約束的復(fù)雜情形下末端軌跡工作空間的邊界范圍。
[0006] 本發(fā)明的目的可以通過以下技術(shù)方案來實(shí)現(xiàn)：
[0007] -種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，該方法包括以下步驟：
[0008] 步驟SI :以支撐腳為坐標(biāo)原點(diǎn)，對(duì)機(jī)器人身體基坐標(biāo)系引入浮體運(yùn)動(dòng)學(xué)，建立一 個(gè)基于全身關(guān)節(jié)的關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型；
[0009] 步驟S2 :基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，引入關(guān)節(jié)角范圍約束、穩(wěn)定性約束，利用蒙特 卡洛法采樣并求解機(jī)器人末端執(zhí)行器的工作空間，獲得工作空間構(gòu)成的三維點(diǎn)云網(wǎng)格；
[0010] 步驟S3 :在三維點(diǎn)云網(wǎng)格的基礎(chǔ)上，進(jìn)行平面截取，并通過極值分析，得到機(jī)器人 在各個(gè)方向上的極值狀態(tài)與位姿圖。
[0011] 所述的步驟S2具體為：
[0012] 步驟201 :在運(yùn)動(dòng)關(guān)節(jié)的關(guān)節(jié)角約束、穩(wěn)定性約束范圍內(nèi)，采用蒙特卡洛法進(jìn)行線 性隨機(jī)采樣獲得關(guān)節(jié)狀態(tài)值，每次隨機(jī)采樣獲得的一組參數(shù)作為一個(gè)樣本點(diǎn)；
[0013] 步驟202 :基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，對(duì)樣本點(diǎn)進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算，獲得機(jī)器人質(zhì) 心以及末端執(zhí)行器位姿點(diǎn)，以基坐標(biāo)系為基礎(chǔ)繪制位姿點(diǎn)的三維點(diǎn)云，得到末端執(zhí)行器工 作空間；
[0014] 步驟203 :三維點(diǎn)云經(jīng)過網(wǎng)格矩陣賦值后得到三維點(diǎn)云網(wǎng)格。
[0015] 所述的網(wǎng)格矩陣賦值具體為：對(duì)三維點(diǎn)云中每個(gè)位姿點(diǎn)用最鄰近的網(wǎng)格點(diǎn)表示， 即將相近的位姿點(diǎn)歸一化到同一個(gè)網(wǎng)格點(diǎn)中，每個(gè)網(wǎng)格點(diǎn)都居于各自網(wǎng)格空間的中心。
[0016] 所述的平面截取具體為：根據(jù)關(guān)鍵位置或需求位置對(duì)三維點(diǎn)云網(wǎng)格進(jìn)行平面截 取，在同一平面上數(shù)值滿足條件=Iz-ZtlI<e，z為平面上數(shù)值，Ztl為約束值，e為設(shè)定值。
[0017] 與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：
[0018] 1)與現(xiàn)有方法相比，本發(fā)明充分運(yùn)用人形機(jī)器人的冗余性，通過對(duì)機(jī)器人基于支 撐腳的坐標(biāo)系，引入浮體運(yùn)動(dòng)學(xué)，采用數(shù)值法中具有代表性蒙特卡洛法來進(jìn)行工作空間中 點(diǎn)集的求解，并且引入各種固有約束，討論滿足這些約束的復(fù)雜情形下的工作空間范圍，以 及單腳支撐情形下的極致情況；
[0019] 2)本發(fā)明針對(duì)求解出的三維點(diǎn)云網(wǎng)格，進(jìn)行平面截取，由于整體樣本點(diǎn)求解值組 成的三維點(diǎn)云不便于進(jìn)行觀察精確的形狀和結(jié)構(gòu)，因此可先對(duì)關(guān)鍵位置或者需求位置進(jìn)行 平面截取，再對(duì)獲得關(guān)鍵位置的截面進(jìn)行工作范圍的分析以及討論；
[0020] 3)本發(fā)明針對(duì)求解出的三維點(diǎn)云進(jìn)行網(wǎng)格矩陣賦值，通過歸一化、網(wǎng)格化解決關(guān) 節(jié)的冗余性，且可區(qū)別出相差較大的網(wǎng)格點(diǎn)，因?yàn)榭赡芏鄠€(gè)參數(shù)空間對(duì)應(yīng)著極其接近的兩 個(gè)位姿點(diǎn)，同樣地在不同的情形下的橫向?qū)Ρ龋罅课蛔它c(diǎn)之間的歐式距離實(shí)際很小，為了 便于進(jìn)行同類位姿點(diǎn)歸一和空間范圍的對(duì)比，需要對(duì)位姿點(diǎn)進(jìn)行歸一化的處理。

【專利附圖】

【附圖說明】
[0021] 圖1為機(jī)器人抓取物體時(shí)工作空間判斷流程圖；
[0022] 圖2為本發(fā)明方法流程圖；
[0023] 圖3為本發(fā)明中機(jī)器人浮體坐標(biāo)系；
[0024] 圖4為本發(fā)明中機(jī)器人關(guān)節(jié)鏈?zhǔn)疽鈭D；
[0025] 圖5為本發(fā)明中工作空間計(jì)算流程圖；
[0026] 圖6為本發(fā)明中三維點(diǎn)云在機(jī)器人空間中的位置坐標(biāo)圖。

【具體實(shí)施方式】
[0027] 下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。本實(shí)施例以本發(fā)明技術(shù)方案 為前提進(jìn)行實(shí)施，給出了詳細(xì)的實(shí)施方式和具體的操作過程，但本發(fā)明的保護(hù)范圍不限于 下述的實(shí)施例。
[0028] 每一個(gè)關(guān)節(jié)的值的組合都對(duì)應(yīng)著一個(gè)機(jī)器人的狀態(tài)空間。末端執(zhí)行器的空間位置 是關(guān)于關(guān)節(jié)的非線性函數(shù)，關(guān)節(jié)空間下的一組參數(shù)對(duì)應(yīng)著唯一的一個(gè)空間位置解。只有當(dāng) 關(guān)節(jié)空間的這組參數(shù)滿足引入的約束時(shí)，相對(duì)應(yīng)的末端執(zhí)行器的位姿點(diǎn)坐標(biāo)才有意義，只 有在滿足約束時(shí)該參數(shù)才可以通過大量采樣關(guān)節(jié)空間下的參數(shù)組合，每個(gè)組合都對(duì)應(yīng)著一 個(gè)空間位置點(diǎn)，大量符合約束條件下的空間位置點(diǎn)組成的三維點(diǎn)云圖可以清晰的在圖形中 描述出工作的范圍大小以及邊界曲面，這就是本發(fā)明的必要性。
[0029] 機(jī)器人的工作空間也就是末端軌跡，如手在各種關(guān)節(jié)空間配置下的三維空間位置 點(diǎn)集。人形機(jī)器人的運(yùn)動(dòng)需要滿足一些基本的固有約束，包括關(guān)節(jié)角約束、穩(wěn)定性約束。本 發(fā)明將求解、討論和對(duì)比兩種情況：分別是單純手臂關(guān)節(jié)空間的工作空間求解以及浮體運(yùn) 動(dòng)學(xué)/全身運(yùn)動(dòng)學(xué)下的可達(dá)空間求解。
[0030] 實(shí)施例一：
[0031] 如圖2所示，針對(duì)圖1中工作空間網(wǎng)格，對(duì)單純使用手臂關(guān)節(jié)空間的工作空間進(jìn)行 求解與描述，以及蒙特卡洛法來求解工作空間的詳細(xì)流程，即固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng) 空間的方法，該方法包括以下步驟：
[0032] 步驟SI:以支撐腳為坐標(biāo)原點(diǎn)，對(duì)機(jī)器人身體基坐標(biāo)系引入浮體運(yùn)動(dòng)學(xué)如Xe = f( 9F，9E)，建立一個(gè)基于全身關(guān)節(jié)的關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，機(jī)器人的末端執(zhí)行器不僅和末 端到質(zhì)心的關(guān)節(jié)有關(guān)還受到質(zhì)心到支撐腳的關(guān)節(jié)的影響，如圖3、4所示。
[0033] 步驟S2 :基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，引入關(guān)節(jié)角約束、穩(wěn)定性約束并利用蒙特卡 洛法采樣并求解機(jī)器人末端執(zhí)行器的工作空間，獲得工作空間構(gòu)成的三維點(diǎn)云網(wǎng)格，如圖5 所示，具體步驟為：
[0034] 步驟201:本實(shí)例的機(jī)器人手臂關(guān)節(jié)空間包含了 5個(gè)關(guān)節(jié)，在運(yùn)動(dòng)關(guān)節(jié)的關(guān)節(jié)角約 束、穩(wěn)定性約束范圍內(nèi)采用蒙特卡洛法進(jìn)行Nsample次線性隨機(jī)采樣獲得關(guān)節(jié)狀態(tài)值，每 次隨機(jī)采樣獲得的一組參數(shù)作為一個(gè)樣本點(diǎn)，其中，關(guān)節(jié)角約束表示為：emin< 0 < 0_， 0為關(guān)節(jié)取值，分別為0該關(guān)節(jié)的最大和最小值，穩(wěn)定性約束如采樣時(shí)約束，選 取的采樣次數(shù)為Nsample= 10000次。
[0035] 步驟202:基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，對(duì)樣本點(diǎn)進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算，獲得機(jī)器人質(zhì) 心，根據(jù)公式<0判斷出左右腳哪只為支撐腳，其中，為左腳末端在z軸上的位 置，為右腳末端在z軸上的位置，然后結(jié)合質(zhì)心與支撐區(qū)域的關(guān)系來判斷出該采樣是否 有效，若是，繼續(xù)計(jì)算末端執(zhí)行器位姿點(diǎn)，若否，繼續(xù)在關(guān)機(jī)空間中隨機(jī)采樣，最后以基坐標(biāo) 系為基礎(chǔ)繪制位姿點(diǎn)的三維點(diǎn)云，得到末端執(zhí)行器工作空間，如圖6所示，為機(jī)器人單純使 用手臂關(guān)節(jié)空間的工作空間。
[0036] 正運(yùn)動(dòng)學(xué)是機(jī)器人的基本的方法和原理，根據(jù)關(guān)節(jié)角的狀態(tài)，求解目標(biāo)連桿在特 定坐標(biāo)系下的位姿，因此它常用于機(jī)器人的重心計(jì)算、機(jī)器人的狀態(tài)描述。
[0037] 步驟203:三維點(diǎn)云經(jīng)過網(wǎng)格矩陣賦值后得到三維點(diǎn)云網(wǎng)格。網(wǎng)格矩陣賦值具體 為：對(duì)三維點(diǎn)云中每個(gè)位姿點(diǎn)用最鄰近的網(wǎng)格點(diǎn)表示，即將相近的位姿點(diǎn)歸一化到同一個(gè) 網(wǎng)格點(diǎn)中，每個(gè)網(wǎng)格點(diǎn)都居于各自網(wǎng)格空間的中心。
[0038] 如圖6所示，由大量樣本點(diǎn)求解的位姿點(diǎn)構(gòu)成的三維點(diǎn)云圖可以看出對(duì)于不同的 參數(shù)樣本下，由于關(guān)節(jié)的冗余性，實(shí)際上可能多個(gè)參數(shù)空間對(duì)應(yīng)著極其接近的兩個(gè)位姿點(diǎn)； 同樣的為了進(jìn)行在不同的情形下的橫向?qū)Ρ?，大量位姿點(diǎn)之間的歐式距離實(shí)際很小。為了 便于進(jìn)行同類位姿點(diǎn)歸一和空間范圍的對(duì)比，需要對(duì)位姿點(diǎn)進(jìn)行歸一的處理。本發(fā)明通過 建立三維網(wǎng)格來進(jìn)行歸一化，通過對(duì)三維空間進(jìn)行網(wǎng)格化處理，把三維空間分為密集的大 量網(wǎng)格點(diǎn)來表示，每個(gè)網(wǎng)格點(diǎn)都居于各自網(wǎng)格空間的中心。這樣對(duì)于每個(gè)位姿點(diǎn)，都能找到 最近鄰的網(wǎng)格點(diǎn)進(jìn)行表示，這樣可以把相近的位姿點(diǎn)都?xì)w一化到同一個(gè)網(wǎng)格點(diǎn)中，看待成 同一個(gè)位姿點(diǎn)，這樣可以區(qū)別出相差較大的網(wǎng)格點(diǎn)。
[0039] 步驟S3:在三維點(diǎn)云網(wǎng)格的基礎(chǔ)上，進(jìn)行平面截取，并通過極值分析，得到機(jī)器人 在各個(gè)方向上的極值狀態(tài)與位姿圖。平面截取具體為：根據(jù)關(guān)鍵位置（如機(jī)器人手臂水平 平面）或需求位置（如研究者關(guān)心的位置）對(duì)三維點(diǎn)云網(wǎng)格進(jìn)行平面截取，在同一平面上 數(shù)值滿足條件：Iz-ZtlI<e，Z為平面上數(shù)值，Ztl為約束值，e為設(shè)定值。
[0040] 從采樣的三維點(diǎn)云圖中可以直接看出工作空間的極值范圍以及邊界曲面，但是 由于整體樣本點(diǎn)求解組成的三維點(diǎn)云，不便于進(jìn)行觀察精確的形狀和結(jié)構(gòu)，所以可以通過 對(duì)關(guān)鍵位置或者需求位置進(jìn)行平面截取，獲得關(guān)鍵位置的截面進(jìn)行工作范圍的分析以及討 論。比如，需要知道高度與初始位置同一水平位置時(shí)的工作空間，即滿足約束條件z=z。。 然而由于采集的樣本點(diǎn)為隨機(jī)的，計(jì)算得出的z值不可能都剛好在平面上。增加一個(gè)松弛 條件，即認(rèn)為滿足條件Iz-ZcJ<e的都認(rèn)為在平面上，在此條件下，得到了滿足約束條件 的截平而。
[0041] 實(shí)施例二：
[0042] 對(duì)復(fù)雜全身運(yùn)動(dòng)下的機(jī)器人工作空間進(jìn)行了求解。采用數(shù)值法中具有代表性的蒙 特卡洛法來進(jìn)行工作空間中點(diǎn)集的求解，并且將同樣引入各種固有約束如全身運(yùn)動(dòng)下的機(jī) 器人各個(gè)關(guān)節(jié)的關(guān)節(jié)范圍約束等討論滿足這些約束下的復(fù)雜情形下的工作空間的范圍。實(shí) 施步驟同實(shí)施例一，求解對(duì)象為機(jī)器人各個(gè)關(guān)節(jié)在固有約束下的工作空間范圍。
【權(quán)利要求】
1. 一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，其特征在于，該方法包括以下步驟： 步驟Sl :以支撐腳為坐標(biāo)原點(diǎn)，對(duì)機(jī)器人身體基坐標(biāo)系引入浮體運(yùn)動(dòng)學(xué)，建立一個(gè)基 于全身關(guān)節(jié)的關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型； 步驟S2 :基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，引入關(guān)節(jié)角范圍約束、穩(wěn)定性約束，利用蒙特卡洛 法采樣并求解機(jī)器人末端執(zhí)行器的工作空間，獲得工作空間構(gòu)成的三維點(diǎn)云網(wǎng)格； 步驟S3 :在三維點(diǎn)云網(wǎng)格的基礎(chǔ)上，進(jìn)行平面截取，并通過極值分析，得到機(jī)器人在各 個(gè)方向上的極值狀態(tài)與位姿圖。
2. 根據(jù)權(quán)利要求1所述的一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，其特征在于， 所述的步驟S2具體為： 步驟201 :在運(yùn)動(dòng)關(guān)節(jié)的關(guān)節(jié)角約束、穩(wěn)定性約束范圍內(nèi)，采用蒙特卡洛法進(jìn)行線性隨 機(jī)采樣獲得關(guān)節(jié)狀態(tài)值，每次隨機(jī)采樣獲得的一組參數(shù)作為一個(gè)樣本點(diǎn)； 步驟202 :基于關(guān)節(jié)鏈和運(yùn)動(dòng)學(xué)模型，對(duì)樣本點(diǎn)進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算，獲得機(jī)器人質(zhì)心以 及末端執(zhí)行器位姿點(diǎn)，以基坐標(biāo)系為基礎(chǔ)繪制位姿點(diǎn)的三維點(diǎn)云，得到末端執(zhí)行器工作空 間； 步驟203 :三維點(diǎn)云經(jīng)過網(wǎng)格矩陣賦值后得到三維點(diǎn)云網(wǎng)格。
3. 根據(jù)權(quán)利要求2所述的一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，其特征在于， 所述的網(wǎng)格矩陣賦值具體為：對(duì)三維點(diǎn)云中每個(gè)位姿點(diǎn)用最鄰近的網(wǎng)格點(diǎn)表示，即將相近 的位姿點(diǎn)歸一化到同一個(gè)網(wǎng)格點(diǎn)中，每個(gè)網(wǎng)格點(diǎn)都居于各自網(wǎng)格空間的中心。
4. 根據(jù)權(quán)利要求1所述的一種固有約束下構(gòu)建機(jī)器人運(yùn)動(dòng)空間的方法，其特征在于， 所述的平面截取具體為：根據(jù)關(guān)鍵位置或需求位置對(duì)三維點(diǎn)云網(wǎng)格進(jìn)行平面截取，在同一 平面上數(shù)值滿足條件：I Z-ZtJ < e，z為平面上數(shù)值，Ztl為約束值，e為設(shè)定值。
【文檔編號(hào)】B25J9/16GK104325462SQ201410521891
【公開日】2015年2月4日 申請(qǐng)日期:2014年9月30日 優(yōu)先權(quán)日:2014年9月30日
【發(fā)明者】陳啟軍, 陳毅鴻, 劉成菊 申請(qǐng)人:同濟(jì)大學(xué)