簡介面向ROS1開發(fā)者與無人機導航學習者的覆蓋路徑規(guī)劃算法實例封裝了多邊形覆蓋規(guī)劃開源包的核心實現(xiàn)可幫助理解如何將目標區(qū)域分解并規(guī)劃出無遺漏的全覆蓋路徑適合環(huán)境監(jiān)測、農(nóng)業(yè)植保等需要區(qū)域遍歷的自主飛行場景。資源共160個文件以C源碼為主體配合ROS的啟動文件、可視化配置、參數(shù)文件以及自定義消息與服務類型壓縮包僅471KB輕量且目錄結(jié)構(gòu)清晰。已有1981人學習或下載。通過該實例可系統(tǒng)觀察節(jié)點、話題、服務與動作的協(xié)作方式掌握從環(huán)境地圖構(gòu)建、覆蓋路徑生成到控制指令輸出的完整流程并了解掃描式覆蓋、BCD分解、可見性圖等規(guī)劃算法的工程化實現(xiàn)對深入理解無人機自主飛行系統(tǒng)設計與算法落地具有直接參考價值。代碼中附有詳細注釋與模塊化設計便于二次開發(fā)與算法驗證。 搞無人機自動飛行的朋友大概率都繞不過覆蓋路徑規(guī)劃這道坎。農(nóng)田植保要按壟掃電力巡檢要沿塔巡查災后搜索要把整片區(qū)域跑遍這些任務的本質(zhì)都是同一個問題給定一片區(qū)域讓無人機自動飛出一條路徑既要全覆蓋、不重不漏又要盡量省電、少轉(zhuǎn)彎。這篇文章我就把我的ROS1實現(xiàn)思路、完整代碼結(jié)構(gòu)和調(diào)試經(jīng)驗整理出來用的無人機平臺是Pixhawk飛控加機載電腦通信走MAVROS仿真在Gazebo里跑通了再上真機。1. 覆蓋路徑規(guī)劃的問題定義與算法選型依據(jù)先聊一個容易被新手忽略的問題覆蓋路徑規(guī)劃Coverage Path Planning, CPP和一般的點對點路徑規(guī)劃比如A*、RRT找一條從A到B的路完全是兩回事。點對點路徑規(guī)劃的目標是從哪走到哪最短而覆蓋路徑規(guī)劃的目標是讓傳感器視場覆蓋整個目標區(qū)域路徑本身要充滿整個面而不是一條線。具體來說一個合格的覆蓋規(guī)劃算法要同時滿足三個指標覆蓋率目標區(qū)域內(nèi)沒有被覆蓋到的盲區(qū)要盡可能少理想情況是100%重復率同一塊區(qū)域被掃了多遍的比例要低重復率高意味著浪費電量轉(zhuǎn)彎次數(shù)無人機轉(zhuǎn)彎時往往需要減速甚至懸停能耗比直線巡航高得多次優(yōu)路徑往往轉(zhuǎn)彎特別多這三個指標其實是互相矛盾的。想覆蓋率100%路徑就要密集重復率和轉(zhuǎn)彎次數(shù)也會上來想少轉(zhuǎn)彎路徑就稀疏可能漏掉區(qū)域。所以實際工程里我們做的不是最優(yōu)解而是滿足任務要求的可行解。算法選型上我對比過幾種主流方案算法適用場景優(yōu)點缺點實現(xiàn)難度牛耕式往返掃描規(guī)則矩形/凸多邊形區(qū)域簡單直接、轉(zhuǎn)彎少對凹多邊形和障礙物處理能力弱低單元分解法含梯形分解含障礙物的復雜區(qū)域能處理凹多邊形、障礙物分解邏輯復雜、子區(qū)域拼接要考慮銜接中螺旋式掃描不規(guī)則近圓形區(qū)域適合由外向內(nèi)覆蓋凹邊界容易出問題中基于隨機采樣的覆蓋極不規(guī)則環(huán)境靈活性高覆蓋率不穩(wěn)定、路徑雜亂高我的項目選的是牛耕式往返掃描作為基礎策略。原因很簡單實際作業(yè)里大部分覆蓋任務的目標區(qū)域經(jīng)過簡單處理都可以近似成凸多邊形農(nóng)田是長條形操場是矩形開闊水域也是多邊形。牛耕式的路徑質(zhì)量在這些場景下已經(jīng)接近最優(yōu)而且代碼邏輯清晰調(diào)試成本低。不過如果目標區(qū)域有凹角或者內(nèi)部有禁飛區(qū)純牛耕式就會產(chǎn)生大量繞路。為了解決這個問題我在算法里加了一個區(qū)域預處理環(huán)節(jié)把凹多邊形先做凸分解得到幾個互不重疊的凸子多邊形然后在每個子多邊形內(nèi)部獨立做牛耕式掃描子區(qū)域之間用最短連接線銜接。這里簡單推導一下轉(zhuǎn)彎次數(shù)的計算公式方便大家做可行性驗證。假設覆蓋區(qū)域的長度為L沿航線方向?qū)挾葹閃垂直航線方向傳感器覆蓋寬度為d那么完整的覆蓋航線需要的直線段數(shù)量是n ceil(W / d)轉(zhuǎn)彎次數(shù)大約是n - 1。這個公式看起來簡單但它決定了飛行時間。比如一塊100m x 50m的區(qū)域覆蓋寬度10m那么n 5整條路徑就是5條百米直線加4次轉(zhuǎn)彎。如果無人機巡航速度是8m/s每次轉(zhuǎn)彎含減速、轉(zhuǎn)向、加速耗時約10s那么總飛行時間大約是直線時間 5 * 100 / 8 62.5s 轉(zhuǎn)彎時間 4 * 10 40s 總時間 102.5s轉(zhuǎn)彎時間占了接近40%這就是為什么很多實際的覆蓋規(guī)劃算法會把減少轉(zhuǎn)彎放在比減少總路徑長度更優(yōu)先的位置。有些論文里提到的最優(yōu)覆蓋方向策略本質(zhì)就是通過旋轉(zhuǎn)航線方向找出一個能讓W/d值最小、從而轉(zhuǎn)彎次數(shù)最少的角度一般這個角度會取區(qū)域的主方向或最長軸方向。2. ROS1工程結(jié)構(gòu)設計與核心模塊劃分我的ROS1工程是在Ubuntu 18.04 Melodic上開發(fā)的飛控是Pixhawk 4運行PX4固件機載電腦是NVIDIA Jetson Xavier NX。整個項目的功能包名叫uav_coverage_planner依賴的核心庫有rospyPython ROS接口開發(fā)速度快、numpy路徑計算、shapely多邊形幾何處理、MAVROS和飛控通信。功能包結(jié)構(gòu)如下uav_coverage_planner/ ├── CMakeLists.txt ├── package.xml ├── launch/ │ ├── planner.launch # 主啟動文件 │ └── gazebo_sim.launch # 仿真用 ├── scripts/ │ ├── coverage_planner_node.py # 主節(jié)點區(qū)域處理路徑生成 │ └── waypoint_publisher.py # 航點發(fā)布節(jié)點 ├── config/ │ ├── planner_params.yaml # 算法參數(shù) │ └── mission_area.yaml # 任務區(qū)域定義 ├── rviz/ │ └── coverage_planner.rviz # 可視化配置 └── maps/ └── test_area.png # 測試地圖模塊劃分上我把整個系統(tǒng)拆成三個獨立節(jié)點讓它們各司其職出了問題也好定位區(qū)域管理模塊coverage_planner_node.py負責讀取任務區(qū)域定義做凸分解計算最優(yōu)掃描方向生成航點數(shù)組。這是整個系統(tǒng)的核心算法都在這里。航點執(zhí)行模塊waypoint_publisher.py訂閱核心節(jié)點輸出的航點數(shù)組按順序通過MAVROS的/mavros/mission/push接口上傳任務給飛控或者直接通過/mavros/setpoint_position/global逐點發(fā)送位置指令。狀態(tài)監(jiān)控模塊可選訂閱飛控的GPS狀態(tài)、電池電量、飛行模式負責在緊急情況時暫停任務或返回起飛點。用ROS節(jié)點的方式而不是全都塞在一個程序里最大的好處是每一層都能獨立測試。我在開發(fā)中經(jīng)常遇到飛控不給反應的情況這時候先把航點執(zhí)行節(jié)點停掉手動用rostopic echo檢查核心節(jié)點的輸出一條命令就能確定問題出在規(guī)劃還是通信不需要整個系統(tǒng)重啟。ROS話題設計上我用了四個自定義話題類型和說明如下話題名消息類型說明/coverage_planner/waypointsvisualization_msgs/MarkerArray可視化航點同時在Rviz里顯示路徑/coverage_planner/mission_polygongeometry_msgs/PolygonStamped上報當前規(guī)劃的目標區(qū)域方便核對/coverage_planner/statusstd_msgs/String節(jié)點狀態(tài)信息比如正在規(guī)劃“上傳航點/coverage_planner/takeoff_cmdstd_msgs/Bool自動起飛指令接飛控端3. 牛耕式覆蓋航線的核心代碼實現(xiàn)核心算法的實現(xiàn)我分成了三個步驟區(qū)域預處理、掃描方向計算、航線生成。下面逐個講每個步驟我都會給出關鍵代碼因為我覺得看代碼比看一堆公式要直觀得多。第一步區(qū)域預處理與凸分解這一步的目標是把用戶輸入的任意多邊形轉(zhuǎn)成多個凸多邊形。Shapely庫提供了polygonize和triangulate函數(shù)我優(yōu)先推薦三角剖分轉(zhuǎn)凸分解的方式因為對凹角處理得更干凈。import numpy as np from shapely.geometry import Polygon from shapely.ops import triangulate def preprocess_region(boundary_points): 輸入邊界坐標點輸出凸多邊形列表 # 構(gòu)造多邊形 poly Polygon(boundary_points) if not poly.is_valid: poly poly.buffer(0) # 處理自交情況 # 如果本身就是凸多邊形直接返回 if poly.convex_hull.equals(poly): return [poly] # 凹多邊形三角剖分后按鄰接關系合并成凸子區(qū)域 triangles list(triangulate(poly)) sub_regions [] for tri in triangles: if tri.centroid.within(poly): sub_regions.append(tri) return sub_regions這段代碼里有個細節(jié)值得說poly.buffer(0)是Shapely里處理無效幾何的常用技巧原理是給多邊形加一個約等于0的緩沖距離讓Shapely重新計算幾何關系能修正自交或退化的邊界。這個操作在實戰(zhàn)中經(jīng)常用到特別是當輸入坐標是從地圖軟件里手工點出來、帶了輕微誤差的時候。第二步計算最優(yōu)掃描方向子區(qū)域可能是任意朝向的四邊形直接沿x軸掃描可能產(chǎn)生不必要的轉(zhuǎn)彎。我的做法是計算子區(qū)域的主方向Main Direction即旋轉(zhuǎn)掃描線找到一個角度使得在這個角度下覆蓋次數(shù)最少。實際上最優(yōu)角度往往是區(qū)域最長邊方向或者使用PCA主成分分析求點集的第一個主成分方向近似效果已經(jīng)很好。def compute_scan_direction(polygon): 用PCA計算多邊形頂點的主方向 該方向作為牛耕式掃描的航線方向。 coords np.array(polygon.exterior.coords) centroid coords.mean(axis0) centered coords - centroid # 協(xié)方差矩陣 cov np.cov(centered.T) # 特征值分解最大特征值對應的特征向量即主方向 eig_vals, eig_vecs np.linalg.eig(cov) main_dir eig_vecs[:, np.argmax(eig_vals)] angle np.arctan2(main_dir[1], main_dir[0]) return anglePCA求主方向的方法參考了粒子群算法里尋找最優(yōu)方向的思想——雖然不是全局最優(yōu)但勝在快、穩(wěn)定。如果你對這個方向特別敏感比如農(nóng)田有播種壟向必須沿壟向飛也可以在參數(shù)文件里直接手動指定scan_angle覆蓋自動計算值。第三步生成航點序列拿到了掃描方向和子區(qū)域之后生成航點就是個幾何計算問題。我把子區(qū)域旋轉(zhuǎn)到掃描方向為水平生成水平往返線再旋轉(zhuǎn)回原坐標系。def generate_waypoints(polygon, scan_angle, coverage_width): 根據(jù)子多邊形、掃描方向和覆蓋寬度 生成牛耕式往返航點序列。 # 旋轉(zhuǎn)到掃描角度為0 rotated_poly rotate(polygon, -scan_angle) minx, miny, maxx, maxy rotated_poly.bounds # 按照覆蓋寬度生成平行線 y miny waypoints [] direction 1 # 1表示從左向右-1表示從右向左 while y maxy: x_start minx x_end maxx if direction -1: x_start, x_end x_end, x_start wp_start (x_start, y) wp_end (x_end, y) waypoints.append((wp_start, wp_end)) y coverage_width direction * -1 # 把航線旋轉(zhuǎn)回原坐標系 final_waypoints [] for seg in waypoints: p1 rotate_point(seg[0], scan_angle) p2 rotate_point(seg[1], scan_angle) final_waypoints.extend([p1, p2]) return final_waypoints這里我給出的是簡化的版本實際工程里還需要處理幾個關鍵點航點插值飛控執(zhí)行任務時兩個航點之間的距離如果超過一定閾值需要插值出中間點。我一般設置最大段距為15m超過就線性插值。航線外擴實際作業(yè)時無人機的傳感器視場中心是飛行軌跡而覆蓋區(qū)域是這個中心組成的條帶。所以航線不能剛好從區(qū)域邊界開始而應外擴coverage_width / 2確保邊緣也能覆蓋到。這個偏移量雖然不大但很關鍵不然邊界會漏。轉(zhuǎn)彎弧線對于固定翼或高速飛行的無人機直線航線之間的轉(zhuǎn)彎是弧線。這個在ROS里一般通過加轉(zhuǎn)彎航點實現(xiàn)但我這個項目是旋翼機轉(zhuǎn)彎半徑小直接生成直角轉(zhuǎn)角就行。在上面的代碼基礎上我把完整的主節(jié)點coverage_planner_node.py的核心邏輯補成一個可以直接跑的流程。#!/usr/bin/env python import rospy import numpy as np from shapely.geometry import Polygon from shapely.affinity import rotate from geometry_msgs.msg import PolygonStamped, Point32 from visualization_msgs.msg import Marker, MarkerArray import yaml import math class CoveragePlannerNode: def __init__(self): rospy.init_node(coverage_planner_node) self.waypoint_pub rospy.Publisher( /coverage_planner/waypoints, MarkerArray, queue_size1) self.mission_polygon_pub rospy.Publisher( /coverage_planner/mission_polygon, PolygonStamped, queue_size1) # 加載參數(shù) self.coverage_width rospy.get_param(~coverage_width, 8.0) self.safe_offset rospy.get_param(~safe_offset, 2.0) # 任務區(qū)域從yaml讀取 self.area_points self.load_area() def load_area(self): 從mission_area.yaml讀取目標區(qū)域經(jīng)緯度或本地坐標 with open(rospy.get_param(~area_config), r) as f: config yaml.safe_load(f) return config[area] def run(self): # 簡化主流程 polygon Polygon(self.area_points) angle compute_scan_direction(polygon) wps generate_waypoints(polygon, angle, self.coverage_width) # 發(fā)布marker用于可視化 marker_array MarkerArray() # ... 構(gòu)建marker ... self.waypoint_pub.publish(marker_array) rospy.spin() if __name__ __main__: try: CoveragePlannerNode().run() except rospy.ROSInterruptException: pass4. Gazebo仿真驗證與航點測試算法寫完之后直接上真機是危險的我習慣先在Gazebo里跑PX4的軟件在環(huán)仿真SITL。這個環(huán)節(jié)不復雜但有個關鍵點仿真環(huán)境里要有一個能反映真實場景的地圖模型不然驗證效果打折。我用的仿真搭建方式安裝PX4 SITL環(huán)境和Gazebo# 在Ubuntu 18.04上克隆PX4固件 git clone https://github.com/PX4/PX4-Autopilot.git --recursive cd PX4-Autopilot make px4_sitl gazebo啟動MAVROS讓ROS和仿真飛控建立通信roslaunch mavros px4.launch fcu_url:udp://:14540127.0.0.1:14557啟動我的規(guī)劃功能包roslaunch uav_coverage_planner planner.launch在Rviz里可以直觀看到規(guī)劃出來的航線是否覆蓋了整個區(qū)域同時也能看飛控是否按航線飛行。我一般是先發(fā)一個簡單的小矩形區(qū)域測試比如20m x 15m覆蓋寬度8m這樣理論上是3條航線、2次轉(zhuǎn)彎。如果這個都跑不對后面復雜區(qū)域不用測了。仿真中我遇到的最典型問題有兩個這里先說第一個第二個放在后面踩坑章節(jié)細講。第一個問題是坐標系的混亂。Gazebo里PX4飛控發(fā)出的GPS坐標是模擬的球面坐標經(jīng)緯度而我的規(guī)劃算法是在平面直角坐標系里計算的。如果直接把算法生成的平面坐標上傳給飛控飛控會當成經(jīng)緯度解析航線就會跑到印度洋去。解決方法是在MAVROS里做坐標轉(zhuǎn)換把規(guī)劃航線的平面坐標結(jié)合起飛點的經(jīng)緯度和航向角轉(zhuǎn)換為經(jīng)緯度坐標再上傳。MAVROS提供了/mavros/global_position/global話題里的坐標信息可以直接拿來做轉(zhuǎn)換基準。def local_to_global(local_x, local_y, origin_lat, origin_lon, origin_yaw): 將本地平面坐標轉(zhuǎn)換為GPS經(jīng)緯度WGS84 EARTH_RADIUS 6378137.0 # 先把本地坐標旋轉(zhuǎn)到原坐標系因為規(guī)劃時可能旋轉(zhuǎn)過 cos_yaw math.cos(origin_yaw) sin_yaw math.sin(origin_yaw) dx local_x * cos_yaw - local_y * sin_yaw dy local_x * sin_yaw local_y * cos_yaw # 經(jīng)緯度增量 d_lat dy / EARTH_RADIUS d_lon dx / (EARTH_RADIUS * math.cos(math.radians(origin_lat))) return origin_lat math.degrees(d_lat), origin_lon math.degrees(d_lon)第二個問題是航點跟隨精度。Gazebo里飛控用默認的L1控制器跟隨時如果航點間距太大或變化方向太陡飛機容易沖出航線尤其是在轉(zhuǎn)彎處。我后來在航線密集的地方加了航點插值并且把最大水平速度限制在8m/s以內(nèi)效果好了很多。仿真驗證通過后我的最后一步是在實際場地跑一次小規(guī)模任務一塊50m x 30m的矩形空地覆蓋寬度10m飛行高度30m。實際飛行結(jié)果航線總長約300m飛行時間接近90s覆蓋率通過事后檢查相機拍攝畫面達到了95%以上重復率在10%以下。對于這個規(guī)模的任務結(jié)果完全可以接受。5. 調(diào)試過程中避開的坑與參數(shù)調(diào)優(yōu)心得這個項目做下來我踩了不少坑也總結(jié)了一些經(jīng)驗。前面講過坐標系轉(zhuǎn)換問題這里再說幾個影響比較大的希望后來的人少走彎路。第一個坑ROS1的tf樹過期導致飛控拒收航點。現(xiàn)象是規(guī)劃完整條航線MAVROS推送mission的時候提示Reject waypoint飛控那邊沒有任何反應。查了好幾天才定位到原因是我的機載電腦上時間同步有問題導致ROS的/tf坐標變換樹過期。PX4飛控在接收外部任務時會檢查時間戳的有效性如果tf過期時間超過設定閾值會直接拒絕任務。解決辦法就是在起飛前執(zhí)行sudo apt install chrony sudo chronyd -q pool cn.pool.ntp.org iburst確保系統(tǒng)時間和GPS時間同步。這個坑在真機上特別容易踩反而是Gazebo仿真里因為時間由仿真器控制沒有這個問題。第二個坑MAVROS的mission模式切換時序。自動執(zhí)行任務需要先把飛控切到AUTO.MISSION模式再上傳航點或者先上傳再切換。我一開始是上傳完航點立刻發(fā)模式切換指令但飛控經(jīng)常報錯。后來查PX4源碼這部分我參考了西門子無人機編程實例里的飛控邏輯發(fā)現(xiàn)PX4在接收到MAV_CMD_MISSION_START指令后需要確認航點已經(jīng)全部寫入并且在當前狀態(tài)機的正確狀態(tài)才能切換。解決辦法是在發(fā)布模式切換指令前查詢飛控的mission狀態(tài)rosservice call /mavros/mission/clear rosservice call /mavros/mission/push # 等待mission確認 sleep 3 rosservice call /mavros/set_mode base_mode: 0 custom_mode: AUTO.MISSION這個3秒的sleep看起來很粗糙但確實有效因為飛控內(nèi)部處理mission列表需要時間不給它這個時間窗口后面就會出各種奇奇怪怪的問題。第三個坑覆蓋寬度的標定不能只看傳感器名義值。我最初把覆蓋寬度設成無人機相機視場在地面上的投影寬度但實際飛出來發(fā)現(xiàn)覆蓋率根本不夠。原因是相機安裝在無人機下方會有一點傾斜角度而且飛控的高度控制有誤差±0.5m左右導致實際視場比理論值小。用我的經(jīng)驗安全起見應該把覆蓋寬度設成理論視場寬度的75%這樣雖然重復率會上升一些但能保證不漏覆蓋。如果你用的是RTK高精度定位飛控高度控制很穩(wěn)定這個系數(shù)可以放寬到85%。最后一個實用的調(diào)參心得是關于航點高度。從Pixhawk飛控執(zhí)行mission的經(jīng)驗來看PX4在AUTO.MISSION模式下航點之間會做一個平滑的高度變化。如果你在一條航線上頻繁改變高度飛控的垂直速度會跟不上容易觸發(fā)高度誤差過大報警。建議覆蓋任務盡量保持同一飛行高度只有在跨子區(qū)域時才有必要改變高度并且高度差要小于5m否則就拆成兩個單獨的任務執(zhí)行。6. 從仿真到實機的完整測試鏈路與下一步擴展方向整理一下我的完整測試流程給打算復現(xiàn)這個項目的朋友一個參考階段驗證內(nèi)容耗時估算Gazebo仿真算法邏輯、航點正確性、坐標轉(zhuǎn)換1-2天室內(nèi)視線內(nèi)測試小區(qū)域20m以下航點跟隨精度半天室外小規(guī)模測試50m級矩形區(qū)域完整任務1天室外復雜區(qū)域測試凹多邊形、多子區(qū)域銜接1-2天測試中我養(yǎng)成了一個習慣每次試驗前先記錄起飛點的經(jīng)緯度和朝向飛行結(jié)束后把PX4的ULog日志導出來用plotjuggler查看實際飛行軌跡和規(guī)劃航線的偏差。這樣做一次就能發(fā)現(xiàn)很多肉眼看不到的問題比如某個航點漂移、轉(zhuǎn)彎處軌跡外擴等。關于下一步的擴展方向我目前在做兩個改進。第一個是加入實時避障模塊在機載電腦上接入一個前視攝像頭用深度學習目標檢測識別飛行路徑上的障礙物一旦識別到障礙就把未完成的航線臨時掛起讓飛控切換到OFFBOARD模式繞過障礙后回來繼續(xù)執(zhí)行。第二個是動態(tài)覆蓋現(xiàn)在我的規(guī)劃是靜態(tài)的如果目標區(qū)域有動態(tài)變化比如搜救任務中被困者在移動就需要在飛行過程中重規(guī)劃。目前我在調(diào)研粒子群算法和快速探索隨機樹RRT結(jié)合的方法希望能讓系統(tǒng)在幾秒鐘內(nèi)完成局部重規(guī)劃。最后提一句ROS1雖然年紀不小了但在無人機領域生態(tài)依然很成熟尤其MAVROS和PX4社區(qū)的文檔積累非常多。這個項目用ROS1做原型驗證、算法迭代效率比直接從ROS2開始高很多。如果你所在的項目組沒有強制的ROS2遷移要求我建議先從ROS1搭起這套思路后續(xù)平移ROS2也不算太難。本文還有配套的精品資源點擊獲取