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

描述

介绍

无人机有多个传感器，可以让它飞行。但他们都无法识别近距离的障碍物或与地面的实际距离。这就是无人机需要测距传感器的原因。它们提供到途中最近物体的准确距离读数。此外，这些对于允许路径规划和自主导航的映射目的很有用。

多年来，我一直在为我的大学项目使用 Crazyflie 并使用库存固件。在找到用 Ada 实现的 Certyflie 固件后，由于它的简单性，我开始对其进行测试。没有大量的文件，很容易在短时间内熟悉固件并实现自己的函数和库。在这里，我计划解释以下主题。

• 粗略介绍 Certyflie 固件的主要功能。
• 集成 ToF 传感器以测量高度
• 实施自主起飞序列
• 使用 ToF 传感器实现高度保持功能。
• 介绍如何使用 ToF 传感器平台来映射环境。

让我们开始为这些设置背景。

设置背景

这些都是对我有用的东西，在开始使用 Ada 对 CF 进行编程时不会遇到太多麻烦。

• 操作系统 - Windows 8.1
• GNAT版-2018手臂精灵
• 固件 - Certyflie (ravenscar-cf-stable)

要设置编译器路径，请打开命令提示符，导航到克隆存储库的根目录并键入以下命令。我们需要设置 GNAT bin 文件夹的路径。根据您的安装目录更改路径。

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

要将固件上传到 CF，我们需要 DFU-util。简单地说，我们可以使用CLI 安装程序将其安装在 windows 上。安装后，您可以dfu-util -l在命令提示符下键入以检查安装。它应该返回已安装的版本。

Certyflie 存储库的自述文件部分提供了有关上传编译文件的明确说明。由于 Windows 没有 sudo 命令，请确保在最后一个命令中删除该部分。

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

添加 Z 游侠甲板

CF 平台带有几个可拆卸的传感器板，以扩展其功能。Z ranger 甲板有一个飞行时间传感器，它是一个距离测量传感器，可帮助无人机保持与地面的恒定高度。

Z ranger 配备 VL53L0X 传感器，最大感应距离为 2m。此传感器的库包含在 Certyflie\Ada_Drivers_Library\components\src\range_sensor 内的克隆存储库中。但是，此库的新版本可在https://github.com/AdaCore/Ada_Drivers_Library 中找到。在这个实现中，我用新的库替换了现有的库。

要定义传感器对象并设置 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内部System_Init过程中可用。为了将我们的传感器添加到这个初始化过程中，我们首先导入 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上电，用手机摄像头检查ToF传感器是否工作正常。如果传感器已成功初始化并开始工作，您会注意到紫光。

PS - 这里我们使用光流甲板，而不是 Z 游侠甲板。因此，如果您的 Z 游侠套牌看起来不同，请不要担心。pmw3901 传感器由于飞行无人机时发生的一些严重碰撞而损坏。

添加到日志

CF 有一个python 库来执行各种任务。使用原始固件，我们可以通过CF 无线电模块使用笔记本电脑与无人机进行通信。检索数据日志是该库最重要的功能之一。它可以访问传感器读数、无人机状态、电池电量和许多其他参数。幸运的是，Certyflie 固件的数据日志数量有限，并且可以与 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”是参数，“float”是数据类型，它与 CF 记录该特定参数的数据类型相同。stabilizer.adb,您可以在其中找到已定义所有日志组的过程Stabilizer_Init。例如，下面显示了偏航日志是如何初始化的。

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

当您运行 python 代码时，它将连接到 CF 并开始打印值。改变无人机的方向，看看值是如何变化的。

现在让我们将 ToF 传感器测量值添加到日志中。这样我们就有机会看到当我们改变无人机的高度时值是如何变化的。

首先，我们需要在可用时从传感器获取测量值。初始化后，系统Stabilizer_Update_Attitude运行stabilizer.adb. 这会更新无人机的状态参数。我们在这个函数中添加了我们的高度测量部分。接收距离以毫米为单位。这里我们将其转换为 m。

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

我添加了一个名为“Range_Measurements”的单独状态变量组来保留这个变量。然后将以下日志添加到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')

 

实现高度保持功能

Certyflie 固件已经具有高度保持功能。它使用预定义的推力作为基础值。你可以在里面找到这个值commander.ads。我假设这个值几乎等于没有额外传感器板的 CF 的重量。

ALT_HOLD_THRUST_F : constant := 32_767.0;

由于现在我们有了一种以 1mm 的分辨率测量距地面距离的方法，因此我们可以使用 z 测量来实现单独的高度保持功能。为此，我们需要找到一种方法来使用我们自己的函数来设置推力值。

为稳定器Stabilizer_Control_Loop功能提供推力、滚动、俯仰和偏航值，以运行电机使无人机飞行。如果仔细观察，我们可以确定无人机是通过两种方法激活的。

