本發(fā)明涉及足式移動機器人運動情況的標定方法，屬于機器人技術(shù)領(lǐng)域，具體地，涉及一種基于機身姿態(tài)角度校正的六足機器人關(guān)節(jié)角度標定方法。

背景技術(shù)：

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

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

經(jīng)檢索：

專利號為201220392380，名為“一種工業(yè)機器人的機械零點標定裝置”，該發(fā)明公開了一種設置關(guān)節(jié)機械零點標定塊的標定方法，該新型專利通過高精度加工和裝配在標定關(guān)節(jié)處設置機械零點標定機構(gòu)，進而實現(xiàn)零位標定。

專利號為201410401317，名為“一種六關(guān)節(jié)工業(yè)機器人的精度標定裝置”，該發(fā)明公開了一種在機器人末端連接固定光源裝置進行標定的方法，該方法利用激光筆和幕布進行快速低成本標定。

專利號為201610188161，名為“仿爬行類四足步行機器人運動參數(shù)的標定”，該發(fā)明公開了一種仿爬行四足步行機器人運動參數(shù)的標定裝置，該方法建立專用的標定裝置，利用測量平臺測量機器人的運動軌跡進行運動參數(shù)的標定。

但是上述專利存在以下不足：標定過程需要使用標準標定塊，或者使用特制的標定平臺，無法避免的機械制造誤差將對標定過程產(chǎn)生誤差累計，無法保證機械零點標定的準確性。

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

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于機身姿態(tài)角度校正的六足機器人關(guān)節(jié)角度標定方法，具有較高的精度和實時性，能快速準確地完成對多足移動機器人關(guān)節(jié)角度的校正，從而保證機器人多足協(xié)調(diào)與運動軌跡精度。

為實現(xiàn)以上目的，本發(fā)明提供一種基于機身姿態(tài)角度校正的六足機器人關(guān)節(jié)角度標定方法，即：

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

建立關(guān)節(jié)角度與姿態(tài)間的函數(shù)關(guān)系，獲得關(guān)于關(guān)節(jié)零點的線性方程組，求解即解出第一條支撐足的各關(guān)節(jié)角度，從而實現(xiàn)對六足機器人的全部關(guān)節(jié)角度標定。

所述的六足機器人，其六個結(jié)構(gòu)相同的支撐足繞正六邊形機身呈旋轉(zhuǎn)對稱布置；從六足機器人機身到支撐足足底上的三個關(guān)節(jié)依次定義為：髖關(guān)節(jié)、臀關(guān)節(jié)、膝關(guān)節(jié)；

在六足機器人機身的重心處設立機身坐標系，其中：機身坐標系的xoy平面與機身平面平行，機身坐標系的z軸垂直于機身平面；六足髖關(guān)節(jié)的轉(zhuǎn)動軸垂直于機身平面，每個支撐足上的臀關(guān)節(jié)轉(zhuǎn)動軸與膝關(guān)節(jié)轉(zhuǎn)動軸相互平行，且均與機身平面平行。

具體的，所述方法包括如下步驟：

步驟1、初始化六足機器人機身的imu傳感器的三自由度陀螺角度；

步驟2、使某一支撐足任意關(guān)節(jié)旋轉(zhuǎn)給定角度；

步驟3、讀取imu傳感器中陀螺三自由度角度；

步驟4、使步驟2中該關(guān)節(jié)再次重復步驟2旋轉(zhuǎn)動作，重復步驟3，直至得到五組姿態(tài)imu傳感器中陀螺三自由度角度數(shù)據(jù)；

步驟5、若步驟4得到的五組數(shù)據(jù)不全相同，則轉(zhuǎn)到步驟6進行求解；

若步驟4得到的五組姿態(tài)數(shù)據(jù)有兩組或多組是相同的，則轉(zhuǎn)動步驟2中該支撐足的任意一個關(guān)節(jié)一定角度，但該關(guān)節(jié)不得是步驟2中已旋轉(zhuǎn)過的關(guān)節(jié)，重復步驟2～4，直至得到五組不全相同的姿態(tài)imu傳感器中陀螺三自由度角度姿態(tài)數(shù)據(jù)；

