雙臂護(hù)理機(jī)器人抱舉重心穩(wěn)定性研究

夏建強(qiáng) 苗新剛,2 許會(huì)超

（1安徽恒利增材制造科技有限公司,安徽蕪湖，241200; 2北京建筑大學(xué)，北京市建筑安全監(jiān)測工程技術(shù)研究中心， 北京，100044; 3 北京電子工程總體研究所，北京，100854）

0 引言

護(hù)理機(jī)器人輔助醫(yī)護(hù)人員對病人進(jìn)行康復(fù)護(hù)理，將醫(yī)護(hù)人員從繁重的體力勞動(dòng)中解放出來，己經(jīng)成為康復(fù)護(hù)理領(lǐng)域研究的熱點(diǎn)問題[1]。護(hù)理機(jī)器人在進(jìn)行護(hù)理任務(wù)、輔助醫(yī)護(hù)人員對病人進(jìn)行康復(fù)護(hù)理、完成對病人的平穩(wěn)轉(zhuǎn)移時(shí)，整個(gè)運(yùn)動(dòng)過程中的運(yùn)動(dòng)穩(wěn)定性是最令人關(guān)心的問題，直接關(guān)系到護(hù)理任務(wù)的成敗[2-3]。

對于護(hù)理機(jī)器人來說，其傾翻的危害遠(yuǎn)比側(cè)滑嚴(yán)重，在抱起病人過程中如果發(fā)生傾翻，那么對病人二次傷害的后果是無法想象的[4]。

仿人機(jī)器人與傳統(tǒng)的輪式機(jī)器人相比，其穩(wěn)定性更值得研究，仿人機(jī)器人穩(wěn)定性的判斷依據(jù)有很多種類型，其中最主要的是 ZMP、FRI、COP等[5]。這幾種判斷依據(jù)都有一定的使用限制范圍，迄今為止，還沒有一種判斷依據(jù)可以完全取代其他類型[6]。

穩(wěn)定的支撐是獲得理想運(yùn)動(dòng)的重要條件，穩(wěn)定區(qū)域即機(jī)器人各輪與地面的接觸點(diǎn)構(gòu)成的凸多邊形在水平方向上的投影，當(dāng)仿人護(hù)理機(jī)器人的雙臂、腰部、軀干及頭部在一定的位姿狀態(tài)下，其整體重心、在水平方向的投影位于穩(wěn)定區(qū)域內(nèi)時(shí)，機(jī)器人即處于穩(wěn)定狀態(tài)。

采用零力矩點(diǎn)ZMP動(dòng)態(tài)穩(wěn)定判據(jù)，是本文解決重心穩(wěn)定性的重要理論依據(jù)，本文以此為基礎(chǔ)對機(jī)器人運(yùn)動(dòng)進(jìn)行分析。

1 護(hù)理機(jī)器人結(jié)構(gòu)

本文設(shè)計(jì)的護(hù)理機(jī)器人由底盤框架、腰部、軀干、雙臂和簡單手掌部分組成，其結(jié)合了輪式機(jī)器人和人形機(jī)器人的各自優(yōu)勢特點(diǎn)，靈活便于控制，穩(wěn)定性良好，既増加護(hù)理機(jī)器人的靈活使用性，又拓展了其工作范圍。護(hù)理機(jī)器人機(jī)械結(jié)構(gòu)如圖1所示。

圖1 護(hù)理機(jī)器人結(jié)構(gòu)

護(hù)理機(jī)器人涵蓋了智能輪椅和護(hù)理床的優(yōu)點(diǎn)，既能輔助醫(yī)護(hù)人員轉(zhuǎn)移病人，又能滿足家庭和醫(yī)院日常護(hù)理中的多樣化需求，其雙臂與移動(dòng)基座的配合，極大地拓展了護(hù)理機(jī)器人的操作范圍。

2 重心穩(wěn)定性分析

護(hù)理機(jī)器人按照己規(guī)劃好的軌跡路徑抱舉病人時(shí)，通過各關(guān)節(jié)協(xié)調(diào)配合來保證底盤、軀干和雙臂運(yùn)動(dòng)過程中整個(gè)系統(tǒng)的ZMP點(diǎn)，位于輪子與地面接觸點(diǎn)形成的支撐多邊形（穩(wěn)定區(qū)域）內(nèi)，護(hù)理機(jī)器人處于動(dòng)態(tài)穩(wěn)定。

