四足機(jī)器人雙足支撐相位力位混合控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明主要涉及到機(jī)器人運(yùn)動(dòng)控制技術(shù)領(lǐng)域，特指一種適用于四足機(jī)器人的雙足 支撐相位力位混合控制方法。
【背景技術(shù)】
[0002] 移動(dòng)機(jī)器人能夠到達(dá)人類無法到達(dá)或因環(huán)境危險(xiǎn)而不宜到達(dá)的地方，四足仿生機(jī) 器人是仿照四足哺乳動(dòng)物的運(yùn)動(dòng)模式的一類足式機(jī)器人，由于其可以在巖石、陡坡等復(fù)雜 地形有非常強(qiáng)的適應(yīng)能力，可以在非結(jié)構(gòu)環(huán)境下進(jìn)行物資運(yùn)輸、巡邏等任務(wù)，并能代替人類 進(jìn)行危險(xiǎn)操作，因而具有重大的研究?jī)r(jià)值。
[0003] 四足機(jī)器人一般由四條仿生腿和一個(gè)本體組成，每條腿包含一個(gè)側(cè)向關(guān)節(jié)和至少 兩個(gè)前向關(guān)節(jié)。通過模仿學(xué)習(xí)自然界中四足機(jī)器人的運(yùn)動(dòng)方式，目前四足機(jī)器人的運(yùn)動(dòng)方 式主要有三種步態(tài)：TROT步態(tài)（對(duì)角小跑步態(tài)）、B0UND步態(tài)（奔跑步態(tài)）和WALK步態(tài)（爬 行步態(tài)）。
[0004] 四足機(jī)器人的控制是典型的浮動(dòng)基座控制問題，控制目標(biāo)一般為其本體位姿，而 其本體位姿主要由支撐腿控制。由于四足機(jī)器人自由度繁多，要實(shí)現(xiàn)對(duì)機(jī)器人的整體控制 非常復(fù)雜，因此需要對(duì)模型進(jìn)行適當(dāng)簡(jiǎn)化。
[0005] 在對(duì)四足機(jī)器人進(jìn)行建模時(shí)，多連桿平面模型和SLIP模型是目前比較常用且有 效的簡(jiǎn)化模型，這兩個(gè)模型均能體現(xiàn)trot步態(tài)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性，基于模型的控制方 法大多根據(jù)其運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程來設(shè)計(jì)控制器。
[0006] 基于運(yùn)動(dòng)學(xué)的控制方法采用伺服控制率，根據(jù)傳感器信息和控制目標(biāo)計(jì)算各關(guān)節(jié) 位置信息，并通過位置伺服律實(shí)現(xiàn)對(duì)期望軌跡的跟蹤，由于位置伺服控制具有較大的剛度， 因而能使機(jī)器人具備較強(qiáng)的負(fù)載能力，但在非結(jié)構(gòu)化環(huán)境下容易出現(xiàn)擺動(dòng)腿提前或滯后落 地的情況，這種情況下產(chǎn)生的非預(yù)期沖擊對(duì)本體位姿的影響很大，因此需要設(shè)計(jì)算法以適 應(yīng)較大的位姿沖擊。
[0007] 基于動(dòng)力學(xué)的控制方法一般采用計(jì)算力矩方法等多環(huán)控制方法，通過機(jī)器人本體 位姿信息計(jì)算足端期望力，并通過計(jì)算關(guān)節(jié)驅(qū)動(dòng)力矩來控制足端期望力，進(jìn)而達(dá)到控制機(jī) 器人本體位姿的目的。這種控制方法具有較好的柔順性，對(duì)非結(jié)構(gòu)化環(huán)境具有較強(qiáng)的適應(yīng) 能力。但是，其缺點(diǎn)在于在兩條支撐腿之間可能產(chǎn)生內(nèi)力，在支撐腿切換到擺動(dòng)腿模式的時(shí) 亥IJ，內(nèi)力瞬間釋放，對(duì)擺動(dòng)腿和本體的姿態(tài)產(chǎn)生非預(yù)期沖擊，并且驅(qū)動(dòng)器的剛度受整體質(zhì)量 的影響相對(duì)于位置控制較大。因此，力控制方法需要設(shè)計(jì)合理的力分配方法，在控制機(jī)器人 位姿的同時(shí)能夠增強(qiáng)機(jī)器人的運(yùn)動(dòng)能力。

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

