JP2023110178A - 全方位移動体の軌道生成装置、軌道生成方法及びプログラム - Google Patents

Abstract

【課題】より高速に時間軌道を算出できること。【解決手段】全方位移動体の軌道生成装置は、全方位移動体の現在の関節状態に基づき導出されたヤコビアンの式に基づいて、関節パラメータの最大加速度を算出する制約算出手段と、関節パラメータの最大加速度を含む加速度制限ベクトルに基づいて、軌道パラメータｓ及びｓ（ドット）の平面上において前進積分及び後退積分を行うことで最適化処理を行い時間軌道を算出する軌道算出手段と、を備える。軌道算出手段は、軌道パラメータｓの平面上において、関節位置軌道の始点と始点の次の点との間に、関節の進行方向上に仮想関節位置を置き、関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、軌道パラメータｓの平面上における始点（０、ｖｎｏｒｍ）から前進積分を行いつつ、関節位置軌道の終点から前進積分と衝突するまで後退積分を行う。【選択図】図４

Description

特許法第３０条第２項適用申請有り 令和３年９月８日 日本ロボット学会学術講演会予稿集にて発表 〔刊行物等〕 令和３年９月１０日 第３９回 日本ロボット学会学術講演会にて発表

本発明は、全方位に移動可能な全方位移動体の時間軌道を生成する全方位移動体の軌道生成装置、軌道生成方法及びプログラムに関するものである。

全方位移動体の運動学に基づいて制約条件を算出し、該算出した制約条件下で時間軌道を算出する軌道生成装置が知られている（例えば、特許文献１参照）。

特許第６４６８１２７号公報

Tobias Kunz and Mike Stilman, Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. Robotics: Science and Systems. 2012.

上記軌道計画装置は、軌道計画する際に関節の現在速度を利用することができず、また、全方位移動体の運動学をそのまま用いているため計算速度が遅く、時間軌道の算出に多くの時間を要する虞がある。

本発明は、このような問題点に鑑みてなされたものであり、より高速に時間軌道を算出できる全方位移動体の軌道生成装置、軌道生成方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、
複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体の軌道生成装置であって、
前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出する制約算出手段と、
前記制約算出手段により算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出する軌道算出手段と、
を備え、
前記軌道算出手段は、前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
全方位移動体の軌道生成装置
である。
この一態様において、前記制約算出手段は、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度及び最大速度を夫々算出し、
前記軌道算出手段は、前記制約算出手段により算出された、前記関節パラメータの最大加速度を含む加速度制限ベクトル、及び、前記関節パラメータの最大速度を含む速度制限ベクトルに基づいて、前記軌道パラメータｓの平面上における前進積分及び後退積分を行うことで最適化処理を行い時間軌道を算出してもよい。
この一態様において、前記制約算出手段は、前記ヤコビアンの式を用いて、前記台車部の位置（ｘ、ｙ）の加速度方向が前記台車部の進行方向で、（ｘ、ｙ）の加速度の絶対値が大きくなるように右駆動輪の加速度α_Ｒ、及び左駆動輪の加速度α_Ｌを夫々探索し、
予め設定された前記旋回軸の加速度α_Ｓの範囲と、前記探索したα_Ｒ及びα_Ｌとに基づいて、前記本体部の方向φの最大加速度を算出し、前記探索したα_Ｒ及びα_Ｌに基づいて、（ｘ、ｙ）の最大加速度を算出し、関節パラメータ（ｘ、ｙ、φ）の最大加速度として、前記軌道算出手段に出力してもよい。
この一態様において、前記軌道算出手段は、前記軌道パラメータｓの平面上において、前記関節位置軌道の始点と前記次の点との間の距離が小さい位置から順に、前記前進積分及び後退積分が成功するまで探索し、前記仮想関節位置の位置を決定してもよい。
上記目的を達成するための本発明の一態様は、
複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体の軌道生成方法であって、
前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出するステップと、
前記算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出するステップと、
を含み、
前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
全方位移動体の軌道生成方法
であってもよい。
上記目的を達成するための本発明の一態様は、
複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体のプログラムであって、
前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出する処理と、
前記算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出する処理と、
をコンピュータに実行させ、
前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
全方位移動体のプログラム
であってもよい。

本発明は、このような問題点に鑑みてなされたものであり、より高速に時間軌道を算出できる全方位移動体の軌道生成装置、軌道生成方法及びプログラムを提供することができる。

本実施形態に係る全方位移動体の概略的構成を示す図である。 本実施形態に係る全方位移動体の概略的なシステム構成を示すブロック図である。 本実施形態に係る軌道生成装置の概略的なシステム構成を示すブロック図である。 本実施形態に係る最適化処理部の概略的なシステム構成を示すブロック図である。 時間軌道の一例を示す図である。 本実施形態に係る軌道生成方法のフローを示すフローチャートである。 全身動作の様子を示す図である。 各関節への指令速度の変化を示す図である。 各関節への指令加速度の変化を示す図である。 台車部の動作時における実際の台車座標系での台車速度を示す図である。

