日本ロボット学会誌 Vol. xx No. xx, pp.1〜4, 202x

1

レター

林内作業車の自動走行に向けた森林環境におけるスキャンマッチングによる自己位置推定の破綻判定
富田健 斗∗1 鈴 木 理 也∗1 岩 澤 尚 樹∗1 新 貝 文
櫻 井 恭 介∗2 南智 好∗2 藤 井 浩 光∗1昭∗2Localization Failure Detection by Scan Matchingin Forest Environment for Automatic Driving of Forestry Work Vehicle
Kento TOMITA∗1 , Naoya SUZUKI∗1 , Naoki IWASAWA∗1 , Fumiaki SHINKAI∗2 ,Kyousuke SAKURAI∗2 , Chiyoshi MINAMI∗2 and Hiromitsu FUJII∗1Smart forestry with ICT technologies is promoted to solve problems in forestry industry such as labor shortages. One of signiﬁcanttask is autonomous operation of forestry vehicles. Self-localization is a fundamental technology in autonomous operation. In severe
environment such as forestry-work-road, recovery is necessary in localization failure. Correct detection of the failure condition is quite
important; however, it is diﬃcult because available sensors are limited in forestry environment. In this paper, we propose a metric for
detecting failure condition in localization with consideration to the reliability of scan-matching on forestry-work-roads.
Key Words: Localization failure detection, Self localization, NDT scan matching, Smart forestry, 3D LiDAR application

1. 緒言

1. 1 森林作業道における自己位置推定の需要と課題
林業分野では近年，従事者数の減少や従事者の高齢化による人手不足などの深刻な問題の解決のために，林野庁を主体としてスマート林業が推進されている [1]．スマート林業は，地理空間情報や ICT 技術などを応用し作業の自動化や安全性の向上を目指す取り組みであり，特に自動化の需要の高い作業の一つが森林作業道における林内作業車の運転である．森林作業道は，Fig. 1: Situation of forestry site and our proposed system

重機で山林を切り拓いて作設される図 1 のような環境であり，己位置推定の精度が大きく低下する恐れがある．また，安全の崖沿いで道幅が狭い場合が多く断崖への滑落や絶壁への衝突な観点からも不確実な行動が許容されない．

どの危険が常に伴う．そのため，森林作業道においては極めて正確な自己位置推定が必要となる．

我々の研究チームでは，森林作業道における自己位置推定の課題に対して，最適化アプローチに基づく手法 [3] を提案してき森林作業道における自己位置推定では厳しい環境条件から，た．具体的には，森林作業道を作設する際に入手可能な 3 次元

Global Navigation Satellite System（GNSS）や Inertial Measurement Unit（IMU）等の慣性センサによる測位，駆動系の回転数測量データを大域点群地図とした NDT スキャンマッチング [4]に基づく自己位置推定法を提案した．特に，最適化アプローチを用いたデッドレコニングなどの精度が低下する．この環境条による自己位置推定の課題である破綻時の復帰に関して，森林件により，使用可能なセンサ情報に制約が生じ，正確な自己位作業道内でのリカバリが可能なシステムを構築し，実際の現場置推定が困難となる．

環境におけるフォワーダを用いた実機検証で有効性を確認した．

1. 2 従来研究の課題と本研究のアプローチ・目的

しかし，自己位置推定の破綻に誤判定が多く生じることが課

移動体の自動走行では，確率的アプローチに基づく自己位置推題として残っており，特に「推定位置は正しいにもかかわらず定 [2] の適用例が数多く存在する．確率的アプローチでは，様々破綻と誤判定するケース」が多発している．ここで，本稿ではなセンサ情報を用いて計算した位置の確率密度から自己位置を自己位置推定における破綻判定の誤判定のうち，この偽の破綻推定するが，森林作業道では上述のセンサの制約が大きく，自を「偽破綻」と定義する．上述した従来手法の課題である偽破綻の多発は，安全性の観点からは許容されるが，作業効率の観原稿受付∗1
千葉工業大学∗2
パシフィックコンサルタンツ株式会社∗1
Chiba Institute of Technology∗2
Paciﬁc Consultants Co., Ltd.
■ 要素分野日本ロボット学会誌 xx 巻 xx 号

