JP6605638B2 - 制御処理装置 - Google Patents

Description

本発明は、２つの処理部を有し、一方の処理部から他方の処理部に基準の時刻を表すカウント値を送信しながら、当該２つの処理部で同期した制御処理を実行し得るように構成された制御処理装置に関する。

例えば移動ロボットにおいて、該移動ロボットに搭載されたステレオカメラ等の外界センサにより、移動ロボットの周囲に存在する物体の配置状態等の外界状態を認識しながら、移動ロボットの動作制御を行うように構成された制御処理装置を搭載したものが従来より知られている（例えば特許文献１を参照）。

この種の制御処理装置は、そのレイアウト上の制約等により、単一の処理部として構成することが困難となる場合が多々あり、そのような場合には、制御処理装置を複数の処理部で構成すると共に、これらの処理部を各別の箇所に配置し、これらの処理部の間で通信ケーブル等を介して通信を行うようにすることが行われる。

例えば、特許文献１に見られる如き移動ロボットでは、外界状態を計測することに関する制御処理を実行する処理部と、移動ロボットの各関節を駆動したり、移動ロボットの実際の動作状態の監視すること等に関する制御処理を実行する処理部とを各別の箇所に配置し、これらの処理部の間で通信を行うように制御処理装置を構成する場合が多々ある。

そして、このように複数の処理部を備える制御処理装置では、該複数の処理部のそれぞれの制御処理（データの生成又は取得等）の同期をとるために、一つの処理部から、基準の時刻を示すカウント値を所定の基準周期で逐次更新しながら他の各処理部に送信し、それぞれの処理部で、該カウント値の更新に同期して制御処理を実行することが一般に行われる。

なお、例えば、特許文献２には、マイコロコンピュータの関する処理技術が開示され、特許文献３には、グローバルポジショニングシステムの受信機に関する処理技術が開示されている。

特開２００９−２８５７５２号公報 特許第４０８２２１１号公報 特許第３３８５３２１号公報

ところで、上記のように基準の時刻を示すカウント値を送信する処理部（以降、第１処理部ということがある）と、該カウント値を受信する処理部（以降、第２処理部ということがある）とを備える制御処理装置においては、該第１処理部と第２処理部との間の通信ケーブルや通信規格等に起因する制約によって、送信するカウント値の更新周期をあまり短くできない場合が多々ある。

一方、上記カウント値を受信する第２処理部では、その制御処理により得られるデータの精度等を高める上で、より高い時間分解能で制御処理を実行し得ることが好ましい場合が多々ある。

例えば、移動ロボットにおいて、、移動ロボットの各関節を駆動したり、移動ロボットの実際の動作状態の監視すること等に関する制御処理を実行する主たる処理部を上記第１処理部とすると共に、ステレオカメラ等の外界センサにより外界状態を計測する制御処理を実行する処理部を上記第２処理部として制御処理装置を構成した場合、外界状態の計測データの精度を高める上では、多くの場合、第２処理部による外界状態の計測のための制御処理の時間分解能を高めることが好ましい。

しかるに、従来は、第２処理部の制御処理は、第１処理部から送信されるカウント値の更新周期により規定される時間分解能で実行されていたため、第１処理部から送信するカウント値の更新周期をあまり短くできない場合には、第２処理部の制御処理の時間分解能が低下する。ひいては、第２処理部の制御処理で得られるデータ（例えば外界状態の計測結果を示すデータ）の精度を高めることが困難となりやすい。

本発明はかかる背景に鑑みてなされたものであり、第１処理部から第２処理部に所定の基準周期で逐次更新されるように送信されるカウント値の更新周期よりも高い時間分解能で第２処理部の処理を実行することを、該第１処理部及び第２処理部のそれぞれの処理の時間的な整合性を確保しつつ実現することができる制御処理装置を提供することを目的とする。

本発明の制御処理装置は、上記の目的を達成するために、所定の基準周期毎の時刻を表す第１カウント値を該基準周期で逐次更新するように出力する第１カウンタを有し、該第１カウンタから出力される各第１カウント値が対応付けられた第１データを生成又は取得する処理と、該第１カウント値を前記基準周期で逐次送信する処理とを実行する機能を有する第１処理部と、
前記第１処理部から送信される前記第１カウント値を受信可能に構成されていると共に、前記基準周期よりも短い所定の周期毎の時刻を表す第２カウント値を該所定の周期で逐次更新するように出力する第２カウンタを有し、該第２カウンタから出力される第２カウント値を、前記第１カウント値の受信に応じて定期的に初期化する処理と、前記第１データに関連する第２データを前記基準周期により規定される時間分解能よりも高い時間分解能で生成又は取得する処理とを実行する機能を有する第２処理部とを備えており、
前記第２処理部は、前記第２データを生成又は取得するとき、前記第１処理部から受信した第１カウント値により示される時刻と、前記第２カウンタから出力された第２カウント値により示される時刻とを合成した時刻を前記第２データに対応付けるように構成されていることを基本構成とする。

