JP4812836B2 - ロボット制御システムおよびロボットの制御方法 - Google Patents

Description

本発明はロボット制御システムおよびロボットの制御方法に関し、特にセンサユニットの機能変更に関する。

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

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

また、特開平１１−３１６７３２号公報には、周辺処理装置にホストから送信されるコマンドとして、動作の指定を行う実行コマンドと、動作の指定を行わない実行コマンドを設け、これら２種類の実行コマンドにより動作の変更が必要なときのみその動作を変更できるようにする技術が記載されている。

加速度データや姿勢データを用いて姿勢制御する場合、姿勢角の精度が要求されるが、姿勢角は時間積分して得られるので誤差が積算し精度が低下する場合がある。そこで、姿勢角をあるタイミングで０ｄｅｇにリセット、あるいは所望の角度に設定する必要が生じる。

また、姿勢を制御すべきロボットのタイプやセンサ取付位置あるいは取付姿勢によりセンサの動特性が異なるため、センサ毎に動特性を調整する必要もある。例えばフィルタ時定数を最適値に調整する等である。

このような要求に応えるため、ロボットを停止させ、センサユニットの特性を変更した後、再度ロボットを動作させることも考えられるが、ロボット動作中に即時にセンサユニットの特性を変更できるのが望ましい。例えば、ロボットが特定の動作を行う際に、センサユニットから出力される複数のセンサ出力のうち特定のセンサ出力のみを必要とするため、センサユニットを当該センサ出力のみを送信する特性に直ちに変更できることが望ましい。上記の特開平１１−３１６７３２号公報においても動作の変更が可能であるが、リアルタイムに動作を変更することは困難である。

本発明の目的は、ロボットに組み込まれたセンサユニットの特性（あるいは機能）を即座に変更することができるロボット制御システムおよびロボットの制御方法を提供することにある。

本発明の第１の態様は、メインプロセッサと、センサ出力を前記メインプロセッサに送信するセンサユニットとを有するロボット制御システムであって、前記センサユニットは、プロセッサと、前記プロセッサの動作を規定するパラメータを記憶するための第１領域及び第２領域を備えるメモリとを有し、前記プロセッサは、所定周期内の送信周期において前記メモリの前記第１領域あるいは前記第２領域の一方に記憶されている前記パラメータを用いて前記センサ出力を前記メインプロセッサに送信するとともに前記所定周期内の残りの受信周期において前記メインプロセッサからの更新パラメータを受信して前記第１領域あるいは前記第２領域の他方に格納し、その後、前記更新パラメータを用いて前記センサ出力を前記メインプロセッサに送信する。

上記第１の態様のロボット制御システムでは、センサユニットはメモリの第１領域あるいは第２領域のいずれか（例えば第１領域とする）に記憶されたパラメータに基づきセンサ出力をロボットのメインプロセッサに送信するが、所定周期の受信周期においてメインプロセッサから更新パラメータを受信すると、この更新パラメータを第２領域に格納する。そして、センサユニットのプロセッサは、第１領域に記憶されたパラメータ（デフォルトのパラメータ）の代わりに、第２領域に記憶された更新パラメータに基づいて実行する。ロボット動作中であっても、受信周期においてメインプロセッサからセンサユニットに更新パラメータを送信し、更新パラメータを第２メモリの異なる領域に格納してパラメータをデフォルトから更新パラメータに切り替えることで、迅速にセンサユニットの特性あるいは機能を変更することができる。第２領域に格納された更新パラメータで動作中に、メインプロセッサから更なる更新パラメータが送信されると、今度は第１領域にその更新パラメータを格納する。新たな更新パラメータを格納すると、第２領域に格納された更新パラメータに代わって、第１領域に記憶された新たな更新パラメータに従いセンサユニットが動作する。メモリの第１領域と第２領域を交互に相補的に使用することで、リアルタイムでセンサユニットの特性あるいは機能を任意に変更できる。

