基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明主要涉及到機(jī)器人運(yùn)動控制技術(shù)領(lǐng)域，特指一種基于支撐線運(yùn)動分解的四 足機(jī)器人trot步態(tài)本體位姿控制方法。
【背景技術(shù)】
[0002] 四足機(jī)器人是一種模仿自然界中四足哺乳動物的骨骼結(jié)構(gòu)和行走步態(tài)的機(jī)器人， 充分結(jié)合了仿生學(xué)和機(jī)器人學(xué)的先進(jìn)技術(shù)，其最大的優(yōu)點(diǎn)是對山地、陡坡、沙漠等非結(jié)構(gòu)化 的地形環(huán)境有極強(qiáng)的適應(yīng)能力，可以用于在危險環(huán)境下執(zhí)行巡邏，物資運(yùn)輸?shù)纫恍?fù)雜任 務(wù)，擁有廣闊的應(yīng)用前景。
[0003] 四足機(jī)器人一般由一個本體和四條仿生腿構(gòu)成，每條仿生腿由2~3個前向關(guān)節(jié) 和一個側(cè)向關(guān)節(jié)組成，以保證足端點(diǎn)的運(yùn)動有三個自由度。目前比較成熟的四足機(jī)器人的 關(guān)節(jié)驅(qū)動器分為兩種：液壓驅(qū)動器和電機(jī)驅(qū)動器。通過對四足哺乳動物的運(yùn)動方式的學(xué)習(xí)、 分析、模仿，四足機(jī)器人的移動主要采用以下三種典型步態(tài)=WALK步態(tài)（爬行步態(tài)）、TROT 步態(tài)（對角小跑步態(tài)）和BOUNDING步態(tài)（奔跑步態(tài)）。
[0004] 在四足機(jī)器人以TROT步態(tài)行進(jìn)過程中，對角兩條腿的運(yùn)動相同，機(jī)器人依靠兩組 對角腿按照一定的規(guī)律在支撐腿模式和擺動腿模式之間周期性切換，完成對本體速度和姿 態(tài)的控制。目前，四足機(jī)器人運(yùn)動控制普遍采用以下方法：
[0005] 1、CPG控制方法；即中樞模式發(fā)生器，借助神經(jīng)元振蕩器模型產(chǎn)生周期性信號，控 制腿部運(yùn)動，這種方法的優(yōu)點(diǎn)是實(shí)現(xiàn)簡單，缺點(diǎn)是參數(shù)多，而且缺乏明確的物理含義，難以 準(zhǔn)確調(diào)節(jié)。
[0006] 2、機(jī)器學(xué)習(xí)；借助機(jī)器學(xué)習(xí)的理論和方法，使機(jī)器人能夠?qū)W習(xí)四足動物穩(wěn)定的行 走步態(tài)，但是需要大量的訓(xùn)練，很難在短時間適應(yīng)新的非結(jié)構(gòu)化環(huán)境。
[0007] 3、在線規(guī)劃；根據(jù)機(jī)器人當(dāng)前的傳感器信息，計算下一個或者幾個步行周期足端 點(diǎn)期望運(yùn)動軌跡，通過調(diào)整關(guān)節(jié)驅(qū)動力矩使得足端點(diǎn)跟蹤期望運(yùn)動軌跡。但是由于非結(jié)構(gòu) 化環(huán)境的未知性，可能出現(xiàn)擺動腿與環(huán)境提前接觸的情況，產(chǎn)生非預(yù)期沖擊力，影響本體的 速度和姿態(tài)。
[0008] 4、足端接觸力控制；通過力傳感器反饋?zhàn)愣伺c環(huán)境的接觸力信息，設(shè)計足端接觸 力控制算法，進(jìn)而控制機(jī)器人本體的速度和姿態(tài)。這種控制方法的缺點(diǎn)在于在兩條支撐腿 之間可能產(chǎn)生內(nèi)力，在支撐腿切換到擺動腿模式的時刻，內(nèi)力瞬間釋放，對擺動腿和本體的 姿態(tài)產(chǎn)生非預(yù)期沖擊。因此，需要進(jìn)一步設(shè)計控制策略，既能夠控制機(jī)器人本體的速度和姿 態(tài)，又能夠有效消除支撐腿之間的內(nèi)力。

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

