JP2015106254A - 自律移動体、その制御方法及び制御プログラム - Google Patents

Abstract

【課題】自律移動体が傾いた状態でも段差を精度良く検出すること。
【解決手段】自律移動体は、移動体本体の車輪を駆動する駆動手段と、路面に向けて設置され、該路面までの距離を計測する複数の距離計測手段と、距離計測手段により計測された距離と閾値とを比較して、駆動手段を制御する制御手段と、移動体本体の傾斜角を検出する傾斜角検出手段と、傾斜角検出手段により検出された移動体本体の傾斜角に応じて、距離計測手段により計測された距離及び閾値のうち少なくとも一方を補正する補正手段と、を備えている。
【選択図】図２

Description

本発明は、自律的に移動を行う自律移動体、その制御方法及び制御プログラムに関するものである。

距離計測手段により計測された路面までの距離が閾値を超えたときに段差を検出し、停止あるいは回避動作することで、その段差による落下や転倒を防止する自律移動体が知られている（例えば、特許文献１参照）。

特開２０１２−１３０７８１号公報

ところで、例えば、上記自律移動体が凸状部を乗り上げて傾いた場合、水平路面を走行する場合と比較して、距離計測手段により計測される距離が大きくなる。このため、凹状段差が存在しないにもかかわらず、距離計測手段により計測される距離が閾値を超え、凹状段差を誤検出する虞がある。
本発明は、このような問題点を解決するためになされたものであり、自律移動体が傾いた状態でも段差を精度良く検出できる自律移動体、その制御方法及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、移動体本体の車輪を駆動する駆動手段と、路面に向けて設置され、該路面までの距離を計測する複数の距離計測手段と、前記距離計測手段により計測された距離と閾値とを比較して、前記駆動手段を制御する制御手段と、を備える自律移動体であって、前記移動体本体の傾斜角を検出する傾斜角検出手段と、前記傾斜角検出手段により検出された移動体本体の傾斜角に応じて、前記距離計測手段により計測された距離及び閾値のうち少なくとも一方を補正する補正手段と、を備えることを特徴とする自律移動体である。
この一態様において、前記補正手段は、前記傾斜角検出手段により検出された移動体本体の傾斜角が増加するに従がって、前記距離計測手段により計測された距離及び閾値のうち少なくとも一方を増加させる補正を行っても良い。
この一態様において、前記複数の距離計測手段は、少なくとも３つ距離センサを含み、 前記傾斜角検出手段は、前記少なくとも３つ距離センサにより検出された距離に基づいて平面を算出し、該算出した平面の法線を算出し、該算出した法線を前記移動体本体の傾斜角として検出してもよい。
この一態様において、前記傾斜角検出手段は、前記移動体本体の進行方向後方側に設けられた前記距離計測手段の距離に基づいて、前記移動体本体の傾斜角を検出してもよい。
この一態様において、前記補正手段は、前記傾斜角検出手段により検出された傾斜角に応じて、前記移動体本体の進行方向前方側に設けられた前記距離計測手段の距離及び閾値のうち少なくとも一方を補正してもよい。
この一態様において、前記傾斜角検出手段は、前記距離計測手段が計測した距離に基づいて前記路面のうち水平面と判断した水平面領域を計測する前記距離計測手段の距離に基づいて、前記移動体本体の傾斜角を検出してもよい。
上記目的を達成するための本発明の一態様は、路面までの距離を夫々計測するステップと、前記計測された各距離と閾値とを比較して、自律移動体の駆動を制御するステップと、移動体本体の傾斜角を検出するステップと、前記検出された移動体本体の傾斜角に応じて、前記計測された距離及び閾値のうち少なくとも一方を補正するステップと、を含むことを特徴とする自律移動体の制御方法であってもよい。
上記目的を達成するための本発明の一態様は、計測された路面までの各距離と閾値とを比較して、自律移動体の駆動を制御する処理と、移動体本体の傾斜角を検出する処理と、 前記検出された移動体本体の傾斜角に応じて、前記計測された距離及び閾値のうち少なくとも一方を補正する処理と、をコンピュータに実行させることを特徴とする自律移動体の制御プログラムであってもよい。

