1、室內自主探索無人機系統，概述室內無人機飛行系統主要針對室內無衛星導航信號，無合作目標，可對室內 空間進行偵察、重復探測、飛行過程中能夠躲避障礙、能自主任務規劃，實現對 室內各個空間的偵察或巡檢等作業。為了滿足此等作業要求，我們設計出地下空 間環境自主飛行無人機系統總體方案。主要內容包括：1）完成室內無源自主定位系統（TSLAM定位系統）2）基于Linux系統超強運算能力的飛控硬件3）適合室內自主起飛降落的飛控軟件4）實現自主路徑探索的飛行導航軟件5）基于視覺引導降落的視覺系統6）適合室內飛行的無人機飛行平臺二，系統框架與結構框架ext odomLidarIMUfilterTSLAMArdupi

2、lot等控制軟件ext odomLidarIMUfilterTSLAMArdupilot等控制軟件系統框架自主巡檢無人機硬件平臺高侑度光電耦合星程計LIDAR飛行控制器1平臺高侑度光電耦合星程計LIDAR飛行控制器1兩臂S rMU結構框架三，內容詳述1, TSLAM定位系統SLAM是采用同步定位與地圖構建的一種無源建圖定位技術，而TSLAM定位系統作為一整 套室內定位系統的解決方案，其獨創的tiny-EKF融合算法，已融合包含氣壓計、高精度imu、 磁羅盤、激光雷達以及高精度光學編碼里程計等傳感器的數據融合。SLAM系統近年來主在智能機器人領域得到了廣泛的應用，比如掃地機器人、無人車等。 目前

3、SLAM技術中Cartographer開源框架比較先進,該框架適合通用性的SLAM，但對于無人 機這種非二維平面的運動物體來講，該框架要做到穩定的SLAM定位效果較差，容易產生發 散漂移。目前TSLAM定位系統主要針對無人機重新開發算法，更符合無人機在室內環境的 自主定位飛行作業。優點如下：1）、基于環境自然導航，無需對客戶現場環境做任何的改造2）、定位精度滿足絕大部分場景應用3）、配套相應的操作軟件，易學易用4）、變更使用場景簡單，客戶自由操作5）、環境亮度要求低，日常光亮環境、昏暗環境皆可作業6）、無懼磁干擾、多金屬環境2，TLSAM定位系統優勢1），對比相關開源項目Cartographe

4、rHectorslamGmappingGmapping 是基于粒子濾波的SLAM算法，而Cartographer 和Hectorslam 都是基于非線性優化的，現在已經很少用濾波方法來做SLAM 了，絕大多數還 是使用最小二乘法。Cartographer 是有后端的SLAM算法，而Hectorslam是無后端的，前者 更適用于以建圖為重點的應用，后者更適合以定位為重點的應用。Cartographer 在無人駕駛平臺上會遇到如下問題：（1）定位噪聲大：在載具不動的情況下，會產生10cm左右漂移。（2）定位錯誤：由于載具運動會在快速移動或者大角度傾斜移動的情景下會 產生匹配錯誤，在融合了 imu的

5、情況下坐標偏差達到10m以上。（3）效率太低：比如在機載飛控板卡上跑cartographer 時常達到400%的cpu 負載（一個核是100%）（4）通訊受阻：目前使用ROS通信方式為Cartographer為通信框架，ROS屬于實驗性驗 證性工具框架，在ROS topic話題中使用TCP本地傳輸數據，數據從用戶層 進入內核，然后再回到用戶層，造成巨大延遲。不符合實時要求，會造成通 訊延遲，穩定性較差。該項目優化建立新的通訊方式，直接使用指針傳遞函 數。數據通信幾乎實現零延時效果。2），對比算法進行優化達到效果（1）優化相關算法實現準確的同步定位，累計誤差減小。（2）針對載具姿態變化問題做出算

