ArduPilot开源代码之AP_VisualOdom_IntelT265
- 1. 源由
- 2. 类定义
- 2.1 类定义
- 2.2 公共成员函数
- 2.2.1 继承构造函数
- 2.2.2 处理姿态估计
- 2.2.3 处理视觉速度估计
- 2.2.4 请求与AHRS对齐偏航
- 2.2.5 对齐位置与AHRS
- 2.2.6 起动检查
- 2.3 保护成员函数
- 2.3.1 旋转和修正位置
- 2.3.2 旋转速度
- 2.3.3 旋转姿态
- 2.3.4 对齐偏航与AHRS
- 2.3.5 对齐偏航
- 2.3.6 是否消耗传感器数据
- 2.3.7 对齐位置与AHRS
- 2.3.8 对齐位置
- 2.3.9 记录voxl摄像头位置和重置计数
- 2.3.10 处理voxl摄像头重置跳跃
- 2.4 成员变量
- 2.4.1 偏航调整
- 2.4.2 偏航旋转
- 2.4.3 姿态旋转
- 2.4.4 位置和速度旋转
- 2.4.5 位置修正
- 2.4.6 使用姿态旋转
- 2.4.7 使用位置和速度旋转
- 2.4.8 对齐偏航
- 2.4.9 对齐xy位置
- 2.4.10 对齐z位置
- 2.4.11 误差方向
- 2.4.12 上次的姿态
- 2.4.13 上次的位置重置计数
- 2.4.14 忽略传感器信息的开始时间
- 2.4.15 voxl重置跳跃处理变量
- 3. 重要例程
- 3.1 AP_VisualOdom_IntelT265::handle_pose_estimate
- 3.2 AP_VisualOdom_IntelT265::handle_vision_speed_estimate
- 3.3 AP_VisualOdom_IntelT265::request_align_yaw_to_ahrs
- 3.4 AP_VisualOdom_IntelT265::align_position_to_ahrs
- 3.5 AP_VisualOdom_IntelT265::pre_arm_check
- 4. 辅助函数
- 4.1 AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump
- 4.2 AP_VisualOdom_IntelT265::align_yaw_to_ahrs
- 4.3 AP_VisualOdom_IntelT265::rotate_and_correct_position
- 4.4 AP_VisualOdom_IntelT265::rotate_attitude
- 4.5 AP_VisualOdom_IntelT265::should_consume_sensor_data
- 4.6 AP_VisualOdom_IntelT265::align_position
- 5. 总结
- 6. 参考资料
1. 源由
继续研读《ArduPilot开源飞控之AP_VisualOdom》关于AP_VisualOdom_IntelT265
设备类。
除了《ArduPilot开源代码之AP_VisualOdom_Backend》抽象的共用方法和数据外,来进一步研读下设备差异导致的函数实现。
2. 类定义
AP_VisualOdom_IntelT265
类继承自AP_VisualOdom_Backend
,并添加了许多与Intel T265摄像头相关的功能。这些功能包括处理姿态和速度估计、对齐传感器与车辆的偏航和位置、记录和处理摄像头重置跳跃等。
该类通过多个保护函数和成员变量管理传感器数据的旋转和修正,并确保传感器数据与扩展卡尔曼滤波器AHRS/EKF算法对齐。
2.1 类定义
class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
{
2.2 公共成员函数
public:
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
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) override;
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
void request_align_yaw_to_ahrs() override { _align_yaw = true; }
void align_position_to_ahrs(bool align_xy, bool align_z) override { _align_posxy = align_xy; _align_posz = align_z; }
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
2.2.1 继承构造函数
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
继承基类的构造函数。
2.2.2 处理姿态估计
handle_pose_estimate
处理来自传感器的姿态估计数据并发送到EKF。
2.2.3 处理视觉速度估计
handle_vision_speed_estimate
处理来自传感器的速度估计数据并发送到EKF。
2.2.4 请求与AHRS对齐偏航
request_align_yaw_to_ahrs
请求传感器的偏航与AHRS/EKF对齐。
2.2.5 对齐位置与AHRS
align_position_to_ahrs
更新位置偏移量以与AHRS位置对齐。
2.2.6 起动检查
pre_arm_check
在系统起动前进行检查。
2.3 保护成员函数
protected:
void rotate_and_correct_position(Vector3f &position) const;
void rotate_velocity(Vector3f &velocity) const;
void rotate_attitude(Quaternion &attitude) const;
bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude);
void align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad);
bool should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter);
bool align_position_to_ahrs(const Vector3f &sensor_pos, bool align_xy, bool align_z);
void align_position(const Vector3f &sensor_pos, const Vector3f &new_pos, bool align_xy, bool align_z);
void record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter);
void handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter);
2.3.1 旋转和修正位置
rotate_and_correct_position
旋转并修正传感器提供的位置。
2.3.2 旋转速度
rotate_velocity
旋转传感器提供的速度。
2.3.3 旋转姿态
rotate_attitude
使用_yaw_trim旋转姿态。
2.3.4 对齐偏航与AHRS
align_yaw_to_ahrs
使用传感器提供的位置和姿态计算旋转以对齐传感器偏航与AHRS/EKF。
2.3.5 对齐偏航
align_yaw
使用新的偏航角度(弧度)对齐传感器偏航。
2.3.6 是否消耗传感器数据
should_consume_sensor_data
判断是否应消耗传感器数据。
2.3.7 对齐位置与AHRS
align_position_to_ahrs
通过更新_pos_correction对齐位置与AHRS。
2.3.8 对齐位置
align_position
通过更新_pos_correction对齐位置与新位置。
2.3.9 记录voxl摄像头位置和重置计数
record_voxl_position_and_reset_count
记录voxl摄像头的位置和重置计数以处理重置跳跃。
2.3.10 处理voxl摄像头重置跳跃
handle_voxl_camera_reset_jump
处理voxl摄像头在姿态和位置上的重置跳跃。
2.4 成员变量
private:
float _yaw_trim;
Quaternion _yaw_rotation;
Quaternion _att_rotation;
Matrix3f _posvel_rotation;
Vector3f _pos_correction;
bool _use_att_rotation;
bool _use_posvel_rotation;
bool _align_yaw = true;
bool _align_posxy;
bool _align_posz;
bool _error_orientation;
Quaternion _attitude_last;
uint8_t _pos_reset_counter_last;
uint32_t _pos_reset_ignore_start_ms;
uint8_t _voxl_reset_counter_last;
Vector3f _voxl_position_last;
};
2.4.1 偏航调整
_yaw_trim
用于将摄像头的偏航与AHRS/EKF对齐的偏航角调整(以弧度为单位)。
2.4.2 偏航旋转
_yaw_rotation
地球坐标系下的偏航旋转,用于将传感器的航向与车辆对齐。
2.4.3 姿态旋转
_att_rotation
体坐标系下的旋转,与ORIENT参数对应。
2.4.4 位置和速度旋转
_posvel_rotation
用于将传感器提供的位置和速度对齐到地球坐标系的旋转矩阵。
2.4.5 位置修正
_pos_correction
应该加到传感器报告的位置上的修正。
2.4.6 使用姿态旋转
_use_att_rotation
如果为真,则应用_att_rotation到传感器的姿态数据。
2.4.7 使用位置和速度旋转
_use_posvel_rotation
如果为真,则应用_posvel_rotation到传感器的位置和速度数据。
2.4.8 对齐偏航
_align_yaw
如果为真,则传感器的偏航应该与AHRS/EKF对齐。
2.4.9 对齐xy位置
_align_posxy
如果为真,则传感器的xy位置应该与AHRS对齐。
2.4.10 对齐z位置
_align_posz
如果为真,则传感器的z位置应该与AHRS对齐。
2.4.11 误差方向
_error_orientation
如果为真,则表示不支持该方向。
2.4.12 上次的姿态
_attitude_last
上次从摄像头接收到的姿态(用于起动检查)。
2.4.13 上次的位置重置计数
_pos_reset_counter_last
上次视觉位置估计的重置计数器值。
2.4.14 忽略传感器信息的开始时间
_pos_reset_ignore_start_ms
开始忽略传感器信息的系统时间,如果传感器数据未被忽略则为0。
2.4.15 voxl重置跳跃处理变量
-
_voxl_reset_counter_last
上次voxl摄像头的重置计数器(仅用于起始跳跃处理)。 -
_voxl_position_last
最后记录的位置(应用了缩放、偏移和方向修正后的位置)。
3. 重要例程
3.1 AP_VisualOdom_IntelT265::handle_pose_estimate
处理来自传感器的姿态估计数据并发送到EKF。
AP_VisualOdom_IntelT265::handle_pose_estimate
|
|-- 1. 获取位置缩放因子并计算位置和姿态
| |-- 获取位置缩放因子 `scale_factor`
| |-- 缩放位置 `pos`
| |-- 获取姿态 `att`
|
|-- 2. 处理姿态和位置的voxl摄像头重置跳跃
| |-- 调用 `handle_voxl_camera_reset_jump`
|
|-- 3. 如果需要,对齐传感器偏航与车辆的AHRS/EKF姿态
| |-- 如果 `_align_yaw` 为真
| |-- 调用 `align_yaw_to_ahrs`
| |-- 如果对齐成功,设置 `_align_yaw` 为假
|
|-- 4. 如果需要,对齐位置与AHRS
| |-- 如果 `_align_posxy` 或 `_align_posz` 为真
| |-- 调用 `align_position_to_ahrs`
| |-- 如果对齐成功,设置 `_align_posxy` 和 `_align_posz` 为假
|
|-- 5. 旋转并修正位置和姿态以与车辆对齐
| |-- 调用 `rotate_and_correct_position`
| |-- 调用 `rotate_attitude`
|
|-- 6. 记录位置以处理voxl重置跳跃
| |-- 调用 `record_voxl_position_and_reset_count`
|
|-- 7. 约束位置误差和角度误差
| |-- 调用 `constrain_float` 限制 `posErr`
| |-- 调用 `constrain_float` 限制 `angErr`
|
|-- 8. 记录质量
| |-- 设置 `_quality`
|
|-- 9. 检查是否应消耗传感器数据
| |-- 调用 `should_consume_sensor_data`
| |-- 如果应消耗
| |-- 调用 `AP::ahrs().writeExtNavData` 将姿态和位置发送到EKF
|
|-- 10. 计算欧拉角用于日志记录
| |-- 调用 `att.to_euler`
|
|-- 11. 如果启用了日志记录,记录传感器数据
| |-- 调用 `Write_VisualPosition`
|
|-- 12. 存储修正后的姿态以用于起动检查
| |-- 设置 `_attitude_last`
|
|-- 13. 记录更新时间用于健康监测
|-- 设置 `_last_update_ms`
3.2 AP_VisualOdom_IntelT265::handle_vision_speed_estimate
处理来自传感器的速度估计数据并发送到EKF。
AP_VisualOdom_IntelT265::handle_vision_speed_estimate
|
|-- 旋转速度以与车辆对齐
| |-- Vector3f vel_corrected = vel;
| |-- rotate_velocity(vel_corrected);
|
|-- 记录质量
| |-- _quality = quality;
|
|-- 检查最近的位置重置
| |-- bool consume = should_consume_sensor_data(false, reset_counter) && (_quality >= _frontend.get_quality_min());
| |-- if (consume)
| | |-- 发送速度到EKF
| | |-- AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
|
|-- 记录时间以进行健康监控
| |-- _last_update_ms = AP_HAL::millis();
|
|-- 如果启用了日志记录
| |-- Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume, _quality);
3.3 AP_VisualOdom_IntelT265::request_align_yaw_to_ahrs
请求传感器的偏航与AHRS/EKF对齐。
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void request_align_yaw_to_ahrs() override { _align_yaw = true; }
3.4 AP_VisualOdom_IntelT265::align_position_to_ahrs
更新位置偏移量以与AHRS位置对齐。
// update position offsets to align to AHRS position
// should only be called when this library is not being used as the position source
void align_position_to_ahrs(bool align_xy, bool align_z) override { _align_posxy = align_xy; _align_posz = align_z; }
3.5 AP_VisualOdom_IntelT265::pre_arm_check
在系统起动前进行检查。
AP_VisualOdom_IntelT265::pre_arm_check
|
|-- 如果不健康,立即退出
| |-- if (!healthy())
| | |-- hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
| | |-- return false;
|
|-- 检查是否存在不支持的方向
| |-- if (_error_orientation)
| | |-- hal.util->snprintf(failure_msg, failure_msg_len, "check VISO_ORIENT parameter");
| | |-- return false;
|
|-- 获取AHRS姿态
| |-- Quaternion ahrs_quat;
| |-- if (!AP::ahrs().get_quaternion(ahrs_quat))
| | |-- hal.util->snprintf(failure_msg, failure_msg_len, "waiting for AHRS attitude");
| | |-- return false;
|
|-- 检查横滚和俯仰角是否相差大于10度
| |-- const float rp_diff_deg = degrees(ahrs_quat.roll_pitch_difference(_attitude_last));
| |-- if (rp_diff_deg > 10.0f)
| | |-- hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
| | |-- return false;
|
|-- 检查偏航角是否相差大于10度
| |-- Vector3f angle_diff;
| |-- ahrs_quat.angular_difference(_attitude_last).to_axis_angle(angle_diff);
| |-- const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
| |-- if (yaw_diff_deg > 10.0f)
| | |-- hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
| | |-- return false;
|
|-- 所有检查通过,返回真
| |-- return true;
4. 辅助函数
4.1 AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump
摄像头重置后姿态、位置对齐。
AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump
|
|-- 如果不使用VOXL相机,立即返回
| |-- if (get_type() != AP_VisualOdom::VisualOdom_Type::VOXL)
| |-- return;
|
|-- 如果重置计数器没有变化,立即返回
| |-- if (reset_counter == _voxl_reset_counter_last)
| |-- return;
|
|-- 发送重置警告给用户
| |-- gcs().send_text(MAV_SEVERITY_WARNING, "VisOdom: reset");
|
|-- 将传感器的偏航与当前估计的偏航对齐
| |-- align_yaw_to_ahrs(sensor_pos, sensor_att);
|
|-- 将位置对齐到上次记录的位置
| |-- align_position(sensor_pos, _voxl_position_last, true, true);
|
|-- 记录重置计数器的变化
| |-- _voxl_reset_counter_last = reset_counter;
4.2 AP_VisualOdom_IntelT265::align_yaw_to_ahrs
偏航角对齐,若外部导航源则无需对齐。
AP_VisualOdom_IntelT265::align_yaw_to_ahrs
|
|-- 如果AHRS正在使用外部导航作为其偏航源
| |-- 如果是,返回false
|
|-- 如果AHRS尚未初始化或者DCM姿态尚未初始化
| |-- 如果是,返回false
|
|-- 调用align_yaw方法,将传感器位置、姿态和AHRS当前的偏航角度作为参数
| |-- align_yaw(position, attitude, AP::ahrs().get_yaw());
|
|-- 返回true,表示成功进行了对齐操作
4.3 AP_VisualOdom_IntelT265::rotate_and_correct_position
位置修正函数。
// apply rotation and correction to position
void AP_VisualOdom_IntelT265::rotate_and_correct_position(Vector3f &position) const
{
if (_use_posvel_rotation) {
position = _posvel_rotation * position;
}
position += _pos_correction;
}
4.4 AP_VisualOdom_IntelT265::rotate_attitude
姿态旋转函数。
// rotate attitude using _yaw_trim
void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const
{
// apply orientation rotation
if (_use_att_rotation) {
attitude *= _att_rotation;
}
// apply earth-frame yaw rotation
if (!is_zero(_yaw_trim)) {
attitude = _yaw_rotation * attitude;
}
return;
}
4.5 AP_VisualOdom_IntelT265::should_consume_sensor_data
AP_VisualOdom_IntelT265::should_consume_sensor_data
|
|-- 如果传感器类型为VOXL
| |-- 返回 true // VOXL类型的传感器数据不会在重置后丢弃
|
|-- 获取当前系统时间
| |-- uint32_t now_ms = AP_HAL::millis();
|
|-- 如果是视觉位置估计消息
| |-- 如果重置计数器与上次记录的不同
| | |-- _pos_reset_counter_last = reset_counter;
| | |-- _pos_reset_ignore_start_ms = now_ms;
|
|-- 检查是否距离上次重置已经过去1秒钟
| |-- 如果(now_ms - _pos_reset_ignore_start_ms) > VISUALODOM_RESET_IGNORE_DURATION_MS
| | |-- _pos_reset_ignore_start_ms = 0;
|
|-- 返回(_pos_reset_ignore_start_ms == 0)
VOXL T265传感器与普通的Intel T265摄像头相比,在硬件和软件上有几个显著的差异:
-
硬件差异:
- 嵌入式系统支持: VOXL T265是基于Qualcomm Snapdragon处理器的嵌入式系统,而不是基于通用计算机(如PC或单板计算机)。
- 紧凑设计: VOXL T265的设计更加紧凑,适合嵌入式系统集成,例如机器人、自动驾驶车辆或其他嵌入式应用。
- 电源管理和热管理: VOXL T265考虑到嵌入式系统的电源需求和热管理,与普通的T265摄像头可能有所不同。
-
软件支持差异:
- 驱动程序和API: VOXL T265通常会提供特定于其嵌入式平台的驱动程序和API,以便与嵌入式系统集成。
- 性能优化和定制: 软件可能针对VOXL平台进行了优化,以提高性能或适应特定的应用场景。
-
集成和部署环境:
- 部署环境: VOXL T265通常用于需要嵌入式处理能力的应用,例如移动机器人或无人机,而普通的T265摄像头可能更适合在桌面或计算机视觉实验室中进行开发和测试。
- 集成需求: VOXL T265可能具有更高的集成需求,例如对于电源、尺寸、传感器与嵌入式系统接口的紧密配合。
总体而言,VOXL T265是为嵌入式系统设计的定制版本,旨在提供更适合于嵌入式应用的特性和性能优化。
4.6 AP_VisualOdom_IntelT265::align_position
位置对齐函数。
// align position with a new position by updating _pos_correction
// sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
// new_pos should be a NED position offset from the EKF origin
void AP_VisualOdom_IntelT265::align_position(const Vector3f &sensor_pos, const Vector3f &new_pos, bool align_xy, bool align_z)
{
// calculate position with current rotation and correction
Vector3f pos_orig = sensor_pos;
rotate_and_correct_position(pos_orig);
// update position correction
if (align_xy) {
_pos_correction.x += (new_pos.x - pos_orig.x);
_pos_correction.y += (new_pos.y - pos_orig.y);
}
if (align_z) {
_pos_correction.z += (new_pos.z - pos_orig.z);
}
}
5. 总结
T265摄像头主要特点和功能
-
双鱼眼相机:
- T265摄像头配备了两个鱼眼相机,每个相机具有180度视场角,用于实时捕捉周围环境的图像。
-
惯性测量单元(IMU):
- 内置IMU传感器,包括加速度计和陀螺仪,用于测量和跟踪设备的加速度和角速度。
-
视觉里程计(V-SLAM)技术:
- 使用视觉和惯性数据进行实时定位和映像技术(SLAM),从而实现精确的位置跟踪和运动估计,无需依赖GPS或其他外部定位系统。
-
全自动定位和映像技术:
- 可以在室内和室外环境中实现全自动的位置和方向跟踪,适用于移动机器人、无人机、增强现实(AR)设备等各种应用场景。
-
USB连接:
- 通过USB接口与计算设备(如PC、嵌入式系统)连接,提供图像和传感器数据。
-
跨平台支持:
- 支持多种操作系统和开发平台,包括Windows、Linux和macOS,适合于开发和部署不同类型的应用程序。
-
开放式软件开发包(SDK):
- 提供开放式软件开发包,支持开发者自定义应用程序和算法,从而利用摄像头提供的视觉和惯性数据进行更高级的处理和分析。
VOXL T265是一个嵌入式视觉定位解决方案
-
ModalAI VOXL: VOXL是由公司ModalAI开发的嵌入式计算机平台,基于Qualcomm Snapdragon处理器。它专门设计用于机器人、无人机和其他自主移动系统的嵌入式应用。VOXL提供了处理能力、传感器接口和丰富的软件支持,使得VOXL T265能够在复杂的嵌入式环境中实现视觉定位和导航。
-
Intel RealSense T265模块: 这是VOXL T265解决方案的核心视觉传感器部件。它基于Intel RealSense T265摄像头模块,提供了同时定位和映像技术(SLAM)以及视觉里程计(V-SLAM)功能。这个模块具有两个鱼眼镜头和一个惯性测量单元(IMU),能够实时地构建环境地图和执行位置跟踪,适用于各种移动机器人和自主系统。
6. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计