一種輪足式機(jī)器人的柔順控制方法
【專利摘要】本發(fā)明公開了一種輪足式機(jī)器人的柔順控制方法，包括：外環(huán)控制，為機(jī)器人本體位姿控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的位姿軌跡；中環(huán)控制，為足端接觸力控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的位姿軌跡；內(nèi)環(huán)控制，為主動(dòng)關(guān)節(jié)驅(qū)動(dòng)力矩控制，控制目標(biāo)是實(shí)現(xiàn)支撐腿足端接觸力跟蹤外環(huán)計(jì)算得到的期望力；通過上述三個(gè)控制，在準(zhǔn)確控制機(jī)器人本體位姿的同時(shí)，對支撐腿足端接觸力進(jìn)行控制。本發(fā)明具有原理簡單、控制精度高、能夠提高機(jī)器人適應(yīng)能力等優(yōu)點(diǎn)。
【專利說明】
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明主要涉及到機(jī)器人的控制【技術(shù)領(lǐng)域】，特指一種適用于輪足式機(jī)器人的柔順 控制方法。 一種輪足式機(jī)器人的柔順控制方法

【背景技術(shù)】
[0002] 輪式機(jī)器人是以輪式機(jī)構(gòu)為支撐和驅(qū)動(dòng)的移動(dòng)機(jī)器人，其主要優(yōu)點(diǎn)是速度快、能 耗低，缺點(diǎn)是越障能力、非結(jié)構(gòu)化地形適應(yīng)能力差、轉(zhuǎn)彎半徑大。足式機(jī)器人以步行、奔跑和 跳躍為移動(dòng)方式，其主要優(yōu)點(diǎn)是重量輕、轉(zhuǎn)彎半徑小和地形適應(yīng)性強(qiáng)，缺點(diǎn)是相對于輪式機(jī) 器人速度較慢，結(jié)構(gòu)復(fù)雜。輪足式機(jī)器人綜合了輪式機(jī)器人和足式機(jī)器人的構(gòu)型優(yōu)點(diǎn)，既繼 承了足式機(jī)器人對非結(jié)構(gòu)化地形的適應(yīng)能力又避免了其結(jié)構(gòu)復(fù)雜的缺點(diǎn)。
[0003] 輪足式機(jī)器人一般分為三種：1、輪式機(jī)構(gòu)和足式機(jī)構(gòu)在安裝方式上相互獨(dú)立，輪 式機(jī)構(gòu)帶有驅(qū)動(dòng)裝置，即輪子為主動(dòng)輪；2、輪式機(jī)構(gòu)和足式機(jī)構(gòu)在安裝方式上相互獨(dú)立，輪 式機(jī)構(gòu)不帶有驅(qū)動(dòng)裝置，即輪子為從動(dòng)輪，機(jī)器人依靠腿部的主動(dòng)關(guān)節(jié)運(yùn)動(dòng)；3、輪式機(jī)構(gòu)安 裝在腿部或者足端。
[0004] 現(xiàn)有對于輪足式機(jī)器人的行走控制，基本的方法是通過計(jì)算腿部各個(gè)主動(dòng)關(guān)節(jié)的 驅(qū)動(dòng)力矩來實(shí)現(xiàn)對機(jī)器人本體位置和姿態(tài)的控制。輪足式機(jī)器人在非結(jié)構(gòu)化地形中的穩(wěn)定 行走，要求腿部對環(huán)境有柔順適應(yīng)性。目前，機(jī)器人控制中普遍采用以下方法：
[0005] 1、被動(dòng)柔順；即借助柔性機(jī)構(gòu)或者柔性關(guān)節(jié)（例如彈簧）使機(jī)器人對環(huán)境作用力 產(chǎn)生自然順從，但是被動(dòng)柔順方法能夠提供的機(jī)器人對非結(jié)構(gòu)化環(huán)境的適應(yīng)能力有限，而 且容易使機(jī)器人足端在行走過程中產(chǎn)生滑動(dòng)。
[0006] 2、腿部關(guān)節(jié)期望運(yùn)動(dòng)軌跡的在線修正；利用接觸開關(guān)檢測信息判斷足端與地面的 接觸狀態(tài)，根據(jù)接觸狀態(tài)對支撐腿部關(guān)節(jié)的期望位置軌跡進(jìn)行在線修正。這種方法能夠?yàn)?機(jī)器人提供一定的柔順性，但是由于落足瞬間地面與足端為剛性接觸，所以會(huì)產(chǎn)生較大的 沖擊力，容易影響機(jī)器人本體的穩(wěn)定性。
[0007] 3、足端接觸力控制；通過力傳感器反饋?zhàn)愣嗽跀[動(dòng)期和支撐期轉(zhuǎn)換階段與地面的 接觸力信息，設(shè)計(jì)足端接觸力控制算法，實(shí)現(xiàn)機(jī)器人足端接觸地面的柔順性。足端接觸力控 制策略可以實(shí)現(xiàn)機(jī)器人足端與地面的柔順接觸，但是會(huì)導(dǎo)致控制目標(biāo)向量維數(shù)擴(kuò)大，影響 機(jī)器人本體位姿的可控性。


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

[0008] 本發(fā)明要解決的技術(shù)問題就在于：針對現(xiàn)有技術(shù)存在的技術(shù)問題，本發(fā)明提供一 種原理簡單、控制精度高、能夠提高機(jī)器人適應(yīng)能力的輪足式機(jī)器人的柔順控制方法。
[0009] 為解決上述技術(shù)問題，本發(fā)明采用以下技術(shù)方案：
[0010] 一種輪足式機(jī)器人的柔順控制方法，包括：
[0011] 外環(huán)控制，為機(jī)器人本體位姿控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期 望的位姿軌跡；
[0012] 中環(huán)控制，為足端接觸力控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的 位姿軌跡；
[0013] 內(nèi)環(huán)控制，為主動(dòng)關(guān)節(jié)驅(qū)動(dòng)力矩控制，控制目標(biāo)是實(shí)現(xiàn)支撐腿足端接觸力跟蹤外 環(huán)計(jì)算得到的期望力；
[0014] 通過上述三個(gè)控制，在準(zhǔn)確控制機(jī)器人本體位姿的同時(shí)，對支撐腿足端接觸力進(jìn) 行控制。
[0015] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述外環(huán)控制中，首先建立基于牛頓-歐拉方程的包 含非完整約束條件的機(jī)器人本體動(dòng)力學(xué)模型，將通過慣性傳感器得到的機(jī)器人本體位姿數(shù) 據(jù)作為反饋量，與期望位姿軌跡比較；然后，基于動(dòng)力學(xué)模型設(shè)計(jì)PID控制器，計(jì)算控制量， 即支撐腿足端期望接觸力；最后，對接觸面切向力進(jìn)行限幅，使之滿足摩擦錐約束。
[0016] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述基于牛頓-歐拉方程的機(jī)器人本體動(dòng)力學(xué)模型方 程如下：
[0017] Μχ β Fey ⑴
[0018] y = R+L〇sin β (2)
[0019] y = 0 (3)
[0020] xsina + zcosa = 0 (4)
[0021] 其中，Mi為慣性矩陣，A為力臂矩陣，x，y，z為機(jī)器人本體位置，x軸方向?yàn)闄C(jī)器人 本體前進(jìn)方向，y軸方向與重力方向相反，z軸方向由右手法則確定；F ex，F(xiàn)ey，F(xiàn)ez為支撐腿足 端接觸力，R為后輪半徑，U為機(jī)器人本體質(zhì)心與輪軸之間的距離，α，β，γ分別為機(jī)器人 本體的方位角、俯仰角和滾動(dòng)角；公式（1)表示機(jī)器人本體的動(dòng)力學(xué)方程，公式（2)和公式 (3)表示完整約束條件，公式（4)表示非完整約束條件。
[0022] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述外環(huán)控制采用PID控制器作為外環(huán)控制器，具體 形式如下： 「1 Kpx (?-x) + Kdx (xd-x) + kix^(xJ-x)dr
[0023] Feyd = A lMl Κρβ {β?~β) + Κ?β {^β? -β) + kip^{β?-β)?τ L Κρα (atl -a) + Kda (?, -?) + Μα ￡ (a, -a)dT
[0024] 其中，F(xiàn)' exd，F(xiàn)' eyd，F(xiàn)' ezd為期望的足端接觸力，xd為期望的χ方向位置，β d為期望 的俯仰角，a d為期望的方位角；加入摩擦錐約束條件：F' exd〈 μ F' eyd、F' ezd〈 μ F' eyd，μ為摩 擦系數(shù)，控制器變?yōu)椋?br> [0025] Feyd = F' eyd π \ F\,d F - -順，-順-
[ ] exd _ I p' f > p' p' -〇 SjuF L exd_max exd - exd_max exd_max * eyd 「?r-, _ I ^ ezd F e=d < F m-輝，!7e:d max=Q^pF ey(i ezd ~\Ρ' F > F F = 〇 %uF L ezd_maK ezd - ezd msy.9 ezd υω?χ
[0028] 通過調(diào)節(jié)參數(shù)Kpx、Kdx、kix、Kp e、Kde、kie、Κρα、Kda、ki a，保證在控制本體位姿的 同時(shí)，避免機(jī)器人在行走和轉(zhuǎn)彎的過程中出現(xiàn)支撐腿足端滑動(dòng)的現(xiàn)象。
[0029] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述中環(huán)控制中，首先基于支撐腿和擺動(dòng)腿解耦控制 的思想建立支撐腿的運(yùn)動(dòng)學(xué)模型和基于拉格朗日方程的動(dòng)力學(xué)模型，通過足端力傳感器反 饋?zhàn)愣私佑|力信息，計(jì)算支撐腿的各個(gè)主動(dòng)關(guān)節(jié)的輸出力矩；中環(huán)控制采用基于ro算法的 力/位混合控制器。
[0030] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述支撐腿的運(yùn)動(dòng)學(xué)模型如下：
[0031 ] 在支撐腿髖部建立坐標(biāo)系，各坐標(biāo)軸方向與機(jī)器人本體坐標(biāo)系方向相同，在髖部 坐標(biāo)系下，足端點(diǎn)的坐標(biāo)為：
[0032] xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3C0S ( θ 3_ ) -L2C0S ( θ 2_ ? 3~*~ )
[0033] ye = -cos θ 4 (Ljsin ( θ θ 2+θ 3-β )+L3sin ( θ 3-β )+L2sin ( θ 2-θ 3+β )) (5)
[0034] ze = -sin θ 4 (I^sin ( θ 廠 θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β ))
[0035] 其中，（xe，ye，ze)為足端點(diǎn)坐標(biāo)，Lp L2, L3分別為腳掌、小腿、大腿的長度， θ2, θ3, 04分別為踝關(guān)節(jié)角、膝關(guān)節(jié)角、髖關(guān)節(jié)角、髖部側(cè)向關(guān)節(jié)角；
[0036] 令 θ = [θ" θ2, θ3, 0jT，q = [xe，ye，zJT，奪= [υ.，ζ』-(?]7'，則 $ = .%，其中J為支撐腿雅克比矩陣，J為增廣雅克比矩陣： dv./o^ cxjd62 cxjde, c\xr/d04
[0037] J= dyJd〇l dye[d02 dyjd03 dye/d04 (6) czJdO^ dzjc02 czJdO. DzJceA_ &',/叫 碑（3λ:/坤（1\',/('沒4 romsi J= dy^d0x ?γ^?θι dy^d°l dy^d0A (7) cz,/c^ czjde, czjde, czJc6a 1 -1 0 0
[0039] 基于拉格朗日方程的支撐腿動(dòng)力學(xué)方程為：
[0040] /W (<9)Θ f €(θ,θ)Θ f Ν[θ,θ) = τ + JrFe (8)
[0041] 其中，Μ(θ)為慣性矩陣，C|>,句為哥式力矩陣，?為重力項(xiàng)，F(xiàn)e為足端接觸 力向量，將$ = 代入公式（8)，得到：
[0042] M (cj)cj + C{q,c/)c/ Ν= v + JlFe
[0043] 其中，々⑷=Μ⑷⑷-\ (Τ'(以）=C(以)(J)'+從⑷。 V J
[0044] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述中環(huán)控制的控制器如下：
[0045] ^ 卜札-匕)+人艱夂)_ τ=Μ 〇 +Q + iV -，仰2(匕-/g + A?/2(Z：,.(/-心)。 ^4 (0 - ^： + )+Kih (〇 - 4 + 4) Kp'A'h - D+κ(!人匕廠匕)_
[0046] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述內(nèi)環(huán)控制是利用關(guān)節(jié)位移傳感器和力傳感器實(shí)時(shí) 檢測作動(dòng)器長度信息和關(guān)節(jié)力信息，根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算關(guān)節(jié)力臂，解算出關(guān)節(jié)期望力，與 實(shí)際的關(guān)節(jié)力進(jìn)行比較；再利用PI控制器，生成關(guān)節(jié)作動(dòng)器的控制電流，實(shí)現(xiàn)關(guān)節(jié)力準(zhǔn)確 跟蹤期望力，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū)動(dòng)力矩對期望力矩的跟蹤。
[0047] 作為本發(fā)明的進(jìn)一步改進(jìn)：所述內(nèi)環(huán)控制的PI控制器如下：
[0048] i - KPi (./；, -./) + Κι) ￡ (./；, -f)dr
[0049] 生成關(guān)節(jié)作動(dòng)器的控制電流i，實(shí)現(xiàn)關(guān)節(jié)力f準(zhǔn)確跟蹤期望力fd，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū) 動(dòng)力矩對期望力矩的跟蹤。
[0050] 與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)在于：
[0051] 1、在輪足式機(jī)器人運(yùn)動(dòng)過程中，采用本發(fā)明設(shè)計(jì)的三環(huán)控制策略可以實(shí)現(xiàn)本體的 位姿準(zhǔn)確跟蹤由步態(tài)規(guī)劃生成的期望位姿軌跡。
[0052] 2、本發(fā)明提出的控制策略能夠有效避免機(jī)器人在行走、轉(zhuǎn)彎過程中支撐腿足端滑 動(dòng)的現(xiàn)象。
[0053] 3、本發(fā)明能夠提高輪足式機(jī)器人對非結(jié)構(gòu)化環(huán)境的適應(yīng)能力，實(shí)現(xiàn)機(jī)器人在不平 整地面上的行走、轉(zhuǎn)彎。
[0054] 4、本發(fā)明具有較強(qiáng)的通用性?？刂撇呗圆灰蕾嚲唧w的輪足式機(jī)器人系統(tǒng)，只要構(gòu) 建合適的傳感器體系，推導(dǎo)機(jī)器人運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型，適當(dāng)調(diào)整控制器參數(shù)，即可實(shí)現(xiàn)本 發(fā)明所提出的方法。
[0055] 5、本發(fā)明結(jié)構(gòu)清晰、層次分明，具有較好的理論價(jià)值和工程意義

