# ros-gio

**Repository Path**: hf26023/ros-gio

## Basic Information

- **Project Name**: ros-gio
- **Description**: 用于无人机和地面车辆的GNSS和imu融合定位的代码仓库
- **Primary Language**: Unknown
- **License**: BSD-3-Clause
- **Default Branch**: test
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 1
- **Created**: 2026-05-15
- **Last Updated**: 2026-08-15

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 适用于无人机、车辆的GNSS+IMU的EKF融合定位包

## 测试方案：UM982 GPS + 航向 + IMU 融合

直接使用下面这套 launch：

```bash
roslaunch robot_localization um982_gps_heading_imu_ekf.launch
```

这套方案会串起以下节点：
- `um982_nmea_converter`：把 `/um982/nmea/raw` 转成 GPS 和可融合的航向 IMU
- `navsat_transform_node`：把经纬度 GPS 转成世界坐标系下的 `odometry/gps`
- `ekf_localization_node`：融合 GPS 位置、UM982 绝对航向和原始 IMU

**核心特性**：
- 最终输出话题是 `/odometry/filtered`
- 由原始 IMU `imu0` 触发滤波更新
- IMU 来一帧，就处理并发布一帧
- GPS 和 UM982 航向作为低频绝对观测参与校正

**默认输入话题**：
- `/um982/nmea/raw`：`std_msgs/String`，UM982 原始 NMEA/heading 串口数据
- `/mavros/imu/data`：`sensor_msgs/Imu`，原始 IMU 数据

**中间话题**：
- `/um982/global_position/raw/fix`：`sensor_msgs/NavSatFix`
- `/um982/global_position/compass_hdg`：`std_msgs/Float64`
- `/um982/global_position/heading_imu`：`sensor_msgs/Imu`
- `/odometry/gps`：`nav_msgs/Odometry`

**最终输出话题**：
- `/odometry/filtered`：`nav_msgs/Odometry`

### 最简启动

```bash
cd /home/hf/gio
catkin_make --pkg robot_localization
source devel/setup.bash
roslaunch robot_localization um982_gps_heading_imu_ekf.launch
```

### 常用启动示例

```bash
roslaunch robot_localization um982_gps_heading_imu_ekf.launch \
  input_topic:=/um982/nmea/raw \
  imu_topic:=/mavros/imu/data
```

如果你的 IMU 频率不是默认值，可以显式指定：

```bash
roslaunch robot_localization um982_gps_heading_imu_ekf.launch \
  imu_topic:=/mavros/imu/data \
  imu_frequency:=200.0
```

### 关键参数

- `imu_frequency`：EKF 期望的 IMU 更新频率，建议填真实 IMU 频率
- `magnetic_declination_radians`：磁偏角补偿
- `yaw_offset`：航向零点补偿，单位弧度
- `heading_offset_deg`：UM982 航向零点补偿，单位度
- `heading_stddev_deg`：UM982 航向标准差，影响航向融合权重
- `gnss_xyz` / `gnss_rpy`：`base_link -> gnss_frame` 的静态外参

### 对应文件

- Launch：`launch/um982_gps_heading_imu_ekf.launch`
- EKF 参数：`params/um982_gps_heading_imu_ekf.yaml`
- navsat 参数：`params/um982_navsat_transform.yaml`
- UM982 转换：`launch/um982_nmea_converter.launch`

---


## 普通gps+imu方案
```bash
mkdir -p gio/src
cd gio/src
#复制该功能包到src
cd ../..
catkin_make
source devel/setup.bash
roslaunch robot_localization gps_imu_ekf.launch
```


### 相关参数

**Launch 文件**：`gps_imu_ekf.launch`

该方案同时使用 `navsat_transform_node` 和 `ekf_localization_node` 实现更好的融合：

```bash
roslaunch robot_localization gps_imu_ekf.launch
```
播放包的指定话题
```
rosbag play your.bag --topic /mavros/imu/data /mavros/global_position/raw/fix 
```

**输入话题**：
- `gps/fix` (sensor_msgs/NavSatFix)：原始 GPS 数据
- `imu/data` (sensor_msgs/Imu)：IMU 姿态和角速度
- `target/gps/fix` (sensor_msgs/NavSatFix)：目标点 GPS 数据（可选）
（可以在launch中remap名称）

**输出话题**：
- `odometry/filtered` (nav_msgs/Odometry)：融合后的位置和姿态估计（世界坐标系）
- `odometry/gps` (nav_msgs/Odometry)：由 GPS 转换得到的机器人位置（中间过程）
- `target/position` (nav_msgs/Odometry)：目标 GPS 转换到当前世界坐标系后的位姿（可选）

## UM982 原始串口转换

如果你的原始话题是 `/um982/nmea/raw` 的 `std_msgs/String`，可以直接启动这个转换节点：

```bash
roslaunch robot_localization um982_nmea_converter.launch
rosbag play /home/hf/gio/dataset/bag2_2026-05-29-10-49-39_nortcm.bag --clock
```

默认会把 `GNGGAH` 转成 `/mavros/global_position/raw/fix`，把 `UNIHEADINGA` 转成 `/mavros/global_position/compass_hdg`。