本発明によれば、自律移動体が傾いた状態でも段差を精度良く検出できる自律移動体、その制御方法及び制御プログラムを提供することができる。

本発明の一実施の形態に係る自律移動体の概略的構成を示す側面図である。 本発明の一実施の形態に係る自律移動体のシステム構成のブロック図である。 自律移動体を上方から見た上面図である。 自律移動体が凸状部を乗り上げ傾いた状態を示す図である。 各距離点の位置を説明するための図である。 進行方向以外の距離センサを選択する場合の図である。 水平面領域を計測する距離センサを選択する場合の図である。 自律移動体が凸状部を乗越える場合の制御方法を説明する図である。 自律移動体が凸状部を乗越える場合の制御方法を説明する図である。 自律移動体が凸状部を乗越える場合の制御方法を説明する図である。 本発明の一実施の形態に係る自律移動体の制御フローを示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の一実施の形態に係る自律移動体の概略的構成を示す側面図である。図２は、本実施の形態に係る自律移動体のシステム構成を示すブロック図である。本実施の形態に係る自律移動体１は、移動体本体２と、移動体本体２に回転可能に設けられた複数の車輪３と、各車輪３を駆動する複数のモータ４（駆動手段の一具体例）と、距離を計測する複数の距離センサ５と、距離センサ５により計測された距離に基づいて各モータ４を制御する制御装置６と、を備えている。なお、上記自律移動体１の構成は一例であり、これに限らず、任意の自律的に移動する移動体であってもよい。

各距離センサ５は、距離計測手段の一具体例であり、進行方向の路面に向けられ、路面までの距離を計測する。各距離センサ５は、例えば、移動体本体２の外周縁に沿って等間隔で６つ設けられているが（図３）、これに限られない。各距離センサ５が進行方向の路面に向けられ、路面までの距離を計測できれば、その数及び位置は任意でよい。各距離センサ５は、計測した距離を制御装置６に出力する。距離センサ５としては、例えば、超音波センサ、ミリ波センサ、赤外線センサなどを用いることができる。

制御装置６は、距離センサ５により計測された距離と予め設定された閾値と比較して、各モータ４を制御する。制御装置６は、例えば、各距離センサ５により計測された路面までの距離が閾値を超えたときに段差を検出する。そして、制御装置６は、検出した段差に基づいて、自律移動体１が停止するように、あるいは段差を回避するように各モータ４を制御する。

制御装置６は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。

ところで、自律移動体が凸状部を乗り上げて傾いた場合、水平路面を走行する場合と比較して、距離センサにより計測される距離が大きくなる（図４）。このため、凹状段差が存在しないにもかかわらず、距離センサにより計測される距離が閾値を超え、凹状段差を誤検出する虞がある。これに対し、本実施の形態に係る自律移動体１は、移動体本体２の傾斜角αを検出し、検出した移動体本体２の傾斜角αに応じて、各距離センサ５により計測された距離及び閾値のうち少なくとも一方を補正する。これにより、自律移動体が凸状部などに乗り上げ傾いた状態でも、上述したような凹状段差を誤検出することなく、精度良く検出できる。

制御装置６は、移動体本体２の傾斜角αを検出する傾斜角検出部６１と、各距離センサ５により計測された距離及び閾値のうち少なくとも一方を補正する補正部６２と、補正部６２により補正された距離及び閾値に基づいて各モータ４を制御する制御部６３と、を有している。

傾斜角検出部６１は、傾斜角検出手段の一具体例であり、例えば、６つの距離センサ５のうち３つ距離センサ５により計測された距離に基づいて移動体本体２の傾斜角αを検出する。より具体的には、傾斜角検出部６１は、３つ距離センサ５により計測された距離に基づいて平面式（ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０）を算出する。

例えば、図３に示す如く、６つの距離センサ５は、距離点Ａ〜Ｆまでの距離を夫々計測する。ここで、傾斜角検出部６１が、距離点Ｃ、Ｄ、Ｅの平面式を算出する場合を想定する。傾斜角検出部６１は、例えば、距離点Ｅの座標（Ｘｅ、Ｙｅ、Ｚｅ）を下記式に基づいて算出する。
Ｘｅ＝（ｄｃｏｓφ＋Ｒ）ｃｏｓθ
Ｙｅ＝（ｄｃｏｓφ＋Ｒ）ｓｉｎθ
Ｚｅ＝ｓｉｎφ