点からは深刻な課題である．例えば，従来研究 [3] のように破綻時の復帰に人工ランドマークなどを用いる場合，それらを破綻が生じ得る地点に設置する必要がある．しかし，森林作業道における作業を妨げる懸念から，標識などの設置数は最小にと—1—202x 年 xx 月

富田2

健斗

鈴木 理也

岩澤尚樹

新貝文昭

櫻井恭介

南智好藤井浩光

どめる必要がある．さらに，破綻時など自己位置推定の精度が低下した車両は，安全性の観点から位置が正しく復帰するまで減速や停止という行動を求められるため作業効率の低下を招く原因となり，偽破綻であっても不正確な破綻判定は問題である．
そのため，森林作業道における自己位置推定の正確な破綻判定は極めて重要となる．

Fig. 2: Schematic of our proposed system

本研究では，森林作業道の大域点群地図を用いたスキャンマッチングに焦点を当て，自己位置推定における破綻を正確に判定

2. 2. 2 評価指標することを目的とする．本稿では，従来研究 [3] の自己位置推

本研究では，林内作業車の NDT スキャンマッチングにおける定手法に基づいた，スキャンマッチングの信頼度を考慮した破自己位置推定の破綻を正確に判定するために，以下の式 (1) で綻判定法を提案する．

表される評価指標を提案する．

score(t) = αA(t) + βB(t) + γC(t)2. 提 案 手 法（1）

2. 1 提案システムの概要

ここで，A，B，C が自己位置推定結果の不整合状態を表す定量提案システムの概要を図 2 に示す．従来研究 [3] で我々が提的な指標（状態指標）であり，それぞれ 2. 2. 1 項における case

案した自己位置推定システムに基づいており，作業道を作設す

1) から case 3) に対応するように，各状態指標の役割が相互補る際の測量などで事前に得られる大域点群地図と，車載 LiDAR完的になるように設計する．さらに，破綻の程度を距離の誤差によるスキャン点群との NDT スキャンマッチング [4] を行う．

として評価することを目的に，各状態指標 A，B，C を全て実世

大域点群地図データとスキャン点群は，森林環境における落葉界の距離尺度で定義する．また，α，β，γ は各状態指標の重みなどの変化に対して頑健化するため反射強度ベースのフィルタ係数であり，各状態指標の挙動を確認しながら α + β + γ = 1 とリングを施し樹木の葉部を除去した上で用いる．本研究では，なるように設定する．score に対して閾値 θcol を設定し，式 (1)NDT スキャンマッチングの破綻判定を行うための評価指標を新規に提案する（2. 2. 2 項）
．破綻判定時には，環境中に設置した局所的な位置情報が付与された人工ランドマークを LiDARの 3 次元点群情報を用いて認識し，人工ランドマークに対するLiDAR の位置姿勢を用いて NDT スキャンマッチングにおけるのスコアが閾値以上の場合に自己位置推定が破綻していると判定する．以下，各状態指標に関する詳細を説明する．
状態指標 A は，case 1) の破綻に対応する鉛直方向の誤差を検出する指標である．本システムでは，事前の測量に基づく正確な大域点群地図を有しており，林内作業車は自己位置推定時に探索の初期位置を補正する．以降，本研究における新規の提案は常に路面上に存在しているため，ある時刻 t において正確にである自己位置推定の破綻判定法について詳細を説明する．

自己位置推定ができている場合，NDT スキャンマッチングで推2. 2 森林作業道でのスキャンマッチングの破綻判定法

定された位置 p(t) の鉛直方向成分の値と対応する大域点群地図

2. 2. 1 森林作業道における自己位置推定の破綻の傾向の標高値は一致するはずである．そこで状態指標 A を，p(t) と本研究では，森林作業道における大域点群地図上の地形や車大域点群地図中の走路における路面までの距離として以下の様両の走行性能を考慮した上で，自己位置推定結果と実際の走行に定義する．

A(t) = ∥ p(t) − p(t)⊥ ∥データとの間に不整合が生じた場合，その自己位置推定の結果（2）の信頼度は低いと見なし破綻として判定する．本研究では破綻

ここで，p(t)⊥ は p(t) から路面に下した垂線の足であり，A はのパターンを以下の 3 通りに分類する．

