基于Crazyflie和TOF傳感器的自旋停效果實(shí)現(xiàn)

今天小編給大家?guī)淼氖切录悠碌腗aker Chathuranga Liyanage使用TOF傳感器讓Crazyflie實(shí)現(xiàn)高度自保持的項(xiàng)目。

設(shè)備列表

硬件：

Crazyflie2.0

Bitcraze Crazyradio PA

Z-ranger deck

軟件：

AdaCore GNAT Community

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

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

粗略了解 Certyflie 固件的主要功能。

集成ToF傳感器以測量高度

實(shí)現(xiàn)自主起飛序列

使用ToF傳感器實(shí)現(xiàn)高度保持功能。

介紹如何實(shí)現(xiàn)ToF傳感器甲板以映射環(huán)境。

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

設(shè)置背景

這些對我來說是有用的，而不會給使用 Ada 開始編程 CF 帶來太多麻煩。

操作系統(tǒng) - Windows 8.1

GNAT 版本 - 2018 arm elf

固件 - Certyflie （ravenscar-cf-stable）

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

path C:GNAT2018in;%path%

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

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

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

添加 ZRangerDeck

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

Z ranger 配備 VL53L0X 傳感器，最大感應(yīng)距離為 2m。此傳感器的庫包含在 CertyflieAda_Drivers_Librarycomponentssrc ange_sensor 內(nèi)的克隆存儲庫中。但是，此庫的新版本在 https://github.com/AdaCore/Ada_Drivers_Library 中可用。在此實(shí)現(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通電后，它會初始化所有傳感器和組件。此過程在 內(nèi)部 過程中可用。為了將我們的傳感器添加到這個初始化過程中，我們首先導(dǎo)入VL53L0X、STM32.board和STM32。I2C 庫。然后我們在初始化過程中添加以下部分。

crazyflie_system.adbSystem_Init

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

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

添加到日志

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

在 examples 文件夾內(nèi)打開。此代碼在運(yùn)行時打印實(shí)時滾動、俯仰和偏航值。

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

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

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

Stabilizer_Update_Attitudestabilizer.adb

if Range_Value_Available (Z_Ranger_Device) then


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

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

Stabilizer_Init

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 固件已具有高度保持功能。它使用預(yù)定義的推力作為基值。您可以在 中找到此值。我假設(shè)這個值幾乎等于 CF 的重量，沒有額外的傳感器甲板。

commander.ads

ALT_HOLD_THRUST_F : constant := 32_767.0;

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

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

Stabilizer_Control_Loop

從飛行員命令

當(dāng)檢測到自由落體時

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

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

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

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

首先，我注釋掉了 Commander_Get_RPY、FF_Get_Recovery_Commands、Commander_Get_Thrust 和 FF_Get_Recovery_Thrust 函數(shù)調(diào)用。然后，我在指令器文件中實(shí)現(xiàn)了一個名為“Autonomous_Sequence”的單獨(dú)函數(shù)，該函數(shù)打開電機(jī)并運(yùn)行 PID 算法以將高度保持在所需水平。添加以下部分，以確保無人機(jī)在運(yùn)行此功能之前處于水平位置。

Stabilizer_Control_Loop

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

以下是高度保持功能的初始測試。該函數(shù)運(yùn)行 8 秒?？梢酝ㄟ^增加循環(huán)計(jì)數(shù)來增加此時間。所需高度設(shè)置為0.1米。

在調(diào)整PID時，我決定升級電機(jī)以獲得更高的有效載荷能力，以連接更多的傳感器并包括更大的電池。CF電機(jī)驅(qū)動器的N溝道MOSFET的最大額定電流為6A，為我們提供了更大的空間來增加電機(jī)尺寸。如封面圖所示，現(xiàn)有的 0716 空心杯電機(jī)被 0720 電機(jī)取代。

以下視頻顯示了使用新電機(jī)和進(jìn)一步調(diào)諧的 PID 的高度保持功能。在本實(shí)驗(yàn)中，所需的高度為0.8m。（Kp - 2000，Kd - 0，Ki - 500）。繩子被綁在兩把椅子上，以獲得更高的位置。

VL35LOX和VL35L1X

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

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

使用多個TOF傳感器

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

CF 上的 STM32 芯片主要有 2 個 I2C 端口。但一個端口僅用于內(nèi)部傳感器，例如 IMU。其他端口連接到擴(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引腳的相同過程。

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

實(shí)現(xiàn)此結(jié)果后，我們可以使用所有距離測量值實(shí)現(xiàn)避障功能。

審核編輯：劉清