つくばチャレンジ 3rd ステージにおける

千葉大学知能機械システム研究室の取り組み

白川 珠生， 饗庭 大， 小池 陽介， 角田 祥次郎， 新井 直樹，大川 一也（千葉大学）

Development Report of Intelligent Mechanical Systems Laboratory at Chiba University for 3rd Stage of Tsukuba Challenge

Tamaki Shirakawa, Futoshi Aiba, Yosuke Koike, Syojiro Tsunoda, Naoki Arai, and Kazuya Okawa (Chiba Univ.)

Abstract: This paper is described about the efforts made by Intelligent Mechanical Systems Laboratory at Chiba University to achieve all tasks in the 3rd stage of Tsukuba Challenge. Since basic traveling and traffic signal recognition was done by last year's system, this paper describes a checkpoint passing task and a person search task.

１．緒言 本稿では，本年度からはじまったつくばチャレンジ

3rdステージにおいて，千葉大学知能機械システム研究室が完全課題達成のためにおこなった取り組みについ

て述べる．本研究室は昨年度のつくばチャレンジにお

いて信号機区間を含む完走をしており，本年度の実験

日においても昨年の走行手法を用いて確認走行を達成

した．そこで本年度は，昨年度までの走行手法[1][2]をベースとして，本年度の選択課題を達成するために必

要なシステムを追加することで完全課題達成を目指す

方針とした．完全課題達成のためには，完走すること以

外に，(1)信号機認識(選択課題 A)，(2)チェックポイント通過(選択課題 B)， (3)人探索(選択課題 C)の 3つの選択課題を達成する必要がある．信号機については，昨年度

用いたテンプレートマッチングを使用した手法[3]により認識可能であることを確認した．そこで本稿では主

に(2)と(3)について説明する．

２．システム構成 本研究室が使用するロボットを Fig.1に示す．ロボッ

トは後輪駆動の対向二輪型であり，内界センサとして

エンコーダおよびジャイロセンサ（日本航空電子工業

製 G-108FD1）を搭載しており，ジャイロオドメトリとして用いる．自己位置推定と障害物検出にはマルチ

エコー機能を有する測域センサ（北陽電機社製 UTM-30LX-EW）を搭載したミラーユニット[2]を用いる．これらは本研究室で従来より使用している基本システム

である．

本年度は，上記に加え，チェックポイント経由で使用

するため，地磁気センサ（バイテック社製 TDS01V），および全方位を観測可能な LiDAR（Velodyne社製 VLP-16），を搭載する（第 3章参照）．また，信号機検出と人物探索のために RGB-D センサ（Intel 社製 Real Sense D435）を 2 つ搭載する．なお，雨天時にはカバーをつけることにより対処する． ロボットの制御には GPU（NVIDIA製 GeForce GTX

1050Ti）が搭載されたノート PC（Dell製 ALIENWARE 13 プレミアム，CPU：Intel Core i7-7700H 2.80GHz×8）を使用する．

Fig. 1 Robot configuration

3D LiDAR

Geomagnetic sensor

RGB-D Sensor

Mirror Unit

2．チェックポイント通過 2.1 手法の概要

本年度におけるチェックポイント通過の課題では，

研究学園駅前公園内に設置された 34個のチェックポイント（以下，CPと呼ぶ）のうち，指定された 3~5個のCP を通過する必要がある．また，指定 CP から道なりで 50[m]以内（ただし隣接する CPとの中間点より手前までの範囲）には探索対象が最大で一人設置されてい

る可能性がある．そのため，指定 CP をすべて通過し，かつ，探索対象がいる可能性のある通路をすべて通る

ような経路を生成する必要がある． この課題の達成の方法として，公園全体を一枚の環

境地図で網羅し，それを用いて走行することが考えら

れる．しかし，公園内は道が複雑に入り組んでいるため，

全体の整合性をとることが難しく，破綻する可能性が

ある．また，大規模な環境を三次元地図で表現すると，

メモリの消費が大きくなる，あるいは自己位置推定な

どに要する計算時間が増加するといった問題も発生す

