[發(fā)明專利]多凸包障礙模型下移動(dòng)機(jī)器人避障的路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910593814.3
• 申請(qǐng)日: 2019-07-03
• 公開（公告）號(hào): CN110160533B
• 公開（公告）日: 2022-02-25
• 發(fā)明（設(shè)計(jì)）人: 劉莉;何慧娟;賈文友;梁利東;江本赤;蘇學(xué)滿;石平;賈昊瑞
• 申請(qǐng)（專利權(quán)）人: 安徽工程大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G05D1/02
• 代理公司: 安徽匯樸律師事務(wù)所 34116
• 代理人: 劉海涵

1.多凸包障礙模型下移動(dòng)機(jī)器人避障的路徑規(guī)劃方法，包括原始起點(diǎn)、移動(dòng)機(jī)器人、不規(guī)則障礙物、凸包障礙模型、當(dāng)前起點(diǎn)、凸包頂點(diǎn)、終點(diǎn)、單點(diǎn)凸包障礙模型、凸包頂點(diǎn)信息、前繼點(diǎn)、標(biāo)記距離、被記錄、障礙線段集合、求解當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)、當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的標(biāo)記距離更新，移動(dòng)機(jī)器人工作場(chǎng)平面設(shè)置為XOY平面，其特征在于，原始起點(diǎn)是指移動(dòng)機(jī)器人工作場(chǎng)的最初起始的位置；移動(dòng)機(jī)器人工作場(chǎng)所有多個(gè)障礙物，且為不規(guī)則障礙物，基于凸包原理建立每個(gè)不規(guī)則障礙物在XOY平面內(nèi)凸包障礙模型，每個(gè)凸包障礙模型是個(gè)凸多邊形，每個(gè)凸多邊形的頂點(diǎn)稱為凸包頂點(diǎn)；終點(diǎn)是指移動(dòng)機(jī)器人工作場(chǎng)的最終到達(dá)的位置，終點(diǎn)設(shè)置為XOY平面內(nèi)的單點(diǎn)凸包障礙模型，終點(diǎn)屬于凸包頂點(diǎn)；凸包頂點(diǎn)信息是指凸包頂點(diǎn)的儲(chǔ)存信息，儲(chǔ)存信息設(shè)置為五維數(shù)組，五維數(shù)組包括X軸坐標(biāo)位置、Y軸坐標(biāo)位置、前繼點(diǎn)、標(biāo)記距離和是否被記錄五項(xiàng)，前繼點(diǎn)是到達(dá)該凸包頂點(diǎn)的前一個(gè)點(diǎn)，標(biāo)記距離是指從原始起點(diǎn)至該凸包頂點(diǎn)的路徑距離之和，被記錄是指該凸包頂點(diǎn)被設(shè)置為過(guò)當(dāng)前起點(diǎn)；障礙線段集合是指凸包障礙模型邊界線段和同一凸包障礙模型間隔一個(gè)凸包頂點(diǎn)的兩個(gè)凸包頂點(diǎn)之間線段；多凸包障礙模型下移動(dòng)機(jī)器人避障的路徑規(guī)劃方法，具體流程如下：

步驟1.1，原始起點(diǎn)的初始化，原始起點(diǎn)的標(biāo)記距離設(shè)置為0，原始起點(diǎn)設(shè)置為當(dāng)前起點(diǎn)；

步驟1.2，凸包障礙模型的構(gòu)建，障礙物包括規(guī)則和不規(guī)則障礙物，規(guī)則障礙物直接在移動(dòng)機(jī)器人工作場(chǎng)的XOY平面形成凸包障礙模型，不規(guī)則障礙物依據(jù)凸包算法在移動(dòng)機(jī)器人工作場(chǎng)平面的XOY平面構(gòu)建凸包障礙模型，每個(gè)凸包障礙模型是個(gè)凸多邊形，每個(gè)凸多邊形的頂點(diǎn)稱為凸包頂點(diǎn)；終點(diǎn)構(gòu)建為移動(dòng)機(jī)器人工作場(chǎng)的XOY平面內(nèi)單點(diǎn)的凸包障礙模型，終點(diǎn)屬于凸包頂點(diǎn)；