6、法研究，加入融合ekf濾波以后的高精度imu數據做相 關數據處理，讓載具達到至少在30度傾斜角時，仍然良好建圖定位效果。（3）專門對激光匹配做出優化算法，讓載具在速度很快的情況下也能良好定位，能滿足正 常大機動。（4）傳統算法很多冗余的不分，需要對算法實現減法處理拋棄不需要的組件，讓框架輕簡， 能挺高運算速率。（5）拋棄ros通信，實現簡潔通信協議讓整個系統的數據鏈路更加快速。（6）實現了對位姿先驗的支持，可以不限于使用激光測距來實現定位，同其他SLAM算法 一樣，使用輪式里程計，視覺里程計，加速度計均可以作為SLAM匹配器的先驗，讓SLAM 更精確，可以排除大量的匹配錯誤，讓SALM更平滑。

7、（7）完成對后驗位姿的準確度判定算法升級，如果判定得到有明顯的匹配錯誤，則丟棄當 前幀定位與建圖，明顯減少了在場景特征稀少的區域出現位姿發散的問題。3, Tslam構成組件1），點云濾波tslam濾波方法是結合了體素濾波和半徑濾波的，首先我們要改變半徑體 素濾波的規則，我們不再以固定半徑的立方體格子作為濾波單位，轉而使用 變化的半徑，考慮激光雷達的特性，最好是以弧度與半徑作為單位格：類似上圖的蛛網狀扇形切分個數作為一個參數，一圈一圈的半徑切分個數 也作為一個參數。以切分出的扇區作為濾波單元，好處顯而易見，當激光雷 達掃的很遠的時候格子體積自然而然的增大了。然后再使用半徑濾波的思想， 如果某個扇

8、區內的點云個數大于設定的閾值，則保留，也不再取幾何中心點 代替，如果小于，我們直接剔除。這種方法在參數設置合理的情況下幾乎可 以完美的剔除95%以上的離群點。.激光雷達除畸變在機器人運動過程中，每個激光點都在不同的基準位姿上產生。激光掃描時伴隨著機器人的運動，每個角度的激光數據都不是瞬時獲得的，當激光雷達掃描 的頻率比較低的時候，機器人運動帶來的激光幀的運動畸變是不能被忽略的。例如掃描頻率 是5Hz的激光雷達，一幀數據的收尾時間差是200ms，如果機器人以0.5m/s的速度沿著x 方向行走并掃描前面的墻體，那么200ms后尾部的測量距離和首部的測量距離在x方向上 就差10cm。所以如果不是高頻

9、掃描，這種運動畸變是不容忽視的。去除激光雷達運動畸變的原理是把一幀激光雷達數據的每個激光點對應的激光雷達坐標轉 換到不同時刻的機器人里程計上由于imu adis16488的加入，我們可以把它作為一個高精度的慣性里程計使用，我們可以得 到一個搞到1khz的位姿慣導，將激光雷達的每一個激光點，都精確的定位到真實的物理坐 標中。流程：已知當前激光幀的起始時間t1兩個激光束之間的時間間隔t慣性里程計數據按照時間順序存儲在一個隊列中求解當前幀激光數據中的每一個激光點對應的里程計數據（機器人位姿）根據求解的位姿把所有的激光點轉換到同一坐標系下重新封裝成一幀激光數據發布出去經過此流程以后，得到的激光點云的畸

10、變被控制在了以往的1%以內，并且，使用的慣性傳 感器越好，效果越大，實測使用adis16448去除畸變后，飛行載具3m/s的速度運動，對建 圖、定位，均無任何副作用。3），點云匹配Tslam通過對獲取到的點云建立誤差函數進行點云匹配i-i若將圖形xy平面看做姿態位置維度Z軸看做誤差維度（z軸越往上誤差越小）， 誤差函數在三維空間中具有如下屬性：若求得尖峰的xy坐標即可求得最小誤差的位置和姿態。Tslam選擇高斯-牛頓方法解這個最小二乘問題，可以達到足夠高的精度和足 夠快的運算速度，實測匹配速率高達60幀/秒。4)，網格插值全新的算法來解決定位漂移和大速度下匹配失敗的問題，進行網格插值， car