実施形態１
以下、図面を参照して本発明の実施形態について説明する。図１は、本発明の実施形態に係る全方位移動体の概略的構成を示す図である。本実施形態に係る全方位移動体１は、例えば、アクティブキャスタ式の全方位移動ロボットとして構成されている。

全方位移動体１は、移動を行う台車部２と、台車部２の上方側に旋回動可能に連結された本体部３と、を有している。台車部２は、一つの補助輪２１と左右一対の駆動輪２２が設けられている。本体部３は、台車部２に対して、旋回軸（Ｙａｗ回転軸）４を中心にして相対回動する。駆動輪２２の駆動輪軸と本体部３の旋回軸４とはオフセットしている。

全方位移動体１は、駆動輪軸と旋回軸４とを独立して駆動することで、全方位移動を実現する。全方位移動体１は、例えば、駆動輪軸による駆動輪２２の回転差に加えて、旋回軸４による本体部３の相対回動によって全方位移動を実現する。本体部３には、例えば、物体を把持できる多関節型アームが回動可能に設けられている。全方位移動体１は、安全かつ小型で物理的作業を行う生活支援ロボットＨＳＲ(Human Support Robot)として構成されていてもよい。

図２は、本実施形態に係る全方位移動体の概略的なシステム構成を示すブロック図である。本実施形態に係る全方位移動体１は、左右の駆動輪２２の回転情報(回転角、回転速度、回転角加速度)を検出する一対の回転センサ５、旋回軸４の回転情報を検出する回転センサ６、一対の駆動輪２２を駆動する一対の車輪アクチュエータ７と、旋回軸４を駆動する旋回アクチュエータ８と、車輪アクチュエータ７および旋回アクチュエータ８を制御する制御装置９と、軌道生成装置１０と、を備えている。制御装置９は、回転センサ５、６により検出される回転情報に基づいて、車輪アクチュエータ７及び旋回アクチュエータ８を制御する。

各回転センサ５は、例えば、左右の駆動輪２２の駆動輪軸に夫々設けられている。各回転センサ５は、ポテンショメータ、エンコーダなどで構成されている。各車輪アクチュエータ７は、例えば、左右の駆動輪２２の駆動輪軸に連結されている。各車輪アクチュエータ７は、モータなどで構成されている。

旋回アクチュエータ８は、例えば、旋回軸４に連結されている。各旋回アクチュエータ８は、モータなどで構成されている。車輪アクチュエータ７及び旋回アクチュエータ８は、制御装置９から出力される制御信号に応じて回転駆動する。

制御装置９は、上述した駆動輪２２及び旋回軸４の加速度の制約を満たしつつ、設定された指令軌道に追従する時間制御を実行する。制御装置９は、軌道生成装置１０により算出された時間軌道に基づいて、車輪アクチュエータ７及び旋回アクチュエータ８を制御する。

軌道生成装置１０は、時間制御を実行するための時間軌道を生成する。なお、軌道生成装置１０と制御装置９とは一体的に構成されていてもよい。また、本実施形態において、軌道生成装置１０は、全方位移動体１に搭載されている構成であるが、これに限定されない。軌道生成装置１０は、全方位移動体１外に設けられていてもよい。この場合、軌道生成装置１０は、算出した時間軌道を、無線通信などを介して、制御装置９に送信してもよい。

軌道生成装置１０は、例えば、ＣＰＵ（Central Processing Unit）やＧＰＵ（Graphics Processing Unit）などのプロセッサと、ＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory）などの内部メモリと、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）などのストレージデバイスと、ディスプレイなどの周辺機器を接続するための入出力Ｉ／Ｆと、装置外部の機器と通信を行う通信Ｉ／Ｆと、を備えた通常のコンピュータのハードウェア構成を有する。

本実施形態に係る軌道生成装置１０は、全方位移動体１の拘束条件 (目標姿勢、障害物情報等) を、動的に変更可能なマニピュレーションを実現するために、周期的な全身軌道計画を行うものである。

図３は、本実施形態に係る軌道生成装置の概略的なシステム構成を示すブロック図である。軌道生成装置１０は、軌道候補群を生成する候補生成部１１と、評価関数に基づいて軌道候補群をソートする候補評価部１２と、軌道候補群と障害物の衝突を判定する衝突判定部１３と、最適化処理を行い軌道候補へ時間情報を付与する最適化処理部１４と、を有している。

候補生成部１１は、例えば、Ｎ個の軌道候補を生成し、候補評価部１２に対して出力する。候補評価部１２は、各軌道候補を評価関数を用いて評価し、その評価結果に基づいて、各軌道候補をソートし、衝突判定部１３に出力する。衝突判定部１３は、各軌道候補で全方位移動体１が移動した場合に障害物と衝突するか否かを夫々判定し、衝突しないと判定した軌道候補を最適化処理部１４に出力する。なお、軌道生成装置１０は、これら全ての処理が一定の時間で完了するように制御を行う。