なお、上記式において、移動体本体２の中心を原点したときの、原点から各距離センサ５までの距離をＲとし、各距離センサ５と原点と結ぶ線がＸ軸と成す角度をθとし、各距離センサ５が計測する距離値をｄとし、俯角をφとする（図５）。

傾斜角検出部６１は、距離点Ｅの座標と同様に距離点Ｃ及びＤの座標を算出し、該算出した距離点Ｃ、Ｄ、Ｅの座標を上記平面式（ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０）に代入する。そして、傾斜角検出部６１は係数ａ〜ｄを算出することで、距離点Ｃ、Ｄ、Ｅを含む平面を算出する。傾斜角検出部６１は、さらに算出した平面の法線を算出する。傾斜角検出部６１は、算出した法線を移動体本体２の傾斜角αとして検出する。傾斜角検出部６１は、検出した移動体本体２の傾斜角αを補正部６２に出力する。

補正部６２は、傾斜角検出部６１により検出された移動体本体２の傾斜角αに応じて、距離センサ５により計測された距離及び閾値のうち少なくとも一方を補正する。ここで、上述の如く、自律移動体が凸状部に乗り上げ、上記移動体本体の傾きが生じた場合、水平路面を走行する場合と比較して、距離センサにより計測される距離が大きくなる（図４）。さらに、上記移動体本体の傾斜角が増加するに従がって、距離センサにより計測される距離も増加する。

従がって、補正部６２は、例えば、傾斜角検出部６１により検出された移動体本体２の傾斜角αが増加するに従がって、各距離センサ５により計測された距離を減少させる補正を行う。あるいは、補正部６２は、例えば、傾斜角検出部６１により検出された移動体本体２の傾斜角αが増加するに従がって、閾値を増加させる補正を行う。これにより、上記移動体本体２の傾斜角αによって生じた各距離センサ５の距離の増加分を補正することができる。

なお、補正部６２は、傾斜角検出部６１の傾斜角検出に使用した各距離センサ５以外の各距離センサ５の距離あるいは閾値に対して、上記補正を行う。例えば、図４に示す如く、傾斜角検出部６１は、進行方向後方側の距離センサ５により計測された距離に基づいて移動体本体２の傾斜角αを検出する。そして、補正部６２は、傾斜角検出部６１により検出された移動体本体２の傾斜角αに応じて、進行方向前方側の各距離センサ５により計測された距離及び閾値のうち少なくとも一方を補正する。

制御部６３は、制御手段の一具体例であり、補正部６２により補正された各距離センサ５の距離と閾値とを比較して、あるいは、各距離センサ５により計測された距離と補正部６２により補正された閾値とを比較して、各モータ４を制御する。制御部６３は、例えば、補正部６２により補正された各距離センサ５の距離が閾値を超えているとき段差を検出する。

ここで、上述した傾斜角検出部６１における平面を算出するための３つ距離センサ５の選択方法について詳細に説明する。
（１）進行方向以外の距離センサを選択する場合
例えば、図６に示す如く、自律移動体１の進行方向前方に設けられた距離センサ５ａ、５ｂは、これから進行する先に存在する段差の検出に必要となる。従がって、傾斜角検出部６１は、進行方向前方に設けられた距離センサ５ａ、５ｂ以外の距離センサ５ｃ〜５ｆのうちの３つの距離センサ５ｃ〜５ｆの距離を用いて自律移動体１の傾斜角αを検出する。

傾斜角検出部６１は、例えば、回転センサにより検出された各車輪の回転情報に基づいて、自律移動体１の進行方向の角度θ１を算出する。そして、傾斜角検出部６１は、算出した自律移動体１の進行方向の角度θ１と、予め設定されている各距離センサ５ａ〜５ｆの取付角度θＡ〜θＦと、を比較する。

