JP6416560B2 - 位置決め制御装置 - Google Patents

Description

本発明は、位置決め制御装置に関する。

特許文献１に記載のロボット制御装置では、ロボットアームにより位置決め対象物が把持され、使用者が操作装置を介してロボットアームの動作を制御することにより、位置決め制御が行われる。上記位置決め制御を介し、位置決め対象物は、当該位置決め対象物が到達すべき位置（目標位置）に配置され、目標位置にて当該位置決め対象物が向くべき向き（目標向き）に向けられる。

上記ロボット制御装置では、操作装置に、位置決め対象物の移動を制限する移動制限設定ボタンスイッチと位置決め対象物の向きの変更を制限する回転制限設定ボタンスイッチとが備えられている。使用者が移動制限設定ボタンスイッチを押下することで、ｘ軸、ｙ軸及びｚ軸の軸毎に位置決め対象物の移動が制限される。同様に、使用者が回転制限設定ボタンスイッチを押下することで、ｘ軸、ｙ軸及びｚ軸回りの回転毎に位置決め対象物の回転（すなわち、位置決め対象物の向きの変更）が制限される。なお、上記ロボット制御装置による位置決め制御において、位置決め対象物の移動及び向きの変更に対する制限は、手ブレ等による使用者の意図と反する位置決め対象物の移動及び向きの変更を防止するために用いられる。

特開２０１０−２６９４１９号公報

しかしながら、上記構成では、位置決め対象物を目標位置へ移動させる過程で位置決め対象物の無駄な移動が多くなり得る。また、位置決め対象物の向きを目標向きに向ける過程で位置決め対象物の無駄な向きの変更が多くなり得る。そのため、効率的に位置決め制御が行われない場合がある。

本発明は、効率的な位置決め制御を可能とする位置決め制御の技術を提供することを目的とする。

本発明の一側面は、位置決め制御装置であって、位置決め対象物と、第１の支持部と、第１の検出部と、第２の検出部と、取得部と、第１の許容部と、第２の許容部と、を備える。第１の支持部は、位置決め対象物を、その位置及び向きを変更可能に支持する。第１の検出部は、第１の支持部の変形状態に基づき、位置決め対象物における基準点の位置を検出する。第２の検出部は、第１の支持部の変形状態に基づき、位置決め対象物の向きを検出する。取得部は、基準点が到達すべき目標位置と、目標位置において位置決め対象物が向くべき目標向きと、を取得する。第１の許容部は、第１の検出部により検出された基準点の位置と、取得部により取得された目標位置と、を結ぶ直線を含む可動範囲を設定し、位置決め対象物の移動のうち、基準点が可動範囲の外へ出ないような移動を許容する。第２の許容部は、位置決め対象物の向きの変更のうち、第２の検出部により検出された位置決め対象物の向きと、取得部により取得された目標向きと、の角度差が広がらないような向きの変更を許容する。

このような構成によれば、効率的な位置決め制御が可能となる。

位置決め制御装置の構成を表すブロック図である。 ツール移動装置及び目標位置検出装置の外観図である。 第１の位置変更処理を表すフローチャートである。 ツールの可動範囲を表す図である。 ツールの基準点が目標位置に到達した様子を表す図である。 向き変更処理を表すフローチャートである。 図７（Ａ）はツールの向きと目標向きとの角度差を表す図、図７（Ｂ）は目標向きを回転軸としてツールを回転させたときにできる円錐を表す図、図７（Ｃ）はツールの向きの変更に伴い円錐が縮小する様子を表す図である。 第２の位置変更処理を表すフローチャートである。

以下、本発明の例示的な実施形態について図面を参照しながら説明する。また、下記の実施形態の説明で用いる符号を特許請求の範囲にも適宜使用しているが、これは本発明の理解を容易にする目的で使用しており、本発明の技術的範囲を限定する意図ではない。

［１．構成］
図１及び図２に示す位置決め制御装置１は、医療分野で使用される装置である。位置決め制御装置１は、医療分野の中でも特に歯科医療分野で使用され、歯科医療器具を患者口内の所定の位置（歯の欠損部の位置）に配置する。位置決め制御装置１は、ツール移動装置１０、目標位置検出装置４０、切替えスイッチ６０及びコントローラ７０を備える。

ツール移動装置１０は、ツール１１及びツール支持アーム１２を備える。
ツール１１は歯科医療器具である。本実施形態では、ツール１１は、患者の顎骨を削るドリルが棒状の剛体（以下、「棒状部」という。）に直角をなすように取り付けられた構造を取る。

ツール支持アーム１２は、複数の関節を備えた（換言すれば多自由度を有する）ロボットアームであり、ツール１１をその位置及び向きを変更可能に支持する。ツール支持アーム１２は、力センサ１３、接続部１４、リンク１５，１６，１７，１８，１９，２０、図示しない連結具、連結具２１、アクチュエータ２２，２３，２４，２５，２６，２７及び位置センサ２８，２９，３０，３１，３２，３３を備える。

力センサ１３は、接続部１４を介して、ツール１１の棒状部に接続される。本実施形態では、歯科医師等の使用者がツール１１の一部を把持し、使用者によって加えられた力の向きにツール１１が動くように、ツール支持アーム１２の形状が電気的な駆動力により変化する。力センサ１３は、ツール１１に力が加えられることでツール１１と力センサ１３との間の接続部１４に加えられる力の大きさ及び向きを検出する。

リンク１５，１６，１７，１８，１９，２０は、剛体の構造物である。第１リンク１５（ツール１１側のリンク）の一端は、力センサ１３に直接接続される。第１リンク１５の他端は、図示しない回転可能な第１連結具を介して第２リンク１６に接続される。ここでいう回転とは、回転する物体（この場合、第１リンク１５）の重心の位置が変わらないような回転（その場での回転）を意味する。

第２リンク１６の一端は、図示しない回転可能な第１連結具を介して第１リンク１５に接続され、他端は、図示しない旋回可能な第２連結具を介して第３リンク１７に接続される。

第３リンク１７の一端は、図示しない旋回可能な第２連結具を介して第２リンク１６に接続され、他端は、図示しない回転可能な第３連結具を介して第４リンク１８に接続される。

