2976 日本 機 械 学 会 論 文 集(C編) 74巻748号(2008-12) 論 文No.08-0373
移 動 障 害 物 に対 応 す る セ ンサ ベ ー ス トナ ビゲ ー シ ョン ア ル ゴ リズ ム*
富 田 正 昭*1, 山 本 元 司*2A Navigation
Algorithm
for Avoidance
of Moving
and Stationary
Obstacles
for Mobile
Robot
Masaaki
TOMITA*3
and Motoji YAMAMOTO
*3
Department of Production System Engineering, Kyushu Polytecnic College,
1665-1 Shii, Kokura Minami-ku, Kitakyushu-shi, Fukuoka, 802-0985 Japan
Conventional sensor-based navigation algorithms for mobile robot such as Tangent Bug algor-ithm work only for stationary obstacles. When a mobile robot is operated in more general unknown environment, moving obstacles such as human should also be considered. In this paper, a navigation algorithm which works for moving obstacles and stationary ones with unknown environment is proposed, using a new idea that distinguishes moving obstacles from stationary ones with distance information by sensor of mobile robot. The idea is based on the definition of an inclination angle on the wall surface that is called the wall surface -angle . The wall surface angle of each step is accumulated while following the boundary. When the difference between the total accumulated angle
and the initial one that is the rotation angle of the robot is over 2π radian, currently following
obstacle is recognized as a moving obstacle. According to this idea with Tangent Bug algorithm for stationary obstacles, the navigation algorithm for moving convex shaped obstacles is constructed . In this paper the effectiveness of this algorithm for moving obstacles is shown by simulations . Key Words : Sensor-Based Navigation Algorithm, Moving Obstacles , Rotation Angle
1. 緒 言 移動 ロ ボ ッ トが 障 害 物 を 回避 し,目 的 地 へ た ど り着 くよ う誘 導 制 御 す る こ とを ナ ビ ゲー シ ョン と言 い,ロ ボ ッ トが 動 作 す る 環境 情 報 が 事 前 に与 え られ る 場 合 を モデ ル ベ ー ス ト ・ナ ビゲー シ ョン,ロ ボ ッ トに搭 載 す る セ ンサ ー 情 報 に基 づ い て動 作 環 境 の 状況 を検 知 し,誘 導 制 御 す る場 合 をセ ンサー ベー ス ト・ナ ビゲー シ ョン と 呼 ん で い る.こ れ ら は移 動 ロ ボ ッ トを 実 環境 で使 用 す るた め の 基 本 的 問題 で あ る.移 動 ロ ボ ッ トの動 作 環 境 にお い て,障 害 物 に関 す る情報 を事 前 に 正 確 に知 って い る こ とは 一 般 に 困難 で あ る.従 って,セ ンサ ー ベ ー ス ト ・ナ ビゲ ー シ ョン に おい て は,一 般 に障 害 物 に 関 す る 情報 は事 前 に ほ とん どわ か らな い か,全 くわ か ら な い も の と して取 り扱 っ てい る.こ の障 害 物 と して は 壁 な どの 固 定 障 害 物 と,人 な どの 移 動 障 害物 が あ る. ナ ビゲー シ ョン 問題 に おい て固 定 障 害物 を 回 避 す る 手 法 は多 く研 究 さ れ て い る.こ の うち,環 境 情 報 が既 知 の モ デ ル ベ ー ス の 問 題 は古 典 的 な 経 路 計 画 問 題 と して ロー ドマ ップ法,セ ル 分 割 法,人 工 ポテ ンシ ャル 法 な ど(1)(2)多くの 手 法 が 提 案 さ れ て い る.ま た,セ ン サ ー ベ ー ス トで 固定 障 害 物 を 回 避 す る手 法 と して,ロ ボ ッ ト動 作 モ デ ル とセ ンサ の特 性 を考 慮 したDynamic Window Approach(3)やVector Field Histgram(4)によ る 手 法 な ど,ま た,サ ブ サ ン プ シ ョンア ー キ テ ク チ ャに 代 表 さ れ る行 動 ベ ー ス の 動 作 制 御 手 法(5)が提 案 され て い る. 一 方,移 動 障 害 物 が存 在 す る場 合 に お い て は,モ デ ル ベー スの 方 法 と して,移 動 障 害 物 を 円筒 か つ 既知 の 一 定 速 度 と して そ の 情報 が与 え られ る と し,障 害物 の 速 度 を推 定 し幾 何 学 関 係 か ら衝 突 回避 を 行 う方 法(や 時 間 軸 を 追 加 して3次 元 空 間 内 で幾 何 学 関 係 か ら衝 突 回避 を 行 う方 法(7)が 提 案 さ れ てい る.ま た,モ デ ル * 原 稿 受 付2008年4月28日. *1 正 員,九 州職 業 能 力 開 発 大 学 校 生産 電 子 シ ステ ム 技術 科 (〓802-0985北 九 州市小 倉南 区志井1665-1). *2 正 員,九 州 大 学大 学 院 工学 研 究 院(〓819-0395福 岡市 西 区 元 岡744).
移 動 障 害 物 に 対 応 す るセ ン サベ ー ス トナ ビゲ ー シ ョン ア ル ゴ リズ ム 2977 Obstacleの 考 え方 に基 づ い て 局所 最 適 な経 路 を 得 る 手 法(8)(9)が提 案 され て い る.セ ンサ ベ ー ス トの場 合 で は 移 動 障 害 物 の モ デ ル は不 完 全 だ として,フ ァ ジ ィル ー ル に よ り障 害 物 回避 を行 う方 法(10)などが あ る. これ らの 研 究 に お い てモ デ ル ベ ー ス トで は ロボ ッ ト が 目標 点 に 至 る こ と(収 束 性)を 満 たす こ とは あ た り まえ と して,生 成 され る軌 道 の 良 さ(最 適 性)や 生 成 効 率 が 主 な 論 点 とな っ てい る.し か しな が ら,前 述 の よ う に完 全 な 動 作 環 境 情 報 の事 前 取得 は 困 難 な た め, モ デ ルベ ー ス トの手 法 は 理 論 的意 義 を 除 い て 実用 性 は 低 い.一 方,セ ンサ ー ベ ー ス トにお い て は,環 境 情 報 を 実 時 間取 得 し,セ ンサー の物 理 特 性 を 実 際 に考 慮 し た 動 作制 御 手 法 で あ るた め,実 用 性 は高 い が,環 境 情 報 の事 前 知 識 が不 足 して い る た め,結 果 と して得 られ る 軌道 の 良 さ は不 明 で あ り,デ ッ ドロ ック な ど によ り 目標 点 へ の 収束 性 を 保証 す る こ とは 一般 に困 難 とな る. と ころ が,こ のセ ンサ ー ベ ー ス ト ・ナ ビゲー シ ョ ン 問題 に対 し,環 境 中 の 障害 物 に関 す る事 前 情 報 が な く て も,目 標 点 の 情 報 さ え わ か れば,目 標点 に ロ ボ ッ ト を 誘 導 で き る こ とが 示 さ れ て い る.例 え ばLumelsky らは 具 体 的 にBug1,Bug2の ア ル ゴ リズ ム(11)を提 案 し,登 尾 ら も同 様 な アル ゴ リズ ム(12)を提 案 して い る. これ らの アル ゴ リズ ム は ロ ボ ッ トの 正確 な現 在 位 置情 報 が得 ら れ る と仮 定 し,提 案 ア ル ゴ リズ ム で は 目標 点 ま で の距 離 が減 少 す る性 質(距 離 条 件)を 用 い て 収 束 性 を証 明 して い る.さ らに,ロ ボ ッ トの現 在 位 置 を 正 確 に知 る こ とは 困難 な た め,ロ ボ ッ トの 回転 角(位 相 条 件)に 基 づ い て 目標 点 へ の 収 束 性 を保 証 す るア ル ゴ リズ ム(13)(14)も提案 さ れ て い る. こ れ らの セ ン サー ベ ー ス ト ・ナ ビゲ ー シ ョン ・ア ル ゴ リズ ム は,セ ンサ ー の特 性 を 考 慮 した実 際 的 な 手法 で あ る と 同時 に,収 束 性 を保 証 す るた め理 論 的 に も興 味 深 い.し か しな が ら,従 来 の セ ンサ ー ベ ー ス ト ・ナ ビ ゲ ー シ ョン ・アル ゴ リズ ム で は 障害 物 は固 定 障 害物 の み を対 象 と してお り,移 動 障 害物 が存 在 す る場 合 に 起 こ り う る 問題 に つ い て は 必 ず し も明 確 で な か っ た. 実 際,本 論 文 で示 す よ うに,従 来 の セ ンサ ーベ ー ス ト ・ ナ ビゲ ー シ ョ ンの ア ル ゴ リズ ム で は 固定 障 害 物 で は 問 題 な くて も,移 動 障 害物 に対 して は 目標 点 へ の 誘 導 を 保 証 で き ない 場 合 が 頻 繁 に起 こ りう る.従 っ て,一 般 的 な動 作 環 境 で の移 動 を考 え る と固定 障害 物 と同 時 に 移 動 障 害 物 を含 む 未 知 環 境 で の ナ ビゲ ー シ ョン ・アル ゴ リズ ム を検 討 す る 必 要 が あ る. こ の よ うな 背 景 か ら本研 究 で は,動 的未 知 環 境,す な わ ち未 知 の 移 動 障 害物 と固 定 障 害物 を含 む 環境 に お い て,移 動 ロ ボ ッ トを 目標 点 へ誘 導 す る動 作 アル ゴ リ ズ ム を検 討 す る.移 動 ロボ ッ トがそ の 動 作 中 に搭 載 セ ンサ に よ り検 出 した壁 面 が,固 定 障 害 物 と移 動 障 害 物 を 区別 す る こ とは一 般 的 に は 簡 単 で ない.ま た,そ の 障 害 物 が どの よ うな 速 度 ベ ク トル で 移 動 してい るか を 正 確 に知 る こ とも 困難 で あ る.従 って,こ の こ とを考 慮 して,ど ち らの 障 害物 に対 して も 目標 点 に至 る収束 性 を 保 証 す る必 要 が あ る.提 案 アル ゴ リズ ム は,移 動 ロボ ッ トが移 動 障 害 物 を一 周 した こ とを検 知 し,目 標 点 に 収 束 で きな い 状 況 を 防 ぐ手 法 とな っ て い る. 本 論 文 は以 下 の よ うに構 成 され る.2章 で は 従 来 の セ ンサ ーベ ー ス ト ・ナ ビゲ ー シ ョン アル ゴ リズ ム に お い て移 動 障 害 物 が 存 在す る場 合 に どの よ うな 問 題 が起 こ り うる かを 説 明 す る.3章 で は この問 題 点 を 克服 す る た め の具 体 的 手 法 を 提案 す る.さ らに,4章 で提 案 す る アル ゴ リズ ムを 用 い た シ ミュ レー シ ョン の結 果 を, 従 来 の アル ゴ リズ ム を用 い た場 合 と比 較 して示 し,提 案 ア ル ゴ リズ ム が有 効 で あ る こ と を示 す. 2. 従 来 ア ル ゴ リズ ム と移 動 障 害物 2・1 セ ン サ ベ ー ス ト ・ナ ビ ゲ ー シ ョン 問 題 固 定 障害 物 と移 動 障害 物 を 含 む 未知 環 境 で 目標 点 へ の収 束 性 を保 証 す るナ ビゲ ー シ ョン ・ア ル ゴ リズ ム を検 討 す る.こ の とき,従 来 ア ル ゴ リズム とで き る だ け 同 じ 条 件 で比 較 検 討 す るた め,移 動 ロ ボ ッ トの 性 能,環 境 条 件 等 の 問題 設 定 と して以 下 を仮 定 す る. (i)作 業 環 境 は2次 元 平 面 で あ り,移 動 障 害 物,固 定 障 害 物 が 存 在 しそ の 周 囲 長 と数 は 有 限 で あ る と す る. (ii)固 定 障 害物 の形 状 は 任意 とす る.移 動 障 害 物 の形 状 は 凸 型 の 任 意 形 状 とす る. (iii)ロボ ッ トは搭 載 す る セ ンサ によ り,検 出 範 囲 内 で の 障 害物 壁 面 まで の距 離 と方 向 を計 測 で きる も の とす る.ま た,ロ ボ ッ トを 半 径rの 円 で 近 似 し, 障 害 物 を そ の 分 仮 想 的 に大 き くす る こ とで 点 ロ ボ ッ トとみ な す. (iv)移 動 障 害物 は未 知 の 任意 の 方 向 に移 動 し,移 動 速 度 は ロボ ッ トの そ れ よ り小 さい もの とす る.ま た, 移 動 障 害 物 は 回 転 しな い もの とす る. (v)移 動 ロ ボ ッ トは 現在 の 自己 位 置 を 測 定(推 定)で き,か つ 目標 点 を事 前 に知 っ て い る も の とす る. (vi)目 標点 に 固定 障 害 物 は存 在 せ ず,か つ,移 動 障 害 物 は 一 時 的 に 目標 点 を 占有 す る か も しれ な い が, 時 間が た て ば必 ず 目標 点 は障 害物 に 占有 され ない. 以 上 の 問 題 設 定 は 従 来 の セ ンサ ベ ー ス トナ ビゲ ー シ ョン 問題 にお け る問 題 設定 とほぼ 同 様 で あ る が,い くつ か,こ れ らの 仮 定 の 根 拠 を 述 べ る.
2978 移 動 障 害 物 に対 応 す る セ ン サベ ー ス トナ ビゲ ー シ ョン ア ル ゴ リズ ム (i)では障 害物 に関 す る条 件 は有 限時 間 で 目標点 に到 達 す る た め に 必 須 で あ り,か つ,現 実 的 仮 定 で あ る. (ii)に関 し,固 定 障 害 物 と して しば しば セ ンサ ベ ー ス ト ・ナ ビゲ ー シ ョン問 題 で扱 わ れ る よ うな 複 雑 な 迷 路 形 状 は現 実 的 で は な いが,デ ス ク,椅 子,テ ー ブ ル な ど が狭 い 隙 間 で 配 置 さ れ てい る場 合 な ど,事 実 上 凹 形 状 と な る場 合 は 頻 繁 に起 こ る と考 え られ る た め,任 意 形 状 と し てい る.移 動 障 害 物 と して本 研 究 で は 主 に 人 を想 定 し てい る.こ の場 合,凸 型 形 状 とす る こ とは 妥 当 と考 え られ る. (iv)は 移 動 ロボ ッ トが 移 動 障 害 物 か ら離 れ て,目 標 点 に到 達 す る た め に理 論 上 必 要 とな る条 件 で あ る. (v)に つ い て は,正 確 な 自己 位 置 同 定 は 一 般 に難 し い.こ の点 を考 慮 した 固 定 障害 物 に対 す る セ ンサベ ー ス トナ ビゲ ー シ ョ ン アル ゴ リズ ム が 提 案 さ れ て お り, これ を 応 用 した 手 法 も考 え られ る. な お,本 研 究 で は 移 動 ロ ボ ッ ト搭 載 の セ ン サ で は検 出 した壁 面 が 固定 障 害物 の も のか 移 動 障 害物 の もの か は 直接 判 断 す る こ とは 困 難 で あ り,ま た,移 動 障 害 物 が そ の瞬 間 に どの よ うな速 度 で 移 動 して い る かを 推 定 す る こ と も困 難 で あ る こ とを 前提 と して い る. 2・2 距 離 条 件 に基 づ く従来 ア ル ゴ リズ ム 前 節 で示 した条 件 で,従 来 の セ ンサ ベ ー ス ト・ナ ビゲー シ ョ ン アル ゴ リズ ム を移 動 障 害 物 の存 在 す る環 境 に 適 用 し た場 合 に 生 ず る問題 点 を示 す.こ こ では 距 離 条 件 に基 づ くア ル ゴ リズ ム と してTangent Bug(15)アル ゴ リズ ム (以下T-bugと 略記)を 用 い る場 合 につ い て示 す.T-bug は 移 動 ロ ボ ッ トで 通 常 用 い られ る距 離 セ ン サ の検 出距 離 範 囲 内 で の距 離 青報 を 最 大 限用 い てお り,得 られ る 経 路 は無 駄 の ない もの とな る こ とが 知 られ て い る.な お,こ の アル ゴ リズ ム以 外 で も距 離条 件 に基 づ くア ル ゴ リズ ム に おい て は 同 じ こ とが言 え る.位 相 条 件 に 基 づ く方 法(13)(14)にお い て も,移 動 障 害物 を 考慮 して い な い た め 目標 点 へ の収 束 性 を保 証 で き な い. T-bugは 距 離 セ ンサ ーか らの距 離 情報 によ って ロボ ッ トの 移 動 方 向 を 決定 す る方 法 で あ る.生 成 経 路 は,同 じ環 境 で も距 離 セ ンサ の 検 出 レ ンジ に よ り異 な る.図 1は 検 出 レ ン ジが0の 場 合(左)と,rsの 場 合(右) に お け る,一 つ の 凸 固 定障 害 物 に対 す る 回避 経 路 を 示 す.十 分 大 きい検 出 レン ジの 場 合 は 障 害物 に接 す るよ うな経 路 とな る.有 限 な方 向解 像 度 を 持 つ距 離 セ ンサ を用 い る とき の この ア ル ゴ リズ ムの 概 略 を以 下 に示 す. (図2参 照.) 1)セ ン サ レ ン ジ 内 で セ ンサ 中 心 位 置Px(=ロ ボ ッ ト位 置)と し,距 離 セ ンサ の検 出 レン ジ内 で 障 害 物 を 検 出 した方 向(図 で は破 線 の 矢 印)と 検 出 し て い な い 方 向(図 で は 実 線 の矢 印)に 分 類 す る. これ らの 矢 印 の本 数 に相 当 す る方 向 解 像度 で,セ ン サ レ ン ジ 内 の 障 害 物 ま で の 距 離 を 検 出 で き る と す る.こ の とき,検 出 方 向 と検 出 して い な い 方 向 の 境 界 の点(検 出 レン ジ 円 上)を ゴー ル 方 向 か ら反 時 計 回 りに 番 号 づ け し,こ れ らの 点 を Pi(i=1,…,n)と す る.図 で はP1∼P4.こ れ らの 点 を 境 界 点(boundary point)と 呼 ぶ こ とに す る. 2)PxとPiと の 距 離 をdl,Piと 目標 点 ま で の距 離 を d2と す る.こ の 和(d1+d2)が 最 小 とな るPiを 選 び,そ の方 向 へ移 動 す る.も し目標 点 な ら終 了 . 3)移 動 方 向 が 目標 点 方 向 で あ れ ば"直 進 モ ー ド",そ うで な けれ ば"回避 モ ー ド"と呼ぶ.も し,回 避 モー ドで あ れ ば,障 害 物 壁 面 を 辿 る方 向 を 変 更 せ ず, 距 離 条 件(後 述)を 満 た す ま で は 回避 モ ー ドを継 続 す る. 4)回 避 モー ドにお い て は現 在 位 置 と目標 位 置 の 距離 dxを 常 に調 べ,そ の 回 避 モ ー ドで の 最小 値dxmin を 記 憶 す る. 5)回 避 モー ドにお い て距 離 条 件(ロ ボ ッ トの現 在 位 置 はdx<dxmin)を 満 足 し,か つ セ ンサ レ ン ジ 内 で 目標 の 方 向 に障 害 物 が存 在 しな け れ ば 障害 物 壁 面 か ら"離 脱"し,"直 進 モー ド"とな っ て,目 標 点 方 向へ 移 動 す る.1)へ. 図3は 簡 単 の ため セ ン サー 検 出 レ ンジ を0と し,こ の ア ル ゴ リズ ム を 多少 複 雑 な 固 定 凹 障害 物 に適 用 した 例 で あ る.ロ ボ ッ ト経 路 に おい て 破 線 は 直 進 モ ー ド,実 線 は 回避 モ ー ドを示 して お り,回 避 モ ー ドに お い て 障 害物 外 周 を1周 し たあ と,点Bで 離 脱 しな い の は 距離
Fig. 1 Generated path by T-bug algorithm
移 動 障 害 物 に対 応 す るセ ン サベ ー ス トナ ビゲ ー シ ョ ン アル ゴ リズ ム 2979 条 件 を 満 足 して い な い た め であ る.逆 に この 点 で 離脱 す る と,い つ ま で も 目標点 に到 達 で きな い状 態 に陥 る. この 例 か ら も明 らか な よ うに,セ ンサ ーベ ー ス ト ・ナ ビゲ ー シ ョン に おい ては 距 離条 件 は 目標 点 へ の収 束 を 保 証 す る た めの 重 要 な ポイ ン トであ る. 2・3 従 来 ア ル ゴ リズ ム の 問 題 点 これ まで は 固 定 障 害 物 の場 合 を考 え たが,移 動 障 害 物 の場 合 は どう で あ ろ うか.例 えば 図4の よ う に移 動 障 害物 が ほ ぼ 目 標 方 向 ヘ ー 定 速 度 で 移 動 して い る場 合 を 考 え る. この 図 の 中 で 口数 字 は 移 動障 害 物 の 通過 位 置 と その 時 の 時 刻 を示 し,○ 数 字 は移 動 ロ ボ ッ トの通 過 位 置, 時刻 を示 す.ロ ボ ッ トは移 動 障 害 物 よ り早 く移 動 可 能 な た め,こ の 例 で は移 動 障 害 物 を 通 り越 して 目標 点 に 到 達 で き る こ とがわ か る.こ の と き,ロ ボ ッ トか ら障 害 物 壁 面 が どの よ う に見 え る か を ハ ッチ ン グで 示 す. 移 動 障 害 物 が 移 動 し続 け る場 合 は障 害物 壁 面 は無 限 に 伸 び て い る よ うに 見 え る可 能 性 が あ る こ と,お よ び, 距 離 セ ンサ で 障 害 物 を 検 出す る ロ ボ ッ トに とっ て は, そ れが 固定 障 害 物 な の か移 動 障 害 物 な のか の判 断 は 難 しい こ とに 注 意 さ れ た い. こ れ に 対 して 図5で は移 動 障 害 物 が 目標 方 向 とは 逆 側 に直 線 矢 印 に示 す方 向 でほ ぼ 一定 速 度 で移 動 して い る場 合 で あ る.丸 数 字2で 示 した衝 突 点 以 降,移 動 ロ ボ ッ トは 障 害 物 壁 面 を辿 り続 け るが,距 離 条 件 を 満 足 しな い ため,い つ ま で も離 脱 点 を見 出す こ とは で き な い.こ れ は,移 動 障 害 物 が 単純 な有 限 周 囲 長 の 凸 形 状 障 害 物 で あ って も移 動 ロボ ッ トか ら見 れ ば 障 害 物壁 面 が 無 限 とな るた め で あ る.こ の よ うな 問 題 は 固 定 障 害 物 で は あ り えな い 問題 で あ り,移 動 障 害 物 特 有 の 問 題 で あ る.こ の 問題 は距 離条 件 に基 づ くセ ンサ ー ベー ス トナ ビゲ ー シ ョン アル ゴ リズ ム一 般 に 生 じる 問題 で あ り,T-Bug以 外 のBug1やBug2に お い て も起 こ り 得 る. この よ うに,移 動 障 害 物 を 含 む環 境 にお い て は,指 摘 した 問 題 を克 服 す る ア ル ゴ リズ ム が必 要 とな る.3 章 で は移 動 障害 物 を 含 む 環境 に おい て 適 用可 能 な アル ゴ リズ ム の 提 案 を 行 う. 3. 移 動 障 害 物 を 考 慮 した ア ル ゴ リズ ム 3・1 ロボ ッ ト回転 角 前 章 で移 動 障害 物 を含 む環 境 で は従 来 の固 定 障 害物 を対 象 とす る ア ル ゴ リズ ム で は 問 題 が生 じ る こ とを 示 した.こ の 問題 を克 服 す る た め に,回 避 中 の 障 害物 が移 動 障 害 物 で あ る こ とを,移 動 ロ ボ ッ トが 障 害物 壁 面 を周 回 す る とき の回 転 角 に よ り検 出 す る必 要 が あ る.こ の 回 転 角 に よ って 回 転 しな い 凸型 移 動 障害 物 を検 出す る.そ こで,一 般 に ロ ボ ッ ト に搭 載 可能 な セ ンサ か ら得 られ る情 報 を 用 い,ロ ボ ッ トが壁 面 を辿 る こ とに よ る回 転 角 を 計算 し,障 害物 壁 面 を2重(以 上)に 辿 る こ とを 防止 す る. 回転 角 取 得 の た め の簡 便 な方 法 と して,オ ドメー タ 車輪 を用 い たデ ッ ドレコニ ン グに よ る ロ ボ ッ ト位 置 推 定 お よ び角 度 推 定 が考 え られ る.し か しなが ら,特 に 障害 物 を周 回 す る 場合 のデ ッ ドレコニ ン グに よ る直 接 的 な角 度 推 定 は 累積 誤 差 に よ り,信 頼 性 が 極 め て低 い と考 え られ る.一 方 で,角 度 情 報 だけ な ら何 らかの ラ ン ドマ ー クの検 出 に よ る 目標方 向 のみ の 検 出 は現 実 的 と考 え られ る(16)ため,こ こ で は 目標 方 向 検 出 セ ンサ と有 限 の 角 度解 像 度 を 持 つ 距離 検 出用 セ ンサ に基 づ く デ ッ ドレコ ニ ン グで の 位 置推 定 値 を 用 い て,以 下 の よ うに ロボ ッ トが 障 害 物 を周 回す る とき の 回転 角 を 計 算 す る.
Fig. 3 Generated path by Tangent bug algorithm
Fig. 4 Successfully generated path by T-bug algorithm for a moving obstacle
Fig. 5 Unsuccessfully generated path by T-bug algo-rithm for a moving obstacle
2980 移 動 障 害 物 に対 応 す る セ ンサ ベ ー ス トナ ビゲ ー シ ョン ア ル ゴ リズ ム
A)
C)
B)
まず,目 標 点 を基 準 座標 系 原 点 Σ0と し,こ のX0軸 方 向 は任 意 に定 め る.移 動 ロ ボ ッ トに 固定 さ れ たロ ボ ッ ト座 標 系 ΣRを 設 定 す る.ロ ボ ッ トが 移動 中は 測定 され た 目標 方 向 角 を用 い て ΣRのX軸 方 向 を常 に 目標 方 向 に 一 致 させ る.搭 載 距 離 セ ンサ の 方 向角 は こ の座 標 系 に お い て 定 義 す る.こ の と き,図6に 示 す よ う にデ ッ ド レコ ニ ン グ で得 られ る ロ ボ ッ ト推 定 位 置(xOR,yOR) を 用 い る と,Σ0で 見 た ロ ボ ッ トのXR軸 方 向角 β は(1)
とな る.こ れ を用 い て,基 準 座 標 系 で の 障 害物 壁 面 位 置(X0ob,y0ob)は ロボ ッ トの 座標 系 で見 た 値(XRob,yRob) を 用 い て(2)
に よ り得 られ る.こ こ で(XRob,YRob)は ロ ボ ッ ト搭 載 の 距 離 セ ンサ よ り測 定 可 能 な,方 向角 α と距 離 値dを 用 い て(3)
(4)
と表 せ る の で,式(2)は(5)
(6)
とな る.た だ し,ロ ボ ッ ト座 標 系XR軸 は仮 想 的 に 目 標 方 向 に一 致 さ せ て い る た め,方 向角 α は,実 際 の 測 定 値 か ら 目標 方 向 角 度 β を差 し引 い て お く必 要 が あ る. 今,ロ ボ ッ トが移 動 中 に図6Bの よ うに障 害物 を 検 出 した とす る.こ の と き,同 図 に 示 す よ うに方 向 解 像度 の た め 離 散 的 な 障 害 物 壁 面 上 の 点(検 出 点:detect point, 図 で は 黒 丸)が 検 出 さ れ る.障 害 物 を 検 出 で き な い,セ ン サ ー レ ン ジ 境 界 上 の 点 を 未 検 出 点 と す る.連 続 し た 検 出 点 の 中 で 最 小 距 離 の 点 をPminと し,検 出 点 の す ぐ 外 側 の 未 検 出 点 を 境 界 点(boundary point,図 で は 白 丸) と す る.動 作 ス テ ッ プiで ロ ボ ッ トが 検 出 し た 障 害 物 壁 面 の 角 度 は 次 の よ う に 見 積 も る.図6Cに 示 す よ う にPminの 両 隣 の 検 出 点 をPi,min-,Pi.min+と す る.も し, 隣 の 検 出 点 が な い 場 合 は 境 界 点 で 代 用 す る.こ の と き, こ れ ら の 点 の 座 標 値 を 式(5),(6)に よ り基 準 座 標 系 の 値 と し て 計 算 し,Pi,min-(xi,b1,.Yi.b1),Pi,min+(xi,b2,ybi,b2) と す る と,こ の2点 の を 結 ぶ 線 分 の 傾 き 角 ξiは(7)
とな る.こ の 角 度 を ロ ボ ッ トが 検 出 した 障 害物 壁 面 角 度 とす る.ロ ボ ッ トが移 動 中最 初 に 発 見 した 障害 物 壁 面 角 度 を ξ0とす と,ロ ボ ッ ト衝 突 点 に お け る壁 面 角 度 か らの ロ ボ ッ ト回 転 角 θiは(8)
この とき(9)
を 一 回 転 条 件 と呼 ぶ. 3・2 ア ル ゴ リズ ム 一 回転 条 件 を 用 い て,移 動 障 害物 を含 む 動 作 環 境 で の セ ンサ ー ベ ー ス トナ ビゲー シ ョ ンア ル ゴ リ ズ ム を示 す.ア ル ゴ リズ ム は,こ の 一 回 転 条件 に よ って 移 動 障 害物 に対 す る収 束 性 を 保証 す る.こ こで は,ま ず,方 向 解 像 度 を 持 つ 距 離 セ ン サ か らの 情 報 を 用 い て 壁 面 追従 す る た め の基 本 動 作 を 図7 を 用 い,ア ル ゴ リズ ム 全 体 を 図8を 用 い て 説 明 す る. な お,こ こで は 移 動 ロ ボ ッ トは一 定 速 度 で 移 動 す る と して い る. [壁面 追従 基 本動 作] 移 動 ロ ボ ッ トが 目標 方 向 に移 動 中,障 害物 を検 出 し, 障 害 物 壁 面 に 沿 っ て 壁 面 追 従 す る た め,移 動 方 向 決 定 の 方 法 を 示 す(以 下 の(i)∼(iv)は図7の(i)∼(iv)に 対 応). (i)移 動 ロ ボ ッ トは ゴー ル 方 向 に前 進 す る.ゴ ー ル 方 向 は 目標 方 向 検 出 セ ン サ で検 出 可 能 とす る.ゴ ー ル 方 向 の セ ンサ レ ンジ 円 上 の 点 を 瑞 とす る. (ii)ロ ボ ッ トが 障害 物 を 検 出 して も,Poが 障 害 物 の 境 界 点(図7(ii)で はP1とP2)間 で な け れば(検 出 点 の 方 向 で な い),そ の ま ま ゴー ル 方 向 瑞 へ 直 進 す る.(iii)ゴー ル方 向の 点Poが 境界 点PiとPjの 間 に あ るよ うな 障 害物 壁 面(ゴ ール 方 向の 障 害 物)を 検 出 し Fig. 6 Robot's coordinate frame and robot's absolute
移 動 障 害 物 に対 応 す る セ ンサ ベ ー ス トナ ビ ゲ ー シ ョン ア ル ゴ リズ ム 2981 た ら,距 離di,dj
(10)
(11)
の 小 さ い 方 向 側 の 境 界 点 方 向 へ 移 動 方 向 を 変 更 (図7(iii)で はP1の 方 向)し,以 降,障 害 物 壁 面 か ら離 脱 す る ま で 同 じ障 害 物 壁 面 方 向 を た どる. た だ し,￨PA-PB￨は 点A,Bの 距 離Pw0は ゴー ル (基準 座 標 系 原 点)を あ ら わす. (iv)∼(vi)1ス テ ップ分 前 進 し,再 びセ ンサ ー で 障 害 物 を検 出 し,同 じ方 向 で の境 界 点 を見 出 し,そ の 方 向 に 移動 方 向 を変 更 す る.こ れを 繰 り返 し,距 離条 件 を 満 た す(ゴ ー ル ま で の距 離 が,前 回の 衝 突点 よ りロ ボ ッ ト現 在 位 置 の 方 が小 さ く,か つ 移 動 方 向 に 障害 物 が存 在 しな い)場 合 は 障害 物 壁 面 か ら離 脱 し,ゴ ー ル 方 向へ 移 動 す る. [ナ ビゲ ー シ ョンア ル ゴ リズム] 次 に 図8に よ り,一 回 転 条 件 を 用 い た移 動 障 害 物 を 含 む 動 作 環 境 に対 応 す る ア ル ゴ リズ ム を説 明 す る. (a)移 動 方 向 を 目標 点 方 向 とす る.移 動 方 向 に障 害 物 が な け れ ば そ の ま まそ の 方 向 に1ス テ ッ プ移 動 す る.ロ ボ ッ ト移 動 方 向 に 障害 物 が あ れ ば,図7で 説 明 した よ うに,移 動 ロ ボ ッ トは 障 害物 の外 周 を た ど り,一 回 転 条 件 を満 た さな い で距 離 条 件 を 満 た せ ば 障 害 物 か ら離 脱 す る 。 (b)壁 面 追 従 時 に 障 害物 壁 面 を1回 転 した場 合(一 回 転 条 件 を 満 た した)場 合 は,固 定 凹 障害 物 か 移 動 凸 障 害 物 か を 区別 す る必 要 が あ る.こ れ は以 下 の よ う に,1回 転 中に ゴー ル方 向 に障 害物 壁 面 が 常 に存 在 して い た か ど うか に よ り,区 別 で き る. (b1)固 定 凹 障 害 物 で あ れ ば1回 転 中 に ロボ ッ ト直 近 の ゴー ル 方 向 に 常 に障 害 物 壁 面 が あ る場 合 が存 在 す る.従 っ て この 場 合 は 固 定 凹障 害 物 と判 断 しで 一 回転 条 件 成 立 以 降 は次 に ゴー ル方 向 に障 害 物壁 面 が な く,ゴ ー ル 方 向 に移 動 可能 な状 態 に な る まで 壁 面 を追 従 す る.ゴ ー ル 方 向 に移 動 で きる よ うに な り,か つ そ の点 が 距 離 条 件 を満 足 す れ ば壁 面 か ら離脱 す る.こ れ は これ まで の 固定 凹 障 害 物 に対 す る距 離 条 件 に 基 づ く回避 方 法 と基 本 的 に 同 じな の で,必 ず 離 脱 点 を 見 出 す こ とが で き る. (b2)1回 転 した に もか か わ らず,1回 転 中 に ロボ ッ ト 直 近 の ゴ ー ル 方 向 に 障 害 物 壁 面 が な い 場 合 が 存 在 す れ ば,そ れ は 固 定 障害 物(凸 と凹 と もに)で は な い の で,移 動 方 向 に障 害物 がな くな る ま で壁 面 回避 を続 け て,ゴ ー ル 方 向 に移 動 で きる よ うに な れ ば"距 離 条 件 を 満 た さ な くて も壁 面 か ら離 脱 す る." (c)上 記(b)で1回 転 条 件 を満 た した と き,ゴ ー ル方 向 に障 害 物 が な けれ ば,距 離 条 件 を調 べ,距 離条 件 を 満 た して いれ ば,壁 面 か ら離脱 す る.固 定 凹 障 害 物 か移 動 凸障 害 物 か不 明 だが,こ れ らの条 件 は 目標 点 へ の 収 束 を 保 証 す るの で 問題 は な い. (d)1回 転条 件 を満 た した あ と で,壁 面 追 従 中 に衝 突 点 を 通 過 した場 合 は,解 が 存在 しない 場 合 な の で 解 な し と して 動 作 を 停 止 す る. 4・ シ ミ ュ レー シ ョン 前章 で提 案 した セ ンサ ー ベ ー ス トナ ビゲ ー シ ョン の 手 法 を用 い て,移 動 障 害 物 を含 む環 境 で の セ ンサベ ー ス トナ ビゲ ー シ ョ ンの シ ミ ュレー シ ョンを お こ な った. ここ で は移 動 ロ ボ ッ トは点 と し,中 央 下 部 を ス タ ー ト 地 点,中 央 上 部 を ゴー ル 地 点 と して い る.シ ミ ュ レー シ ョン空 間 は6.0[m]×6.0[m]を 想 定 して 障害 物 の移 動 速 度0.15[m/s],移 動 ロ ボ ッ トの そ れ は0.3[m/s]と し,ス タ ー トか らゴー ル ま で の距 離 を5.0[m]と した. ま た,移 動 ロボ ッ トはセ ンサー レン ジ(0.5[m])内 で 障 害 物壁 面 を検 出 で き る もの とす る.な お,移 動 ロ ボ ッ トの軌 跡 は黒 丸 で 表 し,時 間経 過 を示 す ため に黒 丸 の 軌 跡 に番 号 を付 した.同 様 に移 動 障害 物 の 移 動 前 の位 置 を外 形 枠 で表 し,番 号 を付 した.移 動 ロボ ッ トと移 動 障 害物 の同 一 番 号 が 同一 時 刻 に お け る それ ぞれ の位 置 を あ らわ して い る. は じめ に従 来 のT-Bugア ル ゴ リズ ム で ひ とつ の 移 動 障 害 物 を対 象 と した 場 合 の例 を図9に 示 す.時 間 順 を (i)∼(iv)で表 し てい る.問 題 点 で指 摘 した よ うに 衝 突 以 降 は移 動 障 害 物 の 壁 面 に沿 って 距離 条 件 を満 た す ま で 移 動 す る が,い つ ま で も距 離 条 件 を満 たす こ とが で きな い た め,壁 面 を 辿 り続 け て い る様 子 が わ か る. 一 方 図10は ,一 つ の 凸移 動 障 害 物 と凹 固 定 障 害 物 が 混 在 す る環 境 で提 案 ア ル ゴ リズ ム に よ るシ ミュ レー シ ョ ンを行 った 結 果 で あ る.凸 移 動 障 害物 に対 し て は 移 動 ロボ ッ トは障 害 物 を1回 転 す る間 に 距離 条 件 を満 た さな い た め,そ の ま ま壁 面 の 追 従 を 継続 し1回 転 後 (すな わ ち1回 転 条 件 を 満 た した後)ゴ ー ル方 向が 空 い た と きに 離脱 してい る.そ の後 この よ うな 凹 固定 障 害 物 に対 して は1回 転 条 件 を 満 た した 後 は(固 定 障 害 物 で あ る とい う判 断 を して)距 離 条 件 を 満 た す 所 で 離 脱 し,ゴ ー ル へ 到 達 して い る こ とが わ か る. 5. 結 言 本 研 究 で は,従 来 の 固定 障 害 物 の み を対 象 とす るセ ンサ ベ ー ス トナ ビゲ ー シ ョン アル ゴ リズ ム は,単 純 な 凸型 移 動 障 害物 で も対 処 で き ず,目 標 点 へ の収 束 が 保2982 移 動 障 害 物 に対 応 す る セ ン サ ベ ー ス トナ ビゲ ー シ ョン ア ル ゴ リズ ム
(i)
(ii)
(iii)
(iv)
(v)
(vi)
(a)
(b1
(b2)
(b1)
(c)
(d)
(d)
Fig. 7 Step by step of changing moving direction when detecting obstacles surface
移 動 障 害 物 に対 応 す るセ ンサ ベ ー ス トナ ビゲ ー シ ョ ン アル ゴ リズ ム 2983 (i) (ii) (iii) (iv) 証 で き な い こ と を示 した.こ の 問 題 に 対 処 す る た め, 移 動 ロボ ッ トが 障害 物 壁 面 を1回 転 す るあ い だ に ゴ ー ル 方 向 に障 害 物 壁 面 が常 に存 在 す るか ど うか を 調 べ る こ とで,固 定 の 凹型 障 害 物 か移 動 す る凸 型 障 害 物 か を 区 別 す る方 法 を 提 案 した.ま た,こ の 方 法 に よ り凸 ま た は凹 型 の 固 定 障 害物 と凸 型 の移 動 障 害 物 が 混 在 す る 動 作 環 境 にお い て も収 束 性 を保 証 で きる ナ ビゲ ー シ ョ ン ・アル ゴ リズ ムを 提 案 した.こ の ア ル ゴ リズ ム に お い て は,障 害物 壁 面 追 従 時 に ロ ボ ッ トの 回 転 角 を検 出 す る必 要 が あ る.こ の とき,単 純 なデ ッ ドレコ ニ ン グ に よ る姿 勢 推 定 で は,壁 面 追従 移 動 に従 っ て誤 差 が過 大 とな りア ル ゴ リズ ムが 機 能 しな くな る可 能 性 が高 い た め,目 標 方 向検 出セ ンサ に よ り得 られ る 目標 方 向情 報 に よ りロ ボ ッ トの 回転 角 を 計算 す る手 法 と,実 際 に 用 い られ る有 限 な方 向解 像 度 を持 つ 距 離 セ ン サ の測 定 値 に よ り壁 面追 従 す る アル ゴ リズ ム も示 した.い くつ か の シ ミュ レー シ ョン結 果 に よ り,凸 ま た は 凹型 の固 定 障 害物 と凸型 の移 動 障 害物 が混 在 す る動 作 環 境 にお い て 機 能 す る こ とを 示 し た. 文 献
(1) J. Ohta, et. al., Intelligent Robots —Motion Planning Problem - (in Japanese), (2001), pp.34-67, Corona Publishing Co. Ltd.
(2) H. Choset, et. al., Principles of Robot Motion, (2005), pp.77-187, The MIT Press.
(i) (ii)
(iii) (iv)
(v) (vi)
(3) D. Fox, et. al., The Dynamic Window Approach to Collision Avoidance, IEEE Robotics and Automation Magazine, Vol.4, No.1 (1997), pp.23-33.
(4) I. Ulrich and J. Borenstein, VFH: Local Obstacle Avoidance with Look-Ahead Verification, Proc. of the Int. Conf. on Robotics and Automation, (2000), pp.2505- 2511.
(5) R.A. Brooks, Solving the Find-Path Problem by Good Representation of Free Space, IEEE Trans. Systems, Man, and Cybernetics, Vol. SMC-13, No.3 (1983), pp.190-197. (6) J. Takeno and M. Kakikura, Collision Avoidance Problem
for a Locomotive Robot to Moving Obstacles, Journal of Robotics Society of Japan, Vol.4, No.5 (1986), pp.499- 503.
(7) T. Tsubouchi, et. al., Planning and Navigation of by a Mobile Robot in the Presence of Multiple Moving Obstacles and Their Velocities,
Journal of Robotics Society of Japan, Vol. 12, No.7 (1994), pp.1029-1037.
(8) P. Fiorini and Z. Shiller, Motion Planning in Dynamic Environments using Velocity Obstacles, Int. Journal of Robotics Research, Vol.17, No.7 (1998), pp.760-772. (9) E. Owen and L. Montano, Motion Planning in Dynamic
environments using the velocity space, Proc. of the Int. Fig. 9 A moving obstacle and a fixed obstacle with
conventional algorithm
2984 移 動 障 害 物 に対 応 す る セ ンサ ベ ー ス トナ ビ ゲ ー シ ョ ンア ル ゴ リズ ム
Conf. on Intelligent Robots and Systems, (2005), pp.997- 1002.
(10) S. Ishikawa and S. Asaka, A Method of Piloting an Autonomous Mobile Robot in Dynamically Chainging Environment Including Moving Obstacles, Journal of Robotics Society of Japan, Vol.11, No.6 (1993), pp.856- 867.
(11) V. J. Lumelsky and A. A. Stepanov, Path-Planning Strategies for a Point Mobile Automaton Moving Amidst Unknown Obstacles of Arbitrary Shape, Algorithmica, Vol.2, (1987), pp.403-430.
(12) H. Noborio and J. Hashime, A Feasible Path Planning Algorithm for a Mobile Robot with a Visible Region in an Uncertain Workspace, Journal of Robotics Society of Japan, Vol.10, No.3 (1992), pp.378-384.
(13) V. J. Lumelsky, A Comparative Study on the Path Length Performance of Maze-Searching and Robot
Motion Planning Algorithm, IEEE Trans. on Robotics and Automation, Vol.7, No.1 (1991), pp.57-66.
(14) X. D. Yang, M. Yamamoto and A. Mohri, Path Planning for Mobile Robot in Unceartain Workspace Using Obstacle Topology, Journal of Robotics Society of Japan, Vol.13, No.8 (1995), pp.1130-1137.
(15) I. Kamon, et. al., A New Range-Sensor Based Globally Convergent Navigation Algorithm for Mobile Robot, Proc. of the IEEE Int. Conf, on Robotics and Automation, (1996), pp.429-435.
(16) M. Yamamoto, N. Ushimi and A. Mohri, Navigation Algorithm for Mobile Robots Using Information of a Target Direction, Transactions of the Japan Society of Mechanical Engineers, Series C, Vol.65, No.631 (1999) , pp.1013-1020.