本実施形態に係る軌道生成装置１０は、上述の如く、軌道候補群の生成、評価、成立性検証、及び動作時間最適化のループを回すことで、リアルタイムに軌道を生成し、環境変化に対してロバストな制御を行うことができる。

上述した、周期的な全身軌道計画で利用される最適化処理部１４に対して、例えば、次の３つの要求が存在する。第１に、周期的な全身軌道計画に間に合うように計算時間が短いことが要求される。第２に、全方位移動体１の動作中に、新たな軌道の時間最適化が行われるため、現在の関節位置及び速度を考慮した時間最適化が可能であることが要求される。第３に、周期的な全身軌道計画では障害物回避等を考慮して多点軌道を利用しているため、多点軌道の時間最適化が可能であることが要求される。

本実施形態に係る最適化処理部１４は、上述した第１乃至第３の要求を満たすように、処理が高速な現在の関節位置及び速度を考慮した多点軌道の時間最適化を実行する。このため、最適化処理部１４は、時間情報を持たない軌道に対して、アクチュエータの加速度制約を考慮して、より短時間で動作するよう最適化処理を実行するものである。

図４は、本実施形態に係る最適化処理部の概略的なシステム構成を示すブロック図である。本実施形態に係る最適化処理部１４は、加速度制約を算出する制約算出部１４１と、時間軌道を算出する軌道算出部１４２と、を有している。

ところで、全方位移動体１を、複数の関節を有するロボットとして考えることができる。すなわち、全方位移動体１の駆動輪２２及び旋回軸４による本体部３の自由度は、仮想的なロボットの関節として仮定することができる。その仮想的なロボットの関節において、加速度制約が存在する。この加速度制約は、例えば、上限値（最大加速度）で与えられる。

制約算出部１４１は、上述した、全方位移動体１の加速度制約を算出する。制約算出部１４１は、制約算出手段の一具体例である。

次に、本実施形態に係る全方位移動体１の関節の加速度制約である最大加速度の算出方法について詳細に説明する。最初に、各パラメータを以下のように設定する。

（ｘ、ｙ、φ）：全方位移動体１の座標系（以下、全方位移動体座標系と称す）における位置及び姿勢を示す関節パラメータ
なお、ｘ、ｙは、台車部２の位置であり、φは本体部３の向いている方向である。
Ｗ： 駆動輪間距離
ｒ：駆動輪半径
ω: 関節速度
ｓ: 旋回軸と駆動輪軸の距離
α: 関節加速度
θ_Ｖ: 旋回軸角度
Ｒ、Ｌ、Ｓ: 右駆動輪、 左駆動輪、 旋回軸

上記パラメータを用いて、全方位移動体１の現在の関節状態に基づいて、ヤコビアンJを求めると、下記（１）式及び（２）式が導出される。

さらに、全方位移動体座標系で加速度を考え、全方位移動体１のヤコビアンJの微分を求めると、下記（３）式及び（４）式が導出される。

制約算出部１４１は、上記式（３）を用いて、（ｘ、ｙ）の加速度方向が入力方向で、（ｘ、ｙ）の加速度の絶対値が大きくなるように右駆動輪２２の加速度α_Ｒ、及び左駆動輪２２の加速度α_Ｌを探索する。

上記「ｘ、ｙの加速度方向が入力方向」とは、入力軌道での台車部２の最初の移動方向を指す。具体的には、現在の台車部２の位置から見た入力軌道のゼロ点目の台車部２の位置の方向を指す。なお、入力軌道は、例えば、再生したい全方位移動体１の全身軌道であり、衝突判定部１３から制約算出部１４１に予め入力される。

左右駆動輪２２の加速度α_Ｒ、α_Ｌの範囲を２次元座標系にプロットした場合、その縁上に、目標とする左右駆動輪２２の加速度α_Ｒ、α_Ｌは存在する。

制約算出部１４１は、具体的に以下の（ｉ）乃至（ｉｉｉ）の処理を行う。左右駆動輪２２の加速度α_Ｒ、 α_Ｌの最大値をα_ｍａｘとする。
（ｉ）制約算出部１４１は、左右駆動輪２２の加速度α_Ｒ、 α_Ｌの組み合わせ（例えば、約２００通りとする）でのｘ、ｙの加速度を計算する。
組合わせ：（±α_ｍａｘ、（ｋ－２５）／２５×α_ｍａｘ）、 （（ｋ－２５）／２５×α_ｍａｘ、 ±α_ｍａｘ)（ｋ＝０、１、 …、５０）
（ｉｉ）制約算出部１４１は、夫々のｘ、ｙの加速度に基づいて、下記の式を用いてｘ、ｙの加速度方向を算出する。
ａｒｃｔａｎ(ｙの加速度／ｘの加速度)
（ｉｉｉ）制約算出部１４１は、加速度方向が入力方向と最も近い左右駆動輪２２の加速度α_Ｒ、 α_Ｌを選択する。

