基于改進(jìn)ACO-DWA算法的輪式植保機(jī)器人避障路徑研究_牛晶.pdf

第38卷 第5期 188 電子測量與儀器學(xué)報(bào) JOURNAL OF ELECTRONIC MEASUREMENT AND INSTRUMENTATION Vol 38 No 5 2024年5月 收稿日期 2024 01 23 Received Date 2024 01 23 基金項(xiàng)目 甘肅省高校教師創(chuàng)新基金 2023A 114 天水師范學(xué)院校級產(chǎn)業(yè)支撐項(xiàng)目 CYZ2023 05 天水師范學(xué)院校級創(chuàng)新創(chuàng)業(yè)引導(dǎo)項(xiàng)目 CXCYJG JGXM202304JD 資助 DOI 10 13382 j jemi B2407237 基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人 避障路徑研究 牛 晶1 申傳艷1 張利鵬2 李奇軍1 劉世鋒1 1 天水師范學(xué)院機(jī)電與汽車工程學(xué)院 天水 741001 2 燕山大學(xué)車輛與能源學(xué)院 秦皇島 066004 摘 要 山地非標(biāo)準(zhǔn)果園內(nèi)大型植保機(jī)械通行性差 小型輪式植保機(jī)器人有廣闊的應(yīng)用前景 為解決因果園枝葉郁閉所造成的 視覺信息誤判 作業(yè)地形復(fù)雜所造成的機(jī)器人避障不及時(shí)等問題 提出了一種基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人路徑 規(guī)劃算法 首先通過激光雷達(dá)獲取果園環(huán)境信息 應(yīng)用體素化網(wǎng)格法精簡點(diǎn)云密度 利用柵格法分割地面點(diǎn)云 采用K means 算法提取機(jī)器人行間通行區(qū)域 再結(jié)合植保機(jī)器人的運(yùn)動(dòng)學(xué)模型及作業(yè)規(guī)范約束 采用基于模型預(yù)測算法 SBMPO 生成一系 列待選軌跡集合 然后采用改進(jìn)的ACO DWA算法 將機(jī)器人的通行成本融入搜索節(jié)點(diǎn)的目標(biāo)函數(shù) 根據(jù)環(huán)境地圖在線進(jìn)行路 徑規(guī)劃 最后 利用MATLAB R2021仿真平臺和機(jī)器人ROS操作系統(tǒng)分別進(jìn)行了仿真驗(yàn)證和實(shí)景布置試驗(yàn) 試驗(yàn)結(jié)果表明 該 方法可以明顯改善機(jī)器人在果園復(fù)雜場景下的通行能力 算法路徑規(guī)劃效果和運(yùn)行效率明顯提高 關(guān)鍵詞 輪式植保機(jī)器人 改進(jìn)ACO DWA算法 路徑規(guī)劃 通行成本 能耗成本 中圖分類號 TP242 6 文獻(xiàn)標(biāo)識碼 A 國家標(biāo)準(zhǔn)學(xué)科分類代碼 510 80 Research on obstacle avoidance path of wheeled plant protection robot based on improved ACO DW A algorithm Niu Jing1 Shen Chuanyan1 Zhang Lipeng2 Li Qijun1 Liu Shifeng1 1 School of Mechanical Electrical and Automotive Engineering Tianshui Normal University Tianshui 741001 China 2 School of Vehicle and Energy Yanshan University Qinhuangdao 066004 China Abstract Large scale plant protection machinery in non standard orchards in mountainous areas has poor accessibility and small wheeled plant protection robots have broad application prospects A path planning algorithm for wheeled plant protection robots based on improved ACO DWA algorithm is proposed to solve the problems of visual information misjudgment caused by closed orchard branches and leaves as well as delayed obstacle avoidance caused by complex working terrain Firstly the orchard environment information is obtained through LiDAR and the voxel grid method is applied to simplify the point cloud density The grid method is used to segment the ground point cloud and the K means algorithm is used to extract the robot s inter row passage area Combined with the kinematic model and job specification constraints of the plant protection robot a series of candidate trajectory sets are generated using the model based prediction algorithm SBMPO Then using the improved ACO DWA algorithm the robot s travel cost is integrated into the objective function of the search node and path planning is carried out online based on the environmental map Finally simulation validation and real world deployment experiments were conducted using MATLAB R2021 simulation platform and robot ROS operating system respectively The experimental results show that this method can significantly improve the traffic capacity of robots in complex orchard scenes and the path planning effect and operational efficiency are significantly improved Keywords wheeled plant protection robot improved ACO DWA algorithm path planning travel costs energy consumption cost 第5期基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人避障路徑研究 189 0 引 言 輪式移動(dòng)機(jī)器人集成了深度視覺傳感器定位 障礙 物檢測與路徑規(guī)劃 能耗與熱管理 系統(tǒng)集成與優(yōu)化等多 項(xiàng)關(guān)鍵技術(shù) 近年來在軍民領(lǐng)域有著快速的發(fā)展與應(yīng)用 尤其隨著農(nóng)業(yè)生產(chǎn)的現(xiàn)代化 信息化 智能化發(fā)展 在田 間地頭能夠自主作業(yè)的植保機(jī)器人有著廣闊的發(fā)展前 景 一方面減輕了農(nóng)業(yè)生產(chǎn)的勞動(dòng)強(qiáng)度 另一方面提高了 作業(yè)效率與質(zhì)量 路徑規(guī)劃是果園植保機(jī)器人技術(shù)領(lǐng)域的重點(diǎn)問題 它決定了機(jī)器人如何行駛到達(dá)指定目標(biāo) 涉及環(huán)境信息 的感知和計(jì)算 目前 路徑節(jié)點(diǎn)搜索使用的算法主要包 括蟻群算法 遺傳算法 粒子群算法等智能算法和A Dijkstra Hybrid A D 等基于圖搜索的算法 A 算法 應(yīng)用廣泛 但面對復(fù)雜非結(jié)構(gòu)化場景進(jìn)行搜索時(shí) 它計(jì)算 量大 內(nèi)存消耗嚴(yán)重 狹窄通道會(huì)產(chǎn)生振蕩 針對A 運(yùn) 算速度慢的問題 文獻(xiàn) 1 提出了跳點(diǎn)搜索法 但該方法 不能在復(fù)雜不規(guī)則地圖中保證路徑全局最優(yōu) 文獻(xiàn) 2 采用直線 圓弧策略對路徑平滑處理后極大改善了路徑 不平滑程度 文獻(xiàn) 3 使用微分方法減少了拐點(diǎn)數(shù) 但 計(jì)算量增大 文獻(xiàn) 4 優(yōu)化了A 算法的啟發(fā)函數(shù) 改進(jìn) 了關(guān)鍵節(jié)點(diǎn)的選擇策略 減少了路徑冗余點(diǎn) 文獻(xiàn) 5 提出了曲率速度法 curvature velocity method CVM 將 避障問題描述為速度空間帶約束的優(yōu)化問題 建立了包 含速度 安全性和路徑3個(gè)因素的優(yōu)化目標(biāo)函數(shù) 文 獻(xiàn) 6 在CVM基礎(chǔ)上 提出了更完善的DWA算法 目標(biāo) 函數(shù)綜合考慮了航向角 速度和障礙物距離3個(gè)因素 得 到的軌跡比較平滑 有效解決了圍繞障礙物繞行的問題 在實(shí)際果園環(huán)境作業(yè)過程中 除了考慮避障約束 距離代 價(jià)之外 還應(yīng)當(dāng)保證機(jī)器人的運(yùn)動(dòng)學(xué)約束 因此 一些學(xué) 者提出了將路徑規(guī)劃與軌跡優(yōu)化結(jié)合的方法 對規(guī)劃好 的路徑進(jìn)行軌跡優(yōu)化處理 7 陳成等 8 采用四階貝塞爾 曲線來表述軌跡形狀 但貝塞爾曲線的局限性限制了軌 形的可塑性 不適用于復(fù)雜路面情況 曹如月等 9 用貝 塞爾曲線對轉(zhuǎn)角處路徑進(jìn)行平滑處理 以 直線一曲線一 直線 的方式表示全局路徑 以分步的形式優(yōu)化全局路 徑 呂恩利等 10 利用三次均勻B樣條曲線描述叉車路 徑 優(yōu)化了整體軌跡形狀 但此方法主要是針對短距離路 線的簡單作業(yè)情況 對于復(fù)雜路面應(yīng)用仍存在一定局 限性 考慮到算法計(jì)算資源的占用量和路徑規(guī)劃的實(shí)時(shí)性 和魯棒性 現(xiàn)有的DWA算法在果園場景中應(yīng)用存在如 下問題 1 路徑狀態(tài)節(jié)點(diǎn)搜索過程中存在大量冗余節(jié)點(diǎn) 計(jì)算量大 運(yùn)算時(shí)間長 2 在果樹行間作業(yè)過程中遇到障 礙物機(jī)器人傾向選擇繞行 導(dǎo)致路徑變長 3 復(fù)雜不規(guī)則 場景下 機(jī)器人運(yùn)行軌跡不平滑 易產(chǎn)生振蕩 基于以上分析 本文提出了一種基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人路徑規(guī)劃算法 通過激光 雷達(dá)提取果園環(huán)境信息 應(yīng)用體素化網(wǎng)格法精簡點(diǎn)云密 度 利用柵格法分割地面點(diǎn)云 采用K means算法提取機(jī) 器人行間通行區(qū)域 再結(jié)合植保機(jī)器人的運(yùn)動(dòng)學(xué)模型及 作業(yè)規(guī)范約束 采用基于模型預(yù)測算法 sampling based model predictive optimization SBMPO 生成一系列待選軌 跡集合 然后采用改進(jìn)的ACO DWA算法 將機(jī)器人的通 行成本融入搜索節(jié)點(diǎn)的目標(biāo)函數(shù) 根據(jù)環(huán)境地圖在線進(jìn) 行路徑規(guī)劃 獲得的路徑規(guī)劃效果在路徑長度 避障能力 和計(jì)算時(shí)間上明顯優(yōu)于傳統(tǒng)DWA算法 1 果園環(huán)境信息識別系統(tǒng) 1 1 系統(tǒng)組成 該植保機(jī)器人由四輪獨(dú)立驅(qū)動(dòng)電動(dòng)底盤 16線激光 雷達(dá) GNSS導(dǎo)航系統(tǒng) 鋰電池組 底盤控制器等組成 如 圖1所示 圖1 植保機(jī)器人實(shí)物圖 Fig 1 Plant protection robot GNSS導(dǎo)航系統(tǒng)安裝在基座上 為機(jī)器人提供全天候 的三維坐標(biāo)和速度以及時(shí)間信息 16線激光雷達(dá)用于感 知周圍環(huán)境信息 識別障礙物位置 鋰電池組作為儲(chǔ)能系 統(tǒng) 為機(jī)器人提供能量 延長續(xù)航時(shí)間 底盤控制器是一 個(gè)緊湊的差速驅(qū)動(dòng)移動(dòng)小車 其處理器采用Jetson NANO B01主控 搭載Ubuntu 18 04操作系統(tǒng) 機(jī)器人作業(yè)過 程中 由GNSS導(dǎo)航系統(tǒng)提供路徑全局定位 由激光雷達(dá) 進(jìn)行局部環(huán)境參數(shù)的感知 植保機(jī)器人的結(jié)構(gòu)尺寸需滿 足果樹行間尺寸的要求 其設(shè)計(jì)寬度為100 cm 山地果園大多依山勢而建 地面易高低起伏 同時(shí) 果樹生長中后期枝葉繁茂遮擋 果園光照強(qiáng)度分布不均 都易對GNSS信號的穩(wěn)定性產(chǎn)生影響 造成機(jī)器人作業(yè) 時(shí)發(fā)生局部碰撞 因此 機(jī)器人局部作業(yè)路徑的規(guī)劃主 要由激光雷達(dá)信號支持 1 2 果樹行間激光點(diǎn)云數(shù)據(jù)處理 受果樹枝葉遮擋和光照不均等因素影響 果樹由激 190 電 子 測 量 與 儀 器 學(xué) 報(bào)第38卷 光雷達(dá)采集的果園行間主干部分點(diǎn)云分布較集中 四周 點(diǎn)云較零散 并且中間空白部分點(diǎn)云區(qū)域與實(shí)際可通行 尺寸相比明顯較小 如圖2所示 因此 可以通過對果樹 行間激光點(diǎn)云數(shù)據(jù)的處理進(jìn)一步優(yōu)化機(jī)器人行間通行 邊界 圖2 雷達(dá)數(shù)據(jù)信息 Fig 2 Radar data information 激光雷達(dá)采集區(qū)域內(nèi)與果樹行間通行尺寸無關(guān)的點(diǎn) 較多 如地面 雜草及空間其他障礙物 故文中僅提取機(jī) 器人行駛方向兩側(cè)果樹所在區(qū)域的點(diǎn)云信息 首先 采 用體素化網(wǎng)格方法降低點(diǎn)云密度 精簡計(jì)算量 再根據(jù)坐 標(biāo)變換法將空間點(diǎn)云轉(zhuǎn)換至圖像坐標(biāo)系 形成地面點(diǎn)云 再利用柵格法將地面點(diǎn)云與其他一切點(diǎn)云信息分割 減 少干擾因素 最后采用K means算法提取機(jī)器人行間通 行的主干區(qū)域 文中根據(jù)柵格單元高度差 高度方差以及高度梯度 分割獲取的地面點(diǎn)云 建立如圖3所示的地面柵格地圖 坐標(biāo)原點(diǎn)為激光雷達(dá)中心 設(shè)定柵格單元邊長0 1 m 激光點(diǎn)P x y z 所屬柵格單元g r c 的計(jì)算方 法為 g r c floor x 0 1 15 floor y 0 22 0 1 1 式中 floor 為向下取整函數(shù) 圖3 平面柵格地圖 Fig 3 Planar raster map 計(jì)算每個(gè)柵格單元的高度差 高度方差和高度 梯度 11 設(shè)定閾值評價(jià)柵格單元的高度差 高度方差和高度 梯度 將滿足條件的柵格單元?jiǎng)澐譃榈孛鎱^(qū)域 12 建立 的點(diǎn)云地面投影如圖4所示 圖4 點(diǎn)云地面投影 Fig 4 Ground projection of points cloud 1 3 K means算法實(shí)現(xiàn)主干區(qū)域點(diǎn)云聚類 激光點(diǎn)云聚類是根據(jù)點(diǎn)云不同特征將其劃分為非空 的 互斥的簇 并使同一簇中的點(diǎn)云具有相近的特征 13 本文采用K means聚類方法聚類果樹行點(diǎn)云 并將聚類中 心作為果樹行的主干區(qū)域點(diǎn) 進(jìn)而擬合機(jī)器人航線方程 設(shè)通過柵格法獲取的地面點(diǎn)云集合為S 則其中任 意點(diǎn) xi yi 與其相鄰點(diǎn) xj yj 之間的歐氏距離表示 如下 dij xi xj 2 yi yj 2 2 點(diǎn)間歐氏距離越短 證明兩個(gè)數(shù)據(jù)相似度越高 對 果樹行點(diǎn)云數(shù)量估計(jì)設(shè)置k值 如式 3 所示 對每個(gè)地 第5期基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人避障路徑研究 191 面點(diǎn)云樣本點(diǎn)計(jì)算到這k個(gè)中心點(diǎn)的距離 將樣本點(diǎn)歸 至與之距離最小的那個(gè)中心點(diǎn)的簇 k hl ptan15 3 式中 h為激光雷達(dá)基準(zhǔn)線離地高度 lp為估算的果樹株 間距 則經(jīng)過聚類后k個(gè)聚類的中心點(diǎn)為 Xi 1 S i x Si x 4 算法的聚類準(zhǔn)則為 k means Si argmin k i 1 x Si x Xi 5 通過聚類獲得的果樹點(diǎn)云信息如圖5所示 由圖可 見 由于16線激光雷達(dá)采集的點(diǎn)云密度大 且果樹枝葉 分布茂盛 存在近端數(shù)據(jù)點(diǎn)云密度高于遠(yuǎn)端數(shù)據(jù)點(diǎn)云 密度 圖5 果樹主干聚類點(diǎn)云圖 Fig 5 Points cloud of fruit tree trunk clustering 2 ACO算法模型 蟻群算法的基本原理已在肖艷秋等 14 著作中詳細(xì) 講解 下文僅扼要說明算法核心中的狀態(tài)轉(zhuǎn)移率和信息 素增量模型 2 1 狀態(tài)轉(zhuǎn)移率 在t時(shí)刻 可將螞蟻由狀態(tài)節(jié)點(diǎn)i運(yùn)動(dòng)到相鄰狀態(tài)節(jié) 點(diǎn)j的狀態(tài)轉(zhuǎn)移率定義為 Pmij t ij t ij t S Um is t is t j Um 0 其他 null null null nullnull nullnull 6 式中 Pmij t 是t時(shí)刻第m只螞蟻從狀態(tài)節(jié)點(diǎn)i運(yùn)動(dòng)到狀 態(tài)節(jié)點(diǎn)j的狀態(tài)轉(zhuǎn)移概率 ij t 是路徑 i j 上的信息素 濃度 其中 是信息啟發(fā)式因子 反映信息素對螞蟻選擇 路徑的影響力 ij t 是第m只螞蟻在狀態(tài)節(jié)點(diǎn)i處選擇 相鄰狀態(tài)節(jié)點(diǎn)j的啟發(fā)函數(shù) 其中 是期望啟發(fā)式因子 反映啟發(fā)式信息在指導(dǎo)蟻群搜索過程中的重要程度 Um 為螞蟻尚未訪問的下一節(jié)點(diǎn)集合 s為與當(dāng)前位置相鄰的 可選節(jié)點(diǎn)集合 is t 為第m只螞蟻在當(dāng)前狀態(tài)節(jié)點(diǎn)i與 各相鄰狀態(tài)節(jié)點(diǎn)之間的信息素濃度 is t 為第m只螞 蟻在當(dāng)前節(jié)點(diǎn)i與各相鄰狀態(tài)節(jié)點(diǎn)之間的啟發(fā)函數(shù) 啟發(fā)函數(shù) ij t 可表達(dá)為 ij t 1 Dij 7 式中 Dij為狀態(tài)節(jié)點(diǎn)i和狀態(tài)節(jié)點(diǎn)j之間的距離 2 2 信息素濃度更新模型 目前 常見的信息素濃度更新模型有ADS ant density system AQS ant quantity system 和ACS ant cycle system 15 ADS模型和AQS模型采取局部更新 策略 ACS模型采取全局更新策略 考慮到算法求解速 度和避障能力 本文采用AQS模型為原型 假設(shè)第m只螞蟻當(dāng)前循環(huán)經(jīng)過路徑 i j 狀態(tài)節(jié)點(diǎn) 的集合為X i j i 1 2 n j 1 2 n 則 mij t Q Dij i j X 0 其他null 8 式中 mij t 是第m只螞蟻從t 1時(shí)刻到t時(shí)刻從狀態(tài) 節(jié)點(diǎn)i運(yùn)動(dòng)到相鄰狀態(tài)節(jié)點(diǎn)j的路徑信息素濃度增量 Q 為信息素強(qiáng)度 是大于0的常數(shù) 3 改進(jìn)的DWA算法設(shè)計(jì) 3 1 經(jīng)典DWA算法 經(jīng)典DWA算法的實(shí)施過程是將機(jī)器人的位置控制 轉(zhuǎn)換成速度控制 將避障問題描述為機(jī)器人速度空間帶 約束的優(yōu)化問題 其約束包括速度 航向及周圍環(huán)境障礙 物的位置約束 16 由機(jī)器人運(yùn)動(dòng)軌跡節(jié)點(diǎn)周圍障礙物的物理約束決定 的 縱向速度和機(jī)器人橫擺角速度限制構(gòu)成的速度集合 Us ua r 必須滿足 US ua r 0 ua uamax rmax r rmax 9 機(jī)器人的行駛軌跡可以認(rèn)為是由n個(gè)時(shí)間段的n段 折線段組成 折線段的連接點(diǎn)處被認(rèn)為是在滿足障礙物 膨脹尺寸限制的前提下 極限靠近障礙物所在位置 為 了保證機(jī)器人運(yùn)動(dòng)過程中遇到障礙物不發(fā)生碰撞 由運(yùn) 動(dòng)學(xué)條件限制可以得到時(shí)間dt后 速度集合Ua必須 滿足 192 電 子 測 量 與 儀 器 學(xué) 報(bào)第38卷 Ua ua r ua 2 dist ua r u a r 2 dist ua r r 10 式中 dist ua r 表示下一時(shí)刻機(jī)器人和障礙物之間的 直線距離 假設(shè)機(jī)器人當(dāng)前時(shí)刻的速度集合為 uacurr rcurr 則下一時(shí)刻的速度集合Ud必須滿足 Ud uad rd uacurr uamaxdt ua uacurr uamaxdt rcurr rmaxdt r rcurr rmaxdtnull 11 最終的速度集合U可表示為 U US Ua Ud 12 通過目標(biāo)函數(shù)對下一時(shí)刻的速度集合進(jìn)行預(yù)測 本 文定義的目標(biāo)函數(shù)綜合考慮了運(yùn)動(dòng)速度 航向和碰撞安 全性 如式 13 所示 G uad rd l m dist ua r n uamax 13 式中 表示機(jī)器人行駛方向與目標(biāo)線之間的夾角 dist ua r 表示機(jī)器人位置與障礙物之間的最短距離 l m n分別是角度 距離和車速權(quán)重系數(shù) 目標(biāo)函數(shù)的 數(shù)值越大代表速度集合越優(yōu)秀 機(jī)器人在路徑規(guī)劃過程 中需要從多個(gè)傳感器獲取位置和速度信息 而這些信號 通常不總是連續(xù)的 評價(jià)結(jié)果可能會(huì)存在一定誤差 為了 減小誤差上述3個(gè)權(quán)重系數(shù)通常做歸一化處理為 0 1 之間的數(shù) 根據(jù)式 13 可以計(jì)算得到每一條機(jī)器人待選軌跡 的目標(biāo)函數(shù)值 能使該值最大的速度集合即為最優(yōu)集合 經(jīng)典DWA算法中3個(gè)權(quán)重系數(shù)的數(shù)值是固定的 為了 驗(yàn)證上述3個(gè)權(quán)重系數(shù)在大小不同情況下的路徑規(guī)劃效 果差異 本文在MATLABR2021仿真平臺中使用柵格法 構(gòu)建了10 10 dm2的環(huán)境地圖 機(jī)器人從原點(diǎn)出發(fā) 實(shí)心 圓點(diǎn)表示障礙物所在位置 圓圈表示障礙物的膨脹尺寸 曲線表示機(jī)器人的運(yùn)行軌跡 空心圓點(diǎn)表示路徑目標(biāo)點(diǎn) 空白區(qū)域表示開放空間 仿真結(jié)果如圖6 8所示 圖中 隱藏了地圖柵格 從仿真結(jié)果可以看出 所有權(quán)重系數(shù)數(shù)值都較大時(shí) 機(jī)器人運(yùn)行步數(shù)偏少 路徑規(guī)劃時(shí)間較短 如圖6所示 l 值較小時(shí) 運(yùn)行步數(shù)偏多 路徑規(guī)劃時(shí)間較長 如圖7所 示 m和n較小時(shí) 運(yùn)行步數(shù)明顯增多 機(jī)器人出現(xiàn)繞行 路徑規(guī)劃時(shí)間明顯增多 如圖8所示 3種情況對應(yīng)的 路徑規(guī)劃效果如表1所示 表1 3種權(quán)重系數(shù)情況的路徑規(guī)劃效果 Table 1 Path planning effect 場景路徑規(guī)劃時(shí)間 s路徑規(guī)劃距離 m 圖6 8 79 10 72 圖7 9 78 9 04 圖8 10 20 14 36 圖6 大權(quán)重系數(shù)運(yùn)動(dòng)軌跡 Fig 6 Trajectory of three high weights 圖7 小角度權(quán)重系數(shù)運(yùn)動(dòng)軌跡 Fig 7 Trajectory of low angle weight 圖8 小距離 小速度權(quán)重系數(shù)運(yùn)動(dòng)軌跡 Fig 8 Trajectory with low distance and velocity weights 因此 當(dāng)機(jī)器人高速運(yùn)動(dòng)時(shí) 如果周圍障礙物密度過 大 經(jīng)典的DWA算法規(guī)劃的路徑會(huì)使機(jī)器人太靠近障 第5期基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人避障路徑研究 193 礙物 通過性顯著下降 安全性大幅降低 17 甚至還會(huì)出 現(xiàn)繞行 導(dǎo)致規(guī)劃路徑和規(guī)劃時(shí)間變長 在某些情況下 機(jī)器人會(huì)在多個(gè)障礙物間振蕩 產(chǎn)生上述問題的根本原 因是目標(biāo)函數(shù)中的權(quán)重系數(shù)一旦確定 將無法適應(yīng)各種 復(fù)雜多變的環(huán)境 當(dāng)障礙物情況和機(jī)器人運(yùn)動(dòng)狀態(tài)發(fā)生 變化時(shí) 還有可能規(guī)劃出一些奇怪的路徑 18 基于上述考慮 本文根據(jù)蟻群算法在全局優(yōu)化和搜 索過程連續(xù)收斂等方面的優(yōu)勢 提出了一種改進(jìn)的ACO DWA算法 通過蟻群算法使目標(biāo)函數(shù)的權(quán)重系數(shù)自適 應(yīng) 19 該算法顯著減少了機(jī)器人的繞行距離和路徑規(guī) 劃時(shí)間 提高了在密集障礙物場景中的通過能力和安全 性 下文將詳細(xì)闡述根據(jù)蟻群算法對上述權(quán)重系數(shù)進(jìn)行 動(dòng)態(tài)更新的具體過程 3 2 融合ACO算法的DWA算法改進(jìn) 假設(shè)當(dāng)前t時(shí)刻 機(jī)器人行駛方向區(qū)域有一定密度 的障礙物 20 若該區(qū)域障礙物的個(gè)數(shù)為K 機(jī)器人與第i個(gè)障礙物 之間的最短距離為Di 方位角為 i 定義當(dāng)M大于閾值 時(shí) 該區(qū)域?yàn)檎系K密集區(qū) 定義第i個(gè)障礙物與第j個(gè)障 礙物之間的最短距離為Dij Dij Di2 D2j DiDjcos i j i j 14 考慮到機(jī)器人在障礙物間穿行的安全性和機(jī)動(dòng)性 為了衡量機(jī)器人在兩障礙物間的可通過性 定義其穿行 機(jī)能數(shù)Ds為 Ds a max rmax b umaxu a 15 式中 rmax為 r中的最大值 max為 i中的最大值 系數(shù) a反映機(jī)器人方向偏差對通過能力的影響 根據(jù)柵格地 圖的建模經(jīng)驗(yàn) 其值取0 6 系數(shù)b反映機(jī)器人速度對通 過能力的影響 根據(jù)柵格地圖的建模經(jīng)驗(yàn) 其值取0 4 引入障礙物的膨脹半徑 則機(jī)器人在兩障礙物間 能夠安全通行的條件是 Ds Dij 16 式中 根據(jù)柵格地圖的建模經(jīng)驗(yàn) 值取0 3 則動(dòng)態(tài)信息素的更新模型如下 mij Dsmax Dij Dij Dsmin Dij Dsmax Dij Dsmin null null null null nullnull null null 17 式中 Dmax Dmin 是第n次迭代可以接受的誤差 為 一常數(shù) Dmax是指機(jī)器人運(yùn)行至局部避障區(qū)域后 在任意 兩個(gè)障礙物間運(yùn)動(dòng)得到的穿行機(jī)能數(shù)的最大值 Dmin是 指機(jī)器人運(yùn)行至局部避障區(qū)域后 在任意兩個(gè)障礙物間 運(yùn)動(dòng)得到的穿行機(jī)能數(shù)的最小值 3 3 算法流程 步驟1 由激光雷達(dá)獲取環(huán)境地圖信息 定位機(jī)器人 運(yùn)動(dòng)起始點(diǎn)和目標(biāo)點(diǎn) 獲取空間所有狀態(tài)節(jié)點(diǎn)信息 計(jì)算 相鄰矩陣和啟發(fā)式信息矩陣 步驟2 參數(shù)初始化 初始化迭代次數(shù)為N 蟻群規(guī) 模M 信息啟發(fā)式因子 期望啟發(fā)式因子 信息素?fù)] 發(fā)系數(shù) 信息素濃度 步驟3 實(shí)時(shí)計(jì)算障礙物密度 機(jī)器人與各障礙物的 實(shí)際距離和方位 步驟4 路徑選擇更新 查詢相鄰矩陣 獲取當(dāng)前節(jié) 點(diǎn)i運(yùn)動(dòng)至下一節(jié)點(diǎn)可行的節(jié)點(diǎn)集合 按照式 1 3 計(jì)算第m只螞蟻選擇相鄰節(jié)點(diǎn)的概率 在節(jié)點(diǎn)更新過 程中依據(jù)捕獲的地圖信息 以障礙物密度 機(jī)器人與各障 礙物的實(shí)際距離和方位等信息判斷是否進(jìn)入障礙物密集 區(qū) 若進(jìn)入 轉(zhuǎn)入步驟5 7 進(jìn)行 若不進(jìn)入 步驟5 中 的權(quán)重系數(shù)全部做定值處理 步驟5 在全局路徑規(guī)劃節(jié)點(diǎn)中剔除dist ua r 大 于閾值的節(jié)點(diǎn)集合 螞蟻序號更新 車載處理器計(jì)算各障 礙物之間的距離Dij和機(jī)器人穿行機(jī)能數(shù)Ds 獲得Dmax 和Dmin值 計(jì)算更新信息素 獲得動(dòng)態(tài)更新的權(quán)重系數(shù)l m和n 步驟6 根據(jù)式 8 獲得備選速度空間U ua r 對3個(gè)權(quán)重系數(shù)分別作歸一化處理 得到t 1時(shí)刻機(jī)器 人的最優(yōu)速度集合 步驟7 執(zhí)行該速度 判斷是否到達(dá)目標(biāo)點(diǎn) 若是 則 結(jié)束迭代過程 否則返回步驟1 4 試驗(yàn)與分析 4 1 仿真參數(shù)初始化 為了進(jìn)一步驗(yàn)證文中提出的改進(jìn)ACO DWA算法的 整體性能 本文在MATLAB R2021仿真平臺上對該算法 進(jìn)行了測試 仿真初始化參數(shù)如表2 4所示 表2 機(jī)器人運(yùn)動(dòng)狀態(tài)初始參數(shù) Table 2 Initial parameters of the robot motion state 參數(shù)x m y m rad ua km h 1 r rad s 1 起始點(diǎn)0 5 0 5 0 68 10 20 目標(biāo)點(diǎn)9 5 8 5 0 0 0 表3 DW A算法初始參數(shù) Table 3 Initial parameters of the DW A algorithm 膨脹半徑 m 時(shí)間分辨率 s 線速度分辨率 m s 1 角速度分辨率 rad s 1 預(yù)測時(shí)間 s 0 3 0 01 0 03 0 2 4 194 電 子 測 量 與 儀 器 學(xué) 報(bào)第38卷 表4 ACO算法初始參數(shù) Table 4 Initial parameters of the ACO algorithm G m Q 100 40 3 4 0 5 100 表3中 考慮到柵格地圖邊長為1 如果膨脹半徑設(shè) 置為0 5 機(jī)器人將與障礙物碰撞 為了確保機(jī)器人和 障礙物之間的距離不會(huì)太近 而且機(jī)器人不會(huì)繞行 膨脹 半徑取值為0 3 柵格地圖中有100個(gè)單元格 分辨率取 機(jī)器人線速度和角速度的1 100即可保證每個(gè)單元格都 有一個(gè)狀態(tài)節(jié)點(diǎn) 因此 線速度和角速度的分辨率分別 取0 03和0 2 根據(jù)機(jī)器人初始線速度的水平和柵格地 圖的大小 考慮路徑規(guī)劃的最長范圍 設(shè)計(jì)仿真測試時(shí)間 為4 s 表4中 的大小反映了路徑搜索中隨機(jī)因素的強(qiáng) 度 它的值越大 選擇先前路徑的螞蟻就越多 搜索的隨 機(jī)性就會(huì)減弱 其值越小 搜索將過早地陷入局部最優(yōu) 根據(jù)經(jīng)驗(yàn) 該值的一般范圍為 1 4 因此 本文選擇了 綜合求解性能較好的3 反映了先驗(yàn)因素和確定性因 素在尋找最優(yōu)路徑過程中的強(qiáng)度 根據(jù)經(jīng)驗(yàn) 一般取值 范圍為 3 5 本文取4 這樣算法綜合性能更好 的 大小影響算法的收斂速度和全局搜索性能 是一個(gè)介于 0和1之間的數(shù) 如果它的取值太大 全局搜索能力增 強(qiáng) 但算法的收斂速度變慢 根據(jù)經(jīng)驗(yàn) 該值取0 5 如 果蟻群m的數(shù)量太小 全局性能將降低 并會(huì)出現(xiàn)過早 停滯 如果m太大 算法的收斂速度將減慢 根據(jù)經(jīng) 驗(yàn) m一般取10 50的值 文中取40 信息素強(qiáng)度Q對 算法性能的影響取決于上述參數(shù)的選擇 因此 Q值的 選擇無需特別考慮 最大進(jìn)化代數(shù)G表示蟻群算法運(yùn)行 結(jié)束條件 一般取100 500 文中取100 4 2 算法優(yōu)化仿真結(jié)果 融合蟻群算法的DWA算法可以明顯加快機(jī)器人接 近障礙物區(qū)域時(shí)的局部路徑動(dòng)態(tài)更新能力 通過獲得的 最優(yōu)目標(biāo)函數(shù) 保證汽車的運(yùn)動(dòng)速度 行駛方向和碰撞安 全性 如圖9所示 在柵格地圖進(jìn)入障礙密集區(qū)開始 時(shí) 刻10 450 s時(shí) 經(jīng)典DWA算法的預(yù)測軌跡線是虛線 融 合蟻群算法的改進(jìn)DWA算法的軌跡線是實(shí)線 可見 虛線線條代表的汽車行駛路徑明顯增長 在此時(shí)刻經(jīng)典 DWA算法得到的目標(biāo)函數(shù)明顯大于改進(jìn)DWA算法的 按照目標(biāo)函數(shù)最大化的算法邏輯 依照經(jīng)典DWA算法 會(huì)使機(jī)器人繼續(xù)增大速度和偏轉(zhuǎn)角度 進(jìn)一步繞開障礙 物密集區(qū) 最終導(dǎo)致路徑增長 1 速度改變下算法仿真試驗(yàn) 圖10所示 虛線表示機(jī)器人車速設(shè)定5 km h 在多 個(gè)障礙物間預(yù)測軌跡產(chǎn)生了一些振蕩 為了驗(yàn)證車速對 改進(jìn)DWA算法的影響 提高機(jī)器人速度提升至15 km 圖9 時(shí)刻10 450 s機(jī)器人預(yù)測軌跡 Fig 9 The trajectory of the robot at 10 450 s h 如實(shí)線所示 可見速度提高軌跡振蕩明顯消除 圖10 速度改變機(jī)器人的預(yù)測軌跡 Fig 10 Predicted trajectory of changing the speed of the robot 圖11所示為此情況下目標(biāo)函數(shù)的仿真結(jié)果 可以 看出 當(dāng)目標(biāo)函數(shù)的3個(gè)權(quán)重系數(shù)進(jìn)行蟻群算法迭代后 若機(jī)器人離障礙物較遠(yuǎn) 優(yōu)化后的備選速度集合是較高 的 若機(jī)器人離障礙物較近 優(yōu)化后的備選速度集合是較 低的 上述算法邏輯完全符合動(dòng)態(tài)調(diào)整機(jī)器人速度和軌 跡的指導(dǎo)思想 保證了機(jī)器人在密集障礙物區(qū)域的機(jī)動(dòng) 性和安全性 2 改變障礙物個(gè)數(shù)和位置下的算法仿真試驗(yàn) 如圖12所示 增加障礙物的個(gè)數(shù)和改變障礙物的位 置后 新預(yù)測的軌跡較前振蕩略有減弱 在保證安全性的 前提下 機(jī)器人從起始點(diǎn)運(yùn)動(dòng)至目標(biāo)點(diǎn)的運(yùn)行總體效率 明顯提高 場景1中機(jī)器人在果樹行間邊沿運(yùn)行軌跡相 對較直 符合園藝規(guī)范要求 場景2中計(jì)算迭代次數(shù)降低 了20 69 總運(yùn)行時(shí)間下降了16 78 場景3中在規(guī)劃 路徑節(jié)點(diǎn)中遇到障礙物導(dǎo)致機(jī)器人規(guī)劃路徑失敗 總體 第5期基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人避障路徑研究 195 圖11 機(jī)器人速度15 km h的目標(biāo)函數(shù)3D圖 Fig 11 3D diagram of the objective function at 15 km h 來說 改進(jìn)的ACO DWA算法基本能夠有效應(yīng)對各種復(fù) 雜園內(nèi)障礙物場景 為了定量對比多種不同場景下改進(jìn)算法前后的綜合 性能 驗(yàn)證算法的普適性 表5列出上述3種場景的算法 性能的對比結(jié)果 可見按照改進(jìn)的ACO DWA算法進(jìn)行 機(jī)器人路徑規(guī)劃 場景變換后對機(jī)器人的運(yùn)動(dòng)性能影響 不大 表5 場景變換后性能對比結(jié)果 Table 5 Comparison results in different scenarios 場景 安全性 dist m 1 迭代步數(shù)總時(shí)間 s總路程 m 場景1 0 246 339 17 801 15 320 場景2 0 213 350 18 091 15 231 場景3 0 177 407 17 443 14 998 3 改進(jìn)的ACO DWA算法與優(yōu)秀DWA算法變體的 性能比較 為了比較文中所述的改進(jìn)ACO DWA算法與其他優(yōu) 秀DWA算法變體的性能差異 文中在10 10 dm2柵格地 圖仿真環(huán)境中設(shè)計(jì)了3種典型靜態(tài)障礙物場景 設(shè)定柵 格單元邊長0 1 m 通過改變機(jī)器人的速度和柵格地圖 精度 比較改進(jìn)的ACO DWA DWA DWA A 和BSDWA 算法在路徑規(guī)劃長度 規(guī)劃時(shí)間和平滑度方面的差異 仿真結(jié)果如圖13所示 圖13中 點(diǎn)線表示改進(jìn)ACO DWA算法的路徑軌 跡 實(shí)線 箭頭線和虛線分別表示DWA DWA A BSDWA算法的路徑軌跡 當(dāng)機(jī)器人速度設(shè)置為10 km h時(shí) 這4種算法規(guī)劃的路徑軌跡存在明顯差異 在環(huán) 形障礙物場景中 改進(jìn)的ACO DWA和DWA A 路徑規(guī) 劃長度相對較短 轉(zhuǎn)彎次數(shù)較少 相比之下 BSDWA和 DWA算法在首次遇到障礙物時(shí)更傾向選擇較遠(yuǎn)路徑 圖12 預(yù)測軌跡 Fig 12 Predicted trajectory 在交錯(cuò)形場景中 BSDWA算法的軌跡在障礙物之間有明 顯振蕩 但DWA和DWA A 的路徑規(guī)劃長度明顯較 長 在迷宮形場景中 4種算法在初始階段的性能沒有 太大差異 然而 改進(jìn)的ACO DWA算法在后期選擇了 196 電 子 測 量 與 儀 器 學(xué) 報(bào)第38卷 圖13 10km h時(shí)3種典型場景的算法性能對比結(jié)果 Fig 13 Comparison of algorithm performance in three typical scenarios at 10 km h 一種更直接的搜索方式 其規(guī)劃長度是最短的 受文章 篇幅限制 表6中僅列出交錯(cuò)形場景中機(jī)器人路徑規(guī)劃 長度 轉(zhuǎn)彎次數(shù)和規(guī)劃時(shí)間的仿真結(jié)果 考慮到機(jī)器人速度是影響算法性能的重要變量 將 機(jī)器人速度由10 km h提高到30 km h 在上述3種情 況下獲得的仿真結(jié)果如圖14所示 表6 多種算法性能仿真結(jié)果對比 Table 6 Performance comparison of different algorithms 算法路徑規(guī)劃長度 m轉(zhuǎn)彎次數(shù)路徑規(guī)劃時(shí)間 s DWA 18 309 1 1 20 改進(jìn)的ACO DWA 14 167 2 0 78 DWA A 16 701 2 0 84 BSDWA 17 860 4 1 19 圖14 30 km h時(shí)3種典型場景的算法性能對比結(jié)果 Fig 14 Comparison of algorithm performance in three typical scenarios at 30 km h 第5期基于改進(jìn)ACO DWA算法的輪式植保機(jī)器人避障路徑研究 197 與圖13的仿真結(jié)果相比 在環(huán)形場景類型中 DWA 算法選擇了以繞遠(yuǎn)的方式來減少轉(zhuǎn)彎次數(shù) 改進(jìn)ACO DWA算法的性能與之前相比沒有太大變化 在交錯(cuò)形 場景中 這4種算法的仿真結(jié)果都有了明顯的變化 為 了避免在交錯(cuò)障礙物中振蕩 改進(jìn)ACO DWA算法選擇 了不轉(zhuǎn)彎的繞遠(yuǎn)路徑 在迷宮形場景中 DWA規(guī)劃了一 條非常奇怪的路徑 規(guī)劃距離明顯增加 其他3條路徑都 有不同程度的繞遠(yuǎn) 其中改進(jìn)ACO DWA的路徑最短 總體而言 隨著速度的增加 改進(jìn)DWA算法規(guī)劃的路徑 相對較短 更符合人們的駕駛邏輯 比較圖13 b 和14 b 中改進(jìn)ACO DWA算法的性 能可以看出 這兩條路徑的長度幾乎相同 但路徑平滑度 顯著不同 原因可以從圖15中的蟻群算法的適應(yīng)度進(jìn) 化曲線中找到 低速下 蟻群在接近局部最優(yōu)解的地方 過早進(jìn)化 如粗實(shí)線所示 減少了搜索的廣度 相反 細(xì) 實(shí)線代表了高速情況 蟻群個(gè)體具有更強(qiáng)的全局搜索能 力 找到了一條全局最優(yōu)路線 因此 在未來算法優(yōu)化過 程中 可以進(jìn)一步引入隨機(jī)因素 使蟻群更具探索性 跳 出局部最優(yōu)解 圖15 蟻群算法適應(yīng)度進(jìn)化曲線 Fig 15 Evolution curve of ant colony algorithm fitness 4 3 實(shí)景試驗(yàn) 為驗(yàn)證本文所提融合算法的正確性 為更精確分析 該方法在多種果樹種植密度下的植保機(jī)器人可通行區(qū)域 識別精度 本試驗(yàn)通過布置標(biāo)定好行距和株距的果樹植 株 模擬真實(shí)場景開展精度分析試驗(yàn) 經(jīng)過調(diào)研 甘肅地 區(qū)機(jī)械化種植果樹作物的間隔為大于50 cm 布置模擬實(shí) 驗(yàn)如圖16所示 調(diào)節(jié)行間距與株間距研究不同行間距與 株間距對該方法識別精度的影響 為更好實(shí)現(xiàn)模擬場景 真實(shí)性 試驗(yàn)中調(diào)整傳感器感知高度 使其所采集點(diǎn)云形 態(tài)與真實(shí)環(huán)境保持一致 所采集對比點(diǎn)云如圖17所示 通過對比基于 K means 聚類獲取的果樹主干定位與模擬 植株真實(shí)位置 分析果樹主干定位誤差 圖16 模擬試驗(yàn) Fig 16 Simulation test 模擬試驗(yàn)場景下果樹植株聚類方法定位位置與真實(shí) 位置對比如圖 18 所示 當(dāng)被測植株距離雷達(dá)越遠(yuǎn)時(shí) 測 量的果樹主干誤差有所增加 根據(jù)兩側(cè)所求得主干中心 區(qū)域 并通過中心區(qū)域圈定可通信區(qū)域規(guī)劃獲得導(dǎo)航線 通過對比 行距70 80 cm寬度內(nèi)變化對中心區(qū)域識別誤 差影響較小 試驗(yàn)中導(dǎo)航線最大誤差7 07 cm 最小誤差 0 5 cm 平均誤差3 1 cm 多次試驗(yàn)結(jié)果表明 采用 K means 算法引入置信度 來估計(jì)果樹主干位置 與果樹主干實(shí)際可通行區(qū)域進(jìn)行 對比 X軸方向3 7 m前視范圍內(nèi)平均誤差均保持在 6 cm以內(nèi) 最大誤差為8 09 cm 虛線代表改進(jìn)ACO DWA算法的路徑軌跡 障礙物間通行的能力最好 路徑 平滑度也很好 實(shí)線 點(diǎn)線和箭頭線分別代表DWA DWA A 和BSDWA算法的路徑規(guī)劃 都存在不同程度地 繞行和抖振現(xiàn)象 由于機(jī)器人寬度限制 移動(dòng)平臺行駛 速度與前視距離成正比 與感知規(guī)劃時(shí)間成反比 誤差 198 電 子 測 量 與 儀 器 學(xué) 報(bào)第38卷 圖17 真實(shí)環(huán)境與模擬試驗(yàn)點(diǎn)云對比 Fig 17 Comparison of the point clouds in the real environment and the simulated experiment 圖18 實(shí)景