本発明の第２の態様は、メインプロセッサと、センサ出力を前記メインプロセッサに送信するセンサユニットとを有し、前記センサユニットは、プロセッサと、前記プロセッサの動作を規定するパラメータを記憶するための第１領域及び第２領域を備えるメモリとを有するロボットの制御方法であって、所定周期内の送信周期において前記メモリの前記第１領域あるいは前記第２領域の一方に記憶されている前記パラメータを用いて前記センサ出力を前記メインプロセッサに送信し、前記所定周期内の残りの受信周期において前記メインプロセッサからの更新パラメータを受信して前記第１領域あるいは前記第２領域の他方に格納し、その後、前記更新パラメータを用いて前記センサ出力を前記メインプロセッサに送信する。

本発明によれば、ロボットに組み込まれたセンサユニットの特性（あるいは機能）を即座に変更することができる。

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

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

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

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

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

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

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

図２Ａにおいて、１制御周期は例えば１０ｍｓｅｃであり、この制御周期を送信周期と受信周期に時分割する。ＣＰＵ２２は、送信周期においてセンサ１５からのセンサ出力をロボットＣＰＵ１２にシリアルで送信する。送信データフォーマットは送信パターン及び計測データを含み、送信パターンでどのセンサ出力を送信するかを設定し、計測データでセンサ１５からの実センサ出力を送信する。送信パターンを１６ビットで規定し、各ビットを例えば以下のように設定する。
最下位ビット（ＬＳＢ）：姿勢角（ロール角、ピッチ角、ヨー角）
第１位ビット：角速度
第２位ビット：加速度
第３位ビット：傾斜角
第４位ビット：重力補正後の加速度
第５位ビット：速度
第６位ビット：位置
第７位ビット：姿勢行列
第８位ビット：姿勢行列
第９位ビット：姿勢行列
第１０位ビット：姿勢行列
第１１位ビット：無効
第１２位ビット：ユニット温度
第１３位ビット：基板温度
第１４位ビット：ダイアグ
第１５位ビット：タイムカウント
各ビットが「１」のときに該当するデータが計測データとして送信される。例えば、ビット０（ＬＳＢ）が「１」のときにはセンサ１５からの姿勢角データが計測データとして送信される。ロボットＣＰＵ１２は、この送信パターンをデコードすることで、どのデータがＣＰＵ２２から送信されたか判別する。図において、ＣＰＵ２２から送信周期において送信されたデータは送信データ１００として示されている。

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

図３に、制御周期の受信周期に順次ロボットＣＰＵ１２からＣＰＵ２２に送信されるデータ列を示す。ロボットＣＰＵ１２は、センサユニット１０のパラメータを更新すべきタイミングにおいて書換コマンド２０２を送信する。ＣＰＵ２２は、この書換コマンド２０２を受信することでＲＡＭ１６の書き込み開始アドレス位置を第２領域の開始アドレスに設定する。そして、ロボットＣＰＵ１２からの受信データ２００を順次ＲＡＭ１６の第２領域に書き込んでいく。ＣＰＵ２２は、全ての受信データ２００を受信してＲＡＭ１６へのデータの書き込みを終了すると、ＲＡＭ１６からの読み出しアドレスを第１領域から第２領域に切替え、第２領域に記憶されているデータを新たなパラメータとして実行する。書換コマンドの一例は「ＳＥＴ」コマンドであり、ロボットＣＰＵ１２はＳＥＴコマンドに続いてデータ番号（設定箇所）、更新パラメータを設定してセンサユニット１０に送信する。センサユニット１０のＣＰＵ２２は、「ＳＥＴ」コマンドを解釈し、更新パラメータをＲＡＭ１６の第２領域に格納する。

図４乃至図９に、センサユニット１０の動作を時系列で示す。図４は、電源投入時の動作である。ＣＰＵ２２は、ＲＯＭ１８に記憶されているデフォルトのパラメータを読み出し、ＲＡＭ１６の第１領域１６ａにロードする。

図５は、動作時の処理である。ＣＰＵ２２は、ＲＡＭ１６の第１領域１６ａに記憶されたパラメータに従ってセンサ１５からのセンサ出力をロボットＣＰＵ１２にシリアル送信する。デフォルトのパラメータが送信パターンの最下位ビット、第１位ビット、第２位ビットを全て「１」とするパラメータである場合、ＣＰＵ２２はこのパラメータに従ってセンサ１５からの姿勢角データ、角速度データ、及び加速度データをロボットＣＰＵ１２に送信する。