11、tographer 和hectorslam 使用的網格插值法，hectorslam 為了更高的效 率選擇了雙線性插值，cartographer為了更好的效果使用了雙三次插值。效 果并不是特別好。本項目使用插值權重來完成插值，能很好的改善定位漂移相關效果。計算公式： weight = alpha / (x2 + alpha)數據結果：可以看出當alpha取0.005時，x每增加一點點，y值急速下降，y值無線趨近與0,但永遠不等于0。這種方式增加了匹配成功的概率和匹配的精度。5）， tiny_ekfTslam ekf組件確保無人機工作在高頻定位信息狀態。若只使用激光雷達產生定位源，顯然無法滿足無人

12、機飛行所需要的定位頻率，tslam使 用高精度慣導adis16488結合tiny_ekf組件產生高達1khz的定位定姿信息。Tiny_ekf與核心slam模塊異步工作，采用遞歸優化的策略，互相約束，產生高精度的 定位，工作時序分為以下幾個步驟：. imu預積分階段，這一階段，tinyekf不斷獲取imu數據，使用畢卡發進行四元數積 分，同時使用加速度計數據進行位置的線性積分，并同步進行所有狀態變量協方差的推演.將所有計算出的位姿信息進行放入ekf-delay管道，轉入歷史融合階段，這一階段 保證可以融合歷史數據，因為激光雷達，激光測距，氣壓計等傳感器都會有數據延遲，而imu 卻幾乎沒有延遲，為

13、了能夠將所有時間軸對齊，不得不緩存imu歷史數據。.將預積分后，并經過第二階段修正了時間的位姿數據，投遞到slam模塊，作為最 小二乘法的先驗，進行點云匹配。這一步由于高精度imu的參與，得到了最為精確的先驗。 保證了點云匹配的精準和快速。（積分越準確，運算效率越高）.將點云匹配以后的位置信息作為tinyekf的位置測量值，修正所有狀態變量。這一 步就是ekf的測量步驟了，這一步我們可以精確的估計出gyrobias accelbias。.跳轉到第一步，再次迭代由于ekf的收斂性，幾次迭代下來，gyrobias accelbias將越來越精確，所以積分出的姿態和 位置也越來越精確，slam也將越

14、來越快，越來越準。準確的slam位置投遞到ekf測量的時 候，協方差也會越來越小。從而看出，整個系統是一個良性的遞歸優化過程。下面附上數學建模：6).分離式建圖分離式建圖是tslam 一個特別的功能，由于slam屬于自建自定的特性，位置與地圖屬 于遞歸依賴的關系，很難做到萬無一失。Tslam分離式建圖使用雙層地圖模式進行定位，底層地圖使用上一次保存的完整地圖， 上層地圖是此次的建圖，將兩層地圖同時映射到一張地圖上，作為這一次的全局地圖進行定 位。底層地圖運行于只讀模式無法更改，上層地圖可以隨著載體的移動，慢慢地進行slam 過程。底層地圖一般是使用tslam普通模式，緩慢移動載體采集到的，可由

15、工作人員手持設備 進行采集，由于移動緩慢，我們可以得到最為精確的地圖。使用雙層地圖定位，好處顯而易見，就算上層地圖由于載具速度過快，或者其他問題， 得到了錯誤的地圖，也不會導致地圖的發散，因為，底層的地圖具有只讀屬性，始終約束著 激光幀，就算有10多幀的錯誤激光數據，也對定位毫無影響。4,基于Linux系統的飛控硬件Alice是一款專為自動駕駛開發者及愛好者打造的自動駕駛控制器，基于linux實時系統，為 二次開發及高級應用擴展提供了友好的環境；支持Ardupilot和PX4固件，支持遠程GBD調 試，并提供多種即插即用的開發接口。可支持：多旋翼（3-8軸）、固定翼、直升機、垂直起降固定翼、無

16、人車、無人船、無人潛 水艇。強大的硬件處理能力采用基于NXP Cortex-A9架構的i.MX6Q四核處理器，主頻高達1GHz，容量8G，極大的提高 硬件處理能力，例如在運行APM時，CPU使用率僅在10%左右，代碼可擴展能力強大。外設豐富，擴展能力強Linux下，傳感器驅動、USB驅動等各種支持的外設豐富，可方便的擴展到視覺等高級應用開發。實時計算搭載 Linux PREEMPT_RT，延時低于 1ms# cycli ctest -t 10 - * /dev/cpy_dna_larency pelicyz rifdz 1oadavg:o toSB*51ptoL# cycli ctest -t

