基于三次貝塞爾曲線的軌跡規(guī)劃方法

廈門大學(xué)航空航天學(xué)院 江浩 彭俠夫

四足機(jī)器人在復(fù)雜環(huán)境下既能夠?qū)崿F(xiàn)靜態(tài)行走，又能夠以動(dòng)態(tài)方式高速運(yùn)動(dòng)，具有較高的研究?jī)r(jià)值。而足端軌跡研究則是四足機(jī)器人運(yùn)動(dòng)的基礎(chǔ)。對(duì)四足機(jī)器人建立了簡(jiǎn)化模型，并對(duì)單腿的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)進(jìn)行建模。然后在運(yùn)動(dòng)學(xué)上根據(jù)彈簧負(fù)載倒立擺模型選取落腳點(diǎn)，軌跡上選取三次貝塞爾曲線進(jìn)行改進(jìn)由彈簧負(fù)載倒立擺模型和三次貝塞爾曲線生成足端軌跡，彈簧負(fù)載倒立擺模型獲取足端落地平衡點(diǎn)保證機(jī)器人支撐相與擺動(dòng)相切換時(shí)的平穩(wěn)。三次貝塞爾曲線結(jié)合地形生成規(guī)避障礙物的足端軌跡，對(duì)曲線生成方式進(jìn)行改進(jìn)使其符合零沖擊步態(tài)的規(guī)劃，在動(dòng)力學(xué)上應(yīng)用拉格朗日方程獲得了驅(qū)動(dòng)力矩與足端軌跡的映射關(guān)系，最后通過(guò)傳感器計(jì)算實(shí)際足端軌跡與規(guī)劃足端軌跡的誤差建立虛擬動(dòng)力學(xué)方程實(shí)現(xiàn)反饋控制，提高了四足機(jī)器人抗干擾能力，降低了單腿高速擺動(dòng)及落地沖擊對(duì)機(jī)身運(yùn)動(dòng)狀態(tài)的影響。

腿足仿生機(jī)器人根據(jù)腿的數(shù)量又分為雙足、四足、六足等多足機(jī)器人。參照哺乳類四足動(dòng)物設(shè)計(jì)的仿生四足機(jī)器人相較于雙足仿生機(jī)器人更穩(wěn)定、靈活、負(fù)載高，相較于六足等多足仿生機(jī)器人結(jié)構(gòu)復(fù)雜度低、易于控制，在復(fù)雜環(huán)境下既能夠?qū)崿F(xiàn)靜態(tài)行走，又同時(shí)能夠以動(dòng)態(tài)方式高速運(yùn)動(dòng)。綜合運(yùn)動(dòng)特性及結(jié)構(gòu)復(fù)雜度，四足機(jī)器人的適應(yīng)性更廣，可研究?jī)r(jià)值更高[1，2]。

1 四足機(jī)器人模型

1.1 四足機(jī)器人簡(jiǎn)化模型

根據(jù)四足機(jī)器人簡(jiǎn)化模型確定世界坐標(biāo)系和機(jī)身坐標(biāo)系。機(jī)身質(zhì)心位置為機(jī)身坐標(biāo)系原點(diǎn)，設(shè)計(jì)足端軌跡時(shí)，足端軌跡坐標(biāo)以初始站立姿態(tài)足端相對(duì)機(jī)身坐標(biāo)系的固定位置為足端軌跡坐標(biāo)原點(diǎn)，整機(jī)的運(yùn)動(dòng)方向?yàn)檎较?。單腿可視作具有三自由度的三連桿，四足機(jī)器人的簡(jiǎn)化模型及坐標(biāo)系定義如圖1所示。

1.2 D-H 法運(yùn)動(dòng)學(xué)方程

對(duì)連桿系統(tǒng)進(jìn)行建模的一種通用方法為D-H 法，在各個(gè)連桿上設(shè)置坐標(biāo)系，根據(jù)連桿的連接結(jié)構(gòu)與空間關(guān)系建立變換矩陣，從連桿一端根據(jù)連桿間的變換矩陣逐步計(jì)算下一連桿一端的空間矩陣，最終計(jì)算出連桿兩端間的空間變換矩陣，即四足機(jī)器人的運(yùn)動(dòng)學(xué)方程。