る．そこで本稿では，環境内のすべての走行経路を，CPから隣接する CPまでの経路に分割し，それぞれの経路について個別の環境地図を生成する．そして，CPを経由する順番を決定することで大域的な経路を生成し，

その CPの順番に対応する環境地図を読み込み，走行する． なお，設定された 34個の CP以外にも，必要に応じて独自に CP（以降，独自 CP）を設置した．Fig.2（[4]を元に著者が手を加え作成）に公園内における独自 CPの設置場所を示す．これは，独自 CPを設けないと，44番のように与えられた CP 同士を繋いだだけでは表現できない経路が存在するためである．また，人探索用の

経路を生成する際は，本来であれば，指定 CPに隣接する CPの中間地点まで往復すれば良いが，本年度は簡略化のため，隣接 CP まで往復することとする．ただし，隣接 CP までの距離が長い場合（およそ 100[m]以上）は，CPと CPとの間に独自 CPを設け，探索時間を削減する．なお，スタートとゴール地点に関しても CPとすることで，すべての経路を本章で説明する経路計画に

より生成することができる． 上記のように作成した経路を走行する際，個別に作

成された環境地図の座標系は互いに独立しているため，

本来であれば連続して自律走行を行うことはできない．

そこで，古い地図の終点に達し，新しい地図を読み込む

（以降，地図乗り換えと呼ぶ）際には，乗り換える前の

地図上の座標系におけるロボットの現在位置を，新し

い地図の座標系上に変換する必要がある． 本章では，経由 CPの順番を決定する手法および，地図乗り換え時の座標変換手法について述べる．

2.2 経由チェックポイント順の決定

経由チェックポイント順の決定は以下の手順で行

う． (1) 3~5個の指定 CPの経由順を決定する．この順番は手動で設定する．

(2) (1)で並べられた指定 CP 同士の間の経路を決定する．これにはすべての CPをノードに見立て，最良優先探索の一種である Dijkstra 法[5]を用いる．Dijkstra 法で用いるノード間のコストには基本的に対応する経路の走行距離を使用する．ただし，

ランドマークが少なく，自己位置推定に不安があ

る箇所や，未舗装路などについては走行距離を 5倍することによって走行に不安のある箇所を避け

るようにした．以上の操作により指定チェックポ

イントを通過する経路が生成される． (3) 指定 CP 周辺の人探索用の経路を生成する．指定 CP に隣接する CP の中で，(2)で生成された経路において，指定 CPの直前，あるいは直後に通過されなかった CP までの往復の経路を指定 CP 部に挿入する．

例として，Fig.2において，スタートを 4番，指定 CPを 5 番と 23 番，ゴールを 33 番として経路計画を行った場合について説明する．まず，手順(1)に従い経由 CPの順番を定める．ここでは 4-5-23-33の順とする．次に手順(2)に従い，4-5，5-23，23-33のそれぞれについて最小コスト経路を求める．すると，最短経路はそれぞれ 4-43-5，5-6-10-16-23，23-29-33となる．これらを一つにまとめると，4-43-5-6-10-16-23-29-33となる．最後に手順(3)により，指定 CP である 5 番，23 番について探索経路を計画する．5番に隣接するチェックポイントのうち，(2)で生成された経路上で通過しなかったのは，2番と 9番であるので，探索用の経路は 5-2-5-9-5 となる（プログラムの都合上，数字が小さい方から先に選択するた

め 2番の後に 9番をめぐる経路になる）．23番についても同様に探索用の経路を求めると，23-44-23である．これらの経路を，重複部を除いた上で，(2)で計画した経路に挿入すると，4-43-5-2-5-9-5-6-10-16-23-44-23-29-33となる．

2.3 地図乗り換え時の座標変換

座標系が異なる地図に乗り換える手法は以下の通り

である．本節では乗り換え前の地図を地図 A，乗り換え先の地図を地図 Bと呼ぶ．また，地図 Aの経路を経路A，地図 A上の座標系を座標系 Aのように呼ぶ． (1) 経路 A の終点の半径 1.4[ｍ]以内に到達すれば地図乗り換えの処理を開始する．まず座標系 A において，経路 A の終点に座標系 B の原点を設置し（向

