具有光流和ToF傳感器的自主Crazyflie

描述

介紹

無(wú)人機(jī)有多個(gè)傳感器，可以讓它飛行。但他們都無(wú)法識(shí)別近距離的障礙物或與地面的實(shí)際距離。這就是無(wú)人機(jī)需要測(cè)距傳感器的原因。它們提供到途中最近物體的準(zhǔn)確距離讀數(shù)。此外，這些對(duì)于允許路徑規(guī)劃和自主導(dǎo)航的映射目的很有用。

多年來(lái)，我一直在為我的大學(xué)項(xiàng)目使用 Crazyflie 并使用庫(kù)存固件。在找到用 Ada 實(shí)現(xiàn)的 Certyflie 固件后，由于它的簡(jiǎn)單性，我開始對(duì)其進(jìn)行測(cè)試。沒有大量的文件，很容易在短時(shí)間內(nèi)熟悉固件并實(shí)現(xiàn)自己的函數(shù)和庫(kù)。在這里，我計(jì)劃解釋以下主題。

• 粗略介紹 Certyflie 固件的主要功能。
• 集成 ToF 傳感器以測(cè)量高度
• 實(shí)施自主起飛序列
• 使用 ToF 傳感器實(shí)現(xiàn)高度保持功能。
• 介紹如何使用 ToF 傳感器平臺(tái)來(lái)映射環(huán)境。

讓我們開始為這些設(shè)置背景。

設(shè)置背景

這些都是對(duì)我有用的東西，在開始使用 Ada 對(duì) CF 進(jìn)行編程時(shí)不會(huì)遇到太多麻煩。

• 操作系統(tǒng) - Windows 8.1
• GNAT版-2018手臂精靈
• 固件 - Certyflie (ravenscar-cf-stable)

要設(shè)置編譯器路徑，請(qǐng)打開命令提示符，導(dǎo)航到克隆存儲(chǔ)庫(kù)的根目錄并鍵入以下命令。我們需要設(shè)置 GNAT bin 文件夾的路徑。根據(jù)您的安裝目錄更改路徑。

path C:\GNAT\2018\bin;%path%

要將固件上傳到 CF，我們需要 DFU-util。簡(jiǎn)單地說(shuō)，我們可以使用CLI 安裝程序將其安裝在 windows 上。安裝后，您可以dfu-util -l在命令提示符下鍵入以檢查安裝。它應(yīng)該返回已安裝的版本。

Certyflie 存儲(chǔ)庫(kù)的自述文件部分提供了有關(guān)上傳編譯文件的明確說(shuō)明。由于 Windows 沒有 sudo 命令，請(qǐng)確保在最后一個(gè)命令中刪除該部分。

dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D obj/cflie.bin

添加 Z 游俠甲板

CF 平臺(tái)帶有幾個(gè)可拆卸的傳感器板，以擴(kuò)展其功能。Z ranger 甲板有一個(gè)飛行時(shí)間傳感器，它是一個(gè)距離測(cè)量傳感器，可幫助無(wú)人機(jī)保持與地面的恒定高度。

Z ranger 配備 VL53L0X 傳感器，最大感應(yīng)距離為 2m。此傳感器的庫(kù)包含在 Certyflie\Ada_Drivers_Library\components\src\range_sensor 內(nèi)的克隆存儲(chǔ)庫(kù)中。但是，此庫(kù)的新版本可在https://github.com/AdaCore/Ada_Drivers_Library 中找到。在這個(gè)實(shí)現(xiàn)中，我用新的庫(kù)替換了現(xiàn)有的庫(kù)。

要定義傳感器對(duì)象并設(shè)置 I2C 端口，請(qǐng)將庫(kù)添加到stm32-board.ads ，其中，

with VL53L0X;        use VL53L0X;

并添加以下行。

Z_Ranger_Device : VL53L0X_Ranging_Sensor(I2C_EXT_Port'Access, Ravenscar_Time.Delays);

啟動(dòng) CF 后，它會(huì)初始化所有傳感器和組件。此過程在crazyflie_system.adb內(nèi)部System_Init過程中可用。為了將我們的傳感器添加到這個(gè)初始化過程中，我們首先導(dǎo)入 VL53L0X、STM32.board 和 STM32.I2C 庫(kù)。然后我們?cè)诔跏蓟^程中添加以下部分。