[0009] 本發(fā)明要解決的技術(shù)問題就在于：針對現(xiàn)有技術(shù)存在的技術(shù)問題，本發(fā)明提供一 種通用性強(qiáng)、控制效果好、控制精度高的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體 位姿控制方法。
[0010] 為解決上述技術(shù)問題，本發(fā)明采用以下技術(shù)方案：
[0011] -種基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，其步驟為：
[0012] Sl :將機(jī)器人分別投影到徑向平面和法向平面上；所述徑向平面為通過支撐腿足 端點(diǎn)連線并且與水平面垂直的平面；所述法向平面為通過本體質(zhì)心并且垂直于徑向平面的 平面；
[0013] S2 :將機(jī)器人本體的期望速度使用與步驟Sl相同的方法投影到徑向平面和法向 平面上，得到兩個投影平面上的本體期望速度；
[0014] S3:分別建立簡化模型的運(yùn)動學(xué)和動力學(xué)方程，通過采用控制器使得兩個投影平 面上的本體速度跟蹤各自的期望軌跡，再進(jìn)行速度合成，完成對機(jī)器人本體的速度和姿態(tài) 的控制。
[0015] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述步驟Sl和S2中，在所述徑向平面上的投影簡化為 平面七連桿模型；通過調(diào)整關(guān)節(jié)驅(qū)動力矩使得平面七連桿的質(zhì)心位置、姿態(tài)跟蹤期望軌跡。
[0016] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述步驟Sl和S2中，在所述法向平面上的投影簡化為 一維線形倒立擺模型。
[0017] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述步驟S3中，通過計算、選擇法向平面內(nèi)擺動腿落 足點(diǎn)的位置，調(diào)整每個支撐期開始時刻機(jī)器人本體的速度、姿態(tài)，控制本體在支撐期結(jié)束時 刻法向平面內(nèi)的速度，與徑向平面內(nèi)機(jī)器人本體的速度進(jìn)行合成，完成對機(jī)器人速度的控 制。
[0018] 作為本發(fā)明的進(jìn)一步改進(jìn)：采用基于足端接觸力控制的雙環(huán)PID控制器，外環(huán)利 用本體質(zhì)心和姿態(tài)誤差計算期望的足端接觸力，且加入支撐腿足端之間內(nèi)力為零的約束條 件，計算出唯一的期望接觸力；內(nèi)環(huán)通過測量實(shí)際足端接觸力和期望接觸力之間的誤差計 算關(guān)節(jié)驅(qū)動力矩。
[0019] 與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)在于：
[0020] 1、本發(fā)明的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，在 四足機(jī)器人以TROT步態(tài)行進(jìn)過程中，采用基于速度分解和合成的控制策略可以實(shí)現(xiàn)本體 的速度姿態(tài)準(zhǔn)確跟蹤期望軌跡。
[0021] 2、本發(fā)明的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，所 提出的控制策略能夠有效減小四足機(jī)器人在行走過程中支撐腿足端之間的內(nèi)力。
[0022] 3、本發(fā)明的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，能 夠提高四足機(jī)器人對非結(jié)構(gòu)化環(huán)境的適應(yīng)能力，實(shí)現(xiàn)機(jī)器人在不平整地面上的行走。
[0023] 4、本發(fā)明的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，具 有較強(qiáng)的通用性?？刂撇呗圆灰蕾嚲唧w的四足機(jī)器人系統(tǒng)，只要構(gòu)建合適的傳感器系統(tǒng)，結(jié) 合機(jī)器人運(yùn)動學(xué)、動力學(xué)模型相關(guān)參數(shù)，適當(dāng)調(diào)整控制器參數(shù)，即可實(shí)現(xiàn)本發(fā)明所提出的方 法。
[0024] 5、本發(fā)明的基于支撐線運(yùn)動分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，結(jié) 構(gòu)清晰、層次分明，具有較好的理論和應(yīng)用價值。
【附圖說明】
[0025] 圖1是四足機(jī)器人的系統(tǒng)結(jié)構(gòu)示意圖。
[0026] 圖2是本發(fā)明在具體應(yīng)用時四足機(jī)器人的徑向平面投影示意圖。
[0027] 圖3是本發(fā)明在具體應(yīng)用時四足機(jī)器人的法向平面投影示意圖。
[0028] 圖4是本發(fā)明在具體應(yīng)用實(shí)例中的控制流程示意圖。
[0029] 圖5是本發(fā)明在具體應(yīng)用實(shí)例中的具體計算過程示意圖。
[0030] 圖6是本發(fā)明方法的流程示意圖。
【具體實(shí)施方式】
[0031] 以下將結(jié)合說明書附圖和具體實(shí)施例對本發(fā)明做進(jìn)一步詳細(xì)說明。
[0032] 本發(fā)明的方法主要適用于四足機(jī)器人，四足機(jī)器人的系統(tǒng)結(jié)構(gòu)如圖1所示，它包 括一個本體和四條仿生腿，即包括左前腿、右前腿、左后腿、右后腿。四條仿生腿的機(jī)械結(jié) 構(gòu)、尺寸相同，左前腿與左后腿，右前腿與右后腿的安裝方式相對于本體互為鏡像。每條仿 生腿包含四個主動關(guān)節(jié)（踝關(guān)節(jié)、膝關(guān)節(jié)、髖關(guān)節(jié)、髖部側(cè)向關(guān)節(jié)），各主動關(guān)節(jié)均采用液壓 作動器驅(qū)動，足端與地面為點(diǎn)接觸。每個主動關(guān)節(jié)裝有位移傳感器和力傳感器，用于檢測作 動器長度信息和關(guān)節(jié)力信息，在足端點(diǎn)通過三維力傳感器反饋?zhàn)愣伺c地面的接觸力信息， 機(jī)器人本體在慣性空間中的位置、姿態(tài)可通過安裝在上體的IMU姿態(tài)傳感器實(shí)時檢測。
[0033] 在本發(fā)明中，行走步態(tài)默認(rèn)為TROT步態(tài)。如圖6所示，本發(fā)明的基于支撐線運(yùn)動 分解的四足機(jī)器人trot步態(tài)本體位姿控制方法，其步驟為：
[0034] Sl :將機(jī)器人分別投影到徑向平面和法向平面上；
[0035] 所述徑向平面為通過支撐腿足端點(diǎn)連線并且與水平面垂直的平面；所述法向平面 為通過本體質(zhì)心并且垂直于徑向平面的平面；
[0036] 在所述徑向平面上的投影簡化為平面七連桿模型（如圖2所示），在所述法向平面 上的投影簡化為一維線形倒立擺模型（如圖3所示）；
[0037] S2 :將機(jī)器人本體的期望速度使用與步驟Sl相同的方法投影到徑向平面和法向 平面上，得到兩個投影平面上的本體期望速度；
[0038] 基于本體的期望前向速度Vd(本體的側(cè)向期望速度為0)，計算兩個投影平面內(nèi)本 體的期望速度。
[0039] Vsd= V dcos α
[0040] Vfd=Vdsina
[0041] 其中Vd為本體的前向期望速度，Vsd為本體在徑向平面內(nèi)的期望速度，V fdS本體在 法向平面內(nèi)的期望速度，a為機(jī)器人前向速度與徑向平面之間的夾角。
[0042] S3:分別建立簡化模型的運(yùn)動學(xué)和動力學(xué)方程，通過采用控制器使得兩個投影平 面上的本體速度跟蹤各自的期望軌跡，再進(jìn)行速度合成，以完成對機(jī)器人本體的速度和姿 態(tài)的控制。
[0043] 以左前腿和右后腿處于支撐期，右前腿和左后腿處于擺動期為例：
[0044] TROT步態(tài)四足機(jī)器人的動力學(xué)模型為：
[0045] 徑向平面：
[0046]
LiN 丄uoid/yoy a yJ^ u ·* 4/5 ^
[0047]
[0048] 其中，qi= [X，Y，β ] T表示本體在徑向平面內(nèi)的水平位置、高度和俯仰角，
[0049] q2= [X D Yc，Θ dl，XD，YD，Θ J T，[Xc，Yc]T ([XD，YD]τ)為右后腿（左前腿）足端相 對于髖關(guān)節(jié)安裝點(diǎn)的位置，9 dl(0d2)為右后腿（左前腿）前向關(guān)節(jié)角線性約束，F(xiàn)e = Fm2, FyJτ為支撐腿足端接觸力向量，τ為支撐腿關(guān)節(jié)驅(qū)動力矩，V/，V/ %慣性矩 陣，為哥氏力矩陣，戈，％為重力項，JuJ5為雅克比矩陣。
[0050] 法向平面：
[0051]
[0052]
[0053]
[0054] 其中，I (t)為本體在法向平面內(nèi)的水平位置，I (0)為支撐期初始時刻本體在法向 平面內(nèi)的水平位置，/(〇)為支撐期初始時刻本體在法向平面內(nèi)的水平速度。g為重力加速 度。
[0055] 控制器分為內(nèi)環(huán)和外環(huán)兩部分：
[0056] 外環(huán)控制器：
[0057]
[0058] 其中如為q i的期望軌跡，F(xiàn) ed為支撐腿足端期望接觸力，Kp i，Kd1, Ki1為外環(huán)控制 參數(shù)。
[0059] 內(nèi)環(huán)控制器：
[0060]
[0061] 其中，Kp2為內(nèi)環(huán)控制參數(shù)。
[0062] h時刻線性倒立擺初始位置的計算公式為：
[0063]
[0064] 其中，T為單個支撐期時間。
[0065] 在上述過程中，將機(jī)器人投影到徑向平面上，簡化為一個平面七連桿后，通過調(diào)整 關(guān)節(jié)驅(qū)動力矩使得平面七連桿的質(zhì)心位置（2維）、姿態(tài)（1維）跟蹤期望軌跡。
[0066] 在上述步驟S3中，本發(fā)明采用基于足端接觸力控制的雙環(huán)PID控制器，外環(huán)利用 本體質(zhì)心和姿態(tài)誤差計算期望的足端接觸力，由于控制量為4維，控制目標(biāo)為3維，所以加 入支撐腿足端之間