MICROSTRAIN IMU 與 ARDUPILOT 集成

文章出處：技術(shù)知識(shí) 責(zé)任編輯：深圳市廣陵達(dá)科技有限公司發(fā)表時(shí)間：2024-07-17 09:58:08

無(wú)人機(jī)已經(jīng)擁有自己的內(nèi)部 IMU，但這些 IMU 的精度不足以完成較大的飛行任務(wù)。對(duì)于短途飛行和娛樂(lè)飛行，內(nèi)部 IMU 的精度足以保證無(wú)人機(jī)按預(yù)期運(yùn)行。A

摘要

我們嘗試將 MicroStrain IMU 的支持集成到 Ardupilot 自主飛行軟件套件中。我們這樣做是為了獲得 CSU 的學(xué)分，是與 CSU 無(wú)人機(jī)中心合作開(kāi)展的一項(xiàng)獨(dú)立研究。這個(gè)學(xué)期的大部分時(shí)間都花在了研究和學(xué)習(xí)上，因?yàn)槲覀兪褂玫拇蠖鄶?shù)技術(shù)和現(xiàn)有庫(kù)對(duì)我們來(lái)說(shuō)都是全新的。經(jīng)過(guò)大量的學(xué)習(xí)，我們開(kāi)始在帶有 MicroStrain 3DM-CX5-GNSS/INS IMU 的 Pixhawk 4 上開(kāi)發(fā)和測(cè)試定制的 Ardupilot。學(xué)期結(jié)束時(shí)，我們完成了一次試飛，使用我們的定制 Ardupilot 構(gòu)建，在自主模式下成功發(fā)射并飛行了一架固定翼飛機(jī)，其中只使用了 MicroStrain IMU 的加速度計(jì)、陀螺儀、氣壓計(jì)和磁力計(jì)數(shù)據(jù)，而完全禁用了 Pixhawk 的內(nèi)置 IMU。要將我們對(duì) Ardupilot 的修改應(yīng)用于生產(chǎn)，還有很多工作要做，我們希望將來(lái)能繼續(xù)這個(gè)項(xiàng)目。

MicroStrain IMU 應(yīng)用于無(wú)人機(jī)

1. 簡(jiǎn)介

我們項(xiàng)目的目標(biāo)是創(chuàng)建固件，將高精度慣性測(cè)量單元（IMU）成功集成到現(xiàn)有的無(wú)人機(jī)設(shè)置中。無(wú)人機(jī)已經(jīng)有了自己的內(nèi)部 IMU，但這些 IMU 缺乏完成大型飛行任務(wù)所需的精度。對(duì)于短途飛行和娛樂(lè)飛行，內(nèi)部 IMU 的精度足以保證無(wú)人機(jī)按預(yù)期運(yùn)行。隨著時(shí)間的推移，IMU 讀數(shù)中的微小誤差會(huì)逐漸增加，到長(zhǎng)途飛行結(jié)束時(shí)，總誤差可能會(huì)非常大。通過(guò)使用高精度 IMU，我們可以通過(guò)提高讀數(shù)精度來(lái)延長(zhǎng)無(wú)人機(jī)的飛行時(shí)間，從而減少微小誤差。我們選擇了 MicroStrain IMU，這是一款專為航天器設(shè)計(jì)的高精度 IMU。由于它是為需要幾乎精確測(cè)量的設(shè)備而設(shè)計(jì)的，因此 IMU 的精度可以讓我們的無(wú)人機(jī)在更長(zhǎng)的時(shí)間內(nèi)保持正確的飛行軌跡。

1.1 應(yīng)用

擁有一架可以長(zhǎng)時(shí)間飛行的無(wú)人機(jī)對(duì)執(zhí)行各種任務(wù)非常有用。估價(jià)師和林業(yè)人員都可以使用這些無(wú)人機(jī)對(duì)野生動(dòng)物和火災(zāi)造成的結(jié)構(gòu)性破壞進(jìn)行大規(guī)?？睖y(cè)。目前，全國(guó)范圍內(nèi)的郵政和包裹遞送公司都在研究無(wú)人機(jī)遞送，這就需要無(wú)人機(jī)有足夠的精確度和飛行時(shí)間，以遞送許多包裹。隨著人們探索不同無(wú)人機(jī)和機(jī)器人的更多用途，高精度的選擇甚至可以幫助考古學(xué)家和海洋生物學(xué)家完成他們的工作，到達(dá)人類(lèi)無(wú)法到達(dá)的地方。提高現(xiàn)有無(wú)人機(jī)軟件的精確度為無(wú)人機(jī)幫助人們和取得新發(fā)現(xiàn)提供了更多可能性，這也是我們開(kāi)展這個(gè)項(xiàng)目的原因。

