JP4762999B2 - ロボット制御システム - Google Patents

Description

本発明はロボット制御システム、特にロボットのメインプロセッサとセンサユニット間のデータ通信に関する。

ロボット等の移動体の姿勢制御に加速度センサや角速度センサが用いられている。直交する３軸をＸ軸、Ｙ軸、Ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りの角速度を３個の角速度センサで検出する。軸回りの角度、あるいは姿勢角は、角速度センサの出力を時間積分して得られ、ロール角、ピッチ角、ヨー角が算出される。

特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。

また、特開平６−３４０１４９号公報には、コマンド及び可変長のパラメータの組からなるデータを送受信することが記載されている。

センサユニットで検出された姿勢角等のセンサデータはロボットの姿勢制御を行うメインプロセッサ（あるいはホストプロセッサ）に送信され、フィードバック制御に用いられるが、データ量の増大に伴い制御周期が長くなるとロボットの制御性が低下するため、必要に応じて送信データ量を調整してデータ通信速度を確保し、制御性を向上できることが望ましい。

また、通信の失敗等によりデータが欠落した場合、メインプロセッサは所定の欠落処理を実行してロボットの制御性を維持できるが、欠落の発生自体をメインプロセッサが確実に検出できることが必要である。

本発明の目的は、ロボット制御の応答性を確保できるロボット制御システムを提供することにある。

本発明の一態様は、ロボットのメインプロセッサと、センサ出力を前記メインプロセッサに送信するセンサユニットとを有するロボット制御システムにおいて、前記メインプロセッサと前記センサユニットとの間は、可変長データフォーマットでデータを送受信し、前記可変長データフォーマットは、転送サイズ部、コマンド部、転送パターン部、及びデータ部を含み、前記転送サイズ部で、転送されるデータ量を規定し、前記転送パターン部は、転送される前記データ部のデータ種類及び順序を規定するものであり、複数ビットで構成され、前記複数ビットの各ビットはそれぞれ特定のデータ種類と対応づけられ、各ビットの値は対応づけられたデータ種類のデータの有無を表し、前記メインプロセッサは、前記センサユニットから転送されるデータ種類を減じる場合に、前記コマンド部に前記転送パターン部の各ビットの値を新たな設定パラメータとして記憶する旨のコマンドを含めて、前記センサユニットに送信し、前記センサユニットは、前記メインプロセッサから送信された設定パラメータを格納するメモリを有し、前記センサ出力のうち、前記メモリに格納された設定パラメータで特定されるデータ種類のセンサ出力のみを前記データ部に含めて前記メインプロセッサに送信することを特徴とする。

上記の態様では、固定長データフォーマットではなく可変長データフォーマットでデータを送受信することで、ロボットの制御性を確保する。すなわち、適宜データ長を短くして送受信することで通信速度を向上させ、制御遅れを抑制する。

また、本発明の第２の態様は、上記第１の態様において、メインプロセッサからセンサユニットに送信されるデータにメインプロセッサで計時された時刻のデータを含め、かつ、センサユニットからメインプロセッサに送信されるデータに時刻のデータ及びセンサユニットで計時された経過時間のデータを含める。センサユニットからメインプロセッサに送信されるデータに時刻のデータ及び経過時間のデータを含めることで、メインプロセッサではセンサユニットから受信したデータの時間情報を得ることができ、受信したデータの時間の不連続性からデータの欠落を容易に検出できる。また、センサユニットからメインプロセッサに送るデータに遅延が生じたとしても、センサユニットからメインプロセッサに送信されるデータに（時刻のデータ＋経過時間のデータ）が含まれているため、メインプロセッサは正確にセンサユニットから送信されたデータの時間（計測時間）を識別できる。また、時刻のデータはメインプロセッサのみで生成するため、メインプロセッサで計測した時刻とセンサユニットで計測した時刻の誤差問題、いわゆる同時性問題を生じない。

本発明によれば、送受信するデータ長を適宜増減調整できるので、ロボットの制御応答性を確保できる。

また、本発明の第２の態様によれば、センサユニットから送信されるデータに時間遅れが生じていても、メインプロセッサは正確に時間情報を取得することができるので、ロボットのリアルタイム処理を実行できる。

以下、図面に基づき本発明の実施形態について説明する。

