本發(fā)明涉及足式移動(dòng)機(jī)器人運(yùn)動(dòng)情況的標(biāo)定方法，屬于機(jī)器人技術(shù)領(lǐng)域，具體地，涉及一種基于機(jī)身重心位移校正的六足機(jī)器人關(guān)節(jié)角度標(biāo)定方法。

背景技術(shù)：

相較于輪式移動(dòng)機(jī)器人，足式移動(dòng)機(jī)器人因其具有靈活多變的運(yùn)動(dòng)模式，故擁有較強(qiáng)的越障能力，在非結(jié)構(gòu)化環(huán)境中具備更好的運(yùn)動(dòng)功能魯棒性。作為足式移動(dòng)機(jī)器人的典型代表，六足移動(dòng)機(jī)器人正逐漸成為足式移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn)。

由于六足移動(dòng)機(jī)器人多屬于混聯(lián)機(jī)構(gòu)，運(yùn)動(dòng)冗余關(guān)節(jié)較多，因此其運(yùn)動(dòng)控制的準(zhǔn)確性及效率難以保證，運(yùn)動(dòng)學(xué)問題也就成為了六足移動(dòng)機(jī)器人控制理論研究領(lǐng)域的主要熱點(diǎn)之一。六足移動(dòng)機(jī)器人的運(yùn)動(dòng)精度因其結(jié)構(gòu)特征主要取決于足部各支鏈的協(xié)調(diào)與運(yùn)動(dòng)軌跡精度，并需要盡量減小足底受力對各關(guān)節(jié)角度的控制精度影響。因此需要準(zhǔn)確的矯正各關(guān)節(jié)轉(zhuǎn)動(dòng)角度，從而提高運(yùn)動(dòng)軌跡控制精度，降低足底與地面接觸沖力。

專利號(hào)為201610017524，名為“一種機(jī)器人的標(biāo)定系統(tǒng)及方法”，該發(fā)明公開了一種通過計(jì)算機(jī)采集標(biāo)定裝置的數(shù)據(jù)以及機(jī)器人的關(guān)節(jié)轉(zhuǎn)角進(jìn)行運(yùn)動(dòng)參數(shù)標(biāo)定的方法。

專利號(hào)為201410231904，名為“串聯(lián)關(guān)節(jié)式機(jī)器人絕對定位誤差校準(zhǔn)方法及標(biāo)定系統(tǒng)”，該發(fā)明公開了一種通過光學(xué)定位儀在上位機(jī)中建立被測關(guān)節(jié)及相鄰關(guān)節(jié)的軸線模型，輸出目標(biāo)轉(zhuǎn)角的網(wǎng)絡(luò)模型，進(jìn)行零位誤差校準(zhǔn)。

但是上述專利存在以下不足：其標(biāo)定過程需要使用精密光學(xué)定位儀或額外位置傳感器，增加了標(biāo)定過程中的額外投入，對于標(biāo)定過程中的環(huán)境也提出了一定的要求。除此以外，需要被標(biāo)定的機(jī)械系統(tǒng)需要增加一定的標(biāo)定裝置以進(jìn)行零點(diǎn)校準(zhǔn)，增加了操作的復(fù)雜度以及成本。

技術(shù)實(shí)現(xiàn)要素：

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于機(jī)身重心位移校正的六足機(jī)器人關(guān)節(jié)角度標(biāo)定方法，以標(biāo)定一種六足式移動(dòng)機(jī)器人關(guān)節(jié)零點(diǎn)與旋轉(zhuǎn)角度為目的，建立了關(guān)節(jié)零點(diǎn)、關(guān)節(jié)變化量同機(jī)器人位姿的映射關(guān)系，并結(jié)合慣性姿態(tài)傳感器，本發(fā)明的標(biāo)定方法基于六足機(jī)器人原有配置的imu傳感器，無需增加額外的儀器或標(biāo)定裝置即可實(shí)現(xiàn)零點(diǎn)標(biāo)定，在六足機(jī)器人設(shè)計(jì)工作范圍及設(shè)計(jì)構(gòu)型條件下即可滿足使用需求。