なお、ｘ、ｙの加速度は、α_Ｒ、 α_Ｌに対して線形である。このため、ｘ、ｙの加速度の大きさ（(ｘの加速度)^２ ＋（ｙの加速度）^２）^１／２が最大となるα_Ｒ、 α_Ｌは、今回探索したα_Ｒ、 α_Ｌの範囲の縁上に存在することとなる。

制約算出部１４１は、全方位移動体座標系での進行方向に基づいて、α_Ｓの範囲としてα_Ｓの下限を使うかα_Ｓの上限を使うかを決定する。具体的には、制約算出部１４１は、現在の台車部２の向いている方向から見た、入力軌道のゼロ点目の台車部２の向くべき方向が正である場合、α_Ｓの範囲をα_Ｓの上限で設定し、その方向が正である場合α_Ｓの範囲をα_Ｓの下限で設定する。

なお、α_Ｓは、旋回軸４の加速度であり、例えば、旋回軸４の旋回アクチュエータ８の出力、ギア比、本体部３の質量等の設計値に基づいて、設定されている。

制約算出部１４１は、設定したα_Ｓの範囲（下限又は上限）と、上記探索したα_Ｒ、α_Ｌとを上記式（３）に代入して、φの最大加速度を算出する。また、制約算出部１４１は、上記探索したα_Ｒ、及びα_Ｌに基づいて、ｘ、ｙの最大加速度を算出する。

制約算出部１４１は、上記探索したα_Ｒ及びα_Ｌとα_Ｓの上限とに基づいて算出したφの最大加速度と、上記探索したα_Ｒ及びα_Ｌとα_Ｓの下限とに基づいて算出したφの最大加速度と、を比較し、小さい方を、最終的なφの最大加速度としてもよい。これは、後述の最大加速度をより小さくすることで、安全性を向上させるためである。

制約算出部１４１は、オフラインで、上記加速度制約を算出しているが、これに限定されず、オンラインで上記加速度制約を算出してもよい。制約算出部１４１は、上述のように算出した加速度制約である関節パラメータ（ｘ、ｙ、φ）の最大加速度を軌道算出部１４２に出力する。

ここで、ロボットが動作すべき指令軌道が空間軌道として与えられる場合を考える。この場合、空間軌道は、一つの軌道パラメータｓ（時間をパラメータ化したもの）で表現できるものとする。

ロボットの関節角は、その空間軌道上の１点が指定されれば、決定される。また、ロボットのある状態の速度も、空間軌道上の対応点における、軌道パラメータｓの速度ｓ（以下、ｓドットと称す）で表現できる。

軌道算出部１４２は、上記に基づき、制約算出部１４１により算出された加速度制約を満たし、かつ、指令軌道を追従する軌道であって、軌道パラメータを横軸とし、その微分値を縦軸とした平面（以下、軌道パラメータｓ－ｓ（ドット）平面と称す）上に表現された時間軌道を算出する。

軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面上で軌道を探索して、時間軌道を算出することで時間制御を実行する。軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面における始点および終点から夫々出発して、所定の積分幅で積分を繰り返して時間軌道を算出する。軌道算出部１４２は、始点側から終点側へ前進で積分（以下、前進積分）を行い、終点側から始点側へ後退で積分（以下、後退積分）を繰り返して時間軌道を算出する。

続いて、本実施形態に係る軌道算出部１４２による時間軌道の算出方法について詳細に説明する。図５は、時間軌道の一例を示す図である。

なお、本実施形態に係る時間軌道の算出方法は、周知の非特許文献１（Tobias Kunz and Mike Stilman, Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. Robotics: Science and Systems. 2012.）の手法を以下の１）乃至３）の点で拡張することで、任意の現在速度を利用可能とし、処理が高速な多点軌道の最適化を行うことができる。

１）関節位置軌道の始点Ｐ０と次の点Ｐ１に間に、Ｐ０からｖ方向（関節の進行方向）に仮想関節位置Ｐｖを置く。
２）関節の現在速度をｖ_ｎｏｒｍとし、軌道パラメータｓ－ｓ（ドット）平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行う。
３）Ｐ０とＰｖとの間の距離が小さい位置から順に、前進積分及び後退積分が成功するまで探索し、Ｐｖの位置を決定する。

以下、本実施形態に係る軌道算出部１４２による時間軌道の算出方法についてより詳細に説明する。なお、以降の処理に関して、部分的に非特許文献１に開示されており、これを適宜援用できるものとする。

最初に、軌道算出部１４２には、下記パラメータ値が入力される。
・ 各関節位置軌道 ［Ｐ０、 Ｐ１、 Ｐ２、 ・・・ Ｐｎ］
（各点はｋ次元）
・各関節の速度制限ベクトルＶＭａｘ (ｋ次元)
・各関節の加速度制限ベクトルＡＭａｘ (ｋ次元)
・各関節の現在速度ｖ