図１に、本実施形態に係るロボット制御システムの概念構成を示す。センサユニット１０及びロボットのメインプロセッサ（ホストプロセッサ）であるロボットＣＰＵ１２が設けられ、センサユニット１０とロボットＣＰＵ１２はシリアルデータライン１４でシリアル通信可能に接続される。なお、センサユニット１０及びロボットＣＰＵ１２が組み込まれるロボットの形状は任意であり、２輪走行ロボット、４輪走行ロボット、２足歩行ロボット、飛行体ロボット等のいずれでもよい。

センサユニット１０は、加速度センサや角速度センサ等のセンサ１５、ＲＡＭ１６、ＲＯＭ１８、ドライバ２０及びＣＰＵ２２を含む。

ＲＯＭ１８は、センサユニット１０の実行処理を記述したＯＳ（operating system）あるいはプログラムを格納する。プログラムには、ロボットＣＰＵ１２に対して送信すべきセンサ出力の種類やリセット機能の切替、内部フィルタの時定数などを設定するパラメータを含む。ＲＯＭ１８は、フラッシュＲＯＭ等の書換可能なメモリで構成される。

ＲＡＭ１６は、ＲＯＭ１８に記憶されたパラメータを記憶する。すなわち、電源投入時のブート動作において、ＣＰＵ２２はＲＯＭ１８に記憶されたパラメータを読み出してＲＡＭ１６に書き込み（ロード）、ＲＡＭ１６に書き込まれたパラメータを読み出して所定の処理を実行する。ＣＰＵ２２は、ＲＯＭ１８から読み出したパラメータをＲＡＭ１６の特定領域に書き込む。本実施形態において、この特定領域を第１領域と称する。第１領域は、ＲＡＭ１６内に予めその開始アドレス（物理アドレス）と終了アドレスが固定的に設定されていてもよいが、可変であってもよい。

ＣＰＵ２２は、ＲＡＭ１６から読み出したパラメータに従い、センサ１５から入力した各種のセンサ出力のうちパラメータで設定されたセンサ出力を選択してドライバ２０を介してロボットＣＰＵ１２に送信する。ドライバ２０は、例えばＲＳ−２３２Ｃドライバであるが、これに限定されるものではなく、ＵＳＢ、ＲＳ４２２、ＩＥＥＥ１３９４等でもよい。ＣＰＵ２２は、ドライバ２０を介してシリアルデータライン１４にセンサ出力データを送出するが、所定制御周期のうちの送信周期においてデータを送信する。所定制御周期のうちの残りの周期は受信周期に割り当てられ、ＣＰＵ２２はロボットＣＰＵ１２からシリアルデータライン１４を経て送信されてきたデータを受信する。

図２Ａ，２Ｂに、センサユニット１０のＣＰＵ２２とロボットＣＰＵ１２との間で行われるシリアル通信のタイミングチャートを示す。図２ＡはＣＰＵ２２から見た送信時のタイミングチャートであり、図２ＢはＣＰＵ２２から見た受信時のタイミングチャートである。

図２Ａにおいて、１制御周期は例えば１０ｍｓｅｃであり、この制御周期を送信周期と受信周期に時分割する。ＣＰＵ２２は、送信周期においてセンサ１５からのセンサ出力をロボットＣＰＵ１２にシリアルで送信する。図において、ＣＰＵ２２から送信周期において送信されたデータは送信データ１００として示されている。送信データ１００は、例えばＢＡＳＥ６４でエンコードされて送信される。ＢＡＳＥ６４は周知技術であり、バイナリデータをＡＳＣＩＩファイル化して送信するための変換方式で、バイナリデータを６ビットずつに区切り、それぞれを０〜６３までの６ビット整数とみなしてアルファベットや文字の全６４文字に対応させるものである。ＢＡＳＥ６４エンコードではデータ量が増大するものの、一般形式なのでデータを読み書きしやすい利点がある。もちろん、他のエンコード方式あるいはデータ圧縮方式を用いてもよい。

ＢＡＳＥ６４エンコードデータの前後には所定の区切り記号（デリミタ）が付加されて送信データの１フレームとされる。デリミタは、「（」や「）」、「＜」が用いられる。「（」や「＜」をフレームの先頭デリミタ、「）」をフレームの最後デリミタとし、送信データの１フレームを例示すると、
送信データ１フレーム＝（ＢＡＳＥ６４エンコードデータ）
送信データ１フレーム＝＜ＢＡＳＥ６４エンコードデータ）
である。「（」は送信データにコマンドが含まれ、「＜」はセンサユニット１０で検出されたセンサデータが含まれることを示すデリミタとする。前者をコマンド系、後者を計測データ系と称する。