為實(shí)現(xiàn)以上目的，本發(fā)明提供一種基于機(jī)身重心位移校正的六足機(jī)器人關(guān)節(jié)角度標(biāo)定方法，即：

驅(qū)動(dòng)六足移動(dòng)機(jī)器人以任意三支撐足支撐地面，選取其中一條支撐足并驅(qū)動(dòng)該支撐足上某一關(guān)節(jié)發(fā)生角度轉(zhuǎn)動(dòng)，且該支撐足足底滑動(dòng)，另外兩支撐足底與地形不滑動(dòng)；保持兩支撐足固定不動(dòng)，另外一支撐足轉(zhuǎn)動(dòng)髖關(guān)節(jié)；

采用機(jī)身配置的imu傳感器中的三自由度加速度模塊，經(jīng)二次積分計(jì)算機(jī)身相對運(yùn)動(dòng)軌跡，根據(jù)多足移動(dòng)機(jī)器人足-身運(yùn)動(dòng)學(xué)模型，求解支撐足末端相對機(jī)身的坐標(biāo)，進(jìn)而校正支撐足各關(guān)節(jié)角度，從而實(shí)現(xiàn)對六足機(jī)器人進(jìn)行全部關(guān)節(jié)角度標(biāo)定。

所述六足機(jī)器人，其六個(gè)結(jié)構(gòu)相同的機(jī)械足繞正六邊形機(jī)身呈旋轉(zhuǎn)對稱布置，從機(jī)身到足底上的三個(gè)關(guān)節(jié)依次定義為：髖關(guān)節(jié)、臀關(guān)節(jié)、膝關(guān)節(jié)；在六足機(jī)器人機(jī)身重心處設(shè)立機(jī)身坐標(biāo)系，機(jī)身坐標(biāo)系中：xoy平面與機(jī)身平面平行、z軸垂直于機(jī)身平面；六足髖關(guān)節(jié)轉(zhuǎn)動(dòng)軸垂直于機(jī)身平面，每個(gè)機(jī)械足上的臀關(guān)節(jié)轉(zhuǎn)動(dòng)軸與膝關(guān)節(jié)轉(zhuǎn)動(dòng)軸相互平行且均與機(jī)身平面平行。

具體的，所述六足機(jī)器人關(guān)節(jié)角度標(biāo)定方法，包括如下步驟：

步驟1、初始化imu傳感器的三自由度陀螺及三自由度加速度：

步驟2、使六足機(jī)器人的某一支撐足轉(zhuǎn)動(dòng)任意關(guān)節(jié)角度；

步驟3、讀取三自由度加速度數(shù)據(jù)；

步驟4、若采集到的三自由度加速度均不為0，則記錄數(shù)據(jù)，反之則舍棄該組數(shù)據(jù)，并重復(fù)步驟2及步驟3，直至采集到兩組均不為0的三自由度加速度數(shù)據(jù)；

步驟5、恢復(fù)該支撐足關(guān)節(jié)至步驟1時(shí)的初始角度，更換另一條支撐足，并重復(fù)步驟2、步驟3和步驟4，直至三條支撐足均完成上述步驟2、步驟3和步驟4操作；

步驟6、得到三條支撐足各兩組均不為0的三自由度加速度數(shù)據(jù)，則轉(zhuǎn)到步驟7求解，若沒有得到，繼續(xù)執(zhí)行步驟5；

步驟7、根據(jù)足底相對機(jī)身坐標(biāo)系的坐標(biāo)，求解六足機(jī)器人三條支撐足中某一支撐足關(guān)節(jié)角度；

步驟8、重復(fù)以上操作，即實(shí)現(xiàn)對六足機(jī)器人六個(gè)支撐足進(jìn)行關(guān)節(jié)角度標(biāo)定。