軌道算出部１４２は、各関節の現在速度ｖを、全方位移動体１のセンサ（駆動輪２２の回転センサ５、旋回軸４の回転センサ６など）から取得する。各関節の速度制限ベクトルＶＭａｘ、各関節の加速度制限ベクトルＡＭａｘ、各関節位置軌道は、軌道算出部１４２に予め設定される。

各関節位置軌道は、実現したい全方位移動体１の動作に基づいて生成される。各関節位置軌道は、例えば、ユーザの手作業で作成されてもよく、ＲＲＴ（Rapidly-exploring Random Tree）などの軌道計画アルゴリズムを利用して生成されてもよい。各関節位置軌道は、例えば、候補生成部１１により上記のように作成され、最終的に、衝突判定部１３から出力される軌道候補である。

軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面上において、Ｐ０とＰ１との間に、Ｐ０からｖ方向に仮想関節位置Ｐｖを置く。このとき、軌道算出部１４２は、Ｐ０とＰｖとの間の距離が小さい位置から探索を行う。このように、距離が小さい位置から探索を行うことで、より短い時間軌道を生成できる。

なお、現在速度ｖに対してＰ０とＰｖ間の距離が小さ過ぎると、上記前進積分がＰ０、Ｐｖ、Ｐ１間で失敗する可能性がある。しかし、その前進積分が失敗した場合でも、前進積分の最初の方で失敗するので、計算時間への影響は小さくて済む。

軌道算出部１４２は、上述の如く、前進積分に失敗した場合、仮想関節位置ＰｖをＰ０から所定量だけ遠ざけて配置し、再度、前進積分を行う。軌道算出部１４２は、仮想関節位置Ｐｖの探索を前進積分が成功するまで繰り返す。

軌道算出部１４２は、関節位置軌道を、軌道長ｓで表現可能な直線セグメントと円弧セグメントへ変換する。

軌道算出部１４２は、例えば、図５に示す如く、Ｐ０からＰｖを直線セグメントａに変換し、ＰｖからＰ１を直線セグメントｂに変換し、Ｐ１からＰ２を直線セグメントｃに変換し、２つの直線セグメントａ、ｂとの間を連続な円弧セグメントｄに変換し、２つの直線セグメントｂ、ｃとの間を連続な円弧セグメントｅに変換する。

関節の現在速度をｖ_ｎｏｒｍとする。
軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行う。また、軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面上における終点（Ｌ、０）から前進積分と衝突するまで後退積分を行う。

ここで、上記前進積分及び後退積分による時間最適化は、各関節の加速度制限ベクトルＡＭａｘの制限下で行われる。すなわち、軌道算出部１４２は、加速度制限ベクトルＡＭａｘに基づいて、軌道パラメータｓ－ｓ（ドット）平面上において、上述の前進積分及び後退積分を行う。なお、本処理は、例えば、上記非特許文献１の（２１）式、（２２）式、（３６）式などを利用しており、これを援用できる。

ＡＭａｘは、上述の如く、各関節の加速度制限ベクトルであり、関節数ｋが要素数となるｋ次元のベクトルである。本実施形態においは、全方位移動体１の位置及び姿勢を示す関節パラメータ（ｘ、ｙ、φ）を夫々関節と見なすことができる。ＡＭａｘには、アーム関節及び台車部関節の最大加速度の情報が含まれる。台車部関節の最大加速度は、制約算出部１４１により算出された関節パラメータ（ｘ、ｙ、φ）の最大加速度である。アーム関節の最大加速度は、アクチュエータやギア比等で定まる固定値として設定される。本実施形態において、例えば、アーム関節は５関節存在し、台車部関節は３関節存在することから、ＡＭａｘは８次元のベクトルとなる。

軌道算出部１４２は、上述の如く、加速度制限ベクトルＡＭａｘに基づいて、軌道パラメータｓ－ｓ（ドット）平面上において、前進積分及び後退積分を行うことで最適化処理を行い、軌道パラメータｓ－ｓ（ドット）平面上における時間軌道を算出する。軌道算出部１４２は、上述の如く、前進積分及び後退積分が成功し、最適化処理が成功した場合、算出した時間軌道を制御装置９に出力する。

制御装置９は、軌道算出部１４２により算出された時間軌道に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御する。制御装置９は、時間軌道に基づいて、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）を算出する。制御装置９は、算出した全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御する。

全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）は、例えば、以下のように算出される。上記最適化処理（前進積分及び後退積分）の結果、ｓ＝０からｓ＝Ｌまでの、幅Δsごとの、ｓとｓ（ドット）の組が得られる。得られたｓとｓ（ドット）の組と１つ前のｓとｓ（ドット）の組とを比較することで、ｓとｓ（ドット）の組の時刻ｔが算出される。ｑ ＝ｆ（ｓ）の関数ｆは、上述の如く、関節位置軌道を軌道長ｓで表現可能な直線セグメントと円弧セグメントへ変換した際に導出される。ｓとｓ（ドット）、ｔの組が分かっているので、関節位置ｑとｔの関係が分かる。

