本發(fā)明涉及一種類人型機器人平衡的控制方法，特別是利用類人型機器人身體重心及下肢關(guān)節(jié)點位置計算并預(yù)測機器人運動時髖關(guān)節(jié)和踝關(guān)節(jié)應(yīng)彎曲的角度以維持平衡的一種控制方法。

背景技術(shù)：

在當(dāng)今高速發(fā)展的機器人領(lǐng)域，類人型機器人是一大代表。類人型機器人與人體外形相近，可以做出與人類相似的動作。由于具有雙足，這類機器人可以適應(yīng)更多的環(huán)境，完成更多的任務(wù)，例如面對包含上臺階相關(guān)動作的任務(wù)時，類人形機器人可以輕易完成，而靠輪子前進的機器人就很難勝任。

而在操作類人形機器人的過程中，不可避免地會遇到機器人的平衡問題?，F(xiàn)在主流的平衡控制方法是根據(jù)負(fù)反饋的思想，在機器人身體移動并使重心移動時，通過機器人全身各部件的聯(lián)動來反方向移動身體重心，使重心到地面的投影始終在支撐腳的腳面到地面投影的面內(nèi)，以此維持平衡。

但是這種方法很難讓機器人做出指定的動作，比如在指定機器人全身大部分關(guān)節(jié)的角度后，這種方法就不太適用了。而且由于需動用機器人全身部件，這一方法計算復(fù)雜度較大，又由于是通過負(fù)反饋實現(xiàn)的控制，此方法需防范震蕩現(xiàn)象。

技術(shù)實現(xiàn)要素：

為了解決類人型機器人在雙腳站立、單腳站立、行走時的平衡控制問題，本發(fā)明提供一種類人型機器人平衡的控制方法，技術(shù)方案如下：

一種類人型機器人平衡的控制方法，包括以下步驟：根據(jù)傳感器獲取的所述機器人全身關(guān)節(jié)彎曲的角度和已知的所述機器人部件尺寸，計算所述機器人的重心及下肢關(guān)節(jié)位置；根據(jù)所述機器人的狀態(tài)計算踝關(guān)節(jié)和/或髖關(guān)節(jié)維持平衡的理想角度；用所述踝關(guān)節(jié)和/或髖關(guān)節(jié)維持平衡的理想角度替換步驟1中傳感器獲取的踝關(guān)節(jié)和/或髖關(guān)節(jié)彎曲的角度。

所述根據(jù)所述機器人的狀態(tài)計算踝關(guān)節(jié)和/或髖關(guān)節(jié)維持平衡的理想角度包括，所述機器人雙腳站立時，計算踝關(guān)節(jié)在前后方向、左右方向應(yīng)彎曲的角度，或所述機器人單腳站立時，計算踝關(guān)節(jié)在前后方向、左右方向應(yīng)彎曲的角度和髖關(guān)節(jié)在左右方向上應(yīng)彎曲的角度，或所述機器人在行走中抬起的腳剛落地時，計算雙腳的踝關(guān)節(jié)在左右、前后方向上應(yīng)彎曲的角度。

所述機器人在行走中抬起的腳剛落地時踝關(guān)節(jié)應(yīng)彎曲的角度，是在所述機器人抬起的腳的底面到地面的距離為0.5～1.5個機器人踝關(guān)節(jié)到腳的底面的距離時進行預(yù)測的。

本發(fā)明的有益效果在于：在指定機器人大部分身體關(guān)節(jié)角度后仍能較好地控制平衡。通過本發(fā)明，機器人能夠動態(tài)地、實時地維持平衡，計算復(fù)雜度較小。

附圖說明

圖1是基于關(guān)節(jié)角度對類人形機器人平衡控制方法的示例的示意圖；

圖2是類人型機器人下肢各關(guān)節(jié)點及重心位置的示意圖；

圖3是類人型機器人雙腳站立時，下肢關(guān)節(jié)及身體重心相對位置經(jīng)抽象后的示意圖；

圖4是類人型機器人雙腳站立且右腿在前時，下肢的左視圖的示意圖；

圖5是類人型機器人雙腳站立且右腿在前時，右腿部分左視圖經(jīng)抽象后的示意圖；