上記基本構成によれば、前記第２処理部は、前記基準周期よりも短い所定の周期毎の時刻を表す第２カウント値を逐次出力する第２カウンタを備えており、該第２カウンタから出力される第２カウント値は、前記第１カウンタの受信に応じて定期的に初期化される。そして、第２処理部が生成又は取得する第２データには、第１処理部から受信した第１カウント値により示される時刻と、前記第２カウンタから出力された第２カウント値により示される時刻とを合成した時刻が対応付られる。

これにより、第２処理部は、第１処理部による第１データの生成又は取得の処理との時間的な整合性を確保しつつ、前記基準周期により規定される時間分解能よりも高い時間分解能で、第２データを生成又は取得する処理を実行することが可能となる。

よって、前記基本構成を有する本発明によれば、第１処理部から第２処理部に所定の基準周期で逐次更新さ れるように送信されるカウント値の更新周期よりも高い時間分解能で第２処理部の処理を実行することを、該第１処理部及び第２処理部のそれぞれの処理の時間的な整合性を確保しつつ実現することが可能となる。

上記基本構成を有する本発明では、より具体的には、前記第２処理部は、前記第２データを生成又は取得した時刻以前に前記第１処理部から受信した最新の第１カウント値により示される時刻に、該最新の第１カウント値により示される時刻から前記第２データを生成又は取得した時刻までに前記第２カウンタから出力された第２カウント値の変化量により示される時間幅を加えた時刻を前記合成した時刻として前記第２データに対応付けるように構成されているという態様を採用し得る（第２発明）。

これにより、前記基準周期よりも高い時間分解能での時刻を、第２データに対応付けることを適正に行うことができる。

上記基本構成を有する本発明又は第２発明では、前記第２処理部は、前記第１カウント値の受信毎に、前記第２カウンタから出力される前記第２カウント値を初期化するように構成されていることが好ましい（第３発明）。

これによれば、第１カウント値と、第２カウント値との同期性のずれを極力抑制することができる。

上記基本構成を有する本発明では、前記第１処理部は、前記各第１カウント値が対応付けられた前記第１データを前記第２処理部に送信する処理を前記基準周期で逐次実行するように構成されており、前記第２処理部は、生成又は取得した前記第２データに対応付けられた時刻での前記第１データの値を、前記第１処理部から受信した前記第１データの時系列から補間処理により推定する処理を実行する機能をさらに有するように構成されている。
また、前記第１処理部は、移動体の所定の部位の動作状態に関する検出データを前記第１データとして取得して、該第１データを前記第２処理部に送信するように構成されており、前記第２処理部は、前記移動体の周囲に存在する物体を検出し得るように該移動体の前記所定の部位に搭載された外界センサから出力される前記物体の検出データを前記第２データとして取得し、さらに、該第２データに対応付けられた時刻での前記第１データの値として前記補間処理により推定した値と、該第２データの値とを用いて、前記外界センサにより検出された物体の前記移動体に対する相対的な配置状態を示す第３データを生成するように構成されている。
さらに、前記第２処理部は、前記第２データを取得するとき、前記外界センサによる検出の終了に応じて該外界センサから入力される終了トリガ信号に応じて、該終了トリガ信号の入力時刻を当該取得した第２データに対応する時刻として決定し、該入力時刻を、前記第１処理部から受信した第１カウント値により示される時刻と、前記第２カウンタから出力された第２カウント値により示される時刻とを合成した時刻として求めて該第２データに対応付けるように構成されている。、
従って、本発明では、前記第１処理部は、前記各第１カウント値が対応付けられた前記第１データとして、移動体の所定の部位の動作状態に関する検出データを取得して、該第１データを前記第２処理部に送信する処理を前記基準周期で逐次実行するように構成されており、
前記第２処理部は、
前記移動体の周囲に存在する物体を検出し得るように該移動体の前記所定の部位に搭載された外界センサから出力される前記物体の検出データを前記第２データとして取得するように構成されていると共に、
前記第２データを取得するとき、前記外界センサによる検出の終了に応じて該外界センサから入力される終了トリガ信号に応じて、該終了トリガ信号の入力時刻を当該取得した第２データに対応する時刻として決定し、該入力時刻を、前記第１処理部から受信した第１カウント値により示される時刻と、前記第２カウンタから出力された第２カウント値により示される時刻とを合成した時刻として求めて該第２データに対応付ける機能と、
該第２データに対応付けられた時刻での前記第１データの値を、前記第１処理部から受信した前記第１データの時系列から補間処理により推定する処理を実行する機能と、
該第２データに対応付けられた時刻での前記第１データの値として前記補間処理により推定した値と該第２データの値とを用いて、前記外界センサにより検出された物体の前記移動体に対する相対的な配置状態を示す第３データを生成する機能とををさらに有するように構成されていることを特徴とする（第１発明）。