步驟6、根據(jù)支撐足足底相對機身坐標系的坐標，求解六足機器人步驟2中進行旋轉(zhuǎn)運動的支撐足的關(guān)節(jié)角度；

步驟7、重復以上操作，即對六足機器人六個支撐足進行關(guān)節(jié)角度標定。

優(yōu)選地，步驟1中，利用imu傳感器的三自由度陀螺感知六足機器人機身姿態(tài)角度；初始化imu傳感器，是通過imu傳感器內(nèi)部的地磁偏角傳感器進行校正。

優(yōu)選地，步驟1中，六足機器人支撐至少需要三條支撐足，考慮機器人內(nèi)的三條支撐足機構(gòu)，設支撐足底到機身重心位置矢量分別為p1c、p2c、p3c，簡化為：pic＝(xic,yic,zic)^t(i＝1,2,3)；由權(quán)利要求書中公式(6)、(7)、(8)：

xi＝(l1+l2cosβi+l3cosθi)sinαi(6)

yi＝(l1+l2cosβi+l3cosθi)cosαi(7)

zi＝l2sinβi+l3sinθi(8)

其中；αi表示第i個髖關(guān)節(jié)相對與零點的轉(zhuǎn)動角度，βi表示第i個臀關(guān)節(jié)相對于零點的轉(zhuǎn)動角度，γi表示第i個膝關(guān)節(jié)相對于零點的轉(zhuǎn)動角度，l1為髖關(guān)節(jié)相對于機身重心距離，l2為大腿機構(gòu)長度，l3為小腿結(jié)構(gòu)長度；

由權(quán)利要求書中公式(9)、(10)、(11)得：

xic＝xiα+xi(9)

yic＝y(tǒng)iα+yi(10)

zic＝ziα+zi(11)

其中：(xiα,yiα,ziα)是髖關(guān)節(jié)在機器人機身坐標系中的坐標。

優(yōu)選地，由權(quán)利要求書中公式(12)，步驟2中，設六足機器人第n次轉(zhuǎn)動時機身坐標系相對世界坐標系的方向余弦矩陣為：

機身坐標系原點c在世界坐標系o中位置矢量為^opcn＝(^oxcn,^oycn,^ozcn)^t，三條支撐足末端在世界坐標系中的位置矢量分別為p1o、p2o、p3o，其中：pio＝(xio,yio,zio)^t

則：

由權(quán)利要求書中公式(13)，三條支撐足末端在機身坐標系c和世界坐標系o中的位置矢量有關(guān)系：

pio＝^orcnpic+^opcn(13)

將公式(13)展開，取第三分量上的方程，得：

其中：rni＝r31nxi0+r32nyi0+r33nzi0為已知常數(shù)項；

由公式(14)得，當?shù)趇足的髖關(guān)節(jié)每次轉(zhuǎn)動角度δα、轉(zhuǎn)動n次后：

z1o-^ozcn＝rn1+r′31nx1c+r′32ny1c+r33nz1c(15)

即權(quán)利要求書中公式(15)、(16)、(17)，其中：

r′31n＝r31ncosnδα-r32nsinnδα(16)

r′32n＝r31nsinnδα+r32ncosnδα(17)

在平地上，足末端位置在世界坐標系中zio相同，即z1o＝z2o＝z3o，由權(quán)利要求書中公式(18)及(19)得：

r′31nx1+r′32ny1+r33nz1-r31nx2-r32ny2-r33nz2＝rn2-rn1(18)

r′31nx1+r′32ny1+r33nz1-r31nx3-r32ny3-r33nz3＝rn3-rn1(19)

因此，得到關(guān)于p＝(x1,y1...y3,z3)^t的非齊次線性方程組，對支撐足關(guān)節(jié)的任意一次轉(zhuǎn)動均得到兩個方程。

優(yōu)選地，步驟4及步驟5中轉(zhuǎn)動5次得到五組imu傳感器中姿態(tài)不同的三自由度角度數(shù)據(jù)，取n＝0,1,2...4，得10個方程，取其中9個，解出p＝(x1,y1...y3,z3)^t；取其中第一至第九式，得權(quán)利要求書中公式(20)：

根據(jù)克拉默法則可求解支撐足足底相對機身坐標系中的坐標。

