專(zhuān)利名稱(chēng)：仿人機(jī)器人及其步行控制方法
技術(shù)領(lǐng)域：
本發(fā)明的實(shí)施例涉及一種仿人機(jī)器人及其步行控制方法，該仿人機(jī)器人對(duì)安裝到 每個(gè)關(guān)節(jié)的電機(jī)的扭矩進(jìn)行伺服控制，以實(shí)現(xiàn)穩(wěn)定的步行。
背景技術(shù)：
對(duì)具有與人類(lèi)的關(guān)節(jié)系統(tǒng)類(lèi)似的關(guān)節(jié)系統(tǒng)的雙足步行機(jī)器人進(jìn)行了研究，從而該 機(jī)器人可被容易地應(yīng)用于人類(lèi)的工作，生存空間得到積極的引導(dǎo)。這樣的雙足機(jī)器人的步行控制方法的示例包括基于位置的零力矩點(diǎn)(ZMP)步行 控制方法、基于扭矩的動(dòng)態(tài)步行控制方法以及有限狀態(tài)機(jī)(FSM)步行控制方法。在基于ZMP的步行控制方法中，預(yù)先設(shè)置步行方向、步寬、步行速率等，利用ZMP約 束條件來(lái)創(chuàng)建與預(yù)設(shè)項(xiàng)目對(duì)應(yīng)的身體和腳的步行模式，并通過(guò)步行模式的逆向運(yùn)動(dòng)學(xué)計(jì)算 來(lái)計(jì)算每條腿的關(guān)節(jié)位置軌跡。另外，基于ZMP的步行控制方法通過(guò)位置伺服控制來(lái)實(shí)現(xiàn)， 以使每條腿的關(guān)節(jié)能夠跟蹤計(jì)算出來(lái)的關(guān)節(jié)位置軌跡。在步行過(guò)程中，控制每條腿的關(guān)節(jié)， 以準(zhǔn)確地跟蹤從步行模式所獲得的關(guān)節(jié)位置軌跡。在基于ZMP的步行控制方法中，機(jī)器人在步行的同時(shí)不斷地彎曲其膝蓋，從而在 通過(guò)逆向運(yùn)動(dòng)學(xué)計(jì)算每個(gè)關(guān)節(jié)的角度時(shí)避免了運(yùn)動(dòng)奇異性。因此，機(jī)器人可能不會(huì)像人類(lèi) 那樣自然地行走。在基于ZMP的步行控制方法中，可準(zhǔn)確地控制每個(gè)關(guān)節(jié)的位置，從而控制ZMP，因 此，位置伺服控制增益高。所以，電機(jī)的電流高，因此能效低。另外，每個(gè)關(guān)節(jié)的剛度增加， 因此，在與障礙物發(fā)生碰撞時(shí)，每個(gè)關(guān)節(jié)可能會(huì)將極大的沖擊力施加到障礙物。在基于FSM的步行控制方法中，預(yù)先設(shè)置步行機(jī)器人的操作狀態(tài)(指示FSM的狀 態(tài))，并通過(guò)參照步行過(guò)程中的操作狀態(tài)來(lái)計(jì)算每個(gè)關(guān)節(jié)的扭矩，從而使機(jī)器人行走得當(dāng)。在基于FSM的步行控制方法中，機(jī)器人可通過(guò)改變步行過(guò)程中的操作狀態(tài)來(lái)采取 各種姿勢(shì)。然而，由于在受約束的操作狀態(tài)下采取每個(gè)姿勢(shì)，因此執(zhí)行用于保持機(jī)器人平衡 的單獨(dú)操作，而不管用于執(zhí)行任務(wù)的步行操作如何。平衡操作的典型示例為機(jī)器人跺腳的 步進(jìn)動(dòng)作。由于這樣的操作而造成時(shí)間延遲和能量浪費(fèi)。仿人機(jī)器人被視為與周?chē)沫h(huán)境相互影響，從而將基于FSM的步行施加到在保持 平衡方面比四足機(jī)器人困難的仿人機(jī)器人，例如，雙足機(jī)器人。即，根據(jù)周?chē)沫h(huán)境對(duì)機(jī)器 人的操作狀態(tài)進(jìn)行反饋，從而控制機(jī)器人的步行操作。