これによれば、前記第２処理部が前記第２データを取得するときに、第２データに対応付けられた時刻での前記第１データの値を前記補間処理により適切に推定することができる。このため、第２処理部では、前記第２データを、その取得の時点での第１データの値に対応付けることが可能となる。

また、前記物体の検出データを前記基準周期よりも高い時間分解能で短い周期で取得できると共に、該物体の検出データを取得した時刻での前記移動体の所定の部位の動作状態を前記補間処理により適切に推定できるので、前記外界センサにより検出された物体の移動体に対する相対的な配置状態を示すデータを精度よく生成することが可能となる。

上記基本構成を有する本発明では、前記第２処理部は、前記第１処理部から送信される前記第１カウント値を正常に受信できない異常状態が発生したとき、該異常状態の発生後に生成又は取得する前記第２データに対応付ける時刻を、前記第２カウンタから出力される第２カウント値に基づいて推定し、当該推定した時刻を前記第２データに対応付けるように構成されていることが好ましい（第４発明）。

これによれば、前記第２処理部は、前記異常状態が発生した場合でも、生成又は取得する第２データに、前記基準時刻よりも高い時間分解能での時刻を対応付けることが可能となる。

上記第４発明では、より具体的には、前記第２処理部は、前記異常状態の発生前に最後に受信した前記第１カウント値により示される時刻に、該時刻から、前記異常状態の発生後に前記第２データを生成又は取得した時刻までに前記第２カウンタから出力された前記第２カウント値の変化量により示される時間幅を加えた時刻を該第２データに対応付ける時刻として決定するように構成されているという態様を採用し得る（第５発明）。

これによれば、前記異常状態が発生した場合に、前記第２データに対応付ける時刻を適切に決定することができる。

本発明の制御処理装置の実施形態を適用するロボット（移動体）を示す図。 実施形態の制御処理装置の構成を示すブロック図。

本発明の一実施形態を図１及び図２を参照して以下に説明する。本実施形態の制御処理装置Ａは、例えば、図１に示す如きロボット１に搭載されている。ロボット１は、移動体の一例である。この例でのロボット１は、人型の脚式移動ロボットであり、上体に相当する基体２と、基体２から延設された複数（本実施形態では４個）の可動リンクとしての左右一対の（２つの）脚リンク３，３及び左右一対の（２つの）腕リンク４，４と、頭部５とを備える。

基体２は、本実施形態では、基体２の下部を構成する下側基体６と、基体２の上部を構成する上側基体７と、該下側基体６及び上側基体７を連結する関節機構８とから構成される。そして、頭部５は、基体２の上端部（上側基体７の上端部）に首関節機構２７を介して取り付けられている。

各脚リンク３は、基体２の下部（下側基体６）から延設されており、大腿部１１、下腿部１２、足部１３にそれぞれ相当する要素リンクを、基体２側から順番に、股関節機構１４、膝関節機構１５、足首関節機構１６を介して連結して構成されている。

各腕リンク４は、基体２の上部（上側基体７）から延設されており、上腕部２１、前腕部２２、ハンド部２３にそれぞれ相当する要素リンクを、基体２側から順番に、肩関節機構２４、肘関節機構２５、手首関節機構２６を介して連結して構成されている。

詳細な図示は省略するが、上記の各関節機構８，１４，１５，１６，２４，２５，２６のそれぞれは、例えば１軸周りの回転自由度を有する一つ以上の関節（回転型の関節）により構成され、該関節のそれぞれを電動モータ等のアクチュエータにより駆動することで、ロボット１の運動が行われる。なお、各関節機構８，１４，１５，１６，２４，２５，２６は、回転型の関節に限らず、直動型の関節を含んでいてもよい。

かかる構成のロボット１には、本実施形態の制御処理装置Ａの構成要素としての第１処理部３０及び第２処理部４０と、ロボット１の周囲の外界状態を計測するための外界センサ５０と、該外界センサ５０に関する制御処理を行うセンサ制御部５１とが任意の適所に搭載されている。

図示例のロボット１では、第１処理部３０は例えば下側基体６に搭載され、第２処理部４０は例えば上側基体７に搭載され、外界センサ５０及びセンサ制御部５１は例えば頭部５に搭載されている。なお、頭部５は、本発明における移動体の所定の部位に相当する。

外界センサ５０は、本実施形態では、より詳しくは、ロボット１の周囲に存在する物体（地面、床面、設置物、構造物等）を検知するためのセンサであり、例えば、ステレオカメラ、レーザ・レンジ・ファインダ（ＬＲＦ）等により構成される。該外界センサ５０は、その正面側の領域に存在する物体の複数の測定点までの距離を計測し、その計測データ（以降、外界計測データという）を出力することができるように構成されている。本実施形態では、該外界計測データが、本発明における物体の検出データに相当する。