坐標(biāo)系的變換矩陣T 由坐標(biāo)系旋轉(zhuǎn)變換矩陣R 和坐標(biāo)系位移矩陣P 組成，旋轉(zhuǎn)矩陣R 是指兩坐標(biāo)系原點(diǎn)重合對(duì)應(yīng)的坐標(biāo)系、三軸的方向余弦，位移矩陣P 是變換后的坐標(biāo)系相對(duì)于參考坐標(biāo)系的三個(gè)方向位移。其變換矩陣構(gòu)成如公式（1）所示。

連桿間的坐標(biāo)系變換由連桿偏距di、連桿長(zhǎng)度li、連桿扭轉(zhuǎn)角φi、連桿間旋轉(zhuǎn)角θi四個(gè)運(yùn)動(dòng)學(xué)模型參數(shù)來(lái)描述，連桿長(zhǎng)度和連桿扭轉(zhuǎn)角為連桿自身的物理參數(shù)，連桿偏距和連桿間夾角為連桿連接物理參數(shù)。在四足機(jī)器人單腿模型中，腿部連桿均不存在扭轉(zhuǎn)，由此可以確定單腿從機(jī)身和大腿連接處到足端的坐標(biāo)變換矩陣。各物理參數(shù)如圖1所示，機(jī)身與單腿連接處即臀關(guān)節(jié)為初始坐標(biāo)系，臀、大腿、小腿長(zhǎng)度分別為l1、l2、l3，臀部與機(jī)身關(guān)節(jié)、臀部與大腿關(guān)節(jié)、大腿與小腿關(guān)節(jié)角度分別為θ1、θ2、θ3，D-H 法的參數(shù)表如表1所示。

圖1 四足機(jī)器人簡(jiǎn)化模型Fig.1 Simplified model of quadruped robot

表1 四足機(jī)器人機(jī)身到足端的D-H 參數(shù)Tab.1 D-H parameters from the quadruped robot body to the foot end

根據(jù)D-H 參數(shù)法建立關(guān)節(jié)角度、角速度、角加速度與足端位移、速度、加速度間的映射關(guān)系，為動(dòng)力學(xué)模型計(jì)算提供基礎(chǔ)。

2 足端軌跡規(guī)劃

2.1 虛擬彈簧負(fù)載倒立擺

倒立擺模型最早用于雙足機(jī)器人步態(tài)控制，該理論提出以倒立擺接觸點(diǎn)為核心，根據(jù)落點(diǎn)與平衡位置的關(guān)系來(lái)控制彈簧倒立擺的加速、勻速、減速運(yùn)動(dòng)。若落足點(diǎn)位于平衡點(diǎn)后方則為減速狀態(tài)；若落足點(diǎn)位于平衡點(diǎn)前方則為加速狀態(tài)；若落足點(diǎn)位于平衡點(diǎn)則為勻速運(yùn)動(dòng)狀態(tài)[3]。在四足機(jī)器人運(yùn)動(dòng)過(guò)程中，分為支撐相與擺動(dòng)相，可將擺動(dòng)相的腿虛擬為彈簧負(fù)載倒立擺，根據(jù)機(jī)身狀態(tài)及指令計(jì)算足端軌跡擺動(dòng)相落點(diǎn)控制四足機(jī)器人機(jī)身跟蹤狀態(tài)指令運(yùn)動(dòng)。彈簧負(fù)載倒立擺如圖2所示。

圖2 彈簧負(fù)載倒立擺模型Fig.2 Spring-loaded inverted pendulum model

四足機(jī)器人擺動(dòng)腿虛擬為彈簧負(fù)載倒立擺，其平衡點(diǎn)取決于機(jī)身速度v、擺動(dòng)周期Tsw、擺動(dòng)腿的最大抬腿高度hsw、指令機(jī)身速度vref，此處計(jì)算速度機(jī)身x、y方向平衡點(diǎn)位置如公式（2）所示：