図６は、パラメータ更新時の処理である。ＣＰＵ２２は、上記のようにＲＡＭ１６の第１領域１６ａに記憶されたデフォルトのパラメータに従ってセンサ出力を送信するが、制御周期のうちの受信周期においてロボットＣＰＵ１２から書換コマンド２０２を受信すると、それに続く受信データをＲＡＭ１６の第２領域１６ｂに書き込んでいく。受信データを第２領域１６ｂに書き込んでいる間、ロボットＣＰＵ１２に対するデータの送信は、デフォルトのパラメータに従って実行する。送信パラメータを最下位ビット、第１位ビット、第２位ビットで例示すると、ＲＡＭ１６の第１領域１６ａには送信パラメータ（デフォルトパラメータ）の「１１１」が記憶され、ＲＡＭ１６ｂの第２領域１６ｂには送信パラメータ（更新パラメータ）の「１００」が記憶される。

図７は、パラメータの更新終了後の処理である。ＣＰＵ２２は、ＲＡＭ１６の読み出し領域を第１領域１６ａから第２領域１６ｂに切替え、第２領域１６ｂに記憶された更新パラメータに従って処理を実行する。更新パラメータが上記のとおり「１００」である場合、ＣＰＵ２２は次の制御周期から加速度データをロボットＣＰＵ１２に送信し、姿勢角データと角速度データは送信しない。また、更新の内容が姿勢角のリセットである場合、ロボットＣＰＵ１２はリセット値を受信データ２００として設定し、更新コマンド２０４としてリセットコマンドを送信する。ＣＰＵ２２は、このリセットコマンドに応じてリセット値をＲＡＭ１６の第２領域１６ｂから読み出し、センサ１５をリセットする。

図８は、電源遮断時の動作である。ＣＰＵ２２は、ＲＡＭ１６の第２領域１６ｂに記憶された更新パラメータをＲＯＭ１８に書き込む。次回の電源投入時には、図４に示すように、ＲＯＭ１８に記憶された更新パラメータがデフォルトパラメータとして読み出され、ＲＡＭ１６の第１領域１６ａにロードされる。

一方、図７に示すように第２領域に記憶された更新パラメータに従って処理を実行している間に、ロボットＣＰＵ１２から別の更新パラメータが送信される場合もある。図９にこの場合の処理を示す。ＣＰＵ２２は、第２領域１６ｂに記憶された更新パラメータに従って処理している。このとき、受信周期で新たな書換コマンド及び更新パラメータを受信すると、新たな更新パラメータを今度は第１領域１６ａに順次格納していく。そして、第１領域１６ａに全ての更新パラメータを格納した後、ＣＰＵ２２はＲＡＭ１６の読み出し領域を第２領域１６ｂから再び第１領域１６ａに切替え、第１領域１６ａに記憶された更新パラメータに従って処理を実行する。このように、第１領域１６ａと第２領域１６ｂとを交互に使用することで、センサユニット１０の動作中においてもパラメータを随時変更できる。ＲＡＭ１６において動作すべきパラメータが記憶されている領域をアクティブ領域とすると、あるタイミングにおいて第１領域１６ａはアクティブ領域、第２領域１６ｂは非アクティブ領域であり、非アクティブ領域に更新パラメータを格納し、全ての更新パラメータを格納して準備が完了した後に、第２領域１６ｂをアクティブ領域、第１領域１６ａを非アクティブ領域に切替え、以後、同様の処理を繰り返す。アクティブ領域と非アクティブ領域の切替は、動作に必要な全ての更新パラメータを格納した直後であり、直後の送信タイミングから更新パラメータでのデータ送信が行われる。なお、ＣＰＵ２２が演算処理の実行中でジョブを割り当てられない、あるいはＲＳ−２３２Ｃが通信中等で時間に余裕がない場合には、以降の送信フレームから更新パラメータでのデータ送信が行われる。非アクティブ領域はロボットＣＰＵ１２から新たな更新パラメータが送信されない限り非アクティブ状態を維持するが、アクティブ領域に記憶されたパラメータをこの領域にコピーし予備領域として使用してもよい。