第４リンク１８の一端は、図示しない回転可能な第３連結具を介して第３リンク１７に接続され、他端は、図示しない旋回可能な第４連結具を介して第５リンク１９に接続される。

第５リンク１９の一端は、図示しない旋回可能な第４連結具を介して第４リンク１８に接続され、他端は、旋回可能な第５連結具２１を介して第６リンク２０に接続される。
第６リンク２０の一端は、旋回可能な第５連結具２１を介して第５リンク１９に接続され、他端は、図示しない回転可能な第６連結具を介して図示しない基台部に接続される。

アクチュエータ２２，２３，２４，２５，２６，２７及び位置センサ２８，２９，３０，３１，３２，３３は、上記５つの図示しない連結具及び連結具２１（すなわち、ツール支持アーム１２の関節）に備えられる。

すなわち、アクチュエータ２２及び位置センサ２８は、第１連結具に備えられ、アクチュエータ２３及び位置センサ２９は、第２連結具に備えられ、アクチュエータ２４及び位置センサ３０は、第３連結具に備えられる。アクチュエータ２５及び位置センサ３１は、第４連結具に備えられ、アクチュエータ２６及び位置センサ３２は、第５連結具２１に備えられ、アクチュエータ２７及び位置センサ３３は、第６連結具に備えられる。本実施形態では、アクチュエータ２２，２３，２４，２５，２６，２７はモータである。ツール１１、力センサ１３、リンク１５，１６，１７，１８，１９，２０及び基台部のそれぞれはアクチュエータ２２，２３，２４，２５，２６，２７が作動することにより互いに相対駆動する。

また、本実施形態では、位置センサ２８，２９，３０，３１，３２，３３はエンコーダである。位置センサ２８は、アクチュエータ２２の回転量を検出する。位置センサ２９は、アクチュエータ２３の回転量を検出する。位置センサ３０は、アクチュエータ２４の回転量を検出する。位置センサ３１は、アクチュエータ２５の回転量を検出する。位置センサ３２は、アクチュエータ２６の回転量を検出する。位置センサ３３は、アクチュエータ２７の回転量を検出する。

ツール支持アーム１２は、回転可能な３つの連結具と旋回可能な３つの連結具とを備える。また、旋回可能な第２連結具と旋回可能な第４連結具との間には回転可能な第３の連結具が位置するため、第２連結具と第４連結具とは異なった方向への旋回動作を実現する。そのため、ツール支持アーム１２は、任意方向への並進及び任意方向への回転によりツール１１の位置及び向きを変更する。

一方、目標位置検出装置４０は、固定部４１及び固定部支持アーム４２を備える。
固定部４１は、目標位置及び目標向きを設定するための基準となる基準物に固定される。ここでいう目標位置とは、ツール１１における基準点が到達すべき位置であり、目標向きとは、目標位置においてツール１１が向くべき向きである。なお、本実施形態では、基準点はドリルの先端、目標位置は患者の欠損歯部の顎骨頂部、目標向きは上記欠損歯部の顎骨において切削方向、基準物は上記欠損歯部近辺の他の歯である。

固定部支持アーム４２は、複数の関節を備えた（換言すれば多自由度を有する）アームであり、固定部４１をその位置及び向きを変更可能に支持する。固定部支持アーム４２は、リンク４３，４４，４５，４６，４７，４８、基台部４９、図示しない連結具、連結具５０，５１，５２及び角度センサ５３，５４，５５，５６，５７，５８を備える。

リンク４３，４４，４５，４６，４７，４８は、剛体の構造物である。第１リンク４３（固定部４１側のリンク）の一端は、固定部４１に直接接続され、他端は、図示しない回転可能な第１連結具を介して第２リンク４４に接続される。

第２リンク４４の一端は、図示しない回転可能な第１連結具を介して第１リンク４３に接続され、他端は、図示しない旋回可能な第２連結具を介して第３リンク４５に接続される。

第３リンク４５の一端は、図示しない旋回可能な第２連結具を介して第２リンク４４に接続され、他端は、図示しない回転可能な第３連結具を介して第４リンク４６に接続される。

第４リンク４６の一端は、図示しない回転可能な第３連結具を介して第３リンク４５に接続され、他端は、旋回可能な第４連結具５０を介して第５リンク４７に接続される。
第５リンク４７の一端は、旋回可能な第４連結具５０を介して第４リンク４６に接続され、他端は、旋回可能な第５連結具５１を介して第６リンク４８に接続される。

第６リンク４８の一端は、旋回可能な第５連結具５１を介して第５リンク４７に接続され、他端は、回転可能な第６連結具５２を介して基台部４９に接続される。
角度センサ５３，５４，５５，５６，５７，５８は、上記３つの図示しない連結具及び連結具５０，５１，５２（すなわち、固定部支持アーム４２の関節）に備えられる。すなわち、角度センサ５３は、第１連結具に備えられる。角度センサ５４は、第２連結具に備えられる。角度センサ５５は、第３連結具に備えられる。角度センサ５６は、第４連結具５０に備えられる。角度センサ５７は、第５連結具５１に備えられる。角度センサ５８は、第６連結具５２に備えられる。本実施形態では、角度センサ５３，５４，５５，５６，５７，５８は、エンコーダである。角度センサ５３は、第１連結具の回転量を検出する。角度センサ５４は、第２連結具の旋回量を検出する。角度センサ５５は、第３連結具の回転量を検出する。角度センサ５６は、第４連結具５０の旋回量を検出する。角度センサ５７は、第５連結具５１の旋回量を検出する。角度センサ５２は、第６連結具５２の回転量を検出する。

固定部支持アーム４２は、任意方向への並進及び任意方向の回転により固定部４１の位置及び向きを変更する。また、固定部４１の位置及び向き（すなわち、目標位置及び目標向き）は患者の体の動き等により変動する。固定部４１の位置及び向きが変動すると、その変動に伴い上記３つの図示しない連結具及び連結具５０，５１，５２が動作することにより、固定部支持アーム４２の形状が変化する。