大域点群地図中の最近傍点までの距離となる．最近傍点探索はcase 1) 標高データにおける大域点群地図と推定位置間の不整合
case 2) 車両の移動量におけるフレーム間の連続性の不整合
case 3) 移動量における車載センサのデータと推定値間の不整合

kd-tree アルゴリズムを用いる．
状態指標 B は，case 2) の破綻に対応するフレーム間の移動量の大きさを表す指標である．時刻 t における推定位置 p(t) を用いて，状態変数 B を式 (3) で定義する．

case 1) は，推定された自己位置における鉛直方向の位置が，大域点群地図上の同地点における標高データと大きくずれていB(t) = ∥ p(t) − p(t − 1) ∥（3）る状態である（図 3(a)）．case 2) は，各フレーム間での推定位置

林内作業車の時刻 t − 1 から t までの平均速度 v̄(t) とサンプリが林内作業車の走行速度を考慮しても不自然に大きく変化した

ング間隔 ∆t を用いると，正しく自己位置推定が行われている状態である（図 3(b)）．case 3) は，車載センサデータの変化に場合には B(t) ≃ v̄(t)∆t が成立する．林内作業車の制限速度をvlim ≥ v̄(t) とすると， B′ (t) = B(t) − vlim ∆t とオフセットを考慮した 0 以下を正常値とした符号付きの指標として再定義するこ対して推定値の変化が大きく異なる状態である（図 3(c)）．case1)，case 2) は主に瞬間移動的な挙動として確認されており，ロボットの自己位置推定における誘拐状態に対応する．case 3) はcase 1) と case 2) に加えて，実際には移動しているが自己位置とができる．
状態指標 C は，case 3) の破綻に対応する，主に実際は移動し推定の結果は停滞している状態も含む．ている状況において自己位置推定結果が停滞することによる誤case 1) の状態は，特に森林作業道特有の勾配の大きな斜面や急旋回の必要なカーブに差し掛かった瞬間など，車載 LiDAR に差を表す指標である．車載センサから求める変位 ∆ps (t) と推定よる路面の計測点が減少した際に生じることが確認できている．

で定義する．

移動量 ∆p(t) = ∥ p(t) − p(t − 1) ∥ を用いて，状態指標 C を式 (4)case 2) と case 3) の状態も，case 1) と同様の環境における走行時

C(t) = | ∆ps (t) − ∆p(t) |に多く確認されるが，特に樹木の配置パターンが似た直線的なここで，例えば ∆ps (t) は IMU の加速度情報の二階積分や，市走路において誤マッチングが生じることで発生する．以下では，販のトラッキングセンサなどから得られる移動ベクトルから獲これら 3 つの破綻のパターンに対応した評価指標を設定する．

得可能である．式 (3) と式 (4) の定義からも確認できる通り，

JRSJ Vol. xx No. xx—2—（4）xx, 202x

林内作業車の自動走行に向けた森林環境におけるスキャンマッチングによる自己位置推定の破綻判定(a) Overview of case 1)(b) Overview of case 2)

3(c) Overview of case 3)

Fig. 3: Overview of localization failure pattern
3. 2. 1 自己位置推定結果
自己位置推定の結果を図 6 に示す．図 6(a)，図 6(b)，図 6(c)はそれぞれ地図座標系における x，y，z 座標での自己位置推定の結果を示しており，縦軸が走行開始地点からの各軸方向への距離，横軸は走行開始からの時間を示しており，走行開始から236.0 s 後に走行が終了している．黒線で走行経路の真値を示し，従来研究 [3] に基づいた自己位置推定による推定位置を示しており，赤線，青線，緑線がそれぞれ x 座標，y 座標，z 座標を示

Fig. 4: 3D point cloud map

している．走行経路の真値は，トータルステーションを用いて取得した走行中の実機の座標を基に手動で正確に作成した．
自己位置推定の破綻については，まず図 6 中の 216.2 s に自己位置推定が破綻し始め，正しい自己位置に復帰できていないことが確認できる．終盤の走路は勾配の極めて大きな地形であり，これは 2. 2. 1 項で述べた破綻パターン case 1)，case 2)，case3) の全てが生じ得る状況に該当する．また，図 6(c) において，78.8 s から 92.8 s の時間帯で z 軸方向への誤差が大きくなり，破Fig. 5: Forwarder running on forestry-work-road綻していることが確認できる．この走行においては，その後のC = | ∆ps (t) − B(t) | であるため，状態指標 B で判定される破綻時には高い相関を示すが， B = 0 となる停滞による破綻時には初期位置として直前の推定位置を利用することで正しい位置に独立して機能する指標である．