圖6是類人型機器人雙腳站立且右腿在后時，下肢的左視圖的示意圖；

圖7是類人型機器人雙腳站立且右腿在后時，右腿部分左視圖經(jīng)抽象后的示意圖；

圖8是類人型機器人在雙腳站立時，下肢的主視圖的示意圖；

圖9是類人型機器人在雙腳站立時，右腿部分的主視圖經(jīng)抽象后的示意圖；

圖10是類人型機器人右腳單腳站立時，下肢關(guān)節(jié)及身體重心相對位置的示意圖；

圖11是類人型機器人左腳單腳站立時，下肢的左視圖的示意圖；

圖12是類人型機器人左腳單腳站立時，左腿部分左視圖經(jīng)抽象后的示意圖；

圖13是類人型機器人左腳單腳站立時，下肢的示意圖；

圖14是類人型機器人左腳單腳站立時，在左右方向上髖關(guān)節(jié)、踝關(guān)節(jié)經(jīng)抽象后的示意圖；

圖15是類人型機器人行走時，落腳前對落腳后關(guān)節(jié)點的預(yù)測示意圖；

圖16是類人型機器人行走時，模式切換的示意圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合附圖及實施例，對本發(fā)明進行進一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

圖1為基于關(guān)節(jié)角度對類人形機器人平衡控制方法的示例的示意圖，本發(fā)明主要是“點坐標(biāo)投影變換與位置預(yù)測”與“三角形中求解關(guān)鍵關(guān)節(jié)角度”部分的內(nèi)容。本方法是先通過傳感器獲取機器人的關(guān)節(jié)角度，結(jié)合已知的機器人部件尺寸，根據(jù)正向運動學(xué)原理，計算出所需關(guān)節(jié)點坐標(biāo)及重心坐標(biāo)。再根據(jù)雙腳站立時，重心在地面的投影要在機器人兩踝關(guān)節(jié)點在地面投影的連線上這一原則，利用各關(guān)節(jié)點及重心的坐標(biāo)，計算踝關(guān)節(jié)的角度；根據(jù)在單腳站立時，重心到地面的投影與踝關(guān)節(jié)點到地面的投影接近重合這一原則，利用各關(guān)節(jié)點及重心的坐標(biāo)，計算踝關(guān)節(jié)的角度和髖關(guān)節(jié)在左右方向的角度；根據(jù)行走時，抬起的腳要落地時，身體的各關(guān)節(jié)點可以近似看作是繞支撐腳的踝關(guān)節(jié)點作旋轉(zhuǎn)這一思路，預(yù)測腳落地的瞬間，身體各關(guān)節(jié)點的坐標(biāo)，從而利用雙腳平衡時的計算方法去計算腳落地時踝關(guān)節(jié)應(yīng)彎曲的角度。然后改變這些關(guān)節(jié)角的角度，使機器人的重心落在維持平衡應(yīng)在的位置，機器人就表現(xiàn)出了一個能維持平衡的姿勢。本方法由于是根據(jù)機器人的質(zhì)量分布來計算機器人關(guān)鍵的關(guān)節(jié)角度，故可在指定了機器人大部分身體關(guān)節(jié)角度后仍能較好地控制平衡，且計算復(fù)雜度較小。

首先說明一種數(shù)學(xué)方法，用于在空間中由已知坐標(biāo)的三點構(gòu)成的三角形中，求解任意內(nèi)角的方法。此舉只是為了說明由空間中已知坐標(biāo)的三點構(gòu)成的三角形，可以求其內(nèi)角，但方法不限于這一種。

已知三點坐標(biāo)分別為：(x1，y1，z1)、(x2，y2，z2)、(x3，y3，z3)，由兩點間距離公式：

其中l(wèi)為兩點間距離，a1、a2、b1、b2、c1、c2分別為空間中兩點的x、y、z軸坐標(biāo)，可求得任意兩點間距離，分別記為l1、l2、l3。再由：

即可求得三角形內(nèi)角的角度。