第１処理部３０、第２処理部４０及びセンサ制御部５１のそれぞれは、マイクロコンピュータもしくはプロセッサ、メモリ、インターフェース回路等を含む電子回路ユニットにより構成される。そして、図２に示す如く、第１処理部３０は、第２処理部４０と通信し得るように、通信ケーブル３４ａ，３４ｂを介して第２制御部４０と接続されている。また、第２制御部４０は、センサ制御部５１と通信し得るように、通信ケーブル４８ａ，４８ｂを介してセンサ制御部５１と接続されていると共に、外界センサ５０から外界計測データを受信し得るように、通信ケーブル４８ｃを介して該外界センサ５０と接続されている。

以降、本実施形態の制御処理装置Ａの構成及び作動をさらに詳細に説明する。まず、第１処理部３０は、実装されたハードウェア構成及びプログラム（ソフトウェア構成）により実現される機能として、ロボット１の動作制御等を行うロボット動作制御部３１と、所定の基準周期毎の時刻を表す第１カウント値Ｎ１（より詳しくは、任意の初期時刻からの経過時間を上記基準周期毎に表す第１カウント値Ｎ１）を逐次更新しつつ出力する第１カウンタ３２とを含む。

第１カウンタ３２は、第１処理部３０に備えられた図示しない発振器等により生成されるクロック信号により規定される所定の基準周期Δｔ１（一定周期）で、第１カウント値Ｎ１を増加させていくように該第１カウント値Ｎ１を逐次更新する。そして、第１カウンタ３２から出力される第１カウント値Ｎ１は、ロボット動作制御部３１に入力されると共に、第１処理部３０から通信ケーブル３４ｂを介して第２処理部４０に送信される。

ロボット動作制御部３１は、第１カウンタ３２から入力される第１カウント値Ｎ１の更新に同期した周期（＝基準周期Δｔ１）で、ロボット１の動作制御のための制御処理を逐次実行する。

この場合、ロボット動作制御部３１には、ロボット１の各関節の変位量（回転角等）の検出データ、及びロボット１の下側基体６（又は上側基体７）の姿勢（傾斜角等）の検出データ等、ロボット１の実際の動作状態を示す検出データが図示を省略するセンサ等から入力されると共に、ロボット１の動作目標が、図示を省略する上位の制御装置あるいは外部のサーバ等から入力される。

そして、ロボット動作制御部３１は、入力された動作目標を実現するように、ロボット１の各関節の変位量の目標値を逐次決定し、該目標値に各関節の実際の変位量を追従させるように、各関節を電動モータ等のアクチュエータを介して駆動する。

また、ロボット動作制御部３１は、上記の如くロボット１の動作制御を行うことと並行して、第１カウンタ３２から入力される第１カウント値Ｎ１のそれぞれに対応する時刻における前記外界センサ５０の搭載部（頭部５）の姿勢の検出値を示す姿勢検出データに、対応する時刻を示す第１カウント値Ｎ１を付加してなるデータ（以降、姿勢検出データ(Ｎ１)と称する）を前記基準周期Δｔ１（＝第１カウント値Ｎ１の更新周期）で逐次出力する。

ここで、上記姿勢検出データ(Ｎ１)は、より詳しくは、例えば、第１カウント値Ｎ１に対応する時刻での外界センサ５０の搭載部（頭部５）の、ロボット１の基準部位（例えば下側基体６）に対する相対的な位置及び向きを特定し得る検出データである。該姿勢検出データ(Ｎ１)としては、例えばロボット１の基準部位に設定されるロボット座標系で見た、外界センサ５０の搭載部（頭部５）の位置及び向きを示す検出データ、あるいは、該位置及び向きを規定する検出データ（例えば、ロボット１の基準部位と外界センサ５０の搭載部（頭部５）との間に存在する各関節の変位量の検出データ）等を使用することができる。

そして、ロボット動作制御部３１から出力される姿勢検出データ(Ｎ１)は、第１処理部３０から通信ケーブル３４ａを介して第２処理部４０に逐次送信される。本実施形態では、かかる姿勢検出データ(Ｎ１)が本発明における第１データに相当する。

なお、第１カウント値Ｎ１の更新周期（＝基準周期Δｔ１）は、第１処理部３０と第２処理部４０との間のデータ通信（通信ケーブル３４ａ又は３４ｂによるデータ通信）に要する時間（所謂、レイテンシ）よりも大きい時間幅である。