ここで、自律移動体１を上方から見たとき、自律移動体１の中心を原点とし、各距離センサ５ａ、５ｄを通る線をＹ軸とし、原点を通りＹ軸に直交する線をＸ軸とする。進行方向のベクトル線とＸ軸とが成す角度を上記進行方向の角度θ１とし、各距離センサ５ａ〜５ｆの取付位置と原点を結ぶ線とＸ軸とが成す角度を各距離センサ５ａ〜５ｆの取付角度θＡ〜θＦとする。傾斜角検出部６１は、算出した自律移動体１の進行方向の角度θ１と、各距離センサ５ａ〜５ｆの取付角度θＡ〜θＦとの差が閾値内となる各距離センサ５ａ〜５ｆを、進行方向に設けられた距離センサ５ａ〜５ｆとする。傾斜角検出部６１は、進行方向に設けられた各距離センサ５ａ〜５ｆ以外の各距離センサ５ａ〜５ｆのうちの任意の３つの各距離センサ５ａ〜５ｆの距離を選択し、選択した距離に基づいて移動体本体２の傾斜角αを検出する。

（２）水平面領域を計測する距離センサを選択する場合
傾斜角検出部６１は、水平路面であると既に判定された水平面領域を計測する距離センサ５ａ〜５ｆの距離に基づいて、移動体本体２の傾斜角αを検出してもよい。傾斜角検出部６１は、各距離センサ５ａ〜５ｆが既に計測した距離に基づいて路面のうち水平面と判断した水平面領域を計測する各距離センサ５ａ〜５ｆの距離に基づいて、移動体本体２の傾斜角αを検出する。

例えば、図７に示す如く、自律移動体１は、既に走行した路面（自律移動体１の進行方向後方側の路面（斜線部）、以下、既知路面と称す）を識別している。したがって、傾斜角検出部６１は、既知路面が水平面領域である場合、その水平路面を計測する各距離センサ５ｃ、５ｄ、５ｅの距離に基づいて、移動体本体２の傾斜角αを検出する。これにより、水平面に存在する各距離点Ｃ、Ｄ、Ｅを用いて高精度に移動体本体２の傾斜角αを検出できる。

（３）自律移動体が凸状部を乗越える場合
例えば、図８に示す如く、制御装置６は、進行方向前方向側の各距離センサ５により検出された凸状部の距離に基づいて凸状部の乗越えを可能と判断し、凸状部を乗越えるように各モータ４を制御する。続いて、図９に示す如く、自律移動体１の前輪３が凸状部に乗ると、傾斜角検出部６１は、進行方向後方側の各距離センサ５により検出された距離に基づいて移動体本体２の傾斜角αを検出する。そして、補正部６２は傾斜角検出部６１により検出された傾斜角αに応じて、進行方向前方側の各距離センサ５の距離又は閾値を補正する。

さらに、図１０に示す如く、自律移動体１の前後車輪３が凸状部を乗越えると、傾斜角検出部６１は、所定時間（進行方向後方側の各距離センサ５が凸状部を計測している時間）だけ移動体本体２の傾斜角αの検出を停止する。したがって、補正部６２は、この所定時間、進行方向後方側の各距離センサ５により計測された距離及び閾値の補正を停止する。

傾斜角検出部６１は、所定時間経過後、進行方向後方側の各距離センサ５の距離に基づいて移動体本体２の傾斜角αを検出する。そして、補正部６２は、傾斜角検出部６１により検出された移動体本体２の傾斜角αに基づいて、進行方向前方側の各距離センサ５の距離及び閾値のうち少なくとも一方を補正する。

図１１は、本実施の形態に係る自律移動体の制御フローを示すフローチャートである。
傾斜角検出部６１は、自律移動体の進行方向に基づいて、例えば、６つの各距離センサ５のうち進行方向後方側の３つ各距離センサ５を選択する（ステップＳ１０１）。

傾斜角検出部６１は、選択した進行方向後方側の３つ各距離センサ５により計測された距離に基づいて平面を算出し、算出した平面の法線を算出する。そして、傾斜角検出部６１は、算出した法線を移動体本体２の傾斜角αとして検出する（ステップＳ１０２）。
補正部６２は、傾斜角検出部６１により検出された移動体本体２の傾斜角αに応じて、例えば、進行方向前方側の各距離センサ５により計測された距離を補正する（ステップＳ１０３）。