發(fā)明內(nèi)容
本發(fā)明的一方面在于提供一種仿人機(jī)器人及其步行控制方法，該仿人機(jī)器人在基 于有限狀態(tài)機(jī)(FSM)的步行控制過(guò)程中對(duì)安裝到每個(gè)關(guān)節(jié)的電機(jī)的扭矩進(jìn)行伺服控制，以實(shí)現(xiàn)穩(wěn)定的步行。本發(fā)明的其他方面將在下面的描述中進(jìn)行部分闡述，部分將通過(guò)描述而明白，或 可通過(guò)本發(fā)明的實(shí)踐而了解。根據(jù)本發(fā)明的一方面，一種仿人機(jī)器人包括機(jī)器人關(guān)節(jié)單元，包括在機(jī)器人步行 過(guò)程中可活動(dòng)的關(guān)節(jié)，電機(jī)安裝到機(jī)器人關(guān)節(jié)單元的每個(gè)關(guān)節(jié)；傳感器單元，測(cè)量機(jī)器人的 落地信息和姿勢(shì)信息；關(guān)節(jié)位置軌跡創(chuàng)建單元，利用機(jī)器人的落地信息和姿勢(shì)信息來(lái)創(chuàng)建 每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡；關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元，計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值，以對(duì) 每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡進(jìn)行補(bǔ)償；關(guān)節(jié)扭矩計(jì)算單元，利用關(guān)節(jié)位置軌跡補(bǔ)償值對(duì)每個(gè) 關(guān)節(jié)的關(guān)節(jié)位置軌跡進(jìn)行補(bǔ)償，并根據(jù)每個(gè)關(guān)節(jié)的補(bǔ)償后的關(guān)節(jié)位置軌跡來(lái)計(jì)算每個(gè)關(guān)節(jié) 的關(guān)節(jié)扭矩；關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元，計(jì)算關(guān)節(jié)扭矩補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn) 行補(bǔ)償；關(guān)節(jié)扭矩伺服控制單元，利用關(guān)節(jié)扭矩補(bǔ)償值對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn)行補(bǔ)償、計(jì) 算電機(jī)電流以跟蹤每個(gè)關(guān)節(jié)的補(bǔ)償后的關(guān)節(jié)扭矩并根據(jù)計(jì)算出來(lái)的電機(jī)電流對(duì)電機(jī)的扭 矩進(jìn)行伺服控制。所述傳感器單元可包括多軸力和力矩(f/t)傳感器，用于測(cè)量機(jī)器人的落地狀 態(tài)；姿勢(shì)傳感器，用于測(cè)量機(jī)器人的姿勢(shì)。所述關(guān)節(jié)位置軌跡創(chuàng)建單元可包括多個(gè)狀態(tài)機(jī)，在仿人機(jī)器人的左腳或右腳落 地時(shí)交替地進(jìn)行操作；關(guān)節(jié)位置軌跡計(jì)算器，根據(jù)所述多個(gè)狀態(tài)機(jī)之間的切換情況來(lái)確定 左腳或右腳的落地狀態(tài)，從而計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡。關(guān)節(jié)扭矩計(jì)算單元可包括第一位置運(yùn)算器、第二位置運(yùn)算器、第三位置運(yùn)算器、比 例增益控制器以及微分增益控制器，所述關(guān)節(jié)扭矩計(jì)算單元可計(jì)算由等式1所表示的每個(gè) 關(guān)節(jié)的關(guān)節(jié)扭矩Td，等式1Td = kp (θ,-0) + kd (θ,-θ) + Tff第一位置運(yùn)算器可將通過(guò)關(guān)節(jié)位置軌跡θ d減去從電機(jī)反饋的關(guān)節(jié)當(dāng)前位置θ 所獲得的減值輸出到比例增益控制器，比例增益控制器可將通過(guò)以比例增益、乘 減值所獲得的乘值kp( θ d-θ)輸出到第二位置運(yùn)算器，第三位置運(yùn)算器可將通過(guò) 由微分得到的關(guān)節(jié)位置軌跡么減去由微分得到的關(guān)節(jié)當(dāng)前位置^所獲得的減值么-々輸出 到微分增益控制器，微分增益控制器可將通過(guò)以微分增益kd乘減值么-々所獲得的乘值 、(么-々)輸出到第二位置運(yùn)算器，第二位置運(yùn)算器可將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的 前饋參數(shù)Tff加到乘值1^(9(1-0)和、(么-4)，從而輸出每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩Td。所述傳感器單元還可包括編碼器，所述編碼器安裝到電機(jī)，用于測(cè)量關(guān)節(jié)當(dāng)前位 置θ。所述關(guān)節(jié)扭矩伺服控制單元可包括電機(jī)控制器以及用于測(cè)量電機(jī)的扭矩的扭矩 傳感器，當(dāng)通過(guò)扭矩傳感器測(cè)得的關(guān)節(jié)扭矩值被反饋到電機(jī)控制器時(shí)，電機(jī)控制器可控制 將被驅(qū)動(dòng)的電機(jī)，使得該電機(jī)跟蹤每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩。電機(jī)控制器可包括第一扭矩運(yùn)算器、第二扭矩運(yùn)算器、第三扭矩運(yùn)算器、比例增益 控制器、積分器、積分增益控制器以及微分增益控制器，所述電機(jī)控制器可計(jì)算由等式2所 表示的電機(jī)電流i，
等式2i = kp (Td -T) + kd (id -T) + kt j(Td - T)dt + iff第一扭矩運(yùn)算器可將通過(guò)關(guān)節(jié)扭矩τ d減去從扭矩傳感器反饋的電機(jī)當(dāng)前扭矩τ 所獲得的減值h-τ輸出到比例增益控制器，比例增益控制器可將通過(guò)以比例增益kp乘減 值τ d_ τ所獲得的乘值kp( τ d_ τ )輸出到第二扭矩運(yùn)算器，積分器可對(duì)減值τ d_ τ進(jìn)行積 分并將其輸出到積分增益控制器，積分增益控制器可將通過(guò)以積分增益&乘積分后的減值 /(Td-T)所獲得的乘值& /(Td-T)輸出到第二扭矩運(yùn)算器，第三扭矩運(yùn)算器可將通過(guò) 由微分得到的關(guān)節(jié)扭矩t減去由微分得到的電機(jī)當(dāng)前扭矩f所獲得的減值之輸出到微分 增益控制器，微分增益控制器可將通過(guò)以微分增益kd乘減值6 - f所獲得的乘值、(、- f ) 輸出到第二扭矩運(yùn)算器，第二扭矩運(yùn)算器可將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)iff 加到乘值1^“(1-1)人(之-0和1^ f ( Td-τ)，從而輸出電機(jī)電流i。所述傳感器單元可包括用于測(cè)量機(jī)器人的落地狀態(tài)的多軸F/T傳感器以及用于 測(cè)量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器，當(dāng)機(jī)器人的腳落地時(shí)，關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元可 基于從多軸F/T傳感器接收的力和力矩信息以及從姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息 來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值h，關(guān)節(jié)扭矩計(jì)算單元可根據(jù)利用關(guān)節(jié)位置軌跡補(bǔ)償值h所補(bǔ) 償?shù)挠傻仁?所表示的關(guān)節(jié)位置軌跡θd來(lái)計(jì)算關(guān)節(jié)扭矩，等式3
θ = ΘΜ + Η(χ,/,ιη,ΖΜΡ,φ)其中，θ㈤為原始的關(guān)節(jié)位置軌跡，關(guān)節(jié)位置軌跡補(bǔ)償值h由從包括機(jī)器人的重心 χ、相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角^的各 種參數(shù)中選擇的一個(gè)參數(shù)所表示的函數(shù)計(jì)算得出。關(guān)節(jié)位置軌跡補(bǔ)償值可由等式4所表示地進(jìn)行計(jì)算，等式4h(x) = kpx(xd -χ) + kj^cd - χ)其中，xd為機(jī)器人的重心的命令值，χ為機(jī)器人的重心的實(shí)際值，kpx為比例增益， 、為機(jī)器人的重心的速度，;t為機(jī)器人的重心的速度的實(shí)際值，kdx為微分增益。所述傳感器單元可包括用于測(cè)量機(jī)器人的落地狀態(tài)的多軸F/T傳感器以及用于 測(cè)量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器，當(dāng)機(jī)器人的腳落地時(shí)，關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元可基于 從多軸F/T傳感器接收的力和力矩信息以及從姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì) 算關(guān)節(jié)扭矩補(bǔ)償值g，關(guān)節(jié)扭矩伺服控制單元可根據(jù)由等式5所表示的補(bǔ)償后的關(guān)節(jié)扭矩 Td來(lái)計(jì)算關(guān)節(jié)扭矩，等式5
^d = Tdo + g(x, /，rn, ZMP, φ)其中，τ㈤為原始的關(guān)節(jié)扭矩，關(guān)節(jié)扭矩補(bǔ)償值g由從包括機(jī)器人的重心χ、相對(duì)于 地面的力、f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角識(shí)的各種參數(shù)中 選擇的一個(gè)參數(shù)所表示的函數(shù)計(jì)算得出。關(guān)節(jié)扭矩補(bǔ)償值可由等式6所表示地進(jìn)行計(jì)算，
等式6gO) = k’px (xd -χ) + ^dx (xd - χ)其中，xd為機(jī)器人的重心的命令值，χ為機(jī)器人的重心的實(shí)際值，k' px為比例增 益，、為機(jī)器人的重心的速度，λ為機(jī)器人的重心的速度的實(shí)際值，k' dx為微分增益。根據(jù)本發(fā)明的另一方面，一種仿人機(jī)器人的步行控制方法包括計(jì)算每個(gè)關(guān)節(jié)的 關(guān)節(jié)位置軌跡，以執(zhí)行機(jī)器人的步行；利用通過(guò)安裝在機(jī)器人中的傳感器單元測(cè)得的機(jī)器 人的落地信息和姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡進(jìn)行 補(bǔ)償；根據(jù)利用關(guān)節(jié)位置軌跡補(bǔ)償值所補(bǔ)償?shù)年P(guān)節(jié)位置軌跡來(lái)計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩； 利用通過(guò)安裝在機(jī)器人中的傳感器單元測(cè)得的機(jī)器人的落地信息和姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié) 扭矩補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn)行補(bǔ)償；計(jì)算電機(jī)電流，以跟蹤利用關(guān)節(jié)扭矩補(bǔ)償 值所補(bǔ)償?shù)年P(guān)節(jié)扭矩，并根據(jù)計(jì)算出來(lái)的電機(jī)電流對(duì)安裝到每個(gè)關(guān)節(jié)的電機(jī)的扭矩進(jìn)行伺 服控制。計(jì)算關(guān)節(jié)位置軌跡的步驟可包括確定左腳或右腳的落地狀態(tài)，以計(jì)算每個(gè)關(guān)節(jié) 的關(guān)節(jié)位置軌跡。計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩的步驟可包括輸出通過(guò)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡θ d 減去關(guān)節(jié)當(dāng)前位置θ所獲得的減值；輸出通過(guò)以比例增益、乘減值所獲得 的乘值kp( Θ d- Θ )；輸出通過(guò)由微分得到的關(guān)節(jié)位置軌跡么減去由微分得到的關(guān)節(jié)當(dāng)前位 置A所獲得的減值-左；-輸出通過(guò)以微分增益kd乘減值色-々所獲得的乘值、(Qd -4);將 利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)τ ff加到乘值、(θ d- Θ )和、(么-々)，從而輸出 每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩Td。計(jì)算電機(jī)電流的步驟可包括輸出通過(guò)關(guān)節(jié)扭矩^減去電機(jī)當(dāng)前扭矩τ所獲 得的減值Td-T ；輸出通過(guò)以比例增益&乘減值h-τ所獲得的乘值kp( Td-τ)；對(duì)減 值h-τ進(jìn)行積分；輸出通過(guò)以積分增益1^乘積分后的減值/ (Td-τ)所獲得的乘值 ki / ( τ d- τ )；輸出通過(guò)由微分得到的關(guān)節(jié)扭矩t減去由微分得到的電機(jī)當(dāng)前扭矩f所獲得 的減值之；輸出通過(guò)以微分增益kd乘減值之- 所獲得的乘值、(4 );將利用逆向動(dòng) 力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)iff加到乘(丨-0和1^ f (^-τ)，從而 輸出電機(jī)電流i。所述步行控制方法還可包括基于從測(cè)量機(jī)器人的落地狀態(tài)的多軸F/T傳感器接 收的力和力矩信息以及從測(cè)量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì) 算關(guān)節(jié)位置軌跡補(bǔ)償值h，關(guān)節(jié)位置軌跡補(bǔ)償值h可由從包括機(jī)器人的重心X、相對(duì)于地面 的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角識(shí)的各種參數(shù)中選擇 的一個(gè)參數(shù)所表示的函數(shù)計(jì)算得出，可通過(guò)將計(jì)算出來(lái)的關(guān)節(jié)位置軌跡補(bǔ)償值h加到原始 的關(guān)節(jié)位置軌跡θ d(1來(lái)獲得補(bǔ)償后的關(guān)節(jié)位置軌跡ed。所述步行控制方法還可包括基于從測(cè)量機(jī)器人的落地狀態(tài)的多軸F/T傳感器接 收的力和力矩信息以及從測(cè)量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì) 算關(guān)節(jié)扭矩補(bǔ)償值g，關(guān)節(jié)扭矩補(bǔ)償值g可由從包括機(jī)器人的重心X、相對(duì)于地面的力f、相 對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角P的各種參數(shù)中選擇的一個(gè) 參數(shù)所表示的函數(shù)計(jì)算得出，可通過(guò)將計(jì)算出來(lái)的關(guān)節(jié)扭矩補(bǔ)償值g加到原始的關(guān)節(jié)扭矩 Ttlci來(lái)獲得補(bǔ)償后的關(guān)節(jié)扭矩Td。