以上説明したように、本実施形態では、ロボットが動作中であっても、ロボットＣＰＵ１２が制御周期内の受信周期においてセンサユニット１０に更新パラメータ及び更新コマンドを送信し、センサユニット１０は更新パラメータをＲＡＭ１６の異なる領域に格納し、更新コマンドの受信をトリガとしてデフォルトのパラメータから更新パラメータに切り替えて処理を行うことで、センサユニット１０の特性あるいは機能を容易に、かつ迅速に変更することができる。

変更が繰り返される時、以前のパラメータ変更履歴を維持するために、新しいアクティブ領域へメインプロセッサの動作領域が変更された後、直ちにその内容が非アクティブ領域にコピーされる。コピーされた非アクティブ領域に、次のパラメータ変更が追加され書き込まれることにより、パラメータ変更の履歴を保持する。ここで、非アクティブ領域へのアクティブ領域データのコピーは全部をコピーする必要は無く、変化分のみをコピーすることにより、動作時間を短くし、同じ効果を発揮させる事ができ、望ましい。

なお、ロボット制御等においては、センサ内部演算速度は非常に早く、時間に対して正確に演算する必要がある。例えば、角速度から角度を演算する場合、角速度センサからの出力を積算して角度を得るが、離散データを処理するため積算周期Δｔが正確で、ｎ、ｎ＋１、ｎ＋２・・と順次正確に行われる必要がある。一方、センサデータを出力あるいは設定変更を受け取る通信は時間がかかり、かつデータ内容や通信環境により変動する。従って、メインプロセッサからのパラメータ設定受信に同期してセンサ側が演算処理内容を変更すると、演算周期や演算タイミングが乱れ、データの正確性を失う。例えば、Δｔが一定でなくなるとか、ｎ、ｎ＋１、ｎ＋３、・・といったデータ処理の飛びが生じる。本発明によれば、パラメータ変更による実動作のタイミングがセンサ内部のタイミングにより決定できるので、演算周期や演算タイミングを一定に保つことができる。また、パラメータ変更中はパラメータが確定しないので演算を待たせる必要があるが、本発明による方法では、パラメータ変更が確定するまで、直前の確定した第１領域１６ａにあるパラメータを用いて内部演算を実行し、変更中のパラメータは第２領域１６ｂに書き込む。次の演算タイミングに、変更パラメータが確定した第２領域１６ｂを用いることにより、演算時間の遅延や休止を防ぎ、連続した演算を可能とするので、実時間制御に適したセンサ出力を得ることができる。

本発明は、上記の実施形態に限定されるものではなく、種々の態様が可能である。例えば、本実施形態では、更新パラメータのデータ量を考慮し、更新パラメータをパケットに分割して複数の制御周期にわたって各パケットを送信しているが、１つの制御周期内の受信周期において更新パラメータを送信してもよい。

また、本実施形態では、図３に示すように、更新パラメータ（受信データ２００の内容は更新パラメータである）の前後に書換コマンド２０２を送信しているが、書換コマンド２０２を送信しなくてもよい。ＣＰＵ２２は、受信周期にロボットＣＰＵ１２から送信されたデータが存在すればＲＡＭ１６の第２領域１６ｂにデータを書き込んでいくことでパラメータをデフォルトから更新パラメータに切り替える。

また、本実施形態では、図８に示すように電源遮断時にＲＡＭ１６の第２領域１６ｂに記憶された更新パラメータをＲＯＭ１８に書き込んでいるが、ロボットＣＰＵ１２から送信された書込コマンドを受信したときに更新パラメータをＲＯＭ１８に書き込んでもよい。

本実施形態におけるロボット制御システムでは、ロボット動作中のセンサユニット１０の特性あるいは機能をほぼリアルタイムで実現できる。姿勢角のリセットを定期的、あるいはロボット停止動作中に実行する場合、ロボットＣＰＵ１２は定期的、あるいはロボット停止動作時においてリセットコマンドを制御周期の受信周期に送信する。ＣＰＵ２２はリセットコマンドを受信し、センサ１５の姿勢角出力を０にリセットする。センサユニット１０にロボット停止動作中の自動補正機能が備えられており、当該機能を有効とするか無効とするかをパラメータで設定する場合、ロボットＣＰＵ１２はユーザからの指定により更新パラメータを送信する。ＣＰＵ２２は、デフォルトのパラメータ（機能無効）を更新パラメータ（機能有効）に切替え、以後はロボット停止を検出する毎にセンサ出力のゼロ点等を自動補正する。