【專利附圖】

【附圖說明】
[0056] 圖1是輪足式機(jī)器人系統(tǒng)平面示意圖。
[0057] 圖2是本發(fā)明的控制結(jié)構(gòu)框圖。
[0058] 圖3是單個(gè)步行周期內(nèi)控制流程圖。

【具體實(shí)施方式】
[0059] 以下將結(jié)合說明書附圖和具體實(shí)施例對本發(fā)明做進(jìn)一步詳細(xì)說明。
[0060] 本發(fā)明主要應(yīng)用于輪足式機(jī)器人系統(tǒng)，主要針對的對象為輪式機(jī)構(gòu)和足式機(jī)構(gòu)在 安裝方式上相互獨(dú)立，輪式機(jī)構(gòu)不帶有驅(qū)動(dòng)裝置的輪足式機(jī)器人，即輪子為從動(dòng)輪，機(jī)器人 依靠腿部的主動(dòng)關(guān)節(jié)運(yùn)動(dòng)。如圖1所示，該輪足式機(jī)器人系統(tǒng)由兩條腿和兩個(gè)從動(dòng)輪輪子 組成，每條腿包含四個(gè)主動(dòng)關(guān)節(jié)（踝關(guān)節(jié)、膝關(guān)節(jié)、髖關(guān)節(jié)、髖部側(cè)向關(guān)節(jié)），各主動(dòng)關(guān)節(jié)均 采用液壓作動(dòng)器驅(qū)動(dòng)，足端與地面為點(diǎn)接觸。每個(gè)主動(dòng)關(guān)節(jié)裝有位移傳感器和力傳感器，用 于檢測作動(dòng)器長度信息和關(guān)節(jié)力信息，在足端點(diǎn)通過三維力傳感器反饋?zhàn)愣伺c地面的接觸 力信息，機(jī)器人本體在慣性空間中的位置、姿態(tài)可通過MU實(shí)時(shí)檢測。作為本發(fā)明的控制對 象，上述輪足式機(jī)器人具有如下機(jī)械結(jié)構(gòu)特性：
[0061] a、機(jī)器人重心靠近后輪，即在單腿支撐狀態(tài)下機(jī)器人滿足ZMP穩(wěn)定性條件，可以 穩(wěn)定站立。
[0062] b、基于特性a，輪足式機(jī)器人在行走過程中，后輪不會(huì)離開地面。
[0063] c、機(jī)器人本體的質(zhì)量遠(yuǎn)大于腿部的質(zhì)量。
[0064] 如圖2所示，發(fā)明輪足式機(jī)器人的柔順控制方法，基于支撐腿和擺動(dòng)腿解耦控制 的思想，采用三環(huán)控制策略，即：外環(huán)為機(jī)器人本體位姿控制，中環(huán)為足端接觸力控制，內(nèi)環(huán) 為主動(dòng)關(guān)節(jié)驅(qū)動(dòng)力矩控制。在準(zhǔn)確控制機(jī)器人本體位姿的同時(shí)，將支撐腿足端接觸力控制 在一個(gè)較小的范圍內(nèi)，避免支撐腿足端滑動(dòng)，提高機(jī)器人對非結(jié)構(gòu)化環(huán)境的適應(yīng)能力。圖 rA Γ 0 ~r3 r2 中，，，= r3 0 -71 ; Fd為足端與地面期望接觸力；F為足端與地面實(shí)際接觸 _r3」 卜2 η 〇 _ 力；fd為關(guān)節(jié)期望力；f為關(guān)節(jié)實(shí)際力；τ d為期望輸出力矩。
[0065] 外環(huán)控制，其控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的位姿軌跡。由于輪 子受到非完整約束（輪子在軸向滿足速度約束，但是無位置約束），所以首先建立基于牛 頓-歐拉方程的包含非完整約束條件的機(jī)器人本體動(dòng)力學(xué)模型，將通過慣性傳感器得到的 機(jī)器人本體位姿數(shù)據(jù)（慣性坐標(biāo)系下的三維位置信息、方位角、俯仰角、滾動(dòng)角）作為反饋 量，與期望位姿軌跡比較；然后，基于動(dòng)力學(xué)模型設(shè)計(jì)PID控制器，計(jì)算控制量一-支撐腿足 端期望接觸力。最后，對接觸面切向力進(jìn)行限幅，使之滿足摩擦錐約束，這就可以在實(shí)現(xiàn)輪 足式機(jī)器人本體位姿控制的同時(shí)，有效避免行走過程中支撐腿足端滑動(dòng)。
[0066] 中環(huán)控制，其控制目標(biāo)是實(shí)現(xiàn)支撐腿足端接觸力跟蹤外環(huán)計(jì)算得到的期望力，從 而提高機(jī)器人對非結(jié)構(gòu)化環(huán)境的適應(yīng)能力。首先，基于支撐腿和擺動(dòng)腿解耦控制的思想，建 立支撐腿的運(yùn)動(dòng)學(xué)模型和基于拉格朗日方程的動(dòng)力學(xué)模型。通過足端力傳感器反饋?zhàn)愣私?觸力信息，設(shè)計(jì)控制算法，計(jì)算支撐腿的各個(gè)主動(dòng)關(guān)節(jié)的輸出力矩。由于單腿有四個(gè)主動(dòng)關(guān) 節(jié)（三個(gè)前向關(guān)節(jié)，一個(gè)側(cè)向關(guān)節(jié)，見附圖1)，而足端接觸力信息的維數(shù)為三維，所以加入 一個(gè)運(yùn)動(dòng)學(xué)約束條件一踝關(guān)節(jié)角與膝關(guān)節(jié)角相等，保證支撐腿雅克比矩陣為方陣。中環(huán)控 制器為基于ro算法的力/位混合控制器。經(jīng)過實(shí)驗(yàn)驗(yàn)證，支撐腿足端接觸力可以準(zhǔn)確跟蹤 期望力。
[0067]內(nèi)環(huán)控制，其作用是實(shí)現(xiàn)對關(guān)節(jié)期望力矩的跟蹤。借助關(guān)節(jié)位移傳感器和力傳感 器實(shí)時(shí)檢測作動(dòng)器長度信息和關(guān)節(jié)力信息，根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算關(guān)節(jié)力臂，解算出關(guān)節(jié)期 望力，與實(shí)際的關(guān)節(jié)力進(jìn)行比較；進(jìn)一步設(shè)計(jì)PI控制器，生成關(guān)節(jié)作動(dòng)器的控制電流，實(shí)現(xiàn) 關(guān)節(jié)力準(zhǔn)確跟蹤期望力，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū)動(dòng)力矩對期望力矩的跟蹤。
[0068] 在上述控制過程中，建模的過程為：
[0069] 首先建立機(jī)器人本體坐標(biāo)系，X軸方向?yàn)闄C(jī)器人本體前進(jìn)方向，y軸方向與重力方 向相反，z軸方向由右手法則確定。由于行走過程中兩個(gè)后輪不會(huì)離開地面，機(jī)器人本體受 到三個(gè)運(yùn)動(dòng)學(xué)約束，其中兩個(gè)為完整約束，一個(gè)為非完整約束?；谂ｎD-歐拉方程的單腿 支撐期機(jī)器人本體動(dòng)力學(xué)方程（本發(fā)明以左前腿支撐為例）如下： '!] P;
[0070] ΜΛ β =A Fey (1) β\ \_Fe：_
[0071] y = R+L〇sin β (2)
[0072] y = 0 (3)
[0073] xsinor + zcosa = 0 (4)
[0074] 其中，Mi為慣性矩陣，A為力臂矩陣，x，y，z為機(jī)器人本體位置，F(xiàn)ex，F(xiàn) ey，F(xiàn)ez為支撐 腿足端接觸力，R為后輪半徑，U為機(jī)器人本體質(zhì)心與輪軸之間的距離，α，β，γ分別為機(jī) 器人本體的方位角、俯仰角和滾動(dòng)角。公式（1)表示機(jī)器人本體的動(dòng)力學(xué)方程，公式（2)和 公式（3)表示完整約束條件，公式（4)表示非完整約束條件。
[0075] 在支撐腿髖部建立坐標(biāo)系（以左腿為例），各坐標(biāo)軸方向與機(jī)器人本體坐標(biāo)系方 向相同，在髖部坐標(biāo)系下，足端點(diǎn)的坐標(biāo)為：
[0076] xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3COS ( θ 3_ ) -L2COS ( θ 2_ ? 3~*~ )
[0077] ye = -cos θ 4 (I^sin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) (5)
[0078] ze = -sin θ 4 (I^sin ( θ 廠 θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β ))
[0079] 其中，（xe，ye，ζε)為足端點(diǎn)坐標(biāo)，Lp L2, L3分別為腳掌、小腿、大腿的長度， θ2, θ3, 04分別為踝關(guān)節(jié)角、膝關(guān)節(jié)角、髖關(guān)節(jié)角、髖部側(cè)向關(guān)節(jié)角。
[0080] 令 θ = [Θ" θ2, θ3, 04]T，q = [xe，ye，zJT，$ = 沒:]'，則 4 = | = %,其中J為支撐腿雅克比矩陣，J為增廣雅克比矩陣： dv,/e<9, dv./oft clv,/e<9, dv./e^4
[0081] J= dyjd0l dyjd02 dyjd03 cyJcO^ (6) czjce, dzjc02 czjc0, οζ^οθΑ_ cxJdO^ dxjd02 cxjde, cxjde4 Γ00821 J= δ)；，/δθ' Cy<,Cai d)；，/d^ 〇}：，^04 (7) νζ,,/νθ^ czJ〇e2 czjde.^ czjde4 1 -1 0 0
[0083] 基于拉格朗日方程的支撐腿動(dòng)力學(xué)方程為：
[0084] Μ (/}θ + C (θ,--θ + N 0,0) = τ + JT Fe (8)
[0085] 其中，Μ( θ )為慣性矩陣，為哥式力矩陣，為重力項(xiàng)，F(xiàn)e為足端接觸 力向量，將| = 代入公式（8)，得到：
[0086] M {q)ij + C(ci，q)q + N(ci，q) = T + JTFe (9)
[0087] 其中，A ⑷=M ⑷(J·) e(iM〇 = C(以)(J)丨+從⑷ 。 V J
[0088] 在上述控制過程中，控制器的設(shè)計(jì)過程為：
[0089] 基于公式（1)，設(shè)計(jì)經(jīng)典PID控制器作為外環(huán)控制器，具體形式如下： 「 ? Κρλ (.Vf/ -x) + Kdx (ir/ -Λ-) + ki^ (.V,, -χ'γ?τ
[0090] F e'.d 二 A ' M、Kp - β) + Kd - 0、+ ki."、、[pd - β、1τ (10) L 以」 卜" (α(/ - a) + - ?) + 人乂』(a" - a )i/r
[0091] 其中，匕!￡(^'_^'^為期望的足端接觸力，&為期望的1方向位置，0 (1為期望 的俯仰角，〇(1為期望的方位角。加入摩擦錐約束條件：？'^〈以？'_、？'^〈以？'_，以為摩 擦系數(shù)，控制器變?yōu)椋?br> [0092] Feyd = F' eyd 「〇1 P j F，eu, F W 咖應(yīng)=〇.8"廣川、
[0093] 巧w= ， F' >F' ^ F' ^ =0SuF' (H) L extf_niax exd - exd _max9 exd max ?。戶i .Λ1 π _ \ F，e,d F，e:d <F，e"l ^F，e,d max L0094J tezd - /7' >F' F' =()8〃F' L ezd _max ezd - ezd _max9 ezii_max · A*J eyd
[0095] 通過調(diào)節(jié)參數(shù)Kpx、Kdx、kix、Kp e、Kde、kie、Kpa、Kda、ki a，可以保證在控制本體位 姿的同時(shí)，避免機(jī)器人在行走和轉(zhuǎn)彎的過程中出現(xiàn)支撐腿足端滑動(dòng)的現(xiàn)象。
[0096] 基于公式（9)，設(shè)計(jì)中環(huán)控制器為：
[0097] r = M 〇 +Cq + N-Jr Kp2 (Feyd - Fey) + Kd2 (Feyd - Fev) Kp4 (0 - 6?, + ) + KdA (〇 - ^ + <92) [ KP3 -K,) + Kd, {F", - K )_ (12)
[0098] 經(jīng)過實(shí)驗(yàn)驗(yàn)證，控制器公式（12)可以使支撐腿足端接觸力準(zhǔn)確跟蹤期望力。
[0099] 內(nèi)環(huán)控制回路的作用是實(shí)現(xiàn)對關(guān)節(jié)期望力矩的跟蹤。借助關(guān)節(jié)位移傳感器實(shí)時(shí)檢 測作動(dòng)器長度信息，根據(jù)支撐腿運(yùn)動(dòng)學(xué)模型，計(jì)算關(guān)節(jié)力臂，解算出關(guān)節(jié)期望力fd，設(shè)計(jì)PI 控制器如下：
[0100] ? = ΚΡχ---?) + Κ?.Ι^\-Γ)?τ (13)
[0101] 生成關(guān)節(jié)作動(dòng)器的控制電流i，實(shí)現(xiàn)關(guān)節(jié)力f準(zhǔn)確跟蹤期望力fd，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū) 動(dòng)力矩對期望力矩的跟蹤。
[0102] 如圖3所示，本發(fā)明中輪足式機(jī)器人前進(jìn)的動(dòng)力主要來自支撐腿與地面之間的摩 擦力，在前進(jìn)過程中兩條前腿以固定周期T交替作為支撐腿，在具體實(shí)施例中，其具體實(shí)施 過程如下：
[0103] (1)輪足式機(jī)器人的初始狀態(tài)為雙足雙輪支撐狀態(tài)。
[0104] (2)在0到T/4時(shí)間內(nèi)，左腿由支撐腿變?yōu)閿[動(dòng)腿，按照足端點(diǎn)規(guī)劃軌跡運(yùn)動(dòng)至最 高點(diǎn)（本發(fā)明主要的對象為支撐腿的柔順控制方法，擺動(dòng)腿的規(guī)劃不在本發(fā)明的研究范圍 內(nèi)），右腿作為支撐腿，根據(jù)公式（10)、公式（11)計(jì)算右腿足端期望接觸力 計(jì)算頻率為400Hz。
[0105] (3)在0到T/4時(shí)間內(nèi)，根據(jù)步驟⑵中得到的右腿足端期望接觸力 通過公式（12)計(jì)算得到右腿各個(gè)主動(dòng)關(guān)節(jié)的輸出力矩計(jì)算頻率為 4KHz。
[0106] (4)在0到T/4時(shí)間內(nèi)，根據(jù)步驟（3)中得到的主動(dòng)關(guān)節(jié)輸出力矩通過公 式（13)計(jì)算得到右腿關(guān)節(jié)作動(dòng)器的控制電流U通過驅(qū)動(dòng)電路輸出控制電流，控制頻率為 ΙΟΚΗζο
[0107] (5)在T/4到T/2時(shí)間內(nèi)，右腿作為支撐腿重復(fù)步驟⑵?（4)，左腿按照 規(guī)劃軌跡從最高點(diǎn)落足，同時(shí)以ΙΚΗζ的頻率實(shí)時(shí)檢測左腿足端接觸力是否滿足條件 |匕| = C > HW，如果不滿足，返回步驟(5)，如果滿足，進(jìn)入步驟（6)。
[0108] (6)在T4到T2時(shí)間內(nèi)，滿足條件||。| = yjK+K,+C之1祕的情況下，右腿作為 支撐腿重復(fù)步驟（2)?（4)，左腿由擺動(dòng)腿變?yōu)橹瓮龋鶕?jù)公式（10)、公式（11)計(jì)算左腿 足端期望接觸力Fexdl，F(xiàn)eydl，F(xiàn)ezdl，計(jì)算頻率為400Hz。
[0109] (7)在T/4到T/2時(shí)間內(nèi)，滿足條件||心|| =」F?F上,>1(W的情況下，右腿作 為支撐腿重復(fù)步驟（2)?（4)。根據(jù)步驟（6)中得到的左腿足端期望接觸力Fexdl，F(xiàn)eydl，F(xiàn) ezdl， 通過公式（12)計(jì)算得到左腿各個(gè)主動(dòng)關(guān)節(jié)的輸出力矩τι，計(jì)算頻率為4KHz。
[0110] (8)在T/4到T/2時(shí)間內(nèi)，滿足條件||心I = 〇 〇 107V的情況下，右腿作 為支撐腿重復(fù)步驟（2)?（4)。根據(jù)步驟（7)中得到的主動(dòng)關(guān)節(jié)輸出力矩τ i，通過公式（13) 計(jì)算得到左腿關(guān)節(jié)作動(dòng)器的控制電流h，通過驅(qū)動(dòng)電路輸出控制電流，控制頻率為ΙΟΚΗζ。
[0111] (9)在T/2到3T/4時(shí)間內(nèi)，右腿由支撐腿變?yōu)閿[動(dòng)腿，按照足端點(diǎn)規(guī)劃軌跡運(yùn)動(dòng)至 最高點(diǎn)，左腿作為支撐腿，重復(fù)步驟￠)?（8)中左腿的計(jì)算過程。
[0112] (10)在3T/4到T時(shí)間內(nèi)，左腿作為支撐腿重復(fù)步驟（6)?（8)，右腿按照 規(guī)劃軌跡從最高點(diǎn)落足，同時(shí)以ΙΚΗζ的頻率實(shí)時(shí)檢測右腿足端接觸力是否滿足條件 ΙΙ^,,Ι-?s/OOd > 10iV，如果不滿足，返回步驟(10)，如果滿足，進(jìn)入步驟（11)。
[0113] (11)在3T4到T時(shí)間內(nèi)，滿足條件|',,|| = 4F^+Kr+Fl 況的情況下，左腿作 為支撐腿重復(fù)步驟￠)?（8)，右腿由擺動(dòng)腿變?yōu)橹瓮?，重?fù)步驟（2)?（4)。
[0114] (12)返回步驟（1)，進(jìn)入下一個(gè)步行周期。
[0115] 以上僅是本發(fā)明的優(yōu)選實(shí)施方式，本發(fā)明的保護(hù)范圍并不僅局限于上述實(shí)施例， 凡屬于本發(fā)明思路下的技術(shù)方案均屬于本發(fā)明的保護(hù)范圍。應(yīng)當(dāng)指出，對于本【技術(shù)領(lǐng)域】的 普通技術(shù)人員來說，在不脫離本發(fā)明原理前提下的若干改進(jìn)和潤飾，應(yīng)視為本發(fā)明的保護(hù) 范圍。
【權(quán)利要求】
1. 一種輪足式機(jī)器人的柔順控制方法，其特征在于，包括： 外環(huán)控制，為機(jī)器人本體位姿控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的 位姿軌跡； 中環(huán)控制，為足端接觸力控制，控制目標(biāo)是使機(jī)器人本體的位姿準(zhǔn)確跟蹤期望的位姿 軌跡； 內(nèi)環(huán)控制，為主動(dòng)關(guān)節(jié)驅(qū)動(dòng)力矩控制，控制目標(biāo)是實(shí)現(xiàn)支撐腿足端接觸力跟蹤外環(huán)計(jì) 算得到的期望力； 通過上述三個(gè)控制，在準(zhǔn)確控制機(jī)器人本體位姿的同時(shí)，對支撐腿足端接觸力進(jìn)行控 制。
2. 根據(jù)權(quán)利要求1所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述外環(huán)控制 中，首先建立基于牛頓-歐拉方程的包含非完整約束條件的機(jī)器人本體動(dòng)力學(xué)模型，將通 過慣性傳感器得到的機(jī)器人本體位姿數(shù)據(jù)作為反饋量，與期望位姿軌跡比較；然后，基于動(dòng) 力學(xué)模型設(shè)計(jì)PID控制器，計(jì)算控制量，即支撐腿足端期望接觸力；最后，對接觸面切向力 進(jìn)行限幅，使之滿足摩擦錐約束。
3. 根據(jù)權(quán)利要求2所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述基于牛 頓-歐拉方程的機(jī)器人本體動(dòng)力學(xué)模型方程如下： Μχ β =A Fer (1) >」_ y = R+L〇sin β (2) Y = 0 (3) ,vsina + icos? =0 (4) 其中，Mi為慣性矩陣，A為力臂矩陣，x，y，z為機(jī)器人本體位置，x軸方向?yàn)闄C(jī)器人本體 前進(jìn)方向，y軸方向與重力方向相反，z軸方向由右手法則確定；Fex，F(xiàn)ey，F(xiàn)ez為支撐腿足端接 觸力，R為后輪半徑，U為機(jī)器人本體質(zhì)心與輪軸之間的距離，α，β，γ分別為機(jī)器人本體 的方位角、俯仰角和滾動(dòng)角；公式（1)表示機(jī)器人本體的動(dòng)力學(xué)方程，公式（2)和公式（3) 表示完整約束條件，公式（4)表示非完整約束條件。
4. 根據(jù)權(quán)利要求3所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述外環(huán)控制 采用PID控制器作為外環(huán)控制器，具體形式如下： 「 ? Kpx {x(l-x) + Kdx (i(/ - i：) + AV, ￡ (.v(/ - x)dr ^ ,,./ = A KP/I {β,ι ~ β) + {β,? ~ β) + ^7/; Jn (A ~βΥτ p' L '」'」 卜凡(?-a) + /? (?" - dr) + 人7: ￡ (a" - ? r 其中，F(xiàn)'《d，F(xiàn)' _，F(xiàn)' ezd為期望的足端接觸力，xd為期望的x方向位置，β d為期望的俯 仰角，a d為期望的方位角；加入摩擦錐約束條件：F' exd〈 μ F' eyd、F' ezd〈 μ F' _，μ為摩擦系 數(shù)，控制器變?yōu)椋? F = F， 丄eyd 丄 eyd P _ f exc^ F ex(i〈 F exd-υο?χ,F w―瓜怒-0.8"/^咖 exd ~ \ p' pr' :> /7. p - 〇 R//F y exd_max exd _ ext/_max > exr^_max ,V^A* X eyd pi _ I ^ ezd F ezd〈 F ezcf -max，F(xiàn) ezd - max - eyif (" I [7 Γ7 > [7 [7 _ Λ Qi?f7 L ezd_ max ezd _ ezd max5 ezd_ max _ eyd 通過調(diào)節(jié)參數(shù)Kpx、Kdx、kix、Kpe、Kd e、kie、Kpa、Kda、kia，保證在控制本體位姿的同 時(shí)，避免機(jī)器人在行走和轉(zhuǎn)彎的過程中出現(xiàn)支撐腿足端滑動(dòng)的現(xiàn)象。
5. 根據(jù)權(quán)利要求1?4中任意一項(xiàng)所述的輪足式機(jī)器人的柔順控制方法，其特征在于， 所述中環(huán)控制中，首先基于支撐腿和擺動(dòng)腿解耦控制的思想建立支撐腿的運(yùn)動(dòng)學(xué)模型和基 于拉格朗日方程的動(dòng)力學(xué)模型，通過足端力傳感器反饋?zhàn)愣私佑|力信息，計(jì)算支撐腿的各 個(gè)主動(dòng)關(guān)節(jié)的輸出力矩；中環(huán)控制采用基于ro算法的力/位混合控制器。
6. 根據(jù)權(quán)利要求5所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述支撐腿的 運(yùn)動(dòng)學(xué)模型如下： 在支撐腿髖部建立坐標(biāo)系，各坐標(biāo)軸方向與機(jī)器人本體坐標(biāo)系方向相同，在髖部坐標(biāo) 系下，足端點(diǎn)的坐標(biāo)為： Xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3C0S ( θ 3_ ) -L2C0S ( θ 2_ ? 3~*~ ^ ) ye = -cos θ 4 (Lpin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) (5) ze = -sin θ 4 (Lpin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) 其中，（Xe，L %)為足端點(diǎn)坐標(biāo)，Lp L2, L3分別為腳掌、小腿、大腿的長度， θ2, θ3, 04分別為踝關(guān)節(jié)角、膝關(guān)節(jié)角、髖關(guān)節(jié)角、髖部側(cè)向關(guān)節(jié)角； 令 0 = [% θ2, Θ 3，04]T，q= [xe，ye，zJT，泛= f'，則々 = /6，| = J6， 其中J為支撐腿雅克比矩陣，J為增廣雅克比矩陣： cxjde^ Dxjde, cxjc6. cxjde^ ?ν,,/?^ 〇ν:,/δθ2 cyjde, cyJcOA (6) czjde, czJ〇e2 czjde, 6zjd04_ cx JcG^ d.\\ /c0·, cx JcG^ av/c6*4 j= 〇yJ〇^ cyJc〇2 cyjee, czJcO^ dzjc02 czjde, czJoO, 1-10 0 基于拉格朗日方程的支撐腿動(dòng)力學(xué)方程為： Μ (θ)θ + C(θ,θ)θ + Ν(θ,θ) = τ + J' F, (8) 其中，Μ(θ)為慣性矩陣，為哥式力矩陣，久巧為重力項(xiàng)，匕為足端接觸力向 量，將| = ?76代入公式（8)，得到： M {q)q + C(q,q)q + N(q,q) - τ + JTFt 其中，Λ；/⑷=M ⑷(J)-、。 v )
7. 根據(jù)權(quán)利要求6所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述中環(huán)控制 的控制器如下： ^ [[A(4-6'.) +叫之)_ τ = Μ 〇 +0j + N-JTKP2(Feyd-Fey) + Kd1(Peytl-Pe})。 Kp, (0-^+0,) + Κ?Λ [?-θ,+θ2) [_ ΚΡ.^ (f，j ~FJ + Kd, (t.,,· ~t)_
8. 根據(jù)權(quán)利要求5所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述內(nèi)環(huán)控制 是利用關(guān)節(jié)位移傳感器和力傳感器實(shí)時(shí)檢測作動(dòng)器長度信息和關(guān)節(jié)力信息，根據(jù)運(yùn)動(dòng)學(xué)模 型計(jì)算關(guān)節(jié)力臂，解算出關(guān)節(jié)期望力，與實(shí)際的關(guān)節(jié)力進(jìn)行比較；再利用PI控制器，生成關(guān) 節(jié)作動(dòng)器的控制電流，實(shí)現(xiàn)關(guān)節(jié)力準(zhǔn)確跟蹤期望力，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū)動(dòng)力矩對期望力矩的 跟蹤。
9. 根據(jù)權(quán)利要求8所述的輪足式機(jī)器人的柔順控制方法，其特征在于，所述內(nèi)環(huán)控制 的PI控制器如下： i = KP, (./；/ -/) + Κ?, ￡ (.A/ - f ￥^ 生成關(guān)節(jié)作動(dòng)器的控制電流i，實(shí)現(xiàn)關(guān)節(jié)力f準(zhǔn)確跟蹤期望力fd，進(jìn)而實(shí)現(xiàn)關(guān)節(jié)驅(qū)動(dòng)力 矩對期望力矩的跟蹤。
【文檔編號】B62D57/028GK104108433SQ201410306901
【公開日】2014年10月22日 申請日期:2014年6月30日 優(yōu)先權(quán)日:2014年6月30日
【發(fā)明者】王劍, 馬宏緒, 郎琳, 韋慶, 王建文, 陳陽禎, 安宏雷, 侯文琦, 朱開盈, 饒錦輝 申請人:中國人民解放軍國防科學(xué)技術(shù)大學(xué)