ArduPilot开源飞控之AP_VisualOdom

🕗 发布于 2024-07-13 02:50 开源 ardupilot

1. 源由

AP_VisualOdom主要是配合视觉定位应用，通常视觉定位应用于室内或者无GPS等辅助定位系统的环境。

2. 类定义

AP_VisualOdom 类用于视觉里程计系统在无人机上的实现。这个类具有多种方法和属性，用于初始化和操作视觉里程计传感器。

这些私有成员变量用于存储类的内部状态，包括传感器类型、位置偏移、方向、缩放比例、延迟时间、噪声等信息，以及一个后端驱动指针 _driver。

总体来说，这个类设计用于处理和管理无人机上的视觉里程计传感器，提供了丰富的接口和功能来初始化、操作和查询传感器状态。

2.1 类与构造函数

class AP_VisualOdom
{
public:
    AP_VisualOdom();
    static AP_VisualOdom *get_singleton() {
        return _singleton;
    }

AP_VisualOdom 类是一个包含各种方法和属性的类。
AP_VisualOdom 构造函数用于初始化对象。
get_singleton 静态方法返回一个单例模式的实例。

2.2 枚举类型

enum class VisualOdom_Type {
    None = 0,
#if AP_VISUALODOM_MAV_ENABLED
    MAV = 1,
#endif
#if AP_VISUALODOM_INTELT265_ENABLED
    IntelT265 = 2,
    VOXL = 3,
#endif
};

VisualOdom_Type 枚举定义了几种视觉里程计类型，如 None, MAV, IntelT265 和 VOXL。

2.3 公共方法

void init();
bool enabled() const;
bool healthy() const;
enum Rotation get_orientation() const;
float get_pos_scale() const;
const Vector3f &get_pos_offset(void) const;
uint16_t get_delay_ms() const;
float get_vel_noise() const;
float get_pos_noise() const;
float get_yaw_noise() const;
int8_t get_quality_min() const;
int8_t quality() const;

这些方法提供了对视觉里程计传感器的各种操作和查询功能：

init：检测并初始化传感器。
enabled：返回传感器是否启用。
healthy：返回传感器是否处于健康状态（是否接收到数据）。
get_orientation：获取用户定义的传感器方向。
get_pos_scale：获取应用于位置估计的缩放比例。
get_pos_offset：获取相机相对于机体坐标系原点的位置偏移。
get_delay_ms：获取传感器的延迟时间（毫秒）。
get_vel_noise：获取速度测量噪声。
get_pos_noise：获取位置测量噪声。
get_yaw_noise：获取航向测量噪声。
get_quality_min：获取质量阈值。
quality：返回质量测量值。

2.4 消息处理

#if HAL_GCS_ENABLED
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
#endif
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);

这些方法用于处理视觉里程计传感器的消息：

handle_vision_position_delta_msg：处理视觉位置增量消息（仅在 HAL_GCS_ENABLED 时可用）。
handle_pose_estimate：处理姿态估计数据。
handle_vision_speed_estimate：处理速度估计数据。

2.5 其他功能

void request_align_yaw_to_ahrs();
void align_position_to_ahrs(bool align_xy, bool align_z);
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
VisualOdom_Type get_type(void) const {
    return _type;
}

request_align_yaw_to_ahrs：请求将传感器的航向与车辆的姿态参考系统（AHRS/EKF）对齐。
align_position_to_ahrs：更新位置偏移以对齐AHRS位置。
pre_arm_check：在启动检查时返回 false 表示检查失败，并提供失败消息。
get_type：获取传感器类型。

2.6 私有成员

private:
    static AP_VisualOdom *_singleton;
    AP_Enum<VisualOdom_Type> _type;
    AP_Vector3f _pos_offset;
    AP_Int8 _orientation;
    AP_Float _pos_scale;
    AP_Int16 _delay_ms;
    AP_Float _vel_noise;
    AP_Float _pos_noise;
    AP_Float _yaw_noise;
    AP_Int8 _quality_min;
    AP_VisualOdom_Backend *_driver;
};

3. 框架设计

3.1 启动代码 AP_VisualOdom::init

传统Ardupilot启动方式。

AP_Vehicle::setup
 └──> visual_odom.init