続いて、本実施形態に係る軌道生成方法について、説明する。図６は、本実施形態に係る軌道生成方法のフローを示すフローチャートである。

制約算出部１４１は、全方位移動体１の現在の関節状態に基づいて導出されたヤコビアンの（３）式に基づいて、全方位移動体座標系における位置及び姿勢を示す関節パラメータ（ｘ、ｙ、φ）の最大加速度を夫々算出する（ステップＳ１０１）。

軌道算出部１４２は、軌道パラメータｓ－ｓ（ドット）平面上において、関節位置軌道の始点Ｐ０と次の点Ｐ１との間に、関節の進行方向ｖ上に仮想関節位置Ｐｖを置く（ステップＳ１０２）。

軌道算出部１４２は、関節位置軌道を軌道長ｓで表現可能な直線セグメントと円弧セグメントとに変換する（ステップＳ１０３）。

軌道算出部１４２は、関節の現在速度をｖ_ｎｏｒｍとして、軌道パラメータｓ－ｓ（ドット）の平面上における始点を（０、ｖ_ｎｏｒｍ）とする（ステップＳ１０４）。

軌道算出部１４２は、制約算出部１４１により算出された関節パラメータ（ｘ、ｙ、φ）の最大加速度を含むと共に全方位移動体１の関節数ｋを次元数とする加速度制限ベクトルＡＭａｘに基づいて、軌道パラメータｓ－ｓ（ドット）平面上において始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ終点から後退積分を行うことで、最適化処理を行い、時間軌道を算出する（ステップＳ１０５）。

以上、本実施形態に係る軌道生成装置１０によれば、全方位移動体座標系基準での加速度制約計算、仮想関節位置Ｐｖの挿入、高速な時間最適化処理を繰り返すことで、全方位移動体１の動作時間を短くすることができる。なお、計算した全方位移動体座標系基準での加速度制約は、全方位移動体１の動作によって役に立たない値となってしまうが、繰り返し計算を行うことで、そのデメリットを打ち消すことができる。

また、本実施形態に係る軌道生成装置１０において、台車軌道を車輪軌道、車輪軌道を台車軌道へ変換する必要がなく、時間最適化処理も速いため、全体の処理を高速化、さらに、仮想関節位置Ｐｖを挿入することで、その時点での全方位移動体１の全身速度を考慮した最適化が可能となる。

さらに、本実施形態に係る軌道生成装置１０は、上述の如く、ループ処理を実行することに加えて、全方位移動体１の全身速度を考慮していることから、動かすための全身軌道を動的に変更することができる。例えば、動作中に認識した障害物を避けるために、全身軌道が変更されるというケースにも十分に対応できる。

実施形態２
本実施形態において、制約算出部１４１は、全方位移動体１の加速度制約として関節パラメータ（ｘ、ｙ、φ）の最大加速度を算出すると共に、速度制約として関節パラメータ（ｘ、ｙ、φ）の最大速度を算出してもよい。軌道算出部１４２は、制約算出部１４１により算出された関節パラメータ（ｘ、ｙ、φ）の最大加速度を含む加速度制限ベクトルＡＭａｘ、及び、関節パラメータ（ｘ、ｙ、φ）の最大速度を含む速度制限ベクトルＶＭａｘに基づいて、軌道パラメータｓ－ｓ（ドット）平面上において、前進積分及び後退積分を行うことで最適化処理を行う。

制約算出部１４１は、上記加速度制約と同様に速度制約である関節パラメータ（ｘ、ｙ、φ）の最大速度を算出する。具体的には、制約算出部１４１は、上記式（１）を用いて、（ｘ、ｙ）の速度方向が入力方向で、（ｘ、ｙ）の速度の絶対値が大きくなる左右駆動輪２２の速度ω_Ｒ、及びω_Ｌを探索する。

制約算出部１４１は、具体的に以下の（ｉ）乃至（ｉｉｉ）の処理を行う。左右駆動輪２２の速度ω_Ｒ、 ω_Ｌの最大値をω_ｍａｘとする。

（ｉ）制約算出部１４１は、（ω_Ｒ、 ω_Ｌ）の組み合わせでのｘ、ｙの速度を計算する。
（ｉｉ）制約算出部１４１は、夫々のｘ、ｙの速度に基づいて、下記の式を用いてｘ、ｙの速度方向を算出する。
ａｒｃｔａｎ(ｙの速度／ｘの速度)
（ｉｉｉ）制約算出部１４１は、速度方向が入力方向と最も近い左右駆動輪２２の速度ω_Ｒ、 ω_Ｌを選択する。

なお、ｘ、ｙの速度は、ω_Ｒ、ω_Ｌに対して線形である。このため、速度の大きさ（(ｘの速度)^２ ＋（ｙの速度）^２）^１／２が最大となるω_Ｒ、ω_Ｌは、今回探索したω_Ｒ、ω_Ｌの範囲の縁上に存在することとなる。

