Crazyflie2 tutorial

最新推荐文章于 2024-09-11 08:00:13 发布

GreatenAnonymous

最新推荐文章于 2024-09-11 08:00:13 发布

阅读量2.5k

点赞数 3

文章标签：自动驾驶 python

原文链接：https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/

版权

Crazyflie2 入门教程

Overview

Overview

Crazyflie2 是BitCraze公司开发的一种小型四轴飞行器, 可以用于科学研究. 本文系crazyflie2的一篇中文入门教程. 想更加深入学习了解的话详见bitcraze官网和论坛.

Crazyflie简介

cf2

组装好的crazyflie2如上图所示, crazyflie2是一种小型四螺旋桨飞行器,质量约为27克, 电池充满大概可以飞行7-10分钟.
crazyflie2有三种模式:

firmware模式: 打开开关, crazyflie发出开机音乐,四个叶片各自转一圈, 这便进入了firmware模式. 飞行的时候使用firmware模式.
bootloader模式: 按住开关2-3秒左右, 背面的蓝色LED大约每秒闪烁一次. 为飞机刷入固件时使用bootloader模式.
DFU模式: 连接USB,一直按住开关,约1秒后蓝灯开始闪烁, 5秒左右后蓝灯闪烁频率加快,此时松开关按钮. 没怎么用过的模式.

crazyflie和其他四螺旋桨飞行器的原理是一样的.螺旋桨旋转所产生的升力(thrust)跟螺旋桨的转速平方成正比, 即 $f_{i}=c_{f}\omega^{2}_i\cdot e_z$ 一条对角线上的两螺旋桨顺时针转而另一条对角线上的俩螺旋桨逆时针转. 这样产生的扭曲力矩(torque)为 $M_{torque}=c_{\tau}\omega^{2}_i \cdot e_z$ 此处 $e_z$ 是局部参考系(body frame)下的z方向单位矢量. 控制飞行器也就是控制四个螺旋桨的转速.

Crazyflie客户端安装

Linux客户端

克隆github项目:

git clone https://github.com/bitcraze/crazyflie-clients-python.git
安装必要的library:

sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg
安装客户端:

pip3 install -e .
运行bin文件夹下的cfclient启动客户端:

python3 bin/cfclient
安装好后便可用CrazyRadio连接crazyflie了. 注意要想使用CrazyRadio连接飞机时需要设置相应的权限. 如下:

sudo groupadd plugdev
sudo usermod -a -G plugdev $USER

重新登录使上述命令生效.然后创建一个名为/etc/udev/rules.d/99-crazyradio.rules的文件并在里面添加

# Crazyradio (normal operation)
SUBSYSTEM==“usb”, ATTRS{idVendor}“1915”, ATTRS{idProduct}“7777”, MODE=“0664”, GROUP=“plugdev”
# Bootloader
SUBSYSTEM==“usb”, ATTRS{idVendor}“1915”, ATTRS{idProduct}“0101”, MODE=“0664”, GROUP=“plugdev”

如果想用USB连接crazyflie的话需要添加访问usb的权限. 创建一个名为/etc/udev/rules.d/99-crazyflie.rules的文件并在里面添加

SUBSYSTEM==“usb”, ATTRS{idVendor}“0483”, ATTRS{idProduct}“5740”, MODE=“0664”, GROUP=“plugdev”

设置好权限后重启. 插入crazyradio,打开crazyflie2为firmware模式, 打开客户端cfclient, 点击scan按钮搜索飞行器设备, 点击connect按钮连接,当crazyflie2上的黄灯变亮便表示已经连接上了。

Windows客户端

打开https://github.com/bitcraze/crazyflie-clients-python/releases.
下载名为 cfclient-win32-install-XXX.exe的文件并安装.
安装Crazyradio usb驱动器: 从http://zadig.akeo.ie/ 上下载 Zadig. 把crazyradio插到电脑上,打开Zadig,选择设备Crazyradio USB Dongle, 选择libusb 点击install安装.

手机客户端

安卓手机和苹果手机可以下载相应的crazyflie app 来控制crazyflie.
打开蓝牙,下好app,开启飞机至firmware模式,在app里点击connect连接飞机. 在app里你需要调节thrust(上升力),yaw(绕z旋转角),pitch(绕y旋转角),roll(绕x旋转角)来控制飞机运动. 个人觉得用app来操控有些难度. cf-app

Crazyflie扩展板(Expansion deck)