方法

2.1 硬件

在這個(gè)項(xiàng)目中，我們的團(tuán)隊(duì)可以使用三個(gè) MicroStrain IMU 和兩個(gè) Pixhawk 飛行控制器。三個(gè) IMU 分別是 3DMCX5-45、3DM-CX5-AHRS 和 3DM-GX5-GNSS/INS。兩個(gè) Pixhawk 分別是 Pixhawk 4 和 Pixhawk 1。我們的目標(biāo)是通過(guò) UART 電纜將 Pixhawk 4 與 3DM-CX5-45 連接起來(lái)，使用 Pixhawk 上的 UART / I2C B 端口，該端口傳統(tǒng)上用于連接外部 GPS 設(shè)備。我們能夠在所有三個(gè) IMU 上測(cè)試數(shù)據(jù)包配置，并使用 SITL 運(yùn)行我們的自定義 Ardupilot 構(gòu)建，但我們只能使用一個(gè) Pixhawk 4 和一個(gè) 3DM-CX5-45 作為在完整硬件上測(cè)試構(gòu)建的唯一方法。在整個(gè)學(xué)期中，我們多次嘗試使用 Arduino Uno R3 和 Raspberry Pi Pico 設(shè)置額外的硬件配置，但這些嘗試大多導(dǎo)致了更多的復(fù)雜問(wèn)題。事實(shí)證明，使用 SITL 進(jìn)行開(kāi)發(fā)并在 Pixhawk 4 上進(jìn)行測(cè)試是最有效的策略。

2.2 研究階段

2.2.1 微應(yīng)變

2.2.1.1

我們嘗試的第一種方法是使用 MicroStrain 通信庫(kù) (MSCL)。由于創(chuàng)建 MSCL 是為了簡(jiǎn)化與 MicroStrain IMU 的交互，因此 MSCL 似乎是讓 MicroStrain 以我們想要的方式進(jìn)行通信的一個(gè)很有前途的解決方案。在研究 MSCL 的過(guò)程中，我們發(fā)現(xiàn) MSCL 需要許多其他庫(kù)作為依賴庫(kù)，而獲取這些依賴庫(kù)將使構(gòu)建的結(jié)果超過(guò) Pixhawk 4 內(nèi)存的總?cè)萘?。由于?nèi)存是個(gè)問(wèn)題，我們無(wú)法使用 MSCL；不過(guò)，我們能夠了解 MSCL 中每個(gè)數(shù)據(jù)包的校驗(yàn)和是如何計(jì)算的，我們?cè)趧?chuàng)建自己的固件時(shí)使用了這些信息。

2.2.1.2 圖形用戶界面工具

用于設(shè)置和測(cè)試 MicroStrain IMU 的圖形用戶界面工具對(duì)我們的研究和開(kāi)發(fā)也很有幫助。它們使我們能夠在學(xué)期初獲得二進(jìn)制 (BIN) 文件。使用 BIN 文件，我們可以開(kāi)始處理數(shù)據(jù)包并解析出我們需要的數(shù)據(jù)，即使我們的所有團(tuán)隊(duì)成員都無(wú)法訪問(wèn)物理 IMU。圖形用戶界面還能讓我們輕松檢查我們解析的數(shù)據(jù)是否正確。它提供了一個(gè)易于使用的平臺(tái)，使我們能夠快速配置 IMU，以一定的速率傳輸數(shù)據(jù)，而無(wú)需通過(guò) uart 端口發(fā)送數(shù)據(jù)包。我們還利用它進(jìn)行調(diào)試，檢查是否在 Ardupilot 中獲得了與 IMU 相同的值。

2.2.2 Ardupilot

2.2.2.1 前端和后端分離