第２処理部４０は、実装されたハードウェア構成及びプログラム（ソフトウェア構成）により実現される機能として、前記基準周期よりも短い所定周期毎の時刻を表す第２カウント値Ｎ２（より詳しくは、任意の初期時刻からの経過時間を上記所定周期毎に表す第２カウント値Ｎ２）を逐次更新しつつ出力する第２カウンタ４１と、第１処理部３０から受信した第１カウント値により示される時刻と該２カウンタ４２から出力される第２カウント値により示される時刻とを合成した時刻を表す内挿カウント値Ｎ３を生成する内挿カウント値生成部４２と、内挿カウント値Ｎ３に対応する時刻でのロボット１の外界センサ５０の搭載部（頭部５）の姿勢を推定するセンサ姿勢推定部４３と、外界センサ５０から前記外界計測データを取得する外界計測データ取得部４４と、外界センサ５０により検出された物体（距離計測がなされた物体）のロボット１に対する相対的な配置状態を示す外界認識データを生成する処理を実行する実行する３次元再構成部４５とを備える。

第２カウンタ４１は、第２処理部４０に備えられた図示しない発振器等により生成されるクロック信号により規定される所定周期Δｔ２（一定周期）で、第２カウント値Ｎ２を増加させていくように該第２カウント値Ｎ２を逐次更新する。この場合、第２カウント値Ｎ２の更新周期である上記所定周期Δｔ２は、前記基準周期Δｔ１（＝第１カウント値Ｎ１の更新周期）よりも十分に短い周期（例えば、基準周期Δｔ１の１／１０００の周期）である。従って、第２カウンタ４１は、第１カウンタ３２よりも高速でカウント値を更新する高速カウンタである。

また、第２カウンタ４１には、第１処理部３０から第２処理部４０で受信された第１カウント値Ｎ１が逐次入力される。そして、第２カウンタ４１は、第１カウント値Ｎ１の入力の都度（受信の都度）、出力する第２カウント値Ｎ２を初期値（例えばゼロ）に初期化する。

従って、第２カウンタ４１から出力される第２カウント値Ｎ２は、第１カウント値Ｎ１の受信時刻からの経過時間（上記所定周期Δｔ２毎の経過時間）を表すものとなる。この場合、第２カウント値Ｎ２の更新周期（＝Δｔ２。以降、高速周期Δｔ２ということがある）は、前記基準周期Δｔ１より短い周期（本実施形態では、例えばΔｔ２＝Δｔ１／１０００）であるので、該第２カウント値Ｎ２は、第１カウント値Ｎ１よりも高い時間分解能での時刻を表すものとなる。

また、外界計測データ取得部４４は、例えば所定の計測処理周期で、あるいは、第２処理部４０で適宜決定される計測タイミング毎に、外界センサ５０による外界計測データを取得する。この場合、本実施形態では、外界計測データ取得部４４は、外界計測データの取得を行う計測タイミングで、センサ制御部５１に計測開始を指示する開始トリガ信号を出力する。

この開始トリガ信号は、センサ制御部５１を介して外界センサ５０に与えられ、これに応じて外界センサ５０による計測が開始される。このとき、外界センサ５０はその正面に存在する物体の複数の測定点までの距離を計測することで得られた外界計測データを第２処理部４０に出力する。そして、該外界計測データが外界計測データ取得部４４で取得される。

一方、外界センサ５０は、計測を終了すると、その旨を示す終了トリガ信号をセンサ制御部５１を介して第２処理部４０に出力する。そして、該終了トリガ信号は、第２処理部４０の内挿カウント値生成部４２に入力される。

該内挿カウント値生成部４２には、上記の如くセンサ制御部５１から終了トリガ信号が入力されるほか、第１処理部３０から第２処理部４０で受信された第１カウント値Ｎ１と、第２カウンタ４１から出力された第２カウント値Ｎ２とが逐次入力される。

そして、内挿カウント値生成部４２は、センサ制御部５１から終了トリガ信号が入力された時刻を表す内挿カウント値Ｎ３を決定して出力する。該内挿カウント値Ｎ３は、換言すれば、外界計測データ取得部４４が外界センサ５０から外界計測データを取得した時刻を表す指標値として、該外界計測データに対応付られるものであり、例えば次のように決定される。

すなわち、内挿カウント値生成部４２は、センサ制御部５１から終了トリガ信号が入力された時刻以前に第１処理部３０から受信した最新の第１カウント値Ｎ１（以降、これをＮ１ａと表記する）と、該終了トリガ信号が入力された時刻で第２カウンタ４１から出力されている第２カウント値Ｎ２（以降、これをＮ２ａと表記する）とを、例えば、次式（１）により合成（線形結合）することにより、内挿カウント値Ｎ３を決定する。

Ｎ３＝Ｎ１ａ＋Ｎ２ａ／ｎ ……（１）
ただし、ｎ＝Δｔ１／Δｔ２（例えばｎ＝１０００）

