提高六足機(jī)器人行走穩(wěn)定性的步態(tài)規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及六足機(jī)器人，具體涉及提高六足機(jī)器人行走穩(wěn)定性的步態(tài)規(guī)劃方法。
【背景技術(shù)】
[0002] 六足機(jī)器人又叫蜘蛛機(jī)器人，是多足機(jī)器人的一種，具有穩(wěn)定性好、承載能力高、 地形適應(yīng)性強(qiáng)等優(yōu)勢(shì)，是復(fù)雜作業(yè)環(huán)境下的最佳選擇，具有廣闊的應(yīng)用前景。
[0003] 目前，六足機(jī)器人的常見(jiàn)步態(tài)以三角步態(tài)為主，即將六足分為交替相隔的兩組，每 組三足，通過(guò)兩組關(guān)節(jié)交替起落旋轉(zhuǎn)來(lái)實(shí)現(xiàn)機(jī)器人的前進(jìn)、轉(zhuǎn)向、后退等行動(dòng)。三角步態(tài)雖 然能實(shí)現(xiàn)移動(dòng)、旋轉(zhuǎn)等動(dòng)作，不僅動(dòng)作單調(diào)生硬、動(dòng)作的節(jié)奏感較強(qiáng)，無(wú)法流暢平滑移動(dòng);而 且由于三角步態(tài)的前進(jìn)動(dòng)作實(shí)際上是通過(guò)兩個(gè)不同方向的交替旋轉(zhuǎn)進(jìn)行的，無(wú)法保持機(jī)器 人機(jī)身正面方向穩(wěn)定一致，對(duì)于有攝像頭功能的六足機(jī)器人來(lái)說(shuō)，會(huì)造成視頻畫(huà)面的劇烈 晃動(dòng)，嚴(yán)重影響視頻質(zhì)量和應(yīng)用范圍。

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

