ArduPilot开源飞控之AP_VisualOdom

  • 开发
  • 9

ArduPilot开源飞控之AP_VisualOdom

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）的方法。
目的是将来自视觉里程计传感器的姿态估计数据传送给后端处理，以便进一步的状态估计和导航。

  1. 参数：

    • remote_time_us：远程时间戳（微秒）。
    • time_ms：本地时间戳（毫秒）。
    • x, y, z：位置坐标（米）。
    • roll, pitch, yaw：姿态角（弧度）。
    • posErr：位置误差。
    • angErr：角度误差。
    • reset_counter：重置计数器。
    • quality：质量评估（-1 表示失败，0 表示未知，1 最差，100 最佳）。

  2. 功能：

    • enabled() 方法检查传感器是否启用，如果未启用则立即返回。
    • 如果 _driver 驱动器不为空，创建一个 Quaternion 对象 attitude，通过 from_euler(roll, pitch, yaw) 方法将欧拉角转换为四元数。
    • 调用 _driver->handle_pose_estimate() 方法，将处理过的姿态估计数据和质量评估传递给后端处理。

  3. 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）的通用方法。

  1. 函数签名和说明：

    • 函数名：handle_vision_speed_estimate
    • 参数：
      • remote_time_us：远程时间戳（微秒）
      • time_ms：本地时间戳（毫秒）
      • vel：速度估计的三维向量（NED坐标系，单位为米每秒）
      • reset_counter：重置计数器
      • quality：质量评估，-1 表示失败，0 表示未知，1 表示最差，100 表示最佳。

  2. 功能说明：

    • 首先，检查视觉里程计是否启用（调用了类的 enabled() 方法）。如果未启用，则立即返回，不执行后续操作。
    • 然后，检查 _driver 指针是否为 nullptr。如果 _driver 不为空，则调用 _driver 对象的 handle_vision_speed_estimate 方法，将速度估计数据传递给后端处理。

  3. 执行流程：

    • 如果视觉里程计未启用，函数直接返回，不执行后续任何操作。
    • 如果视觉里程计已启用且 _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设计

阅读全部

相关推荐

  1. ArduPilot开源飞控之AP_VisualOdom

    2024-07-11 00:36:05       10 阅读

  8. 主流无人机开源飞控

    2024-07-11 00:36:05       39 阅读

最近更新

  3. docker php8.1+nginx base 镜像 dockerfile 配置

    2024-07-11 00:36:05       5 阅读

  4. Could not load dynamic library ‘cudart64_100.dll‘

    2024-07-11 00:36:05       5 阅读

  9. 在Django里面运行非项目文件

    2024-07-11 00:36:05       4 阅读

  19. Python语言-面向对象

    2024-07-11 00:36:05       8 阅读

  20. 如何分清楚常见的 Git 分支管理策略Git Flow、GitHub Flow 和 GitLab Flow

    2024-07-11 00:36:05       6 阅读

热门阅读

  1. 如何实现跨域

    2024-07-11 00:36:05       8 阅读

  3. centos7yum-mysql-community-server安装流程步骤

    2024-07-11 00:36:05       12 阅读

  6. toFixed 四舍五入问题

    2024-07-11 00:36:05       9 阅读

  12. [C++][CMake][嵌套的CMake]详细讲解

    2024-07-11 00:36:05       10 阅读

  13. 65.指针函数和函数指针

    2024-07-11 00:36:05       10 阅读

  14. 网络安全测评技术与标准

    2024-07-11 00:36:05       10 阅读

  15. Qt之多线程编程(QThread)

    2024-07-11 00:36:05       11 阅读

  18. MySQL：left join 后用 on 还是 where？

    2024-07-11 00:36:05       11 阅读

  19. 最短路径算法(算法篇)

    2024-07-11 00:36:05       12 阅读

  22. 上海市计算机学会竞赛平台2023年1月月赛丙组实验日志

    2024-07-11 00:36:05       9 阅读

  23. 【数学建模】生产企业原材料的订购与运输

    2024-07-11 00:36:05       10 阅读

  24. Spring Boot与Traefik的集成

    2024-07-11 00:36:05       12 阅读