2.2 三次貝塞爾曲線

根據(jù)腿部傳感器可以計(jì)算出四足機(jī)器人支撐相切換為擺動(dòng)相時(shí)腿部足端的位置，結(jié)合彈簧負(fù)載倒立擺模型可計(jì)算得四足機(jī)器人落地平衡點(diǎn)。四足機(jī)器人為適用于復(fù)雜地形設(shè)計(jì)的機(jī)器人，在其結(jié)合三維地圖傳感器或激光雷達(dá)傳感器生成三維地圖時(shí)，四足機(jī)器人擺動(dòng)相軌跡及落點(diǎn)需要靈活根據(jù)地圖調(diào)整，保持機(jī)身平穩(wěn)性。

三次貝塞爾曲線是應(yīng)用于二維圖形的數(shù)學(xué)曲線，通常是用于計(jì)算機(jī)矢量作圖，n次貝塞爾曲線是由n+1 個(gè)點(diǎn)組合而成，曲線可以通過(guò)改變節(jié)點(diǎn)位置生成。在曲線生成過(guò)程中也可以通過(guò)拖動(dòng)節(jié)點(diǎn)實(shí)時(shí)更改未完成的曲線軌跡，應(yīng)用于四足機(jī)器人擺動(dòng)相軌跡的生成，既能夠結(jié)合彈簧負(fù)載倒立擺模型生成軌跡，還能夠在運(yùn)行過(guò)程中根據(jù)三維地圖實(shí)時(shí)調(diào)整軌跡。三次貝塞爾曲線的基本方程如公式（3）所示：

其中P0、P1、P2、P4為軌跡生成的四個(gè)基礎(chǔ)坐標(biāo)，P0為支撐相切換為擺動(dòng)相時(shí)足端所處位置，P4為根據(jù)彈簧負(fù)載倒立擺生成的落足點(diǎn)位置，通過(guò)調(diào)整P1、P2的位置可以控制足端擺動(dòng)的高度等參數(shù)，以此來(lái)實(shí)現(xiàn)規(guī)避障礙物等功能。在原方程中t為每一段中點(diǎn)位置位于當(dāng)前線段中的比例，若直接將其作為擺動(dòng)周期時(shí)間的等比例縮放，在求導(dǎo)后獲取擺動(dòng)相軌跡的速度時(shí)，其速度初始不為零，此處將原式中的t替換為與時(shí)間有關(guān)的比例公式，則方程如公式（4）所示：

此處時(shí)間t為單腿從支撐相切換到擺動(dòng)相開(kāi)始到擺動(dòng)相結(jié)束。四足機(jī)器人關(guān)節(jié)控制方式為力控制，因此加速度可突變，不考慮初始加速度為零的情況，僅考慮求導(dǎo)后兩端點(diǎn)值均為零，且該函數(shù)為從0 到1 為遞增函數(shù)，本文采用的比例函數(shù)如公式（5）所示：

將替換后的三次貝塞爾曲線對(duì)時(shí)間求導(dǎo)可以得到足端軌跡速度、加速度。如公式（6）、公式（7）所示：

由此獲得初始速度和終點(diǎn)速度均為零的三次貝塞爾曲線方程及速度、加速度方程。該方程用于規(guī)劃擺動(dòng)相的足端軌跡，獲取擺動(dòng)腿末端的運(yùn)動(dòng)狀態(tài)，結(jié)合運(yùn)動(dòng)學(xué)方程的逆解可得到關(guān)節(jié)轉(zhuǎn)角所需的角度、角速度、角加速度作為關(guān)節(jié)控制的目標(biāo)量，然后通過(guò)單腿動(dòng)力學(xué)計(jì)算電機(jī)的按照規(guī)劃的三次貝塞爾曲線運(yùn)動(dòng)所需的力矩。

3 實(shí)驗(yàn)

3.1 ADAMS 與Simulink 聯(lián)合仿真