制約算出部１４１は、全方位移動体座標系での進行方向に基づいて、ω_Ｓの範囲としてω_Ｓの下限を使うかω_Ｓの上限を使うかを決定する。具体的には、制約算出部１４１は、現在の台車部２の向いている方向から見た、入力軌道のゼロ点目の台車部２の向くべき方向が正である場合、ω_Ｓの範囲をω_Ｓの上限で設定し、その方向が正である場合ω_Ｓの範囲をω_Ｓの下限で設定する。

なお、ω_Ｓは、旋回軸４の速度であり、例えば、旋回軸４のアクチュエータの出力、ギア比、本体部３の質量等の設計値に基づいて、設定されている。

制約算出部１４１は、設定したω_Ｓの範囲（下限又は上限）と、上記探索したω_Ｒ、ω_Ｌとを上記式（１）に代入して、φの最大速度を算出する。また、制約算出部１４１は、上記探索したα_Ｒ、及びα_Ｌに基づいて、ｘ、ｙの最大速度を算出する。

制約算出部１４１は、上記探索したω_Ｒ及びω_Ｌとω_Ｓの上限とに基づいて算出したφの最大速度と、上記探索したω_Ｒ及びω_Ｌとω_Ｓの下限とに基づいて算出したφの最大速度と、を比較し、小さい方を、最終的なφの最大速度としてもよい。これは、後述の最大速度をより小さくすることで、安全性を向上させるためである。

制約算出部１４１は、算出した関節パラメータ（ｘ、ｙ、φ）の最大速度を軌道算出部１４２に出力する。

軌道算出部１４２は、制約算出部１４１により算出された関節パラメータ（ｘ、ｙ、φ）の最大加速度を含む加速度制限ベクトルＡＭａｘ、及び、関節パラメータ（ｘ、ｙ、φ）の最大速度を含む速度制限ベクトルＶＭａｘに基づいて、軌道パラメータｓ－ｓ（ドット）平面上において、前進積分及び後退積分を行うことで最適化処理を行い、時間軌道を算出する。

続いて、本実施形態に係る軌道生成装置１０により実験結果について説明する。本実施形態において、ＨＳＲの周期的な全身軌道計画の動作実験を行った。全身軌道計画による全身軌道の発行周期は５［Ｈｚ］とした。

まず、全身動作の最適化の実験として、手先位置及び姿勢を拘束条件として与え、それを連続的に変化させていく実験を行った。図７は、その全身動作の様子を示す図である。図７に示す如く、ＨＳＲは、与えられた手先位置・姿勢を満たそうと、その動作が止まることなく、手先が目標へ追従していることが分かる。

次に、本実施形態に係る最適化処理を評価するために、台車部２の移動に限った実験を行った。台車部２の移動には、ＨＳＲの周期的な全身軌道計画を用い、台車部２の位置及び姿勢の拘束条件の次のように更新している。
１．（ｘ、ｙ、φ）＝（０．０、０．０、０．０）にＨＳＲを配置する。
２.（ｘ、ｙ、φ）＝（１．０、０．０、０．０）を目標として与える。
３. 目標までの距離が０．２５［ｍ］以下になるのを待つ。
４.（ｘ、 ｙ、φ）＝（１．０、１．０、１．５７）を目標として与える。
５. 目標までの距離が０．２５［ｍ］以下になるのを待つ。
６.（ｘ、ｙ、φ）＝（１．０、０．０、０．０）を目標として与える。
７. 動作の完了を待つ。

関節速度及び加速度の上限は次の通りに設定している。
ω_Ｒ＝８．５［ｒａｄ/ｓｅｃ］、ω_Ｌ＝８．５［ｒａｄ/ｓｅｃ］、ω_Ｈ＝１．８［ｒａｄ/ｓｅｃ］、α_Ｒ＝５．０［ｒａｄ/ｓｅｃ^２］、α_Ｌ＝５．０［ｒａｄ／ｓｅｃ^２］、α_Ｈ ＝１．８［ｒａｄ/ｓｅｃ^２］．

これらの目標、パラメータ設定で台車部２の動作を実行すると、動作時間は１６．３［ｓｅｃ］ だった。一方で、従来の最適化モジュールでは、上記同様の指令値を入力すると、その動作時間は、１５．８［ｓｅｃ］だった。本実施形態に係る軌道生成装置１０の最適化処理による動作時間と、従来の最適化モジュールによる動作時間とには、大きな差は無いと言える。

一方で、従来の最適化モジュールによる計算時間は、平均７５［ｍｓｅｃ］に対し、本実施形態に係る軌道生成装置１０の最適化処理による計算時間は、最適化処理１回あたり平均 ５［ｍｓｅｃ］である。すなわち、本実施形態に係る軌道生成装置１０の最適化処理による計算時間は、従来の最適化モジュールよりも、圧倒的に計算時間が短いことが分かる。

次に、台車部２の動作のプロットについて説明する。図８は、各関節への指令速度（関節速度指令値）の変化を示す図である。図９は、各関節への指令加速度（関節加速度指令値）の変化を示す図である。