護(hù)理機(jī)器人的4個(gè)全向輪與地面的接觸點(diǎn)圍成一個(gè)矩形，以邊平行于Z軸和Y軸的矩形中心為原點(diǎn)，原點(diǎn)到矩形輪廓邊界的最小距離r0為半徑的圓，此區(qū)域本文定義為有效穩(wěn)定安全區(qū)域，如圖2所示。

圖2 機(jī)器人的有效穩(wěn)定安全區(qū)域

若 上述ZMP點(diǎn)處于有效穩(wěn)定安全區(qū)域外，則無法保證護(hù)理機(jī)器人是否處于穩(wěn)定狀態(tài)，此時(shí)需要調(diào)整各構(gòu)件的運(yùn)動(dòng)參數(shù)，使系統(tǒng)的ZMP點(diǎn)調(diào)整到穩(wěn)定有效區(qū)域內(nèi)，具體判定公式為：

在SolidWorks中，設(shè)置模型的真實(shí)材料和質(zhì)量，可以從系統(tǒng)中測量出護(hù)理機(jī)器人各個(gè)部位的重心參數(shù)，如表1所示。

表1 護(hù)理機(jī)器人重心參數(shù)表

根據(jù)機(jī)器人各部分的質(zhì)心位置和質(zhì)量參數(shù)，可以計(jì)算護(hù)理機(jī)器人在抱取病人時(shí)的不穩(wěn)定邊界的位置（等效質(zhì)心位置），見式（2）：

在式（2）中，mc為負(fù)荷質(zhì)量。當(dāng)mc為40kg時(shí)，Dmax=330mm。mc取不同值時(shí)，傾倒邊界位置Dmax的變化如圖3所示。

圖3 傾倒邊界隨負(fù)荷質(zhì)量變化曲線

從圖3可以看出，當(dāng)護(hù)理機(jī)器人無抱取任務(wù)時(shí)，傾倒邊界距離穩(wěn)定區(qū)域最遠(yuǎn)，發(fā)生傾倒的可能性最小；隨著抱取負(fù)荷質(zhì)量的增加，傾倒邊界距離穩(wěn)定區(qū)域越來越近，穩(wěn)定區(qū)域越來越小，此時(shí)越容易發(fā)生傾倒。

由ZMP得到關(guān)于YOZ平面上任一點(diǎn)Pzmp的力矩平衡式見公式（3）。

以穩(wěn)定區(qū)域中心建立坐標(biāo)系，ZMP點(diǎn)Pzmp為（yzmp，zzmp），式（3）中ri為質(zhì)心位置矢量，ΣT為受到合外力矩，g為重力加速度。

由于護(hù)理機(jī)器人軀干在整個(gè)護(hù)理過程中運(yùn)動(dòng)緩慢，考慮忽略其慣性力/力矩，由此可得到護(hù)理機(jī)器人ZMP點(diǎn)簡化計(jì)算公式（4）：

式（4）中，fx1為部件作用在底盤上沿x軸方向的合外力（包含慣性力），nz1為部件作用在底盤上繞z軸的合外力矩（包含慣性力矩），ny1為部件作用在底盤上繞y軸的合外力矩（包含慣性力矩）。

當(dāng)機(jī)器人抱取位置為最遠(yuǎn)距離，且雙臂一致前伸時(shí)，由此可以得到機(jī)器人ZMP點(diǎn)為最不穩(wěn)定的點(diǎn)，計(jì)算得到rzmp（rmax）的值為357mm。

當(dāng)rzmp≤r0=272mm時(shí)，點(diǎn)Pzmp在主穩(wěn)定區(qū)域，為了定量分析不同區(qū)域穩(wěn)定性程度的大小，設(shè)定在主穩(wěn)定區(qū)域的點(diǎn)的穩(wěn)定度都為1，次穩(wěn)定區(qū)域的點(diǎn)的穩(wěn)定度根據(jù)rzmp的r0的差值乘以一定的系數(shù)得到。

總之，ZMP點(diǎn)越接近主穩(wěn)定區(qū)域，則護(hù)理機(jī)器人的穩(wěn)定系數(shù)越高。通過以上分析，筆者把重心穩(wěn)定性判定的理論應(yīng)用于優(yōu)化軌跡規(guī)劃，并作為運(yùn)動(dòng)規(guī)劃的參考標(biāo)準(zhǔn)。

