[發(fā)明專利]一種自主導(dǎo)航農(nóng)用車最短避障路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811323946.6
• 申請(qǐng)日: 2018-11-08
• 公開(kāi)（公告）號(hào): CN109471432B
• 公開(kāi)（公告）日: 2021-09-28
• 發(fā)明（設(shè)計(jì)）人: 劉宇峰;田光兆;顧寶興;魏建勝;安秋;周俊
• 申請(qǐng)（專利權(quán)）人: 南京農(nóng)業(yè)大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 南京天華專利代理有限責(zé)任公司 32218
• 代理人: 陸海天;徐冬濤

本發(fā)明涉及一種自主導(dǎo)航農(nóng)用車最短避障路徑規(guī)劃方法，包括以下步驟：1)基于雙目視覺(jué)的障礙物定位；2)判斷障礙物的有效性；3)車輛避障時(shí)極限轉(zhuǎn)向半徑的確定，4)四段式路徑規(guī)劃；5)確定最優(yōu)轉(zhuǎn)向半徑；6)分段路徑跟蹤控制。本發(fā)明設(shè)計(jì)合理，操控方便，可以更加符合車輛的運(yùn)動(dòng)規(guī)律，實(shí)現(xiàn)車輛運(yùn)行過(guò)程中的最優(yōu)避障，從而，可有效提高車輛的工作效率，充分滿足市場(chǎng)的需求。

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

本發(fā)明涉及一種農(nóng)業(yè)機(jī)械，尤其是一種農(nóng)用車的控制方法，具體的說(shuō)是一種自主導(dǎo)航農(nóng)用車最短避障路徑規(guī)劃方法。

背景技術(shù)

自主導(dǎo)航農(nóng)用車輛能夠顯著提高作業(yè)效率、改善作業(yè)質(zhì)量、節(jié)省人力成本。因此，國(guó)家和部分地方科技主管部門都將自主導(dǎo)航拖拉機(jī)作為重點(diǎn)支持項(xiàng)目列入科技計(jì)劃。

目前，一般的自主導(dǎo)航農(nóng)用車輛在田間工作時(shí)，遇到障礙物需自主執(zhí)行避障動(dòng)作，但普遍規(guī)劃的避障路徑大都較為復(fù)雜，有些不符合車輛的運(yùn)動(dòng)規(guī)律，有些大幅度偏離了原來(lái)的導(dǎo)航路徑，不利于車輛的控制，也影響車輛的工作效率。

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

本發(fā)明的目的是針對(duì)現(xiàn)有技術(shù)的不足，提供一種自主導(dǎo)航農(nóng)用車最短避障路徑規(guī)劃方法，可以更加符合車輛的運(yùn)動(dòng)規(guī)律，提高車輛的工作效率。

本發(fā)明的技術(shù)方案是：

一種自主導(dǎo)航農(nóng)用車最短避障路徑規(guī)劃方法，包括以下步驟：

步驟一：基于雙目視覺(jué)的障礙物定位，包括：

(5)通過(guò)雙目相機(jī)實(shí)時(shí)采集車輛前方道路圖像；

(6)對(duì)采集得到的圖像利用Harris-SIFT算法進(jìn)行特征點(diǎn)檢測(cè)，獲取障礙物特征點(diǎn)的像素坐標(biāo)；

(7)將獲得的特征點(diǎn)像素坐標(biāo)傳入相機(jī)深度信息計(jì)算模塊，得到障礙物在相機(jī)坐標(biāo)系下的空間坐標(biāo)；

(8)再次經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換，將相機(jī)坐標(biāo)系下的空間坐標(biāo)轉(zhuǎn)換到世界坐標(biāo)系下，得到障礙物幾何中心坐標(biāo)(X_z，Y_z)，完成障礙物的定位；

步驟二：判斷障礙物的有效性，包括：

(4)建立矩形車輛幾何模型，以車輛幾何中心O為原點(diǎn)建立平面坐標(biāo)系，x軸正方為水平向右，y軸正方向與車輛的航向重合；

(5)以障礙物的幾何中心Z點(diǎn)為圓心，以Z點(diǎn)到障礙物邊界的最長(zhǎng)距離R_z為半徑作圓，構(gòu)建防碰撞區(qū)域；

(6)計(jì)算Z點(diǎn)到y(tǒng)軸的距離d，并比較d和R_z的差值c與半車寬L/2的大小，若cL/2，則為無(wú)效障礙物，反之，則為有效障礙物；

步驟三：車輛避障時(shí)最大轉(zhuǎn)向半徑的確定，包括：

(4)設(shè)車輛能夠避障的最大轉(zhuǎn)向半徑為r_m，則可確定初始轉(zhuǎn)向中心X₁的位置為(-r_m，0)；

(5)連接X(jué)₁與Z點(diǎn)，與防碰撞區(qū)域圓Z相交于G點(diǎn)；

(6)根據(jù)運(yùn)動(dòng)過(guò)程中的臨界碰撞條件“車輛右前角F與G點(diǎn)重合”，列出方程：其中，X_f和Y_f是車輛右前角F的坐標(biāo)；

(4)解方程，可得最大轉(zhuǎn)向半徑r_m；

步驟四：四段式路徑規(guī)劃，包括：

(6)設(shè)車輛的實(shí)際轉(zhuǎn)向半徑為r，r≤r_m，則實(shí)際轉(zhuǎn)向中心X₂為(-r，0)；以X₂為圓心，以r為半徑作圓；