剛開(kāi)始使用 Ardupilot 時(shí)，我們發(fā)現(xiàn)學(xué)習(xí)代碼庫(kù)以便進(jìn)行擴(kuò)展是一項(xiàng)艱巨的任務(wù)。我們花了幾周時(shí)間研究文檔，試圖了解現(xiàn)有的傳感器庫(kù)是如何為軟件的其他部分提供數(shù)據(jù)的。最初，我們發(fā)現(xiàn) InertialSensor 庫(kù)提供了從大多數(shù)飛行控制器的內(nèi)部 IMU 讀取數(shù)據(jù)的代碼，并認(rèn)為這將是一個(gè)很好的方法。我們記錄并追蹤代碼，以確定 Pixhawk 4 的內(nèi)部 IMU 如何向其他庫(kù)提供數(shù)據(jù)。我們發(fā)現(xiàn)，傳感器庫(kù)被分成兩個(gè)部分，一個(gè)是每個(gè)傳感器模型的特定后臺(tái)類(lèi)，用于直接與硬件通信；另一個(gè)是通用后臺(tái)類(lèi)，用于接收來(lái)自后臺(tái)的數(shù)據(jù)并將其提供給軟件的其他部分。我們集成 MicroStrain IMU 的首次嘗試包括編寫(xiě)一個(gè)新的后端類(lèi)，擴(kuò)展 AP_InertialSensor_Backend 類(lèi)。當(dāng)我們朝著這個(gè)方向前進(jìn)時(shí)，我們開(kāi)始意識(shí)到這種方法更適合集成內(nèi)部傳感器，而不是通過(guò) UART 連接的 IMU。

2.2.2.2 外部 AHRS

在進(jìn)一步探索代碼庫(kù)時(shí)，我們發(fā)現(xiàn) Ardupilot 中新增了 AP_ExternalAHRS 類(lèi)。編寫(xiě)該類(lèi)的目的是為了支持 VectorNav VN-300 IMU 的集成，該傳感器的用途與我們的 MicroStrain IMU 非常相似。我們意識(shí)到，利用該類(lèi)將使我們的工作變得更加輕松。

2.3 開(kāi)發(fā)階段

2.3.1 Ping 和數(shù)據(jù)包解析

為了了解串行設(shè)備如何與計(jì)算機(jī)協(xié)同工作，我們首先用 python 編寫(xiě)了一個(gè)簡(jiǎn)單的腳本，向 IMU 發(fā)送 ping 并從 IMU 接收信息。腳本運(yùn)行后，我們開(kāi)始將腳本翻譯成 C++，以便與同樣使用 C++ 的 Ardupilot 庫(kù)配合使用。兩種語(yǔ)言之間的轉(zhuǎn)換有些棘手。第一個(gè) C++ 腳本是使用以 pyserial 為模型的庫(kù)開(kāi)發(fā)的，但它有太多的依賴關(guān)系，無(wú)法與 Ardupilot 庫(kù)兼容。隨后，C++ 腳本被改寫(xiě)成一個(gè)測(cè)試草圖，使用 Ardupilot 庫(kù)中的串行通信資源，一旦該腳本可以正常工作，就進(jìn)一步開(kāi)發(fā)該腳本以讀取和解析數(shù)據(jù)包。

2.3.2 SITL 測(cè)試

如前所述，在項(xiàng)目過(guò)程中，我們只能使用一對(duì)正常工作的 IMU 和 Pixhawk。這意味著大部分開(kāi)發(fā)和測(cè)試都是在 Ardupilot SITL（環(huán)路軟件）模擬器上完成的。當(dāng)我們學(xué)會(huì)使用 SITL 時(shí)，我們發(fā)現(xiàn)它實(shí)際上提供了許多好處。使用 SITL 運(yùn)行代碼可以更方便地進(jìn)行斷點(diǎn)調(diào)試，而且 SITL 的代碼構(gòu)建速度比硬件快得多。我們?cè)?SITL 開(kāi)發(fā)過(guò)程中遇到的一個(gè)難題是連接 IMU。最終，我們找到了將其插入 USB 端口并將 USB 端口映射到 Ardupilot 串行管理器中的設(shè)備的方法，使我們能夠通過(guò) SITL 有效地測(cè)試與 IMU 的通信。

2.3.3 GDB 調(diào)試