**处理流程**：
1. `navsat_transform_node` 将 GPS 坐标（经纬度）转换到世界坐标系（map）
2. `ekf_localization_node` 融合转换后的 GPS 位置和 IMU 姿态
3. 如果接入 `target/gps/fix`，`navsat_transform_node` 还会把目标 GPS 转换到当前世界坐标系并发布到 `target/position`

**优点**：
- 灵活的融合：可通过协方差矩阵调整 GPS 和 IMU 的权重
- 卡尔曼滤波输出更平滑
- 更好地处理 GPS 信号丢失和 IMU 漂移

**缺点**：
- 配置相对复杂
- 增加了一个节点的开销

**需要调整的关键参数**（`params/gps_imu_ekf.yaml`）：
- yaw相关：
  - `magnetic_declination_radians`：当地磁偏角，单位为弧度。用于把 IMU 基于磁北的航向修正到真北/ENU 坐标系
  - `yaw_offset`：IMU 航向零点补偿，单位为弧度。用于把 IMU 的 yaw 零点对齐到 ENU 约定的“朝东为 0”
- 额外的 EKF 参数：
  - `process_noise_covariance`：对运动模型的信任度
  - `initial_estimate_covariance`：位置和姿态的初始不确定度
  - EKF 中的传感器配置（设置每个传感器测量哪些状态变量）

### 磁偏角和 yaw_offset 在代码中的作用
在 ROS/mavros 的标准地理坐标系（ENU：东-北-天）中：
- $Yaw = 0$ 代表机头朝正东
- $Yaw = 90^\circ$ 代表机头朝正北
- $Yaw = 180^\circ / -180^\circ$ 代表机头朝正西
- $Yaw = -90^\circ$ 代表机头朝正南

`magnetic_declination_radians` 用来补偿当地磁偏角。若 IMU 输出的是相对磁北的航向，该参数会把它修正到系统使用的 ENU 世界坐标系航向。

`yaw_offset` 用来补偿 IMU 自身的零点定义差异。`navsat_transform_node` 默认假设 yaw 在“朝东”时为 0；如果你的 IMU 在“朝北”时读数为 0（很多设备都是这样），通常需要把 `yaw_offset` 设为 `pi/2`，使修正后的航向满足 ENU 约定。

在 `navsat_transform_node` 中，航向修正实际按下面公式执行：

$$
yaw_{used} = yaw_{imu} + magnetic\_declination + yaw\_offset + utm\_meridian\_convergence
$$

也就是：
- `magnetic_declination_radians` 负责把“磁北参考”纠正为“真北参考”
- `yaw_offset` 负责把“传感器自己的零点定义”纠正为“朝东为 0”

对应代码位置：
- `src/navsat_transform.cpp` 中读取参数：`nh_priv.getParam("magnetic_declination_radians", magnetic_declination_)`
- 航向修正计算：`yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_)`

如果该参数配置错误，常见现象包括：
- 地图坐标下轨迹整体偏转一个固定角度
- 机器人朝向与实际前进方向不一致
- GPS 与 IMU 融合后出现“斜着走”或长期方向偏差

---

## 位姿曲线绘制

仓库内提供了一个 Python 绘图节点 `plot_odometry_pose.py`，用于订阅 `nav_msgs/Odometry` 并实时绘制：
- XY 平面轨迹
- x、y、z 随时间变化曲线
- roll、pitch、yaw 随时间变化曲线

脚本会自动把四元数姿态转换为 RPY；默认角度单位为度。

**默认订阅话题**：`/odometry/filtered`

**使用方法**：
```bash
cd gio
catkin_make
source devel/setup.bash
rosrun robot_localization plot_odometry_pose.py
```

**订阅其他里程计话题**：
```bash
rosrun robot_localization plot_odometry_pose.py _topic:=/your/odometry/topic
```

**常用参数**：
- `_topic`：要订阅的里程计话题，默认 `/odometry/filtered`
- `_max_points`：缓存并显示的最大采样点数，默认 `3000`
- `_use_degrees`：是否将 RPY 以角度显示，默认 `true`，设为 `false` 时显示弧度

**示例**：
```bash
rosrun robot_localization plot_odometry_pose.py _topic:=/odometry/gps _max_points:=5000 _use_degrees:=true
```

如果运行时报 matplotlib 缺失，请先安装 Python 版 matplotlib。

---

## 故障排除

**查看可用话题**：
```bash
rostopic list | grep -E 'odometry|gps|imu'
```

**监控融合输出**：
```bash
rostopic echo /odometry/filtered     # 方案 2 的输出
# 或
rostopic echo /odometry/gps          # 方案 1 的输出
```

**对于自定义话题名称**，在 launch 文件中重映射：
```xml
<remap from="gps/fix" to="/your_gps_topic"/>
<remap from="imu/data" to="/your_imu_topic"/>
<remap from="target/gps/fix" to="/your_target_gps_topic"/>
```

---

## 双 EKF*(轮速+imu) + GPS

如需更完整的例子（包含轮速里程计、IMU 和 GPS），参考 `dual_ekf_navsat_example.launch` 及其配置文件 `params/dual_ekf_navsat_example.yaml`。该示例会同时产出本地里程计系和全局地图系的估计。