一方、図２Ｂにおいて、制御周期のうちの送信周期の残りの周期が受信周期として割り当てられ、ロボットＣＰＵ１２はこのタイミングでシリアルデータライン１４にデータを送信する。ＣＰＵ２２は、このタイミングでロボットＣＰＵ１２から送信されたデータを受信する。図において、ロボットＣＰＵ１２から送信されたデータは受信データ２００として示されている。ＣＰＵ２２は、受信周期においてロボットＣＰＵ１２からのデータを受信すると、受信データ２００をＲＡＭ１６に格納する。受信データ２００の格納領域は、第１領域と異なる第２領域である。第２領域の開始アドレスは第１領域の終了アドレスの次番地としてもよく、所定番地だけ離れていてもよい。ロボットＣＰＵ１２は、送信すべきデータ量が多い場合には複数の制御周期にわたってデータをパケットに分割して順次送信する。ＣＰＵ２２は、これらのデータを順次受信して、ＲＡＭ１６の第２領域に格納する。第２領域に格納されたパラメータは、センサユニット１０からロボットＣＰＵ１２に送信するデータの種類等を変更する際に用いられる。

＜可変長データフォーマット＞
図３に、センサユニット１０とロボットＣＰＵ１２間のデータ送信に用いられるデータフォーマット３００を示す。送信データ量を増減調整できる可変長データフォーマットである。

データフォーマット３００は、転送サイズ部３０２、コマンド部３０４、転送パターン部３０６、計測データ部３０８及びＣＲＣ部３１０の順序で構成される。

転送サイズ部３０２は、送信データ１フレームの全データ量を規定する。全データ量は、例えば２バイトで表現される。

コマンド部３０４は、転送先の実行内容を規定する。特に、センサユニット１０が実行すべき内容を規定する。コマンドは１バイトで表現される。コマンドを例示すると以下のとおりである。

「ＳＴＡＲＴ」コマンドは、センサユニット１０での計測を開始するコマンドである。「ＳＴＡＲＴ」コマンドを受信したＣＰＵ２２は、センサ１５からのセンサ出力を指定された周期でロボットＣＰＵ１２に送信する。

「ＳＴＯＰ」コマンドは、センサユニット１０での計測を停止するコマンドである。

「ＧＥＴ」コマンドは、ＲＡＭ１６の第１領域あるいは第２領域に記憶されたパラメータを読み出すコマンドである。

「ＳＥＴ」コマンドは、ＲＡＭ１６の第２領域に新たなパラメータを書き込むコマンドであり、この「ＳＥＴ」コマンドによりＣＰＵ２２はロボットＣＰＵ１２から受信したデータを上記のようにＲＡＭ１６の第２領域に格納し、第２領域に格納された新たなパラメータ（更新パラメータ）を読み出し実行することでその特性を変更できる。本実施形態におけるセンサユニット１０の特性変更には送信データの種類あるいは送信データ数の変更が含まれる。

「ＷＲＩＴＥ」コマンドは、ＲＡＭ１６の第２領域に記憶された新たなパラメータをＲＯＭ１８に書き込むコマンドである。これにより、電源遮断後においても新たなパラメータがセンサユニット１０内に保持される。

「ＲｓｔＴｉｍ」コマンドは、センサユニット１０のタイムカウントを０にリセットするコマンドである。センサユニット１０のタイムカウントについてはさらに後述する。

転送パターン部３０６は、センサユニット１０からロボットＣＰＵ１２に送信するセンサデータの種類を規定する。転送パターンは、例えば６バイトで表現される。計測データ系では転送パターンを指定する必要があるが、コマンド系では必要ない。転送パターンを例示すると以下のとおりである。
最下位ビット（ＬＳＢ）：姿勢角（ロール角、ピッチ角、ヨー角）
第１位ビット：角速度
第２位ビット：加速度
第３位ビット：傾斜角
第４位ビット：重力補正後の加速度
第５位ビット：速度
第６位ビット：位置
第７位ビット：姿勢行列
第８位ビット：姿勢行列
第９位ビット：姿勢行列
第１０位ビット：姿勢行列
第１１位ビット：無効
第１２位ビット：ユニット温度
第１３位ビット：基板温度
第１４位ビット：ダイアグ
第１５位ビット：タイムカウント
各ビットが「１」のときに該当するデータが計測データとして送信される。例えば、ビット０（ＬＳＢ）が「１」のときにはセンサ１５からの姿勢角データが計測データとして送信される。

