專利名稱：帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的攀爬樓梯控制方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人技術(shù)領(lǐng)域，是一種針對(duì)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人如何 在運(yùn)動(dòng)過程中遇到樓梯并自主攀爬樓梯的問題提出的一種新的控制方法。
背景技術(shù)：
機(jī)器人技術(shù)因其涵蓋了機(jī)械學(xué)、計(jì)算機(jī)科學(xué)、人工智能、生物學(xué)等領(lǐng)域，成 為了當(dāng)今世界最前沿的科技熱點(diǎn)之一。移動(dòng)機(jī)器人是機(jī)器人技術(shù)領(lǐng)域的一個(gè)重要 分支，移動(dòng)機(jī)器人通常是作為人類身體與感知的延伸，代替人類進(jìn)入危險(xiǎn)環(huán)境開 展工作，而人類通過無(wú)線或有線的方式對(duì)機(jī)器人進(jìn)行遠(yuǎn)程控制。由于其具有其他 機(jī)器人所不具有的"移動(dòng)性"，正越來(lái)越多的被應(yīng)用于危險(xiǎn)環(huán)境監(jiān)測(cè)、未知區(qū)域 探測(cè)、行星探測(cè)、救援搜索、排爆等領(lǐng)域。而在各種應(yīng)用中，常常要面對(duì)攀爬樓 梯的問題，由于樓梯結(jié)構(gòu)的特殊性和攀爬樓梯過程的復(fù)雜性使得移動(dòng)機(jī)器人攀爬 樓梯問題一直是移動(dòng)機(jī)器人領(lǐng)域的一個(gè)難點(diǎn)和熱點(diǎn)。
目前對(duì)移動(dòng)機(jī)器人的控制方法最主要的是遙操作控制方法，即機(jī)器人將所處 環(huán)境的物理信息傳回控制端，由操作員根據(jù)現(xiàn)場(chǎng)的狀況做出判斷，控制機(jī)器人運(yùn) 動(dòng)。在遙操作控制方式下，機(jī)器人的運(yùn)動(dòng)完全取決與操作員的控制，只是單純的 操作員肢體的延伸，由于操作員僅能依靠機(jī)器人搭載的傳感器采集的環(huán)境信息做 判斷來(lái)控制機(jī)器人，臨場(chǎng)感差，往往會(huì)給出錯(cuò)誤的控制命令。同時(shí)在遙操作方式 下，操作員始終要觀察處理機(jī)器人傳回的環(huán)境信息并操縱機(jī)器人，精神高度集中， 很容易身心疲勞，通常無(wú)法長(zhǎng)時(shí)間工作。隨著人工智能技術(shù)和計(jì)算機(jī)技術(shù)的發(fā)展， 移動(dòng)機(jī)器人本身的智能化、自主化成為了目前該領(lǐng)域的發(fā)展趨勢(shì)，但是移動(dòng)機(jī)器 人攀爬樓梯是一個(gè)復(fù)雜的過程，有很多不確定因素，使得各種算法都有其巨大的 局限性，同時(shí)由于移動(dòng)機(jī)器人工作環(huán)境的非固定性和非結(jié)構(gòu)化，使得自主算法的 可靠性有待提高。