步驟1.3，凸包障礙模型的初始化，凸包點(diǎn)信息五維數(shù)組中的X軸坐標(biāo)位置和Y軸坐標(biāo)位置是凸包點(diǎn)在移動(dòng)機(jī)器人工作場(chǎng)平面的XOY平面的具體的位置坐標(biāo)，前繼點(diǎn)設(shè)置空，標(biāo)記距離設(shè)置無(wú)窮大，未被記錄；

步驟1.4，多凸包障礙模型下移動(dòng)機(jī)器人避障的路徑規(guī)劃方法的流程終止判斷，判斷條件為終點(diǎn)是否已被記錄，如果終點(diǎn)已被記錄，終點(diǎn)設(shè)置為過(guò)當(dāng)前起點(diǎn)，輸出終點(diǎn)信息中標(biāo)記距離，該標(biāo)記距離是移動(dòng)機(jī)器人工作場(chǎng)從原始起點(diǎn)到終點(diǎn)的最短距離；輸出最短距離對(duì)應(yīng)路徑，采用逆向方法，逆向路徑是指終點(diǎn)逆向至終點(diǎn)的前繼點(diǎn)，前繼點(diǎn)逆向至前繼點(diǎn)的前繼點(diǎn)，一直逆向至原始起點(diǎn)，該逆向路徑的反方向路徑就是最短距離對(duì)應(yīng)路徑；如果終點(diǎn)未被記錄，流程繼續(xù)往下執(zhí)行；

步驟1.5，求解當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)，獲得凸包障礙模型的凸包頂點(diǎn)相對(duì)當(dāng)前起點(diǎn)可見(jiàn)的所有凸包頂點(diǎn)；

求解當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)，具體流程如下：

步驟2.1，構(gòu)建障礙線段集合，由所有凸包障礙模型邊界線段和每一個(gè)凸包障礙模型中在該凸包障礙模型間隔一個(gè)凸包頂點(diǎn)的兩個(gè)凸包頂點(diǎn)之間線段組成集合體；

步驟2.2，該凸包頂點(diǎn)與當(dāng)前起點(diǎn)可見(jiàn)判斷，判斷條件為當(dāng)前起點(diǎn)與凸包頂點(diǎn)連線與障礙線段集合中線段是否相交，如果當(dāng)前起點(diǎn)與凸包頂點(diǎn)連線與障礙線段集合中線段有相交，則該凸包頂點(diǎn)與當(dāng)前起點(diǎn)不可見(jiàn)；如果當(dāng)前起點(diǎn)與凸包頂點(diǎn)連線與障礙線段集合中線段沒(méi)有相交，則該凸包頂點(diǎn)與當(dāng)前起點(diǎn)可見(jiàn)；

步驟2.3，求解當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的流程終止判斷，判斷條件為是否遍歷每個(gè)凸包障礙模型的所有頂點(diǎn)，如果已遍歷每個(gè)凸包障礙模型的所有頂點(diǎn)，則流程終止；如果未遍歷每個(gè)凸包障礙模型的所有頂點(diǎn)，流程返回到步驟2.2；

步驟1.6，當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的標(biāo)記距離更新，遍歷相對(duì)當(dāng)前起點(diǎn)可見(jiàn)的所有凸包頂點(diǎn)；

當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的標(biāo)記距離更新，具體流程如下：

步驟3.1，計(jì)算當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的路徑距離和，方法是當(dāng)前起點(diǎn)與凸包障礙模型的可見(jiàn)凸包頂點(diǎn)之間距離加上當(dāng)前起點(diǎn)的標(biāo)記距離的距離和；

步驟3.2，凸包頂點(diǎn)的標(biāo)記距離更新判斷，判斷條件為當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的路徑距離和與該凸包頂點(diǎn)的標(biāo)記距離對(duì)比結(jié)果，如果對(duì)比結(jié)果是當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的路徑距離和小于該凸包頂點(diǎn)的標(biāo)記距離，則更新凸包頂點(diǎn)的標(biāo)記距離和前繼點(diǎn)；如果對(duì)比結(jié)果是當(dāng)前起點(diǎn)的可見(jiàn)凸包頂點(diǎn)的路徑距離和不是小于該凸包頂點(diǎn)的標(biāo)記距離，則保留原有凸包頂點(diǎn)的標(biāo)記距離和前繼點(diǎn)的信息；流程終止；

步驟1.7，當(dāng)前起點(diǎn)更新，未記錄的凸包頂點(diǎn)中標(biāo)記距離最小設(shè)置為當(dāng)前起點(diǎn)，返回步驟1.4。