優(yōu)選地，步驟1中，利用imu傳感器的三自由度陀螺運(yùn)動(dòng)感知六足機(jī)器人的機(jī)身重心在空間中三個(gè)方向的加速度；

所述的初始化，通過imu傳感器內(nèi)部的地磁偏角傳感器進(jìn)行校正。

與現(xiàn)有技術(shù)相比，本發(fā)明具有如下的有益效果：

本發(fā)明通過微型慣性傳感器獲取六足機(jī)器人機(jī)身姿態(tài)角度，結(jié)合足式機(jī)器人系統(tǒng)運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)模型完成多足運(yùn)動(dòng)機(jī)器人支撐足各關(guān)節(jié)角度校正；本發(fā)明所述標(biāo)定方法具有較高的精度和實(shí)時(shí)性，能快速準(zhǔn)確地完成對多足移動(dòng)機(jī)器人關(guān)節(jié)角度的校正，從而保證機(jī)器人多足協(xié)調(diào)與運(yùn)動(dòng)軌跡精度。本發(fā)明所述標(biāo)定方法也可用于檢查機(jī)器人足關(guān)節(jié)故障，為機(jī)器人運(yùn)動(dòng)決策提供依據(jù)。

附圖說明

通過閱讀參照以下附圖對非限制性實(shí)施例所作的詳細(xì)描述，本發(fā)明的其它特征、目的和優(yōu)點(diǎn)將會(huì)變得更明顯：

圖1為本發(fā)明一實(shí)施例的方法流程圖；

圖2為本發(fā)明一實(shí)施例的集成慣性傳感器的六足機(jī)器人構(gòu)型與坐標(biāo)系示意圖；

圖3為本發(fā)明一實(shí)施例的機(jī)身關(guān)于固定支撐足轉(zhuǎn)動(dòng)示意簡圖。

具體實(shí)施方式

下面結(jié)合具體實(shí)施例對本發(fā)明進(jìn)行詳細(xì)說明。以下實(shí)施例將有助于本領(lǐng)域的技術(shù)人員進(jìn)一步理解本發(fā)明，但不以任何形式限制本發(fā)明。應(yīng)當(dāng)指出的是，對本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)。這些都屬于本發(fā)明的保護(hù)范圍。

如圖1所示，一種基于機(jī)身重心位移校正的六足機(jī)器人關(guān)節(jié)角度標(biāo)定方法，所述方法針對的六足機(jī)器人，其六個(gè)結(jié)構(gòu)相同的機(jī)械足繞正六邊形機(jī)身呈旋轉(zhuǎn)對稱布置，，以任意三足支撐地面、另外三足抬起并始終保持不接觸地面。

所述方法包括如下步驟：

步驟1、初始化imu傳感器的三自由度陀螺及三自由度加速度：

如圖2所示，從六足機(jī)器人的機(jī)身到機(jī)械足底上的三個(gè)關(guān)節(jié)依次定義為：髖關(guān)節(jié)、臀關(guān)節(jié)、膝關(guān)節(jié)；在六足機(jī)器人機(jī)身重心處設(shè)立機(jī)身坐標(biāo)系，其xoy平面與機(jī)身平面平行，z軸垂直于機(jī)身平面。六足髖關(guān)節(jié)轉(zhuǎn)動(dòng)軸垂直于機(jī)身平面，每個(gè)機(jī)械足上的臀關(guān)節(jié)轉(zhuǎn)動(dòng)軸與膝關(guān)節(jié)轉(zhuǎn)動(dòng)軸相互平行，且均與機(jī)身平面平行；