図１における切替えスイッチ６０は、位置制御モードと向き制御モードとを切り替えるためのスイッチである。位置制御モードでは、可動範囲が設定され、基準点が可動範囲の外に出ないようにツール１１の移動が制限される。一方、向き制御モードでは、基準点の位置が動かないようにツール１１の移動が制限され、ツール１１（のドリル）の向きと目標向きとの角度差が広がらないようにツール１１の向きが制限される。なお、後述するように、位置制御モードには２種類の位置制御モード（第１の位置制御モード及び第２の位置制御モード）がある。切替えスイッチ６０の状態を切り替えることで、第１の位置制御モードと向き制御モードとが切り替えられ、第２の位置制御モードと向き制御モードとが切り替えられる。

コントローラ７０は、ＣＰＵ、ＲＯＭ及びＲＡＭを中心とする周知のマイクロコンピュータを備える。コントローラ７０は、力センサ１３、アクチュエータ２２，２３，２４，２５，２６，２７、位置センサ２８，２９，３０，３１，３２，３３、角度センサ５３，５４，５５，５６，５７，５８及び切替えスイッチ６０に接続される。

コントローラ７０は、位置センサ２８，２９，３０，３１，３２，３３により検出されたアクチュエータ２２，２３，２４，２５，２６，２７の回転量に基づいてツール支持アーム１２の変形状態を検出する。そして、コントローラ７０は、ツール支持アーム１２の変形状態に基づいてツール１１の基準点の位置及びツール１１の向きを検出する。

また、コントローラ７０は、角度センサ５３，５４，５５，５６，５７，５８により検出された第１連結具の回転量、第２連結具の旋回量、第３連結具の回転量、第４連結具５０の旋回量、第５連結具５１の旋回量及び第６連結具５２の回転量に基づいて固定部支持アーム４２の変形状態を検出する。そして、コントローラ７０は、固定部支持アーム４２の変形状態に基づいて固定部４１の位置及び向きを検出する。

コントローラ７０は、３つの制御モード（第１の位置制御モード、第２の位置制御モード及び向き制御モード）を切り替える。第１の位置制御モードでは、ツール１１の向きは自由に変更可能である。これに対し、第２の位置制御モードでは、ツール１１の向きは目標向きを向いた状態で固定されており、変更不可能である。本実施形態では、位置決め制御は第１の位置制御モードから開始され、ツール１１の基準点が目標位置に到達すると、向き制御モードへとモードが切り替えられる。また、コントローラ７０は、切替えスイッチ６０の状態の切替えに基づき、基準点が目標位置に到達していない場合であっても、第１の位置制御モードと向き制御モードとを切り替える。同様に、コントローラ７０は、切替えスイッチ６０の状態の切替えに基づき、向き制御モードと第２の位置制御モードとを切り替える。

コントローラ７０は、第１の位置制御モード中に使用者がツール１１に力を加えた場合、後述する第１の位置変更処理（図３）を実行することにより、ツール支持アーム１２の形状を変化させる。また、コントローラ７０は、向き制御モード中に使用者がツール１１に力を加えた場合、後述する向き変更処理（図６）を実行することにより、ツール支持アーム１２の形状を変化させる。また、コントローラ７０は、第２の位置制御モード中に使用者がツール１１に力を加えた場合、後述する第２の位置変更処理（図８）を実行することにより、ツール支持アーム１２の形状を変化させる。

［２．処理］
次に、コントローラ７０が実行する第１の位置変更処理について、図３のフローチャートを用いて説明する。第１の位置変更処理は、第１の位置制御モードにおいて力センサ１３により力が検出されると開始される。

まず、コントローラ７０は、Ｓ１０１（Ｓはステップを表す）にて、力センサ１３から検出値を取得する。力センサ１３により検出された力は、力センサ１３を基準とする座標系（力センサ基準座標系）にて３次元ベクトルとして表される。力センサ基準座標系は、力センサ１３の位置が変化した場合に力センサ１３に追従する。本実施形態では、力センサ基準座標系の座標軸のうちの１つは、力センサ１３の表面であって接続部１４が接続されている面に垂直に取られ、他の２つの座標軸は、上記座標軸に垂直に取られる。

次に、コントローラ７０は、ツール１１を基準とする座標系（ツール基準座標系）にてツール１１に加えられた力の大きさ及び向きを計算する（Ｓ１０２）。ツール基準座標系は、ツール１１の位置が変化した場合にツール１１に追従する。本実施形態では、ドリルの軸がツール基準座標系の座標軸の１つであり、ドリルの先端であって棒状部に接続されていない方の先端がツール基準座標系の原点である。Ｓ１０２では、Ｓ１０１で取得されたツール１１と力センサ１３との間の接続部１４に加えられた力に基づき、ツール１１に加えられた力が計算される。

続いて、コントローラ７０は、図２に記載のツール移動装置座標系３４におけるツール１１の途中目標位置及び途中目標向きを計算する（Ｓ１０３）。ここでいう途中目標位置とは、Ｓ１０２にて計算されたツール１１に加えられた力に基づいて決定される、ツール１１の基準点の移動すべき移動先（すなわち、使用者が意図した基準点の移動先）である。また、途中目標向きとは、Ｓ１０２にて計算されたツール１１に加えられた力に基づいて決定される、ツール１１の向くべき向き（すなわち、使用者が意図したツール１１の向き）である。

次に、コントローラ７０は、ツール移動装置座標系３４におけるツール１１の位置及び向きを検出する（Ｓ１０４）。ツール１１の位置及び向きは、位置センサ２８，２９，３０，３１，３２，３３により検出されるアクチュエータ２２，２３，２４，２５，２６，２７の回転量に基づき、検出される。

次に、コントローラ７０は、ツール移動装置座標系３４から図２に記載の目標位置検出装置座標系５９へ移行する（Ｓ１０５）。その後、コントローラ７０は、目標位置検出装置４０の固定部４１の位置及び向きに基づき、目標位置及び目標向きの情報を取得する（Ｓ１０６）。そして、コントローラ７０は、Ｓ１０６にて取得した目標位置及び目標向きの情報に基づき、目標位置（患者の欠損歯部の顎骨頂部）を基準とする座標系（目標位置座標系）へ移行し、目標位置座標系から見たツール１１の位置及び向きを検出する（Ｓ１０７）。ここで、目標位置座標系は、目標位置が変化した場合に目標位置に追従する。本実施形態では、目標位置座標系の原点は目標位置に取られ、座標軸のうちの１つは目標向きと同一方向に取られる。

