ROS 2自主移动机器人（AMR）传感器融合与定位(4)
·
多源数据融合与状态估计
本章深入展开机器人定位的核心——多传感器融合与状态估计。将从扩展卡尔曼滤波（EKF）与自适应蒙特卡洛定位（AMCL）两种主流算法的实现出发，构建一套完整的定位管道，涵盖 IMU、里程计、激光雷达、相机等多源数据的同步、融合、降级与重定位机制。
1.1 定位系统总体架构
机器人定位依赖三种信息来源：本体感知（里程计、IMU）、环境感知（激光雷达、相机）和地图。EKF 负责融合本体感知与简单环境特征，AMCL 则利用粒子滤波完成全局定位。下面这幅文字流程图展示了整体数据流。
┌────────────────────── 传感器层 ──────────────────────────┐ │ │ │ [Wheel Encoders]──► motor_driver ──► /odom │ │ [IMU]─────────────► imu_driver ────► /imu │ │ [LIDAR]───────────► lidar_driver ──► /scan │ │ [Camera]──────────► camera_driver ─► /image_raw │ │ │ └──────────────────────┬────────────────────────────────────┘ │ ┌──────────────────────▼────────────────────────────────────┐ │ 数据预处理层 │ │ │ │ /odom ──► odom_predictor (外推 + 协方差估计) │ │ /imu ──► imu_preprocessor (低通滤波 + 偏航角提取) │ │ /scan ──► scan_preprocessor (降采样 + 噪声滤除) │ │ │ └──────────────────────┬────────────────────────────────────┘ │ ┌──────────────────────▼────────────────────────────────────┐ │ 定位核心 (Localization Engine) │ │ │ │ ┌─────────────────────────────────────────────────────┐ │ │ │ EKF Localization Node (ekf_node) │ │ │ │ 输入: /odom, /imu │ │ │ │ 输出: /ekf_odom, /tf (odom→base_footprint) │ │ │ │ 作用: 高频位姿平滑，消除轮子打滑漂移 │ │ │ └─────────────────────────────────────────────────────┘ │ │ │ │ ┌─────────────────────────────────────────────────────┐ │ │ │ AMCL Node (amcl_node) │ │ │ │ 输入: /scan, /map, /tf │ │ │ │ 输出: /amcl_pose, /tf (map→odom) │ │ │ │ 作用: 全局定位，消除累积误差 │ │ │ └─────────────────────────────────────────────────────┘ │ │ │ └──────────────────────┬────────────────────────────────────┘ │ ┌──────────────────────▼────────────────────────────────────┐ │ 上层消费者 │ │ │ │ nav2_controller ◄── /tf (map→base_link 最终变换) │ │ rviz2 ◄── /amcl_pose, /scan, /map │ └───────────────────────────────────────────────────────────┘
EKF 提供高频（50-100Hz）的局部位姿估计，AMCL 提供低频（约10Hz）的全局定位修正，两者通过 tf 变换链接：map → odom → base_footprint。
1.2 文件结构树
amr_localization/ ├── CMakeLists.txt ├── package.xml ├── include/ │ └── amr_localization/ │ ├── ekf_node.hpp # EKF 节点类 │ ├── ekf_filter.hpp # EKF 滤波器核心 │ ├── motion_model.hpp # 差速运动模型 │ ├── sensor_sync.hpp # 传感器同步器 │ └── imu_preprocessor.hpp # IMU 预处理 ├── src/ │ ├── ekf_node.cpp │ ├── ekf_filter.cpp │ ├── motion_model.cpp │ ├── sensor_sync.cpp │ └── imu_preprocessor.cpp ├── config/ │ ├── ekf_params.yaml │ ├── amcl_params.yaml │ └── sensor_qos.yaml ├── launch/ │ └── localization_launch.py └── test/ └── test_ekf.cpp
1.3 主要函数树分析
EKF 定位节点 (EKFNode)
EKFNode::EKFNode() ├── 声明参数 (频率、噪声方差、初始协方差等) ├── 创建发布器: /ekf_odom, /tf ├── 创建订阅器: /odom, /imu (使用 message_filters 同步) │ └── 同步器 policy: ApproximateTime (10ms) ├── 初始化 EKFilter 和 MotionModel └── 启动定时器 (50Hz 预测步骤) EKFNode::sensor_callback(odom_msg, imu_msg) ├── 提取里程计线速度/角速度 ├── 提取 IMU 角速度 (绕Z轴) ├── 调用 ekf_->predict(dt, control_input) // 运动预测 ├── 调用 ekf_->update_odom(odom_measurement) // 观测更新 ├── 调用 ekf_->update_imu(imu_measurement) ├── 发布 /ekf_odom └── 广播 /tf (odom → base_footprint)
EKF 滤波器核心 (EKFilter)
EKFilter::predict(dt, u) ├── 从运动模型计算状态转移矩阵 F ├── 计算过程噪声 Q ├── 状态预测: x = f(x, u) ├── 协方差预测: P = F·P·F^T + Q └── 返回 EKFilter::update_odom(z) ├── 计算观测矩阵 H ├── 计算卡尔曼增益 K = P·H^T·(H·P·H^T + R)^-1 ├── 状态更新: x += K·(z - H·x) ├── 协方差更新: P = (I - K·H)·P └── 返回 EKFilter::update_imu(z) ├── 类似 update_odom，但观测仅包含偏航角 └── 返回
1.4 扩展卡尔曼滤波器实现
状态向量为 [x, y, theta, v, omega]，其中 v 为线速度，omega 为角速度。控制输入来自里程计的速度指令，观测来自里程计的位置和 IMU 的偏航角速度。
1.4.1 头文件
/**
* @file ekf_filter.hpp
* @brief 5状态扩展卡尔曼滤波器 (EKF)
*
* 状态向量: [x, y, yaw, v, ω] (平面运动)
* 控制输入: [v_cmd, ω_cmd] (期望速度)
* 观测向量1 (里程计): [x, y, yaw]
* 观测向量2 (IMU): [ω_imu]
*
* 采用差速机器人运动模型，假设无侧滑。
*/
#ifndef AMR_LOCALIZATION__EKF_FILTER_HPP_
#define AMR_LOCALIZATION__EKF_FILTER_HPP_
#include <Eigen/Dense>
#include <random>
namespace amr_localization
{
class EKFilter
{
public:
EKFilter();
/**
* @brief 初始化滤波器状态与协方差矩阵
* @param state 初始状态 [x, y, yaw, v, ω]
* @param covariance 初始协方差 (5x5)
*/
void init(const Eigen::VectorXd & state, const Eigen::MatrixXd & covariance);
/**
* @brief 执行预测步骤
* @param dt 时间步长 (秒)
* @param control 控制输入 [v_cmd, ω_cmd]
*/
void predict(double dt, const Eigen::VectorXd & control);
/**
* @brief 使用里程计位姿观测更新
* @param measurement 观测 [x, y, yaw]
* @param R 观测噪声协方差 (3x3)
*/
void updateOdom(const Eigen::VectorXd & measurement, const Eigen::MatrixXd & R);
/**
* @brief 使用IMU角速度观测更新
* @param measurement 观测 [ω]
* @param R 观测噪声协方差 (1x1)
*/
void updateIMU(const Eigen::VectorXd & measurement, const Eigen::MatrixXd & R);
/** @brief 获取当前状态 (5x1) */
const Eigen::VectorXd & getState() const { return x_; }
/** @brief 获取当前协方差 (5x5) */
const Eigen::MatrixXd & getCovariance() const { return P_; }
private:
Eigen::VectorXd x_; // 状态
Eigen::MatrixXd P_; // 协方差
/**
* @brief 运动模型状态转移函数 f(x, u, dt)
* @return 下一时刻状态
*/
Eigen::VectorXd motionModel(const Eigen::VectorXd & state,
const Eigen::VectorXd & control, double dt);
/**
* @brief 运动模型雅可比矩阵 (F)
*/
Eigen::MatrixXd jacobianF(const Eigen::VectorXd & state,
const Eigen::VectorXd & control, double dt);
/**
* @brief 观测模型 h(x) (位姿观测)
*/
Eigen::VectorXd observePose(const Eigen::VectorXd & state);
/**
* @brief 位姿观测雅可比矩阵 (H)
*/
Eigen::MatrixXd jacobianHPose(const Eigen::VectorXd & state);
/**
* @brief IMU角速度观测模型 h_imu(x)
*/
Eigen::VectorXd observeIMU(const Eigen::VectorXd & state);
/**
* @brief IMU观测雅可比矩阵 (H_imu)
*/
Eigen::MatrixXd jacobianHIMU(const Eigen::VectorXd & state);
};
} // namespace amr_localization
#endif // AMR_LOCALIZATION__EKF_FILTER_HPP_
1.4.2 实现文件
/**
* @file ekf_filter.cpp
* @brief EKF 核心实现
*
* 运动模型:
* x_{t+1} = x_t + v_t*cos(θ_t)*dt
* y_{t+1} = y_t + v_t*sin(θ_t)*dt
* θ_{t+1} = θ_t + ω_t*dt
* v_{t+1} = v_cmd + noise_v
* ω_{t+1} = ω_cmd + noise_ω
*/
#include "amr_localization/ekf_filter.hpp"
#include <cmath>
namespace amr_localization
{
EKFilter::EKFilter()
: x_(Eigen::VectorXd::Zero(5)),
P_(Eigen::MatrixXd::Identity(5,5) * 0.1)
{
}
void EKFilter::init(const Eigen::VectorXd & state, const Eigen::MatrixXd & covariance)
{
x_ = state;
P_ = covariance;
}
void EKFilter::predict(double dt, const Eigen::VectorXd & control)
{
Eigen::MatrixXd F = jacobianF(x_, control, dt);
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(5,5);
// 过程噪声：基于控制量的不确定性
Q.diagonal() << 0.01, 0.01, 0.01, 0.1, 0.1;
x_ = motionModel(x_, control, dt);
P_ = F * P_ * F.transpose() + Q;
}
void EKFilter::updateOdom(const Eigen::VectorXd & measurement, const Eigen::MatrixXd & R)
{
Eigen::VectorXd z_pred = observePose(x_);
Eigen::MatrixXd H = jacobianHPose(x_);
Eigen::MatrixXd S = H * P_ * H.transpose() + R;
Eigen::MatrixXd K = P_ * H.transpose() * S.inverse();
Eigen::VectorXd innovation = measurement - z_pred;
// 角度归一化
innovation(2) = std::atan2(std::sin(innovation(2)), std::cos(innovation(2)));
x_ += K * innovation;
P_ = (Eigen::MatrixXd::Identity(5,5) - K * H) * P_;
}
void EKFilter::updateIMU(const Eigen::VectorXd & measurement, const Eigen::MatrixXd & R)
{
Eigen::VectorXd z_pred = observeIMU(x_);
Eigen::MatrixXd H = jacobianHIMU(x_);
Eigen::MatrixXd S = H * P_ * H.transpose() + R;
Eigen::MatrixXd K = P_ * H.transpose() * S.inverse();
Eigen::VectorXd innovation = measurement - z_pred;
x_ += K * innovation;
P_ = (Eigen::MatrixXd::Identity(5,5) - K * H) * P_;
}
// ============================================================
// 运动模型
// ============================================================
Eigen::VectorXd EKFilter::motionModel(const Eigen::VectorXd & state,
const Eigen::VectorXd & control, double dt)
{
Eigen::VectorXd next(5);
double x = state(0), y = state(1), theta = state(2);
double v = state(3), omega = state(4);
double v_cmd = control(0), omega_cmd = control(1);
next(0) = x + v * cos(theta) * dt;
next(1) = y + v * sin(theta) * dt;
next(2) = theta + omega * dt;
next(3) = v_cmd; // 速度假设直接跟随命令
next(4) = omega_cmd;
return next;
}
Eigen::MatrixXd EKFilter::jacobianF(const Eigen::VectorXd & state,
const Eigen::VectorXd & control, double dt)
{
double theta = state(2);
double v = state(3);
Eigen::MatrixXd F = Eigen::MatrixXd::Identity(5,5);
F(0,2) = -v * sin(theta) * dt;
F(0,3) = cos(theta) * dt;
F(1,2) = v * cos(theta) * dt;
F(1,3) = sin(theta) * dt;
F(2,4) = dt;
return F;
}
// ============================================================
// 观测模型 (位姿)
// ============================================================
Eigen::VectorXd EKFilter::observePose(const Eigen::VectorXd & state)
{
return state.head(3); // x, y, theta
}
Eigen::MatrixXd EKFilter::jacobianHPose(const Eigen::VectorXd & state)
{
Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3,5);
H(0,0) = 1.0;
H(1,1) = 1.0;
H(2,2) = 1.0;
return H;
}
// ============================================================
// 观测模型 (IMU 角速度)
// ============================================================
Eigen::VectorXd EKFilter::observeIMU(const Eigen::VectorXd & state)
{
Eigen::VectorXd z(1);
z(0) = state(4); // ω
return z;
}
Eigen::MatrixXd EKFilter::jacobianHIMU(const Eigen::VectorXd & state)
{
Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1,5);
H(0,4) = 1.0;
return H;
}
} // namespace amr_localization
1.5 EKF 定位节点实现
1.5.1 节点代码
/**
* @file ekf_node.cpp
* @brief 扩展卡尔曼滤波器定位节点
*
* 订阅 /odom 和 /imu，利用 message_filters::Synchronizer 实现近似时间同步。
* 每个同步周期执行 EKF 预测与更新，发布融合后的 /ekf_odom 及 tf 变换。
*/
#include "amr_localization/ekf_node.hpp"
#include "amr_localization/ekf_filter.hpp"
#include <rclcpp/logging.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <Eigen/Dense>
#include <cmath>
namespace amr_localization
{
EKFNode::EKFNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("ekf_node", options),
ekf_(std::make_unique<EKFilter>())
{
// 参数
this->declare_parameter("frequency", 50.0);
this->declare_parameter("odom_noise_std", std::vector<double>{0.05, 0.05, 0.02});
this->declare_parameter("imu_noise_std", 0.01);
this->declare_parameter("initial_state", std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0});
// 初始化滤波器
auto init_vec = this->get_parameter("initial_state").as_double_array();
Eigen::VectorXd init_state(5);
for (int i = 0; i < 5; ++i) init_state(i) = init_vec[i];
Eigen::MatrixXd init_cov = Eigen::MatrixXd::Identity(5,5) * 0.1;
ekf_->init(init_state, init_cov);
// 发布器
pub_ekf_odom_ = this->create_publisher<nav_msgs::msg::Odometry>("ekf_odom", 10);
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// 同步订阅
odom_sub_.subscribe(this, "odom", rclcpp::SensorDataQoS().get_rmw_qos_profile());
imu_sub_.subscribe(this, "imu", rclcpp::SensorDataQoS().get_rmw_qos_profile());
sync_ = std::make_unique<Sync>(MySyncPolicy(10), odom_sub_, imu_sub_);
sync_->registerCallback(&EKFNode::syncedCallback, this);
RCLCPP_INFO(this->get_logger(), "EKF Node started");
}
void EKFNode::syncedCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odom,
const sensor_msgs::msg::Imu::ConstSharedPtr & imu)
{
// 当前时间
rclcpp::Time current_time = odom->header.stamp;
// 计算 dt
static rclcpp::Time last_time = current_time;
double dt = (current_time - last_time).seconds();
last_time = current_time;
if (dt <= 0.0 || dt > 0.5) dt = 0.02; // 异常保护
// 控制输入：使用里程计消息中的速度
Eigen::VectorXd u(2);
u << odom->twist.twist.linear.x,
odom->twist.twist.angular.z;
// 预测
ekf_->predict(dt, u);
// 更新1：里程计位姿观测
{
Eigen::VectorXd z_odom(3);
z_odom << odom->pose.pose.position.x,
odom->pose.pose.position.y,
tf2::getYaw(odom->pose.pose.orientation);
auto noise_odom = this->get_parameter("odom_noise_std").as_double_array();
Eigen::MatrixXd R_odom = Eigen::MatrixXd::Zero(3,3);
R_odom.diagonal() << noise_odom[0]*noise_odom[0],
noise_odom[1]*noise_odom[1],
noise_odom[2]*noise_odom[2];
ekf_->updateOdom(z_odom, R_odom);
}
// 更新2：IMU 角速度观测
{
Eigen::VectorXd z_imu(1);
z_imu << imu->angular_velocity.z;
double imu_std = this->get_parameter("imu_noise_std").as_double();
Eigen::MatrixXd R_imu = Eigen::MatrixXd::Zero(1,1);
R_imu(0,0) = imu_std * imu_std;
ekf_->updateIMU(z_imu, R_imu);
}
// 发布融合里程计
const auto & state = ekf_->getState();
const auto & cov = ekf_->getCovariance();
auto ekf_odom_msg = nav_msgs::msg::Odometry();
ekf_odom_msg.header.stamp = current_time;
ekf_odom_msg.header.frame_id = "odom";
ekf_odom_msg.child_frame_id = "base_footprint";
ekf_odom_msg.pose.pose.position.x = state(0);
ekf_odom_msg.pose.pose.position.y = state(1);
tf2::Quaternion q;
q.setRPY(0, 0, state(2));
ekf_odom_msg.pose.pose.orientation = tf2::toMsg(q);
for (int i = 0; i < 6; ++i)
for (int j = 0; j < 6; ++j)
ekf_odom_msg.pose.covariance[i*6+j] = (i<5 && j<5) ? cov(i,j) : 0.0;
ekf_odom_msg.twist.twist.linear.x = state(3);
ekf_odom_msg.twist.twist.angular.z = state(4);
pub_ekf_odom_->publish(ekf_odom_msg);
// 广播 tf
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = current_time;
tf.header.frame_id = "odom";
tf.child_frame_id = "base_footprint";
tf.transform.translation.x = state(0);
tf.transform.translation.y = state(1);
tf.transform.rotation = tf2::toMsg(q);
tf_broadcaster_->sendTransform(tf);
}
} // namespace amr_localization
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(amr_localization::EKFNode)
1.6 AMCL 配置与集成
AMCL 节点由 Nav2 提供，无需自定义编码，但需要精细的参数配置。以下为完整的 amcl_params.yaml：
amcl: ros__parameters: alpha1: 0.2 # 旋转噪声 (旋转 → 旋转) alpha2: 0.2 # 旋转噪声 (旋转 → 平移) alpha3: 0.2 # 平移噪声 (平移 → 平移) alpha4: 0.2 # 平移噪声 (平移 → 旋转) base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 do_beamskip: false global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 10.0 laser_min_range: 0.3 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 min_particles: 500 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 robot_model_type: "differential" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 0.5 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05
关键参数解释：
-
laser_model_type: "likelihood_field"：使用似然域模型，计算效率高，适合动态环境。 -
odom_frame_id: "odom"：告知 AMCL 从odom坐标系接收里程计。 -
tf_broadcast: true：由 AMCL 直接发布map→odom变换，省去额外节点。
1.7 传感器同步与时间戳对齐
EKF 节点中我们使用了 message_filters 的近似时间同步器（ApproximateTime policy, 允许10ms 误差）。但如果需要更精确的时间对齐，可采用线性插值外推，以下为 IMU 数据插值到里程计时间戳的实现片段：
/**
* @brief 将 IMU 数据插值到指定时间戳
* @param imu1 较早的 IMU 消息
* @param imu2 较晚的 IMU 消息
* @param target_time 目标时间
* @return 插值后的角速度 (z轴)
*/
double interpolateIMUAngularZ(
const sensor_msgs::msg::Imu::SharedPtr & imu1,
const sensor_msgs::msg::Imu::SharedPtr & imu2,
const rclcpp::Time & target_time)
{
double t1 = imu1->header.stamp.sec + imu1->header.stamp.nanosec * 1e-9;
double t2 = imu2->header.stamp.sec + imu2->header.stamp.nanosec * 1e-9;
double t = target_time.seconds();
double ratio = (t - t1) / (t2 - t1);
ratio = std::clamp(ratio, 0.0, 1.0);
return imu1->angular_velocity.z * (1.0 - ratio) + imu2->angular_velocity.z * ratio;
}
1.8 传感器故障检测与降级策略
定位系统可能面临单个传感器失效（如 IMU 断连、里程计滑转）。我们在 EKF 节点中添加简单的故障检测与降级逻辑：
void EKFNode::checkSensorHealth()
{
auto now = this->now();
// 里程计超时检测 (超过 0.5s 无数据视为故障)
double odom_delay = (now - last_odom_time_).seconds();
odom_healthy_ = odom_delay < 0.5;
// IMU 超时检测
double imu_delay = (now - last_imu_time_).seconds();
imu_healthy_ = imu_delay < 0.5;
if (!odom_healthy_ && !imu_healthy_) {
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
"All localization sensors lost!");
// 停止发布，保持最后位姿
} else if (!odom_healthy_) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
"Odometry lost, relying on IMU only");
// 仅使用 IMU 更新
} else if (!imu_healthy_) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
"IMU lost, relying on odometry only");
}
}
1.9 多传感器融合流程图（逻辑树）
每个同步周期 ├── 检查传感器健康状态 │ ├── 都健康 → 正常融合 │ ├── 仅里程计健康 → 跳过 IMU 更新，增加过程噪声 │ ├── 仅 IMU 健康 → 使用 IMU 角速度进行航向传播，禁用位置更新 │ └── 都不健康 → 仅预测，发布警告 ├── 计算时间步长 dt ├── 提取控制输入 u ├── EKF 预测步骤 ├── if (里程计健康) │ └── EKF 更新 odom ├── if (IMU 健康) │ └── EKF 更新 IMU ├── 发布 /ekf_odom 和 /tf └── 更新协方差 (若协方差过大，触发重定位请求)
1.10 单位数据规划与缓存区规划
-
状态向量单位：x, y (米), yaw (弧度), v (米/秒), ω (弧度/秒)
-
协方差矩阵：5×5 double，对应状态顺序。
-
传感器消息队列：
SensorDataQoS+KEEP_LAST=5，保证实时性。 -
同步缓冲区：
message_filters的 ApproximateTime 缓存最多 10 帧，内存占用 < 1MB。 -
IMU 插值缓存：保留最新两帧 IMU 数据，用于时间戳对齐。
1.11 可能遇到的问题与解决方案
-
EKF 协方差矩阵奇异
-
原因：过程噪声或观测噪声设置过小，导致协方差矩阵非正定。
-
解决：增大噪声参数，或在每次更新后强制协方差对称化
P = (P + P^T)/2。
-
-
AMCL 粒子退化
-
原因：激光模型与真实环境不匹配，或粒子数量不足。
-
解决：增加
min_particles和max_particles，启用recovery_alpha_slow和recovery_alpha_fast。
-
-
tf 树断裂 (map→odom 未发布)
-
原因：AMCL 未启动或初始化位置错误。
-
解决：提供初始位姿估计（通过
initial_pose话题），确保amcl节点成功加载。
-
-
时间同步丢失
-
原因：传感器时间戳跳变或系统时钟不同步。
-
解决：启用
use_sim_time进行仿真调试，或在真实机器人上运行 Chrony 同步时钟。
-
-
定位漂移在长走廊
-
原因：缺少纵向约束（走廊特征只约束横向）。
-
解决：融合视觉里程计或增加码盘轮速计权重，考虑引入回环检测。
-
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
1
0- 0
已为社区贡献83条内容
所有评论(0)