優(yōu)選地，步驟6中，對五組不相同的imu傳感器中陀螺三自由度角度數(shù)據(jù)帶入方程組，通過克拉默法則求解初始各支撐足足底在機身坐標系中坐標，即反解出各支撐足的關(guān)節(jié)角度；

所述求解過程基于六足移動機器人運動學逆解，根據(jù)支撐足足底相對機身坐標系的坐標求解六足機器人關(guān)節(jié)角度：

由

xic＝xiα+[l1+l2cosβi+l3cos(βi+γi)]sinαi(21)

yic＝y(tǒng)iα+[l1+l2cosβi+l3cos(βi+γi)]cosαi(22)

zic＝ziα+l2sinβi+l3sin(βi+γi)(23)

則權(quán)利要求書中公式(1)、(2)及(3)得：

其中，由權(quán)利要求書中公式(4)及(5)：

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

b＝(zic-zi0)²(5)

更優(yōu)選地，公式(4)、(5)有兩個解，需要根據(jù)六足機器人的運動空間及關(guān)節(jié)的運動范圍限制，選擇合適的關(guān)節(jié)角度。

本發(fā)明所述標定方法針對的六足機器人，其六個結(jié)構(gòu)相同的支撐足繞正六邊形機身呈旋轉(zhuǎn)對稱布置。

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

本發(fā)明通過微型慣性傳感器獲取六足機器人機身姿態(tài)角度，結(jié)合足式機器人系統(tǒng)運動學與逆運動學模型完成多足運動機器人支撐足各關(guān)節(jié)角度校正；能快速準確完成對多足移動機器人關(guān)節(jié)角度的校正，保證機器人多足協(xié)調(diào)與運動軌跡精度。

本發(fā)明標定基于六足機器人配置的imu傳感器，其具有較高的測量精度，而且標定過程只受傳感器精度及測量過程數(shù)值計算的精度所影響，相比利用外置的機械標定方法具有更高的精度。

附圖說明

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

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

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

圖3為本發(fā)明一實施例的足末端在機身坐標系c和世界坐標系o中的位置矢量示意圖。

具體實施方式

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

如圖1所示，一種基于機身姿態(tài)角度校正的六足機器人關(guān)節(jié)角度標定方法，所述方法針對的六足機器人，其六個結(jié)構(gòu)相同的支撐足繞正六邊形機身呈旋轉(zhuǎn)對稱布置。如圖2所示，從機身到足底上的三個關(guān)節(jié)依次定義為：髖關(guān)節(jié)、臀關(guān)節(jié)、膝關(guān)節(jié)；在六足機器人的機身重心處設立機身坐標系，該機身坐標系中：xoy平面與機身平面平行、z軸垂直于機身平面；六足髖關(guān)節(jié)轉(zhuǎn)動軸垂直于機身平面，每個支撐足上的臀關(guān)節(jié)轉(zhuǎn)動軸與膝關(guān)節(jié)轉(zhuǎn)動軸相互平行，且均與機身平面平行；

所述方法具體步驟如下：

步驟1、初始化imu傳感器的三自由度陀螺角度

由于六足機器人支撐至少需要三條支撐足，考慮機器人內(nèi)的三條足機構(gòu)，并設足底到機身重心位置矢量分別為p1c,p2c,p3c，簡化為：pic＝(xic,yic,zic)^t(i＝1,2,3)。如圖2所示

由權(quán)利要求書中公式(6)、(7)、(8)：

xi＝(l1+l2cosβi+l3cosθi)sinαi(6)

yi＝(l1+l2cosβi+l3cosθi)cosαi(7)

zi＝l2sinβi+l3sinθi(8)

其中；αi表示第i個髖關(guān)節(jié)相對與零點的轉(zhuǎn)動角度，βi表示第i個臀關(guān)節(jié)相對于零點的轉(zhuǎn)動角度，γi表示第i個膝關(guān)節(jié)相對于零點的轉(zhuǎn)動角度，l1為髖關(guān)節(jié)相對于機身重心距離，l2為大腿機構(gòu)長度，l3為小腿結(jié)構(gòu)長度。

由權(quán)利要求書中公式(9)、(10)、(11)得：

xic＝xiα+xi(9)

yic＝y(tǒng)iα+yi(10)

zic＝ziα+zi(11)

