[發(fā)明專利]基于三角形內(nèi)心引導(dǎo)RRT算法的路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910964030.7
• 申請(qǐng)日: 2019-10-11
• 公開(kāi)（公告）號(hào): CN110705803B
• 公開(kāi)（公告）日: 2022-06-21
• 發(fā)明（設(shè)計(jì)）人: 張衛(wèi)波;肖繼亮;陳泉泉
• 申請(qǐng)（專利權(quán)）人: 福州大學(xué)
• 主分類號(hào): G06Q10/04
• 分類號(hào): G06Q10/04
• 代理公司: 福州元?jiǎng)?chuàng)專利商標(biāo)代理有限公司 35100
• 代理人: 陳明鑫;蔡學(xué)俊

本發(fā)明涉及一種基于三角形內(nèi)心引導(dǎo)RRT算法的路徑規(guī)劃方法。本方法為了克服RRT算法存在的缺點(diǎn)及將目標(biāo)點(diǎn)以一定概率出現(xiàn)在隨機(jī)點(diǎn)中會(huì)導(dǎo)致陷入局部最小的危險(xiǎn),提出利用三角形內(nèi)心來(lái)引導(dǎo)隨機(jī)樹的方法。通過(guò)將隨機(jī)函數(shù)生成的隨機(jī)點(diǎn)、隨機(jī)樹中與該隨機(jī)點(diǎn)距離最近的點(diǎn)及目標(biāo)點(diǎn)三點(diǎn)構(gòu)成三角形的三個(gè)頂點(diǎn)，再計(jì)算該三角形的內(nèi)心坐標(biāo)，用該內(nèi)心坐標(biāo)作為隨機(jī)樹的生長(zhǎng)方向；另外通過(guò)在一定循環(huán)次數(shù)下，記錄使用內(nèi)心引導(dǎo)的次數(shù)及隨機(jī)樹的生長(zhǎng)情況來(lái)調(diào)整采樣方式。這樣不僅對(duì)隨機(jī)樹的生長(zhǎng)進(jìn)行了引導(dǎo)，還有效的避免了陷入局部最小風(fēng)險(xiǎn)。本發(fā)明的方法提高了規(guī)劃的效率，規(guī)劃路徑所需時(shí)間更少，迭代次數(shù)更少，路徑更短。

技術(shù)領(lǐng)域

本發(fā)明涉及路徑規(guī)劃領(lǐng)域，具體涉及一種基于三角形內(nèi)心引導(dǎo)RRT算法的路徑規(guī)劃方法。

背景技術(shù)

路徑規(guī)劃問(wèn)題一直以來(lái)都是移動(dòng)機(jī)器人領(lǐng)域的研究熱點(diǎn)。路徑規(guī)劃主要任務(wù)就是在空間中找到一條從初始狀態(tài)點(diǎn)到目標(biāo)狀態(tài)點(diǎn)的連續(xù)無(wú)碰撞路徑，同時(shí)滿足相應(yīng)的條件約束。

常用的軌跡規(guī)劃算法有圖搜索算法、仿生智能算法、人工勢(shì)場(chǎng)法等。傳統(tǒng)的路徑規(guī)劃算法，如人工勢(shì)場(chǎng)法，該方法基于矢量合成，通過(guò)障礙物對(duì)移動(dòng)機(jī)器人的排斥力和目標(biāo)位置對(duì)移動(dòng)機(jī)器人的吸引力的合力作用下規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)路徑，但當(dāng)吸引力與斥力的合力為零時(shí)，機(jī)器人就陷入了局部最小的情況。

RRT(快速搜索隨機(jī)樹)算法作為一種基于采樣的增量式快速搜索算法，可以直接用于非完整性規(guī)劃和動(dòng)力學(xué)規(guī)劃中。但由于基本RRT算法采用均勻采樣的策略，缺乏引導(dǎo)信息，由此得出的路徑迂回曲折，在較復(fù)雜的障礙物分布下，規(guī)劃時(shí)間長(zhǎng)；另外，對(duì)于將目標(biāo)點(diǎn)以一定概率作為隨機(jī)點(diǎn)出現(xiàn)在采樣點(diǎn)中，這樣雖然可以提高規(guī)劃效率，但也可能導(dǎo)致陷入局部最小，最終在規(guī)定時(shí)間或迭代次數(shù)下找不到路徑。因此，對(duì)于以上問(wèn)題，急需提出一種既能夠引導(dǎo)RRT算法快速找到路徑，又應(yīng)當(dāng)避免陷入局部最小風(fēng)險(xiǎn)的算法。

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