由于六足機(jī)器人支撐至少需要三條足，考慮六足機(jī)器人內(nèi)的三條足機(jī)構(gòu)，并設(shè)機(jī)械足足底到機(jī)身重心位置矢量分別為p1c、p2c、p3c，簡化為：pic＝(xic,yic,zic)^t(i＝1,2,3)，令αi表示第i個(gè)髖關(guān)節(jié)相對零點(diǎn)的轉(zhuǎn)動(dòng)角度，βi表示第i個(gè)臀關(guān)節(jié)相對零點(diǎn)的轉(zhuǎn)動(dòng)角度，γi表示第i個(gè)膝關(guān)節(jié)相對零點(diǎn)的轉(zhuǎn)動(dòng)角度；髖關(guān)節(jié)相對于機(jī)身重心距離為l1，大腿機(jī)構(gòu)長度為l2，小腿結(jié)構(gòu)長度為l3；(xiα,yiα,ziα)是髖關(guān)節(jié)在機(jī)器人機(jī)身坐標(biāo)系中的坐標(biāo)。

步驟2、使某一支撐足轉(zhuǎn)動(dòng)任意關(guān)節(jié)角度，如圖3所示，即使六足機(jī)器人重心從o點(diǎn)運(yùn)動(dòng)至q1點(diǎn)；

步驟3、讀取陀螺三自由度加速度數(shù)據(jù)；

步驟4、若采集到的三自由度加速度均不為0，則記錄數(shù)據(jù)，反之則舍棄該組數(shù)據(jù)，重復(fù)步驟2、3，直至采集到兩組均不為0的三自由度加速度數(shù)據(jù)，如圖3所示六足機(jī)器人重心從q1點(diǎn)運(yùn)動(dòng)至q2點(diǎn)；

設(shè)初始狀態(tài)時(shí)六足機(jī)器人機(jī)身坐標(biāo)系c初始時(shí)刻與世界坐標(biāo)系重合，原點(diǎn)坐標(biāo)為(0,0,0)；初始時(shí)刻六足機(jī)器人的三支撐足足末端在世界坐標(biāo)系o中的坐標(biāo)分別為p1o,p2o，p3o，其中：pio＝(xio,yio,zio)^t；

六足機(jī)器人的一個(gè)機(jī)械足的任意關(guān)節(jié)發(fā)生兩次轉(zhuǎn)動(dòng)，使得六足機(jī)器人的機(jī)身繞未轉(zhuǎn)動(dòng)支撐足末端所在直線p1op2o轉(zhuǎn)動(dòng)；利用加速度計(jì)積分，測得六足機(jī)器人的重心從坐標(biāo)原點(diǎn)移動(dòng)至點(diǎn)q1＝(x1,y1,z1)^t、q2＝(x2,y2,z2)^t，可求解直線p1op2o相對于機(jī)身坐標(biāo)系的解析坐標(biāo)；設(shè)直線的單位方向向量為e1＝(ex1,ey1,ez1)^t，

可由權(quán)利要求書中公式(6)求解：

取p1op2o直線上存在一點(diǎn)q01＝(x01,y01,z01)^t在oq1q2三點(diǎn)確定的平面上，由幾何關(guān)系可由權(quán)利要求書中公式(7)、(8)、(9)解出q01坐標(biāo)為：

p1op2o方程可由權(quán)利要求書中公式(10)表示為：

步驟5、恢復(fù)該支撐足關(guān)節(jié)至步驟1時(shí)初始角度，更換另一條支撐足，重復(fù)步驟2、3、4；

步驟6、得到三條不同的支撐足各兩組均不為0的三自由度加速度數(shù)據(jù)，通過步驟7求解，反之繼續(xù)執(zhí)行步驟5；

與權(quán)利要求書中公式(11)、(12)一致，由步驟4中推導(dǎo)方法可解出p2op3o、p3op1o方程分別為：

其中，ex2，ey2，ez2與ex3，ey3，ez3分別為直線p2op3o與p3op1o的單位方向向量坐標(biāo)，x02，y02，z02與x03，y03，z03分別為直線p2op3o與p3op1o上任意一點(diǎn)q02與q03的坐標(biāo)，其求解方法如步驟4所述。

