[發(fā)明專利]一種機(jī)器人在矩形斜坡上行駛的路徑導(dǎo)航控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201610836025.4
• 申請(qǐng)日: 2016-09-21
• 公開（公告）號(hào): CN106584454B
• 公開（公告）日: 2019-03-08
• 發(fā)明（設(shè)計(jì)）人: 彭芳;周艷榮;徐建榮
• 申請(qǐng)（專利權(quán)）人: 蘇州瑞得恩光能科技有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16;G05D1/02
• 代理公司: 蘇州市華揚(yáng)翼晟知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 32275
• 代理人: 李焱

1.一種機(jī)器人在矩形斜坡上行駛的路徑導(dǎo)航控制方法，其特征在于，包括如下步驟：

步驟S401）設(shè)定所述矩形斜坡的右下角為導(dǎo)航起點(diǎn)；

步驟S402）控制所述機(jī)器人從所述導(dǎo)航起點(diǎn)向所述矩形斜坡的右上角直線行駛；

步驟S403）檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的第一角落；若所述機(jī)器人未到達(dá)所述第一角落，返回步驟S402）；若所述機(jī)器人到達(dá)所述第一角落，控制所述機(jī)器人向左進(jìn)行U字回轉(zhuǎn)，回轉(zhuǎn)后控制所述機(jī)器人直線行駛；

步驟S404）檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的邊緣處，若所述機(jī)器人到達(dá)所述矩形斜坡的一邊緣處；控制所述機(jī)器人向右進(jìn)行U字回轉(zhuǎn)，回轉(zhuǎn)后控制所述機(jī)器人直線行駛，執(zhí)行步驟S405）；以及

步驟S405）檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的邊緣處，若所述機(jī)器人到達(dá)所述矩形斜坡的一邊緣處；控制所述機(jī)器人向左進(jìn)行U字回轉(zhuǎn)，回轉(zhuǎn)后控制所述機(jī)器人直線行駛；返回步驟S404）；

在步驟S404）和步驟S405）的執(zhí)行過程中，實(shí)時(shí)檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的第二角落；若所述機(jī)器人到達(dá)所述第二角落，實(shí)時(shí)檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的第三角落；若所述機(jī)器人到達(dá)所述第三角落，控制所述機(jī)器人停止行駛；

其中，控制所述機(jī)器人直線行駛的步驟，包括

判斷一機(jī)器人是否沿著預(yù)設(shè)的直線路徑行駛的步驟，具體包括如下步驟：

步驟S1）在所述機(jī)器人上建立三維坐標(biāo)系，定義所述機(jī)器人行進(jìn)方向?yàn)閅軸正方向，定義垂直于斜坡平面的方向?yàn)閆軸方向，X軸垂直于所述Y軸及所述Z軸；所述X軸與所述Y軸所處平面與所述斜坡平面平行，所述斜坡平面為所述矩形斜坡所處平面；

步驟S2）定義所述機(jī)器人行進(jìn)方向?yàn)門s時(shí)，重力加速度g在所述三維坐標(biāo)系三個(gè)方向上的標(biāo)準(zhǔn)分向量g_xs0、g_ys0、g_zs0；

步驟S3）生成一標(biāo)準(zhǔn)方向參數(shù)庫；

步驟S4）控制所述機(jī)器人在所述斜坡平面上沿著預(yù)設(shè)的一直線路徑向任一方向Tm直線行駛；

步驟S5）從所述標(biāo)準(zhǔn)方向參數(shù)庫中調(diào)取對(duì)應(yīng)該行進(jìn)方向Tm的標(biāo)準(zhǔn)分向量g_xm0、g_ym0、g_zm0數(shù)據(jù)；

步驟S6）每隔一定時(shí)間間隔t實(shí)時(shí)采集一組實(shí)時(shí)方向參數(shù)，所述實(shí)時(shí)方向參數(shù)包括重力加速度g在所述三維坐標(biāo)系三個(gè)方向上的實(shí)時(shí)分向量g_xm1、g_ym1、g_zm1；

步驟S7）計(jì)算重力加速度g在所述X軸方向上的實(shí)時(shí)分向量與標(biāo)準(zhǔn)分向量的分向量差值g_xd =g_xm1 -g_xm0 ；

步驟S8）判定所述機(jī)器人是否沿著預(yù)設(shè)的直線路徑行駛；當(dāng)g_xd等于0時(shí)，判定所述機(jī)器人沿著預(yù)設(shè)的直線路徑行駛，返回步驟S106）；當(dāng)g_xd不等于0時(shí)，判定所述機(jī)器人偏離預(yù)設(shè)的直線路徑。

2.如權(quán)利要求1所述的機(jī)器人在矩形斜坡上行駛的路徑導(dǎo)航控制方法，其特征在于，檢測(cè)所述機(jī)器人是否行進(jìn)至所述矩形斜坡的一角落或一邊緣處，具體包括如下步驟：

步驟S1011）在所述機(jī)器人的左前部、右前部、左后部及右后部分別設(shè)置一距離傳感器，所述距離傳感器延伸至所述機(jī)器人的外部，所述距離傳感器朝向所述矩形斜坡；

步驟S1012）依次為四個(gè)距離傳感器編號(hào)，將所述機(jī)器人的左前部、右前部、左后部及右后部設(shè)置的距離傳感器分別定義為傳感器N1、傳感器N2、傳感器N3及傳感器N4；

步驟S1013）所述機(jī)器人根據(jù)任一時(shí)刻同時(shí)獲取的傳感器信號(hào)判斷所述機(jī)器人的位置；當(dāng)所述機(jī)器人同時(shí)獲取傳感器N3信號(hào)和傳感器N4信號(hào)時(shí)，判定所述機(jī)器人到達(dá)所述矩形斜坡的一邊緣處；當(dāng)所述機(jī)器人只能獲取傳感器N4信號(hào)時(shí)，判定所述機(jī)器人到達(dá)所述矩形斜坡的第一角落或第二角落；當(dāng)所述機(jī)器人只能獲取傳感器N3信號(hào)時(shí)，判定所述機(jī)器人到達(dá)所述矩形斜坡的第三角落或第四角落。