続いて、コントローラ７０は、図４に示すように、ツール１１の可動範囲８１を設定する（Ｓ１０８）。コントローラ７０は、目標位置（図４では点線で表されるドリルの下部先端位置）と、ツール１１の基準点の位置（図４では実線で表されるドリルの下部先端位置）と、を結ぶ直線を含む可動範囲８１を設定する。本実施形態では、可動範囲８１は、目標位置と基準点の位置とを結ぶ直線を最長の対角線とし、その一辺が目標向き（図４では点線で表されるドリルの向き）に平行である直方体である。コントローラ７０は、ツール１１の移動のうち、基準点が直方体の外へ出ないような移動のみを許容する。コントローラ７０は、ツール１１の移動に伴い、上記直線が常に最長の対角線になるように直方体の各辺の長さを変更し、直方体の体積を縮小させる。なお、患者の体の動き等により目標位置と基準点の位置とが遠ざかった場合は、上記直線が最長の対角線になるように直方体の体積は拡大される。

続いて、コントローラ７０は、ツール１１の基準点が目標位置に到達しているか否かを判定する（Ｓ１０９）。図５に示すように、基準点が目標位置に到達すると、コントローラ７０はＳ１０９にて基準点が目標位置に到達していると判定し（Ｓ１０９：ＹＥＳ）、基準点の位置が動かないようにツール１１の位置を固定する（Ｓ１１０）。そして、コントローラ７０は、モードを向き制御モードへ切り替え（Ｓ１１１）、第１の位置変更処理を終了する。

一方、コントローラ７０は、基準点が目標位置に到達していないと判定した場合は（Ｓ１０９：ＮＯ）、切替えスイッチ６０の状態が向き制御モードであるか否かを判定する（Ｓ１１２）。コントローラ７０は、切替えスイッチ６０の状態が向き制御モードであると判定した場合は（Ｓ１１２：ＹＥＳ）、先述したＳ１１０の処理を実行した後に先述したＳ１１１の処理を行い、第１の位置変更処理を終了する。すなわち、コントローラ７０は、基準点が目標位置に到達していない場合であっても、切替えスイッチ６０の状態の切替えがあった場合は、モードを向き制御モードへ切り替える。一方、コントローラ７０は、切替えスイッチ６０の状態が向き制御モードでないと判定した場合は（Ｓ１１２：ＮＯ）、処理をＳ１１３へ移行させる。

Ｓ１１３で、コントローラ７０は、Ｓ１０３にて計算された途中目標位置が可動範囲８１内に存在するか否かを判定する。コントローラ７０は、途中目標位置が可動範囲８１内に存在しないと判定した場合は（Ｓ１１３：ＮＯ）、途中目標位置を修正する（Ｓ１１４）。ここで、コントローラ７０は、ツール１１の基準点の実際の位置と修正前の途中目標位置とを結ぶ直線と、直方体である可動範囲８１の面と、の交点が、修正後の途中目標位置となるように、途中目標位置を修正する。コントローラ７０は、Ｓ１１４の処理を実行すると、処理をＳ１１５へ移行させる。

一方、コントローラ７０は、途中目標位置が可動範囲８１内に存在すると判定した場合は（Ｓ１１３：ＹＥＳ）、Ｓ１１４の処理を飛ばしてＳ１１５の処理を実行する。
Ｓ１１５で、コントローラ７０は、ツール移動装置座標系３４へ移行する。そして、コントローラ７０は、アクチュエータ２２，２３，２４，２５，２６，２７を駆動させ、ツール１１の位置及び向きを変更する（Ｓ１１６）。この際、コントローラ７０は、途中目標位置の方向にツール１１の位置を変更し、途中目標向きへツール１１の向きを変更する。

続いて、コントローラ７０は、位置センサ２８，２９，３０，３１，３２，３３からアクチュエータ２２，２３，２４，２５，２６，２７の回転量を取得し（Ｓ１１７）、取得した回転量に基づきツール１１の位置及び向きを検出する（Ｓ１１８）。

その後、コントローラ７０は、ツール１１の位置と途中目標位置との差分及びツール１１の向きと途中目標向きとの差分のいずれもが０か否かを判定する（Ｓ１１９）。
コントローラ７０は、上記差分のうち少なくとも一方が０でないと判定した場合は（Ｓ１１９：ＮＯ）、前述したＳ１１６に戻り、Ｓ１１６以降の処理を繰り返す。一方、コントローラ７０は、上記差分のいずれもが０であると判定した場合は（Ｓ１１９：ＹＥＳ）、第１の位置変更処理を終了する。したがって、コントローラ７０は、Ｓ１１９にて上記差分のいずれもが０である、すなわち、ツール１１の位置が途中目標位置に到達し、かつ、ツール１１が途中目標向きを向いたと判定されるまで、Ｓ１１６からＳ１１９までの処理を繰り返す。

次に、コントローラ７０が実行する向き変更処理について、図６のフローチャートを用いて説明する。向き変更処理（図６）は、基本的な構成は第１の位置変更処理（図３）と同様であるため、同様な処理については説明を省略し、相違点を中心に説明する。また、図３においてなされた説明と同様な説明は省略する。向き変更処理は、向き制御モードにおいて力センサ１３により力が検出されると開始される。なお、向き制御モードでは、基準点の位置が動かないようにツール１１の位置が固定される。

図６におけるＳ２０１〜Ｓ２０２は、図３におけるＳ１０１〜Ｓ１０２と同様な処理であるため、説明を省略する。
コントローラ７０は、Ｓ２０３にて途中目標向きを計算する。向き制御モードでは基準点の位置は固定され、ツール１１の向きのみが変更可能であるため、コントローラ７０は途中目標向きのみを計算する。同様に、コントローラ７０は、Ｓ２０４にてツール１１の向きのみを検出する。コントローラ７０は、Ｓ２０５にて目標位置検出装置座標系５９へ移行し、Ｓ２０６にて目標向きの情報を取得する。その後、コントローラ７０は、目標位置座標系へ移行し、目標位置座標系から見たツール１１の向きを検出する（Ｓ２０７）。

