I 精准落地概述

1\。一般的

无人机正逐渐在生产和生活中得到越来越多的使用。京东物流无人机有望解决用户快递最后一分钟的问题,对无人机精准识别落地的要求也越来越高。经过仔细研究,我决定使用openmv和pixhawk飞控结合apriltag图像识别来完成精准着陆。飞控端固件是apm,不是px4,需要注意,但这并不代表px4固件不能实现精准落地。只是说明apm固件中有完美的精准落地逻辑,openmv例程也提供了相应的代码。

2.apriltag 标签

所谓的apriltag其实就是一个由黑白色块组成的被识别物体。它的本质是最简单的二维码。 Apriltag 分为不同的家族。下面是几个家庭的apriltag标签

3.openmv

Openmv是一个低成本的图像处理模块,可以轻松实现常用的图像处理和串口通信功能。我们第一次接触openmv的时候,可以只关心功能,不了解底层算法的实现,从而简化我们的步骤。 Openmv 可以识别 apriltag 标签并计算其在镜头中的位置。在本项目中,openmv只起到传递着陆位置相关信息的作用,即通过串口通信将数据传回飞控,引导着陆的具体逻辑由飞控完成。 openmv作为传感器,所以可以更换,比如官方的IRLOCK模块。

4.mavlink通信协议

Mavlink通信协议是专门为无人机数据通信制定的通信协议。 openmv通过mavlink通信协议向飞控传输数据。有兴趣的可以研究一下协议报文包中各个代表的含义。

II 精准落地流程

1\。多旋翼无人机调试

无人机调试是在地面站对无人机的传感器、电气调节器、遥控器等部件进行校准,使无人机能够相对平稳的起降。这个非常重要。无人机偏航严重也会影响精准着陆的实现

2\。 openmv与飞控pixhawk的硬件连接

openmv和pixhwak需要连接三根线,两根电源线,一根通讯线。飞控上有TELEM1和TELEM2。两者都可以连接,但后面的参数设置略有不同。至于连接方式,只要能接触良好,可以用杜邦线连接,也可以焊接。 openmv镜头的焦距也要在飞行前调整好,保证清晰度。

3.openmv代码烧写

直接使用官方例程,打开openmv IDE,在example_apriltags_landing_target中选择mavlink。 py文件,可以保存在openmv中。为了方便判断无人机在飞行过程中是否被识别,可以添加LED灯的代码来判断识别的位置是否亮。可以参考openmv函数库中LED的控制函数。

4\。无人机地面站参数设置

需要设置三个参数,PLND_ENABLED 设置为 1(启用),PLND_TYPE 设置为 1。如果使用 TELEM1,则将设置 Serial1_Baud 设置为 115(115200)。如果使用TELEM2,将设置serial2_baud设置为115(115200),参数可以在missionplaner地面站的所有参数表中查找。如果有些参数找不到,可以尝试重新刷新版本的固件解决问题。至于为什么要设置 PLND_ENABLED 和 PLND_ 类型参数在第三部分代码逻辑层面介绍,而 SERIAL_BAUD 参数是波特率的设置。

5\。地面站mavlink消息接收

这一步可以用来判断硬件连接和通讯功能是否有效。无人机连接openmv后,连接地面站。手拿apriltag图片,让openmv识别。 openmv识别后会在地面站的mavlink消息窗口生成两条84HZ的消息,LANDING_TARGET和DISTANCE_SENSOR,如果能收到这两条消息,就证明通信和硬件连接没有问题。

5\。测试

无人机起飞后,控制在目标图像的斜顶附近,切换到着陆模式,无人机开始准确着陆。在这一步,建议去室外空旷的地方进行测试。

III 代码逻辑流程

1.openmv侧码

而(真):

时钟.tick()

img u003d sensor.snapshot()

标签 u003d 排序(img.find_apriltags(fxu003df_x,fyu003df_y,cxu003dc_x,cyu003dc_y),键 u003d lambda x: x.w() * x.h(),反向u003d真)

如果标签和(标签[0].is() 无效\tag\ids):

如果 MAX_DISTANCE_SENSOR_enable: 发送_distance_sensor_packet(tags[0], valid_tag_ids[tags[0].id()])

发送_landing_target_packet(tags[0], img.width(), img.height(), 有效_tag_ids[tags[0].id()])

image.draw\rectangle(标签[0].rect())

