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）的方法。
目的是将来自视觉里程计传感器的姿态估计数据传送给后端处理，以便进一步的状态估计和导航。
-
参数：
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设计
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