コントローラ７０は、図７（Ａ）に示すように、Ｓ２０８にてツール１１の向きの変更可能範囲８２を設定する。コントローラ７０は、ツール１１の向きと目標向きとの角度差θが広がらないようにツール１１の向きを制限する。すなわち、変更可能範囲８２は、目標向きを回転軸としてツール１１を回転させたときにできる曲面上及び上記曲面を円錐面とする円錐の内部である（図７（Ｂ））。コントローラ７０は、ツール１１の向きの変更に伴い、上記曲面が円錐面となるように円錐を縮小させる。円錐の縮小に伴い、円錐の底面の円は小さくなる（図７（Ｃ））。

続いて、コントローラ７０は、Ｓ２０９にてツール１１が目標向きを向いているか否かを判定する。コントローラ７０は、ツール１１が目標向きを向いていると判定した場合は（Ｓ２０９：ＹＥＳ）、基準点が目標位置に到達しているか否かを判定する（Ｓ２１０）。コントローラ７０は、基準点が目標位置に到達していると判定した場合は（Ｓ２１０：ＹＥＳ）、ツール１１の向きを固定し（Ｓ２１１）、向き変更処理を終了する。なお、この場合、基準点が目標位置に到達し、ツール１１が目標向きを向いたため、位置決め制御が完了する。

一方、コントローラ７０は、Ｓ２１０にて基準点が目標位置に到達していないと判定した場合（Ｓ２１０：ＮＯ）、切替えスイッチ６０の状態が向き制御モードであるか否かを判定する（Ｓ２１２）。なお、Ｓ２１０にて基準点が目標位置に到達していないと判定される場合は、第１の位置制御モードにおいて基準点が目標位置に到達する前に切替えスイッチ６０によりモードが向き制御モードへ切り替えられた場合に対応する。

コントローラ７０は、Ｓ２１２にて切替えスイッチ６０の状態が向き制御モードであると判定した場合は（Ｓ２１２：ＹＥＳ）、先述したＳ２１１の処理を実行し、向き変更処理を終了する。なお、Ｓ２１１にて切替えスイッチ６０の状態が向き制御モードであると判定され、向き変更処理が終了された後に切替えスイッチ６０の状態の切替えがない場合は、ツール１１が目標位置でない位置で目標向きを向いた状態で位置決め制御が終了する。このような状況は、例えば、使用者が目標位置近辺で切替えスイッチ６０によりモードを意識的に切り替え、ツール１１の向きを目標向きに一致させた後に位置決め制御を終了する場合等に起こる。

一方、コントローラ７０は、Ｓ２１１にて切替えスイッチ６０の状態の切替えがあったと判定した場合は（Ｓ２１１：ＹＥＳ）、ツール１１の向きを固定する（Ｓ２１３）。その後、コントローラ７０は、モードを第２の位置制御モードへ切り替え（Ｓ２１４）、向き変更処理を終了する。

一方、コントローラ７０は、Ｓ２０９にてツール１１が目標向きを向いていないと判定した場合は（Ｓ２０９：ＮＯ）、切替えスイッチ６０の状態が向き制御モードであるか否かを判定する（Ｓ２１５）。コントローラ７０は、切替えスイッチ６０の状態が向き制御モードでないと判定した場合は（Ｓ２１５：ＮＯ）、モードを第１の位置制御モードへ切り替え（Ｓ２１６）、向き変更処理を終了する。

一方、コントローラ７０は、切替えスイッチ６０の状態が向き制御モードであると判定した場合は（Ｓ２１５：ＹＥＳ）、途中目標向きが変更可能範囲８２内に存在するか否かを判定する（Ｓ２１７）。コントローラ７０は、途中目標向きが変更可能範囲８２内に存在しないと判定した場合（Ｓ２１７：ＮＯ）、途中目標向きを修正する（Ｓ２１８）。ここで、コントローラ７０は、ツール１１の実際の向きから修正前の途中目標向きへとツール１１の向きが変化させられたときにツール１１が描く曲面と円錐である変更可能範囲８２との交線が、修正後の途中目標向きと重なるように、途中目標向きを修正する。コントローラ７０は、Ｓ２１８の処理を実行すると、処理をＳ２１９へ移行させる。

一方、コントローラ７０は、途中目標向きが変更可能範囲８２内に存在すると判定した場合（Ｓ２１７：ＹＥＳ）、Ｓ２１８の処理を飛ばしてＳ２１９の処理を実行する。
図６におけるＳ２１９〜Ｓ２２１は、図３におけるＳ１１５〜Ｓ１１７と同様な処理であるため、説明を省略する。

コントローラ７０は、Ｓ２２２にてツール１１の向きを検出する。次に、コントローラ７０は、ツール１１の向きと途中目標向きとの差分が０か否かを判定する（Ｓ２２３）。コントローラ７０は、上記差分が０でないと判定した場合は（Ｓ２２３：ＮＯ）、Ｓ２２０に戻り、Ｓ２２０以降の処理を繰り返す。一方、コントローラ７０は、上記差分が０であると判定した場合は（Ｓ２２３：ＹＥＳ）、向き変更処理を終了する。したがって、コントローラ７０は、Ｓ２２３にて上記差分が０である、すなわち、ツール１１が途中目標向きを向いたと判定されるまで、Ｓ２２０からＳ２２３までの処理を繰り返す。

次に、コントローラ７０が実行する第２の位置変更処理について、図８のフローチャートを用いて説明する。第２の位置変更処理（図８）は、基本的な構成は第１の位置変更処理（図３）と同様であるため、同様な処理については説明を省略し、相違点を中心に説明する。また、図３においてなされた説明と同様な説明を省略する。第２の位置変更処理は、第２の位置制御モードにおいて力センサ１３により力が検出されると開始される。なお、第２の位置制御モードでは、ツール１１は目標向きを向いており、ツール１１の向きが変わらないようにツール１１の向きが固定される。