本発明の実施形態に係るロボット制御システムの概念構成図である。 データ送受信のタイミングチャートである。 データ送受信のタイミングチャートである。 ＣＰＵ２２の受信データ説明図である。 センサユニットの動作説明図（その１）である。 センサユニットの動作説明図（その２）である。 センサユニットの動作説明図（その３）である。 センサユニットの動作説明図（その４）である。 センサユニットの動作説明図（その５）である。 センサユニットの動作説明図（その６）である。

Claims (9)

1. ロボットのメインプロセッサと、
   センサ出力を前記メインプロセッサに送信するセンサユニットと、
   を有するロボット制御システムであって、
   前記センサユニットは、
   プロセッサと、
   センサと、
   前記プロセッサの動作を規定するパラメータを記憶するための第１領域及び第２領域を備えるメモリと、
   を有し、
   前記プロセッサは、所定周期内の送信周期において前記メモリの前記第１領域あるいは前記第２領域の一方に記憶されている前記パラメータを用いて前記センサ出力を前記メインプロセッサに送信するとともに前記所定周期内の残りの受信周期において前記メインプロセッサからの更新パラメータを受信して前記第１領域あるいは前記第２領域の他方に格納し、その後、前記更新パラメータを用いて前記センサ出力を前記メインプロセッサに送信する
   ことを特徴とするロボット制御システム。
2. 請求項１記載のシステムにおいて、
   前記プロセッサは、複数の所定周期にわたって前記更新パラメータのパケットを順次受信して前記メモリに順次格納するロボット制御システム。
3. 請求項１記載のシステムにおいて、
   前記プロセッサは、前記メモリに記憶された前記更新パラメータを不揮発性メモリに格納し、次のブート時に前記更新パラメータを読み出して前記メモリに格納するロボット制御システム。
4. 請求項１記載のシステムにおいて、
   前記メインプロセッサは、前記更新パラメータを前記センサユニットに送信する前に、書換コマンドを前記センサユニットに送信するロボット制御システム。
5. 請求項１記載のシステムにおいて、
   前記プロセッサは、前記更新パラメータの格納中は、前記第１領域あるいは前記第２領域の一方に記憶されている前記パラメータを用いて前記センサ出力を前記メインプロセッサに送信するロボット制御システム。
6. 請求項１記載のシステムにおいて、
   前記プロセッサは、前記更新パラメータの格納が完了した後、前記メインプロセッサに送信する前記センサ出力の読み出し領域を前記第１領域あるいは前記第２領域の他方に切り替えるロボット制御システム。
7. 請求項６記載のシステムにおいて、
   前記センサ出力の読み出し領域を切り替える際、前記更新パラメータを前記第１領域あるいは前記第２領域の一方へコピーするロボット制御システム。
8. 請求項３記載のシステムにおいて、
   前記プロセッサは、電源遮断時に前記更新パラメータを前記不揮発性メモリに格納するロボット制御システム。
9. ロボットのメインプロセッサと、センサ出力を前記メインプロセッサに送信するセンサユニットと、を有し、前記センサユニットは、プロセッサと、前記プロセッサの動作を規定するパラメータを記憶するための第１領域及び第２領域を備えるメモリと、を有するロボットの制御方法であって、
   所定周期内の送信周期において前記メモリの前記第１領域あるいは前記第２領域の一方に記憶されている前記パラメータを用いて前記センサ出力を前記メインプロセッサに送信し、
   前記所定周期内の残りの受信周期において前記メインプロセッサからの更新パラメータを受信して前記第１領域あるいは前記第２領域の他方に格納し、
   前記更新パラメータの格納後、前記更新パラメータを用いて前記センサ出力を前記メインプロセッサに送信する
   ことを特徴とするロボットの制御方法。

Images