img.draw_cross(标签[0].cx(), 标签[0].cy())

print("距离 %f mm - FPS %f" % (z_to_mm(tags[0].z_translation(), 有效_tag_ids[tags[0].id( )])、clock.fps()))

其他:

打印(“FPS %f”%时钟.fps())

这里我只截取主要的逻辑部分,完整的代码可以在openmv ide中找到。这段代码中,如果识别出的标签是目标标签,发送两条消息分别是send_landing_target_packet和send_distance_sensor_packet,有没有发现其实是mavlink消息中收到的消息上一个目录中提到的地面上的窗口。实际上,它就像飞控发送目标位置的横坐标和纵坐标以及距离信息一样。

2\。飞控终端代码

从 github 下载 apm 固件(ardupilot)的源代码。精确着陆相关的代码分布在以下两个位置

ardupilot\libraries\AC_ Precouport \landmode 文件夹 CPP 源文件

3.AC_PrecLand 文件夹

该文件夹包含以下头文件和源文件

有C++基础的应该能看出来,其实这个文件夹下定义了一个基类AC_Preland,其他的继承自AC_preland的派生类。其实我们可以理解为什么这个精准落地过程可以使用openmv、irlock或者其他替代品。原因是这里定义了适用于不同硬件的接口。其实在这个文件夹中,我们可以看到Companion、irlock、SITL和SITL_Gazebo等不同的精准登陆方式,而Companion应该是指那些非官方的硬件,也就是Companion电脑,比如openmv、树莓派等

此代码取自 AC_Preland.h 头文件

枚举 PrecLandType {

PRECLAND_TYPE_NONE u003d 0,

PRECLAND_TYPE_COMPANION,

PRECLAND_TYPE_IRLOCK,

PRECLAND_TYPE_SITL_GAZEBO,

PRECLAND_TYPE_SITL,

};

这段代码定义了枚举变量PrecLandType,分别对应0到1。其实和我们选择的硬件类型是对应的。从这里我们应该明白为什么要在地面站设置参数PLND_Type设置为1。

此代码取自 AC_Preland.h 头文件

无效初始化(uint16_t 更新_rate_hz);

// 如果精确着陆是健康的,则返回 true

bool health() const { 返回 _backend_state.healthy; }

// 如果启用精确着陆,则返回 true(仅用于记录)

bool enabled() const { return _enabled.get(); }

// 返回上次更新时间

uint32_t last_update_ms() const { 返回 _last_update_ms; }

// 返回上次看到目标的时间

uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; }

// 返回估计器类型

uint8_t 估计器_type() 常量 { 返回 _estimator_type; }

// 返回 ekf 异常值计数

uint32_t ekf_outlier_count() 常量 { 返回 _outlier_reject_count; }

// 让驱动程序有机会从传感器获取更新,应该在 400hz 时调用

无效更新(浮动测距仪_alt_cm,布尔测距仪_alt_valid);

// 返回相对于 EKF 原点的目标位置

bool get_target_position_cm(Vector2f& ret);

// 将目标相对位置作为 3D 向量返回

void get\target\position\measurement\cm(Vector3f& ret);

// 返回目标相对于车辆的位置

bool get\target\position\relative\cm(Vector2f&ret);

// 返回相对于车辆的目标速度

bool get_target_velocity_relative_cms(Vector2f&ret);

// 检测到着陆目标时返回true

预订目标\获得();

// 处理一个 LANDING_TARGET mavlink 消息

无效句柄_msg(const mavlink_message_t &msg);

// 参数变量表

静态常量结构 AP_Param::GroupInfo var_info[];

在这段代码中,定义了精确着陆过程中需要的方法,包括状态安全检测、更新时间检测、目标获取时间检测、目标三维向量、目标二维向量,以及扩展卡尔曼滤波器(EKF)相关函数)。

此代码取自 AC_Preland.h 头文件

AP_Int8 _ estimator_type;//精确落地估计器类型

AP_Float _ yaw_align;//机身X轴到传感器x轴的偏航角。

AP_Float_land_ofs_cm_x;//车体框架中目标前方摄像头的期望着陆位置

AP_Float_land_ofs_cm_y;//相机在车身框架中目标右侧的理想着陆位置

AP_Float_ accel_noise;//加速度计过程噪声

AP_Vector3f _ cam_offset;//相机相对CG位置

