robots

编程入门行业动态更新时间:2024-10-15 20:25:06

robots

文章目录

概要
依赖项
订阅话题
发布话题
输入格式
基本算法
状态矩阵
回调函数
测量处理
过滤器更新 – 传感器集成：(odom_estimation_node.cpp )
去除冗余
为各个传感器创建协方差矩阵
添加更多传感器
删除现有传感器
参考

概要

“Robot Pose EKF 包用于根据来自不同来源的（部分）姿态测量来估计机器人的 3D 姿态。它使用具有 6D 模型（3D 位置和 3D 方向）的扩展卡尔曼滤波器来结合车轮里程计、IMU 传感器和视觉里程计的测量结果。基本思想是提供与不同传感器的松散耦合集成，其中传感器信号作为 ROS 消息接收” – robots_pose_ekf wiki

主页： http: //ros/wiki/robot_pose_ekf

robots_pose_ekf 包可用于融合里程计、惯性测量单元和视觉里程计的传感器输出，从而减少测量中的总体误差。

依赖项

bfl – Bayesian filter library
roscpp
geometry_msgs
nav_msgs
sensor_msgs
tf
std_msgs
rosbag

订阅话题

/odom – 里程计数据消息 (nav_msgs/Odometry)
/imu_data – imu 数据消息 (sensor_msgs/IMU)
/vo – 视觉里程计数据消息 (nav_msgs/Odometry)

发布话题

/odom_combined – 组合数据消息 (nav_msgs/Odometry)

输入格式

里程计消息包含 2D 位姿，即 x、y 和偏航角
IMU 消息包含 3D 方向，即滚动角、俯仰角和偏航角。
VO 消息包含完整的 3D 姿势，即x、y、z 和滚动角、俯仰角、偏航角。

这些消息必须先转换为正确的格式，然后才能应用于过滤器。这在回调函数下进行了解释。

基本算法

从每个传感器获取数据
检查它们是否有效
如果它们有效，则将它们相对于基本参考系进行转换
当获得传感器信息时，它将被存储，直到来自所有传感器的信息可用。每个接收到的数据都有自己的时间戳
一旦所有数据都可用，扩展卡尔曼滤波器（如 Orocos-BFL 库中定义）会在所有传感器数据可用时（即，如果来自里程计的数据在以下时间可用）针对可用的每个传感器数据进行更新： t_0(>0)时刻，t_1(>t_0)时刻获取imu的数据，t_2(>t_1)时刻获取视觉里程计的数据，然后对三组数据都运行过滤器在时间 t_1)。
该融合的传感器数据被转换为里程计消息并发布在主题 /odom_combined 上

状态矩阵

状态矩阵给出机器人的姿势。位姿由表示位置的 x、y、z 和表示方向的四元数给出。

回调函数

Odometry：
使用 tf 包，来自 /odom 主题的数据消息被转换为 x、y、z 和四元数。然后将四元数转换为 Vector3。然后设置协方差矩阵。然后该函数将带有时间戳的测量结果添加到缓冲区中。
IMU：
使用 tf 包，来自 /imu_data 主题的数据消息被转换为方向向量。然后将其转换为矢量 3 形式。然后设置协方差矩阵。方向数据现在转换为 base_footprint 参考系。测量结果会添加到带有时间戳的缓冲区中。
视觉里程计：
使用 tf.来自 /vo 主题的数据消息转换为姿势格式。现在设置了测量矩阵的协方差，并将数据与时间戳一起添加到测量缓冲区中。

测量处理

里程计消息：
设置车轮里程计坐标系（绝对）与相对坐标系之间的转换，并将测量数据转换为base_footprint（相对）参考系。
过滤器相对于里程计测量模型和相关帧中的里程计数据进行更新。
IMU消息：
IMU数据转换为base_footprint帧并校正角度溢出。还设置了两者之间的转换。滤波器根据imu测量模型和相关帧中的imu数据进行更新。
VO消息：
将VO数据转换为base_footprint帧并校正角度溢出。还设置了两者之间的转换。滤波器根据相关帧中的 vo 测量模型和 imu 数据进行更新。

过滤器更新 – 传感器集成：(odom_estimation_node.cpp )

过滤器首先检查定时错误，如果所有传感器都处于活动状态，则将帧时间设置为所有测量值均可用时的时间。然后，它检查是否可以应用 ekf 过滤器，即检查必要的参数。如果所有参数都正确，则执行过滤器。这会调用输入处理来处理和更新过滤器。最终的估计将被存储为下一次更新的先前估计。过滤器的执行取决于提供给函数的参数。然后过滤器退出，输出数据发布在 /odom_combined 主题上。

去除冗余

如上所述，仅当来自所有传感器的所有数据都可用时，过滤器才会更新。然后过滤器分别更新每个传感器数据的结果。特定数据集对总体最终结果的影响取决于数据表中通常给出的协方差矩阵。

为各个传感器创建协方差矩阵

对于 IMU 和里程计，可以根据数据表形成协方差矩阵。

对于视觉里程计，可以从将测量变量与位姿坐标相关联的测量方程获得协方差矩阵[1]。

*协方差矩阵都不应该为零，它们应该有一些值，但我不确定它是如何通过的。

添加更多传感器

截至目前，除现有 3 个传感器外，无法将新传感器添加到套件中。但是，我们可以在删除另一个传感器后添加新传感器，然后在已删除传感器的主题上发布数据消息。

在现有主题上发布时，应将 odom_combined 指定为标头框架 id，将 base_footprint 指定为 child_frame_id。

删除现有传感器

可以通过修改启动文件来删除三个现有传感器中的任何一个。

启动文件有以下几行代码：

<launch>
<node pkg=”robot_pose_ekf” type=”robot_pose_ekf” name=”robot_pose_ekf”>
<param name=”output_frame” value=”odom”/>
<param name=”freq” value=”30.0″/>
< param name=”sensor_timeout” value=”1.0″/>
<param name=”odom_used” value=”true”/>
<param name=”imu_used” value=”true”/>
<param name=”vo_used” value= ”true”/>
<param name=”debug” value=”false”/>
<param name=”self_diagnose” value=”false”/>
</node>
</launch>

操作频率受到最慢传感器频率的限制。如果未使用传感器，则其值应更改为 false。不能将新传感器添加到此包中，但是三个传感器中的任何一个都可以用另一个传感器替换，只要传感器发布的消息具有正确的格式即可。