扩展板是用来拓展无人机的功能性的. 比如Flow Deck和Lighthouse Deck可以为无人机提供位置信息, Multi-ranger deck可以用来检测周围障碍物避免发生碰撞. 我这里介绍两种我用过的两种扩展板.

Flowdeck

Flowdeck是装在无人机底部的一块扩展板，装的时候有摄像头的一面要向外，板上的VL53L0x ToF 探测器可以高精度地测量无人机到地面的距离而PMW3901光学流探测器可以测量无人机相对地面的运动状态。

安装Flowdeck，装在底部，有摄像头的方向朝下，板上有个打叉加箭头的标志代表正x方向，要与无人机机身板上的箭头方向一致。
为无人机刷入固件(firmware)：在https://github.com/bitcraze/crazyflie-release/releases这里下载最新的firmware-cf2.zip，不用解压。打开客户端，插上crazyradio，开启crazyflie至bootloader模式，菜单上选择Connet–>Bootloader, 然后点击Initiate bootloader cold boot，连接上无人机。然后便可以点击Browse选择下好的固件，点击Program刷入固件，刷好后重启无人机至firmware模式。
安装cflib：pip3 install cflib
把下面示例脚本下载下来，保存为test.py。
运行脚本，python3 test.py.

import logging
import time

import cflib.crtp
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander

URI = 'radio://0/80/250K'  #Change to your cf2's URI

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    with SyncCrazyflie(URI) as scf:
        # We take off when the commander is created
        with MotionCommander(scf) as mc:
            print('Taking off!')
            time.sleep(1)
            # There is a set of functions that move a specific distance
            # We can move in all directions
            print('Moving forward 0.5m')
            mc.forward(0.5)
            # Wait a bit
            time.sleep(1)
            print('Moving up 0.2m')
            mc.up(0.2)
            # Wait a bit
            time.sleep(1)
            print('Doing a 270deg circle');
            mc.circle_right(0.5, velocity=0.5, angle_degrees=270)
            print('Moving down 0.2m')
            mc.down(0.2)
            # Wait a bit
            time.sleep(1)
            print('Rolling left 0.2m at 0.6m/s')
            mc.left(0.2, velocity=0.6)
            # Wait a bit
            time.sleep(1)
            print('Moving forward 0.5m')
            mc.forward(0.5)
            # We land when the MotionCommander goes out of scope
            print('Landing!')

Lighthouse Positioning System

灯塔定位系统类似于Vicon系统，用来定位无人机。这个系统至少需要一块Lighthouse deck和一个lighthouse base station

首先安装Lighthouse base station, 尽量安高一些，调好角度固定好，插上电源。
为无人机装上Lighthouse deck，装在无人机机身上面，板上标有"up front"的一面朝上，板的x方向与无人机的保持一致。
为无人机刷入固件. 下载固件并安装相应library.

git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git
cd crazyflie-firmware
git submodule init
git submodule update

pip3 install numpy
pip3 install opencv-python
sudo apt-get install gcc-arm-none-eabi

将固件里面的tools/make/config.mk.example文件改名为config.mk。打开config.mk加入代码

CFLAGS += -DDISABLE_LIGHTHOUSE_DRIVER=0

开启无人机至bootloader模式，terminal里面输入下列两行命令来编译固件并给无人机刷入固件：

make
make cload
校准系统。在使用灯塔定位系统之前需要校准。首先打开无人机至firmware模式。将无人机放在地板上，处于lighthouse测量范围内的某个地方，这个点作为你的起始点，x方向要对准你需要的方向。运行脚本，

python3 tools/lighthouse/get_bs_geometry.py --uri radio://0/80/2M

上面的radio://0/80/2M改成自己无人机的URI。运行后结果类似于

Reading sensor data…
Connecting to radio://0/80/2M
Connected to radio://0/80/2M
Estimating position of base stations…
{.origin = {0.929796, -1.359615, 3.187089, }, .mat = {{-0.621841, -0.513463, -0.591329, }, {0.451725, -0.851970, 0.264749, }, {-0.639733, -0.102486, 0.761734, }, }},
{.origin = {0.044224, 3.075050, 3.035368, }, .mat = {{-0.367098, 0.847259, -0.383915, }, {-0.658670, -0.528202, -0.535869, }, {-0.656805, 0.056157, 0.751967, }, }},

这些mat矩阵便是需要的校准数据。如果只用一个lighthouse便只有一个校准矩阵，上面的结果表示用了两个lighthouse。把这些矩阵复制，打开固件里的src/modules/src/lighthouse/lighthouse_position_est.c文件，替换lighthouseBaseStationsGeometry[]的内容：