雖然 SITL 的調(diào)試相當(dāng)簡(jiǎn)單，但當(dāng)我們?cè)?Pixhawk 和 IMU 上運(yùn)行新代碼并遇到問(wèn)題時(shí)，故障排除就比較困難了。起初，許多調(diào)試都是通過(guò)在代碼周?chē)胖么蛴≌Z(yǔ)句來(lái)確定故障所在。這種策略有明顯的缺點(diǎn)，即效率低下，而且會(huì)造成更多混亂。為了進(jìn)行硬件斷點(diǎn)調(diào)試，我們購(gòu)買(mǎi)了一臺(tái) Segger J-Link EDU Mini。我們將 JLink GDB 服務(wù)器與內(nèi)置的 Ardupilot GDB 調(diào)試命令結(jié)合使用。這種設(shè)置使我們能夠更精確地分析代碼在硬件上運(yùn)行時(shí)的情況。

2.3.4 測(cè)試草圖內(nèi)部的數(shù)據(jù)包收集

通過(guò)測(cè)試草圖實(shí)現(xiàn)與 IMU 的通信后，下一步就是收集和解析 IMU 的信息。

2.3.4.1 MIP 協(xié)議

雖然我們知道 IMU 工作時(shí)的數(shù)據(jù)包大小和速率，但以指定速率讀取數(shù)據(jù)包的字節(jié)數(shù)并不是一種有效的數(shù)據(jù)包收集方法。我們需要知道數(shù)據(jù)包開(kāi)始和結(jié)束的時(shí)間，處理數(shù)據(jù)包損壞的可能性，并處理數(shù)據(jù)包之間的垃圾字節(jié)。我們想出的解決方案是建立一個(gè)單獨(dú)的線程，以 MicroStrain 向我們提供數(shù)據(jù)的速度，將字節(jié)讀入一個(gè)足夠大的環(huán)形緩沖區(qū)。該線程運(yùn)行一個(gè)循環(huán)，執(zhí)行三項(xiàng)操作：將字節(jié)讀入緩沖區(qū)，將可用字節(jié)解析為數(shù)據(jù)包，當(dāng)構(gòu)建了一個(gè)完整的數(shù)據(jù)包后，解析該數(shù)據(jù)包中的傳感器數(shù)據(jù)并將其發(fā)送出去。MIP 協(xié)議包括兩個(gè)同步字節(jié)，告訴我們數(shù)據(jù)包的開(kāi)始位置，以及一個(gè)弗萊徹校驗(yàn)和，允許我們驗(yàn)證字節(jié)的正確性。環(huán)形緩沖區(qū)允許我們?cè)跇?gòu)建數(shù)據(jù)包時(shí)保留字節(jié)，并在同步字節(jié)后重新開(kāi)始，以防校驗(yàn)和不匹配。

2.3.4.2 數(shù)據(jù)包解析

在研究了 MicroStrain IMU 使用的 MIP 協(xié)議并閱讀了 IMU 手冊(cè)以了解數(shù)據(jù)包結(jié)構(gòu)之后，我們查看了從 IMU 收集到的數(shù)據(jù)包，并開(kāi)始對(duì)其進(jìn)行解析。我們將數(shù)據(jù)包頭和校驗(yàn)和分離到各自的變量中，然后將數(shù)據(jù)包有效載荷發(fā)送到另一個(gè)可以解析數(shù)據(jù)包的函數(shù)中。通過(guò)查看 Ardupilot 的 AP_ExternalAHRS 庫(kù)，我們確定了需要解析的數(shù)據(jù)。起初，使用該庫(kù)非常復(fù)雜，因?yàn)樗饕菫?VectorNav 傳感器構(gòu)建的，但我們發(fā)現(xiàn) Ardupilot 的一位首席開(kāi)發(fā)人員正在創(chuàng)建可分割前端和后端固件的代碼。這種拆分使我們能夠使用現(xiàn)有的后端為 MicroStrain 傳感器進(jìn)行開(kāi)發(fā)。我們以現(xiàn)有實(shí)現(xiàn)為指導(dǎo)，從 MicroStrain 數(shù)據(jù)集中解析信息，并將數(shù)據(jù)發(fā)送到相應(yīng)的處理程序。由于我們首先在測(cè)試草圖中編寫(xiě)了數(shù)據(jù)包解析程序，因此無(wú)論有無(wú)硬件，都能輕松進(jìn)行測(cè)試和調(diào)試，但在將代碼移入 AP_ExternalAHRS 庫(kù)時(shí)，我們必須進(jìn)行修改。

2.3.5 將數(shù)據(jù)包解析移至 Ardupilot Build 中