推定に失敗する可能性があるため，本来はこの場合も走行を停復帰しているが，状況においてはこのような誤差から自己位置止し自己位置の復帰を図る必要がある．以降，これらの破綻の3. 森林作業道の走行データを用いた実験
提案手法による自己位置推定の破綻判定の有効性を検証する

判定の正確さに関して，従来手法と提案手法の比較を行う．

3. 2. 2 破綻判定結果ために，実際の森林作業道において実機実験を行った．

自己位置推定の破綻判定の比較対象として，従来研究 [3] で

3. 1 実験環境用いられている NDT スキャンマッチングのマッチングスコア

実験は，福井県永平寺町において福井森林組合が管理する森林作業道で行った．実験環境を図 5 に示す．実験に利用した森

score NDT を扱う．score NDT は，NDT スキャンマッチング後の 2つの点群のユークリッド距離の和であり，マッチング結果の信林作業道の経路長は約 320 m であり，高低差は 10 m，最大傾頼度として一般的に広く用いられる指標である．従来研究に関斜角は 14deg であった．大域点群地図は，走行実験とは別日にしては，自己位置推定の精度が向上するように事前に十分な調Leica 社の Pegasus 2 Ultimate を用いた地上レーザ測量によって作成した．図 4 は計測した大域点群地図であり，経路全体を示整を行い，マッチングの閾値を 4.0 と設定し，閾値以上の時のすとともに枠で囲った部分について拡大図を示している．点群

ら，score NDT と，提案手法における状態指標 A，B，C および評から路面や樹々の形状が得られていることが確認でき，提案手推定結果を自己位置推定の破綻とした．自己位置推定の結果か価指標である式 (1) を算出した．

法ではこれら点群の分布を用いたマッチングを行う．

score NDT の時間変化を図 7 に示す．縦軸はマッチングスコア

林内作業車として，イワフジ社製のフォワーダ U-40D を用いの値，横軸は走行開始からの自己位置推定がされた回数を示して，スキャン点群を取得するために Livox 社製の LiDAR は HAPている．図 7 の橙色で網掛けした領域が，自己位置推定が破綻を 2 台使用した．式 (4) の算出には HAP に内蔵された IMU をしたと判定された時間帯である．時刻 30 s 付近，160.4 s から用いて，加速度情報の二階積分から移動量を算出した．

3. 2 実験結果

169.2 s，172.2 s から 177.0 s，180.4 s から 186.8 s，188.6 s から
196.4 s の各時間帯において破綻として判定されているが，図 6

森林作業道の走行中に NDT スキャンマッチングによる自己から確認できるように，これらは本来は正確な自己位置推定がな位置推定を行い，自己位置推定の破綻が確認できた走行データされている時間帯である．また，図 6 中で z 軸方向への誤差が大で検証を行った．我々の従来研究 [3] では，検証のために自己きくなっている本来は破綻と判定されるべき 78.8 s から 92.8 s

位置推定の破綻が生じた際に人工ランドマーク認識により初期の時間帯については，破綻と判定されていない．さらに，216.2 s

位置の補正を行いリカバリを試みるが，本実験では検証のためから走行終了時までの破綻については正確に判定できているが，にリカバリは行わず，直前のフレームの推定位置を初期位置と約 17 秒前の 199.6 s から破綻と判定されてしまっている．

して用いることで自己位置推定を継続した．各フレームにおけ提案手法における状態変数 A， B，C の時間変化を図 8(a)，る自己位置推定結果に対して従来手法と提案手法による破綻判

図 8(b)，図 8(c) にそれぞれ示し，最終的に算出される評価指標定を行い，その結果の比較から提案手法の性能を評価した．

である式 (1) の時間変化を図 9 に示す．各図とも，縦軸は変数