AP_VisualOdom::init()
└── switch (VisualOdom_Type(_type.get()))
    ├── case VisualOdom_Type::None:
    │   └── // do nothing
    ├── #if AP_VISUALODOM_MAV_ENABLED
    │   ├── case VisualOdom_Type::MAV:
    │   │   └── _driver = new AP_VisualOdom_MAV(*this);
    │   └── #endif
    ├── #if AP_VISUALODOM_INTELT265_ENABLED
    │   ├── case VisualOdom_Type::IntelT265:
    │   ├── case VisualOdom_Type::VOXL:
    │   │   └── _driver = new AP_VisualOdom_IntelT265(*this);
    │   └── #endif
    └── default:
        └── // handle default case if necessary

根据不同的传感器类型选择性地实例化不同的后端驱动对象，从而初始化并准备使用视觉里程计系统。

VisualOdom_Type::None：如果 _type 是 None，则什么也不做。
VisualOdom_Type::MAV：如果 _type 是 MAV，则创建一个 AP_VisualOdom_MAV 的实例并将其赋给 _driver。
VisualOdom_Type::IntelT265 或 VisualOdom_Type::VOXL：如果 _type 是 IntelT265 或 VOXL，则创建一个 AP_VisualOdom_IntelT265 的实例并将其赋给 _driver。
其他情况：可能需要处理默认情况，具体实现取决于代码中是否有定义。

3.2 消息处理

3.2.1 AP_VisualOdom::handle_vision_position_delta_msg

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK::packetReceived
             └──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_POSITION_DELTA
                 └──> GCS_MAVLINK::handle_vision_position_delta
                     └──> AP_VisualOdom::handle_vision_position_delta_msg

handle_vision_position_delta_msg(msg)
├── if HAL_GCS_ENABLED
│   ├── if !enabled()
│   │   └── return
│   └── if _driver != nullptr
│       └── _driver->handle_vision_position_delta_msg(msg)
└── (end of method)

如果 HAL_GCS_ENABLED 没有定义，整个方法将被忽略，不会编译和执行。
如果 enabled() 方法返回 false，则立即退出方法，不执行后续的逻辑。
如果 _driver 不为 nullptr，则调用 _driver 对象的 handle_vision_position_delta_msg 方法，将 msg 作为参数传递给它。

这样的设计确保了在满足条件且传感器已启用时，有效地将接收到的视觉位置增量消息传递给相应的后端处理。

3.2.2 AP_VisualOdom::handle_pose_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK::packetReceived
             └──> GCS_MAVLINK::handle_message //MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE
                 └──> GCS_MAVLINK::handle_vision_position_estimate/handle_global_vision_position_estimate/handle_vicon_position_estimate
                     └──> GCS_MAVLINK::handle_common_vision_position_estimate_data
                         └──> AP_VisualOdom::handle_pose_estimate

handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
│
├── if (!enabled())
│   └── return;
│
└── if (_driver != nullptr)
    │
    ├── Quaternion attitude
    │   └── attitude.from_euler(roll, pitch, yaw)
    │
    └── _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality)

这段代码是用于处理姿态估计数据并将其发送给扩展卡尔曼滤波器（EKF）的方法。
目的是将来自视觉里程计传感器的姿态估计数据传送给后端处理，以便进一步的状态估计和导航。

参数：
- remote_time_us：远程时间戳（微秒）。
- time_ms：本地时间戳（毫秒）。
- x, y, z：位置坐标（米）。
- roll, pitch, yaw：姿态角（弧度）。
- posErr：位置误差。
- angErr：角度误差。
- reset_counter：重置计数器。
- quality：质量评估（-1 表示失败，0 表示未知，1 最差，100 最佳）。
功能：
- enabled() 方法检查传感器是否启用，如果未启用则立即返回。
- 如果 _driver 驱动器不为空，创建一个 Quaternion 对象 attitude，通过 from_euler(roll, pitch, yaw) 方法将欧拉角转换为四元数。
- 调用 _driver->handle_pose_estimate() 方法，将处理过的姿态估计数据和质量评估传递给后端处理。
API：
- uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t qualit
- uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality

// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
{
    // exit immediately if not enabled
    if (!enabled()) {
        return;
    }

    // call backend
    if (_driver != nullptr) {
        // convert attitude to quaternion and call backend
        Quaternion attitude;
        attitude.from_euler(roll, pitch, yaw);
        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
    }
}