如圖3，本發(fā)明涉及的機器人在下肢中，右髖關(guān)節(jié)點2、右踝關(guān)節(jié)點4、左髖關(guān)節(jié)點6、左踝關(guān)節(jié)點8應(yīng)至少有兩個自由度，可實現(xiàn)前后方向和左右方向的運動；右膝關(guān)節(jié)點3、左膝關(guān)節(jié)點7應(yīng)至少有一個自由度，可實現(xiàn)前后方向的運動。

圖2為類人型機器人在雙腳站立時，下肢各關(guān)節(jié)以及重心相對位置的示意圖，經(jīng)抽象后如圖3。圖3中節(jié)點5為機器人重心節(jié)點1到地面的投影。為保證機器人維持平衡不摔倒，重心在地面的投影必須在兩腳及其連線上。為簡化計算、保證穩(wěn)定性，本發(fā)明將其簡化為：重心在地面的投影要在機器人兩踝關(guān)節(jié)點在地面投影的連線上。

根據(jù)正向運動學(xué)，即通過已知的機器人各關(guān)節(jié)角度和各部件的尺寸可計算出各關(guān)節(jié)的坐標(biāo)。若各部件的重量及重心也為已知，利用：

可以計算出機器人整體重心的坐標(biāo)位置。其中，x軸的正方向可規(guī)定為機器人的正前方向，y軸的正方向可規(guī)定為機器人的正左方向。至此，可以得到機器人各關(guān)節(jié)的角度、各關(guān)節(jié)的坐標(biāo)、整體重心的坐標(biāo)。

圖4是類人型機器人雙腳站立且右腿在前時，下肢的左視圖的示意圖。經(jīng)抽象后，右腿部分左視圖的示意圖如圖5。圖5中節(jié)點10為重心投影節(jié)點5在過右腳踝節(jié)點4的前后方向的直線上的投影。右腳在前，故投影節(jié)點10在右腳踝節(jié)點4的正后方。所以投影節(jié)點10的x軸坐標(biāo)與重心節(jié)點1的x軸坐標(biāo)相同，投影節(jié)點10的y軸坐標(biāo)與右踝關(guān)節(jié)點4的y軸坐標(biāo)相同，投影節(jié)點10的z軸坐標(biāo)為0。

右膝蓋關(guān)節(jié)點3到右髖關(guān)節(jié)點2的距離、右髖關(guān)節(jié)點2到右踝關(guān)節(jié)點4的距離為已知，圖5中12的角度可由無線傳感器獲取，故在以右髖關(guān)節(jié)點2、右膝蓋關(guān)節(jié)點3、右踝關(guān)節(jié)點4為三個端點的三角形中，可求得圖5中14的角度。在以右髖關(guān)節(jié)點2、投影節(jié)點10、右踝關(guān)節(jié)點4為端點的三角形中，三端點坐標(biāo)已知，可求得圖5中13的角度。計算：

π-(∠13+∠14)(4)

可得右腳踝在前后方向上應(yīng)彎曲的角度15。

圖6是類人型機器人雙腳站立且右腿在后時，下肢的左視圖的示意圖。經(jīng)抽象后，右腿部分左視圖的示意圖如圖7。圖7中關(guān)節(jié)點10為重心投影節(jié)點5在過右腳踝節(jié)點4的前后方向的直線上的投影。右腳在后，故投影節(jié)點10在右腳踝節(jié)點4的正前方。故投影節(jié)點10的x軸坐標(biāo)與重心節(jié)點1的x軸坐標(biāo)相同，投影節(jié)點10的y軸坐標(biāo)與右腳踝節(jié)點4的y軸坐標(biāo)相同，投影節(jié)點10的z軸坐標(biāo)為0。

與圖5情況下的方法相似。右膝蓋關(guān)節(jié)點3到右髖關(guān)節(jié)點2的距離、右髖關(guān)節(jié)點2到右踝關(guān)節(jié)點4的距離為已知，圖7中12的角度可由無線傳感器獲取，故在以右髖關(guān)節(jié)點2、右膝蓋關(guān)節(jié)點3、右踝關(guān)節(jié)點4為三個端點的三角形中，可求得圖7中14的角度。在以右髖關(guān)節(jié)點2、投影節(jié)點10、右踝關(guān)節(jié)點4為端點的三角形中，三端點坐標(biāo)已知，可求得圖7中13的角度。將13與14做差，即可求得右腳踝在前后方向上應(yīng)彎曲的角度15。