きは経路 Aの終点の向きとする），座標系 Bにおけるロボットの現在位置を求める．これによりロボッ

トの経路 Aの終点までの未到達分が計算される． (2) 手順(1)で求めたロボットの現在位置は，座標系Bの向きが，座標系 A上における経路 Aの終点と同じ向きであるという仮定のもと求めたが，実際は座標

系 B の向きは経路 A の終点の向きとは異なる．そこで地図生成の際に，地磁気センサにより，経路 Bの始点における地磁気による方位を求め，記録する．

そして，地図乗り換えの際にも地磁気により方位を

求め，比較することで，座標系 Bの設置角度を地図生成の際に取得したものに修正する．そして，それ

に伴いロボットの現在位置を変換する．これにより，

座標系 B におけるおおよそのロボットの位置を決定する．

(3) 手順(2)では経路 Aの終点と経路 Bの始点が同じ位置に存在するという仮定のもと現在位置を修正し

たが，地図の開始地点は手動で設定しているため，

グローバル座標でみると，経路 Aの終点と，経路 Bの始点は完全には一致しない．また，地磁気センサ

により得られた方位角にも誤差が含まれる．そこで，

地図生成の際に，各地図の始点において，三次元

LiDARにより，走行用の環境地図とは別に，三次元環境地図を取得しておく（この地図を走行用の地図

と区別するために以降，始点地図と呼ぶ）．そして，

地図乗り換えの際にも同様に，始点地図を作成し，

２枚の地図のマッチングにより，座標系 B におけるロボットの現在位置を求める．なお，始点地図の

詳細およびマッチング手法については次項で説明

する，

2.4 始点地図について

始点地図はロボットに搭載された三次元 LiDARによって取得する．本年度の環境では，CP付近にランドマークが著しく少ない場合もあった．また，始点地図は，

各経路の始点で生成されるため，一点において全周囲

の障害物情報を取得する必要があった．そのため，測定

距離が 100[m]であり，一度のスキャンで全周囲の障害物情報を取得することができる VLP-16を用いた． 始点地図は，三次元 LiDARによって取得された点群情情報を，ロボットを中心として，水平方向に 200[m]×200[m]の範囲を三次元格子地図に変換することで生成する．グリッドサイズは，水平方向に 0.2[m]×0.2[m]で，鉛直方向 zは，2.0[m]≦z<4.2[m]，4.0[m]≦z<8.2 [m]，8.0[m]≦z (z<2.0[m]は無視)の 3通りに分け，格子地図に変換する．鉛直方向の分解能を縦横方向に比べて粗く

とっているのは，計算負荷の軽減と，ロボットのわずか

な傾きによりレーザで取得する障害物の z の値が変化してしまうことへの対策である．また，各グリッド間に

共有部を設けたのもロボットの傾きより障害物の z 情報がずれてしまうことへの対策である．このようにし

て得られた地図の例として，スタート地点において生

成した始点地図を Fig.3に示す． 上述のようにして，地図取りの際に得られた始点地

図(以下，保存地図)と，地図乗り換え時に取得した始点地図(以下，現在地図)とのマッチングを行う．マッチングには最適化手法のひとつである滑降シンプレックス

法[6]を用いる．最適化するパラメータはロボットの位置姿勢(x,y,θ)である．保存地図中において，最も現在地図と一致度の高い点を探索する．そのため，滑降シンプ

レックス法の評価関数には，二つの地図の障害物があ

るグリッドの一致度を得点として用いる．ここで，一致

するグリッドのうち，鉛直方向に高い位置にあるグリ

ッドが一致した際の得点を高くした（一番下のグリッ

ドから 1.0，4.0，8.0とした）．これは背の高い障害物の量が少ないため，背の低い障害物よりも位置合わせの

目印になると考えたからである．また，近い障害物より

も遠くの障害物の一致時の得点を高くした（ロボット

からの距離が 0[m]である障害物の得点を 1.0 倍， 100 [m]の位置にある障害物の得点を 10 倍となるように線型的に定めた）．これは遠くのものを重視して位置合わ