図８及び図９において、設定した関節速度及び加速度の最大値は、Ｍａｘで表記されている。図１０は、台車部の動作時における実際の全方位移動体座標系での台車速度を示す図である。図１０における２つの矢印は、目標台車位置及び姿勢が切り替わった点を示している。

指令速度は設定した上限を守っている。一方、指令加速度は、一時的に与えられた上限を超えていることがあった。ただし、実際の動作速度を見ると連続的に動作しており、指令値の一時的な超過による動作への影響はないと言える。すなわち、本実施形態に係る軌道生成装置１０の最適化処理によって、台車部２の動作に関しても問題無いことが分かる。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他のさまざまな形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

本発明は、例えば、図６に示す処理を、プロセッサにコンピュータプログラムを実行させることにより実現することも可能である。

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

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

上述した各実施形態に係る軌道生成装置１０を構成する各部は、プログラムにより実現するだけでなく、その一部または全部を、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）などの専用のハードウェアにより実現することもできる。

１ 全方位移動体、２ 台車部、３ 本体部、４ 旋回軸、５ 回転センサ、６ 回転センサ、７ 車輪アクチュエータ、８ 旋回アクチュエータ、９ 制御装置、１０ 軌道生成装置、１１ 候補生成部、１２ 候補評価部、１３ 衝突判定部、１４ 最適化処理部、２１ 補助輪、２２ 駆動輪、１４１ 制約算出部、１４２ 軌道算出部

Claims (6)

1. 複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体の軌道生成装置であって、
   前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出する制約算出手段と、
   前記制約算出手段により算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出する軌道算出手段と、
   を備え、
   前記軌道算出手段は、前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
   全方位移動体の軌道生成装置。
2. 請求項１記載の全方位移動体の軌道生成装置であって、
   前記制約算出手段は、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度及び最大速度を夫々算出し、
   前記軌道算出手段は、前記制約算出手段により算出された、前記関節パラメータの最大加速度を含む加速度制限ベクトル、及び、前記関節パラメータの最大速度を含む速度制限ベクトルに基づいて、前記軌道パラメータｓの平面上における前進積分及び後退積分を行うことで最適化処理を行い時間軌道を算出する、
   全方位移動体の軌道生成装置。
3. 請求項１又は２記載の全方位移動体の軌道生成装置であって、
   前記制約算出手段は、
   前記ヤコビアンの式を用いて、前記台車部の位置（ｘ、ｙ）の加速度方向が前記台車部の進行方向で、（ｘ、ｙ）の加速度の絶対値が大きくなるように右駆動輪の加速度α_Ｒ、及び左駆動輪の加速度α_Ｌを夫々探索し、
   予め設定された前記旋回軸の加速度α_Ｓの範囲と、前記探索したα_Ｒ及びα_Ｌとに基づいて、前記本体部の方向φの最大加速度を算出し、前記探索したα_Ｒ及びα_Ｌに基づいて、（ｘ、ｙ）の最大加速度を算出し、関節パラメータ（ｘ、ｙ、φ）の最大加速度として、前記軌道算出手段に出力する、
   全方位移動体の軌道生成装置。
4. 請求項１乃至３のうちいずれか１項記載の全方位移動体の軌道生成装置であって、
   前記軌道算出手段は、前記軌道パラメータｓの平面上において、前記関節位置軌道の始点と前記次の点との間の距離が小さい位置から順に、前記前進積分及び後退積分が成功するまで探索し、前記仮想関節位置の位置を決定する、
   全方位移動体の軌道生成装置。
5. 複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体の軌道生成方法であって、
   前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出するステップと、
   前記算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出するステップと、
   を含み、
   前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
   全方位移動体の軌道生成方法。
6. 複数の駆動輪が設けられた台車部と、該台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体のプログラムであって、
   前記全方位移動体の駆動輪及び旋回軸による本体部の自由度を関節として、前記全方位移動体の現在の関節状態に基づいて導出されたヤコビアンの式に基づいて、前記全方位移動体の座標系における位置及び姿勢を示す関節パラメータの最大加速度を夫々算出する処理と、
   前記算出された関節パラメータの最大加速度を含むと共に前記全方位移動体の関節数を次元数とする加速度制限ベクトルに基づいて、軌道パラメータｓ及びその微分値ｓ（ドット）の平面上において前進積分及び後退積分を行うことで、最適化処理を行い、時間軌道を算出する処理と、
   をコンピュータに実行させ、
   前記軌道パラメータｓの平面上において、関節位置軌道の始点と該始点の次の点との間に、前記関節の進行方向上に仮想関節位置を置き、前記関節位置軌道を軌道長で表現可能な直線セグメントと円弧セグメントとに変換し、前記関節の現在速度をｖ_ｎｏｒｍとして、前記軌道パラメータｓの平面上における始点（０、ｖ_ｎｏｒｍ）から前進積分を行いつつ、前記関節位置軌道の終点から前進積分と衝突するまで後退積分を行う、
   全方位移動体のプログラム。

Images