當(dāng)右腳踝節(jié)點4與投影點10重合時，則在前后方向上達到單腳站立的條件，可參照下面圖12的方法計算右腳踝在前后方向上應(yīng)彎曲的角度。

圖8是類人型機器人在雙腳站立時，下肢的主視圖的示意圖。經(jīng)抽象后，可得右腿部分的主視圖的示意圖9。圖9中關(guān)節(jié)點11為重心投影節(jié)點5在過右腳踝節(jié)點4的左右方向的直線上的投影。投影節(jié)點11的y軸坐標(biāo)與重心投影節(jié)點5的y軸坐標(biāo)相同，投影節(jié)點11的x軸坐標(biāo)與右腳踝節(jié)點4的x軸坐標(biāo)相同，投影節(jié)點11的z軸坐標(biāo)為0。在以右髖關(guān)節(jié)點2、右腳踝節(jié)點4、投影節(jié)點11為端點的三角形中，三端點坐標(biāo)已知，可求得圖9中16的角度，再由：

π/2-∠16(5)

即得到右腳踝在左右方向上應(yīng)彎曲的角度。此處嚴(yán)格的來說，應(yīng)將關(guān)節(jié)點2替換為點2到過點4與點11且與地面垂直的面的投影點，來計算角16的角度，但為了簡單，做了近似處理。

在右踝關(guān)節(jié)點4與投影節(jié)點11重合時，則在左右方向上達到單腳站立的條件，可參照下面圖14的方法。

在雙腳站立時，左腳踝在前后和左右方向上彎曲角度的計算方法與右腳相似，此處不再贅述。

當(dāng)機器人單腳站立時，如圖10，為保證機器人的平衡，機器人整體重心的到地面的投影點應(yīng)在承重的那只腳到地面投影的面內(nèi)。為簡化計算、增加精度，本發(fā)明將其簡化為：重心到地面的投影與踝關(guān)節(jié)點到地面的投影接近重合。

圖11是類人型機器人左腳單腳站立時，下肢的左視圖的示意圖。經(jīng)抽象后，左腿部分左視圖的示意圖如圖12。圖12中，由左髖關(guān)節(jié)6、左膝關(guān)節(jié)7、左踝關(guān)節(jié)8構(gòu)成的三角形中，三個端點坐標(biāo)已知，故可求出18的角度。在由重心節(jié)點1、左髖節(jié)點6、左踝關(guān)節(jié)點8構(gòu)成的三角形中，三個端點已知，可求得19的角度。由

π/2-(∠18-∠19)(6)

可計算得，機器人在單腳站立時，腳踝在前后方向上應(yīng)彎曲的角度20。

圖13是類人型機器人左腳單腳站立時，下肢的示意圖。經(jīng)抽象后，在左右方向上髖關(guān)節(jié)、踝關(guān)節(jié)的示意圖如圖14。

在計算左右方向上的角度時，分別從髖關(guān)節(jié)6、踝關(guān)節(jié)8做垂線，因此圖14中兩個21角呈內(nèi)錯角。參照圖14，可得圖中兩個21角相等，而可由：

π/2+∠21(7)

輕易得到機器人的髖關(guān)節(jié)和踝關(guān)節(jié)在左右方向上應(yīng)彎曲的角度22。而在以重心節(jié)點1、髖關(guān)節(jié)6、左踝關(guān)節(jié)8為端點的三角形中，三端點坐標(biāo)已知，可求得角21的角度。

在類人型機器人以右腳單腳站立時，髖關(guān)節(jié)、踝關(guān)節(jié)彎曲角度的計算方法與以左腳單腳站立相似，不再贅述。

至此，已經(jīng)介紹完機器人在雙腳、單腳情況下，計算關(guān)鍵關(guān)節(jié)角度的方法。下面，介紹有關(guān)機器人行走時，計算關(guān)節(jié)角度以維持平衡的方法。