有鑒于此，本發(fā)明的目的在于提供一種基于三角形內(nèi)心引導(dǎo)RRT算法的路徑規(guī)劃方法，克服當(dāng)前RRT算法進(jìn)行路徑規(guī)劃存在的缺點(diǎn)，在引入三角形內(nèi)心降低搜索的隨機(jī)性，提高收斂速率，另外通過(guò)對(duì)利用三角形內(nèi)心進(jìn)行采樣點(diǎn)的數(shù)量和隨機(jī)采樣點(diǎn)的數(shù)量控制，可以有效避免陷入地圖陷阱中，使得算法能夠快速穩(wěn)定的規(guī)劃出相應(yīng)路徑。

為實(shí)現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：

一種基于三角形內(nèi)心引導(dǎo)RRT算法的路徑規(guī)劃方法，提供規(guī)劃路徑的起始點(diǎn)qstart、目標(biāo)點(diǎn)qgoal、障礙物位置和規(guī)劃地圖，包括以下步驟：

步驟1：定義步長(zhǎng)eps和最大迭代次數(shù)n，以qstart為隨機(jī)樹T的根節(jié)點(diǎn)，并將其作為下一次擴(kuò)展隨機(jī)樹的父節(jié)點(diǎn)，以qgoal為隨機(jī)樹T的目標(biāo)節(jié)點(diǎn)，三角形內(nèi)心采樣點(diǎn)數(shù)Ndc初始值為0，隨機(jī)點(diǎn)采樣數(shù)Rdc初始值為0，初始化隨機(jī)樹；

步驟2：通過(guò)隨機(jī)函數(shù)生成隨機(jī)點(diǎn)qrand的坐標(biāo)值；

步驟3：遍歷隨機(jī)樹T中的每個(gè)節(jié)點(diǎn)，通過(guò)計(jì)算T中節(jié)點(diǎn)與隨機(jī)生成的節(jié)點(diǎn)qrand之間的距離，找出離節(jié)點(diǎn)qrand距離最小的節(jié)點(diǎn)設(shè)為qnear；

步驟4：在Ndc數(shù)值小于100的情況下，以隨機(jī)節(jié)點(diǎn)qrand、離節(jié)點(diǎn)qrand距離最小的節(jié)點(diǎn)qnear及目標(biāo)節(jié)點(diǎn)qgoal三點(diǎn)的坐標(biāo)值作為三角形三個(gè)頂點(diǎn)的坐標(biāo)值，計(jì)算以這三點(diǎn)組成的三角形的內(nèi)心坐標(biāo)節(jié)點(diǎn)qinh重新賦值給qrand；

步驟5：從節(jié)點(diǎn)qnear向節(jié)點(diǎn)qrand方向延伸步長(zhǎng)eps，得到新節(jié)點(diǎn)qnew；

步驟6：判斷新節(jié)點(diǎn)qnew與節(jié)點(diǎn)qnear之間是都存在障礙物，若不存在，則將生成的新節(jié)點(diǎn)qnew加入的隨機(jī)樹T中，并判斷新節(jié)點(diǎn)qnew的坐標(biāo)值是否在目標(biāo)點(diǎn)qgoal坐標(biāo)值步長(zhǎng)eps范圍內(nèi)，若在目標(biāo)點(diǎn)qgoal坐標(biāo)值步長(zhǎng)eps范圍內(nèi)，表明已經(jīng)找到路徑，跳出循環(huán)，否者進(jìn)行下一步；若存在障礙物，則舍棄節(jié)點(diǎn)qnew；

步驟7：更新三角形內(nèi)心采樣點(diǎn)數(shù)Ndc及隨機(jī)點(diǎn)采樣數(shù)Rdc；