[0004] 本發(fā)明所要解決的技術(shù)問(wèn)題是現(xiàn)有六足機(jī)器人的三角步態(tài)不僅動(dòng)作單調(diào)生硬、動(dòng) 作的節(jié)奏感較強(qiáng)，無(wú)法流暢平滑移動(dòng)，而且無(wú)法保持機(jī)器人機(jī)身正面方向穩(wěn)定一致的問(wèn)題。
[0005] 為了解決上述技術(shù)問(wèn)題，本發(fā)明所采用的技術(shù)方案是提供一種提高六足機(jī)器人行 走穩(wěn)定性的步態(tài)規(guī)劃方法，包括以下基本動(dòng)作：
[0006] 行進(jìn):抬起不相鄰兩足，開(kāi)始沿平行于機(jī)身中心和終點(diǎn)連線的每足末端的平移路 徑行進(jìn)，行進(jìn)時(shí)始終保證四足著地，不相鄰兩足抬起，且每足的各關(guān)節(jié)按周期旋轉(zhuǎn)到對(duì)應(yīng)預(yù) 設(shè)角度，使每足末端在其平移路徑上抬起、落下；
[0007] 變向：機(jī)器人頭部攝像模塊旋轉(zhuǎn)到正對(duì)目標(biāo)終點(diǎn)方向，再執(zhí)行行進(jìn)基本動(dòng)作。
[0008] 在上述方案中，行進(jìn)時(shí)始終保證四足著地，不相鄰兩足抬起包括交替抬起不相鄰 的兩足和不相鄰的兩足交替抬起兩種步態(tài)，其中，
[0009] 交替抬起不相鄰的兩足為:行進(jìn)開(kāi)始后，抬起的兩足一起落地，著地、不相鄰的兩 足一起抬起；
[0010] 不相鄰的兩足交替抬起為:行進(jìn)開(kāi)始后，抬起的一足落地，同時(shí)著地的四足中一足 抬起，且該抬起足與原抬起足不相鄰。
[0011] 在上述方案中，每足末端的平移路徑計(jì)算方式為:利用變換算法建立每足的足坐 標(biāo)系與機(jī)身坐標(biāo)系的對(duì)應(yīng)關(guān)系，在標(biāo)準(zhǔn)站立姿態(tài)下，通過(guò)機(jī)身中心平移路徑，計(jì)算出每足末 端的平移路徑;其中，
[0012] 機(jī)身中心平移路徑的計(jì)算方法為:在機(jī)身坐標(biāo)系中，將機(jī)身中心映射到經(jīng)過(guò)目標(biāo) 終點(diǎn)垂直于地面的直線上，通過(guò)目標(biāo)終點(diǎn)坐標(biāo)得到機(jī)身映射中心坐標(biāo)，機(jī)身中心與機(jī)身映 射中心連線為機(jī)身中心平移路徑。
[0013] 在上述方案中，在機(jī)身坐標(biāo)系中，目標(biāo)終點(diǎn)坐標(biāo)的獲取是攝像模塊搜索得到在環(huán) 境坐標(biāo)系下的目標(biāo)終點(diǎn)坐標(biāo)，再通過(guò)變換算法將目標(biāo)終點(diǎn)在環(huán)境坐標(biāo)系坐標(biāo)投射到機(jī)身坐 標(biāo)系中，獲取在機(jī)身坐標(biāo)系中目標(biāo)終點(diǎn)坐標(biāo)。
[0014] 在上述方案中，通過(guò)變換算法建立每足的足坐標(biāo)系與機(jī)身坐標(biāo)系的對(duì)應(yīng)關(guān)系具體 包括以下步驟：
[0015] 第一步、通過(guò)對(duì)機(jī)身坐標(biāo)系坐標(biāo)(x,y)進(jìn)行旋轉(zhuǎn)，得到足坐標(biāo)系坐標(biāo)(X'，y'）為：
[0016]
[0017] 其中，Θ為機(jī)身中心和攝像模塊方向連線與機(jī)身中心和足根部在機(jī)身連接點(diǎn)連線 的夾角；
[0018] 第二步、對(duì)坐標(biāo)(x'，y'）在足坐標(biāo)系的y軸上平移r，r為機(jī)身半徑，在X軸上不變，則 得到機(jī)身坐標(biāo)系坐標(biāo)(x，y，z)在足坐標(biāo)系的坐標(biāo)(X' '，y' '，z' '）；其中，平移后X' ' =x'，y' ' = y'+r，由于z軸未發(fā)生變換，所以z' ' = z。
[0019] 在上述方案中，機(jī)器人包括髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)三個(gè)關(guān)節(jié)，標(biāo)準(zhǔn)站立姿態(tài)時(shí)三 個(gè)關(guān)節(jié)的角度如下：
[0020] 髖關(guān)節(jié):與機(jī)身圓周接觸點(diǎn)的切線方向垂直；
[0021 ]膝關(guān)節(jié):與機(jī)身水平方向平行；
[0022]踝關(guān)節(jié):使兩段肢體成90度，足末端與地面垂直。
[0023] 在上述方案中，所述髖關(guān)節(jié)能沿著機(jī)身圓周運(yùn)動(dòng)，使機(jī)身轉(zhuǎn)動(dòng)。
[0024] 在上述方案中，各關(guān)節(jié)旋轉(zhuǎn)的預(yù)設(shè)角度的計(jì)算方法為:在每個(gè)足坐標(biāo)系中，將機(jī)器 人各個(gè)關(guān)節(jié)角度與足末端點(diǎn)坐標(biāo)進(jìn)行映射，通過(guò)坐標(biāo)映射計(jì)算出各個(gè)足末端在其平移路徑 豎直平面上的運(yùn)動(dòng)軌跡，并獲得足末端在該運(yùn)動(dòng)軌跡不同坐標(biāo)點(diǎn)上對(duì)應(yīng)的各關(guān)節(jié)旋轉(zhuǎn)角 度，配置關(guān)節(jié)旋轉(zhuǎn)預(yù)設(shè)角度；
[0025] 足末端在運(yùn)動(dòng)軌跡不同坐標(biāo)點(diǎn)上對(duì)應(yīng)的各關(guān)節(jié)旋轉(zhuǎn)角度采取D-H矩陣進(jìn)行計(jì)算， 具體公式為：
[0026] 足末端的變換矩陣T為：[T] = [Z1] [Z2]... [Zn];
[0027] 相鄰兩連桿間的變換矩陣Zi為：
[0028]
[0029]其中，Q1表示運(yùn)動(dòng)時(shí)各關(guān)節(jié)繞自身坐標(biāo)系z(mì)軸的旋轉(zhuǎn)角;Cl1表示運(yùn)動(dòng)時(shí)在z軸上各關(guān) 節(jié)之間的偏移;i取值為1至3。
[0030] 在上述方案中，每足按相同周期、不同相位差進(jìn)行循環(huán)動(dòng)作，各足平穩(wěn)交替平移；
[0031] 每足的運(yùn)動(dòng)周期大致分為三部分:抬起、下落、后蹬;其中，后蹬部分時(shí)間占到周期 的 2/3。
[0032] 在上述方案中，每足的運(yùn)動(dòng)周期的三部分動(dòng)作的實(shí)現(xiàn)是通過(guò)將每個(gè)運(yùn)動(dòng)周期足末 端的運(yùn)動(dòng)軌跡均勻設(shè)置多個(gè)關(guān)鍵坐標(biāo)點(diǎn)，再根據(jù)關(guān)鍵坐標(biāo)點(diǎn)數(shù)量將整個(gè)運(yùn)動(dòng)周期分為均勻 的小周期，然后在一個(gè)小周期內(nèi)將足末端從一個(gè)關(guān)鍵坐標(biāo)點(diǎn)移到下一個(gè)關(guān)鍵坐標(biāo)點(diǎn)。
[0033] 本發(fā)明依賴(lài)變換算法保證各足末端位置在移動(dòng)中始終保持直線，以使得機(jī)身位置 也保持直線向前，且保證在任何情況下保證不相鄰四足著地，避免動(dòng)作頓挫、生硬、機(jī)身傾 斜，步態(tài)流暢穩(wěn)定；同時(shí)旋轉(zhuǎn)時(shí)僅旋轉(zhuǎn)頭部，機(jī)身不動(dòng)，保持機(jī)器人正面始終直線向前，不出 現(xiàn)角度扭曲，能夠?qū)崿F(xiàn)不必移動(dòng)肢體即可快速改變正面朝向或向指定方向移動(dòng)。
【附圖說(shuō)明】
[0034] 圖1為本發(fā)明提供的一種提高六足機(jī)器人行走穩(wěn)定性的步態(tài)規(guī)劃方法每足的平移 路徑不意圖；
[0035] 圖2、圖3為本發(fā)明提供的一種提高六足機(jī)器人行走穩(wěn)定性的步態(tài)規(guī)劃方法的六足 末端狀態(tài)不意圖；
[0036] 圖4為本發(fā)明中機(jī)器人各相鄰連桿的空間關(guān)系示意圖。
【具體實(shí)施方式】
[0037] 本發(fā)明是基于機(jī)身為圓周對(duì)稱(chēng)結(jié)構(gòu)、六足均勻分布于機(jī)身圓周的六足機(jī)器人，且 設(shè)在機(jī)身之上的機(jī)器人頭部可360度旋轉(zhuǎn)，并在頭部正臉帶有攝像模塊。為了方便說(shuō)明，本 發(fā)明定義了三個(gè)坐標(biāo)系，具體如下：
[0038] 機(jī)身坐標(biāo)系，以機(jī)器人機(jī)身中心為原點(diǎn)，經(jīng)過(guò)原點(diǎn)、平行于機(jī)身平面、垂直于頭部 正臉表面中分線的直線為Y軸，平行機(jī)身平面、垂直于Y軸直線為X軸，垂直于機(jī)身平面的直 線為Z軸；
[0039] 足坐標(biāo)系，以每足根部與機(jī)身連接處為坐標(biāo)原點(diǎn)，經(jīng)坐標(biāo)原點(diǎn)的機(jī)身切線為X軸， 坐標(biāo)原點(diǎn)與機(jī)身中心連接延長(zhǎng)線為Y軸、經(jīng)坐標(biāo)原點(diǎn)垂直于機(jī)身平面的直線Z軸，簡(jiǎn)稱(chēng)足坐 標(biāo)系，共6個(gè)；
[0040] 環(huán)境坐標(biāo)系，以機(jī)器人足末端所踏的地平面作為X-Y平面，原點(diǎn)可以設(shè)置在X-Y平 面任意位置上，經(jīng)過(guò)原點(diǎn)并垂直于地平面方向?yàn)閆軸。
[0041] 下面結(jié)合說(shuō)明書(shū)附圖和具體實(shí)施例對(duì)本發(fā)明做出詳細(xì)的說(shuō)明。
[0042] 本發(fā)明提供的一種提高六足機(jī)器人行走穩(wěn)定性的步態(tài)規(guī)劃方法，主要包括以下基 本動(dòng)作：
[0043] 行進(jìn):抬起不相鄰兩足，如圖1所示，開(kāi)始沿平行于機(jī)身中心和終點(diǎn)連線的每足末 端的平移路徑行進(jìn)，行進(jìn)時(shí)始終保證四足著地，不相鄰兩足抬起且每足的各關(guān)節(jié)按周期旋 轉(zhuǎn)到對(duì)應(yīng)預(yù)設(shè)角度，使每足末端在其平移路徑上抬起、落下，以便消除頓挫感，動(dòng)作流暢平 滑，按照此步態(tài)方案，六足末端狀態(tài)僅存在圖2、圖3所示兩種情況，其中空心圓表示抬起足 的末端，黑色填充圓表示著地足的末端；
[0044] 變向：機(jī)器人頭部攝像模塊旋轉(zhuǎn)到正對(duì)目標(biāo)終點(diǎn)方向，再執(zhí)行行進(jìn)基本動(dòng)作，不需 要機(jī)身進(jìn)行旋轉(zhuǎn)，從而保正機(jī)器人機(jī)身正面方向穩(wěn)定一致，對(duì)于有攝像頭功能的機(jī)器人來(lái) 說(shuō)，避免視頻畫(huà)面的劇烈晃動(dòng)，在本發(fā)明中，機(jī)器人頭部可進(jìn)行360度旋轉(zhuǎn)。
[0045] 在本發(fā)明中，行進(jìn)時(shí)始終保證四足著地，不相鄰兩足抬起包括交替抬起不相鄰的 兩足和不相鄰的兩足交替抬起兩種步態(tài)，其中，交替抬起不相鄰的兩足為:行進(jìn)開(kāi)始后，抬 起的兩足一起落地，著地、不相鄰的兩足一起抬起;不相鄰的兩足交替抬起為:行進(jìn)開(kāi)始后， 抬起的一足落地，同時(shí)著地的四足中一足抬起，且該抬起足與原抬起足不相鄰。
[0046] 在本發(fā)明中，每足末端的平移路徑計(jì)算方式為:利用變換算法建立每足的足坐標(biāo) 系與機(jī)身坐標(biāo)系的對(duì)應(yīng)關(guān)系，通過(guò)機(jī)身中心平移路徑，計(jì)算出每足末端的平移路徑（當(dāng)機(jī)器 人由一個(gè)坐標(biāo)移動(dòng)到另一個(gè)坐標(biāo)點(diǎn)時(shí)，從俯視角度來(lái)看，機(jī)器人六足末端平移路徑為直線 路徑，每足末端在該直線上，抬起、落下），這里以在標(biāo)準(zhǔn)站立姿態(tài)下各足末端的位置為參 照，計(jì)算每足末端的平移路徑，在標(biāo)準(zhǔn)站立姿態(tài)下，足末端和機(jī)身中心的相對(duì)位置是固定 的，所以在機(jī)身位于任意位置時(shí)，都能算出各足末端的坐標(biāo)，也就能通過(guò)機(jī)身中心平移路徑 計(jì)算出各足末端的平移路徑。
[0047] 機(jī)身中心平移路徑的計(jì)算方法為:在機(jī)身坐標(biāo)系中，將機(jī)身中心映射到經(jīng)過(guò)目標(biāo) 終點(diǎn)垂直于地面的直線上，通過(guò)目標(biāo)終點(diǎn)坐標(biāo)得到機(jī)身映射中心坐標(biāo)，機(jī)身中心與機(jī)身映 射中心連線即為機(jī)