// general purpose method to consume position estimate data and send to EKF
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
{
    // exit immediately if not enabled
    if (!enabled()) {
        return;
    }

    // call backend
    if (_driver != nullptr) {
        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
    }
}

3.2.3 AP_VisualOdom::handle_vision_speed_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK::packetReceived
             └──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE/MAVLINK_MSG_ID_ODOMETRY
                 └──> GCS_MAVLINK::handle_vision_speed_estimate/handle_odometry
                     └──> AP_VisualOdom::handle_vision_speed_estimate

handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)
└── if not enabled()
    └── return
└── if _driver != nullptr
    └── _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)

用于处理视觉里程计传感器的速度估计数据，并将其发送到扩展卡尔曼滤波器（EKF）的通用方法。

函数签名和说明：
- 函数名：handle_vision_speed_estimate
- 参数：
  - remote_time_us：远程时间戳（微秒）
  - time_ms：本地时间戳（毫秒）
  - vel：速度估计的三维向量（NED坐标系，单位为米每秒）
  - reset_counter：重置计数器
  - quality：质量评估，-1 表示失败，0 表示未知，1 表示最差，100 表示最佳。
功能说明：
- 首先，检查视觉里程计是否启用（调用了类的 enabled() 方法）。如果未启用，则立即返回，不执行后续操作。
- 然后，检查 _driver 指针是否为 nullptr。如果 _driver 不为空，则调用 _driver 对象的 handle_vision_speed_estimate 方法，将速度估计数据传递给后端处理。
执行流程：
- 如果视觉里程计未启用，函数直接返回，不执行后续任何操作。
- 如果视觉里程计已启用且 _driver 指针有效，则通过 _driver 对象处理速度估计数据。

3.3 辅助函数

3.3.1 AP_VisualOdom::enabled

通过判断类型设置，确认视觉传感是否被使能。

// return true if sensor is enabled
bool AP_VisualOdom::enabled() const
{
    return ((_type != VisualOdom_Type::None));
}

3.3.2 AP_VisualOdom::healthy

首先检查传感器是否已启用，然后再检查是否有有效的驱动程序实例来处理传感器数据。
只有在这两个条件都满足时，才会进一步检查传感器的健康状态并返回结果。

healthy()
├── if (!enabled())
│   └── return false
├── if (_driver == nullptr)
│   └── return false
└── return _driver->healthy()

3.3.3 AP_VisualOdom::quality

返回质量测量值（百分比）。

0：Unknown
1：Worst
100：Best

// return quality as a measure from 0 ~ 100
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
int8_t AP_VisualOdom::quality() const
{
    if (_driver == nullptr) {
        return 0;
    }
    return _driver->quality();
}

3.4 参数接口

3.4.1 AP_VisualOdom::get_orientation

获取用户定义的传感器方向。

    // get user defined orientation
    enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }

3.4.2 AP_VisualOdom::get_pos_scale

获取应用于位置估计的缩放比例。

    // get user defined scaling applied to position estimates
    float get_pos_scale() const { return _pos_scale; }

3.4.3 AP_VisualOdom::get_pos_offset

获取相机相对于机体坐标系原点的位置偏移。

    // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
    const Vector3f &get_pos_offset(void) const { return _pos_offset; }

3.4.4 AP_VisualOdom::get_vel_noise

获取速度测量噪声。

    // return velocity measurement noise in m/s
    float get_vel_noise() const { return _vel_noise; }

3.4.5 AP_VisualOdom::get_pos_noise

获取位置测量噪声。

    // return position measurement noise in m
    float get_pos_noise() const { return _pos_noise; }

3.4.6 AP_VisualOdom::get_yaw_noise

获取航向测量噪声。

    // return yaw measurement noise in rad
    float get_yaw_noise() const { return _yaw_noise; }

3.4.7 AP_VisualOdom::get_delay_ms

获取传感器的延迟时间（毫秒）。

    // return the sensor delay in milliseconds (see _DELAY_MS parameter)
    uint16_t get_delay_ms() const { return MAX(0, _delay_ms); }

3.4.8 AP_VisualOdom::get_quality_min

后端驱动获取配置最小质量阈值。

    // return quality threshold
    int8_t get_quality_min() const { return _quality_min; }

4. 总结