其中：(xiα,yiα,ziα)是髖關(guān)節(jié)在機器人機身坐標系中的坐標。

步驟2、使某一支撐足髖關(guān)節(jié)旋轉(zhuǎn)一定給定角度δα，讀取陀螺三自由度角度；

本實施例中，具體方法為：驅(qū)動六足移動機器人僅以任意三支撐足支撐地面，選取其中第一條支撐足并驅(qū)動該支撐足足上某一關(guān)節(jié)發(fā)生角度轉(zhuǎn)動，且該支撐足足底滑動，另外第二和第三條支撐足足底與地形不滑動，保持第二和第三條支撐足固定不動，第一條支撐足轉(zhuǎn)動髖關(guān)節(jié)，通過建立關(guān)節(jié)角度與姿態(tài)間的函數(shù)關(guān)系，獲得關(guān)于關(guān)節(jié)零點的線性方程組，求解出支撐足各關(guān)節(jié)角度。

其原理推導如下：

設機器人第n次轉(zhuǎn)動時機身坐標系相對世界坐標系的方向余弦矩陣為，由權(quán)利要求書中公式(12)：

機身坐標系原點c在世界坐標系o中位置矢量為^opcn＝(^oxcn,^oycn,^ozcn)^t，三支撐足足末端在世界坐標系中的位置矢量分別為p1o、p2o、p3o，其中：pio＝(xio,yio,zio)^t，

則：

足末端在機身坐標系c和世界坐標系o中的位置矢量(如圖3所示)有關(guān)系：

pio＝^orcnpic+^opcn(13)

即權(quán)利要求書中公式(13)，將式(13)展開，取第三分量上的方程，得：

其中：rni＝r31nxi0+r32nyi0+r33nzi0為已知常數(shù)項。

由公式(14)得，當?shù)趇足的髖關(guān)節(jié)每次轉(zhuǎn)動角度δα、轉(zhuǎn)動n次后得到權(quán)利要求書中公式(15)：

z1o-^ozcn＝rn1+r′31nx1c+r′32ny1c+r33nz1c(15)

其中：

r′31n＝r31ncosnδα-r32nsinnδα(16)

r′32n＝r31nsinnδα+r32ncosnδα(17)

即權(quán)利要求書中公式(16)及(17)，在平地上，足末端位置在世界坐標系中zio相同，即z1o＝z2o＝z3o，得到權(quán)利要求書中公式(18)、(19)：

r′31nx1+r′32ny1+r33nz1-r31nx2-r32ny2-r33nz2＝rn2-rn1(18)

r′31nx1+r′32ny1+r33nz1-r31nx3-r32ny3-r33nz3＝rn3-rn1(19)

因此，得到關(guān)于p＝(x1,y1...y3,z3)^t的非齊次線性方程組，對支撐足關(guān)節(jié)的任意一次轉(zhuǎn)動都可以得到兩個方程。

步驟3、重復步驟2，直至得到五組數(shù)據(jù)；

步驟4、若步驟3得到的五組數(shù)據(jù)姿態(tài)不全相同，通過步驟5進行求解，反之則轉(zhuǎn)動任意臀關(guān)節(jié)或膝關(guān)節(jié)，重復步驟2、3，直至得到五組不全相同的姿態(tài)數(shù)據(jù)；

轉(zhuǎn)動5次，取n＝0,1,2...4得10個方程，只須取其中9個，解出p＝(x1,y1...y3,z3)^t。取其中第一至第九式，得權(quán)利要求書中公式(20)：

根據(jù)克拉默法則，可求解各支撐足足底相對機身坐標系中的坐標。

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

其求解過程基于六足移動機器人運動學逆解。根據(jù)足底相對機身坐標系的坐標，求解六足機器人關(guān)節(jié)角度：

由公式(21)、(22)及(23)：

xic＝xiα+[l1+l2cosβi+l3cos(βi+γi)]sinαi(21)

yic＝y(tǒng)iα+[l1+l2cosβi+l3cos(βi+γi)]cosαi(22)

zic＝ziα+l2sinβi+l3sin(βi+γi)(23)

則由公式(1)、(2)及(3)：

其中：

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

b＝(zic-zi0)²(5)

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

步驟6、重復以上操作，即對六足機器人六個支撐足進行關(guān)節(jié)角度標定。

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