計測データ部３０８は、センサ１５の出力のうちの転送パターンで規定されたセンサ出力である。例えば、姿勢角、角速度、加速度、温度、タイムスタンプ、タイムカウント、ユニット名等である。各センサデータは、それぞれ固有のデータ形式を有する。すなわち、姿勢角、角速度、加速度、温度等は浮動小数点型データ、タイムスタンプやタイムカウントは整数型データ、ユニット名は文字型データである。

ＣＲＣ部３１０は、ＣＲＣ（Cyclic Redundancy Check：巡回冗長検査）データを規定する。ＣＲＣも周知技術であり、検査対象のデータ伝送ブロックを２進データとみなし、２進データを生成多項式という計算式で処理して一定のビット数（１６ビットや３２ビット）の検査データを作成し、生成した検査用データを実際のデータにつけて送信し、受信側で同様の多項式で処理してエラーの有無を検査する。

このように、送信データフォーマットの先頭の転送サイズ部３０２でデータ量を規定し、転送パターン部３０６でどのようなセンサデータがどの順で送信されているかを規定するため、データ長が変化したとしても受信側では各センサデータを正確に受信することができる。また、フレームの先頭と最後に特定のデリミタを付加しているため、受信側はデータ長を気にせずにデータの取り込み開始と終了を容易に判別できる。さらに、ロボットＣＰＵ１２（あるいはユーザ）がデータの種類や形式の変更をセンサユニット１０に対して要求し、実時間処理のために当該変更内容が反映された送信フレームが遅れてセンサユニット１０からロボットＣＰＵ１２に送信されたとしても、データフォーマット３００内にデータ種類等についての情報が書き込まれているので、ロボットＣＰＵ１２は遅れフレームであることを意識することなく読み取ることができる。

以下、ロボットＣＰＵ１２からセンサユニット１０に対して送信すべきセンサ出力を変更するコマンドを出力し、このコマンドに応じてセンサユニットがセンサ出力を変化させ、センサユニット１０からロボットＣＰＵ１２に送信されるデータ長を可変にする場合を例にとり説明する。

センサユニット１０のＣＰＵ２２は、センサ１５の出力のうち、姿勢角、角速度、及び加速度を送信周期において送信しているものとする。ロボットＣＰＵ１２は、図３のデータフォーマット３００において、コマンド部３０４に「ＳＥＴ」コマンドを設定し、続いてデータ番号（設定箇所）、設定パラメータを設定して図２の受信周期にセンサユニット１０に送信する。センサユニット１０のＣＰＵ２２は、「ＳＥＴ」コマンドを解釈し、計測データ部３０８に設定されているパラメータをＲＡＭ１６の第２領域に格納する。ＲＡＭ１６の第１領域に既に格納されているパラメータは、転送パターンの各ビット値であり、姿勢角、角速度、加速度を全て送信するパターンであるから、第２位ビット、第１位ビット、及び最下位ビットで示すと「１１１」である。一方、ロボットＣＰＵ１２から受信しＲＡＭ１６の第２領域に格納される新たなパラメータは「１００」である。これは、加速度を出力し、角速度及び姿勢角を出力しないパターンである。ＣＰＵ２２は、ＲＡＭ１６の第２領域に全てのパラメータを格納した後、第２領域に格納されたパラメータに従ってセンサ１５からのセンサ出力を図３のデータフォーマット３００でロボットＣＰＵ１２に送信する。ＣＰＵ２２は、「ＳＥＴ」コマンドを解釈し、新たなパラメータをＲＡＭ１６の第２領域に格納した後に、次の送信タイミングから新たなパラメータに切り替えてデータを送信する。ＣＰＵ２２が演算処理の実行中でジョブを割り当てられない、あるいはＲＳ−２３２Ｃが通信中等で時間に余裕がない場合には、以降の送信フレームから新たなパラメータに切り替えられ反映される。このようにして転送パターン部３０６の転送パターンは「１１１」から「１００」に変更され、計測データ部３０８に含まれるセンサ出力も（加速度、角速度、姿勢角）から（加速度）に変更される。フレームの先頭には「＜」のデリミタが付加され、フレームの最後には「）」のデリミタが付加される。計測データのうち角速度及び姿勢角が削除されているためデータ長が短くなり、１フレームのデータ量も小さくなる。総データ量は先頭の転送サイズ部３０２で設定される。