AP_VisualOdom视觉定位传感器应用遵循《ArduPilot之开源代码Sensor Drivers设计》。

ront-end / back-end分层
消息机制驱动
评估视觉位置、姿态、速度

后续我们继续研讨三个后端程序：

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计

原文地址：https://blog.csdn.net/lida2003/article/details/140246581

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：Spring Boot中的依赖注入和控制反转
下一篇：面试经典150题

leetcode289:生命游戏
根据，简称为，是英国数学家约翰·何顿·康威在 1970 年发明的细胞自动机。给定一个包含m × n个格子的面板，每一个格子都可以看成是一个细胞。每个细胞都具有一个初始状态：1即为（live），或0即为
阅读更多2024-10-20
MongoDB数据恢复
注意：两个MongoDB的版本要一致，本文使用的是mongo:4.2.24。先把K8S上面的MongoDB 容器停止（可以把副本改成0）。1、将容器挂载MongoDB的数据目录备份到本地。经常是数据文
阅读更多2024-10-20
C#中实现事务
C#中实现事务
阅读更多2024-10-20
【LeetCode每日一题】——560.和为 K 的子数组
给你一个整数数组 nums 和一个整数 k ，请你统计并返回该数组中和为 k 的子数组的个数。子数组是数组中元素的连续非空序列。
阅读更多2024-10-20
「漏洞复现」满客宝智慧食堂系统 selectUserByOrgId 未授权访问漏洞
请勿利用文章内的相关技术从事非法测试，由于传播、利用此文所提供的信息而造成的任何直接或者间接的后果及损失，均由使用者本人负责，作者不为此承担任何责任。工具来自网络，安全性自测，如有侵权请联系删除。本次
阅读更多2024-10-20
React面试题目（从基本到高级）
React前端面试常见题目涵盖了React的基础概念、组件、状态管理、生命周期、性能优化等多个方面。
阅读更多2024-10-20
12.个人博客系统（Java项目基于spring和vue）
1 在校学习的学生，可用于日常学习使用或是毕业设计使用 2 毕业一到两年的开发人员，用于锻炼自己的独立功能模块设计能力，增强代码编写能力。 3 亦可以部署为商化项目使用。 4 需要完整资料及源码
阅读更多2024-10-20
YoloV8改进策略：注意力改进|DeBiFormer，可变形双级路由注意力|引入DeBiLevelRoutingAttention注意力模块（全网首发）
本次改进的核心在于将DeBiLevelRoutingAttention模块嵌入到YoloV8的主干网络中，具体位置是在SPPF（Spatial Pyramid Pooling Fast）模块之后。这一
阅读更多2024-10-20
word取消自动单词首字母大写
情况说明：在word输入单词后首字母会自动变成大写取消单词首字母大写步骤：（1）点击菜单栏文件（2）点击“更多”——>“选项”（3）点击“校对”——>“自动更正选项”（4）取消“句首字母大
阅读更多2024-10-20
web前端网页用户注册页面
【代码】web前端网页用户注册页面。
阅读更多2024-10-20

ArduPilot开源飞控之AP_VisualOdom

ArduPilot开源飞控之AP_VisualOdom

1. 源由

2. 类定义

2.1 类与构造函数

2.2 枚举类型

2.3 公共方法

2.4 消息处理

2.5 其他功能

2.6 私有成员

3. 框架设计

3.1 启动代码 AP_VisualOdom::init

3.2 消息处理

3.2.1 AP_VisualOdom::handle_vision_position_delta_msg

3.2.2 AP_VisualOdom::handle_pose_estimate

3.2.3 AP_VisualOdom::handle_vision_speed_estimate

3.3 辅助函数

3.3.1 AP_VisualOdom::enabled

3.3.2 AP_VisualOdom::healthy

3.3.3 AP_VisualOdom::quality

3.4 参数接口

3.4.1 AP_VisualOdom::get_orientation

3.4.2 AP_VisualOdom::get_pos_scale

3.4.3 AP_VisualOdom::get_pos_offset

3.4.4 AP_VisualOdom::get_vel_noise

3.4.5 AP_VisualOdom::get_pos_noise

3.4.6 AP_VisualOdom::get_yaw_noise

3.4.7 AP_VisualOdom::get_delay_ms

3.4.8 AP_VisualOdom::get_quality_min

4. 总结

5. 参考资料

相关文章