図８におけるＳ３０１〜Ｓ３０２は、図３におけるＳ１０１〜Ｓ１０２と同様な処理であるため、説明を省略する。
コントローラ７０は、Ｓ３０３にて途中目標位置を計算する。第２の位置制御モードではツール１１の向きは固定され、ツール１１の位置のみが変更可能であるため、コントローラ７０は途中目標位置のみを計算する。同様に、コントローラ７０は、Ｓ３０４にてツール１１の位置のみを検出する。コントローラ７０は、Ｓ３０５にて目標位置検出装置座標系５９へ移行し、Ｓ３０６にて目標位置の情報を取得する。その後、コントローラ７０は、目標位置座標系へ移行し、目標位置座標系から見たツール１１の位置を検出する（Ｓ３０７）。

続いて、コントローラ７０は、ツール１１の可動範囲８１を設定する（Ｓ３０８）。可動範囲８１の設定の仕方は、図３のＳ１０８と同様である。
次に、コントローラ７０は、基準点が目標位置に到達しているか否かを判定する（Ｓ３０９）。コントローラ７０は、基準点が目標位置に到達していると判定した場合は（Ｓ３０９：ＹＥＳ）、ツール１１の位置を固定し（Ｓ３１０）、第２の位置変更処理を終了する。なお、この場合、基準点が目標位置に到達し、ツール１１が目標向きを向いているため、位置決め制御が完了する。一方、コントローラ７０は、基準点が目標位置に到達していないと判定した場合は（Ｓ３０９：ＮＯ）、処理をＳ３１１へ移行させる。

図８におけるＳ３１１〜Ｓ３１５は、図３におけるＳ１１３〜Ｓ１１７と同様な処理であるため、説明を省略する。
コントローラ７０は、Ｓ３１６にてツール１１の位置を検出する。次に、コントローラ７０は、ツール１１の位置と途中目標位置との差分が０か否かを判定する（Ｓ３１７）。コントローラ７０は、上記差分が０でないと判定した場合は（Ｓ３１７：ＮＯ）、Ｓ３１４に戻り、Ｓ３１４以降の処理を繰り返す。一方、コントローラ７０は、上記差分が０であると判定した場合は（Ｓ３１７：ＹＥＳ）、第２の位置変更処理を終了する。したがって、コントローラ７０は、Ｓ３１７にて上記差分が０である、すなわち、ツール１１の位置が途中目標位置に到達したと判定されるまで、Ｓ３１４からＳ３１７までの処理を繰り返す。

［３．効果］
以上詳述した実施形態によれば、以下の効果が得られる。
（１）上記実施形態では、ツール１１の移動のうち、基準点が可動範囲８１の外へ出ないような移動のみが許容され、ツール１１の移動に伴い可動範囲８１が狭められる。また、ツール１１の向きの変更のうち、ツール１１の向きと目標向きとの角度差が広がらないような向きの変更のみが許容される。そのため、位置決め制御装置１による位置決め制御では、ツール１１を目標位置へ移動させる過程で生じ得るツール１１の無駄な移動を抑制できる。また、ツール１１の向きを目標向きに向ける過程で生じ得るツール１１の無駄な向きの変更を抑制できる。また、上記のようにツール１１の移動が許容され、可動範囲８１が狭められるため、例えば欠損歯部の周囲の歯や看護師の手等により使用者が目標位置を目視確認できない状況であっても、ツール１１を目標位置に到達させることができる。同様に、使用者が目標向きを目視確認できない状況であっても、ツール１１の向きを目標向きに向けることができる。

また、単にツール１１の無駄な移動を抑制する場合、ツール１１の基準点の位置と目標位置とを結ぶ直線上のみをツール１１の可動範囲として設定してもよい。しかしながら、そのように可動範囲を設定した場合、可動範囲である上記直線状に障害物（例えば、欠損歯部の周囲の歯や看護師の手等）があった場合、障害物を回避することができない。それに対し、位置決め制御装置１による位置決め制御では、基準点が可動範囲８１内に位置する限りにおいてツール１１は任意方向に移動可能である。そのため、基準点の位置と目標位置とを結ぶ直線上に障害物がある場合においても、障害物を回避しつつ、ツール１１を目標位置へと近づけることができる。同様に、ツール１１を目標向きに向ける過程では、ツール１１の向きが変更可能範囲８２内に存在する限りにおいてツール１１の向きは任意方向に変更可能である。そのため、障害物がある場合でも、障害物を回避しつつ目標向きへとツール１１の向きを近づけることができる。

（２）位置決め制御装置１は目標位置検出装置４０を備え、コントローラ７０は目標位置検出装置４０から目標位置及び目標向きの情報をリアルタイムに取得する。コントローラ７０は、目標位置及び目標向きが変化した場合、可動範囲８１及び変更可能範囲８２を再設定する。そのため、患者の体動等により目標位置及び目標向きが変化した場合であっても、使用者はツール１１を目標位置に近づけることができ、また、ツール１１を目標向きへ向けることができる。

（３）位置決め制御装置１は切替えスイッチ６０を備える。切替えスイッチ６０の状態の切替えがあった場合は、ツール１１の基準点が目標位置に到達したと判定されていなくても、位置制御モードと向き制御モードとが切り替わる。コントローラ７０が基準点が目標位置に到達したと判定し、位置制御モードから向き制御モードへとモードが自動的に切り替えられたときに、使用者がモードが切り替わったことに気付かない場合がある。そのような場合、使用者はツール１１の位置を変更しようとしたつもりが、意図せずツール１１の向きを変えてしまうことがある。しかしながら、切替えスイッチ６０を使って使用者が意識的にモードを切り替えることで、上記のような意図しないツール１１の動作の発生を抑制することができる。

なお、本実施形態では、ツール１１が位置決め対象物の一例に相当し、ツール支持アーム１２が第１の支持部の一例に相当する。また、Ｓ１０４、Ｓ１１８、Ｓ３０４及びＳ３１６が第１の検出部としての処理の一例に相当し、Ｓ１０４、Ｓ１１８、Ｓ２０４及びＳ２２２が第２の検出部としての処理の一例に相当する。また、Ｓ１０６、Ｓ２０６及びＳ３０６が取得部としての処理の一例に相当し、Ｓ１０８、Ｓ１１０及びＳ３０８が第１の許容部としての処理の一例に相当し、Ｓ２０８が第２の許容部としての処理の一例に相当する。固定部４１が固定部の一例に相当し、固定部支持アーム４２が第２の支持部の一例に相当する。また、Ｓ１０９が判定部の処理の一例に相当する。