せを行い，ロボットの角度 θ の精度を向上させることを狙ったものである．なお，最適化手法に滑降シンプレ

ックス法を用いたのは，ロボットの走行時の自己位置

推定に滑降シンプレックス法[1]を用いており，移植が容易であると考えたためである．

40

41

43

42 45

4644

47

48

49

50

Fig. 2 Position of added checkpoint For the added CP number, the number after 40 is used,

and it is surrounded by a square.

3．人探索

人探索では，RGB-D カメラの RGB 画像から画像認識により探索対象の検出を行った．同時に，検出によっ

て得た探索対象の画像内での位置から，探索対象の方

向を決定する．同時に RGB-Dカメラの Depthマップから探索対象までの距離の計測を行う．本章では，画像認

識の手法と探索対象の方向及び距離の算出について述

べる．

3.1 RGB-D カメラによる探索

探索対象を検出した場合，アプローチを行うか否かの

判断や実際のアプローチ時に，対象までの距離を知る

必要がある．本研究室では，RGB-D カメラから得たDepth マップに基づき距離を算出する．RGB 画像から得た探索対象の位置とそれに基づく LiDARによる距離計測も可能であるが，得られる情報量は Depth マップが大きく上回る．そのため，LiDAR に比べ，よりロバストに距離算出が行えると考えRGB-Dカメラを用いた．また，つくばチャレンジでは，屋外環境で使用できるカ

メラを選択する必要がある．そこで，Intel Realsense D435を用いた探索を行った．しかし，RGB-D カメラの水平方向の画角は 69.4°と狭い．そこで，人探索におけるロボットの走行距離の節約や検出率の向上を目的に

RGB-D カメラを Fig.4 のように 2 台用いることで広範囲の情報を RGB-Dカメラによって取得する． 2つのカメラで検出できる角度を広げる際に，ロボットの正面に両方のカメラに写る重複領域を設ける必要

がある．仮に重複がなく，正面に探索対象がいた場合，

2 つのカメラにはともに探索対象の一部が写りこむが，このような場合，検出精度が下がってしまう懸念があ

る．よって，正面に重複部を設ける必要がある．また，

検出に必要な左右の画角として 120°を採用した．これ

らの条件を満たすために，各カメラは正面に対し左右

に±25°に傾いて設置されている．また，探索対象の方向や距離の算出は Fig.4の Color cameraの位置を基準にするため，2つの RGB-Dカメラの Color cameraを同一鉛直線上に位置させた．このようにして，計 119.4°の範囲の情報を取得することができる．

3.2 YOLOv3

画像から探索対象を検出するにあたり，深層学習モ

デルの一つである YOLOv3 [6](Tiny version)を用いる．ロボットに搭載された GPU 搭載ノート PC を用いた場合，RGB-Dカメラ 2つによって最大 40fpsでの検出を行えることを確認した．なお，本走行では人探索における

消費電力の節約のため，15fpsにて検出を行った．

3.3 データセット

カメラから得た RGB画像にて人探索を行った． RGB 画像を用いた学習においては，データセットとして，文献[7]を参考に，背景画像と探索対象のマスク画像を組み合わせた合成画像を作成する． 背景画像と探索対象画像は，実験走行日に RGB-Dカメラを用いて収集した．なお得られた探索対象画像に

関して，ジャケットが青色と橙色のものを比較すると，

橙色が青色に比べ約 2倍多かった．そこで，探索対象画像の橙色と青色のジャケット部分の色相を，それぞれ

青色と橙色に，手作業で変更し，枚数の偏りを無くした．

これにより探索対象の学習データを 2倍に増強した こうして背景画像は公園内の画像 125 枚，探索対象のマスク画像は計 367枚を用いている．合成画像は，学習のステップごとにバッチサイズ分の背景画像をラン

ダムに選択し，1 枚あたり探索対象の画像(1~3 枚)，貼り付け位置，大きさなどをランダムに選択，さらに水増

しのための画像処理を行い作成している．これにより，

学習データの偏りを軽減する．また，学習前の大量の画

像データおよびラベルデータの作成を行うことなく学