• 从飞行员命令
• 当检测到自由落体时

Pilot 命令使用 CrazyFlie 移动应用程序发送。您可以使用移动应用程序中的虚拟操纵杆连接蓝牙并驾驶无人机。这些命令被无人机捕获为 CRTP 数据包，并对消息进行解码以获取相关参数。

使用 IMU 测量的 Z 加速度值检测自由落体。一旦检测到自由落体，无人机就会开始产生推力以从坠毁中恢复并运行一个循环以减少推力以降低高度。起始推力和减量值可以在 free_fall.ads 中找到。您可以减少递减值以获得平稳着陆。

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

通过模仿这些函数中的任何一个，我们可以传递推力值来实现我们自己的高度保持函数。但我更喜欢模仿飞行员的命令来实现。

首先，我在Stabilizer_Control_Loop. 然后我在命令文件中实现了一个名为“Autonomous_Sequence”的单独函数，该函数打开电机并运行 PID 算法以将高度保持在所需的水平。添加以下部分以确保无人机在运行此功能之前处于水平位置。

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

PID 变量在commander.ads 中定义。

如果您曾经调整过无人机的 PID，您可能知道这是一个非常麻烦的过程。由于无人机开始侧向漂移，因此没有精确的水平位置控制器使这变得更加困难。为了避免这种情况，我从左右两侧将两条绳子连接到无人机上。这允许无人机在我们调整 PID 值时在有限区域内移动。

以下是高度保持功能的初步测试。该函数运行 8 秒。这个时间可以通过增加循环计数来增加。所需的高度设置为 0.1 米。

在调整 PID 时，我决定升级电机以获得更高的有效负载能力，以连接更多传感器并包括更大的电池。在这个实验中，期望的高度是 0.8m。（Kp - 2000，Kd - 0，Ki - 500）。绳子系在两把椅子上以获得升高的位置。

VL53L0X 和 VL53L1X

VL53L1X 的最大感应范围为 4m，而我们在本项目中使用的 VL53L0X 的最大感应范围为 2m。如上图所示，VL53L1X 的镜头比其他传感器大。

Z Ranger 甲板 V2配备了这个新传感器。在浏览了两个数据表之后，我注意到 VL53L1X 带有相同的默认 I2C 地址 (0x29)，这也是 VL53L0X 的地址。此外，大多数重要寄存器在两个传感器中具有相同的地址。因此，Ada 中的 VL53L0X 库可以与 VL53L1X 一起运行基本功能，足以满足我们的要求。

使用多个 ToF 传感器

很明显，我们需要无人机上更多的传感器来覆盖周围的环境。但问题是我们如何与具有相同 I2C 地址的多个传感器进行通信。

CF上的STM32芯片主要有2个I2C口。但一个端口仅用于 IMU 等内部传感器。其他端口连接到扩展引脚以与传感器板通信。所以我们需要想出一种方法来使用这个 I2C 端口与多个 ToF 传感器进行通信。

两种 ToF 传感器型号都能够更改其 I2C 地址。当我们将新地址写入传感器时，它会存储在易失性存储器中。因此，我们每次打开设备并写入I2C地址时都需要一个一个连接传感器。为了避免这个过程，ToF 传感器带有一个称为 XSHUT 的额外引脚。

要打开传感器，我们需要拉起这个引脚。在 Z ranger 面板中，此引脚通过一个电阻器永久连接到 Vcc。解决方案是编写一个程序，使除 z ranger 以外的所有其他传感器中的 XSHUT 引脚保持低电平，写入不同的 I2C 地址，将一个 XSHUT 引脚设置为高电平，写入不同的 I2C 地址等等。在您有足够的 GPIO 引脚来连接所有 ToF 传感器之前，这看起来是一个不错的方法。

为了克服缺少 GPIO 引脚的问题，具有 5 个 ToF 传感器的 CF Multiranger 平台配备了一个 8 位 GPIO 扩展 IC PCA9534。该芯片通过不同的 I2C 地址与无人机通信。当我们将值写入其注册表时，它会根据给定值将其 8 个 GPIO 引脚设置为高电平和低电平。这样，我们就有机会仅通过 I2C 端口控制所有 ToF 传感器。因此，可以毫无困难地使用与上述 XSHUT 引脚相同的程序。

Ada 库带有几个 IO 扩展器库。但不幸的是，它没有配置PCA9534芯片的库。目前，我正在为这个芯片实现一个库。附件库的 Ada 库文件夹中提供了一个 beta 库，但它需要更多的工作来实现所有可用的功能。同样，我计划在实施取得​​进展时更新存储库。

在这个实现之后，我们可以使用所有的距离测量来实现一个避障功能。