［４．他の実施形態］
以上、本発明の実施形態について説明したが、本発明は、上記実施形態に限定されることなく、種々の形態を採り得ることは言うまでもない。

（１）上記実施形態では、位置決め制御は位置制御モードから開始され、基準点が目標位置に到達すると、向き制御モードへとモードが切り替わった。しかし、モードの順番はこれに限られない。向き制御モードから位置決め制御が開始され、ツール１１の向きが目標向きを向いたら、位置制御モードへとモードが切り替わってもよい。また、切替えスイッチ６０が使われない場合において、位置制御モードと向き制御モードとの間をモードが複数回切り替わってもよい。例えば、位置制御モードから向き制御モードへとモードが切り替わり、向き制御モードから位置制御モードへ再度モードが切り替わってもよい。この場合、例えば、位置制御モードにおいてツール１１の基準点と目標位置とのｘ座標が一致すると向き制御モードへとモードが切り替わり、ツール１１の向きと目標向きとが一致すると位置制御モードへモードが再度切り替わってもよい。そして、その後、基準点と目標位置とのｙ，ｚ座標を一致させてもよい。

（２）上記実施形態では、第１の位置制御モードにおいてはツール１１の向きは変更可能だったが、ツール１１の向きが変更不可能であってもよい。この場合、ツール１１は、目標向きを向いている必要はない。

（３）上記実施形態では、目標位置近辺の基準物に固定された固定部４１の位置及び向きの変化を取得することで、コントローラ７０は目標位置及び目標向きの継時的な変化を取得した。しかし、目標位置及び目標向きの継時的な変化を取得する方法はこれに限られない。例えば、コントローラ７０は、カメラ等により取得された画像に基づき目標位置及び目標向きの継時的な変化を取得してもよい。また、コントローラ７０は、目標位置及び目標向きの変化に伴う磁界の変化を取得することにより、上記継時的な変化を取得してもよい。

（４）上記実施形態において、位置決め制御装置１は目標位置検出装置４０を備えていなくてもよい。目標位置検出装置４０が備えられていない場合は、目標位置及び目標向きの情報はコントローラ７０に事前に入力されてもよい。また、位置決め制御装置１は切替えスイッチ６０を備えていなくてもよい。切替えスイッチ６０が備えられていない場合は、ツール１１の基準点が目標位置に到達したときやツール１１の向きが目標向きを向いたとき等に位置制御モードと向き制御モードとが切り替えられてもよい。

（５）上記実施形態では、ツール１１の可動範囲として直方体を例示したが、可動範囲は、基準点の位置と目標位置とを結ぶ直線を回転軸とし、かつ、回転軸としての直線の長さが高さとなる回転体であってもよい。例えば、回転体は円錐であってもよい。可動範囲が回転体である場合も、可動範囲は基準点の位置及び目標位置のそれぞれが頂点及び面上のいずれか一方に位置する立体形状の空間であってもよい。可動範囲が上記実施形態のように直方体である場合は、基準点の位置が直方体の頂点に位置し、目標位置は、上記直方体の頂点であって基準点が位置する頂点から最も遠い頂点に位置してもよい。また、可動範囲が円錐である場合は、目標位置が円錐の頂点に位置し、基準点が上記円錐の底面上（例えば、底面である円の中心）に位置してもよい。

また、可動範囲が立体形状である場合において、コントローラ７０は、ツール１１の移動に伴い、立体形状の種類を変えずに立体形状の体積を縮小させてもよい。ここで、立体形状の種類を変えない縮小とは、立体形状が直方体の場合は、縮小の前後の立体形状がともに直方体であることを意味する。なお、この場合、縮小前の直方体と縮小後の直方体とは互いに相似でなくてもよい。また、可動範囲が回転体である場合は、ツール１１の移動に伴い、回転体を相似的に縮小させてもよい。

（６）上記実施形態では、位置決め対象物はツール１１であったが、位置決め対象物はツール１１以外の物であってもよい。
（７）上記実施形態では、ツール１１は患者の顎骨を削るドリルであったが、ツールは顎骨以外の骨を削るドリルであってもよい。つまり、ツールは、患者の骨を削るドリルであってもよい。また、ツールは、患者の骨に代えて、又は、患者の骨に加えて、患者の歯を削るドリルであってもよい。さらに、ツールは、骨及び歯以外の物を削るドリルであってもよく、また、ドリル以外の物であってもよい。

（８）上記実施形態の位置決め制御装置１は医療分野（歯科医療分野）で使用される装置であるが、本発明に係る位置決め制御装置は医療分野以外の分野で使用されてもよい。例えば、位置決め制御装置は産業分野で使用されてもよい。

（９）上記実施形態では、コントローラ７０は、ツール１１に加えられた力の大きさ及び向きを計算し取得した。しかし、コントローラ７０は、ツール１１に加えられた力の大きさ及び向きに加えて、ツール１１における力が加えられた箇所（作用点）を取得してもよい。作用点の位置と事前に設定されたツール１１上の制御点の位置が異なる場合、使用者がツール１１に並進力しか加えなかったのに対しコントローラ７０は制御点周りの力のモーメントを取得し、ツール支持アーム１２に回転運動をさせてしまうことがある。しかしながら、コントローラ７０が作用点を取得することで、作用点と制御点とが異なる場合であっても、使用者の意図した通りにツール支持アーム１２を変形させることができる。

（１０）上記実施形態では、途中目標位置が可動範囲８１内に存在しない場合は途中目標位置が修正されたが、途中目標位置は修正されなくてもよい。例えば、コントローラ７０は、途中目標位置が可動範囲８１内に存在しない場合において、ツール１１の位置を移動させず、第１の位置変更処理及び第２の位置変更処理を終了してもよい。同様に、上記実施形態では、途中目標向きが変更可能範囲８２内に存在しない場合は途中目標向きが修正されたが、途中目標向きは修正されなくてもよい。例えば、コントローラ７０は、途中目標向きが変更可能範囲８２内に存在しない場合において、ツール１１の向きを変更せず、向き変更処理を終了してもよい。