發(fā)明內(nèi)容
本發(fā)明提供一種具有攀爬最大坡度為45°樓梯能力的帶^導(dǎo)臂的履帶式移 動(dòng)機(jī)器人的攀爬樓梯控制方法，本發(fā)明能夠使機(jī)器人運(yùn)行穩(wěn)定可靠。 本發(fā)明采用如下技術(shù)方案-
一種帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的樓梯攀爬方法，所述帶有導(dǎo)臂的履帶式 移動(dòng)機(jī)器人包括前輪和后輪，前輪為驅(qū)動(dòng)輪，后輪為隨動(dòng)輪，左右兩側(cè)的前、后輪分別被兩條履帶包覆，組成機(jī)器人的行進(jìn)機(jī)構(gòu)，與機(jī)器人前輪共軸安裝有履帶 式導(dǎo)臂機(jī)構(gòu)，并以前輪軸為轉(zhuǎn)動(dòng)中心，沿機(jī)器人行進(jìn)方向做360度圓周旋轉(zhuǎn)，在 機(jī)器人的前方安裝有超聲測(cè)距傳感器和紅外測(cè)距傳感器，用于測(cè)量遙操作小型移 動(dòng)機(jī)器人與前方障礙物之間的距離，在機(jī)器人前輪軸安裝有力矩傳感器，用于測(cè) 量導(dǎo)臂對(duì)前輪軸產(chǎn)生的力矩，在機(jī)器人幾何中心安裝有兩維姿態(tài)傳感器，用于測(cè) 量機(jī)器人的俯仰角度和橫滾角度，在機(jī)器人的后部裝有廣角攝像機(jī)，用于拍攝到
機(jī)器人前方的畫面，其特征在于
歩驟l:將移動(dòng)機(jī)器人的車頭對(duì)準(zhǔn)樓梯并駛向樓梯，利用帶有導(dǎo)臂的履帶式 移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅外測(cè)距傳感器測(cè)量機(jī)器人距前方樓梯的距
離S，當(dāng)S〈L-D時(shí)，其中，L為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人導(dǎo)臂的長(zhǎng)度，D為 帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人前輪的直徑，機(jī)器人停止前進(jìn)，導(dǎo)臂向前旋轉(zhuǎn)，在 與樓梯踏步接觸后繼續(xù)向前旋轉(zhuǎn)，導(dǎo)致車體被抬起，當(dāng)導(dǎo)臂履帶下側(cè)與車體履帶 下側(cè)位于同一平面上時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人開始向前 運(yùn)動(dòng)攀爬樓梯，當(dāng)設(shè)置于帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅 外測(cè)距傳感器測(cè)得的機(jī)器人與前方水平方向上的物體之間的距離大于L且L-樓 梯踏歩的水平寬度時(shí)，認(rèn)為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的前部已攀爬至樓梯頂 部，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停止攀爬。
步驟2:導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，在導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生
轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢測(cè)到轉(zhuǎn)矩M大于設(shè)定值且該設(shè)定值大于1 牛-米時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù)向上攀爬，導(dǎo)臂隨機(jī)器人的攀爬而脫離與 地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn)矩隨著消失，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停 止攀爬。
步驟3:重復(fù)步驟2， 二維姿態(tài)傳感器測(cè)量到的車體與水平面夾角cp也隨機(jī) 器人攀爬而減小，此時(shí)，機(jī)器人繼續(xù)攀爬至所述夾角(p不再減小時(shí)，停止攀爬， 導(dǎo)臂向后旋轉(zhuǎn)，直到與車體的夾角e為45度時(shí)停止轉(zhuǎn)動(dòng)。
與現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點(diǎn)
本發(fā)明針對(duì)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人系統(tǒng)提出了一種自主控制方法。本 發(fā)明的要點(diǎn)是(1)利用自身攜帶的超聲測(cè)距傳感器和紅外測(cè)距傳感器對(duì)環(huán)境的
感知，通過控制機(jī)器人導(dǎo)臂搭載第一級(jí)樓梯的踏歩上并向下旋轉(zhuǎn)至特定角度e，
將機(jī)器人的車體抬起，實(shí)現(xiàn)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人從水fF地面到樓梯這-
攀爬過程，并使這一過程平穩(wěn)可靠。(2)利用機(jī)器人搭載的二維姿態(tài)傳感器來(lái)檢 測(cè)機(jī)器人與樓梯之間的夾角，通過控制機(jī)器人左右履帶的前進(jìn)速度，使得二維姿 態(tài)傳感器檢測(cè)到的機(jī)器人橫滾角為零，即機(jī)器人垂直于樓梯向上爬行，使得帶有 導(dǎo)臂的履帶式移動(dòng)機(jī)器人攀爬過程穩(wěn)定可靠，不會(huì)發(fā)生傾覆。(3)當(dāng)機(jī)器人行進(jìn)
4至樓梯頂端時(shí)，導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，利用導(dǎo)臂轉(zhuǎn)軸上安裝 的轉(zhuǎn)矩傳感器測(cè)量在導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢 測(cè)到轉(zhuǎn)矩M大于設(shè)定值且該設(shè)定值大于1牛，米時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù) 向上攀爬，導(dǎo)臂隨機(jī)器人的攀爬而脫離與地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn)矩隨著消 失，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停止攀爬。如此往復(fù)。直到機(jī)器人的重心 完全越過最后一級(jí)臺(tái)階，此時(shí)導(dǎo)臂不會(huì)隨著機(jī)器人的前進(jìn)而與地面脫離，即導(dǎo)臂 旋轉(zhuǎn)軸上始終有大于給定值的轉(zhuǎn)矩存在，此時(shí)，二維姿態(tài)傳感器測(cè)量到的車體與 水平面夾角(p也隨機(jī)器人攀爬而減小。機(jī)器人繼續(xù)攀爬至所述夾角cp不再減小 時(shí)，停止攀爬，導(dǎo)臂向后旋轉(zhuǎn)，直到與車體的夾角e為45度時(shí)停止轉(zhuǎn)動(dòng)。這一 過程使得帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人在從樓梯平面到水平面的過程中始終保 持運(yùn)動(dòng)的平衡和平穩(wěn)，避免了剛性沖擊，使得機(jī)器人的運(yùn)動(dòng)安全可靠。
利用機(jī)器人搭載的超聲、紅外測(cè)距傳感器、扭矩傳感器和二維姿態(tài)傳感器的 測(cè)量結(jié)果，控制機(jī)器人的導(dǎo)臂旋轉(zhuǎn)，配合機(jī)器人車體的運(yùn)動(dòng)，實(shí)現(xiàn)帶有導(dǎo)臂的履 帶式移動(dòng)機(jī)器人從樓梯到水平地面行走過程的柔性化，使得這一過程平穩(wěn)，對(duì)機(jī) 器人的沖擊小。上述控制方法層次清晰，結(jié)構(gòu)簡(jiǎn)單，控制精確，運(yùn)行穩(wěn)定可靠。


圖l是本發(fā)明結(jié)構(gòu)示意圖。
圖2是本發(fā)明樓梯攀爬的初始階段示意圖，其中，圖2a是本發(fā)明攀爬樓梯 的起始狀態(tài)，圖2b是本發(fā)明攀上樓梯的示意圖。 圖3是本發(fā)明樓梯攀爬的過程示意圖。
圖4是本發(fā)明樓梯攀爬結(jié)束過程的示意圖。其中，圖4a是本發(fā)明攀爬至樓 梯頂端但重心未越過樓梯頂端階段示意圖，圖4b是本發(fā)明重心越過樓梯頂端階 段示意圖，圖4c是本發(fā)明完成攀爬階段示意圖。
圖5是本發(fā)明的一個(gè)具體實(shí)例示意圖。
具體實(shí)施例方式
一種帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的樓梯攀爬方法，所述帶有導(dǎo)臂的履帶式 移動(dòng)機(jī)器人包括前輪和后輪，前輪為驅(qū)動(dòng)輪，后輪為隨動(dòng)輪，左右兩側(cè)的前、后 輪分別被兩條履帶包覆，組成機(jī)器人的行進(jìn)機(jī)構(gòu)，與機(jī)器人前輪共軸安裝有履帶 式導(dǎo)臂機(jī)構(gòu)，并以前輪軸為轉(zhuǎn)動(dòng)中心，沿機(jī)器人行進(jìn)方向做360度圓周旋轉(zhuǎn)，在
機(jī)器人的前方安裝有超聲測(cè)距傳感器和紅外測(cè)距傳感器，用于測(cè)量遙操作小型移動(dòng)機(jī)器人與前方障礙物之間的距離，在機(jī)器人前輪軸安裝有力矩傳感器，用于測(cè) 量導(dǎo)臂對(duì)前輪軸產(chǎn)生的力矩，在機(jī)器人幾何中心安裝有兩維姿態(tài)傳感器，用于測(cè) 量機(jī)器人的俯仰角度和橫滾角度，在機(jī)器人的后部裝有廣角攝像機(jī)，用于拍攝到 機(jī)器人前方的畫面，其特征在于
歩驟l:將移動(dòng)機(jī)器人的車頭對(duì)準(zhǔn)樓梯并駛向樓梯，利用帶有導(dǎo)臂的履帶式 移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅外測(cè)距傳感器測(cè)量機(jī)器人距前方樓梯的距 離S，當(dāng)S〈L-D時(shí)，其中，L為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人導(dǎo)臂的長(zhǎng)度，D為 帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人前輪的直徑，機(jī)器人停止前進(jìn)，導(dǎo)臂向前旋轉(zhuǎn)，在 與樓梯踏步接觸后繼續(xù)向前旋轉(zhuǎn)，導(dǎo)致車體被抬起，當(dāng)導(dǎo)臂履帶下側(cè)與車體履帶 下側(cè)位于同一平面上時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人開始向前 運(yùn)動(dòng)攀爬樓梯，當(dāng)設(shè)置于帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅
外測(cè)距傳感器測(cè)得的機(jī)器人與前方水平方向上的物體之間的距離大于L且L^樓
梯踏歩的水平寬度時(shí)，認(rèn)為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的前部已攀爬至樓梯頂 部，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停止攀爬。
歩驟2:導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，在導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生
轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢測(cè)到轉(zhuǎn)矩M大于設(shè)定值且該設(shè)定值大于1
牛-米時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù)向上攀爬，導(dǎo)臂隨機(jī)器人的攀爬而脫離與 地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn)矩隨著消失，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停 止攀爬。
歩驟3:重復(fù)步驟2，直至設(shè)置于二維姿態(tài)傳感器測(cè)量到的車體與水平面夾
角cp也隨機(jī)器人攀爬而減小，此時(shí)，機(jī)器人繼續(xù)攀爬至所述夾角cp不再減小吋， 停止攀爬，導(dǎo)臂向后旋轉(zhuǎn)，直到與車體的夾角e為45度時(shí)停止轉(zhuǎn)動(dòng)。
下面參照附圖，對(duì)本發(fā)明做出更為具體的描述
本實(shí)施例是一種針對(duì)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人如何在運(yùn)動(dòng)過程中遇到
最大坡度為45°的樓梯并自主攀爬樓梯的問題提出的一種新的控制方法，所使 用的小型機(jī)器人系統(tǒng)如圖l所示，其中1為導(dǎo)臂，2為扭矩傳感器，3二維姿態(tài) 傳感器，4為機(jī)器人車體，5為廣角攝像機(jī)，6為操作員控制端。同時(shí)在車體前 端還裝有超聲和紅外測(cè)距傳感器。導(dǎo)臂可以繞轉(zhuǎn)動(dòng)軸360度旋轉(zhuǎn)，且與車體夾角 如圖所示為e，向上為正。
局部自主攀爬樓梯方法分為三個(gè)部分，開始攀爬樓梯階段，攀爬樓梯階段和 結(jié)束攀爬樓梯階段。
(1)開始攀爬樓梯階段
帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人向樓梯行近。在行進(jìn)過程中，機(jī)器人前方的超 聲和紅外測(cè)距傳感器實(shí)時(shí)測(cè)量機(jī)器人距樓梯立面的距離S，當(dāng)S滿足200910028019.6
其中丄為導(dǎo)臂的長(zhǎng)度，"為機(jī)器人前輪的直徑，機(jī)器人認(rèn)為已經(jīng)接近樓梯，開始 將導(dǎo)臂由初始位置即處于向上45°處向下轉(zhuǎn)動(dòng)，隨著導(dǎo)臂的轉(zhuǎn)動(dòng)，在與樓梯踏
步接觸后繼續(xù)向前旋轉(zhuǎn)，車身逐漸被抬起，此時(shí)導(dǎo)臂與車體的夾角e逐漸減小， 因?yàn)樵谂逝罉翘莸倪^程中機(jī)器人整體的長(zhǎng)度越長(zhǎng)越有利于攀爬，所以調(diào)整導(dǎo)臂與 車體的夾角為en使得導(dǎo)臂履帶的底部與車體下方的履帶在一條直線上，使機(jī)器 人與樓梯接觸面積最大。通過安裝在導(dǎo)臂旋轉(zhuǎn)軸上的角度傳感器測(cè)量得到導(dǎo)臂旋 轉(zhuǎn)的角度。隨著機(jī)器人繼續(xù)前進(jìn)，機(jī)器人所帶的二維姿態(tài)傳感器測(cè)量到機(jī)器人與
水平面的夾角cp逐漸增大，當(dāng)此夾角不再增大時(shí)，表明機(jī)器人已經(jīng)攀爬上了樓梯。
設(shè)機(jī)器人與水平面夾角最大為CPmax,如果9超過(Pmax時(shí)移動(dòng)機(jī)器人會(huì)向后翻倒。 當(dāng)(p達(dá)到CPmax時(shí)機(jī)器人停止攀爬，并向控制端發(fā)送信息，等待新的命令。其過 程如圖2所示。
另外在測(cè)量機(jī)器人距樓梯里面距離S時(shí)，由于超聲測(cè)距傳感器和紅外測(cè)距傳 感器的測(cè)量范圍和精度不同，所以采用以下的測(cè)量方法-
其中,,為紅外測(cè)距傳感器的測(cè)量值，S,為超聲測(cè)距傳感器的測(cè)量值，、《 為紅外
測(cè)距傳感器的最大測(cè)量值。由于紅外測(cè)距傳感器在距離較近時(shí)比較準(zhǔn)確，而超聲 測(cè)距傳感器在距離較遠(yuǎn)時(shí)比較準(zhǔn)確。所以當(dāng)紅外的測(cè)量值達(dá)到其最大測(cè)量值時(shí)， 就使用超聲測(cè)距傳感器的測(cè)量結(jié)果作為最終結(jié)果，反之則認(rèn)為紅外測(cè)距傳感器的 測(cè)量結(jié)果可信度高，并被采納為最終結(jié)果。
(2) 攀爬樓梯階段
當(dāng)機(jī)器人結(jié)束開始攀爬樓梯階段，爬上樓梯開始，就進(jìn)入攀爬樓梯階段。過 程如圖3所示。
在攀爬過程中為保證機(jī)器人的安全，要求機(jī)器人始終垂直與樓梯向上攀爬。 機(jī)器人所搭載的二維姿態(tài)傳感器實(shí)時(shí)檢測(cè)機(jī)器人與水平面的夾角。通過調(diào)整左右 履帶的速度，使得二維姿態(tài)傳感器檢測(cè)到的水平方向傾角為零，這樣就可以保證 機(jī)器人攀爬樓梯過程中始終與樓梯垂直。另外，在攀爬的過程屮，機(jī)器人前部的 紅外、超聲測(cè)距傳感器使用公式(2)中的方法不停檢測(cè)距前方樓梯立面的距離
s，確定機(jī)器人是否到達(dá)樓梯的盡頭。
(3) 結(jié)束攀爬樓梯階段
當(dāng)機(jī)器人搭載的廣角攝像機(jī)拍攝的圖像中沒有檢測(cè)到有樓梯，而且機(jī)器人前
7部的紅外、超聲測(cè)距傳感器測(cè)量的結(jié)果超出預(yù)設(shè)的樓梯最大寬度，則機(jī)器人認(rèn)為 已經(jīng)到達(dá)樓梯的盡頭，進(jìn)入結(jié)束攀爬樓梯階段。
進(jìn)入結(jié)束攀爬樓梯階段后，導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，在導(dǎo) 臂的旋轉(zhuǎn)軸上產(chǎn)生轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢測(cè)到轉(zhuǎn)矩M大于設(shè)定值
且該設(shè)定值大于1牛'米時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù)向上攀爬，導(dǎo)臂隨機(jī)器 人的攀爬而脫離與地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn)矩隨著消失，此時(shí)帶有導(dǎo)臂的履 帶式移動(dòng)機(jī)器人停止攀爬，導(dǎo)臂繼續(xù)向下轉(zhuǎn)動(dòng)，直到再次接觸地面并在轉(zhuǎn)動(dòng)軸上 產(chǎn)生一個(gè)大于設(shè)定值得轉(zhuǎn)矩。機(jī)器人不停地重復(fù)這一過程， 一直到機(jī)器人的重心
完全越過最后一級(jí)臺(tái)階。此過程如圖4 (a)所示。
當(dāng)機(jī)器人重心完全越過最后一級(jí)臺(tái)階后，導(dǎo)臂不會(huì)隨著機(jī)器人的前進(jìn)而與地 面脫離，所以導(dǎo)臂也不會(huì)再向下轉(zhuǎn)動(dòng)。在此情況下，二維姿態(tài)傳感器測(cè)量到的車 體與水平面夾角9也隨機(jī)器人運(yùn)動(dòng)而減小，當(dāng)夾角(p不再減小時(shí)，則表示機(jī)器 人車體已全部越過最后一級(jí)樓梯。此過程如圖4 (b)所示。然后，導(dǎo)臂向上轉(zhuǎn) 動(dòng)到正常行駛位置，完成整個(gè)局部自主攀爬樓梯過程。如圖4 (c)所示。
小型移動(dòng)機(jī)器人的遙操作局部半自主攀爬樓梯控制方法的實(shí)例如圖5所示 其中1為車體，2為廣角攝像機(jī)，3為二維姿態(tài)傳感器，4為扭矩傳感器，5 為導(dǎo)臂，6為指甲，7為紅外、超聲測(cè)距裝置，8為紅外測(cè)距傳感器，9為超聲測(cè) 距傳感器，10為履帶。其中導(dǎo)臂可以繞轉(zhuǎn)動(dòng)軸360度旋轉(zhuǎn)，且與車體夾角如圖 所示為6。左右履帶的速度為F,和^。廣角攝像機(jī)2固定在車體1的后部，二維
姿態(tài)傳感器3固定在車體1的重心位置，扭矩傳感器固定在導(dǎo)臂5的轉(zhuǎn)動(dòng)軸上， 紅外、超聲測(cè)距傳感器7通過支架6固定于車體1的前部，由于紅外、超聲測(cè)距 裝置的重心位于與支架6連接軸之下，所以當(dāng)車體與水平面角度變化時(shí)紅外測(cè)距 傳感器8和超聲測(cè)距傳感器9始終可以測(cè)量與水平面垂直的障礙物。
局部自主攀爬樓梯方法分為三個(gè)部分，開始攀爬樓梯階段，攀爬樓梯階段和 結(jié)束攀爬樓梯階段。
(1)開始攀爬樓梯階段
帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人向樓梯行近。在行進(jìn)過程中，機(jī)器人前方的超 生和紅外測(cè)距傳感器實(shí)時(shí)測(cè)量機(jī)器人距樓梯立面的距離S,當(dāng)S滿足
0<S<(i-￡)) (1)
其中丄40cm為導(dǎo)臂的長(zhǎng)度，》15cm為機(jī)器人前輪的直徑，機(jī)器人認(rèn)為已經(jīng)接 近樓梯，開始將導(dǎo)臂向下轉(zhuǎn)動(dòng)，隨著導(dǎo)臂的轉(zhuǎn)動(dòng)，在與樓梯踏歩接觸后繼續(xù)向前
旋轉(zhuǎn)，車身逐漸被抬起，此時(shí)前導(dǎo)與車體的夾角e逐漸減小，當(dāng)e減小為e產(chǎn)一
10°時(shí)導(dǎo)臂停止轉(zhuǎn)動(dòng)，此時(shí)導(dǎo)臂與車體在同一直線上。隨著機(jī)器人繼續(xù)前進(jìn)，機(jī)
8器人所帶的二維姿態(tài)傳感器測(cè)量到機(jī)器人與水平面的夾角<P逐漸增大，并最終保 持<p=40°時(shí)，表明機(jī)器人已經(jīng)攀爬上了樓梯，此時(shí)車體傾角與樓梯角度相同。
設(shè)機(jī)器人與水平面夾角最大為(Pmax^45。，如果9超過(Pmax時(shí)移動(dòng)機(jī)器人會(huì)向后 翻倒。當(dāng)(？達(dá)到(Pmax時(shí)機(jī)器人停止攀爬，并向操作員控制端發(fā)送信息，等待新 的命令。
另外在測(cè)量機(jī)器人距樓梯里面距離S時(shí)，由于超聲測(cè)距傳感器和紅外測(cè)距傳 感器的測(cè)量范圍和精度不同，所以采用以下的測(cè)量方法
其中&為紅外測(cè)距傳感器的測(cè)量值，^為超聲測(cè)距傳感器的測(cè)量值，slm =40Cm
為紅外測(cè)距傳感器的最大測(cè)量值。由于紅外測(cè)距傳感器在距離較近時(shí)比較準(zhǔn)確， 而超聲測(cè)距傳感器在距離較遠(yuǎn)時(shí)比較準(zhǔn)確。所以當(dāng)紅外的測(cè)量值達(dá)到其最大測(cè)量 值時(shí)，就使用超聲測(cè)距傳感器的測(cè)量結(jié)果作為最終結(jié)果，反之則認(rèn)為紅外測(cè)距傳 感器的測(cè)量結(jié)果可信度高，并被采納為最終結(jié)果。
(2) 攀爬樓梯階段 當(dāng)機(jī)器人結(jié)束開始攀爬樓梯階段，就進(jìn)入攀爬樓梯階段。
在攀爬過程中為保證機(jī)器人的安全，要求機(jī)器人始終垂直與樓梯向上攀爬。 機(jī)器人所搭載的二維姿態(tài)傳感器實(shí)時(shí)檢測(cè)機(jī)器人與水平面的夾角。通過調(diào)整左右 履帶的速度，使得二維姿態(tài)傳感器檢測(cè)到的水平方向傾角為零，這樣就可以保證 機(jī)器人攀爬樓梯過程中始終與樓梯垂直。另外，在攀爬的過程中，機(jī)器人前部的 紅外、超聲測(cè)距傳感器使用公式(2)中的方法不停檢測(cè)距前方樓梯立面的距離
S，通過判斷S是否小于Sn^來(lái)確定機(jī)器人是否到達(dá)樓梯的盡頭，其中Smax=30cm
為設(shè)定的樓梯踏步最大寬度。
(3) 結(jié)束攀爬樓梯階段
當(dāng)機(jī)器人搭載的廣角攝像機(jī)拍攝的圖像中沒有檢測(cè)到有樓梯，而且機(jī)器人前
部的紅外、超聲測(cè)距傳感器測(cè)量的結(jié)果超出預(yù)設(shè)的樓梯踏歩最大寬度smax，則機(jī)
器人認(rèn)為已經(jīng)到達(dá)樓梯的盡頭，進(jìn)入結(jié)束攀爬樓梯階段。
進(jìn)入結(jié)束攀爬樓梯階段后，首先導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，
在導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢測(cè)到轉(zhuǎn)矩M大于設(shè)
定值1牛.米時(shí)，停止轉(zhuǎn)動(dòng)。機(jī)器人繼續(xù)向上攀爬，由于此時(shí)機(jī)器人重心并未越 過最后一級(jí)臺(tái)階，導(dǎo)臂將隨機(jī)器人的攀爬而脫離與地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn) 矩隨著消失，此時(shí)導(dǎo)臂向下轉(zhuǎn)動(dòng)，直到再次接觸地面并在轉(zhuǎn)動(dòng)軸上產(chǎn)生--個(gè)大于 設(shè)定值1牛'米的轉(zhuǎn)矩。機(jī)器人不停的重復(fù)這一過程， 一直到機(jī)器人的重心完全
9越過最后一級(jí)臺(tái)階。
當(dāng)機(jī)器人重心完全越過最后一級(jí)臺(tái)階后，導(dǎo)臂不會(huì)隨著機(jī)器人的前進(jìn)而與地 面脫離，所以導(dǎo)臂也不會(huì)再向下轉(zhuǎn)動(dòng)。在此情況下，二維姿態(tài)傳感器測(cè)量到的車 體與水平面夾角cp也隨機(jī)器人運(yùn)動(dòng)而減小，當(dāng)夾角9不再減小時(shí)，則表示機(jī)器 人車體己全部越過最后一級(jí)樓梯。然后，導(dǎo)臂向上轉(zhuǎn)動(dòng)到正常行駛位置，完成整 個(gè)局部自主攀爬樓梯過程。
權(quán)利要求
1. 一種帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的樓梯攀爬方法，所述帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人包括前輪和后輪，前輪為驅(qū)動(dòng)輪，后輪為隨動(dòng)輪，左右兩側(cè)的前、后輪分別被兩條履帶包覆，組成機(jī)器人的行進(jìn)機(jī)構(gòu)，與機(jī)器人前輪共軸安裝有履帶式導(dǎo)臂機(jī)構(gòu)，并以前輪軸為轉(zhuǎn)動(dòng)中心，沿機(jī)器人行進(jìn)方向做360度圓周旋轉(zhuǎn)，在機(jī)器人的前方安裝有超聲測(cè)距傳感器和紅外測(cè)距傳感器，用于測(cè)量遙操作小型移動(dòng)機(jī)器人與前方障礙物之間的距離，在機(jī)器人前輪軸安裝有力矩傳感器，用于測(cè)量導(dǎo)臂對(duì)前輪軸產(chǎn)生的力矩，在機(jī)器人幾何中心安裝有兩維姿態(tài)傳感器，用于測(cè)量機(jī)器人的俯仰角度和橫滾角度，在機(jī)器人的后部裝有廣角攝像機(jī)，用于拍攝到機(jī)器人前方的畫面，其特征在于步驟1將移動(dòng)機(jī)器人的車頭對(duì)準(zhǔn)樓梯并駛向樓梯，利用帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅外測(cè)距傳感器測(cè)量機(jī)器人距前方樓梯的距離S，當(dāng)S<L-D時(shí)，其中，L為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人導(dǎo)臂的長(zhǎng)度，D為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人前輪的直徑，機(jī)器人停止前進(jìn)，導(dǎo)臂向前旋轉(zhuǎn)，在與樓梯踏步接觸后繼續(xù)向前旋轉(zhuǎn)，導(dǎo)致車體被抬起，當(dāng)導(dǎo)臂履帶下側(cè)與車體履帶下側(cè)位于同一平面上時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人開始向前運(yùn)動(dòng)攀爬樓梯，當(dāng)設(shè)置于帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人上的超聲測(cè)距傳感器和紅外測(cè)距傳感器測(cè)得的機(jī)器人與前方水平方向上的物體之間的距離大于L且L＝樓梯踏步的水平寬度時(shí)，認(rèn)為帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的前部已攀爬至樓梯頂部，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停止攀爬。步驟2導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，在導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生轉(zhuǎn)矩M，當(dāng)旋轉(zhuǎn)軸上的轉(zhuǎn)矩傳感器檢測(cè)到轉(zhuǎn)矩M大于設(shè)定值且該設(shè)定值大于1牛米時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù)向上攀爬，導(dǎo)臂隨機(jī)器人的攀爬而脫離與地面的接觸，導(dǎo)臂轉(zhuǎn)軸上的轉(zhuǎn)矩隨著消失，此時(shí)帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人停止攀爬。步驟3重復(fù)步驟2，此過程中二維姿態(tài)傳感器測(cè)量到的車體與水平面夾角也隨機(jī)器人攀爬而減小，此時(shí)，機(jī)器人繼續(xù)攀爬至所述夾角不再減小時(shí)，停止攀爬，導(dǎo)臂向后旋轉(zhuǎn)，直到與車體的夾角θ為45度時(shí)停止轉(zhuǎn)動(dòng)。
全文摘要
帶有導(dǎo)臂的履帶式移動(dòng)機(jī)器人的樓梯攀爬方法，其特征在于步驟1機(jī)器人駛近樓梯，導(dǎo)臂向下旋轉(zhuǎn)，在與樓梯踏步接觸后機(jī)器人被抬起，并向前攀爬樓梯，當(dāng)機(jī)器人上的超聲、紅外測(cè)距傳感器測(cè)得機(jī)器人與前方物體距離大于樓梯踏步的寬度時(shí)，認(rèn)為機(jī)器人已攀爬至樓梯頂部，停止攀爬。步驟2導(dǎo)臂向下旋轉(zhuǎn)，直到導(dǎo)臂前端與地面接觸，當(dāng)導(dǎo)臂的旋轉(zhuǎn)軸上產(chǎn)生轉(zhuǎn)矩M大于設(shè)定值時(shí)，導(dǎo)臂停止轉(zhuǎn)動(dòng)，機(jī)器人繼續(xù)向上攀爬，導(dǎo)臂隨機(jī)器人的攀爬而與地面脫離，轉(zhuǎn)矩M消失，機(jī)器人停止攀爬。步驟3重復(fù)步驟2，二維姿態(tài)傳感器測(cè)得的車體與水平面夾角φ隨機(jī)器人攀爬而減小，機(jī)器人繼續(xù)攀爬至所述夾角φ不再減小時(shí)，停止攀爬，導(dǎo)臂向后旋轉(zhuǎn)，與車體的夾角θ為45度時(shí)停止轉(zhuǎn)動(dòng)。
文檔編號(hào)B62D55/00GK101486360SQ200910028019
公開日2009年7月22日 申請(qǐng)日期2009年1月5日 優(yōu)先權(quán)日2009年1月5日
發(fā)明者包加桐, 唐鴻儒, 宋愛國(guó), 屈傳坤, 崔建偉, 彥 曹, 晏 郭, 韓益利 申請(qǐng)人:東南大學(xué)