位置推定不確実性の可視化と診断モニタリング 自律走行フレームワークautowareでは、センサー融合による自己位置推定の信頼性を担保するため、共分散行列に基づく不確実性のリアルタイム監視機能があります。本項では、位置ズレや拡大率を検知して警告を発する診断パッケージと、連続した姿勢データから平滑な軌跡を生成する補間・変換ユーティリティの実装構造について解説します。
1.1 ノード本体の実装ロジック このノードは、オドメトリ情報に含まれる6x6共分散行列からXY平面の2x2共分散サブ行列を抽出し、固有値分解を実行することで誤差楕円の幾何学的特性を算出します。計算されたパラメータはROSのマーカインタフェースを通じて可視化され、同時にシステム統合ステータスを管理するdiagnosticメッセージとして出力されます。閾値超過時にWARNINGまたはERRORレベルのステータスが設定される構成となっています。
/// @brief 位置推定不確実性を監視するノード本体
class LocalizationErrorMonitor : public rclcpp::Node
{
public:
explicit LocalizationErrorMonitor() : Node("loc_error_diag")
{
// 外部パラメータの定義
vis_scale_ = declare_parameter<double>("visual_scale");
warn_limit_ = declare_parameter<double>("warning_threshold_m");
err_limit_ = declare_parameter<double>("error_threshold_m");
// オドメトリ情報の購読開始
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
"input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdomMessage, this, std::placeholders::_1));
// 可視化用Publisher設定（latching有効化）
rclcpp::QoS view_qos(1); view_qos.transient_local();
ellipse_pub_ = create_publisher<visualization_msgs::msg::Marker>("debug/error_cov_marker", view_qos);
// ディアグノーシス用Publisher設定
diag_pub_ = create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
}
private:
/// @brief コバリアンス行列から楕円の形状パラメータを算出し、結果を出力
/// @param target_odom 入力オドメトリメッセージ
void onOdomMessage(const nav_msgs::msg::Odometry::ConstSharedPtr target_odom)
{
// 2x2 XY平面共分散行列の構築
Eigen::Matrix2d plane_cov;
plane_cov << target_odom->pose.covariance[0], target_odom->pose.covariance[1],
target_odom->pose.covariance[6], target_odom->pose.covariance[7];
// 固有値分解による楕円パラメータ取得
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig_solver(plane_cov);
EllipseParams ellipse;
ellipse.major_radius = vis_scale_ * std::sqrt(eig_solver.eigenvalues()(1));
ellipse.minor_radius = vis_scale_ * std::sqrt(eig_solver.eigenvalues()(0));
// 主成分方向ベクトルから航向角を計算
const auto principal_vec = eig_solver.eigenvectors().col(1);
ellipse.heading_angle = std::atan2(principal_vec.y(), principal_vec.x());
// 車体座標系における横方向幅の計算
const double current_heading = tf2::getYaw(target_odom->pose.pose.orientation);
ellipse.lateral_width = vis_scale_ * computeLateralSpan(plane_cov.inverse(), current_heading);
// マーカの生成と発行
visualization_msgs::msg::Marker vis_marker = generateCovVisual(ellipse, target_odom);
ellipse_pub_->publish(vis_marker);
// 診断ステータスの作成と発行
auto check_result = assessAccuracy(ellipse.major_radius, warn_limit_, err_limit_);
diag_msg_.header.stamp = now();
diag_msg_.status.clear();
diag_msg_.status.push_back(check_result);
diag_pub_->publish(diag_msg_);
}
// メンバー変数宣言は省略（標準的なROS2ノード構造に従う）
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr ellipse_pub_;
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double vis_scale_;
double warn_limit_;
double err_limit_;
diagnostic_msgs::msg::DiagnosticArray diag_msg_;
};
1.2 診断ステータス判定とマージ処理 精度評価関数は、計算された楕円サイズを预设定的な警告閾値と比較します。複数のステータスを統合する際には、最も重大なレベルを採用し、メッセージテキストを連結することで一元的な監視状態を提供します。これにより、上位制御ループやヒューマンマシンインターフェースでの迅速な障害特定が可能になります。
/// @brief 閾値比較による定位精度の評価ルーチン
/// @param measured_radius 計測された楕円半径
/// @param threshold_warn 警告基準値
/// @param threshold_err エラー基準値
diagnostic_msgs::msg::DiagnosticStatus evaluatePositionAccuracy(
double measured_radius, double threshold_warn, double threshold_err)
{
diagnostic_msgs::msg::DiagnosticStatus result;
result.values.push_back({ "accuracy_radius", std::to_string(measured_radius) });
result.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
result.message = "normal_operation";
// エラー条件を最優先で判定
if (measured_radius >= threshold_err) {
result.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
result.message = "radius_exceeds_safety_limit";
} else if (measured_radius >= threshold_warn) {
result.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
result.message = "radius_approaching_warning_level";
}
return result;
}
/// @brief 複数ステータスの優先度付きマージ処理
/// @param status_list 評価結果リスト
diagnostic_msgs::msg::DiagnosticStatus mergeMultiStatuses(
const std::vector<diagnostic_msgs::msg::DiagnosticStatus>& status_list)
{
diagnostic_msgs::msg::DiagnosticStatus combined;
combined.message = "";
for (const auto& item : status_list) {
// より重大なステータスを優先的に採用
if (item.level > combined.level) {
combined.level = item.level;
combined.message += item.message + "; ";
} else {
combined.message += item.message + ", ";
}
// 関連付加値の転記
for (const auto& kv : item.values) {
combined.values.push_back(kv);
}
}
if (combined.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
combined.message = "all_systems_nominal";
}
return combined;
}
- 姿勢補間と座標変換ユーティリティ リアルタイム制御において、センサーサンプリングの非同期性による軌道の断絶を防ぐためには、履歴バッファ内の最近傍データを線形補間し、連続的な軌道情報を生成する必要があります。また、各モジュール間で発生する座標系の変換遅延を吸収するためのTFリスナー実装も不可欠です。
2.1 姿勢配列の補間と連続性検証 補間クラスは、指定時刻に前后する最新の二つの姿勢メッセージを取得し、時系列線形補間を適用します。初期化直後の位置ジャンプや、通信遅延によるタイムスタンプの不整合を検知すると、フラグを立てて上位ロジックへ安全側に遷移させる設計になっています。
/// @brief 履歴Poseデータからの時系列補間クラス
class PoseInterpolationHandler
{
public:
PoseInterpolationHandler(rclcpp::Node::SharedPtr node,
const rclcpp::Time& request_time,
const std::deque<PoseWithCovarianceStamped::ConstSharedPtr>& history_buffer)
{
fetchNearestPoses(history_buffer, request_time, prev_pose_, next_pose_);
interpolated_result_ = linearInterpolate(prev_pose_, next_pose_, request_time);
// 補間結果の妥当性検証実行
bool temporal_valid = checkTimeDelta(request_time, prev_pose_.header.stamp, 0.5);
bool spatial_valid = checkPositionJump(prev_pose_.pose, next_pose_.pose, 2.0);
if (!temporal_valid || !spatial_valid) {
interpolation_success_ = false;
RCLCPP_WARN(node->get_logger(), "Interpolation validation failed due to discontinuity.");
} else {
interpolation_success_ = true;
}
}
[[nodiscard]] PoseWithCovarianceStamped getCurrentEstimate() const { return interpolated_result_; }
[[nodiscard]] bool isSuccess() const { return interpolation_success_; }
private:
[[nodiscard]] bool checkTimeDelta(const rclcpp::Time& req, const rclcpp::Time& base, double max_delta_sec) const {
double diff = std::abs((req - base).seconds());
if (diff > max_delta_sec) {
RCLCPP_WARN(__func__, "Timestamp mismatch exceeds limit: %lf sec", diff);
return false;
}
return true;
}
[[nodiscard]] bool checkPositionJump(const geometry_msgs::msg::Pose& p1, const geometry_msgs::msg::Pose& p2, double max_dist) const {
double dist = norm(p1.position, p2.position);
if (dist > max_dist) {
RCLCPP_WARN(__func__, "Position jump detected: %lf m", dist);
return false;
}
return true;
}
PoseWithCovarianceStamped prev_pose_, next_pose_, interpolated_result_;
bool interpolation_success_ = true;
};
2.2 TF2座標変換リスナーモジュール このモジュールは、主要な座標系間の剛体変換行列を取得する役割を担います。変換情報の取得に失敗した場合でも、システム全体のパニックを防ぐため、単位行列によるアイデンティティ変換を安全デフォルトとして返却します。これは特に起動直後の初期化フェーズや、一時ネットワーク断発生時のフォールバック処理として有効です。
/// @brief 座標変換情報の取得とフォールバック処理モジュール
class CoordinateFrameListener
{
public:
explicit CoordinateFrameListener(rclcpp::Node::SharedPtr node_ptr)
: logger_(node_ptr->get_logger()), tf_buffer_(node_ptr->get_clock()), listener_(tf_buffer_) {}
/// @brief 指定された親子関係の剛体変換を取得。失敗時はアイデンティティ変換を返す
/// @param target_frame ターゲット座標系
/// @param source_frame ソース座標系
/// @return 変換行列。取得不可時は単位行列
TransformStamped getTransformSafe(const std::string& target_frame, const std::string& source_frame) {
TransformStamped safe_identity;
safe_identity.header.frame_id = source_frame;
safe_identity.child_frame_id = target_frame;
safe_identity.transform.rotation.w = 1.0;
safe_identity.transform.translation.x = 0.0;
try {
return listener_.lookupTransform(target_frame, source_frame, tf2::TimePointZero);
} catch (const tf2::TransformException& ex) {
RCLCPP_WARN(logger_, "TF Lookup Failed [%s]: %s. Returning identity fallback.",
source_frame.c_str(), ex.what());
return safe_identity;
}
}
private:
rclcpp::Logger logger_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener listener_;
};