機器人在行走時，與人行走類似，首先機器人將處于雙腳站立的狀態(tài)，此時利用雙腳站立時的平衡維持方法。然后，機器人將抬起一只腳，此時將處于單腳站立狀態(tài)，利用單腳站立時維持平衡的方法可維持這一階段的平衡。接著，機器人的身體將會逐漸前傾，同時機器人將抬起的腳逐漸放向地面，但是由于處在單腳模式下，機器人仍舊能處于平衡狀態(tài)。如圖15，當(dāng)機器人抬起的腳到地面的距離，即圖中距離23，為0.5～1.5個機器人踝關(guān)節(jié)到腳底面的距離，即圖中距離25的0.5～1.5倍時，本方法將對腳落到地面時各關(guān)節(jié)點位置進行預(yù)測。預(yù)測是通過設(shè)定機器人支撐腳的腳踝為轉(zhuǎn)軸，如15中的踝關(guān)節(jié)8，計算機器人的下肢各關(guān)節(jié)點繞轉(zhuǎn)軸旋轉(zhuǎn)后，機器人的腳剛好能夠落到地面時各關(guān)節(jié)點的坐標(biāo)。旋轉(zhuǎn)過程中各關(guān)節(jié)點的相對位置將近似不變。利用預(yù)測出的關(guān)節(jié)點坐標(biāo)，帶入維持雙腳平衡時的角度計算方法，即可預(yù)測出機器人抬起的腳落到地面時，兩踝關(guān)節(jié)應(yīng)彎曲的角度。預(yù)測完成后，機器人調(diào)整踝關(guān)節(jié)，表現(xiàn)出預(yù)測的彎曲角度。這時，機器人將會繞支撐腳的腳踝旋轉(zhuǎn)，重心向前，抬起的腳將會落地，并維持平衡。本方法通過預(yù)測落腳時各關(guān)節(jié)的位置，實現(xiàn)狀態(tài)的切換，從而使機器人在向前行走時保持平衡。

本方法涉及的機器人在腳踝部分會有前后和左右兩個方向的自由度，故機器人在落腳時，繞著旋轉(zhuǎn)的軸是與y軸平行的。以圖15的情況為例，當(dāng)機器人右腳即將落下時，旋轉(zhuǎn)軸過關(guān)節(jié)點8。記關(guān)節(jié)點8的坐標(biāo)為(x4，y4，z4)，記關(guān)節(jié)點4的坐標(biāo)為(x5，y5，z5)。則旋轉(zhuǎn)軸可表示為關(guān)節(jié)點4到旋轉(zhuǎn)軸的距離可通過公式：

計算得關(guān)節(jié)點4到旋轉(zhuǎn)軸的距離。

點26為預(yù)測到的關(guān)節(jié)點4的落點，設(shè)點26的坐標(biāo)為(x6，y6，z6)，故點26到旋轉(zhuǎn)軸距離也為l4。圖中點4和點26間的距離可近似為機器人腳底到地面的距離，即距離23。故，在由點4和點26的連線以及這兩點分別到旋轉(zhuǎn)軸的垂線，在這三條線構(gòu)成的三角形中，三邊長分別為l4、l4和距離23，可由公式(2)計算得旋轉(zhuǎn)角度，記為ω。

已知旋轉(zhuǎn)軸、旋轉(zhuǎn)角度、旋轉(zhuǎn)的對象即點4，即可求得旋轉(zhuǎn)后的點26。具體求解方法為數(shù)學(xué)問題，此處列舉一種。可利用旋轉(zhuǎn)矩陣求解，各關(guān)節(jié)點坐標(biāo)關(guān)系如下：

求解其他關(guān)節(jié)點旋轉(zhuǎn)后的坐標(biāo)，例如圖15中點3、點2、點7旋轉(zhuǎn)后的坐標(biāo)，也可用這種方法。求解完成后，即可預(yù)測腳落下后各關(guān)節(jié)點的位置，利用雙腳平衡時的計算方法，即可計算出機器人右腳落地時身體各關(guān)節(jié)應(yīng)彎曲的角度，以此維持機器人右腳剛落地時的平衡。當(dāng)機器人是左腳即將落下時，計算方法與右腳落下的方法相似，此處不再贅述。當(dāng)機器人的腳落下后，即可按照圖16所示的循環(huán)結(jié)構(gòu)繼續(xù)向前走動。