日本ロボット学会誌 xx 巻 xx 号

—3—

202x 年 xx 月

富田4

健斗

鈴木 理也

岩澤尚樹

新貝文昭

櫻井恭介

南智好藤井浩光(a) Estimated position of x(a) Condition value of A(b) Estimated position of y(b) Condition value of B(c) Estimated position of z(c) Condition value of C

Fig. 6: Result by NDT scan matching

Fig. 8: Change of condition values of A, B, and C

Fig. 7: Change of score NDT in the NDT scan matching

Fig. 9: Change of score in the proposed systemの値，横軸は走行開始からの自己位置推定がされた回数を示しであることが確認できた．提案手法で正確に破綻を判定するこている．図 9 の青色で網掛けした領域が，式 (1) が閾値以上ととで，偽破綻がより少数となり，実車両の作業効率の向上や人なり自己位置推定が破綻したと判定された時間帯である．式 (1)工ランドマーク設置数の少数化への貢献が可能になる．

における各状態変数の重みと閾値は，各状態指標の取り得る値4. 結から α = 0.5，β = 0.3，γ = 0.2 と設定した．従来研究 [3] における自己位置推定の位置精度に関するシステムの要求仕様から，
式 (1) の閾値を 0.4 m と設定し，閾値以上の時の推定結果を自己位置推定の破綻とした．図 9 の 78.8 s から 92.8 s の時間帯において，図 6 中で z 軸方向への誤差が大きくなっている破綻が正確に判定されていることを確認できる．図 8(a) から，同タイミ

言本研究では森林作業道における自己位置推定のために，スキャンマッチング結果の信頼度を考慮した破綻判定のための評価指標を提案し，より正確な破綻判定が可能であることを実機実験により確認した．今後の展望として，姿勢に関する評価項目の追加や，IMU データの補正方法の検討が挙げられる．
謝 辞ングにおいて状態指標 A の値が大きくなったことが確認でき，

本研究の一部は，JSPS 科研費 JP21K14225 からの助鉛直方向の位置ずれとして正確に判定されている．また，時刻成を受けたものである．また森林作業道の実験において多大な

216.2 s から走行終了時までの破綻は，破綻開始時から正しく判定することができている．図 8 からも，状態指標 A，B，C の全ての値が増大していることが確認できる．197.6 s から 198.4 s

るご協力を頂いた福井森林組合の皆様に深く感謝の意を表す．

において，本来は破綻ではない状態が破綻と判定されており，偽破綻が生じていることが確認できる．本検証では，C で参照する車載センサのデータとして IMU の加速度情報から算出した移動量を用いているが，終盤の険しい地形において車体の振動が激しくなったことが確認できており，IMU の値に大きな誤差が乗ることで C 値が大きく変動してしまったことが原因として考えられる．
以上より，提案した評価指標を用いた破綻判定の比較検証の結果，提案手法によって自己位置推定の破綻を正確に判定可能

JRSJ Vol. xx No. xx—4—参考

文

献[ 1 ] 林 野 庁:
“令 和 4 年 度 ス マ ー ト 林 業 構 築 普 及 展 開 事 業報 告 書” https://www.rinya.maff.go.jp/j/keikaku/smartforest/attach/pdf/smart_forestry-4.pdf, (2023), 閲覧日 2024.02.08.
[ 2 ] M. Montemerlo et al. : “FastSLAM: A Factored Solution to the SimultaneousLocalization and Mapping Problem,” Proc. of the AAAI National Conf. on
Artiﬁcial Intelligence, pp. 593–598, 2002.
[ 3 ] 富田 健斗，藤井 浩光，鈴木 理也，新貝 文昭，櫻井 恭介，南 智好: “林内作業車の自動走行のための LiDAR データを用いた人工ランドマー
ク認識とスキャンマッチングによる自己位置推定”, 第 28 回ロボティ
クスシンポジア予稿集, pp. 163–170, 2023.
[ 4 ] E. Takeuchi et al. : “A 3-D Scan Matching using Improved 3-D Normal Distributions Transform for Mobile Robotic Mapping,” Proc. of 2006 IEEE/RSJ Intl.
Conf. on Intelligent Robots and Systems (IROS2006), pp. 3068–3073, 2006.

xx, 202x