例えば、Ｎ１ａ＝５０、Ｎ２ａ＝１５０である場合、内挿カウント値Ｎ３は、Ｎ３＝５０．１５０となる。このように、内挿カウントＮ３を決定した場合、該内挿カウント値Ｎ３に、前記基準周期Δｔ１を乗じてなる時刻（＝Δｔ１・Ｎ３＝Δｔ１・Ｎ１ａ＋Δｔ２・Ｎ２ａ）が、外界計測データ取得部４４が外界センサ５０から外界計測データを取得した時刻（初期時刻からの経過時間）を第２カウント値Ｎ２と同じ時間分解能（第１カウント値Ｎ１のｎ倍（本実施形態では１０００倍）の時間分解能）で表すものとなる。

なお、内挿カウント値Ｎ３は、上記式（１）により算出される値に、ゼロ以外の任意の定数（例えば、ｎ）を乗じた値であってもよい。また、内挿カウント値Ｎ３は、外界計測データ取得部４４が外界センサ５０から外界計測データを取得した時刻（初期時刻からの経過時間）そのものを表す値（＝Δｔ１・Ｎ１ａ＋Δｔ２・Ｎ２ａ）であってもよい。

ところで、前記通信ケーブル３４ｂの断線、あるいは通信不良等により、第２処理部４０が第１処理部３０から第１カウント値Ｎ１を正常に受信できなくなると、第２カウンタ４１から出力される第２カウント値Ｎ２が初期化されなくなると共に、内挿カウント値Ｎ３の生成に用いる第１カウント値Ｎ１が内挿カウント値生成部４２に入力されなくなる。

この場合、内挿カウント値生成部４２は、第２カウンタ４１から出力される第２カウント値Ｎ２だけを用いて、内挿カウント値Ｎ３を推定する。

具体的には、内挿カウント値生成部４２は、第２カウンタ４１から出力される第２カウント値Ｎ２が、あらかじめ定められた所定値Ｎ２ｘを超えた場合に、第２処理部４０が第１処理部３０から第１カウント値Ｎ１を正常に受信できない異常状態が発生したことを検知する。上記所定値Ｎ２ｘは、例えば、前記基準周期Δｔに相当する第２カウント値Ｎ２の変化量（＝Δｔ１／Δｔ２）に所定のマージンα（＜Δｔ１／Δｔ２）を加えた値に設定されている。

そして、内挿カウント値生成部４２は、上記異常状態の発生を検知した場合には、その後、センサ制御部５１から終了トリガ信号が入力されたときに、外界計測データの取得時刻（取得した外界計測データに対応付ける時刻）を表す内挿カウント値Ｎ３を、例えば、次式（２）により決定する。

Ｎ３＝Ｎ１ｂ＋ΔＮ１＋（Ｎ２ａ−ｎ・ΔＮ１）／ｎ ……（２）
ただし、
Ｎ１ｂ：異常状態の発生前に第２処理部４０が最後に受信した第１カウント値
ΔＮ１：現在時刻での第２カウント値Ｎ２ａを比率ｎ（＝Δｔ１／Δｔ２）で除算して得られる商（整数値）
Ｎ２ａ−ｎ・ΔＮ１：現在時刻での第２カウント値Ｎ２ａを比率ｎ（＝Δｔ１／Δｔ２）で除算して得られる剰余

この場合、式（２）の右辺のＮ１ｂ＋ΔＮ１が、第１カウント値Ｎ１を第２処理部４０が正常に受信できる状態が継続していると仮定した場合に、外界計測データの取得時刻以前に、第２処理部４０で受信していたはずの最新の第１カウント値Ｎ１ａの推定値（前記式（１）の右辺のＮ１ａの推定値）に相当し、（Ｎ２ａ−ｎ・ΔＮ１）は、上記仮定の基での該第１カウント値Ｎ１ａの受信時点以後の第２カウント値Ｎ２の変化量の推定値（前記式（１）の右辺のＮ２ａの推定値）に相当する。

上記式（２）により内挿カウント値Ｎ３を決定することで、第２処理部４０が第１カウント値Ｎ１を正常に受信できない異常状態が発生した後であっても、外界計測データの取得時刻を示す内挿カウント値Ｎ３を適正に決定することができる。

上記のように内挿カウント値生成部４２で決定された内挿カウント値Ｎ３は、外界計測データ計測部４４による外界計測データの取得の都度、センサ姿勢推定部４３に入力される。該センサ姿勢推定部４３には、内挿カウント値Ｎ３の他、第１処理部３０から送信されて第２処理部４０が受信した姿勢検出データ(Ｎ１)が逐次入力される。

そして、センサ姿勢推定部４３は、入力された内挿カウント値Ｎ３により示される時刻（外界計測データの取得時刻）での外界センサ５０の搭載部（頭部５）の位置及び向きを表す姿勢検出データ(Ｎ３)を推定する。

この場合、センサ姿勢推定部４３は、内挿カウント値Ｎ３により示される時刻が、ある第１カウント値Ｎ１ｃにより示される時刻に一致する場合（本実施形態では、Ｎ３＝Ｎ１ｃである場合）には、第２処理部４０で受信された姿勢検出データ(Ｎ１)のうち、該第１カウント値Ｎ１ｃが対応付られた姿勢検出データ(Ｎ１ｃ)を、内挿カウント値Ｎ３により示される時刻での姿勢検出データ(Ｎ３)として決定する。