ロボットＣＰＵ１２は、センサユニット１０から受信したデータの先頭デリミタ及び最後デリミタを検出することで１フレーム分のデータを判別し、転送サイズ部３０２で受信したフレームのデータ量を判別し、転送パターン部３０６で加速度のみが送信されたことを判別し、計測データ部３０８に設定された加速度を取得する。ロボットＣＰＵ１２は、受信した加速度に基づいてロボットの姿勢をフィードバック制御する。かくして送信データ量を小さくし（あるいはデータ長を短くし）、通信速度を向上できる。

図４Ａ，４Ｂに、パラメータ変更前のデータフォーマット（図４Ａ）、及びパラメータ変更後のデータフォーマット（図４Ｂ）を示す。計測データ部３０８においてデータ長が短くなることが模式的に示されている。

＜タイムスタンプによる時間管理＞
一方、センサユニット１０からロボットＣＰＵ１２にセンサ出力を送信する場合、データが欠落してデータの「飛び」が生じる場合がある。各制御周期をＴ１、Ｔ２、Ｔ３とすると、センサユニット１０は間断なくこれらの周期でデータを送信し、周期Ｔ１、Ｔ３においてロボットＣＰＵ１２はデータを受信するが、周期Ｔ２のデータが欠落する場合である。このようなデータ欠落が生じた場合、ロボットＣＰＵ１２は欠落処理を実行するためにデータ欠落が生じたことを確実に検出する必要がある。データ欠落を検出できない場合、誤った時間におけるセンサ出力に基づいて姿勢制御が行われてしまう一方、データ欠落を検出できれば、欠落したデータを補完処理等して姿勢制御を維持できる。

そこで、本実施形態では、上記の可変長データフォーマットに加え、計測データ部３０８に「タイムスタンプ」データ及び「タイムカウント」データを付加している。「タイムスタンプ」データは、ロボットＣＰＵ１２からセンサユニット１０に送信するデータに周期的タイミング、あるいは任意のタイミングで付加される。タイムスタンプの付加タイミングはユーザが設定する。ロボットＣＰＵ１２は時計を内蔵し、データを送信するときの参照時刻のデータをタイムスタンプとしてセンサユニット１０に送信する。

センサユニット１０のＣＰＵ２２はタイマを内蔵し、センサ１５のセンサ出力をロボットＣＰＵ１２に送信する場合に、ロボットＣＰＵ１２からのデータに含まれるタイムスタンプ、つまり参照時刻のデータと、タイマでカウントアップした経過時間とをロボットＣＰＵ１２に送信する。ＣＰＵ２２は、ロボットＣＰＵ１２からタイムスタンプを受信する毎にこれを更新してＲＡＭ１６に記憶する。また、ＣＰＵ２２のタイマは電源投入時、あるいはロボットＣＰＵ１２から「ＲｔＴｉｍ」コマンドを受信したときにリセットされる。したがって、経過時間は、電源投入時からの経過時間、あるいはＲｔＴｉｍコマンドを受信してからの経過時間を示す。ロボットＣＰＵ１２から「ＲｔＴｉｍ」コマンドを送信するとともにタイムスタンプを送信することで、タイムカウントはタイムスタンプで示された時刻からの経過時間を示すことになり、ロボットＣＰＵ１２はタイムスタンプとタイムカウントの２つの情報を用いてセンサユニット１０から受信したデータの時間情報を正確に検出できる。