制御部６３は、補正部６２により補正された進行方向前方側の各距離センサ５の距離と閾値とを比較して、段差が存在するか否かを判定する（ステップ１０４）。制御部６３は、段差が存在すると判定したとき（ステップＳ１０４のＹＥＳ）、例えば、自律移動体１を停止させるように、各モータ４を制御する（ステップＳ１０５）。一方、制御部６３は、段差が存在しないと判定したとき（ステップ１０４のＮＯ）、上記（ステップＳ１０１）に戻る。

以上、本実施の形態に係る自律移動体１において、移動体本体２の傾斜角αを検出し、検出した移動体本体２の傾斜角αに応じて各距離センサ５により計測された距離及び閾値のうち少なくとも一方を補正する。これにより、自律移動体１が凸状部などに乗り上げ傾いた状態でも凹状段差を誤検出することなく精度良く検出できる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態において、傾斜角検出部６１は、各距離センサ５により計測された距離に基づいて移動体本体２の傾斜角αを検出しているが、これに限られない。傾斜角検出部６１は、例えば、移動体本体２に設けられたジャイロセンサにより計測されたセンサ値を基づいて移動体本体２の傾斜角αを検出してもよい。

本発明は、例えば、図１１に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現するものである。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 自律移動体、２ 移動体本体、３ 車輪、４ モータ、５ 距離センサ、６ 制御装置、６１ 傾斜角検出部、６２ 補正部、６３ 制御部

Claims (8)

1. 移動体本体の車輪を駆動する駆動手段と、
   路面に向けて設置され、該路面までの距離を計測する複数の距離計測手段と、
   前記距離計測手段により計測された距離と閾値とを比較して、前記駆動手段を制御する制御手段と、を備える自律移動体であって、
   前記移動体本体の傾斜角を検出する傾斜角検出手段と、
   前記傾斜角検出手段により検出された移動体本体の傾斜角に応じて、前記距離計測手段により計測された距離及び閾値のうち少なくとも一方を補正する補正手段と、を備えることを特徴とする自律移動体。
2. 請求項１記載の自律移動体であって、
   前記補正手段は、前記傾斜角検出手段により検出された移動体本体の傾斜角が増加するに従がって、前記距離計測手段により計測された距離及び閾値のうち少なくとも一方を増加させる補正を行う、ことを特徴とする自律移動体。
3. 請求項１又は２記載の自律移動体であって、
   前記複数の距離計測手段は、少なくとも３つ距離センサを含み、
   前記傾斜角検出手段は、前記少なくとも３つ距離センサにより検出された距離に基づいて平面を算出し、該算出した平面の法線を算出し、該算出した法線を前記移動体本体の傾斜角として検出する、ことを特徴とする自律移動体。
4. 請求項１乃至３のうちいずれか１項記載の自律移動体であって、
   前記傾斜角検出手段は、前記移動体本体の進行方向後方側に設けられた前記距離計測手段の距離に基づいて、前記移動体本体の傾斜角を検出する、ことを特徴とする自律移動体。
5. 請求項４記載の自律移動体であって、
   前記補正手段は、前記傾斜角検出手段により検出された傾斜角に応じて、前記移動体本体の進行方向前方側に設けられた前記距離計測手段の距離及び閾値のうち少なくとも一方を補正する、ことを特徴とする自律移動体。
6. 請求項１乃至５のうちいずれか１項記載の自律移動体であって、
   前記傾斜角検出手段は、前記距離計測手段が計測した距離に基づいて前記路面のうち水平面と判断した水平面領域を計測する前記距離計測手段の距離に基づいて、前記移動体本体の傾斜角を検出する、ことを特徴とする自律移動体。
7. 路面までの距離を夫々計測するステップと、
   前記計測された各距離と閾値とを比較して、自律移動体の駆動を制御するステップと、
   移動体本体の傾斜角を検出するステップと、
   前記検出された移動体本体の傾斜角に応じて、前記計測された距離及び閾値のうち少なくとも一方を補正するステップと、を含むことを特徴とする自律移動体の制御方法。
8. 計測された路面までの各距離と閾値とを比較して、自律移動体の駆動を制御する処理と、
   移動体本体の傾斜角を検出する処理と、
   前記検出された移動体本体の傾斜角に応じて、前記計測された距離及び閾値のうち少なくとも一方を補正する処理と、をコンピュータに実行させることを特徴とする自律移動体の制御プログラム。

Images