競(jìng)賽無人機(jī)搭積木式編程

——以2021年電賽國(guó)獎(jiǎng)標(biāo)準(zhǔn)完整復(fù)現(xiàn)為例學(xué)習(xí)

?

????????首先我們需要了解下自動(dòng)飛行任務(wù)執(zhí)行過程幾組關(guān)鍵變量的用法與實(shí)際作用效果：

????????flight_subtask_cnt用于控制飛行任務(wù)中具體執(zhí)行哪一個(gè)子線程，比如我們需要對(duì)偏航角度進(jìn)行控制順時(shí)針旋轉(zhuǎn)90度，我們可以將轉(zhuǎn)動(dòng)過程拆解成兩個(gè)子線程，第一個(gè)線程為設(shè)置目標(biāo)偏航角度，第二個(gè)任務(wù)為執(zhí)行偏航控制并實(shí)時(shí)檢測(cè)偏航控制完成與否，每一個(gè)子線程完成后都會(huì)對(duì)flight_subtask_cnt計(jì)數(shù)器自加，下次運(yùn)行任務(wù)時(shí)會(huì)自動(dòng)進(jìn)入下一線程中繼續(xù)執(zhí)行未完成的子線程。

?

????????新手對(duì)任務(wù)進(jìn)行拆分時(shí)，實(shí)現(xiàn)某一控制任務(wù)可能存在著多種實(shí)現(xiàn)方法，比如在本案例中為了實(shí)現(xiàn)偏航逆時(shí)針旋轉(zhuǎn)90度，我們完成可以不用讓yaw_ctrl_mode工作在CLOCKWISE模式，直接運(yùn)用絕對(duì)偏航角模式AZIMUTH?，在第一步給定期望時(shí)，用當(dāng)前偏航角度WP_AHRS.Yaw減去90作為偏航期望同樣可以實(shí)現(xiàn)上述控制效果，具體過程大家可以根據(jù)自己掌握的熟練程度來針對(duì)性的設(shè)計(jì)。

????????flight_global_cnt用于子線程中判斷是否滿足某些條件的計(jì)數(shù)器，比如當(dāng)判斷條件為位置偏差時(shí)，通過flight_global_cnt的值可以用于判斷航點(diǎn)位置是否抵達(dá)。?

????????execute_time_ms用于子線程設(shè)置子線程的執(zhí)行時(shí)間，比如在上一子線程執(zhí)行完畢后，對(duì)下一子線程執(zhí)行時(shí)間進(jìn)行設(shè)置，在接下來執(zhí)行下一子線程的過程中，會(huì)對(duì)execute_time_ms進(jìn)行自減，execute_time_ms減到0時(shí)表示執(zhí)行時(shí)間完畢，用戶可以通過判斷execute_time_ms是否為0來決策進(jìn)入下一子任務(wù)。?

????????根據(jù)上面的講解，我們知道flight_global_cnt與execute_time_ms都可以用于決策子線程是否執(zhí)行完畢，通常簡(jiǎn)單一點(diǎn)的子線程只需要用二者之一就可以，特殊一點(diǎn)的情況比如在某一子任務(wù)內(nèi)需要結(jié)合視覺處理，但是由于自己寫的視覺代碼識(shí)別失敗、現(xiàn)場(chǎng)某些不可控因素或者自己設(shè)計(jì)的搜索路徑太過冗雜，導(dǎo)致在有限的時(shí)間內(nèi)，程序不能及時(shí)完成本階段子線程，但是競(jìng)賽比賽賽題時(shí)間上是有要求的，所以為了保險(xiǎn)起見，我們可以在判斷位置、視覺標(biāo)記是否完成的基礎(chǔ)上，加上合理的最大執(zhí)行時(shí)間限定，當(dāng)子線程處理超時(shí)會(huì)強(qiáng)制進(jìn)入下一線程或者直接返航降落。

IO控制函數(shù)

void Laser_Light_Work(_laser_light *light)

?

IO控制初始化過程和賦值用法如下：

????????了解了上述自主飛行任務(wù)設(shè)計(jì)關(guān)鍵要點(diǎn)后，我們以2021年全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽中G題植保飛行器題目要求為例，編寫自動(dòng)飛行任務(wù)函數(shù)完成比賽內(nèi)容。

?

?

• ?第一階段——自動(dòng)起飛到航巡高度

uint8_t Auto_Takeoff(float target)//自動(dòng)起飛到某一高度

?