步驟7、根據(jù)足底相對機(jī)身坐標(biāo)系的坐標(biāo)，求解六足機(jī)器人某一支撐足關(guān)節(jié)角度；

六足機(jī)器人的三個(gè)支撐足坐標(biāo)，為直線p1op2o、p2op3o、p3op1o兩兩相交的交點(diǎn)坐標(biāo)，求解三條直線空間方程即可獲得足末端相對機(jī)身坐標(biāo)位置；

直線p1op2o、p2op3o、p3op1o理論上兩兩相交于一點(diǎn)，但鑒于imu傳感器噪聲及加速度積分誤差的存在，使得計(jì)算出來的直線位置與實(shí)際存在微小偏差，因此可能導(dǎo)致空間內(nèi)三條直線不相交。因此，通過在三條直線上各搜索兩個(gè)點(diǎn)分別到另外兩條直線距離最短，取相鄰兩直線上相近的兩點(diǎn)的中點(diǎn)作為足末端坐標(biāo)點(diǎn)。所以，設(shè)p1op2o，p2op3o上兩點(diǎn)坐標(biāo)分別由權(quán)利要求書中公式(13)、(14)表示為：

x2＝ex1m2+x01，y2＝ey1m2+y01，z2＝ez1m2+z01(13)

x2'＝ex2n2+x02，y2'＝ey2n2+y02，z2'＝ez2n2+z02(14)

其中，為任意常數(shù)。

由權(quán)利要求書中公式(15)：

兩直線上最近的兩點(diǎn)坐標(biāo)即滿足f最小，由權(quán)利要求書中公式(16)、(17)求得此時(shí)mi、ni(i＝1,2，3)：

由權(quán)利要求書中公式(18)到(26)，得到：

c1＝ex1ex3+ey1ey3+ez1ez3(18)

d1＝ex1(x01-x03)+ey1(y01-y03)+ez1(z01-z03)(19)

e1＝ex3(x01-x03)+ey3(y01-y03)+ez3(z01-z03)(20)

c2＝ex1ex2+ey1ey2+ez1ez2(21)

d2＝ex1(x01-x02)+ey1(y01-y02)+ez1(z01-z02)(22)

e2＝ex2(x01-x02)+ey2(y01-y02)+ez2(z01-z02)(23)

c3＝ex3ex2+ey3ey2+ez3ez2(24)

d3＝ex3(x03-x02)+ey3(y03-y02)+ez3(z03-z02)(25)

e3＝ex2(x03-x02)+ey2(y03-y02)+ez2(z03-z02)(26)

進(jìn)而求出兩連線段中點(diǎn)坐標(biāo)，求解p1op2o與p1op3o、p2op3o與p1op3o交點(diǎn)坐標(biāo)得p1o、p2o、p3o的坐標(biāo)為，由權(quán)利要求書中公式(27)、(28)、(29)得：

在求出足末端位置后，代入權(quán)利要求書中公式(1)(2)、(3)中求解，可得關(guān)節(jié)初始值；

即：

其中：

a＝(xic-xiα-l1sinαi)²+(yic-yiα-l1cosαi)²(4)

b＝(zic-zi0)²(5)

如權(quán)利要求書中公式(4)、(5)。需要注意的是，公式(4)(5)有兩個(gè)解，需要根據(jù)六足機(jī)器人的運(yùn)動(dòng)空間及關(guān)節(jié)的運(yùn)動(dòng)范圍限制，選擇合適的關(guān)節(jié)角度。

步驟8、重復(fù)以上操作，即可對六足機(jī)器人六個(gè)支撐足進(jìn)行關(guān)節(jié)角度標(biāo)定。

以上對本發(fā)明的具體實(shí)施例進(jìn)行了描述。需要理解的是，本發(fā)明并不局限于上述特定實(shí)施方式，本領(lǐng)域技術(shù)人員可以在權(quán)利要求的范圍內(nèi)做出各種變形或修改，這并不影響本發(fā)明的實(shí)質(zhì)內(nèi)容。