[發明專利]一種帶有導臂的履帶式移動機器人自主上下樓梯控制方法

有效

• 申請號: 201410243459.4
• 申請日: 2014-06-03
• 公開（公告）號: CN104002880A
• 公開（公告）日: 2014-08-27
• 發明（設計）人: 宋愛國;紀鵬;錢夔;崔建偉;宋光明;郝小蕾;張立云
• 申請（專利權）人: 東南大學
• 主分類號: B62D55/075
• 分類號: B62D55/075
• 代理公司: 南京瑞弘專利商標事務所(普通合伙) 32249
• 代理人: 楊曉玲

1.一種帶有導臂的履帶式移動機器人自主上下樓梯控制方法，其特征在于，該方法首先在履帶式移動機器人車體頂面的前端設置面向前進方向的雙目視覺傳感器，在履帶式移動機器人的導臂主動輪的旋轉軸兩端分別設置一個用于檢測導臂力矩的三軸力傳感器，上揚導臂至與地面夾角成β度，其中β為移動機器人的最大爬坡度，然后進行自主上樓和下樓控制流程；

所述自主上樓控制流程中，雙目視覺傳感器始終保持水平方向并實時對視野范圍內的樓梯進行三維模型重建，包括以下步驟：

步驟A1：移動機器人將頭部對準并駛向第一級臺階的中間處，實時檢測雙目視覺傳感器中心分別與第一級踢面和第二級踢面的水平距離L1、L2，滿足L1≤S時，移動機器人停止前進，其中S為導臂的長度，然后根據此時檢測到的L1、L2，按照下式計算出樓梯傾斜角度θ：

θ＝(180*arctan(H/W))/π，

其中H為通過三維模型計算出的第一級臺階踏步的高度，W為踏步的寬度，W＝L2-L1；

將樓梯的傾斜角度θ與移動機器人的最大爬坡度β作對比，如果θ≤β，則接受并確認上樓指令，進入步驟A2；否則駁回上樓指令，并發送警告信息給監控端的操作人員后結束自主上樓控制流程；

步驟A2：上揚導臂至與地面夾角成θ度，驅動移動機器人前進，實時檢測兩個三軸力傳感器的力矩值，當兩個力矩值的正負號同時發生反轉且兩個力矩值相等時，認為移動機器人完全與樓梯對正，進入步驟A3；當只有一個三軸力傳感器檢測到力矩值正負號發生反轉時，停止前進，然后驅動正負號未發生反轉的一側的履帶向前轉動，直至兩個力矩值相等時履帶停止轉動，進入步驟A3；

步驟A3：驅動移動機器人前進，直到兩個三軸力傳感器檢測到的力矩值的正負號同時發生反轉時，停止前進，放下導臂，當兩個力矩值的正負號再次同時發生反轉并且等到導臂履帶底面與車體履帶底面共面后，導臂停止下放，進入步驟A4；

步驟A4：驅動移動機器人繼續前進，同時不斷調整左右履帶的速度差，始終保持前進方向與離車體前端最近一級臺階的踏步前緣垂直，當兩個三軸力傳感器檢測到的力矩值正負號同時反轉時，停止前進，進入步驟A5；

步驟A5：放下導臂，當兩個三軸力傳感器檢測到的力矩值的正負號同時反轉時，停止放下導臂，驅動移動機器人前進，直至兩個三軸力傳感器檢測到力矩值的正負號連續兩次同時發生反轉時，上揚導臂，同時保持移動機器人繼續前進，直至導臂上揚至與地面夾角為β度，完成自主上樓過程；

所述自主下樓控制流程中，雙目視覺傳感器始終保持垂直朝向地面并實時對視野范圍內的樓梯進行三維模型重建，包括以下步驟：

步驟B1：移動機器人下放導臂至導臂履帶底面與車體履帶底面共面，將頭部對準并駛向樓梯最頂一級臺階的踏步前緣的中點，實時檢測雙目視覺傳感器中心與地面的垂直距離K，滿足K>T時，停止前進，其中T為雙目視覺傳感器的中心與移動機器人車體履帶底面的垂直距離；然后根據此時的K、T和W，按照下式計算樓梯的傾斜角度θ：

θ＝(180*arctan(H/W))/π，

其中H為通過K和T得到的踏步的高度，H＝K-T，W為根據三維模型計算出的樓梯的踏步的寬度W；

將樓梯的傾斜角度θ與移動機器人的最大爬坡度β作對比，如果θ≤β，則接受并確認下樓指令，進入步驟B2；否則駁回下樓指令，并發送警告信息給監控端的操作人員后結束自主下樓控制流程；

步驟B2：驅動移動機器人前進，當兩個三軸力傳感器檢測到力矩值的正負號同時反轉且兩個力矩值相等時，停止前進，進入步驟B3；當只有一個三軸力傳感器檢測到的力矩值的正負號反轉時，移動機器人停止前進，驅動力矩值的正負號未發生反轉的一側的履帶向前轉動，直到兩個三軸力傳感器檢測到的力矩值相等時，移動機器人停止移動，進入步驟B3；

步驟B3：放下導臂，當兩個三軸力傳感器均檢測到力矩值的正負號同時發生反轉時，導臂停止下放，驅動移動機器人前進，當兩個三軸力傳感器檢測到力矩值正負號連續兩次同時發生反轉時停止前進，進入步驟B4；

步驟B4：上揚導臂直到導臂履帶底面與移動機器人車體履帶底面平行時停止，移動機器人前進，同時不斷調整左右履帶轉動的速度差，始終保持前進方向與離車體前端最近一級臺階的踏步前緣垂直，直至雙目視覺傳感器檢測不到臺階的邊緣線時，上揚導臂，同時保持移動機器人繼續向前移動，直至導臂上揚至與地面夾角為β度，完成自主下樓梯過程。