函數(shù)輸入?yún)?shù)target為目標(biāo)高度，自動(dòng)起飛任務(wù)分為兩個(gè)線程，第一步為記錄當(dāng)前3維位置信息，作為導(dǎo)航初始原點(diǎn)位置。并且通過導(dǎo)航控制函數(shù)設(shè)置期望目標(biāo)位置。第二步為實(shí)時(shí)檢測(cè)高度偏差值，連續(xù)2S滿足位置偏差在10cm以內(nèi)后，函數(shù)返回值置1后，自動(dòng)起飛到目標(biāo)高度任務(wù)完成，用法參照Developer_Mode.c開發(fā)者模式中case 11用法，自主起飛任務(wù)完成后會(huì)進(jìn)入case 12執(zhí)行航點(diǎn)遍歷作業(yè)任務(wù)。

?

• 第二階段——航點(diǎn)遍歷作業(yè)任務(wù)

void Agriculture_UAV_Closeloop(void)

第一步生成航點(diǎn)軌跡數(shù)組，記錄當(dāng)前高度height，并發(fā)布第一個(gè)目標(biāo)航點(diǎn)位置0,0,height)，接著會(huì)在第二線程中判斷水平位置誤差，來確定完成水平航點(diǎn)任務(wù)與否。檢測(cè)懸停目標(biāo)完成后會(huì)發(fā)布21號(hào)區(qū)域位置航點(diǎn)。

?

????????第二步無人機(jī)飛向21號(hào)作業(yè)點(diǎn)，同時(shí)實(shí)時(shí)檢測(cè)無人機(jī)是否到達(dá)21號(hào)航點(diǎn)，達(dá)到后判斷底部視覺OPENMV識(shí)別底部顏色情況，當(dāng)無人機(jī)正下方為農(nóng)作物時(shí)，會(huì)設(shè)置激光筆閃爍打點(diǎn)。隨即會(huì)依次發(fā)布28、27、20、19、26、25、...、1、2、3、4航點(diǎn)任務(wù)，飛機(jī)會(huì)依次飛到對(duì)用航點(diǎn)上見結(jié)合OPENMV識(shí)別情況決策激光筆打點(diǎn)與否。

?

????????第三步4號(hào)區(qū)域作業(yè)點(diǎn)遍歷完畢之后，會(huì)發(fā)布導(dǎo)航原點(diǎn)坐標(biāo)，飛到起飛點(diǎn)正上方后，判斷是否對(duì)準(zhǔn)，位置對(duì)準(zhǔn)后飛機(jī)原地降落，直至落到地面后觸發(fā)地面檢測(cè)條件后會(huì)自動(dòng)給無人機(jī)上鎖停槳。

?

• 發(fā)揮部分

?

void Agriculture_UAV_Innovation(void)//發(fā)揮部分

????????發(fā)揮題目部分的前三個(gè)階段和基礎(chǔ)部分一致，故不再贅述，我們直接從4號(hào)區(qū)域播撒完畢開始解析，首先飛機(jī)會(huì)降低巡航高度，到達(dá)巡航高度后會(huì)進(jìn)入下一子進(jìn)程。

?

????????到達(dá)目標(biāo)高度后，飛機(jī)會(huì)飛到14號(hào)作業(yè)區(qū)域，達(dá)到后無人機(jī)會(huì)采用激光雷達(dá)去識(shí)別塔桿的距離和角度方位。這里飛向14號(hào)區(qū)域的目的是使得無人機(jī)飛到賽場(chǎng)中心位置，避免比賽現(xiàn)場(chǎng)人員靠得太近而造成塔桿誤檢或者額外增加不必要的邏輯處理過程。

????????在無人機(jī)到達(dá)14作業(yè)區(qū)域后，無人機(jī)會(huì)根據(jù)檢測(cè)到的塔桿方位、距離信息，首先控制無人機(jī)機(jī)頭對(duì)準(zhǔn)塔桿，然后再靠近塔桿，控制無人機(jī)中心與塔桿的最近距離為50cm。此過程過程中前向機(jī)器視覺OPENMV會(huì)實(shí)時(shí)檢測(cè)是否有識(shí)別到條形碼信息，對(duì)準(zhǔn)和靠近過程中只要有識(shí)別到條碼信息，就會(huì)提前結(jié)束當(dāng)前子線程。

?

????????識(shí)別到條形碼信息后，無人機(jī)會(huì)連續(xù)兩次閃爍條形碼表征的id信息，兩次閃爍的間隔時(shí)間為3S。

?

????????首次閃爍激光筆的過程中，飛機(jī)會(huì)向后回退到14號(hào)作業(yè)區(qū)域，有的同學(xué)可能會(huì)好奇，為什么這里并沒有再次發(fā)布14號(hào)區(qū)域航點(diǎn)，只有基本自動(dòng)飛行支持函數(shù)，為什么會(huì)有自動(dòng)退回到剛開始對(duì)準(zhǔn)機(jī)頭的位置的控制效果呢？這里還是需要回到上一子線程分析：