通過(guò)下面結(jié)合附圖對(duì)實(shí)施例進(jìn)行的描述，本發(fā)明的這些和/或其他方面將會(huì)變得 清楚和更加容易理解，其中圖1是示出根據(jù)本發(fā)明的實(shí)施例的仿人機(jī)器人的外觀的視圖；圖2是示出圖1的仿人機(jī)器人的關(guān)節(jié)結(jié)構(gòu)的視圖；圖3是示出根據(jù)本發(fā)明的實(shí)施例的在基于有限狀態(tài)機(jī)(FSM)的步行過(guò)程中施加的 控制操作和機(jī)器人操作狀態(tài)的視圖；圖4是根據(jù)本發(fā)明的實(shí)施例的仿人機(jī)器人的控制框圖；圖5是圖4的關(guān)節(jié)位置軌跡創(chuàng)建單元的詳細(xì)構(gòu)造圖；圖6是示出根據(jù)本發(fā)明的實(shí)施例的根據(jù)利用關(guān)節(jié)位置軌跡計(jì)算出來(lái)的關(guān)節(jié)扭矩 來(lái)驅(qū)動(dòng)電機(jī)的操作的框圖；圖7是圖4的關(guān)節(jié)扭矩伺服控制單元的詳細(xì)構(gòu)造圖；圖8是示出根據(jù)本發(fā)明的實(shí)施例的對(duì)根據(jù)關(guān)節(jié)扭矩而驅(qū)動(dòng)的電機(jī)進(jìn)行伺服控制 的操作的框圖；圖9是示出根據(jù)本發(fā)明的實(shí)施例的仿人機(jī)器人的步行控制方法的流程圖。
具體實(shí)施例方式現(xiàn)在將詳細(xì)描述本發(fā)明的實(shí)施例，其示例在附圖中示出，其中，相同的標(biāo)號(hào)始終指 示相同的元件。如圖1中所示，頭104通過(guò)頸120被連接到仿人機(jī)器人100的上身102的上部。雙 臂106L和106R分別通過(guò)肩膀114L和114R被連接到上身102的上部的相對(duì)兩側(cè)。手33L 和33R被連接到臂106L和106R的各自的端部。雙腿IlOL和IlOR分別被連接到上身102 的下部的相對(duì)兩側(cè)。腳112L和112R被連接到腿IlOL和IlOR的各自的端部。頭104、臂 106L 禾口 106R、腿 IlOL 禾Π 110R、手 33L 禾口 33R、腳 112L 禾口 112R 通過(guò)各自 的關(guān)節(jié)具有預(yù)定的自由度。上身102的內(nèi)部通過(guò)蓋116來(lái)保護(hù)。在標(biāo)號(hào)中，“R”和“L”分 別指示機(jī)器人100的右側(cè)和左側(cè)。圖2是示出在圖1中示出的仿人機(jī)器人的關(guān)節(jié)結(jié)構(gòu)的視圖。如圖2中所示，仿人機(jī) 器人100的腿IlOL和IlOR中的每條腿具有股骨連桿21、腓骨連桿22以及腳112L和112R 中相應(yīng)的一只腳。股骨連桿21通過(guò)髖關(guān)節(jié)單元210被連接到上身102。股骨連桿21與腓 骨連桿22通過(guò)膝關(guān)節(jié)單元220彼此連接，腓骨連桿22與腳112L或112R通過(guò)踝關(guān)節(jié)單元 230彼此連接。髖關(guān)節(jié)單元210具有三個(gè)自由度。具體地講，髖關(guān)節(jié)單元210包括橫擺方向(Ζ_軸 旋轉(zhuǎn))的旋轉(zhuǎn)關(guān)節(jié)211、俯仰方向(Y-軸旋轉(zhuǎn))的旋轉(zhuǎn)關(guān)節(jié)212以及側(cè)傾方向(X-軸旋轉(zhuǎn)) 的旋轉(zhuǎn)關(guān)節(jié)213。膝關(guān)節(jié)單元220包括俯仰方向的旋轉(zhuǎn)關(guān)節(jié)221。即，膝關(guān)節(jié)單元220具有一個(gè)自由 度。踝關(guān)節(jié)單元230包括俯仰方向的旋轉(zhuǎn)關(guān)節(jié)231以及側(cè)傾方向的旋轉(zhuǎn)關(guān)節(jié)232。S卩，踝關(guān) 節(jié)單元230具有兩個(gè)自由度。腿IlOL和IlOR中的每條腿包括相對(duì)于三個(gè)關(guān)節(jié)單元210、220和230的六個(gè)旋轉(zhuǎn)關(guān)節(jié)。因此，兩腿IlOL和IlOR包括總計(jì)十二個(gè)旋轉(zhuǎn)關(guān)節(jié)。多軸力和力矩(F/T)傳感器M被安裝在腳112L或112R與腿IlOL和IlOR中的每 條腿的踝關(guān)節(jié)單元230之間。多軸F/T傳感器M測(cè)量從腳112L或112R傳遞的力的三個(gè) 方向的分量Fx、Fy和Fz，以及力矩的三個(gè)方向的分量Mx、My和Mz，以檢測(cè)腳112L或112R 的著地情況以及施加到腳112L或112R的載荷。對(duì)周?chē)h(huán)境進(jìn)行拍照的相機(jī)41以及接收用戶(hù)語(yǔ)音的麥克風(fēng)42被安裝在頭104 中。頭104通過(guò)頸關(guān)節(jié)單元280被連接到上身102。頸關(guān)節(jié)單元280包括橫擺方向的 旋轉(zhuǎn)關(guān)節(jié)觀1、俯仰方向的旋轉(zhuǎn)關(guān)節(jié)觀2以及側(cè)傾方向的旋轉(zhuǎn)關(guān)節(jié)觀3。S卩，頸關(guān)節(jié)單元觀0 具有三個(gè)自由度。頭旋轉(zhuǎn)電機(jī)(未示出)被連接到頸關(guān)節(jié)單元觀0的旋轉(zhuǎn)關(guān)節(jié)281、282和觀3。肩關(guān)節(jié)組件250L和250R被安裝到上身102的相對(duì)兩側(cè)，以將臂106L和106R連 接到上身102。臂106L和106R中的每條臂具有肱骨連桿31、前臂骨連桿32以及手33。肱骨連 桿31通過(guò)肩關(guān)節(jié)組件250L或250R被連接到上身102。肱骨連桿31與前臂骨連桿32通過(guò) 肘關(guān)節(jié)單元260彼此連接，前臂骨連桿32與手33通過(guò)腕關(guān)節(jié)270彼此連接。肘關(guān)節(jié)單元260包括俯仰方向的旋轉(zhuǎn)關(guān)節(jié)以及橫擺方向的旋轉(zhuǎn)關(guān)節(jié)沈2。艮口， 肘關(guān)節(jié)單元260具有兩個(gè)自由度。腕關(guān)節(jié)270包括俯仰方向的旋轉(zhuǎn)關(guān)節(jié)271以及側(cè)傾方向 的旋轉(zhuǎn)關(guān)節(jié)272。即，腕關(guān)節(jié)270具有兩個(gè)自由度。手33包括五個(gè)手指33a。由電機(jī)驅(qū)動(dòng)的多個(gè)關(guān)節(jié)(未示出)可被設(shè)置在手指33a 中。手指33a執(zhí)行各種操作，例如，抓握物體或與臂106的運(yùn)動(dòng)相結(jié)合地指示特定方向。姿勢(shì)傳感器14被安裝在上身102中。姿勢(shì)傳感器14感測(cè)上身102相對(duì)于垂直軸 的傾角以及上身102的角速度，從而創(chuàng)建姿勢(shì)信息。除上身102之外，在頭104中也可安裝 姿勢(shì)傳感器14。另外，橫擺方向的旋轉(zhuǎn)關(guān)節(jié)15被安裝在構(gòu)成上身102的胸10 與腰102b 之間，使得胸10 相對(duì)于腰102b旋轉(zhuǎn)。用于驅(qū)動(dòng)旋轉(zhuǎn)關(guān)節(jié)的致動(dòng)器(例如，電機(jī)405)(見(jiàn)圖6)被安裝在仿人機(jī)器人100 中。對(duì)電機(jī)405進(jìn)行適當(dāng)?shù)目刂疲詧?zhí)行仿人機(jī)器人100的各種操作。在基于有限狀態(tài)機(jī)(FSM)的步行控制方法中，機(jī)器人可具有六種預(yù)定的操作狀態(tài) Si、S2、S3、S4、S5和S6，如圖3中所示。各個(gè)操作狀態(tài)是指機(jī)器人在步行過(guò)程中呈現(xiàn)的姿勢(shì)。第一操作狀態(tài)Sl (飛行)表示一條腿擺動(dòng)的姿勢(shì)，第二操作狀態(tài)S2(加載)表示 一只腳放在地上的姿勢(shì)，第三操作狀態(tài)S3 (腳跟著地)表示一只腳的腳跟著地的姿勢(shì)，第四 操作狀態(tài)S4(腳跟和腳尖著地)表示一只腳的腳跟和腳尖同時(shí)著地的姿勢(shì)，第五操作狀態(tài) S5(腳尖著地)表示一只腳的腳尖著地的姿勢(shì)，第六操作狀態(tài)S6(卸載)表示一只腳離地的 姿勢(shì)。圖4是根據(jù)本發(fā)明的實(shí)施例的仿人機(jī)器人的控制框圖，圖5是圖4的關(guān)節(jié)位置軌 跡創(chuàng)建單元的詳細(xì)構(gòu)造圖。如圖4中所示，仿人機(jī)器人100包括關(guān)節(jié)位置軌跡創(chuàng)建單元300、關(guān)節(jié)扭矩計(jì)算單 元310、關(guān)節(jié)扭矩伺服控制單元320、機(jī)器人關(guān)節(jié)單元101、傳感器單元330、關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元340以及關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元350。機(jī)器人關(guān)節(jié)單元101包括總計(jì)十二個(gè)旋轉(zhuǎn)關(guān)節(jié)，所述十二個(gè)旋轉(zhuǎn)關(guān)節(jié)構(gòu)成用于機(jī) 器人步行的兩腿IlOL和IlOR的髖關(guān)節(jié)單元210、膝關(guān)節(jié)單元220以及踝關(guān)節(jié)單元230。機(jī) 器人關(guān)節(jié)單元101不僅包括屬于兩腿的關(guān)節(jié)，而且還可包括仿人機(jī)器人100(包括用于穩(wěn)定 步行的上身和臂)的所有關(guān)節(jié)。傳感器單元330包括姿勢(shì)傳感器14，安裝在上身102中；多軸F/T傳感器M， 安裝在腳112L或112R與腿IlOL和IlOR中的每條腿的踝關(guān)節(jié)單元230之間；扭矩傳感器 40 ，用于測(cè)量電機(jī)405的扭矩，電機(jī)405安裝到機(jī)器人關(guān)節(jié)單元101的每個(gè)關(guān)節(jié)。關(guān)節(jié)位置軌跡創(chuàng)建單元300利用傳感器單元330的測(cè)量值來(lái)創(chuàng)建每個(gè)關(guān)節(jié)的關(guān)節(jié) 位置軌跡θ d。在圖5中，關(guān)節(jié)位置軌跡創(chuàng)建單元300包括第一狀態(tài)機(jī)301、第二狀態(tài)機(jī)302以及 關(guān)節(jié)位置軌跡計(jì)算器303。當(dāng)右腳或左腳落地時(shí)，根據(jù)安裝在腳112L或112R與腿IlOL和IlOR中的每條腿 的踝關(guān)節(jié)單元230之間的多軸F/T傳感器M的落地信號(hào)，第一狀態(tài)機(jī)301和第二狀態(tài)機(jī) 302交替地操作。當(dāng)左腳擺動(dòng)而右腳處于落地狀態(tài)時(shí)，從第二狀態(tài)機(jī)302切換到第一狀態(tài)機(jī) 301。當(dāng)右腳擺動(dòng)而左腳處于落地狀態(tài)時(shí)，從第一狀態(tài)機(jī)301切換到第二狀態(tài)機(jī)302。關(guān)節(jié)位置軌跡計(jì)算器303根據(jù)第一狀態(tài)機(jī)301和第二狀態(tài)機(jī)302之間的切換情況 來(lái)確定左腳和右腳的落地狀態(tài)，從而計(jì)算機(jī)器人關(guān)節(jié)單元101的每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡 θ d。作為創(chuàng)建關(guān)節(jié)位置軌跡θ d的另一示例，可捕獲人類(lèi)的運(yùn)動(dòng)、可對(duì)每個(gè)關(guān)節(jié)的運(yùn)動(dòng) 進(jìn)行周期性采樣并基于采樣信息來(lái)匹配仿人機(jī)器人的關(guān)節(jié)位置，從而計(jì)算關(guān)節(jié)位置軌跡。關(guān)節(jié)扭矩計(jì)算單元310利用每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡θ d來(lái)計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié) 扭矩Td。關(guān)節(jié)扭矩計(jì)算單元310可包括第一位置運(yùn)算器401、第二位置運(yùn)算器403、第三位 置運(yùn)算器408、比例增益控制器402、微分增益控制器409。圖6是示出根據(jù)本發(fā)明的實(shí)施例的根據(jù)利用關(guān)節(jié)位置軌跡計(jì)算出來(lái)的關(guān)節(jié)扭矩 來(lái)驅(qū)動(dòng)電機(jī)的操作的框圖。在圖6中，第一位置運(yùn)算器401將通過(guò)關(guān)節(jié)位置軌跡Qd減去從電機(jī)405反饋的 關(guān)節(jié)當(dāng)前位置Θ所獲得的減值輸出到比例增益控制器402。關(guān)節(jié)當(dāng)前位置Θ可通 過(guò)安裝到電機(jī)405的編碼器40 進(jìn)行測(cè)量。比例增益控制器402將通過(guò)以比例增益kp乘減值θ d- θ所獲得的乘值kp( θ d- θ ) 輸出到第二位置運(yùn)算器403。第三位置運(yùn)算器408將通過(guò)由微分控制器407微分得到的關(guān)節(jié)位置軌跡么減去 由微分控制器406微分得到的關(guān)節(jié)當(dāng)前位置力所獲得的減值么輸出到微分增益控制器 409。微分增益控制器409將通過(guò)以微分增益kd乘減值之-々所獲得的乘值、(么輸出 到第二位置運(yùn)算器403。第二位置運(yùn)算器403將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)τ ff加到乘值 kp(ed-e)和夂(夂-幻，并輸出加值、。加值為通過(guò)關(guān)節(jié)扭矩計(jì)算單元310計(jì)算出來(lái) 的每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩Td。加值τ 示例可由等式1來(lái)表示。等式1
Tj = kp{θ -0) + kdφ -θ)+ Ttf等式1使用比例微分(PD)控制器和前饋(FF)控制器，然而，等式1不限于使用上 述控制器。等式1可使用任何控制器來(lái)控制每個(gè)關(guān)節(jié)的關(guān)節(jié)位置θ，以跟蹤關(guān)節(jié)位置軌跡
θ d。在等式1中，比例增益、微分增益和前饋參數(shù)可通過(guò)為實(shí)現(xiàn)穩(wěn)定步行而進(jìn)行的重復(fù)調(diào) 整來(lái)獲得。通過(guò)衰減控制器404使關(guān)節(jié)扭矩τ d乘以電機(jī)扭矩常數(shù)Ι/kt來(lái)獲得乘值(電機(jī)電 流i)，安裝到機(jī)器人關(guān)節(jié)單元101的每個(gè)關(guān)節(jié)的電機(jī)405根據(jù)電機(jī)電流i而被驅(qū)動(dòng)。此時(shí)， 通過(guò)關(guān)節(jié)扭矩計(jì)算單元310計(jì)算出來(lái)的每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩τ d被提供到關(guān)節(jié)扭矩伺服控 制單元320。在圖7中，關(guān)節(jié)扭矩伺服控制單元320包括電機(jī)控制器321。當(dāng)通過(guò)扭矩傳感器 405b測(cè)得的關(guān)節(jié)扭矩值被反饋到電機(jī)控制器321時(shí)，電機(jī)控制器321控制將被驅(qū)動(dòng)的電機(jī) 405，使得電機(jī)405跟蹤關(guān)節(jié)扭矩Td。此時(shí)，電機(jī)控制器321不限于通過(guò)扭矩傳感器40 測(cè)得的關(guān)節(jié)扭矩值被反饋到電機(jī)控制器321的構(gòu)造。例如，電機(jī)控制器321可利用從通過(guò) 電流傳感器測(cè)得的電流所估計(jì)的關(guān)節(jié)扭矩值來(lái)測(cè)量電機(jī)405的電流。電機(jī)控制器321可包 括第一扭矩運(yùn)算器501、第二扭矩運(yùn)算器505、第三扭矩運(yùn)算器508、比例增益控制器502、積 分器503、積分增益控制器504以及微分增益控制器509，以計(jì)算電機(jī)405的電機(jī)電流i。圖8是示出根據(jù)本發(fā)明的實(shí)施例的對(duì)根據(jù)關(guān)節(jié)扭矩而驅(qū)動(dòng)的電機(jī)進(jìn)行伺服控制 的操作的框圖。在圖8中，第一扭矩運(yùn)算器501將通過(guò)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩^減去從扭矩傳感器 40 反饋的電機(jī)當(dāng)前扭矩τ所獲得的減值Td-τ輸出到比例增益控制器502。比例增益 控制器502將通過(guò)以比例增益kp乘減值τ d- τ所獲得的乘值kp ( τ d_ τ )輸出到第二扭矩 運(yùn)算器505。積分器503對(duì)減值τ d- τ進(jìn)行積分并將其輸出到積分增益控制器504。積分增益 控制器504將通過(guò)以積分增益Iii乘積分后的減值f ( τ d- τ )所獲得的乘值Ici / ( τ d- τ ) 輸出到第二扭矩運(yùn)算器505。第三扭矩運(yùn)算器508將通過(guò)由微分控制器507微分得到的關(guān)節(jié)扭矩t減去由微分 控制器506微分得到的電機(jī)當(dāng)前扭矩f所獲得的減值之輸出到微分增益控制器509。微 分增益控制器509將通過(guò)以微分增益kd乘減值之―所獲得的乘值t (& - f )輸出到第二扭 矩運(yùn)算器505。第二扭矩運(yùn)算器505將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)iff加到乘值 MTd-q^C^-f^Pki/ (“-τ)，并輸出加值。該加值為施加到電機(jī)405的電機(jī)電 流i。電機(jī)電流i的示例可由等式2來(lái)表示。等式2i = kp (Td -T) + kd (Td -t) + k, — r)dt + iff等式2使用比例積分微分(PID)控制器和前饋(FF)控制器，然而，等式2不限于 使用上述控制器。等式2可使用任何控制器來(lái)控制每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩τ，以跟蹤關(guān)節(jié)扭 矩Td。在等式2中，比例增益、微分增益、積分增益和前饋參數(shù)可通過(guò)為實(shí)現(xiàn)穩(wěn)定步行而進(jìn) 行的重復(fù)調(diào)整來(lái)獲得。
當(dāng)機(jī)器人在改變姿勢(shì)之后行走時(shí)，取決于周?chē)沫h(huán)境條件(例如，步行表面的狀 況和障礙物的外觀)，可能會(huì)使步行控制執(zhí)行不當(dāng)，其結(jié)果是，機(jī)器人可能會(huì)失去平衡。因 此，與周?chē)沫h(huán)境條件相互影響的信息以及姿勢(shì)信息可被反饋到仿人機(jī)器人，仿人機(jī)器人 對(duì)關(guān)節(jié)位置軌跡或關(guān)節(jié)扭矩進(jìn)行補(bǔ)償，從而穩(wěn)定地控制步行。在圖4中，關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元340利用通過(guò)傳感器單元330測(cè)得的值 來(lái)創(chuàng)建關(guān)節(jié)位置軌跡補(bǔ)償值，以對(duì)關(guān)節(jié)位置軌跡進(jìn)行補(bǔ)償，關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元 340將創(chuàng)建的關(guān)節(jié)位置軌跡補(bǔ)償值提供給關(guān)節(jié)扭矩計(jì)算單元310。即，當(dāng)機(jī)器人的腳落地 時(shí)，關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元340可基于從多軸F/T傳感器M接收的力和力矩信息以 及從姿勢(shì)傳感器14接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值h。關(guān)節(jié)扭矩計(jì)算單元310可根據(jù)由等式3所表示的補(bǔ)償后的關(guān)節(jié)位置軌跡θ d來(lái)計(jì) 算關(guān)節(jié)扭矩。等式3
Od = eM+h{x,f,m’ZMP,(p)其中，Qtffi為原始的關(guān)節(jié)位置軌跡，h為關(guān)節(jié)位置軌跡補(bǔ)償值，h可從由各種參數(shù) (例如，機(jī)器人的重心X、相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以 及機(jī)器人的傾角P)表示的函數(shù)計(jì)算得出。等式4Kx) = kpx (xd -x)+ Udx (xd — χ)等式4為關(guān)節(jié)位置軌跡補(bǔ)償值h (χ)的示例。關(guān)節(jié)位置軌跡補(bǔ)償值h(x)通過(guò)以下方法獲得將機(jī)器人的重心的命令值^和 機(jī)器人的重心的實(shí)際值X之間的差值與比例增益kpx相乘；將機(jī)器人的重心的速度 、和機(jī)器人的重心的實(shí)際速度i之間的差值、與微分增益kdx相乘；將乘積kpxUd-x)和
-義-)相加。此外，可利用從由其他參數(shù)(例如，相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器 人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角P)組成的組中選擇的至少一個(gè)參數(shù)來(lái)計(jì)算關(guān)節(jié)位置 軌跡補(bǔ)償值h。關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元350利用通過(guò)傳感器單元330測(cè)得的值來(lái)創(chuàng)建關(guān)節(jié)扭 矩補(bǔ)償值，以對(duì)關(guān)節(jié)扭矩進(jìn)行補(bǔ)償，關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元350將創(chuàng)建的關(guān)節(jié)扭矩補(bǔ)償 值提供給關(guān)節(jié)扭矩伺服控制單元320。即，當(dāng)機(jī)器人的腳落地時(shí)，關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元 350可基于從多軸F/T傳感器M接收的力和力矩信息以及從姿勢(shì)傳感器14接收的機(jī)器人 的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)扭矩補(bǔ)償值g。關(guān)節(jié)扭矩伺服控制單元320可根據(jù)由等式5所表示的補(bǔ)償后的關(guān)節(jié)扭矩τ d來(lái)計(jì) 算關(guān)節(jié)扭矩。等式5
Td = Tda + g(x,/, m, ZMP, φ)其中，Ttffi為原始的關(guān)節(jié)扭矩，g為關(guān)節(jié)扭矩補(bǔ)償值，g可從由各種參數(shù)(例如，機(jī) 器人的重心X、相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角P)表示的函數(shù)計(jì)算得出。等式6g(x) = k'px (xd -χ) + ^dx {xd - χ)等式6為關(guān)節(jié)扭矩補(bǔ)償值g (χ)的示例。關(guān)節(jié)扭矩補(bǔ)償值g(x)通過(guò)以下方法獲得將機(jī)器人的重心的命令值&和機(jī)器人 的重心的實(shí)際值X之間的差值與比例增益k' px相乘；將機(jī)器人的重心的速度、和機(jī) 器人的重心的實(shí)際速度;t之間的差值、-λ與微分增益k' dx相乘；將乘積k' px(Xd-X)禾口相加。此外，可利用從由其他參數(shù)(例如，相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器 人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角￠0組成的組中選擇的至少一個(gè)參數(shù)來(lái)計(jì)算關(guān)節(jié)扭矩 補(bǔ)償值go在本實(shí)施例中，對(duì)關(guān)節(jié)位置軌跡和關(guān)節(jié)扭矩都進(jìn)行了補(bǔ)償。可選地，可對(duì)關(guān)節(jié)位置 軌跡或關(guān)節(jié)扭矩進(jìn)行補(bǔ)償，以實(shí)現(xiàn)仿人機(jī)器人的穩(wěn)定步行。以下，將對(duì)根據(jù)本發(fā)明的實(shí)施例的仿人機(jī)器人的步行控制方法進(jìn)行描述?？偟膩?lái)說(shuō)，仿人機(jī)器人100基于FSM而步行。為了執(zhí)行步行操作，關(guān)節(jié)位置軌跡計(jì) 算器303利用傳感器單元330的測(cè)量值來(lái)確定左腳和右腳的落地狀態(tài)，從而計(jì)算機(jī)器人關(guān) 節(jié)單元101的每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡θ d(601)。作為創(chuàng)建關(guān)節(jié)位置軌跡θ d的另一示例， 可捕獲人類(lèi)的運(yùn)動(dòng)、可對(duì)每個(gè)關(guān)節(jié)的運(yùn)動(dòng)進(jìn)行周期性采樣并基于采樣信息來(lái)匹配仿人機(jī)器 人的關(guān)節(jié)位置，從而計(jì)算關(guān)節(jié)位置軌跡。將計(jì)算出來(lái)的關(guān)節(jié)位置軌跡θ d提供給關(guān)節(jié)扭矩計(jì)算單元310。關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元340基于從多軸F/T傳感器M接收的力和力矩信 息以及從姿勢(shì)傳感器14接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值h (60 。關(guān) 節(jié)位置軌跡補(bǔ)償值h可從由各種參數(shù)(例如，機(jī)器人的重心χ、相對(duì)于地面的力f、相對(duì)于地 面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角P)表示的函數(shù)計(jì)算得出。將計(jì)算出來(lái)的關(guān)節(jié)位置軌跡補(bǔ)償值h提供給關(guān)節(jié)扭矩計(jì)算單元310。關(guān)節(jié)扭矩計(jì) 算單元310將關(guān)節(jié)位置軌跡補(bǔ)償值h加到原始的關(guān)節(jié)位置軌跡ed(1，以執(zhí)行補(bǔ)償(605)，并 根據(jù)由等式3所表示的補(bǔ)償后的關(guān)節(jié)位置軌跡Θ d來(lái)計(jì)算關(guān)節(jié)扭矩(607)。關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元350基于從多軸F/T傳感器M接收的力和力矩信息以 及從姿勢(shì)傳感器14接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)扭矩補(bǔ)償值g (609)。關(guān)節(jié)扭矩補(bǔ) 償值g可從由各種參數(shù)(例如，機(jī)器人的重心χ、相對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī) 器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角￠0表示的函數(shù)計(jì)算得出。將計(jì)算出來(lái)的關(guān)節(jié)扭矩補(bǔ)償值g提供給關(guān)節(jié)扭矩伺服控制單元320。關(guān)節(jié)扭矩伺 服控制單元320將關(guān)節(jié)扭矩補(bǔ)償值g加到原始的關(guān)節(jié)扭矩τ d0,以執(zhí)行補(bǔ)償(611)，并根據(jù) 由等式5所表示的補(bǔ)償后的關(guān)節(jié)扭矩τ d來(lái)驅(qū)動(dòng)安裝到機(jī)器人關(guān)節(jié)單元101的每個(gè)關(guān)節(jié)的 電機(jī) 405 (613)。在驅(qū)動(dòng)電機(jī)的過(guò)程中，將通過(guò)扭矩傳感器40 測(cè)得的關(guān)節(jié)扭矩值反饋到關(guān)節(jié)扭 矩伺服控制單元320，扭矩伺服控制單元320通過(guò)伺服控制來(lái)控制電機(jī)電流i而驅(qū)動(dòng)電機(jī) 405，使得電機(jī)405跟蹤關(guān)節(jié)扭矩τ d(615)。通過(guò)以上描述清楚的是，仿人機(jī)器人及其步行控制方法通過(guò)基于FSM的步行控制對(duì)電機(jī)扭矩進(jìn)行伺服控制。因此，在步行過(guò)程中，無(wú)需準(zhǔn)確地控制每個(gè)關(guān)節(jié)的位置，從而實(shí) 現(xiàn)了具有低伺服增益的步行，因此降低了能量消耗。另外，在步行過(guò)程中每個(gè)關(guān)節(jié)具有低的 剛度，因此，在與障礙物發(fā)生碰撞時(shí)，施加到每個(gè)關(guān)節(jié)的沖擊力減小。另外，執(zhí)行在求解逆向 運(yùn)動(dòng)學(xué)的傳統(tǒng)的步行方法中難以執(zhí)行的膝蓋伸展式步行，從而實(shí)現(xiàn)人性化的步行并節(jié)省與 膝蓋彎曲相關(guān)的能量。另外，無(wú)需求解復(fù)雜的動(dòng)力學(xué)方程，因此適用于包括具有六個(gè)自由度 的關(guān)節(jié)的機(jī)器人。 盡管已經(jīng)示出和描述了本發(fā)明的一些實(shí)施例，但是本領(lǐng)域的技術(shù)人員應(yīng)該理解， 在不脫離由權(quán)利要求及其等同物限定其范圍的本發(fā)明的精神和原理的情況下，可以對(duì)這些 實(shí)施例進(jìn)行改變。
權(quán)利要求
1.一種仿人機(jī)器人，包括機(jī)器人關(guān)節(jié)單元，包括在機(jī)器人步行過(guò)程中可活動(dòng)的關(guān)節(jié)，電機(jī)安裝到機(jī)器人關(guān)節(jié)單 元的每個(gè)關(guān)節(jié)；傳感器單元，測(cè)量機(jī)器人的落地信息和姿勢(shì)信息；關(guān)節(jié)位置軌跡創(chuàng)建單元，利用機(jī)器人的落地信息和姿勢(shì)信息來(lái)創(chuàng)建每個(gè)關(guān)節(jié)的關(guān)節(jié)位 置軌跡；關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元，計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置 軌跡進(jìn)行補(bǔ)償；關(guān)節(jié)扭矩計(jì)算單元，利用關(guān)節(jié)位置軌跡補(bǔ)償值對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡進(jìn)行補(bǔ)償， 并根據(jù)每個(gè)關(guān)節(jié)的補(bǔ)償后的關(guān)節(jié)位置軌跡來(lái)計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩；關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元，計(jì)算關(guān)節(jié)扭矩補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn)行補(bǔ)償；關(guān)節(jié)扭矩伺服控制單元，利用關(guān)節(jié)扭矩補(bǔ)償值對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn)行補(bǔ)償、計(jì)算 電機(jī)電流以跟蹤每個(gè)關(guān)節(jié)的補(bǔ)償后的關(guān)節(jié)扭矩并根據(jù)計(jì)算出來(lái)的電機(jī)電流對(duì)電機(jī)的扭矩 進(jìn)行伺服控制。
2.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中，所述傳感器單元包括多軸力和力矩傳感 器，用于測(cè)量機(jī)器人的落地狀態(tài)；姿勢(shì)傳感器，用于測(cè)量機(jī)器人的姿勢(shì)。
3.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中，所述關(guān)節(jié)位置軌跡創(chuàng)建單元包括多個(gè)狀 態(tài)機(jī)，在仿人機(jī)器人的左腳或右腳落地時(shí)交替地進(jìn)行操作；關(guān)節(jié)位置軌跡計(jì)算器，根據(jù)所述 多個(gè)狀態(tài)機(jī)之間的切換情況來(lái)確定左腳或右腳的落地狀態(tài)，從而計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)位置 軌跡。
4.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中關(guān)節(jié)扭矩計(jì)算單元包括第一位置運(yùn)算器、第二位置運(yùn)算器、第三位置運(yùn)算器、比例增益 控制器以及微分增益控制器，所述關(guān)節(jié)扭矩計(jì)算單元計(jì)算由等式1所表示的每個(gè)關(guān)節(jié)的關(guān) 節(jié)扭矩τ d， 等式1Td=kp(ed-0) + kd(ed-0) + Tff第一位置運(yùn)算器將通過(guò)關(guān)節(jié)位置軌跡θd減去從電機(jī)反饋的關(guān)節(jié)當(dāng)前位置θ所獲得 的減值輸出到比例增益控制器，比例增益控制器將通過(guò)以比例增益kp乘減值θ d-θ所獲得的乘值kp( θ d-θ)輸出到第二位置運(yùn)算器，第三位置運(yùn)算器將通過(guò)由微分得到的關(guān)節(jié)位置軌跡么減去由微分得到的關(guān)節(jié)當(dāng)前位 置々所獲得的減值么-々輸出到微分增益控制器，微分增益控制器將通過(guò)以微分增益kd乘減值么-々所獲得的乘值K么-々)輸出到第二位置運(yùn)算器，第二位置運(yùn)算器將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)τ ff加到乘值、(θ d- θ ) 和t (么從而輸出每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩Td。
5.根據(jù)權(quán)利要求4所述的仿人機(jī)器人，其中，所述傳感器單元還包括編碼器，所述編碼器安裝到電機(jī)，用于測(cè)量關(guān)節(jié)當(dāng)前位置Θ。
6.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中，所述關(guān)節(jié)扭矩伺服控制單元包括電機(jī)控 制器以及用于測(cè)量電機(jī)的扭矩的扭矩傳感器，當(dāng)通過(guò)扭矩傳感器測(cè)得的關(guān)節(jié)扭矩值被反饋 到電機(jī)控制器時(shí)，電機(jī)控制器控制將被驅(qū)動(dòng)的電機(jī)，使得該電機(jī)跟蹤每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩。
7.根據(jù)權(quán)利要求6所述的仿人機(jī)器人，其中電機(jī)控制器包括第一扭矩運(yùn)算器、第二扭矩運(yùn)算器、第三扭矩運(yùn)算器、比例增益控制 器、積分器、積分增益控制器以及微分增益控制器，所述電機(jī)控制器計(jì)算由等式2所表示的 電機(jī)電流i， 等式2i = kP - τ) + K (rd -T) + k, j(rd — r)dt + if1第一扭矩運(yùn)算器將通過(guò)關(guān)節(jié)扭矩Td減去從扭矩傳感器反饋的電機(jī)當(dāng)前扭矩τ所獲 得的減值h-τ輸出到比例增益控制器，比例增益控制器將通過(guò)以比例增益kp乘減值τ d- τ所獲得的乘值kp( τ d_ τ )輸出到 第二扭矩運(yùn)算器，積分器對(duì)減值h-τ進(jìn)行積分并將其輸出到積分增益控制器， 積分增益控制器將通過(guò)以積分增益&乘積分后的減值f (τ -τ)所獲得的乘值 ki / ( τ d- τ )輸出到第二扭矩運(yùn)算器，第三扭矩運(yùn)算器將通過(guò)由微分得到的關(guān)節(jié)扭矩4減去由微分得到的電機(jī)當(dāng)前扭矩f所 獲得的減值之-★輸出到微分增益控制器，微分增益控制器將通過(guò)以微分增益kd乘減值之所獲得的乘值、(之-f)輸出到第 二扭矩運(yùn)算器，第二扭矩運(yùn)算器將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)iff加到乘值、(τ d_ τ )、 、(之-0和、f ( Td-τ)，從而輸出電機(jī)電流i。
8.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中所述傳感器單元包括用于測(cè)量機(jī)器人的落地狀態(tài)的多軸力和力矩傳感器以及用于測(cè) 量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器，當(dāng)機(jī)器人的腳落地時(shí)，關(guān)節(jié)位置軌跡補(bǔ)償值計(jì)算單元基于從多軸力和力矩傳感器接收 的力和力矩信息以及從姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值 h，關(guān)節(jié)扭矩計(jì)算單元根據(jù)利用關(guān)節(jié)位置軌跡補(bǔ)償值h所補(bǔ)償?shù)挠傻仁?所表示的關(guān)節(jié)位 置軌跡θ d來(lái)計(jì)算關(guān)節(jié)扭矩， 等式3θ,=θ +Η{χ,/,νη,ΖΜΡ,φ)其中，θ d(1為原始的關(guān)節(jié)位置軌跡，關(guān)節(jié)位置軌跡補(bǔ)償值h由從包括機(jī)器人的重心X、相 對(duì)于地面的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角^的各種參 數(shù)中選擇的一個(gè)參數(shù)所表示的函數(shù)計(jì)算得出。
9.根據(jù)權(quán)利要求8所述的仿人機(jī)器人，其中，關(guān)節(jié)位置軌跡補(bǔ)償值由等式4所表示地進(jìn) 行計(jì)算，等式4Kx) = kpx(xd -χ) + - χ)其中，&為機(jī)器人的重心的命令值，χ為機(jī)器人的重心的實(shí)際值，kpx為比例增益，、為 機(jī)器人的重心的速度，;t為機(jī)器人的重心的速度的實(shí)際值，kdx為微分增益。
10.根據(jù)權(quán)利要求1所述的仿人機(jī)器人，其中所述傳感器單元包括用于測(cè)量機(jī)器人的落地狀態(tài)的多軸力和力矩傳感器以及用于測(cè) 量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器，當(dāng)機(jī)器人的腳落地時(shí)，關(guān)節(jié)扭矩補(bǔ)償值計(jì)算單元基于從多軸力和力矩傳感器接收的力 和力矩信息以及從姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)扭矩補(bǔ)償值g，關(guān)節(jié)扭矩伺服控制單元根據(jù)由等式5所表示的補(bǔ)償后的關(guān)節(jié)扭矩τ d來(lái)計(jì)算關(guān)節(jié)扭矩，等式5h = Tdo + g(x,f,m,ZMP^)其中，τ d(l為原始的關(guān)節(jié)扭矩，關(guān)節(jié)扭矩補(bǔ)償值g由從包括機(jī)器人的重心X、相對(duì)于地面 的力f、相對(duì)于地面的力矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角識(shí)的各種參數(shù)中選擇 的一個(gè)參數(shù)所表示的函數(shù)計(jì)算得出。
11.根據(jù)權(quán)利要求10所述的仿人機(jī)器人，其中，關(guān)節(jié)扭矩補(bǔ)償值由等式6所表示地進(jìn)行 計(jì)算，等式6g(x) = k’px (xd -x) + k'iix (xd - χ)其中，xd為機(jī)器人的重心的命令值，χ為機(jī)器人的重心的實(shí)際值，k' px為比例增益，、 為機(jī)器人的重心的速度，i為機(jī)器人的重心的速度的實(shí)際值為微分增益。
12.—種仿人機(jī)器人的步行控制方法，包括計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡，以執(zhí)行機(jī)器人的步行；利用通過(guò)安裝在機(jī)器人中的傳感器單元測(cè)得的機(jī)器人的落地信息和姿勢(shì)信息來(lái)計(jì)算 關(guān)節(jié)位置軌跡補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡進(jìn)行補(bǔ)償；根據(jù)利用關(guān)節(jié)位置軌跡補(bǔ)償值所補(bǔ)償?shù)年P(guān)節(jié)位置軌跡來(lái)計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩；利用通過(guò)安裝在機(jī)器人中的傳感器單元測(cè)得的機(jī)器人的落地信息和姿勢(shì)信息來(lái)計(jì)算 關(guān)節(jié)扭矩補(bǔ)償值，以對(duì)每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩進(jìn)行補(bǔ)償；計(jì)算電機(jī)電流，以跟蹤利用關(guān)節(jié)扭矩補(bǔ)償值所補(bǔ)償?shù)年P(guān)節(jié)扭矩，并根據(jù)計(jì)算出來(lái)的電 機(jī)電流對(duì)安裝到每個(gè)關(guān)節(jié)的電機(jī)的扭矩進(jìn)行伺服控制。
13.根據(jù)權(quán)利要求12所述的步行控制方法，其中，計(jì)算關(guān)節(jié)位置軌跡的步驟包括確定 左腳或右腳的落地狀態(tài)，以計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡。
14.根據(jù)權(quán)利要求12所述的步行控制方法，其中，計(jì)算每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩的步驟包 括輸出通過(guò)每個(gè)關(guān)節(jié)的關(guān)節(jié)位置軌跡θ d減去關(guān)節(jié)當(dāng)前位置Θ所獲得的減值；輸 出通過(guò)以比例增益kp乘減值所獲得的乘值1^(9(1-0)；輸出通過(guò)由微分得到的關(guān)節(jié) 位置軌跡之減去由微分得到的關(guān)節(jié)當(dāng)前位置々所獲得的減值么-々；輸出通過(guò)以微分增益kd 乘減值么-々所獲得的乘值t(么-4);將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)τΗ加 到乘值kp(ed-e)和、(么-4)，從而輸出每個(gè)關(guān)節(jié)的關(guān)節(jié)扭矩Td。
15.根據(jù)權(quán)利要求12所述的步行控制方法，其中，計(jì)算電機(jī)電流的步驟包括輸出通過(guò)關(guān)節(jié)扭矩Td減去電機(jī)當(dāng)前扭矩τ所獲得的減值；輸出通過(guò)以比例增益、乘減值 τ d" τ所獲得的乘值kp( τ d_ τ )；對(duì)減值τ d_ τ進(jìn)行積分；輸出通過(guò)以積分增益Ici乘積分 后的減值/ ( Td-τ)所獲得的乘值Iii / (Td-T)；輸出通過(guò)由微分得到的關(guān)節(jié)扭矩t減去 由微分得到的電機(jī)當(dāng)前扭矩f所獲得的減值t ；輸出通過(guò)以微分增益kd乘減值t -f所獲 得的乘值、(之-〖);將利用逆向動(dòng)力學(xué)預(yù)先計(jì)算出來(lái)的前饋參數(shù)iff加到乘值kp( τ d_ τ )、 、(之-0禾口、f ( Td-τ)，從而輸出電機(jī)電流i。
16.根據(jù)權(quán)利要求12所述的步行控制方法，還包括基于從測(cè)量機(jī)器人的落地狀態(tài)的多軸力和力矩傳感器接收的力和力矩信息以及從測(cè) 量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值h，其 中，關(guān)節(jié)位置軌跡補(bǔ)償值h由從包括機(jī)器人的重心χ、相對(duì)于地面的力f、相對(duì)于地面的力 矩m、機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角爐的各種參數(shù)中選擇的一個(gè)參數(shù)所表示的函 數(shù)計(jì)算得出，通過(guò)將計(jì)算出來(lái)的關(guān)節(jié)位置軌跡補(bǔ)償值h加到原始的關(guān)節(jié)位置軌跡θ㈤來(lái)獲得補(bǔ)償后 的關(guān)節(jié)位置軌跡θ d。
17.根據(jù)權(quán)利要求12所述的步行控制方法，還包括基于從測(cè)量機(jī)器人的落地狀態(tài)的多軸力和力矩傳感器接收的力和力矩信息以及從測(cè) 量機(jī)器人的姿勢(shì)的姿勢(shì)傳感器接收的機(jī)器人的姿勢(shì)信息來(lái)計(jì)算關(guān)節(jié)扭矩補(bǔ)償值g，其中，關(guān)節(jié)扭矩補(bǔ)償值g由從包括機(jī)器人的重心χ、相對(duì)于地面的力f、相對(duì)于地面的力矩m、 機(jī)器人的零力矩點(diǎn)ZMP以及機(jī)器人的傾角爐的各種參數(shù)中選擇的一個(gè)參數(shù)所表示的函數(shù)計(jì) 算得出，通過(guò)將計(jì)算出來(lái)的關(guān)節(jié)扭矩補(bǔ)償值g加到原始的關(guān)節(jié)扭矩τ㈤來(lái)獲得補(bǔ)償后的關(guān)節(jié)扭矩Td。
全文摘要
在此公開(kāi)一種仿人機(jī)器人及其步行控制方法，所述仿人機(jī)器人基于對(duì)關(guān)節(jié)扭矩的伺服控制來(lái)實(shí)現(xiàn)穩(wěn)定的步行。所述仿人機(jī)器人利用傳感器的測(cè)量值來(lái)計(jì)算關(guān)節(jié)位置軌跡補(bǔ)償值和關(guān)節(jié)扭矩補(bǔ)償值、利用計(jì)算出來(lái)的補(bǔ)償值對(duì)關(guān)節(jié)位置軌跡和關(guān)節(jié)扭矩進(jìn)行補(bǔ)償并根據(jù)補(bǔ)償后的關(guān)節(jié)扭矩來(lái)驅(qū)動(dòng)安裝到每個(gè)關(guān)節(jié)的電機(jī)。
文檔編號(hào)G05B19/418GK102139714SQ20111002173
公開(kāi)日2011年8月3日 申請(qǐng)日期2011年1月14日 優(yōu)先權(quán)日2010年1月18日
發(fā)明者盧昌賢, 樸在浩, 樸重炅, 權(quán)雄, 李柱錫, 李珉炯, 郭鎬成, 金柱亨 申請(qǐng)人:三星電子株式會(huì)社