四足機(jī)器人為強(qiáng)非線性、強(qiáng)時(shí)變系統(tǒng)，該系統(tǒng)用力控制方法在地面行走時(shí)具有不確定性，四足機(jī)器人在行走中參數(shù)不斷變化，與地面交互接觸面小，系統(tǒng)必須能夠準(zhǔn)確及時(shí)的對(duì)四足機(jī)器人的四條腿、機(jī)身的所有狀態(tài)共49 個(gè)狀態(tài)量進(jìn)行反饋并實(shí)現(xiàn)閉環(huán)控制。在進(jìn)行實(shí)物樣機(jī)運(yùn)行前，必須由計(jì)算機(jī)仿真調(diào)整控制參數(shù)使得近似的建模模型具有理想的控制效果。建立整機(jī)模型的程序框圖如圖3所示。

圖3 單腿擺動(dòng)相控制框圖Fig.3 Block diagram of single-leg swing phase control

目前常用的機(jī)器人力學(xué)控制方法是由Simulink 控制的數(shù)學(xué)模型和ADAMS 導(dǎo)入的四足機(jī)器人建模模型生成的動(dòng)力學(xué)模型聯(lián)合仿真驗(yàn)證效果。整體思路為：Simulink 中建立控制器中的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、模型預(yù)測(cè)控制等數(shù)學(xué)模型，搭建能夠?qū)崿F(xiàn)對(duì)四足機(jī)器人狀態(tài)數(shù)據(jù)即規(guī)劃軌跡完成力控輸出的整體控制框架。SoildWorks 建立物理樣機(jī)的3D 模型，將軟件生成的3D 模型導(dǎo)入ADAMS，建立連接關(guān)系和驅(qū)動(dòng)器，設(shè)置輸入和輸出量，構(gòu)建控制系統(tǒng)。由Simulink 的模型于ADAMS 的物理樣機(jī)動(dòng)力學(xué)模型數(shù)據(jù)交互實(shí)現(xiàn)仿真控制。

軌跡生成器主要功能為接受上層算法或遙控的控制指令，其指令包括運(yùn)動(dòng)方向、旋轉(zhuǎn)角、速度、預(yù)設(shè)步態(tài)類別。該運(yùn)動(dòng)方向指四足機(jī)器人不改變機(jī)體朝向，僅改變整機(jī)的運(yùn)動(dòng)方向，主要用于實(shí)現(xiàn)側(cè)向行走。旋轉(zhuǎn)角指機(jī)身朝向，但不影響四足機(jī)器人的運(yùn)動(dòng)方向，可以實(shí)現(xiàn)機(jī)身直線運(yùn)動(dòng)過(guò)程中的機(jī)身旋轉(zhuǎn)，完成四足機(jī)器人作為移動(dòng)平臺(tái)需要完成的其他任務(wù)。速度僅指前進(jìn)方向，通過(guò)運(yùn)動(dòng)方向分解為世界坐標(biāo)系下x與y軸方向的運(yùn)動(dòng)速度。步擺動(dòng)相控制器主要功能是接收到平衡控制器發(fā)送的腿部擺動(dòng)指令、擺動(dòng)腿的位置關(guān)系、四足機(jī)器人腿部編碼器反饋的腿部狀態(tài)數(shù)據(jù)。在擺動(dòng)相控制器接受四足機(jī)器人腿部狀態(tài)數(shù)據(jù)，該數(shù)據(jù)包括關(guān)節(jié)角度、角速度、角加速度，同時(shí)四足機(jī)器人軌跡中的速度，包括目標(biāo)速度與實(shí)際速度，機(jī)身軌跡速度用于落足點(diǎn)計(jì)算。由于該方法用于優(yōu)化負(fù)載控制，未考慮環(huán)境變化和地形識(shí)別方法，因此軌跡控制參數(shù)固定，并內(nèi)置到擺動(dòng)相控制器中，根據(jù)抬腿高度自動(dòng)生成軌跡。然后將生成的軌跡包括足端位置、速度、加速度和關(guān)節(jié)傳感器反饋的角度、角速度、角加速度帶入動(dòng)力學(xué)方程得出目標(biāo)軌跡的力矩。再將傳感器反饋的腿部關(guān)節(jié)角度、角速度、角加速度由運(yùn)動(dòng)學(xué)正解計(jì)算出四足機(jī)器人實(shí)際的足端狀態(tài)即足端實(shí)際的位置、速度、加速度，根據(jù)列出的控制參數(shù)調(diào)整的動(dòng)力學(xué)微分方程，計(jì)算反饋的足底力，再將反饋計(jì)算的足底力通過(guò)傳感器反饋的關(guān)節(jié)角度映射為關(guān)節(jié)力矩作為反饋調(diào)節(jié)量。擺動(dòng)相控制器輸出的為目標(biāo)軌跡的控制力矩與足端動(dòng)力學(xué)微分方程計(jì)算的反饋力矩的求和。