3 安全路徑的運(yùn)動(dòng)控制分析

由于護(hù)理機(jī)器人在運(yùn)動(dòng)過程中，護(hù)理對象的重心基本在其小臂的中間位置，所以其雙臂的軌跡規(guī)劃問題就轉(zhuǎn)換為中間點(diǎn)的軌跡規(guī)劃。

本文研究的目的就是要提高機(jī)器人運(yùn)動(dòng)過程中的安全性，安全性是由系統(tǒng)重心穩(wěn)定度決定的，由機(jī)器人的動(dòng)力學(xué)方程可知，系統(tǒng)的穩(wěn)定度是機(jī)器人關(guān)節(jié)運(yùn)動(dòng)的函數(shù)，通過逆解算法可以規(guī)劃機(jī)器人關(guān)節(jié)運(yùn)動(dòng)，提高機(jī)器人運(yùn)動(dòng)的安全性。

軌跡規(guī)劃是從當(dāng)前位置到目標(biāo)位置的路徑形狀，在預(yù)定任務(wù)進(jìn)行之前，需設(shè)定機(jī)器人的當(dāng)前位姿，因?yàn)橐?guī)劃的是雙臂末端的中間點(diǎn)，所以需要從護(hù)理機(jī)器人雙臂位姿確定當(dāng)前位姿和目標(biāo)位姿。

該中間點(diǎn)的當(dāng)前位姿為：

式（6）的矩陣作為路徑規(guī)劃的起始位置，由護(hù)理機(jī)器人左臂和右臂的末端位姿矩陣得到。

正向抱取運(yùn)動(dòng)是護(hù)理機(jī)器人最簡單的運(yùn)動(dòng)形式，主要是向前伸出雙臂來接近平躺的護(hù)理對象。因?yàn)橐?guī)劃的是雙臂末端的中間點(diǎn)，所以需要從雙臂位姿確定當(dāng)前位姿和目標(biāo)位姿。

該中間點(diǎn)的目標(biāo)位姿為：

最優(yōu)穩(wěn)定性軌跡規(guī)劃的規(guī)劃目標(biāo)是平均穩(wěn)定度St，所以以St為目標(biāo)優(yōu)化函數(shù)，利用基本遺傳算法求得路徑點(diǎn)。

設(shè)n=20，t=20s，由MATLAB仿真中的二階拋物線函數(shù)擬合，路徑點(diǎn)的擬合曲線如圖4所示。

圖4 最優(yōu)穩(wěn)定性路徑點(diǎn)軌跡圖

從圖4中可以看出，最優(yōu)路徑基本趨于一條直線，而且在Z軸的偏差很小，此路徑形狀可以作為理論最優(yōu)規(guī)劃路徑。

根據(jù)護(hù)理機(jī)器人始末位置的位姿，可以選擇常用的兩種路徑形狀規(guī)劃，即分別用直線和圓弧去規(guī)劃中間點(diǎn)的當(dāng)前位置到目標(biāo)位置的運(yùn)動(dòng)，如圖5所示。

圖5 護(hù)理機(jī)器人抱舉路徑形狀圖

筆者根據(jù)結(jié)構(gòu)參數(shù)、逆運(yùn)動(dòng)學(xué)解、路徑形狀以及平均穩(wěn)定度的數(shù)學(xué)模型公式，求得：

圓弧軌跡的平均穩(wěn)定度Stc=0.021 ；

直線軌跡Stl=0.047。

可以看出，直線路徑形狀的平均穩(wěn)定度大于圓弧路徑形狀的平均穩(wěn)定度，也就是說，直線軌跡的安全性優(yōu)于圓弧的安全性。

至此，直線路徑形狀跟最優(yōu)規(guī)劃路徑基本一致。

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

本文采用輪輻式壓力稱重傳感器用于測量4個(gè)全向輪與地面的壓力，從而間接測量穩(wěn)定程度。系統(tǒng)通過一定的頻率采集數(shù)據(jù)，并且把數(shù)據(jù)以Excel格式存入計(jì)算機(jī)，分析數(shù)據(jù)時(shí)可以直接從計(jì)算機(jī)中導(dǎo)出。