Initialize_I2C_GPIO (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all));
Configure_I2C (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all));

Set_Device_Address (Z_Ranger_Device, 16#52#, Status);

Data_Init (Z_Ranger_Device, Status);
Static_Init (Z_Ranger_Device, New_Sample_Ready, Status);
Perform_Ref_Calibration (Z_Ranger_Device, Status);
Set_VCSEL_Pulse_Period_Pre_Range (Z_Ranger_Device, 18, Status);
Set_VCSEL_Pulse_Period_Final_Range (Z_Ranger_Device, 14, Status);
      
Start_Continuous (Z_Ranger_Device, 0 ,Status);

上傳后，您可以給CF上電，用手機(jī)攝像頭檢查ToF傳感器是否工作正常。如果傳感器已成功初始化并開始工作，您會(huì)注意到紫光。

PS - 這里我們使用光流甲板，而不是 Z 游俠甲板。因此，如果您的 Z 游俠套牌看起來(lái)不同，請(qǐng)不要擔(dān)心。pmw3901 傳感器由于飛行無(wú)人機(jī)時(shí)發(fā)生的一些嚴(yán)重碰撞而損壞。

添加到日志

CF 有一個(gè)python 庫(kù)來(lái)執(zhí)行各種任務(wù)。使用原始固件，我們可以通過CF 無(wú)線電模塊使用筆記本電腦與無(wú)人機(jī)進(jìn)行通信。檢索數(shù)據(jù)日志是該庫(kù)最重要的功能之一。它可以訪問傳感器讀數(shù)、無(wú)人機(jī)狀態(tài)、電池電量和許多其他參數(shù)。幸運(yùn)的是，Certyflie 固件的數(shù)據(jù)日志數(shù)量有限，并且可以與 CF python 庫(kù)一起使用。

在示例文件夾中打開。basiclogSync.py此代碼在運(yùn)行時(shí)打印實(shí)時(shí)滾動(dòng)、俯仰和偏航值。

lg_stab.add_variable('stabilizer.roll', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')
lg_stab.add_variable('stabilizer.yaw','float')

在第一行中，“stabilizer”是日志組，“roll”是參數(shù)，“float”是數(shù)據(jù)類型，它與 CF 記錄該特定參數(shù)的數(shù)據(jù)類型相同。stabilizer.adb,您可以在其中找到已定義所有日志組的過程Stabilizer_Init。例如，下面顯示了偏航日志是如何初始化的。

Log.Add_Log_Variable (Group    => "stabilizer",
                               Name     => "yaw",
                               Log_Type => Log.LOG_FLOAT,
                               Variable => Euler_Yaw_Actual'Address,
                               Success  => Dummy)

當(dāng)您運(yùn)行 python 代碼時(shí)，它將連接到 CF 并開始打印值。改變無(wú)人機(jī)的方向，看看值是如何變化的。

現(xiàn)在讓我們將 ToF 傳感器測(cè)量值添加到日志中。這樣我們就有機(jī)會(huì)看到當(dāng)我們改變無(wú)人機(jī)的高度時(shí)值是如何變化的。

首先，我們需要在可用時(shí)從傳感器獲取測(cè)量值。初始化后，系統(tǒng)Stabilizer_Update_Attitude運(yùn)行stabilizer.adb. 這會(huì)更新無(wú)人機(jī)的狀態(tài)參數(shù)。我們?cè)谶@個(gè)函數(shù)中添加了我們的高度測(cè)量部分。接收距離以毫米為單位。這里我們將其轉(zhuǎn)換為 m。

if Range_Value_Available (Z_Ranger_Device) then
   Z_Height := 0.001 * Float (Read_Range_Millimeters (Z_Ranger_Device));
end if;

我添加了一個(gè)名為“Range_Measurements”的單獨(dú)狀態(tài)變量組來(lái)保留這個(gè)變量。然后將以下日志添加到Stabilizer_Init函數(shù)中。

Log.Add_Log_Variable (Group    => "range",
                               Name     => "z_range",
                               Log_Type => Log.LOG_FLOAT,
                               Variable => Z_Height'Address,
                               Success  => Dummy);

要通過 PC 查看這些值，可以在 python 腳本中添加以下行。

lg_stab.add_variable('stabilizer.yaw','float')

?

實(shí)現(xiàn)高度保持功能

Certyflie 固件已經(jīng)具有高度保持功能。它使用預(yù)定義的推力作為基礎(chǔ)值。你可以在里面找到這個(gè)值commander.ads。我假設(shè)這個(gè)值幾乎等于沒有額外傳感器板的 CF 的重量。

ALT_HOLD_THRUST_F : constant := 32_767.0;

由于現(xiàn)在我們有了一種以 1mm 的分辨率測(cè)量距地面距離的方法，因此我們可以使用 z 測(cè)量來(lái)實(shí)現(xiàn)單獨(dú)的高度保持功能。為此，我們需要找到一種方法來(lái)使用我們自己的函數(shù)來(lái)設(shè)置推力值。

為穩(wěn)定器Stabilizer_Control_Loop功能提供推力、滾動(dòng)、俯仰和偏航值，以運(yùn)行電機(jī)使無(wú)人機(jī)飛行。如果仔細(xì)觀察，我們可以確定無(wú)人機(jī)是通過兩種方法激活的。

• 從飛行員命令
• 當(dāng)檢測(cè)到自由落體時(shí)

Pilot 命令使用 CrazyFlie 移動(dòng)應(yīng)用程序發(fā)送。您可以使用移動(dòng)應(yīng)用程序中的虛擬操縱桿連接藍(lán)牙并駕駛無(wú)人機(jī)。這些命令被無(wú)人機(jī)捕獲為 CRTP 數(shù)據(jù)包，并對(duì)消息進(jìn)行解碼以獲取相關(guān)參數(shù)。

使用 IMU 測(cè)量的 Z 加速度值檢測(cè)自由落體。一旦檢測(cè)到自由落體，無(wú)人機(jī)就會(huì)開始產(chǎn)生推力以從墜毀中恢復(fù)并運(yùn)行一個(gè)循環(huán)以減少推力以降低高度。起始推力和減量值可以在 free_fall.ads 中找到。您可以減少遞減值以獲得平穩(wěn)著陸。

MAX_RECOVERY_THRUST       : constant T_Uint16 := 48_000;
RECOVERY_THRUST_DECREMENT : constant T_Uint16 := 100;

通過模仿這些函數(shù)中的任何一個(gè)，我們可以傳遞推力值來(lái)實(shí)現(xiàn)我們自己的高度保持函數(shù)。但我更喜歡模仿飛行員的命令來(lái)實(shí)現(xiàn)。

首先，我在Stabilizer_Control_Loop. 然后我在命令文件中實(shí)現(xiàn)了一個(gè)名為“Autonomous_Sequence”的單獨(dú)函數(shù)，該函數(shù)打開電機(jī)并運(yùn)行 PID 算法以將高度保持在所需的水平。添加以下部分以確保無(wú)人機(jī)在運(yùn)行此功能之前處于水平位置。

if abs (Euler_Pitch_Actual) < 2.0 and abs (Euler_Roll_Desired) < 2.0 then
         Activate_Autopilot := True;
end if;

PID 變量在commander.ads 中定義。

如果您曾經(jīng)調(diào)整過無(wú)人機(jī)的 PID，您可能知道這是一個(gè)非常麻煩的過程。由于無(wú)人機(jī)開始側(cè)向漂移，因此沒有精確的水平位置控制器使這變得更加困難。為了避免這種情況，我從左右兩側(cè)將兩條繩子連接到無(wú)人機(jī)上。這允許無(wú)人機(jī)在我們調(diào)整 PID 值時(shí)在有限區(qū)域內(nèi)移動(dòng)。

以下是高度保持功能的初步測(cè)試。該函數(shù)運(yùn)行 8 秒。這個(gè)時(shí)間可以通過增加循環(huán)計(jì)數(shù)來(lái)增加。所需的高度設(shè)置為 0.1 米。

在調(diào)整 PID 時(shí)，我決定升級(jí)電機(jī)以獲得更高的有效負(fù)載能力，以連接更多傳感器并包括更大的電池。在這個(gè)實(shí)驗(yàn)中，期望的高度是 0.8m。（Kp - 2000，Kd - 0，Ki - 500）。繩子系在兩把椅子上以獲得升高的位置。

VL53L0X 和 VL53L1X

VL53L1X 的最大感應(yīng)范圍為 4m，而我們?cè)诒卷?xiàng)目中使用的 VL53L0X 的最大感應(yīng)范圍為 2m。如上圖所示，VL53L1X 的鏡頭比其他傳感器大。