また、内挿カウント値Ｎ３により示される時刻が、ある第１カウント値Ｎ１ｄにより示される時刻と、その次の第１カウント値Ｎ１ｄ＋１により示される時刻との間の時刻である場合（本実施形態では、Ｎ１ｄ＜Ｎ３＜Ｎ１ｄ＋１である場合）には、センサ姿勢推定部４３は、第１カウント値Ｎ１ｄが対応付けられた姿勢検出データ(Ｎ１ｄ)と、次の第２カウント値Ｎ１ｄ＋１が対応付られた姿勢検出データ(Ｎ１ｄ＋１)とから、補間処理（例えば線形補間処理）により、内挿カウント値Ｎ３により示される時刻での姿勢検出データ(Ｎ３)を推定する。これにより、外界計測データの各取得時刻（各内挿カウント値Ｎ３により示される時刻）での姿勢検出データを得ることができる。

センサ姿勢推定部４３により上記の如く推定された姿勢検出データ(Ｎ３)と、外界計測データ取得部４４で取得された外界計測データとは、それぞれ時系列的に３次元再構成部４５に入力される。そして、該３次元再構成部４５は、各内挿カウント値Ｎ３により示される時刻で取得された外界計測データ(Ｎ３)と、該内挿カウント値Ｎ３により示される時刻での姿勢検出データ(Ｎ３)の推定値とから、該内挿カウント値Ｎ３により示される時刻での物体配置状態（外界センサ５０により検出された物体のロボット１に対する相対的な配置状態）を特定する。該物体配置状態は、例えば、ロボット１に設定された座標系で見た３次元の点群によって表される。

上記のように３次元再構成部４５により特定された物体配置状態を示すデータである外界認識データは、第２処理部４０から、第１処理部３０、あるいは、上位の制御装置等に送信される。本実施形態では、かかる外界認識データが本発明における第３データに相当する。

以上が本実施形態の制御処理装置Ａの詳細である。かかる制御処理装置Ａによれば、第２処理部４０は、第１処理部３０での制御処理に係る基準周期Δｔ１（第１カウント値Ｎ１の更新周期）よりも十分に短い高速周期Δｔ２により規定される高い時間分解能で、外界計測データの取得時刻を得ることができる。

また、第２処理部４０は、第１処理部３０から送信される姿勢検出データ(Ｎ１)の時系列から補間処理により、外界計測データの取得時刻での姿勢検出データを推定することができる。

このため、３次元再構成部４５により、精度よくロボット１に対する周囲の物体の相対的な配置状態を示す外界認識データ（３次元点群のデータ）を生成することができる。ひいては、該外界認識データを用いてロボット１の動作制御を行う場合に、ロボット１の周囲の物体の実際の配置状態を適切に反映させて、ロボット１の動作目標を決定することが可能となる。

また、通信ケーブル３４ｂの断線等により、第２処理部４０が第１処理部３０から送信される第１カウント値Ｎ１を正常に受信することができなくなる異常状態が発生しても、外界計測データの取得時刻を示す内挿カウント値Ｎ３を、姿勢検出データ(Ｎ１)との時間的な関係を特定し得るように推定することができる。

すなわち、あたかも、第２処理部４０が第１処理部３０から送信される第１カウント値Ｎ１を正常に受信し得る状態が継続しているようにして、外界計測データの取得時刻を示す内挿カウント値Ｎ３を決定することができる。

ひいては、第２処理部４０が第１処理部３０から送信される第１カウント値Ｎ１を正常に受信することができなくなる異常状態が発生しても、外界計測データとの時間的な整合性を適切にとり得るように、外界検出データの各取得時刻での姿勢検出データを推定することを適切に実現できる。その結果、ロボット１の動作の非常停止等を適切に実現することも可能となる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、他の実施形態を採用することもできる。以下に他の実施形態を説明する。

前記実施形態では、外界センサ５０による計測を、第２処理部４０からセンサ制御部５１に送信する開始トリガ信号に応じて実行するようにした。ただし、外界センサ５０による計測を、第２処理部４０からセンサ制御部５１への開始トリガ信号の送信を必要とせずに、センサ制御部５１が周期的に、もしくは適宜決定したタイミングで開始させるようにしてもよい。

また、前記実施形態では、制御処理装置Ａを適用する移動体として、脚式移動ロボットを例示したが、移動体は、例えば車輪型の移動体等であってもよい。また、本発明の制御処理装置は、ロボット等の移動体に限らず、産業ロボット、輸送機器等、種々様々な装置に適用できる。この場合、本発明の制御処理装置を適用する装置毎に、第１処理部で生成又は取得する第１データ、あるいは、第２処理部で生成又は取得する第２データとして、種々様々な種類のデータを採用し得る。これらの第１データ又は第２データは、検出データに限らず、目標値等であってもよい。