図５に、データフォーマット３００における計測データ部３０８の一部を模式的に示す。「タイムスタンプ」３０８ａは、ロボットＣＰＵ１２からセンサユニット１０に送信されたデータに含まれる、ロボットＣＰＵ１２で計測した参照時刻のデータであり、例えば１２：０１：１５等である。「タイムカウント」３０８ｂは、センサユニット１０で計測した経過時間のデータであり、例えば００：００：１２である。ロボットＣＰＵ１２は、センサユニット１０から受信したデータに付加された、タイムスタンプ３０８ａ及びタイムカウンタ３０８ｂを検出することで、センサユニット１０から受信したデータがどのタイミングのデータであるのかを識別できる。「タイムスタンプ」３０８ａは、「タイムカウント」３０８ｂに付された時間ラベルと云うこともできる。図６に示すように、ロボットＣＰＵ１２がセンサユニット１０から順次受信したフレーム１、２、３のタイムスタンプ及びタイムカウントが以下のようであったとする。
＜受信フレーム１＞
タイムスタンプ １２：０１：１５
タイムカウント ００：００：１２
＜受信フレーム２＞
タイムスタンプ １２：０１：１５
タイムカウント ００：００：１４
＜受信フレーム３＞
タイムスタンプ １２：０１：１５
タイムカウント ００：００：１８
ロボットＣＰＵ１２は、受信フレーム２と受信フレーム３のタイムカウントの差分から、受信フレーム２と受信フレーム３との間でデータが欠落したことを検出できる。

図７Ａ，７Ｂに、センサユニット１０とロボットＣＰＵ１２との間のタイムマネジメント、すなち時間データ送受による時間管理を模式的に示す。まず、図７Ａにおいて、時刻ｔ１でロボットＣＰＵ１２はタイムスタンプを送信する。センサユニット１０のＣＰＵ２２は、タイムスタンプを受信し、時刻ｔ１を示すこのタイムスタンプをＲＡＭ１６に格納する。タイムスタンプとともに「ＲｓＴｉｍ」コマンドを受信した場合には、このコマンドによりタイマを０にリセットし、タイムスタンプの時刻ｔ１から再びカウントアップする。図７Ｂに示すように、ＣＰＵ２２は、センサ１５からのセンサ出力に時刻ｔ１のタイムスタンプ、並びにタイマで計測したタイムカウントΔｔを付加してロボットＣＰＵ１２に送信する。ロボットＣＰＵ１２は、受信したデータの時刻をｔ１＋Δｔとして識別できる。したがって、たとえ通信に時間を要し、センサユニット１０からロボットＣＰＵ１２に送信するフレームに遅延時間が生じていたとしても、ロボットＣＰＵ１２は受信フレームの時刻、つまりセンサユニット１０での計測時間を特定できるため、実時間処理を実行できる。また、順次受信したデータの有効時刻がそれぞれｔ１＋Δｔ、ｔ１＋２・Δｔ、ｔ１＋３・Δｔである場合、欠落なくデータを受信していることを検出できる。順次受信したデータの有効時刻がそれぞれｔ１＋Δｔ、ｔ１＋２・Δｔ、ｔ１＋４・Δｔである場合、センサユニット１０から予め定められた周期で間断なくデータが送信されているから、ｔ１＋３・Δｔのデータが欠落したことを検出できる。

本実施形態において、ロボットＣＰＵ１２のみならず、センサユニット１０にも固有の時計を内蔵し、センサユニット１０からデータを送信する場合にその内蔵時計で計測した現在時刻を付加して送信することも可能であるが、センサユニット１０の時計とロボットＣＰＵ１２の時計とを正確に一致させる必要がある。本実施形態では、センサユニット１０は現在時刻を計測するのではなく、時間間隔だけを計測してタイムスタンプとともに送信しているため、２つの時計の同時性について考慮する必要がなくなる。

以上説明したように、本実施形態では、
（１）センサユニット１０からロボットＣＰＵ１２に対して予め決められた間隔で間断なくデータを送信する
（２）センサユニット１０からロボットＣＰＵ１２に対して可変長データフォーマットでデータを送信し、どのようなデータをどのようなタイミングで送信してもロボットＣＰＵ１２が確実に受信しこれを読み取ることができるようにする
（３）センサユニット１０からロボットＣＰＵ１２に対してデータを送信する際に、センサユニット１０での計測時間データを付加してロボットＣＰＵ１２に送信し、同時性の問題、あるいは２つの時計間の誤差問題を生じることなく、いつのデータであるかをロボットＣＰＵ１２が正確に特定できるようにする
ことで、円滑な姿勢制御が可能となる。

本実施形態では、センサユニット１０は可変長データフォーマット３００においてタイムスタンプ及びタイムカウントのデータをロボットＣＰＵ１２に送信しているが、固定長データフォーマットを含む任意のデータフォーマットに適用することもできる。