[0008] 本發(fā)明要解決的技術(shù)問題就在于：針對(duì)現(xiàn)有技術(shù)存在的技術(shù)問題，本發(fā)明提供一 種控制效果好、能夠提高機(jī)器人適應(yīng)能力的四足機(jī)器人雙足支撐相位力位混合控制方法。
[0009] 為解決上述技術(shù)問題，本發(fā)明采用以下技術(shù)方案：
[0010] -種四足機(jī)器人雙足支撐相位力位混合控制方法，其步驟為：
[0011] S1 :將機(jī)器人的整體運(yùn)動(dòng)投影到徑向平面和法向平面上；所述徑向平面為對(duì)腳支 撐并垂直于水平面的平面；所述法向平面為通過本體質(zhì)心并且垂直于徑向平面的平面；
[0012] S2 :建立控制模型；機(jī)器人沿徑向平面上的運(yùn)動(dòng)簡(jiǎn)化為平面七連桿模型，在法向 平面上的運(yùn)動(dòng)簡(jiǎn)化為線性倒立擺模型；然后將平面七連桿模型簡(jiǎn)化為平面虛擬伸縮腿模 型；平面虛擬伸縮腿模型的控制目標(biāo)為質(zhì)心高度、本體俯仰角以及水平位移；
[0013] S3 :根據(jù)控制模型進(jìn)行混合控制；建立平面虛擬伸縮腿模型的運(yùn)動(dòng)學(xué)方程并通過 牛頓-歐拉方法建立其動(dòng)力學(xué)方程，對(duì)于質(zhì)心高度和本體俯仰角通過位置伺服控制方法， 對(duì)虛擬伸縮腿平面模型的水平位移采用力控制方式。
[0014] 作為本發(fā)明的進(jìn)一步改進(jìn)：在步驟S3中，通過腿長(zhǎng)來控制本體質(zhì)心高度和俯仰 角，通過控制水平足端力來控制本體水平位移，以實(shí)現(xiàn)對(duì)本體位姿的近似解耦控制。
[0015] 作為本發(fā)明的進(jìn)一步改進(jìn)：采用經(jīng)典PID控制器作為力外環(huán)控制器，使支撐腿足 端接觸力準(zhǔn)確跟蹤期望力。
[0016] 作為本發(fā)明的進(jìn)一步改進(jìn)：采用位置伺服控制器對(duì)膝關(guān)節(jié)和踝關(guān)節(jié)進(jìn)行控制以實(shí) 現(xiàn)對(duì)關(guān)節(jié)角度的跟蹤。
[0017] 與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)在于：
[0018] 1、本發(fā)明在四足機(jī)器人Trot步態(tài)運(yùn)動(dòng)時(shí)，能夠?qū)崿F(xiàn)對(duì)本體位姿的準(zhǔn)確跟蹤控制， 并對(duì)不連續(xù)的期望軌跡具有較強(qiáng)的適應(yīng)性。
[0019] 2、本發(fā)明基于足端正壓力的力分配策略能夠有效避免機(jī)器人在行走過程中支撐 腿足端滑動(dòng)的現(xiàn)象，并使機(jī)器人具備較強(qiáng)的加速性能。
[0020] 3、本發(fā)明能夠提高四足機(jī)器人對(duì)非結(jié)構(gòu)化環(huán)境的適應(yīng)能力，實(shí)現(xiàn)機(jī)器人在不平整 地面上的行走。
[0021] 4、本發(fā)明能夠通過增大位置控制器剛度提高機(jī)器人的負(fù)載能力。
[0022] 5、本發(fā)明結(jié)構(gòu)清晰、層次分明，具有較好的理論價(jià)值和工程意義。
【附圖說明】
[0023] 圖1是本發(fā)明在具體應(yīng)用實(shí)例中四足機(jī)器人平臺(tái)的結(jié)構(gòu)示意圖。
[0024] 圖2是本發(fā)明在具體應(yīng)用實(shí)例中徑向平面投影的示意圖。
[0025] 圖3是本發(fā)明在具體應(yīng)用實(shí)例中平面虛擬伸縮腿結(jié)構(gòu)的解耦控制框圖。
[0026] 圖4是本發(fā)明方法的流程示意圖。
【具體實(shí)施方式】
[0027] 以下將結(jié)合說明書附圖和具體實(shí)施例對(duì)本發(fā)明做進(jìn)一步詳細(xì)說明。
[0028] 本發(fā)明的方法主要適用于四足機(jī)器人。如圖1所示，為四足機(jī)器人的系統(tǒng)結(jié)構(gòu)，由 本體和四條腿組成，每條腿包含一個(gè)髖部側(cè)向轉(zhuǎn)動(dòng)關(guān)節(jié)和三個(gè)前向轉(zhuǎn)動(dòng)關(guān)節(jié)（髖前關(guān)節(jié)、 膝關(guān)節(jié)和踝關(guān)節(jié)）。每個(gè)關(guān)節(jié)均由液壓驅(qū)動(dòng)器驅(qū)動(dòng)，并裝有用于檢測(cè)驅(qū)動(dòng)器長(zhǎng)度和驅(qū)動(dòng)力 的位移傳感器和力傳感器。每條腿足端均裝設(shè)三維力傳感器，用于檢測(cè)機(jī)器人足端與環(huán)境 的接觸力信息，并通過頂U(kuò)檢測(cè)機(jī)器人在慣性系下的位姿信息。上述四足機(jī)器人具有如下 機(jī)械結(jié)構(gòu)特性：（1)機(jī)器人重心接近本體幾何中心；(2)機(jī)器人本體的質(zhì)量遠(yuǎn)大于腿部的質(zhì) 量。
[0029] 在本發(fā)明中，以TROT步態(tài)雙足支撐相為例，說明四足機(jī)器人雙足支撐時(shí)的力位混 合控制方法。本發(fā)明的四足機(jī)器人雙足支撐相位力位混合控制方法，它采用力位混合控制 的方法對(duì)機(jī)器人本體位姿進(jìn)行解耦控制，以增強(qiáng)機(jī)器人對(duì)非結(jié)構(gòu)地面和負(fù)載變化的適應(yīng)能 力。本發(fā)明提出了基于足端正壓力的力分配方法，該方法減小了支撐腿足端發(fā)生滑動(dòng)的可 能性，并提尚了機(jī)器人的加速性能。
[0030] 如圖4所示，本發(fā)明的四足機(jī)器人雙足支撐相位力位混合控制方法，步驟為：
[0031] S1 :將機(jī)器人的整體運(yùn)動(dòng)投影到徑向平面和法向平面上；所述徑向平面為對(duì)腳支 撐并垂直于水平面的平面；所述法向平面為通過本體質(zhì)心并且垂直于徑向平面的平面；
[0032] 由于四足機(jī)器人自由度繁多，建立機(jī)器人整體的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程較為繁雜， 因此需將四足機(jī)器人進(jìn)行合理簡(jiǎn)化：trot步態(tài)為兩個(gè)對(duì)腳支撐狀態(tài)的重復(fù)切換過程，因此 本發(fā)明將機(jī)器人的整體運(yùn)動(dòng)投影到由對(duì)腳支撐并垂直于水平面的徑向平面，以及通過質(zhì)心 并垂直于徑向平面的法向平面。
[0033] S2 :建立控制模型；
[0034] 在具體應(yīng)用實(shí)例中，機(jī)器人沿徑向平面上的運(yùn)動(dòng)可簡(jiǎn)化為平面七連桿模型，在法 向平面上的運(yùn)動(dòng)則可簡(jiǎn)化為線性倒立擺模型。
[0035] 其中，平面七連桿模型單腿具有三個(gè)前向轉(zhuǎn)動(dòng)關(guān)節(jié)，由于單腿的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué) 特性可等效為伸縮腿結(jié)構(gòu)模型，因此又將平面七連桿模型簡(jiǎn)化為具有伸縮腿結(jié)構(gòu)的平面虛 擬模型。平面虛擬伸縮腿模型的控制目標(biāo)為質(zhì)心高度、本體俯仰角以及水平位移。
[0036] S3 :根據(jù)控制模型進(jìn)行混合控制；建立平面虛擬伸縮腿模型的運(yùn)動(dòng)學(xué)方程并通過 牛頓-歐拉方法建立其動(dòng)力學(xué)方程，對(duì)于質(zhì)心高度和本體俯仰角通過位置伺服控制方法， 對(duì)虛擬伸縮腿平面模型的水平位移采用力控制方式。即：
[0037] 首先，建立該模型的運(yùn)動(dòng)學(xué)方程并通過牛頓-歐拉方法建立其動(dòng)力學(xué)方程。由于 位置控制方法具有很大剛度，因此對(duì)于本體質(zhì)心高度及俯仰角通過位置伺服控制方法來提 高機(jī)器人的負(fù)重能力。
[0038] 由于平面虛擬伸縮腿模型共含有4個(gè)驅(qū)動(dòng)關(guān)節(jié)，而整體控制目標(biāo)只有三個(gè)，因此 需添加一個(gè)約束條件才能實(shí)現(xiàn)對(duì)模型的整體控制。
[0039] 由于支撐腿水平方向足端力受摩擦錐的自然約束，整體結(jié)構(gòu)亦受ZMP點(diǎn)的動(dòng)力學(xué) 約束，為了使虛擬伸縮腿模型避免足端滑動(dòng)，需在水平方向足端力添加一個(gè)約束方程，即為 水平方向的力分配問題。因此，對(duì)虛擬伸縮腿平面模型的水平位移采用力控制方式。
[0040] 在具體應(yīng)用實(shí)例中，四足機(jī)器人在徑向平面內(nèi)的投影可以簡(jiǎn)化為一個(gè)七連桿結(jié) 構(gòu)，如圖2所示（左前腿和右后腿作為支撐腿）。其中，（x，z)為本體質(zhì)心在慣性坐標(biāo)系下 的位置，m為本體質(zhì)量，L。為機(jī)器人本體長(zhǎng)度，d為本體的寬度，1d12, 13分別為腿部踝關(guān)節(jié)、 膝關(guān)節(jié)、髖關(guān)節(jié)的長(zhǎng)度，θ1;θ2,θ3為左前腿踝關(guān)節(jié)角、膝關(guān)節(jié)角、髖關(guān)節(jié)角，Θ4,θ5, 06為 右后腿踝關(guān)節(jié)角、膝關(guān)節(jié)角、髖關(guān)節(jié)角，％為本體俯仰角，〇)3、(：、0分別為世界系原點(diǎn)（即 兩支撐腿足端連線中點(diǎn)）、右后腿髖部與本體連接點(diǎn)、左前腿髖部與本體連接點(diǎn)、右后腿足 端點(diǎn)、左前腿足端點(diǎn)，F(xiàn)xl，F(xiàn)zl，F(xiàn)x2,Fz2分別為C點(diǎn)和D點(diǎn)受到的接觸力。由于腿部質(zhì)量相對(duì)于 本體質(zhì)量很小，在建模時(shí)忽略腿部質(zhì)量與慣量。
[0041] 由圖2中左邊的示意