SMSL Robot Positioning Toolkit
自動運転やロボティクスで不可欠な、ロボット位置処理やlaserscan処理機能をパッケージ化しています。主に二次元LiDARから取得されるlaserscan信号から、RTMaps 4環境で動作する包括的な二次元点群処理コンポーネント群を提供します。laserscan信号からのポーズ推定、様々な種類のカルマンフィルタ機能をサポートしています。
SMSL Robot Positioning Toolkitは、サードパーティー製のlaserscan処理ライブラリを使用しておらず、外部ライブラリのバージョン依存性がありません。
SMSL Robot Positioning Toolkitは、鈴鹿経営科学研究所の有償IPであり、使用者の方からの機能拡張のご相談を受け付けております。
ご購入方法
SMSL Robot Positioning Toolkitは現在予約受付中であり、購入や価格のお問い合わせは、お問い合わせフォームから受け付けています。
また、RTMaps AI storeから購入いただくことも可能となる予定です。
提供コンポーネントリスト
2-D laserscan 処理
laserscan_angle_generator
- laserscanデータから角度信号を生成します。
laserscan_outlier_removal_filter
- laserscanデータから外れ点を除去します。
laserscan_sorting
- laserscanデータの角度信号を昇順/降順にソートします。
laserscan_resampling
- laserscanデータを線形補間を使用してリサンプリングします。
laserscan_downsample
- 2次元の点を規則的なグリッドでダウンサンプリングし、NDTによるポーズ推定で使うセルごとの統計量を出力します。
laserscan_polar2cart
- 極座標系のlaserscanデータを直交座標のXYデータに変換します。
laserscan_cart2polar
- 直交座標系のXYデータを極座標のlaserscanデータに変換します。
laserscan_range_filter
- laserscanデータに範囲フィルタを適用します。
2-D オドメトリ
laserscan_pose_estimation_icp
- icpアルゴリズムで2Dポーズを推定します。誤差の測り方を点対点と点対線（壁の線に合わせる）から選べます。
laserscan_pose_estimation_ndt
- ndtアルゴリズムで2Dポーズを推定します。
laserscan_object_detection
- laserscanデータから動体点オブジェクトを検出します。
pose2D_transform
- 2D の姿勢と XY の点（共分散も）を、センサの取り付け位置などの固定のずれを使って別の座標に変換します。
realobject2drawingobject
- RealObject のリストを、地図画像に重ねる楕円の描画オブジェクトに変換します。
egomotion_correction_2D
- 二次元で自己車両の移動、回転（エゴモーション）に伴う点群の歪みを補正します。
pose2D_accumulator
- 二次元の相対ポーズを積分します。
position2D_phys2pixel
- XYデータの物理座標を画像内のピクセル座標に変換します。
position2D_pixel2phys
- 画像内のピクセル座標をXYデータの物理座標に変換します。
カルマンフィルタ
KalmanFilter_Linear1D
- 初歩的な一次元の線形カルマンフィルタを適用します。
KalmanFilter_Linear2D
- 初歩的な二次元の線形カルマンフィルタを適用します。
TrackingKF_2DConstSpeed
- 等速度モデルの二次元の追跡カルマンフィルタを適用します。計測はワールド座標の位置 [x, y] です。
TrackingKF_2DConstAcc
- 等加速度モデルの二次元の追跡カルマンフィルタを適用します。計測はワールド座標の位置 [x, y] です。
TrackingEKF_2DConstSpeed
- 等速度モデルの二次元の追跡拡張カルマンフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingEKF_2DConstAcc
- 等加速度モデルの二次元の追跡拡張カルマンフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingUKF_2DConstSpeed
- 等速度モデルの二次元の追跡アンセンテッドカルマンフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingUKF_2DConstAcc
- 等加速度モデルの二次元の追跡アンセンテッドカルマンフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingPF_2DConstSpeed
- 等速度モデルの二次元の追跡パーティクルフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingPF_2DConstAcc
- 等加速度モデルの二次元の追跡パーティクルフィルタを適用します。計測はセンサから見た距離と方位（視線速度も使えます）です。
TrackingExtractor_2DConstSpeed
- 等速度モデルの二次元の追跡フィルタの状態から、位置と速度を取り出します。
TrackingExtractor_2DConstAcc
- 等加速度モデルの二次元の追跡フィルタの状態から、位置・速度・加速度を取り出します。
補助コンポーネント
angle_deg2rad
- 角度信号の単位を度からラジアンに変換します。
angle_rad2deg
- 角度信号の単位をラジアンから度に変換します。
Value2Diag
- 入力された数値を対角成分にもつ対角行列を生成します。
ExtractDiagonals
- 入力された対角行列から対角成分の数値ベクトルを取り出します。
ソフトウェア仕様
| パッケージ名 | RobotPositioningToolkit.pck |
|---|---|
| バージョン | 1.52 |
| 対応OS | Windows 10/11 (64bit) / Ubuntu 22.04 LTS, 24.04 LTS (64bit) |
| 対応RTMapsバージョン | RTMaps 4.9.2 以降 |
| 提供物 | パッケージ本体(.pck)、ユーザーマニュアル(PDF)、サンプルダイヤグラム(.rtd)、laserscanサンプルデータ |
デモ機能
本パッケージには、導入後すぐに動作を確認できる以下のデモが同梱されています。
- Laserscan_creation_demo: 数ベクトル信号からlaserscanデータを生成する例。
- Laserscan_resampling_demo: laserscanデータをリサンプリングする例。
- Laserscan_pose_processing_demo: laserscanデータから二次元ポーズを推定し、点群の歪を補正する例。
- KalmanFilter_1D_demo: 初歩的な一次元カルマンフィルタを適用する例。
- KalmanFilter_2D_demo: 初歩的な二次元カルマンフィルタを適用する例。
- TrackingFilter_2DConstSpeed_demo: 等速度モデルの二次元の各追跡フィルタ（KF、EKF、UKF、PF）を使用する例。
- TrackingFilter_2DConstAcc_demo: 等加速度モデルの二次元の各追跡フィルタ（KF、EKF、UKF、PF）を使用する例。
- Laserscan_pose_processing_robot_spec_demo: このデモは、Laserscan_pose_processing_demo にロボットの仕様を足したものです。
更新履歴
- 2026 11/7: バージョンを1.52に更新しました。laserscan_pose_estimation_icpのmap_window_in_m、laserscan_pose_estimation_ndtのmap_window_in_m、robot_spec、robot_motion_limitsを追加しました。laserscan_pose_estimation_icp、laserscan_pose_estimation_ndtを修正しました。
- 2026 11/2: バージョンを1.51に更新しました。laserscan_pose_estimation_icpのrobot_spec、robot_motion_limitsを追加しました。laserscan_pose_estimation_icp、laserscan_pose_estimation_ndt、pose2D_accumulatorを修正しました。
- 2026 10/24: バージョンを1.5に更新しました。pose2D_transform、realobject2drawingobject、2DのトラッキングフィルタTrackingKF/EKF/UKF/PF_2DConstAcc/2DConstSpeedとTrackingExtractor_2DConstAcc/2DConstSpeed（3Dのトラッキングフィルタに代わるもの）、laserscan_pose_estimation_icpの外部オドメトリ入力、laserscan_pose_estimation_ndtのオドメトリ誤差のプロパティ、laserscan_object_detectionの出力real_objects、Value2Diagのuse_array_inputを追加しました。KalmanFilter_Linear1D、KalmanFilter_Linear2D、Value2Diag、laserscan_object_detection、laserscan_outlier_removal_filter、laserscan_polar2cart、laserscan_pose_estimation_icp、laserscan_pose_estimation_ndt、laserscan_range_filterを修正しました。
- 2026 10/23: バージョンを1.4に更新しました。laserscan_pose_estimation_ndtにpoint-to-distributionとdegeneracy_handlingのモード、外部のオドメトリによる予測を、laserscan_pose_estimation_icpにdegeneracy_handlingを、laserscan_outlier_removal_filterにmin_deviation_in_mを追加しました。ExtractDiagonals、KalmanFilter_Linear1D、KalmanFilter_Linear2D、TrackingEKF_3DConstAcc、TrackingEKF_3DConstSpeed、TrackingExtractor_ConstAcc、TrackingExtractor_ConstSpeed、TrackingKF_3DConstAcc、TrackingKF_3DConstSpeed、TrackingPF_3DConstAcc、TrackingPF_3DConstSpeed、TrackingUKF_3DConstAcc、TrackingUKF_3DConstSpeed、Value2Diag、angle_rad2deg、laserscan_angle_generator、laserscan_cart2polar、laserscan_object_detection、laserscan_outlier_removal_filter、laserscan_polar2cart、laserscan_pose_estimation_icp、laserscan_pose_estimation_ndt、laserscan_range_filter、laserscan_resampling、laserscan_sorting、pose2D_accumulator、position2D_phys2pixel、position2D_pixel2physを修正しました。
- 2026 10/19: バージョンを1.3に更新しました。position2D_phys2pixel、position2D_pixel2phys、laserscan_downsampleを修正しました。
- 2026 10/19: バージョンを1.2に更新しました。laserscan_pose_estimation_icpに点対線モードを追加しました。laserscan_pose_estimation_icp、egomotion_correction_2D、laserscan_downsampleを修正しました。
- 2026 10/16: バージョンを1.1に更新しました。laserscan_range_filter、egomotion_correction_2D、position2D_phys2pixel、position2D_pixel2phys、KalmanFilter_Linear1D、KalmanFilter_Linear2D、TrackingEKF/KF/PF/UKF_3DConstAcc・_3DConstSpeedを修正しました。