習した．なお，学習はバッチサイズを 32，75000ステップ行ったため，合計 240万(=32×75000)枚の画像を学習したことになる．

3.4 角度と距離の算出

Fig.5 を用いて探索対象を検出した場合の角度と距離の算出について説明する．まず，RGB-DカメラからRGB画像と Depthマップを取得する(図中①)．次に YOLOによってRGB画像内での探索対象の位置を矩形領域で得る(図中②)．この時の，矩形領域の水平方向の中心線の位置𝑋から探索対象の角度を算出する．ロボットの正面を 0°としたとき，カメラは左右±25°傾いて設置されている．よって画像中央の角度を𝐶%(=±25)[deg]と表す．

Fig. 3 3D map at start point

また，水平方向の画像サイズは 640[pixel]，画角が 69.4°である．以上の条件から，正面からの探索対象の角度

𝑋%[deg]を次式によって得る(図中③)

𝑋% = 𝐶% +69.4640

(𝑋 − 320) (1)

次に Depth マップを用いた距離の算出について説明する．距離算出は，RGB 画像内の探索対象の矩形領域を Depthマップと対応させることで行う．しかし，RGB-D カメラから得た Depth マップは，カメラの仕様によりRGB画像の画角より広い画角を持つため，直接Depthマップにその領域を当てはめることは出来ない． そこで，Depthマップから，RGB画像に対応する領域を切り抜いてリサイズすることで Depthマップと RGB画像の対応づけを行った(図中④)．図中の切り抜きの際の大きさ𝑊45,𝑊47, 𝐻45𝐻47は次の通りである．

𝐻45 =12 ∙42.565.5 ∙ 𝐻 + 14

(2)

𝐻47 =12 ∙42.565.5 ∙ 𝐻

(3)

𝑊45 =12 ∙69.491.2 ∙ 𝑊 + 11 (4)

𝑊47 =12 ∙69.491.2 ∙ 𝑊 + 27 (5)

これより，RGB 画像と対応付けられた Depth マップに矩形領域を当てはめることが出来る．距離の算出は，当

てはめられた矩形領域から 10[m]以上の点を除外し，残った点の距離情報を平均することで算出した(図中⑤)．

4．結言 本稿ではチェックポイント通過課題よび人探索課題

を達成するための手法について述べた．つくばチャレ

ンジ 2018における本走行により，指定されたチェックポイントをすべて通過し，4人の探索対象をすべて発見し，これらの手法に有効性を確認した．

参考文献 [1] 大川一也：“滑降シンプレックス法を用いた屋外移動ロ

ボットの位置推定”，第 15回計測自動制御学会システムインテグレーション部門講演会（SI2014），1F1-1，pp.390- 393（2014）

[2] 中易隆太郎，倉島一輝，大川一也：“モジュール型コンピュータを搭載したつくばチャレンジ用ロボットの開発”，

第 17 回計測自動制御学会システムインテグレーション部門講演会（SI2016），2B2-6，pp.1410- 1413（2016）

[3] 饗庭大，井関統，小池陽介，中易隆太郎，大川一也：“つくばチャレンジ 2017 における千葉大学知能機械システム研究室の取り組み”，つくばチャレンジ 2017参加レポート集，pp.111-114（2017）

[4] つ く ば チ ャ レ ン ジ https://www.tsukubachallenge.jp (2018.12.12アクセス)

[5] Dijkstra, E.W. “A note on two problems in connexion with graphs”. In Numerische Mathematik, 1, pp. 269-271. (1959)

[6] Nelder, John A., and Roger Mead. "A simplex method for function minimization." The computer journal 7.4, pp. 308-313. (1965)

[7] Redmon, Joseph, and Ali Farhadi. "Yolov3: An incremental improvement." arXiv preprint arXiv: 1804.02767 (2018)

[8] 阪東茂，中林達彦，川本駿，椎名誠，阪東華子“つくばチャレンジ2017における土浦プロジェクトの取り組み”，つくばチャレンジ 2017 参加レポート集，pp.3-9（2017）

Camer

Camer

Fig. 4 RGB-D camera and FOV

Fig. 5 Calculation of angle and distance to target