Z Ranger 甲板 V2配備了這個(gè)新傳感器。在瀏覽了兩個(gè)數(shù)據(jù)表之后，我注意到 VL53L1X 帶有相同的默認(rèn) I2C 地址 (0x29)，這也是 VL53L0X 的地址。此外，大多數(shù)重要寄存器在兩個(gè)傳感器中具有相同的地址。因此，Ada 中的 VL53L0X 庫(kù)可以與 VL53L1X 一起運(yùn)行基本功能，足以滿足我們的要求。

使用多個(gè) ToF 傳感器

很明顯，我們需要無(wú)人機(jī)上更多的傳感器來(lái)覆蓋周圍的環(huán)境。但問題是我們?nèi)绾闻c具有相同 I2C 地址的多個(gè)傳感器進(jìn)行通信。

CF上的STM32芯片主要有2個(gè)I2C口。但一個(gè)端口僅用于 IMU 等內(nèi)部傳感器。其他端口連接到擴(kuò)展引腳以與傳感器板通信。所以我們需要想出一種方法來(lái)使用這個(gè) I2C 端口與多個(gè) ToF 傳感器進(jìn)行通信。

兩種 ToF 傳感器型號(hào)都能夠更改其 I2C 地址。當(dāng)我們將新地址寫入傳感器時(shí)，它會(huì)存儲(chǔ)在易失性存儲(chǔ)器中。因此，我們每次打開設(shè)備并寫入I2C地址時(shí)都需要一個(gè)一個(gè)連接傳感器。為了避免這個(gè)過程，ToF 傳感器帶有一個(gè)稱為 XSHUT 的額外引腳。