また、上記の実施形態では、ロボットＣＰＵ１２とセンサユニット１０との間の１対１での通信を例示したが、本発明はこれに限定されるものではない。ＵＳＢやＩＥＥＥ１３９４、イーサネット（登録商標）（Ｅｔｈｅｒｎｅｔ（登録商標）等）の通信ラインに多数のデバイスが接続されていてもよい。具体的には、各種センサやアクチュエータが多数存在し、ロボットＣＰＵ１２も多数存在して通信ネットワークに接続される状況である。このような多対多のシステムでは、リアルタイム性を確保するためには通信速度が十分速いだけでは不十分で、データの解読（デコード）がそのデータ単位で実行できること、データに時刻補正を行うに必要な時刻情報が含まれていることが重要となる。すなわち、通信ネットワーク上には高級で通信速度の速い機器や、低級で通信速度の遅い機器、高級で通信速度が速いがデータ量が極めて大きく通信時間をかなり占有するデバイス等が混在し、これらがランダムに通信を行うため正確に実時間で全ての機器が動作することはできず、実用上十分な時間幅でタイミングを合わせることが必要である。実用上十分な時間幅はシステムに依存するが、一般に１ｍｓ〜１００ｍｓ程度である。このように実用上の実時間制御を確保するためには自己解読性及び時刻算出機能を有することがキーとなる。なお、自己を識別し、あるいはどのデバイスからの指示に基づくものかも識別する必要があるが、これはヘッダ部で管理できる。また、本実施形態では上記のようにＢＡＳＥ６４を用いているが、ＢＡＳＥ６４は一般的な形式なのでデータサイズとヘッダが読めれば内容（測定データ）を読まずとも複数のデバイスが自分に必要なデータかそうでないかを容易に判別できる利点もある。

以下、ロボットＣＰＵ１２やセンサユニット１０がネットワークに接続される場合について、より具体的に説明する。なお、ネットワーク上の各センサを互いに識別する記号（センサ名前やセンサ番号等）、ＣＰＵを識別する記号（プロセッサ名やプロセッサ番号等）はそれぞれに付与されており、これらは送信データに含まれるものとする。

＜ネットワーク上に複数のセンサが存在する場合＞
センサからのデータの中に、センサを識別する記号が含まれているので、ロボットＣＰＵは複数のセンサの中から特定のセンサを識別できる。また、ロボットＣＰＵはセンサを識別する記号を用いて特定のセンサへコマンドを送信することができる。

＜ネットワーク上に複数のＣＰＵが存在する場合＞
センサからのデータの中に、複数のＣＰＵの中から特定のＣＰＵを識別する記号が含まれているので、ロボットの複数のＣＰＵは該当センサのデータを使用すべき特定のＣＰＵを識別できる。また、ロボットの複数のＣＰＵの中で、特定のＣＰＵが自分を識別する記号をデータに含ませて送信するので、センサへ特定のＣＰＵを識別させながらコマンドを送信することができる。

＜ネットワーク上に複数のセンサと複数のＣＰＵが存在する場合＞
センサからのデータの中に、センサを識別する記号とＣＰＵを識別する記号が含まれているので、ロボットの複数のＣＰＵと複数のセンサが互いに該当するセンサとＣＰＵの組合せを識別できる。また、ロボットの複数のＣＰＵの中で、特定のＣＰＵが特定のセンサを識別する記号とＣＰＵを識別する記号をデータに含ませて送信するので、特定のセンサへ特定のＣＰＵを識別させながらコマンドを送信することができる。

＜ネットワーク上に複数のセンサと複数のＣＰＵが存在する場合のタイムスタンプとタイムカウント＞
タイムカウントは各センサに固有のタイムカウント器により積算カウントされる。タイムスタンプは各ＣＰＵ固有のタイムスタンプを持ち、所定のセンサとの組合せでセンサとＣＰＵがタイムスタンプを共有する。これにより、複数のセンサが認識、管理する時刻を同一とし、かつ保持しなくてもロボットを実時間制御できる。センサは簡単なタイムカウント器（クロックカウンタ等）で動作が可能で、小型、低価格で本機能を実現できる。