3.2 單腿實(shí)驗(yàn)仿真

單腿實(shí)驗(yàn)主要驗(yàn)證擺動(dòng)相控制器的力控制跟蹤效果。驗(yàn)證內(nèi)容為軌跡生成器生成的軌跡狀態(tài)即位置、速度、加速度是否符合設(shè)計(jì)需求；動(dòng)力學(xué)模型開(kāi)環(huán)控制是否有效；動(dòng)力學(xué)微分方程進(jìn)行的驅(qū)動(dòng)器力補(bǔ)償是否能夠滿足軌跡跟蹤。

足端軌跡初速度與末速度均為0，在力控制條件下，加速度屬于可突變項(xiàng)，因此無(wú)需考慮其初始和末端加速度不為0 的情況。足端軌跡狀態(tài)量如圖4所示。

圖4 三次貝塞爾曲線足底軌跡狀態(tài)量Fig.4 Cubic Bezier curve plantar trajectory state quantity

3.3 整機(jī)控制實(shí)驗(yàn)

整機(jī)控制實(shí)驗(yàn)首先要驗(yàn)證平衡控制器的支撐力優(yōu)化效果，然后驗(yàn)證該四足機(jī)器人實(shí)現(xiàn)Trot 基本步態(tài)。四足機(jī)器人仿真模型如圖5所示。

圖5 ADAMS 四足機(jī)器人模型Fig.5 ADAMS quadruped robot model

本文支撐相方法采用靜力學(xué)分配的方式，通過(guò)速度增加橫向足底力以此保證支撐相的足端軌跡。結(jié)合本文優(yōu)化后的三次貝塞爾曲線軌跡的力控解算方法可獲得單腿關(guān)節(jié)力矩。實(shí)際的足端軌跡及與本文方法生成的規(guī)劃軌跡如圖6所示。

圖6 規(guī)劃與實(shí)際的足端軌跡對(duì)比圖Fig.6 Comparison of planned and actual foot trajectories

四足機(jī)器人應(yīng)用本文控制方法在Tort 步態(tài)下的機(jī)身狀態(tài)，四足機(jī)器人狀態(tài)變化規(guī)律，沒(méi)有產(chǎn)生突變，證明該方法能夠有效避免擺動(dòng)相落地沖擊及擺動(dòng)時(shí)的動(dòng)能影響。

4 結(jié)語(yǔ)

通過(guò)機(jī)身狀態(tài)計(jì)算出落地平衡點(diǎn)，結(jié)合地形信息計(jì)算中間控制點(diǎn)，可以生成具有地圖適應(yīng)性的零沖擊足端軌跡，降低落地時(shí)對(duì)機(jī)身狀態(tài)產(chǎn)生的影響。同時(shí)對(duì)三次貝塞爾曲線進(jìn)行了優(yōu)化，降低了擺動(dòng)腿自身在運(yùn)動(dòng)過(guò)程中擺動(dòng)對(duì)機(jī)身造成的影響，使其能夠相互抵消，機(jī)身狀態(tài)規(guī)律變化，實(shí)現(xiàn)動(dòng)態(tài)平穩(wěn)。