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

描述

介紹

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

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

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

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

設(shè)置背景

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

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

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

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

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

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

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

添加 Z 游俠甲板

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

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

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

with VL53L0X;        use VL53L0X;

并添加以下行。

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

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

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傳感器是否工作正常。如果傳感器已成功初始化并開始工作，您會注意到紫光。

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

添加到日志

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

在示例文件夾中打開。basiclogSync.py此代碼在運行時打印實時滾動、俯仰和偏航值。

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)您運行 python 代碼時，它將連接到 CF 并開始打印值。改變無人機(jī)的方向，看看值是如何變化的。

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

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

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

我添加了一個名為“Range_Measurements”的單獨狀態(tài)變量組來保留這個變量。然后將以下日志添加到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')

?

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

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

ALT_HOLD_THRUST_F : constant := 32_767.0;

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

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

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

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

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

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

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

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

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

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

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

VL53L0X 和 VL53L1X

VL53L1X 的最大感應(yīng)范圍為 4m，而我們在本項目中使用的 VL53L0X 的最大感應(yīng)范圍為 2m。如上圖所示，VL53L1X 的鏡頭比其他傳感器大。

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

使用多個 ToF 傳感器

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

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

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

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

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

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

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

?