本方式により、複数のＣＰＵの時刻を同一とし、かつ保持しなくてもロボットを実時間制御できる。そのため、ＣＰＵの小型化、低価格化が実現できる。また、システムを追加したり、プラグインしたりしても、追加したＣＰＵ、センサの時刻の同期も必要としないので、システムの追加、修正が容易である。一般に、複数のセンサやＣＰＵの時刻を同一にし、保持することは難しく、サイズの大型化、価格の上昇、動作の遅延を生じる。通常、簡単なクロックカウント器は低品位な水晶発振器のクロックをカウントするので、複数のセンサと複数のＣＰＵのクロック周期とクロックタイミングを正確に合わせることが困難で、複数のセンサと複数のＣＰＵの時刻は同一とならない。ロボット等の実時間制御に必要なことは、所定のＣＰＵと所定のセンサとの同期と時刻把握にあるので、固有のタイムスタンプを共有することにより、時刻を同一とすることと等価な行為が容易に実現できる。また、所定の長周期でタイムスタンプを更新することにより、低品位な水晶発振器のクロックをカウントすることによる積算誤差を除去し、実用に耐える時刻管理が可能となる。複数のＣＰＵが同一のタイムスタンプを共有することにより、お互いの時刻を等価的に合わせることも容易に実現できる。

本発明の実施形態に係るロボット制御システムの概念構成図である。 データ送受信のタイミングチャートである。 データ送受信のタイミングチャートである。 データフォーマット図である。 可変長データの説明図である。 可変長データの説明図である。 計測データ部に含まれるタイムスタンプ及びタイムカウントの説明図である。 タイムスタンプ及びタイムカウントの一例を示す図である。 ロボット制御システムの時間管理を示す説明図である。 ロボット制御システムの時間管理を示す説明図である。

Claims (6)

1. ロボットのメインプロセッサと、
   センサ出力を前記メインプロセッサに送信するセンサユニットと、
   を有するロボット制御システムにおいて、
   前記メインプロセッサと前記センサユニットとの間は、可変長データフォーマットでデータを送受信し、
   前記可変長データフォーマットは、転送サイズ部、コマンド部、転送パターン部、及びデータ部を含み、
   前記転送サイズ部で、転送されるデータ量を規定し、
   前記転送パターン部は、転送される前記データ部のデータ種類及び順序を規定するものであり、複数ビットで構成され、前記複数ビットの各ビットはそれぞれ特定のデータ種類と対応づけられ、各ビットの値は対応づけられたデータ種類のデータの有無を表し、
   前記メインプロセッサは、前記センサユニットから転送されるデータ種類を減じる場合に、前記コマンド部に前記転送パターン部の各ビットの値を新たな設定パラメータとして記憶する旨のコマンドを含めて、前記センサユニットに送信し、
   前記センサユニットは、前記メインプロセッサから送信された設定パラメータを格納するメモリを有し、前記センサ出力のうち、前記メモリに格納された設定パラメータで特定されるデータ種類のセンサ出力のみを前記データ部に含めて前記メインプロセッサに送信する
   ことを特徴とするロボット制御システム。
2. 請求項１記載のシステムにおいて、
   前記メインプロセッサから前記センサユニットに送信されるデータには前記メインプロセッサで計時された時刻のデータが含まれ、
   前記センサユニットから前記メインプロセッサに送信されるデータには前記時刻のデータ及び前記センサユニットで計時された前記時刻のデータが示す時刻からの経過時間のデータが含まれる
   ことを特徴とするロボット制御システム。
3. 請求項２記載のシステムにおいて、
   前記経過時間は、各センサに固有のタイムカウント器により計時される
   ことを特徴とするロボット制御システム。
4. 請求項１記載のシステムにおいて、
   前記センサユニットは、所定制御周期のうち、送信周期においてデータをメインプロセッサに送信し、残りの周期においてデータをメインプロセッサから受信する
   ことを特徴とするロボット制御システム。
5. 請求項１記載のシステムにおいて、
   前記センサ出力は、姿勢角、角速度、加速度、温度の少なくとも一つを含む
   ことを特徴とするロボット制御システム。
6. 請求項１記載のシステムにおいて、
   前記メインプロセッサと前記センサユニットの少なくともいずれかは複数存在し、
   前記センサ出力は、複数の前記センサユニットを識別する記号と複数の前記メインプロセッサを識別する記号の少なくともいずれかを含む
   ことを特徴とするロボット制御システム。

Images