（１１）上記実施形態では、位置決め制御装置１は２つの位置制御モード（第１の位置制御モード及び第２の位置制御モード）を有していたが、位置決め制御装置１の有する位置制御モードの数は２でなくてもよい。例えば、位置決め制御装置１は、第１の位置制御モード及び第２の位置制御モードのうちのいずれか１つのみを有していてもよい。同様に、位置決め制御装置１は、２以上の向き制御モードを有していてもよい。

（１２）上記実施形態における１つの構成要素が有する機能を複数の構成要素として分散させたり、複数の構成要素が有する機能を１つの構成要素に統合したりしてもよい。また、上記実施形態の構成の少なくとも一部を、同様の機能を有する公知の構成に置き換えてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加、置換等してもよい。なお、特許請求の範囲に記載の文言から特定される技術思想に含まれるあらゆる態様が本発明の実施形態である。

１…位置決め制御装置、１０…ツール移動装置、１１…ツール、１２…ツール支持アーム、１３…力センサ、１４…接続部、１５，１６，１７，１８，１９，２０…ツール支持アームのリンク、２１…ツール支持アームの連結具、２２，２３，２４，２５，２６，２７…アクチュエータ、２８，２９，３０，３１，３２，３３…位置センサ、３４…ツール移動装置座標系、４０…目標位置検出装置、４１…固定部、４２…固定部支持アーム、４３，４４，４５，４６，４７，４８…固定部支持アームのリンク、４９…基台部、５０，５１，５２…固定部支持アームの連結具、５３，５４，５５，５６，５７，５８…角度センサ、５９…目標位置検出装置座標系、６０…切替えスイッチ、７０…コントローラ、８１…可動範囲、８２…変更可能範囲。

Claims (7)

1. 位置決め対象物（１１）と、
   前記位置決め対象物を、その位置及び向きを変更可能に支持する第１の支持部（１２）と、
   前記第１の支持部の変形状態に基づき、前記位置決め対象物における基準点の位置を検出する第１の検出部（Ｓ１０４，Ｓ１１８，Ｓ３０４，Ｓ３１６）と、
   前記第１の支持部の変形状態に基づき、前記位置決め対象物の向きを検出する第２の検出部（Ｓ１０４，Ｓ１１８，Ｓ２０４，Ｓ２２２）と、
   前記基準点が到達すべき目標位置と、前記目標位置において前記位置決め対象物が向くべき目標向きと、を取得する取得部（Ｓ１０６，Ｓ２０６，Ｓ３０６）と、
   前記第１の検出部により検出された前記基準点の位置と、前記取得部により取得された前記目標位置と、を結ぶ直線を含む可動範囲（８１）を設定し、前記位置決め対象物の移動のうち、前記基準点が前記可動範囲の外へ出ないような移動を許容する第１の許容部（Ｓ１０８，Ｓ１１０，Ｓ３０８）と、
   前記第２の検出部により検出された前記位置決め対象物の向きと、前記取得部により取得された前記目標向きと、を含む変更可能範囲（８２）を設定し、前記位置決め対象物の向きの変更のうち、前記位置決め対象物の向きが前記変更可能範囲の外へ出ないような向きの変更を許容する第２の許容部（Ｓ２０８）と、
   を備え、
   前記可動範囲は、前記基準点の位置及び前記目標位置のそれぞれが頂点及び面上のいずれか一方に位置する立体形状の空間であり、
   前記第１の許容部は、前記位置決め対象物の移動に伴い、前記立体形状の種類を変えずに前記立体形状の体積を縮小させ、
   前記第２の許容部は、前記位置決め対象物の向きの変更に伴い、前記変更可能範囲を狭める、位置決め制御装置。
2. 前記目標位置及び前記目標向きを設定するための基準となる基準物に固定される固定部（４１）と、
   前記固定部を、その位置及び向きを変更可能に支持する第２の支持部（４２）と、
   を更に備え、
   前記取得部は、前記第２の支持部の変形状態に基づき前記固定部の位置及び向きを検出し、検出された前記固定部の位置及び向きに基づき、前記目標位置及び前記目標向きを取得する、請求項１に記載の位置決め制御装置。
3. 前記立体形状の空間は、前記基準点の位置と前記目標位置とを結ぶ直線を最長の対角線とする直方体であり、
   前記第１の許容部は、前記位置決め対象物の移動に伴い、前記直線が常に対角線になるように前記直方体の各辺の長さを変更し、前記直方体の体積を縮小させる、請求項１又は請求項２に記載の位置決め制御装置。
4. 前記基準点が前記目標位置に到達したか否かを判定する判定部（Ｓ１０９）を更に備え、
   前記第１の許容部（Ｓ１１０）は、前記判定部により前記基準点が前記目標位置に到達したと判定された場合に、前記基準点の位置が動かないように前記位置決め対象物の位置を固定し、
   前記第２の許容部は、前記判定部により前記基準点が前記目標位置に到達したと判定された後に、前記位置決め対象物の向きの変更のうち、前記位置決め対象物の向きが前記変更可能範囲の外へ出ないような向きの変更を許容する、請求項１から請求項３までのいずれか１項に記載の位置決め制御装置。
5. 前記第１の許容部により、前記位置決め対象物の移動のうち、前記基準点が前記可動範囲の外へ出ないような移動が許容される第１の状態と、前記第２の許容部により、前記位置決め対象物の向きの変更のうち、前記位置決め対象物の向きが前記変更可能範囲の外へ出ないような向きの変更が許容される第２の状態と、を切り替える切替えスイッチ（６０）を更に備える、請求項１から請求項４までのいずれか１項に記載の位置決め制御装置。
6. 医療分野に用いられる、請求項１から請求項５までのいずれか１項に記載の位置決め制御装置。
7. 前記位置決め対象物は患者の歯及び骨の少なくとも１つを削るドリルである、請求項１から請求項６までのいずれか１項に記載の位置決め制御装置。