17、 10 - * /dev/cpy_dna_larency pelicyz rifdz 1oadavg:o toSB*51ptoL0U5 05 0.56 2/151 11210 1 N 1 4 有 7 E9-El - -ae- -aaabBe cTTTTTTTTTT1112 1111)1114)1115)1116111171118)1119)1120) 1121OOOOOOOO0OEs8s8g&s8ppppppppppI-100G 1:1500- I； 200( I：25OQ I；300Q I:55OQI;400G I：45OQ K：500G I：5500C C c t c c c c c cn

18、rlnrlnrnnrlH41 i 41 Ju 41 JI 41 Ju JI JJmmmmmmmmmm8 rM 4 1 607 了 -H a0-5233 7 566317di2o9B7ttttttttctccr-cccr-cccAAAAAAAAAAoooooooooo2 2 2 2 2 2 2 27-223 Avg22 AvgAvg24 Avg23 Avg2:3 Avg29 Avg24 Avg24 Avgzz Avg2424 Max24 Max23 Max24 Max24 Max24 Max23 Max24 Max2fi Max116 7677 5156 4342 SI41 74支持遠程GDB調

19、試（軟硬件斷點，內存斷點，單步步入、步過、跳出），VS 遠程開發在Visual Studio開發環境下，實現遠程附加調試（例如在使用APM或者PX4下，不用再搭 建開發環境，VS下可直接開發）ihrii_upd*ij卜01臺小 if fspafftartu?4 =AFjTFSi iGF5_M_Fn_IDi 1 re Muid ihrii_upd*ij卜01臺小 if fspafftartu?4 =AFjTFSi iGF5_M_Fn_IDi 1 re Muid naad bIitt. r rairdxE-i-fldC：kk hrtlyit T-x, tIdfl-I jr. yl ;it 1 z：

20、r-t_hww_ro_GUrflnrlDsi imd f / Ljnxfffl thL2 tlLlufd- m-Ju grf U h- riwZy EvTbit -=irpLn0 1ih EKF jt i. xp4d -dvt* :nwUhi口lR veLE；f hflh-awll/E-Fai 值.p - ihy*d-c*礎乜 LA, 支持SFTP文件傳輸使用SFTP，全圖形化管理板卡的根文件系統boot192.168A371O1 x過短homelost-l-foundmn-mntmedia- etchomelibboot192.168A371O1 x過短homelost-l-foundmn-

21、mntmedia- etchomelib- lost+found- media dXWKT-JKT-K巨。勺后Thu l-Jazi-19700S:Q4siahome(S)* drwxT-KT-x409Thu01-Jan-197008:02:00sbin(S)才1膽24晚依丁今隋藏I的proc支持遠程桌面基于SSH的遠程圖形界面支持wifi通訊，4G/5G擴展可實現WiFi圖數傳功能產品參數基本參數主處理器NXP i.MX6Q 四核協處理器STM32F405架構Cortex-A9主頻1GHz內存1GB DDRROM8GB eMMC操作系統Linux-RT接口引出I2C1路USB HOST3 路（

22、2 路為 gh1.25-4p 接口, 1 路為 Micro-usb 接口）USB OTG1 路（Micro-usb 2.0 接口）UART3路（包含一個可做debug調試串口）pwm12路（標準8個+擴展4個）s.bus1路adc2路Can1路板載傳感器氣壓計MS5611加速度計/陀螺儀MPU6050磁力計QMC5883LRGBLED1個格尺寸170*73*16（mm）電源輸入DC5V 2A額定功率4W環境溫度050重量97g（鋁殼）5,實現自主路徑探索規劃導航軟件VNAV是一款自主設計的高實時性、高效率的投影匹配地面端操作軟件，結合我們自主研 發的ALICE自駕儀控制器，已經實現了實時操作和顯示激光建模、避障和最優路徑自主 規劃的功能。以下為軟件界面展示vnav viewer回r r ._ _0 bytes/seconGPSO LOCK1參數名值*CA MJSOUTDOORMODE1CAMJS_NEAR_MODE01 1CAMJNT