為了證明本設(shè)計(jì)的護(hù)理機(jī)器人的重心穩(wěn)定性，筆者在護(hù)理機(jī)器人雙臂各負(fù)重20kg的砝碼，即總負(fù)重為40kg進(jìn)行測試。

MATLAB的優(yōu)化結(jié)果和ZMP的計(jì)算結(jié)果證明：所規(guī)劃的運(yùn)動(dòng)能夠?qū)崿F(xiàn)仿人機(jī)器人的重心最優(yōu)穩(wěn)定性。

根據(jù)COP的定義，筆者在護(hù)理機(jī)器人的腳底安裝4個(gè)壓力傳感器。利用物理樣機(jī)，在護(hù)理機(jī)器人的實(shí)驗(yàn)平臺(tái)上進(jìn)行抱取動(dòng)作的實(shí)驗(yàn)，并利用重力測試傳感系統(tǒng)采集運(yùn)動(dòng)過程中4個(gè)全向輪對地面的壓力變化。

4個(gè)壓力傳感器上表面與全向輪接觸，下表面直接和地面接觸，所測量的壓力值也就是輪子對地面的壓力。采集的數(shù)據(jù)導(dǎo)出后，然后等間距取出10個(gè)數(shù)據(jù)點(diǎn)，通過COP計(jì)算公式得到穩(wěn)定性判定的數(shù)值，然后再利用MATLAB擬合出變化曲線。

在式（8）中，F(xiàn)R1，F(xiàn)R2，F(xiàn)R3和FR4為壓力傳感器測得的壓力值，l1，l2，l3和l4為壓力傳感器與平臺(tái)中心位置的距離。

針對前面提到的兩種運(yùn)動(dòng)任務(wù)，本文采取正向抱取和側(cè)向抱取，并通過物理樣機(jī)進(jìn)行驗(yàn)證。

重力測量系統(tǒng)的數(shù)據(jù)采集頻率是1Hz，實(shí)驗(yàn)的時(shí)間為20s，從系統(tǒng)軟件里導(dǎo)出數(shù)據(jù)格式為Excel數(shù)據(jù)，分別等間隔抽取4個(gè)傳感器采集的10個(gè)數(shù)據(jù)，列表如表2所示。

表2 正向運(yùn)動(dòng)傳感器壓力值數(shù)據(jù)采集表

根據(jù)表2中數(shù)據(jù)，通過COP計(jì)算公式得到穩(wěn)定性判定的數(shù)值與中間點(diǎn)Y軸位置坐標(biāo)的變化數(shù)據(jù)，然后用MATLAB中的二階拋物線函數(shù)擬合，曲線如圖6所示。

圖6 COP參數(shù)值與中間點(diǎn)Y軸位置的變化

5 結(jié)論

本文對護(hù)理機(jī)器人在執(zhí)行“抱起病人”這一典型護(hù)理任務(wù)過程中的穩(wěn)定性做了深入研究，分析了重心對護(hù)理機(jī)器人的運(yùn)動(dòng)穩(wěn)定性的影響；主要考慮護(hù)理機(jī)器人運(yùn)動(dòng)過程中重力對穩(wěn)定性的影響，采用零力矩點(diǎn)ZMP動(dòng)態(tài)穩(wěn)定判據(jù)，從而盡可能地得到符合實(shí)際情況的結(jié)果；在滿足ZMP約束的情況下進(jìn)行機(jī)器人軌跡規(guī)劃，使護(hù)理機(jī)器人在兩工作點(diǎn)之間或沿規(guī)定軌跡運(yùn)動(dòng)的安全最優(yōu)。分析結(jié)果表明，該規(guī)劃算法能夠顯著提高護(hù)理機(jī)器人的工作安全性，對實(shí)際護(hù)理機(jī)器人的軌跡規(guī)劃具有一定的理論指導(dǎo)意義。

實(shí)驗(yàn)通過位置控制驅(qū)動(dòng)護(hù)理機(jī)器人按照最優(yōu)軌跡規(guī)劃的路徑進(jìn)行運(yùn)動(dòng)，同時(shí)應(yīng)用重力測試系統(tǒng)采集數(shù)據(jù)，最后根據(jù)采集的數(shù)據(jù)對實(shí)驗(yàn)結(jié)果進(jìn)行分析，驗(yàn)證了本規(guī)劃的合理性。