當(dāng)我們最初將代碼從測(cè)試草圖中移出時(shí)，我們的目標(biāo)是以任何能讓代碼工作的方式將其移入庫(kù)中。我們從測(cè)試草圖中復(fù)制和粘貼了大量代碼，并對(duì)其進(jìn)行了處理，直到它可以構(gòu)建和運(yùn)行為止。這項(xiàng)工作導(dǎo)致了一些混亂的實(shí)現(xiàn)和一些艱苦的編碼，以使構(gòu)建能夠正確運(yùn)行。運(yùn)行成功后，我們又回頭清理了實(shí)現(xiàn)過(guò)程，制作了與我們正在做的事情相關(guān)的函數(shù)，整理了代碼以提高可讀性，并研究了我們?cè)陲w行前無(wú)人機(jī)檢查中需要使用的不同標(biāo)志和字段。在數(shù)據(jù)包解析方面，我們實(shí)現(xiàn)了一個(gè)帶有開(kāi)關(guān)語(yǔ)句的函數(shù)，該函數(shù)可以檢查我們需要的數(shù)據(jù)集中的數(shù)據(jù)包，并將它們發(fā)送到不同的函數(shù)中進(jìn)行更多解析。這些單獨(dú)的函數(shù)確定所給數(shù)據(jù)的類(lèi)型，并將數(shù)據(jù)存儲(chǔ)在相應(yīng)的變量中。這些變量隨后會(huì)被傳遞給不同函數(shù)中的相應(yīng)處理程序。通過(guò)以這種方式構(gòu)建代碼，我們可以在開(kāi)關(guān)語(yǔ)句中引入更多新變量，并構(gòu)建任何必要的函數(shù)來(lái)解析和處理更多數(shù)據(jù)，這使得我們可以繼續(xù)進(jìn)一步集成 IMU，與開(kāi)始集成時(shí)相比相對(duì)容易。

結(jié)論

3.1 成果

經(jīng)過(guò)大量的研究、測(cè)試和開(kāi)發(fā)，我們成功地將定制的 Ardupilot 部署到固定翼平臺(tái)上，并使用僅來(lái)自 MicroStrain IMU 的加速度計(jì)、指南針、氣壓計(jì)和陀螺儀數(shù)據(jù)進(jìn)行飛行。為了達(dá)到這一點(diǎn)，我們必須解決許多棘手的問(wèn)題，其中最主要的是在 Ardupilot 中與 MicroStrain IMU 通信的問(wèn)題，以及將傳感器數(shù)據(jù)傳送到固件中正確位置以便其他 Ardupilot 庫(kù)能夠訪問(wèn)和利用這些數(shù)據(jù)的問(wèn)題。此外，我們還在 ExternalAHRS_MicroStrain 類(lèi)中創(chuàng)建了一個(gè)結(jié)構(gòu)，使我們能夠在未來(lái)實(shí)現(xiàn) GNSS 數(shù)據(jù)。我們的首次試飛在自主模式下出現(xiàn)了嚴(yán)重的振蕩。不過(guò)，飛機(jī)能夠穩(wěn)定下來(lái)，在航點(diǎn)之間飛行，甚至能夠利用 MicroStrain IMU 的數(shù)據(jù)自主起飛。

未來(lái)工作

我們未來(lái)工作的主要目標(biāo)是將 MicroStrain IMU 支持合并到 Ardupilot 庫(kù)中。這將涉及重寫(xiě)我們現(xiàn)有的代碼，使其更加簡(jiǎn)潔并為生產(chǎn)做好準(zhǔn)備，以更加優(yōu)雅的方式實(shí)現(xiàn)當(dāng)前硬編碼的一些功能，并為我們的代碼及其使用方法創(chuàng)建詳盡的文檔。除了這些后勤工作，我們還需要添加對(duì) MicroStrain 數(shù)據(jù)的全面支持。這項(xiàng)工作包括將我們目前讀取的原始數(shù)據(jù)轉(zhuǎn)換為過(guò)濾數(shù)據(jù)，以及添加對(duì) GPS 的支持。此外，我們還需要添加代碼，以便在初始化過(guò)程中發(fā)送數(shù)據(jù)包，確保 IMU 每次都配置正確。我們還將在 Ardupilot 中添加 MicroStrain 參數(shù)，并提供詳細(xì)的文檔說(shuō)明如何構(gòu)建支持 MicroStrain IMU 的 Ardupilot。雖然還有很多工作要做，但我們已經(jīng)克服了最大的障礙，因此完成我們的主要目標(biāo)是可行且有希望的。