?

?

????????在34子線程中，我們有對(duì)target_position.x，target_position.y進(jìn)行賦值，并發(fā)布航點(diǎn)位置?？刂仆戤吅髸?huì)進(jìn)入35子線程中。無人機(jī)在機(jī)頭對(duì)準(zhǔn)塔桿的過程中，開始時(shí)target_yaw_err會(huì)大于5°，所以在下方位置-速度控制部分首先會(huì)執(zhí)行②中位置控制部分內(nèi)容，由于fix_flag初始值為0，故OpticalFlow_Control_Pure(fix_flag)控制效果就是原地懸停，懸停鎖定的位置即為34子線程中發(fā)布的14號(hào)作業(yè)區(qū)。

????????接著無人機(jī)會(huì)繼續(xù)轉(zhuǎn)動(dòng)直到機(jī)頭與塔桿的角度偏差小于5°，此時(shí)無人機(jī)水平方向會(huì)進(jìn)入③中的水平速度控制模式，無人機(jī)前后速度期望來源于前后方向的位置偏差計(jì)算出的位置比例控制的輸出值；左右方向速度期望為0，即希望無人機(jī)左右不發(fā)生調(diào)整。

????????在無人機(jī)各項(xiàng)參數(shù)穩(wěn)定的前提下，一次完整的機(jī)頭對(duì)準(zhǔn)—靠近塔桿控制過程中，有且只會(huì)存在一次從②→①的過程，即不存在飛機(jī)在靠近的過程中，機(jī)頭與塔桿之間的方向角不會(huì)再次出現(xiàn)大于5°的情況。當(dāng)且只存在一次只對(duì)準(zhǔn)塔桿（大角度偏差）→對(duì)準(zhǔn)的同時(shí)靠近塔桿（小角度偏差）時(shí)，即不存在②→①→②這一過程，全程位置期望不會(huì)被刷新掉，即位置期望仍為34子線程中的位置刷新值，這種情況下當(dāng)識(shí)別到條形碼后，進(jìn)入36子線程后，就可以實(shí)現(xiàn)自動(dòng)回退到14號(hào)作業(yè)區(qū)的效果。當(dāng)然此處完全可以用位置導(dǎo)航函數(shù)Horizontal_Navigation重新發(fā)布一個(gè)14號(hào)作業(yè)點(diǎn)坐標(biāo)，實(shí)現(xiàn)主動(dòng)回退到14作業(yè)區(qū)。

????????在第二次閃爍條形碼id值后，無人機(jī)會(huì)飛到4號(hào)作業(yè)區(qū)正下方的區(qū)塊(200,-50,height)上，然后飛到起飛點(diǎn)正下方的區(qū)塊(0,-50,height)上。

?

????????到達(dá)起飛點(diǎn)正下方的區(qū)塊(0,-50,height)后，無人機(jī)會(huì)根據(jù)條形碼id值計(jì)算出降落點(diǎn)并發(fā)布降落點(diǎn)坐標(biāo)，無人機(jī)會(huì)飛到降落點(diǎn)上方后自動(dòng)降落到底面完成整個(gè)飛行任務(wù)。

????????在熟練掌握本案例中的各個(gè)API用法前提下，用戶進(jìn)行二次開發(fā)可以把機(jī)載計(jì)算機(jī)端當(dāng)做一個(gè)能提供高精度位置信息的傳感器來使用就可以，新手用戶可以實(shí)現(xiàn)不用在機(jī)載計(jì)算機(jī)下編寫代碼的情況下，就可以完成比賽中的所有內(nèi)容。在有一定基礎(chǔ)積累后，就可以考慮在機(jī)載計(jì)算機(jī)下用ROS系統(tǒng)做導(dǎo)航、避障、路徑規(guī)劃去實(shí)現(xiàn)比賽內(nèi)容，解決問題的途徑會(huì)更多，處理起來也會(huì)更加靈活，另外利用機(jī)載計(jì)算機(jī)端單目攝像頭OPENCV處理視覺問題會(huì)比傳統(tǒng)OPENMV更加強(qiáng)大、高效。

?

NC360深度開源競(jìng)賽無人機(jī)開發(fā)平臺(tái)

電賽G題植保無人機(jī)國(guó)獎(jiǎng)標(biāo)準(zhǔn)方案學(xué)習(xí)樣例

https://www.bilibili.com/video/BV11T4y1v7uW?spm_id_from=333.999.0.0

?

https://www.bilibili.com/video/BV1QR4y1F7vj?spm_id_from=333.999.0.0