また、前記実施形態では、第１処理部３０と第２処理部４０との間の通信は、有線通信であるが、該通信は無線通信であってよい。

また、前記実施形態では、第２処理部４０が第１カウント値Ｎ１を受信する毎に、第２カウント値Ｎ２を初期化するようにした。ただし、例えば、基準周期の複数倍の期間毎に、第２カウント値Ｎ２を初期化するようにすることも可能である。ただし、前記基準周期Δｔ１と、高速周期Δｔ２との比率ｎ（＝Δｔ１／Δｔ２）のばらつきを考慮すると、第２処理部４０が第１カウント値Ｎ１を受信する毎に、第２カウント値Ｎ２を初期化することが望ましい。

Ａ…制御処理装置、１…ロボット（移動体）、５…頭部（移動体の所定の部位）、３０…第１処理部、４０…第２処理部、３２…第１カウンタ、４１…第２カウンタ、５０…外界センサ。

Claims (5)

1. 所定の基準周期毎の時刻を表す第１カウント値を該基準周期で逐次更新するように出力する第１カウンタを有し、該第１カウンタから出力される各第１カウント値が対応付けられた第１データを取得する処理と、該第１カウント値を前記基準周期で逐次送信する処理とを実行する機能を有する第１処理部と、
   前記第１処理部から送信される前記第１カウント値を受信可能に構成されていると共に、前記基準周期よりも短い所定の周期毎の時刻を表す第２カウント値を該所定の周期で逐次更新するように出力する第２カウンタを有し、該第２カウンタから出力される第２カウント値を、前記第１カウント値の受信に応じて定期的に初期化する処理と、前記第１データに関連する第２データを前記基準周期により規定される時間分解能よりも高い時間分解能で取得する処理とを実行する機能を有する第２処理部とを備えており、
   前記第１処理部は、前記各第１カウント値が対応付けられた前記第１データとして、移動体の所定の部位の動作状態に関する検出データを取得して、該第１データを前記第２処理部に送信する処理を前記基準周期で逐次実行するように構成されており、
   前記第２処理部は、
   前記移動体の周囲に存在する物体を検出し得るように該移動体の前記所定の部位に搭載された外界センサから出力される前記物体の検出データを前記第２データとして取得するように構成されていると共に、
   前記第２データを取得するとき、前記外界センサによる検出の終了に応じて該外界センサから入力される終了トリガ信号に応じて、該終了トリガ信号の入力時刻を当該取得した第２データに対応する時刻として決定し、該入力時刻を、前記第１処理部から受信した第１カウント値により示される時刻と、前記第２カウンタから出力された第２カウント値により示される時刻とを合成した時刻として求めて該第２データに対応付ける機能と、
   該第２データに対応付けられた時刻での前記第１データの値を、前記第１処理部から受信した前記第１データの時系列から補間処理により推定する処理を実行する機能と、
   該第２データに対応付けられた時刻での前記第１データの値として前記補間処理により推定した値と該第２データの値とを用いて、前記外界センサにより検出された物体の前記移動体に対する相対的な配置状態を示す第３データを生成する機能とををさらに有するように構成されていることを特徴とする制御処理装置。
2. 請求項１記載の制御処理装置において、
   前記第２処理部は、前記第２データを取得した時刻以前に前記第１処理部から受信した最新の第１カウント値により示される時刻に、該最新の第１カウント値により示される時刻から前記第２データを取得した時刻までに前記第２カウンタから出力された第２カウント値の変化量により示される時間幅を加えた時刻を前記合成した時刻として前記第２データに対応付けるように構成されていることを特徴とする制御処理装置。
3. 請求項１又は２記載の制御処理装置において、
   前記第２処理部は、前記第１カウント値の受信毎に、前記第２カウンタから出力される前記第２カウント値を初期化するように構成されていることを特徴とする制御処理装置。
4. 請求項１〜３のいずれか１項に記載の制御処理装置において、
   前記第２処理部は、前記第１処理部から送信される前記第１カウント値を正常に受信できない異常状態が発生したとき、該異常状態の発生後に取得する前記第２データに対応付ける時刻を、前記第２カウンタから出力される第２カウント値に基づいて推定し、当該推定した時刻を前記第２データに対応付けるように構成されていることを特徴とする制御処理装置。
5. 請求項４記載の制御処理装置において、
   前記第２処理部は、前記異常状態の発生前に最後に受信した前記第１カウント値により示される時刻に、該時刻から、前記異常状態の発生後に前記第２データを取得した時刻までに前記第２カウンタから出力された前記第２カウント値の変化量により示される時間幅を加えた時刻を該第２データに対応付ける時刻として推定するように構成されていることを特徴とする制御処理装置。