自学内容网 自学内容网

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, IntelT265VOXL

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:如果 _typeNone,则什么也不做。
  • VisualOdom_Type::MAV:如果 _typeMAV,则创建一个 AP_VisualOdom_MAV 的实例并将其赋给 _driver
  • VisualOdom_Type::IntelT265VisualOdom_Type::VOXL:如果 _typeIntelT265VOXL,则创建一个 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设计


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

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