baseStationGeometry_t lighthouseBaseStationsGeometry[PULSE_PROCESSOR_N_BASE_STATIONS] = {
{.origin = {0.929796, -1.359615, 3.187089, }, .mat = {{-0.621841, -0.513463, -0.591329, }, {0.451725, -0.851970, 0.264749, }, {-0.639733, -0.102486, 0.761734, }, }},
{.origin = {0.044224, 3.075050, 3.035368, }, .mat = {{-0.367098, 0.847259, -0.383915, }, {-0.658670, -0.528202, -0.535869, }, {-0.656805, 0.056157, 0.751967, }, }},
};

保存后再次用make和make cload编译固件并为无人机刷入校准后的固件。校准好后的无人机就可以飞行了。注意Lighthouse base station不能再动了，否则要重新校准。
测试飞行。刷好固件后，打开无人机至firmware模式，一开始lighthouse板上红蓝黄三个灯依次亮起，最后红黄两灯熄灭只剩蓝灯亮，这就代表lighthouse deck已经安好正常运行了。运行下面的脚本进行测试

import time
import cflib.crtp
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')
    log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
    log_config.add_variable('kalman.varPX', 'float')
    log_config.add_variable('kalman.varPY', 'float')
    log_config.add_variable('kalman.varPZ', 'float')
    var_y_history = [1000] * 10
    var_x_history = [1000] * 10
    var_z_history = [1000] * 10
    threshold = 0.001
    with SyncLogger(scf, log_config) as logger:
        for log_entry in logger:
            data = log_entry[1]

            var_x_history.append(data['kalman.varPX'])
            var_x_history.pop(0)
            var_y_history.append(data['kalman.varPY'])
            var_y_history.pop(0)
            var_z_history.append(data['kalman.varPZ'])
            var_z_history.pop(0)

            min_x = min(var_x_history)
            max_x = max(var_x_history)
            min_y = min(var_y_history)
            max_y = max(var_y_history)
            min_z = min(var_z_history)
            max_z = max(var_z_history)

            # print("{} {} {}".
            #       format(max_x - min_x, max_y - min_y, max_z - min_z))

            if (max_x - min_x) < threshold and (
                    max_y - min_y) < threshold and (
                    max_z - min_z) < threshold:
                break
def reset_estimator(scf):
    cf = scf.cf
    cf.param.set_value('kalman.resetEstimation', '1')
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', '0')
    wait_for_position_estimator(scf)
    
def activate_high_level_commander(scf):
    scf.cf.param.set_value('commander.enHighLevel', '1')
    
def activate_mellinger_controller(scf, use_mellinger):
    controller = 1
    if use_mellinger:
        controller = 2
    scf.cf.param.set_value('stabilizer.controller', controller)

def run_shared_sequence(scf):
    activate_mellinger_controller(scf, False)

    box_size = 1
    flight_time = 2

    commander = scf.cf.high_level_commander

    commander.takeoff(1.0, 2.0)
    time.sleep(3)

    commander.go_to(box_size, 0, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)

    commander.go_to(0, box_size, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)

    commander.go_to(-box_size, 0, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)

    commander.go_to(0, -box_size, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)

    commander.land(0.0, 2.0)
    time.sleep(2)

    commander.stop()


uris = {
    'radio://0/30/2M/E7E7E7E711', #change to your cf2 URI
    'radio://0/30/2M/E7E7E7E712',
    # Add more URIs if you want more copters in the swarm
}

if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)
    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        swarm.parallel_safe(activate_high_level_commander)
        swarm.parallel_safe(reset_estimator)
        swarm.parallel_safe(run_shared_sequence)

把代码中的“radio://xxxxxx”改成你自己的crazyflie的uri。如果只飞一个飞机的话把另一个uri删去。如果有多块lighthouse deck的话，每一个crazyflie都要校准一次，并在脚本里加入相应的uri。运行脚本便可以起飞。

Crazyflie相关项目

简要介绍两个crazyflie相关的github repo。

CrazyS

CrazyS是RotorS的一个扩展包，里面加入了crazyflie的一些ROS模拟功能。个人感觉这个包还不是很完善，比如里面的crazyflie_hovering_example可以在[0,0,1]处盘旋，但如果把盘旋点(hovering point)从[0,0,1]改成[1,1,1]，无人机不管怎么样都飞不到[1,1,1]，这就很奇怪，理论上调节pid参数可以解决这些问题，但我暂时没调成功过。