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