uint32_t _ last_update_ms;//上次调用更新时的系统时间(毫秒)

bool _ target_acquired;//如果最近看到目标则为真

uint32_t _ last_backend_los_meas_ms;//最后出现的系统时间目标

PosVelEKF _ ekf_x _ekf_y;//卡尔曼滤波器的x轴和y轴

uint32_t _ outlier_reject_count;// mini EKF的异常值计数器(连续3个异常值导致EKF接受更新)

Vector3f _ target_pos_rel_meas_NED;//目标的相对位置作为3D向量

Vector2f _ target_pos_rel_est_NE;//目标相对于IMU的位置不补偿延迟

Vector2f _ target_vel_rel_est_NE;//目标相对于IMU的速度没有补偿滞后

Vector2f _ target_pos_rel_out_NE;//将目标相对于相机的位置馈入位置控制器

Vector2f _ target_vel_rel_out_NE;//将目标相对于CG的速度送入位置控制器

此代码定义了精确着陆过程中要使用的变量

此代码取自 AC_Preland_Companion.cpp 源文件

无效 AC_PrecLand_Companion::handle_msg(const mavlink_message_t &msg)

{

// 解析mavlink消息

__mavlink_landing_target_t 数据包;

mavlink_msg_landing_target_decode(&msg, &packet);

_timestamp_us u003d packet.time_usec;

\distance\to\target u003d packet.distance;

// 计算朝向目标的单位向量

_los_meas_body u003d Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);

\los_meas_body /u003d \los_meas\body.length();

_los_meas_time_ms u003d AP_HAL::millis();

_have_los_meas u003d true;

}

这是mavlink消息处理函数

_ distance_ to_ target u003d packet.distance 将消息包中的距离信息分配给_ distance_ to_ target

_ los_meas_body u003d Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);计算并分配消息包中的 X 和 Y 坐标给一个三维向量进行着陆。这也对应于地面站mavlink消息窗口中的数据。

此代码取自模式 CPP 源文件

无效模式::land_run_vertical_control(bool pause_descent)

{

#if PRECISION_LANDING u003du003d 已启用

const bool navigating u003d pos_control->is_active_xy();

bool doing_precision_landing u003d !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;

#其他

布尔做_precision_landing u003d false;

#endif

}

这段代码定义了垂直方向的控制功能,

#if PRECISION_落地的预处理代码u003d u003d ENABLED用于判断精准落地模式是否为ENABLED。看完这里,我们也应该明白为什么在地面站设置参数PLND_ENABLED设置为ENABLED了。其实就是通过这个标志进入准确落地的入口。

此代码取自模式 CPP 源文件

#if PRECISION_LANDING u003du003d 已启用

布尔做_precision_landing u003d !copter.ap.land_repo_active && copter.precland.target_acquired();

// 运行精确着陆

如果(做_precision_landing){

Vector2f 目标_pos,目标_vel_rel;

if (!copter.precland.get_target_position_cm(target_pos)) {

目标_pos.x u003d 惯性_nav.get_position().x;

目标_pos.y u003d 惯性_nav.get_position().y;

}

if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {

目标_vel_rel.x u003d -inertial_nav.get_velocity().x;

目标_vel_rel.y u003d -inertial_nav.get_velocity().y;

}

pos\control->set\xy_target(target_pos.x, target_pos.y);

pos_control->覆盖_vehicle_velocity_xy(-target_vel_rel);

}

此代码是 CPP 中的模式水平控制功能

目标_pos.x u003d 惯性_nav.get_position().x;

目标_pos.y u003d 惯性_nav.get_position().y;

将目标位置设置为航点位置

pos\control->set\xy_target(target_pos.x, target_pos.y);

进行位置控制,引导无人机到达目标

四、总结、反思与改进

1\。无人机的初始调试非常重要。稳定飞行无人机是准确着陆的保障

2\。建议在户外测试。代码中的位置控制函数被调用,可能需要GPS的支持(GPS有很多功能)

3\。 apritag 二维码的大小直接影响有效识别距离。建议大一点。我测试了A3二维码的识别高度可以达到2.5m,水平范围是1.5m。当然,水平范围受高度影响。

4\。 openmv与pixhawk飞控的连接要稳定,容易接触不良

5\。后期可以通过修改飞控代码来实现识别目标自主着陆的功能。目前采用遥控方式实现落地

6\。感谢阅读,欢迎交流