要打開傳感器，我們需要拉起這個(gè)引腳。在 Z ranger 面板中，此引腳通過一個(gè)電阻器永久連接到 Vcc。解決方案是編寫一個(gè)程序，使除 z ranger 以外的所有其他傳感器中的 XSHUT 引腳保持低電平，寫入不同的 I2C 地址，將一個(gè) XSHUT 引腳設(shè)置為高電平，寫入不同的 I2C 地址等等。在您有足夠的 GPIO 引腳來(lái)連接所有 ToF 傳感器之前，這看起來(lái)是一個(gè)不錯(cuò)的方法。

為了克服缺少 GPIO 引腳的問題，具有 5 個(gè) ToF 傳感器的 CF Multiranger 平臺(tái)配備了一個(gè) 8 位 GPIO 擴(kuò)展 IC PCA9534。該芯片通過不同的 I2C 地址與無(wú)人機(jī)通信。當(dāng)我們將值寫入其注冊(cè)表時(shí)，它會(huì)根據(jù)給定值將其 8 個(gè) GPIO 引腳設(shè)置為高電平和低電平。這樣，我們就有機(jī)會(huì)僅通過 I2C 端口控制所有 ToF 傳感器。因此，可以毫無(wú)困難地使用與上述 XSHUT 引腳相同的程序。

Ada 庫(kù)帶有幾個(gè) IO 擴(kuò)展器庫(kù)。但不幸的是，它沒有配置PCA9534芯片的庫(kù)。目前，我正在為這個(gè)芯片實(shí)現(xiàn)一個(gè)庫(kù)。附件庫(kù)的 Ada 庫(kù)文件夾中提供了一個(gè) beta 庫(kù)，但它需要更多的工作來(lái)實(shí)現(xiàn)所有可用的功能。同樣，我計(jì)劃在實(shí)施取得??進(jìn)展時(shí)更新存儲(chǔ)庫(kù)。

在這個(gè)實(shí)現(xiàn)之后，我們可以使用所有的距離測(cè)量來(lái)實(shí)現(xiàn)一個(gè)避障功能。

?