JP2010271918A

JP2010271918A - Mobile unit, correction value calculation method and program

Info

Publication number: JP2010271918A
Application number: JP2009123201A
Authority: JP
Inventors: Yusuke Kida; 祐介木田
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2009-05-21
Filing date: 2009-05-21
Publication date: 2010-12-02
Anticipated expiration: 2029-05-21
Also published as: JP5267324B2

Abstract

<P>PROBLEM TO BE SOLVED: To reduce the influence due to errors that may occur in an output value from an acceleration sensor, and control the mobile unit more accurately. <P>SOLUTION: An inverted two-wheel robot 100 is an mobile unit that spatially moves, in a state where a relative position of a body 50 is controlled with respect to wheels 51. The inverted two-wheel robot 100 includes a sensor 10 detecting the angular velocity of its body 50; an encoder 15 measuring variations in the relative position between the body 50 and the wheel 51, in a rotational direction of the wheel 51; and an instruction value calculator 20 that generates an instruction for controlling the relative position between the wheel 51 and the body 50, in a condition where the error included in the output value of the sensor 10 is corrected, according to a correction value preliminarily calculated, on the basis of each of outputs from the sensor 10 and the encoder 15. According to the adoption of this configuration, deterioration in controllability of the robot 100 due to the deterioration in the output value from the sensor 10 is suppressed. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

本発明は、移動体、補正値の算出方法、及びプログラムに関する。 The present invention relates to a moving object, a correction value calculation method, and a program.

近年、ロボット技術の進展を背景として、ヒトの空間移動に用いられるロボットに係る技術も飛躍的に高度化している。移動体として活用されるロボットの筐体には、各種センサ、マイコン、電源、及びリンク機構等の様々な電気／機械コンポーネントが内蔵される。 In recent years, with the development of robot technology, technology related to robots used for human space movement has also been dramatically advanced. Various electric / mechanical components such as various sensors, a microcomputer, a power source, and a link mechanism are built in a housing of a robot used as a moving body.

このような移動型ロボットでは、移動時の安定性／安全性が強く要求される。ロボットの移動時の安全性／安定性を確保するために、各種センサの入力に基づいて、ロボットの姿勢制御を高精度に実行することが強く要求される。一般的に、移動体の姿勢制御には、角速度センサ（以下、ジャイロセンサと呼ぶこともある）及び加速度センサが用いられる。移動体の低コスト化を図るためには安価なジャイロセンサを活用することが要求される。 Such a mobile robot is strongly required to have stability / safety during movement. In order to ensure safety / stability during movement of the robot, it is strongly required that the posture control of the robot be executed with high accuracy based on the input of various sensors. In general, an angular velocity sensor (hereinafter also referred to as a gyro sensor) and an acceleration sensor are used for posture control of a moving body. In order to reduce the cost of the moving body, it is required to use an inexpensive gyro sensor.

なお、安価なジャイロセンサでは、ＭＥＭＳ(Micro-Electro Mechanical Systems)が活用されている。具体的には、半導体プロセス技術を活用して、半導体、ガラス等の基板上に３次元構造を高精細に形成する。このようなジャイロセンサでは、一般的に、振動体の空間的な変位を電気的に検出することによって、ジャイロセンサが受けるコリオリの力を電気的に検出する。例えば、振動体を振動状態とし、振動体と検出電極間の容量値の変化を検出することによって、コリオリの力を電気的に検出する。なお、ジャイロセンサの具体的な構造、コリオリの力の検出方法は様々なタイプのものが開発されている。 An inexpensive gyro sensor uses MEMS (Micro-Electro Mechanical Systems). Specifically, a three-dimensional structure is formed with high definition on a substrate such as a semiconductor or glass by utilizing semiconductor process technology. In such a gyro sensor, generally, the Coriolis force received by the gyro sensor is electrically detected by electrically detecting the spatial displacement of the vibrating body. For example, the Coriolis force is electrically detected by placing the vibrating body in a vibrating state and detecting a change in capacitance value between the vibrating body and the detection electrode. Various types of gyro sensor structures and Coriolis force detection methods have been developed.

特許文献１には、倒立車輪型の走行体が開示されており、特に、倒立車輪型の走行体の車体を後方に大きく傾斜させないでも急制動させる技術が開示されている。なお、特許文献２には、車椅子に関して、急傾斜地域や不整地でも安全且つ快適な姿勢を確保する技術が開示されている。 Patent Document 1 discloses an inverted wheel type traveling body, and particularly discloses a technique for suddenly braking the vehicle body of an inverted wheel type traveling body without greatly inclining backward. Patent Document 2 discloses a technique for ensuring a safe and comfortable posture on a wheelchair even in a steeply inclined region or rough terrain.

特開２００７−２０３９６５号公報JP 2007-203965 A 特開２０００−１１６７１８号公報JP 2000-116718 A

上述のように、移動体の低価格化を図るためには、安価なジャイロセンサを採用することが要求される。しかしながら、安価なジャイロセンサでは、その出力値が経時的に劣化してしまう場合がある。例えば、ジャイロセンサ内に組み込まれた振動体の振動の影響によって、ジャイロセンサ内の部品同士が部分的に磨耗し、ジャイロセンサの出力値に誤差が生じてしまうおそれがある。また、ロボットの筐体内に組み込まれた他の部品（例えば、熱源等）の影響によって、ジャイロセンサの出力値に誤差が生じてしまう場合がある。ジャイロセンサの出力値が劣化すると、移動体ロボットの姿勢制御を高精度に実現することが阻害されてしまう。 As described above, in order to reduce the price of a moving body, it is required to employ an inexpensive gyro sensor. However, with an inexpensive gyro sensor, its output value may deteriorate over time. For example, due to the influence of vibration of a vibrating body incorporated in the gyro sensor, parts in the gyro sensor may be partially worn and an error may occur in the output value of the gyro sensor. In addition, an error may occur in the output value of the gyro sensor due to the influence of other parts (for example, a heat source) incorporated in the housing of the robot. When the output value of the gyro sensor deteriorates, it is impeded to realize the attitude control of the mobile robot with high accuracy.

上述の説明から明らかなように、加速度センサの出力値に生じうる誤差の影響を低減して、移動体を高精度に制御することが強く要求されている。 As is clear from the above description, there is a strong demand to control the moving body with high accuracy by reducing the influence of errors that may occur in the output value of the acceleration sensor.

本発明に係る移動体は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、前記本体部の角速度を検出する検出部と、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、前記検出部及び前記計測部の各出力に基づいて予め算出された補正値に応じて前記検出部の出力値に含まれる誤差を補正した条件で、前記車輪部と前記本体部間の相対位置を制御するための指令を生成する指令生成部と、を備える。この構成を採用することによって、加速度検出部の出力値に生じうる誤差の影響を低減して、移動体を高精度に制御することができる。 The moving body according to the present invention is a moving body that moves spatially in a state where the relative position of the main body portion is controlled with respect to the wheel portion, the detection portion that detects the angular velocity of the main body portion, and the rotation of the wheel portion A measurement unit that measures a change in relative position between the main body unit and the wheel unit in a direction, and a detection value of the detection unit according to a correction value calculated in advance based on each output of the detection unit and the measurement unit A command generation unit configured to generate a command for controlling a relative position between the wheel unit and the main body unit under a condition in which an error included in the output value is corrected. By adopting this configuration, it is possible to reduce the influence of an error that may occur on the output value of the acceleration detection unit, and to control the moving body with high accuracy.

前記計測部の前記出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の前記出力値の変動を評価して前記補正値を算出する補正値算出部を更に備える、と良い。 Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the variation in the output value of the detection unit is evaluated to calculate the correction value. It is preferable to further include a correction value calculation unit.

前記補正値算出部は、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に取得した前記検出部及び前記計測部の各出力に基づいて前記補正値を算出する、と良い。 The correction value calculation unit may calculate the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center.

前記補正値算出部は、少なくとも、前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて、前記本体部に与えられる外力を推定する、と良い。 The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a drive source that relatively moves between the wheel unit and the main body, a torque generated by the drive source, and an output value of the measurement unit It is preferable to estimate the external force applied to the main body based on the angle value determined from

前記補正値算出部は、算出した前記外力の値が零になるタイミングを前記本体部が鉛直方向上に位置するタイミングとして検出する、と良い。 The correction value calculation unit may detect a timing at which the calculated value of the external force becomes zero as a timing at which the main body unit is positioned in the vertical direction.

前記検出部は、前記本体部の加速度も検出し、前記指令生成部は、前記センサの出力に基づいて、前記本体部の姿勢を評価して前記本体部の姿勢角度を算出する第１演算部と、前記第１演算部及び前記計測部の各出力に基づいて、前記指令を生成する第２演算部と、を備える、と良い。 The detection unit also detects acceleration of the main body, and the command generation unit evaluates the posture of the main body based on the output of the sensor and calculates a posture angle of the main body. And a second calculation unit that generates the command based on the outputs of the first calculation unit and the measurement unit.

前記補正値算出部は、算出した前記タイミングに基づいて、前記第１演算部の出力値の変動を評価して前記補正値を算出する、と良い。 The correction value calculation unit may calculate the correction value by evaluating a variation in the output value of the first calculation unit based on the calculated timing.

前記検出部は、コリオリの力を検出する機構を備える、と良い。 The detection unit may include a mechanism for detecting Coriolis force.

前記補正値を保持する補正値保持部を更に備える、と良い。 It is preferable to further include a correction value holding unit that holds the correction value.

前記本体部は、実質的に一定の速度で前記車輪部を回転中心として回転する、と良い。 The main body may rotate at a substantially constant speed with the wheel portion as a rotation center.

本発明に係る移動体は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、前記本体部の角速度を検出する検出部と、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の出力値の変動を評価して、前記検出部の出力値に含まれる誤差を補正するための補正値を算出する補正値算出部と、を備える。 The moving body according to the present invention is a moving body that moves spatially in a state where the relative position of the main body portion is controlled with respect to the wheel portion, the detection portion that detects the angular velocity of the main body portion, and the rotation of the wheel portion A measurement unit that measures a change in relative position between the main body unit and the wheel unit in a direction, and a timing at which the main body unit is positioned in the vertical direction based on an output of the measurement unit, A correction value calculating unit that evaluates fluctuations in the output value of the detection unit based on the timing and calculates a correction value for correcting an error included in the output value of the detection unit.

本発明に係る補正値の算出方法は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれた角速度センサ部の出力値を補正するための補正値の算出方法であって、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に前記角速度センサ部の出力を取得し、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力を取得し、前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して、前記角速度センサ部の出力値を補正するための補正値を算出する。 The correction value calculation method according to the present invention is a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit. In the calculation method, an output of the angular velocity sensor unit is acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center, and the main body unit rotates with the wheel unit in a fixed state as a rotation center. In a period, an output of a measurement unit that measures a change in relative position between the main body unit and the wheel unit in a rotation direction of the wheel unit is acquired, and the main body unit is in a vertical direction based on the output of the measurement unit The timing positioned above is calculated, and based on the calculated timing, the fluctuation of the output value of the angular velocity sensor unit is evaluated, and a correction value for correcting the output value of the angular velocity sensor unit is calculated.

本発明に係るプログラムは、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれる角速度センサ部の出力値を補正するための補正値をコンピュータに算出させるプログラムであって、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力値に基づいて、前記本体部が鉛直方向上に位置するタイミングを算出させ、算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して前記補正値を算出させる。 A program according to the present invention causes a computer to calculate a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit. The timing at which the main body is positioned in the vertical direction is determined based on the output value of the measurement unit that measures the change in the relative position between the main body and the wheel in the rotation direction of the wheel. Based on the calculated timing, the fluctuation value of the angular velocity sensor unit is evaluated to calculate the correction value.

本発明によれば、加速度センサの出力値に生じうる誤差の影響を低減して、移動体をより高精度に制御することができる。 ADVANTAGE OF THE INVENTION According to this invention, the influence of the error which can arise in the output value of an acceleration sensor can be reduced, and a moving body can be controlled more accurately.

本発明の第１実施形態にかかる移動体の概略的な模式図である。It is a schematic schematic diagram of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体の概略的なブロック図である。It is a schematic block diagram of the moving body concerning a 1st embodiment of the present invention. 本発明の第１実施形態にかかる移動体の通常動作を示す説明図である。It is explanatory drawing which shows the normal operation | movement of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体の姿勢制御を示す説明図である。It is explanatory drawing which shows the attitude | position control of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体のオフセット値算出動作を示す概略的なフローチャートである。It is a schematic flowchart which shows the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体のオフセット値算出動作時に用いられる治具の概略的な斜視図である。It is a schematic perspective view of the jig | tool used at the time of the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体のオフセット値算出動作を示す概略的な模式図である。It is a schematic diagram which shows the offset value calculation operation | movement of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体のエンコーダ部の出力値の時間変動を示す模式図である。It is a schematic diagram which shows the time fluctuation of the output value of the encoder part of the moving body concerning 1st Embodiment of this invention. 本発明の第１実施形態にかかる移動体が算出した外力値の時間変動を示す模式図である。It is a schematic diagram which shows the time fluctuation of the external force value which the mobile body concerning 1st Embodiment of this invention calculated. 本発明の第１実施形態にかかる移動体が算出した姿勢角度の時間変動を示す模式図である。It is a schematic diagram which shows the time fluctuation | variation of the attitude | position angle which the mobile body concerning 1st Embodiment of this invention calculated. 本発明の第１実施形態にかかる移動体の補正値の算出方法を示す概略的な模式図である。It is a schematic schematic diagram which shows the calculation method of the correction value of the moving body concerning 1st Embodiment of this invention.

以下、図面を参照しつつ、本発明の実施の形態について説明する。なお、各実施の形態は、説明の便宜上、簡略化されている。図面は簡略的なものであるから、図面の記載を根拠として本発明の技術的範囲を狭く解釈してはならない。図面は、もっぱら技術的事項の説明のためのものであり、図面に示された要素の正確な大きさ等は反映していない。同一の要素には、同一の符号を付し、重複する説明は省略するものとする。上下左右といった方向を示す言葉は、図面を正面視した場合を前提として用いるものとする。 Hereinafter, embodiments of the present invention will be described with reference to the drawings. Each embodiment is simplified for convenience of explanation. Since the drawings are simple, the technical scope of the present invention should not be interpreted narrowly based on the drawings. The drawings are only for explaining the technical matters, and do not reflect the exact sizes or the like of the elements shown in the drawings. The same elements are denoted by the same reference numerals, and redundant description is omitted. Words indicating directions such as up, down, left, and right are used on the assumption that the drawing is viewed from the front.

［第１実施形態］
以下、図１乃至図１１を参照して本発明の第１実施形態について説明する。図１は、移動体の概略的な模式図である。図２は、移動体の概略的なブロック図である。図３は、移動体の通常動作を示す説明図である。図４は、移動体の姿勢制御を示す説明図である。図５は、オフセット値算出動作を示す概略的なフローチャートである。図６は、オフセット値算出動作時に用いられる治具の概略的な斜視図である。図７は、オフセット値算出動作を示す概略的な模式図である。図８は、エンコーダ部の出力値の時間変動を示す模式図である。図９は、移動体が算出した外力の時間変動を示す模式図である。図１０は、移動体が算出した姿勢角度の時間変動を示す模式図である。図１１は、補正値の算出方法を示す概略的な模式図である。 [First Embodiment]
The first embodiment of the present invention will be described below with reference to FIGS. FIG. 1 is a schematic diagram of a moving body. FIG. 2 is a schematic block diagram of the moving body. FIG. 3 is an explanatory diagram showing a normal operation of the moving body. FIG. 4 is an explanatory diagram illustrating posture control of the moving body. FIG. 5 is a schematic flowchart showing an offset value calculation operation. FIG. 6 is a schematic perspective view of a jig used during the offset value calculation operation. FIG. 7 is a schematic diagram showing an offset value calculation operation. FIG. 8 is a schematic diagram showing temporal variation of the output value of the encoder unit. FIG. 9 is a schematic diagram showing temporal variation of the external force calculated by the moving body. FIG. 10 is a schematic diagram illustrating temporal variation of the posture angle calculated by the moving body. FIG. 11 is a schematic diagram illustrating a correction value calculation method.

図１に示すように、倒立二輪型ロボット（以下、単にロボットと呼ぶこともある）１００は、ヒトが搭乗する移動体であり、本体部５０、及び一組の車輪（車輪部）５１を有する。本体部５０は、起立部５０ａ、着座部５０ｂ、及び足台５０ｃを有する。一組の車輪５１は、車輪５１ａ、及び車輪５１ｂからなる。 As shown in FIG. 1, an inverted two-wheeled robot (hereinafter also simply referred to as a robot) 100 is a moving body on which a person is boarded, and has a main body 50 and a pair of wheels (wheels) 51. . The main body 50 includes a standing part 50a, a seating part 50b, and a footrest 50c. The set of wheels 51 includes a wheel 51a and a wheel 51b.

本体部５０には、各種センサ、マイコン、電源、及びリンク機構等の様々な電気／機械コンポーネントが内蔵される。着座部５０ｂは、搭乗者が着座する部分である。起立部５０ａは、搭乗者の背中を受け止める部分である。足台５０ｃは、搭乗者の足が置かれる部分である。車輪５１ａ、車輪５１ｂは、共通する軸線ＡＸ上に配置される。車輪５１ａの回転軸は、軸線ＡＸに一致する。車輪５１ｂの回転軸は、軸線ＡＸに一致する。 The main body 50 incorporates various electric / mechanical components such as various sensors, a microcomputer, a power supply, and a link mechanism. The seating part 50b is a part on which a passenger sits. The standing portion 50a is a portion that receives the back of the passenger. The footrest 50c is a portion on which the passenger's feet are placed. The wheel 51a and the wheel 51b are arranged on a common axis AX. The rotation axis of the wheel 51a coincides with the axis AX. The rotation axis of the wheel 51b coincides with the axis AX.

図１に模式的に示すように、ロボット１００は、モータ部５３、コントローラ５４、電源５５、及び入力部５６を有する。モータ部５３は、車輪５１ａ、車輪５１ｂに対応して、モータ５３ａ、及びモータ５３ｂを有する。モータ５３ａ、５３ｂの回転軸は、軸線ＡＸに一致するように配置されている。電源５５の出力は、コントローラ５４に供給される。また、電源５５の出力は、必要に応じて、入力部５６に供給される。コントローラ５４の出力は、モータ部５３に接続される。 As schematically illustrated in FIG. 1, the robot 100 includes a motor unit 53, a controller 54, a power supply 55, and an input unit 56. The motor unit 53 includes a motor 53a and a motor 53b corresponding to the wheel 51a and the wheel 51b. The rotation axes of the motors 53a and 53b are arranged so as to coincide with the axis AX. The output of the power supply 55 is supplied to the controller 54. Moreover, the output of the power supply 55 is supplied to the input part 56 as needed. The output of the controller 54 is connected to the motor unit 53.

コントローラ５４は、入力部５６からの入力値に応じて算出した指令値をモータ部５３に供給する。モータ５３ａは、コントローラ５４から供給される指令値に応じて、車輪５１ａを回転させる。モータ５３ｂも、同様に、コントローラ５４から供給される指令値に応じて、車輪５１ｂを回転させる。 The controller 54 supplies a command value calculated according to an input value from the input unit 56 to the motor unit 53. The motor 53a rotates the wheel 51a according to the command value supplied from the controller 54. Similarly, the motor 53b rotates the wheel 51b according to the command value supplied from the controller 54.

なお、入力部５６は、角速度センサ、加速度センサ、操作子等の入力装置に相当する。また、モータ５３ａ、モータ５３ｂは、コントローラ５４によって個別に制御される。車輪５１ａ、５１ｂの回転を個別に制御可能とすることによって、操縦者は、倒立二輪型ロボット１００に搭乗した状態で空間内を任意の方向へ移動することができる。 The input unit 56 corresponds to an input device such as an angular velocity sensor, an acceleration sensor, or an operator. The motor 53a and the motor 53b are individually controlled by the controller 54. By making it possible to individually control the rotation of the wheels 51a and 51b, the operator can move in the space in any direction while riding the inverted two-wheeled robot 100.

倒立二輪型ロボット１００の具体的な構成は任意である。車輪５１は複数である必要はない。本体部５０と車輪５１間の連結方法は任意である。モータ５３ａ、５３ｂの配置位置は任意である。 The specific configuration of the inverted two-wheeled robot 100 is arbitrary. There is no need for a plurality of wheels 51. The connection method between the main-body part 50 and the wheel 51 is arbitrary. The arrangement positions of the motors 53a and 53b are arbitrary.

図２を参照して、ロボット１００の概略的なブロック図を示す。図２に示すように、ロボット１００は、センサ部（検出部）１０、エンコーダ部（計測部）１５、指令値算出部（指令生成部）２０、オフセット値算出部（補正値算出部）３０、オフセット値格納部（補正値格納部）３５、及び駆動部４０を有する。なお、センサ部１０、及びエンコーダ部１５は、図１の入力部５６に含まれる。駆動部４０は、図１のモータ部５３に相当する。指令値算出部２０は、図１のコントローラ５４に含まれる。なお、指令値算出部２０、及びオフセット値算出部３０は、ハードウェア／ソフトウェアのいずれでも実現可能である。より好適には、ハードディスク等のストレージに格納したプログラムをＣＰＵ(Central Processing Unit)で実行することによって、指令値算出部２０及びオフセット値算出部３０の機能を実現すると良い。 Referring to FIG. 2, a schematic block diagram of the robot 100 is shown. As shown in FIG. 2, the robot 100 includes a sensor unit (detection unit) 10, an encoder unit (measurement unit) 15, a command value calculation unit (command generation unit) 20, an offset value calculation unit (correction value calculation unit) 30, An offset value storage unit (correction value storage unit) 35 and a drive unit 40 are included. The sensor unit 10 and the encoder unit 15 are included in the input unit 56 of FIG. The drive unit 40 corresponds to the motor unit 53 in FIG. The command value calculation unit 20 is included in the controller 54 of FIG. The command value calculation unit 20 and the offset value calculation unit 30 can be realized by either hardware / software. More preferably, the functions of the command value calculation unit 20 and the offset value calculation unit 30 may be realized by executing a program stored in a storage such as a hard disk by a CPU (Central Processing Unit).

センサ部１０は、角速度センサ部１１、及び加速度センサ部１２を有する。エンコーダ部１５は、右輪エンコーダ１６、及び左輪エンコーダ１７を有する。指令値算出部２０は、姿勢角度演算部（第１演算部）２１、及び倒立制御演算部（第２演算部）２２を有する。オフセット値算出部３０は、外力推定実行部３１、及びオフセット値演算実行部３２を有する。駆動部４０は、右輪駆動部４１、及び左輪駆動部４２を有する。なお、角速度センサ部１１は、ジャイロセンサと呼ばれることもある。 The sensor unit 10 includes an angular velocity sensor unit 11 and an acceleration sensor unit 12. The encoder unit 15 includes a right wheel encoder 16 and a left wheel encoder 17. The command value calculation unit 20 includes a posture angle calculation unit (first calculation unit) 21 and an inversion control calculation unit (second calculation unit) 22. The offset value calculation unit 30 includes an external force estimation execution unit 31 and an offset value calculation execution unit 32. The drive unit 40 includes a right wheel drive unit 41 and a left wheel drive unit 42. The angular velocity sensor unit 11 is sometimes called a gyro sensor.

はじめに、上述の各要素の接続関係について説明する。角速度センサ部１１の出力は、姿勢角度演算部２１に接続される。加速度センサ部１２の出力は、姿勢角度演算部２１に接続される。姿勢角度演算部２１の出力は、倒立制御演算部２２に接続される。倒立制御演算部２２の出力は、右輪駆動部４１及び左輪駆動部４２に接続される。右輪エンコーダ１６の出力は、倒立制御演算部２２に接続され、また、外力推定実行部３１に接続される。左輪エンコーダ１７の出力は、倒立制御演算部２２に接続され、また、外力推定実行部３１に接続される。外力推定実行部３１の出力は、オフセット値演算実行部３２に接続される。また、オフセット値演算実行部３２には、姿勢角度演算部２１の出力も接続される。オフセット値演算実行部３２の出力は、オフセット値格納部３５に接続される。オフセット値格納部３５の出力は、姿勢角度演算部２１に接続される。 First, the connection relationship between the above-described elements will be described. The output of the angular velocity sensor unit 11 is connected to the attitude angle calculation unit 21. The output of the acceleration sensor unit 12 is connected to the attitude angle calculation unit 21. The output of the attitude angle calculation unit 21 is connected to the inverted control calculation unit 22. The output of the inverted control calculation unit 22 is connected to the right wheel drive unit 41 and the left wheel drive unit 42. The output of the right wheel encoder 16 is connected to the inverted control calculation unit 22 and is also connected to the external force estimation execution unit 31. The output of the left wheel encoder 17 is connected to the inversion control calculation unit 22 and also connected to the external force estimation execution unit 31. The output of the external force estimation execution unit 31 is connected to the offset value calculation execution unit 32. The output of the attitude angle calculation unit 21 is also connected to the offset value calculation execution unit 32. The output of the offset value calculation execution unit 32 is connected to the offset value storage unit 35. The output of the offset value storage unit 35 is connected to the attitude angle calculation unit 21.

図３に示すように、ロボット１００は、センシング（ｓ１０１）、指令生成（ｓ１０２）、ドライブ（ｓ１０３）といった動作を順次実行する。具体的には、次のとおりである。 As shown in FIG. 3, the robot 100 sequentially performs operations such as sensing (s101), command generation (s102), and drive (s103). Specifically, it is as follows.

センサ部１０は、所定のタイミングでセンシングする（ｓ１０１）。角速度センサ部１１は、角速度を取得する。加速度センサ部１２は、加速度を取得する。エンコーダ部１５は、車輪５１の回転量を取得する。次に、指令値算出部２０は、角速度センサ部１１、加速度センサ部１２、及びエンコーダ部１５の出力値に応じて指令値を算出する（ｓ１０２）。次に、駆動部４０は、指令値算出部２０からの出力値に応じて、車輪５１ａ、５１ｂを個別に回転制御する（ｓ１０３）。ドライブ後のロボット１００は、上述の説明と同様にして、センシング、指令生成、ドライブといった各動作を実行する。 The sensor unit 10 performs sensing at a predetermined timing (s101). The angular velocity sensor unit 11 acquires an angular velocity. The acceleration sensor unit 12 acquires acceleration. The encoder unit 15 acquires the rotation amount of the wheel 51. Next, the command value calculation unit 20 calculates a command value according to the output values of the angular velocity sensor unit 11, the acceleration sensor unit 12, and the encoder unit 15 (s102). Next, the drive unit 40 individually controls the rotation of the wheels 51a and 51b in accordance with the output value from the command value calculation unit 20 (s103). The robot 100 after driving performs each operation such as sensing, command generation, and driving in the same manner as described above.

このようなフィードバック動作を繰り返すことによって、ロボット１００は、例えば、倒立状態を維持することができる。図４に模式的に示すように、ロボット１００は、倒立状態を維持するために、ロボット１００の固定軸ＡＸ２が鉛直方向に沿う軸線になす角度θ１の値を零（θ１＝０°）となるように指令値を生成する。なお、操縦者が入力した方向指示を考慮して指令値を算出すれば、操縦者の意図した位置へロボット１００を移動させることもできる。 By repeating such a feedback operation, the robot 100 can maintain an inverted state, for example. As schematically shown in FIG. 4, in order to maintain the robot 100 in an inverted state, the value of the angle θ1 formed by the fixed axis AX2 of the robot 100 along the axis along the vertical direction becomes zero (θ1 = 0 °). The command value is generated as follows. If the command value is calculated in consideration of the direction instruction input by the pilot, the robot 100 can be moved to the position intended by the pilot.

本実施形態では、角速度センサ部１１は、ＭＥＭＳ(Micro-Electro Mechanical Systems)を活用したセンサである。具体的には、角速度センサ部１１は、半導体プロセス技術を活用して、半導体、ガラス等の基板上に３次元構造を高精細に形成して製造される。角速度センサ部１１は、基板上に配置された振動体の空間的な変位を電気的に検出することによって、ジャイロセンサが受けるコリオリの力を電気的に検出する。例えば、角速度センサ部１１は、振動体を振動状態とし、振動体と検出電極間の容量値の変化を検出することによって、コリオリの力を電気的に検出する。なお、ジャイロセンサの具体的な構造、コリオリの力の検出方法は任意である。 In the present embodiment, the angular velocity sensor unit 11 is a sensor utilizing MEMS (Micro-Electro Mechanical Systems). Specifically, the angular velocity sensor unit 11 is manufactured by forming a three-dimensional structure with high definition on a substrate such as a semiconductor or glass by utilizing semiconductor process technology. The angular velocity sensor unit 11 electrically detects the Coriolis force received by the gyro sensor by electrically detecting the spatial displacement of the vibrating body arranged on the substrate. For example, the angular velocity sensor unit 11 electrically detects the Coriolis force by setting the vibrating body in a vibrating state and detecting a change in the capacitance value between the vibrating body and the detection electrode. The specific structure of the gyro sensor and the method for detecting the Coriolis force are arbitrary.

ロボット１００の高精度な姿勢制御を実現するためには、ロボット１００の固有軸ＡＸ２が鉛直方向に沿う軸線に一致したとき、角速度センサ部１１の出力値は零に示すことが要求される。しかしながら、様々な要因によって、角速度センサ部１１の出力値が経時的に劣化してしまう場合がある。角速度センサ部１１の出力値に誤差が含まれると、ロボット１００が鉛直方向に起立していると判断しているとき、実際には、鉛直方向上の位置からみて本体部５０が斜め前又は斜め後ろに傾いた状態にあるおそれがある。このような場合、搭乗者は、ロボット１００に搭乗して移動する際に、身体的なストレスを感じたり、自身の安定性を維持すること自体が難しくなってしまったりするおそれがある。また、加速度センサ部１２の出力値が経時的に劣化することによって、ロボット１００を定期的にメンテナンスする必要性が高まってしまうおそれがある。角速度センサ部１１の交換が前提になると、ロボット１００の管理コストが増大してしまうおそれがある。 In order to realize highly accurate posture control of the robot 100, it is required that the output value of the angular velocity sensor unit 11 be zero when the natural axis AX2 of the robot 100 coincides with the axis along the vertical direction. However, the output value of the angular velocity sensor unit 11 may deteriorate with time due to various factors. If the output value of the angular velocity sensor unit 11 includes an error, when it is determined that the robot 100 is standing in the vertical direction, the body unit 50 is actually tilted forward or diagonally as viewed from the position in the vertical direction. There is a risk of leaning back. In such a case, when the passenger rides on the robot 100 and moves, the passenger may feel physical stress or it may be difficult to maintain his / her own stability. Further, since the output value of the acceleration sensor unit 12 deteriorates with time, the need for regular maintenance of the robot 100 may increase. If the replacement of the angular velocity sensor unit 11 is assumed, the management cost of the robot 100 may increase.

本実施形態では、これらの点に鑑みて、角速度センサ部１１の出力値を補正するための補正値を算出する。そして、算出した補正値に基づいて、角速度センサ部１１の出力値に含まれる誤差の影響を低減する。具体的には、角速度センサ部１１の出力値が補正値によって補正された条件で、駆動部４０に伝達される指令値を算出する。これによって、角速度センサ部１１の出力値に含まれる誤差によってロボット１００の姿勢制御の精度が劣化してしまうことを抑制することができる。以下、上述の点を踏まえ、ロボット１００の構成及び動作についてより具体的に説明する。 In the present embodiment, in view of these points, a correction value for correcting the output value of the angular velocity sensor unit 11 is calculated. Then, based on the calculated correction value, the influence of the error included in the output value of the angular velocity sensor unit 11 is reduced. Specifically, the command value transmitted to the drive unit 40 is calculated under the condition that the output value of the angular velocity sensor unit 11 is corrected by the correction value. Thereby, it is possible to prevent the posture control accuracy of the robot 100 from deteriorating due to an error included in the output value of the angular velocity sensor unit 11. Hereinafter, the configuration and operation of the robot 100 will be described more specifically based on the above points.

角速度センサ部１１は、上述のように、ＭＥＭＳを活用したセンサであり、角速度を検出して、これに応じた値を出力する。なお、角速度センサ部１１は、アナログ値をデジタル値に変換してからデジタル値を出力するものとする。また、角速度センサ部１１は、ロール、ピッチ、ヨーの３軸方向の回転を角速度として検出するものとする。 As described above, the angular velocity sensor unit 11 is a sensor utilizing MEMS, detects the angular velocity, and outputs a value corresponding to the angular velocity. It is assumed that the angular velocity sensor unit 11 outputs a digital value after converting an analog value into a digital value. In addition, the angular velocity sensor unit 11 detects rotation in three axis directions of roll, pitch, and yaw as an angular velocity.

加速度センサ部１２は、角速度センサ部１１と同様に、ＭＥＭＳを活用したセンサであり、加速度を検出して、これに応じた値を出力する。なお、加速度センサ部１２は、アナログ値をデジタル値に変換し、Ａ／Ｄ変換により生成したデジタル値を出力する。また、加速度センサ部１２は、ロール、ピッチ、ヨーの３軸方向の加速度を検出するものとする。加速度センサ部１２の原理は当業者にとって周知であり、説明は省略する。 Similar to the angular velocity sensor unit 11, the acceleration sensor unit 12 is a sensor that utilizes MEMS, detects acceleration, and outputs a value corresponding to the detected acceleration. The acceleration sensor unit 12 converts an analog value into a digital value and outputs a digital value generated by A / D conversion. In addition, the acceleration sensor unit 12 detects acceleration in three axis directions of roll, pitch, and yaw. The principle of the acceleration sensor unit 12 is well known to those skilled in the art, and a description thereof will be omitted.

姿勢角度演算部２１は、角速度センサ部１１及び加速度センサ部１２から伝達される角速度値及び加速度値に基づいて姿勢角度を演算する。周知のように、加速度値の積分によって角度は算出される。加速度値の積分によって速度は算出される。ロボット１００の進行方向は、加速度の値から算出される。姿勢角度演算部２１は、これらの値から姿勢角度を算出する。姿勢角度演算部２１によって現時点の姿勢角度が出力される点を考慮すると、センサ部１０及び姿勢角度演算部２１を包括してジャイロセンサユニットと把握することもできる。 The posture angle calculation unit 21 calculates a posture angle based on the angular velocity value and the acceleration value transmitted from the angular velocity sensor unit 11 and the acceleration sensor unit 12. As is well known, the angle is calculated by integrating the acceleration values. The speed is calculated by integrating the acceleration values. The traveling direction of the robot 100 is calculated from the acceleration value. The posture angle calculation unit 21 calculates a posture angle from these values. Considering the point at which the current posture angle is output by the posture angle calculation unit 21, the sensor unit 10 and the posture angle calculation unit 21 can be comprehensively understood as a gyro sensor unit.

なお、姿勢角度とは、鉛直方向に沿う軸線に対してロボット１００の固有軸がなす角度を示し、図４に示すθ１に相当する。なお、図４では、ｙ軸は鉛直方向に沿う軸線であり、軸線ＡＸ２がロボット１００の固有軸に相当する。ロボット１００は、固有軸に対応するフレーム棒５２を有する。なお、姿勢角度の定義方法は任意であり、これを他の方法によって定義しても良い。 The posture angle indicates an angle formed by the natural axis of the robot 100 with respect to the axis along the vertical direction, and corresponds to θ1 shown in FIG. In FIG. 4, the y axis is an axis along the vertical direction, and the axis AX <b> 2 corresponds to a natural axis of the robot 100. The robot 100 has a frame bar 52 corresponding to the natural axis. The method for defining the posture angle is arbitrary, and this may be defined by other methods.

本実施形態では、姿勢角度演算部２１は、オフセット値格納部３５に格納されたオフセット値に応じて、角速度センサ部１１の出力値に含まれる誤差を補正した条件で、姿勢角度を算出する。例えば、角速度センサ部１１の出力値を積分して算出した角度値からオフセット値を減算する。これによって、角速度センサ部１１の出力値に生じる誤差によって、ロボット１００の姿勢制御の精度が劣化することを抑制することができる。なお、オフセット値格納部３５にオフセット値に格納されていないとき、姿勢角度演算部２１は、オフセット値を考慮せずに、上述のように角速度値及び加速度値に基づいて姿勢角度を演算する。 In the present embodiment, the posture angle calculation unit 21 calculates the posture angle on the condition that the error included in the output value of the angular velocity sensor unit 11 is corrected according to the offset value stored in the offset value storage unit 35. For example, the offset value is subtracted from the angle value calculated by integrating the output value of the angular velocity sensor unit 11. As a result, it is possible to prevent the posture control accuracy of the robot 100 from deteriorating due to an error occurring in the output value of the angular velocity sensor unit 11. When the offset value is not stored in the offset value storage unit 35, the posture angle calculation unit 21 calculates the posture angle based on the angular velocity value and the acceleration value as described above without considering the offset value.

倒立制御演算部２２は、姿勢角度演算部２１で算出した姿勢角度、及びエンコーダ部１５からの出力値に基づいて、ロボット１００を制御するための指令値を演算する。なお、倒立制御演算部２２は、操縦者による操作子の出力値に応じて、ロボット１００を前進等させることもできる。 The inverted control calculation unit 22 calculates a command value for controlling the robot 100 based on the posture angle calculated by the posture angle calculation unit 21 and the output value from the encoder unit 15. Note that the inverted control calculation unit 22 can also advance the robot 100 in accordance with the output value of the operator by the operator.

倒立制御演算部２２は、姿勢角度演算部２１の出力に基づいて、ロボット１００の現在の姿勢を検出する。倒立制御演算部２２は、エンコーダ部１５の出力に基づいて、車輪５１の回転方向における本体部５０と車輪５１間の相対的な位置変化を検出する。 The inverted control calculation unit 22 detects the current posture of the robot 100 based on the output of the posture angle calculation unit 21. The inverted control calculation unit 22 detects a relative position change between the main body unit 50 and the wheel 51 in the rotation direction of the wheel 51 based on the output of the encoder unit 15.

倒立制御演算部２２の具体的な演算手順は任意である。例えば、倒立制御演算部２２は、予め設定された目標値にセンサ部１０の出力値が近付くように指令値を生成する。同様に、倒立制御演算部２２は、予め設定された目標値にエンコーダ部１５の出力値が近付くように指令値を生成する。具体例を挙げると、静止して倒立状態を維持するため、倒立制御演算部２２は、角速度センサ部１１の出力値＝０、右輪エンコーダ１６、左輪エンコーダ１７の出力値＝０となるように指令値を生成する。また、姿勢角度演算部２１で算出した現在の姿勢角度が、図４に示すような場合、倒立制御演算部２２は、車輪５１ａ、５１ｂを後ろに回転させる制御を実行する。また、倒立して前方に進行するためには、倒立制御演算部２２は、角速度センサ部１１の出力値＝０、右輪エンコーダ１６、左輪エンコーダ１７の出力値＝整数となるように指令値を生成する。なお、上述のように倒立制御演算部２２の具体的な演算方法は任意であり、例えば、倒立制御演算部２２は、姿勢角度演算部２１、エンコーダ部１５からの入力値を所定の関係式に入力することによって指令値を算出する。 The specific calculation procedure of the inverted control calculation unit 22 is arbitrary. For example, the inverted control calculation unit 22 generates a command value so that the output value of the sensor unit 10 approaches a preset target value. Similarly, the inverted control calculation unit 22 generates a command value so that the output value of the encoder unit 15 approaches a preset target value. As a specific example, in order to maintain the inverted state in a stationary state, the inversion control calculation unit 22 is set so that the output value of the angular velocity sensor unit 11 = 0, and the output values of the right wheel encoder 16 and the left wheel encoder 17 = 0. Generate a command value. When the current posture angle calculated by the posture angle calculation unit 21 is as shown in FIG. 4, the inversion control calculation unit 22 performs control to rotate the wheels 51 a and 51 b backward. In order to move upside down and move forward, the inversion control calculation unit 22 sets the command value so that the output value of the angular velocity sensor unit 11 is 0, and the output values of the right wheel encoder 16 and the left wheel encoder 17 are integers. Generate. As described above, the specific calculation method of the inversion control calculation unit 22 is arbitrary. For example, the inversion control calculation unit 22 converts the input values from the attitude angle calculation unit 21 and the encoder unit 15 into a predetermined relational expression. The command value is calculated by inputting.

右輪駆動部４１は、倒立制御演算部２２から伝達する指令値に応じて、任意の方向に任意の回転量だけ右車を回転させる。左輪駆動部４２も、右輪駆動部４１と同様に、倒立制御演算部２２から伝達する指令値に応じて、任意の方向に任意の回転量だけ左輪を回転させる。 The right wheel drive unit 41 rotates the right wheel by an arbitrary amount of rotation in an arbitrary direction according to the command value transmitted from the inversion control calculation unit 22. Similarly to the right wheel drive unit 41, the left wheel drive unit 42 also rotates the left wheel by an arbitrary amount of rotation in an arbitrary direction according to the command value transmitted from the inversion control calculation unit 22.

右輪エンコーダ１６は、右輪の相対的な回転量を検出し、回転量に応じたカウント値を出力する。右輪エンコーダ１６の具体的な構成は任意である。例えば、光学式又は磁気式エンコーダのいずれかを採用しても良い。光学式エンコーダの場合、右輪エンコーダ１６は、車輪に設けられた回転体の外周領域に形成されたスリットの数を光学的に検出し、スリット数に応じたカウント値を出力する。磁気式エンコーダの場合、右輪エンコーダ１６は、上述の回転体の外周領域に順次配置された磁石の個数を磁気的に検出し、磁石数に応じたカウント値を出力しても良い。 The right wheel encoder 16 detects the relative rotation amount of the right wheel and outputs a count value corresponding to the rotation amount. The specific configuration of the right wheel encoder 16 is arbitrary. For example, either an optical or magnetic encoder may be employed. In the case of an optical encoder, the right wheel encoder 16 optically detects the number of slits formed in the outer peripheral area of the rotating body provided on the wheel, and outputs a count value corresponding to the number of slits. In the case of a magnetic encoder, the right wheel encoder 16 may magnetically detect the number of magnets sequentially arranged in the outer peripheral area of the rotating body and output a count value corresponding to the number of magnets.

左輪エンコーダ１７は、右輪エンコーダ１６と同様である。ただし、右輪の回転量に変えて、左輪の回転量を検出する。 The left wheel encoder 17 is the same as the right wheel encoder 16. However, the rotation amount of the left wheel is detected instead of the rotation amount of the right wheel.

オフセット値算出部３０は、後述の説明から明らかなように、エンコーダ部１５から出力されるカウント値に基づいて本体部５０が受ける外力の変化を推定し、この外力が零になるタイミングで姿勢角度演算部２１の出力の変動を評価してオフセット値を算出する。オフセット値格納部３５は、オフセット値算出部３０が算出したオフセット値を格納する。オフセット値格納部３５は、レジスタ等の一般的な記憶素子である。 As will be apparent from the following description, the offset value calculation unit 30 estimates a change in the external force received by the main body unit 50 based on the count value output from the encoder unit 15, and at the timing when the external force becomes zero, the posture angle The offset value is calculated by evaluating the fluctuation of the output of the calculation unit 21. The offset value storage unit 35 stores the offset value calculated by the offset value calculation unit 30. The offset value storage unit 35 is a general storage element such as a register.

次に、図５乃至図１１を参照して、オフセット値の算出方法について説明する。これらの説明からオフセット値算出部３０の動作は明らかになる。 Next, an offset value calculation method will be described with reference to FIGS. From these descriptions, the operation of the offset value calculation unit 30 becomes clear.

なお、上述のように、安価な角速度センサ部１１では、経時的に出力値に誤差が生じてしまうおそれがある。オフセット値は、角速度センサ部１１の出力値の誤差による悪影響を回避するために算出される。従って、通常、オフセット値を算出する時点は、ロボット１００の出荷後、相当期間が経過した後になる。但し、ロボット１００の製造直後にオフセット値を求め、ロボット１００の初期不良の有無を検査しても良い。つまり、オフセット値を算出する時点は任意である。また、オフセット値格納部３５には、オフセット値は格納されていないものとする。 As described above, the inexpensive angular velocity sensor unit 11 may cause an error in the output value over time. The offset value is calculated in order to avoid an adverse effect due to an error in the output value of the angular velocity sensor unit 11. Therefore, the time point at which the offset value is calculated is usually after a considerable period has elapsed after the robot 100 is shipped. However, an offset value may be obtained immediately after the manufacture of the robot 100 to inspect whether the robot 100 has an initial defect. That is, the time point at which the offset value is calculated is arbitrary. It is assumed that no offset value is stored in the offset value storage unit 35.

図５に示すように、まず初期設定をする（ｓ２０１）。具体的には、図６に示す治具６０によって図７に模式的に示すようにロボット１００を固定する。 As shown in FIG. 5, first, initialization is performed (s201). Specifically, the robot 100 is fixed by the jig 60 shown in FIG. 6 as schematically shown in FIG.

図６に示すように、治具６０は、ベース板６１、柱６２、及び凸部６３を有する。治具６０は、例えば、金属等の成形により製造される。 As shown in FIG. 6, the jig 60 includes a base plate 61, a column 62, and a convex portion 63. The jig 60 is manufactured by molding metal or the like, for example.

図７に模式的に示すように、一対の治具６０によって、ロボット１００の車輪５１を固定する。なお、車輪５１には、凸部６３を受け入れる凹部が設けられているものとする。なお、凸部６３を角柱状に形成し、これに対応する凹部を車輪５１に設けることによって、治具６０と車輪５１とを嵌め合いにより位置決めさせても良い。この場合には、車輪５１を確実に固定状態にすることができる。 As schematically shown in FIG. 7, the wheels 51 of the robot 100 are fixed by a pair of jigs 60. It is assumed that the wheel 51 is provided with a concave portion that receives the convex portion 63. In addition, you may position the jig | tool 60 and the wheel 51 by fitting by forming the convex part 63 in prismatic shape, and providing the recessed part corresponding to this in the wheel 51. FIG. In this case, the wheel 51 can be reliably fixed.

次に、回転指示をする（ｓ２０２）。具体的には、例えばタイミングコントローラ等の指示に応じて、倒立制御演算部２２は指令を生成し、駆動部４０にトルクを生じさせる。この状態で、ヒトが本体部５０から手を離すと、本体部５０は、図７に模式的に示すように、最も後方に傾斜した位置から最も前方に傾斜した位置へ車輪５１を回転中心として回転する。なお、本実施形態では、本体部５０の回転速度が一定となるように、倒立制御演算部２２は指令を生成するものとする。 Next, a rotation instruction is given (s202). Specifically, for example, in response to an instruction from a timing controller or the like, the inversion control calculation unit 22 generates a command and causes the drive unit 40 to generate torque. In this state, when a person releases his / her hand from the main body 50, the main body 50 moves from the position inclined most backward to the position inclined most forward, as shown schematically in FIG. Rotate. In the present embodiment, the inverted control calculation unit 22 generates a command so that the rotation speed of the main body 50 is constant.

次に、センシングをする（ｓ２０３）。具体的には、回転指示の直後、タイミングコントローラの指示に応じて、角速度センサ部１１は、センシングを開始する。加速度センサ部１２も同様である。右輪エンコーダ１６、及び左輪エンコーダ１７も同様である。 Next, sensing is performed (s203). Specifically, immediately after the rotation instruction, the angular velocity sensor unit 11 starts sensing according to the instruction of the timing controller. The same applies to the acceleration sensor unit 12. The same applies to the right wheel encoder 16 and the left wheel encoder 17.

次にオフセット値の算出処理を実行する（ｓ２０４）。具体的には、次のとおりである。 Next, an offset value calculation process is executed (s204). Specifically, it is as follows.

まず、外力推定実行部３１は、右輪エンコーダ１６及び左輪エンコーダ１７の一方または両方から出力されるカウント値に基づいて、次の式（１）により外力を推定する。ただし、各符号は、次のとおりである。τｅは、駆動部４０に発生するトルクである。Ｉは、本体部５０の慣性モーメントである。ｑは、エンコーダ部１５の出力値から算出された角度値である。Ｄは、モータの粘性係数（駆動部４０の固有特性値）である。τａは、外力（外力トルク）である。
・・・（１） First, the external force estimation execution unit 31 estimates the external force by the following equation (1) based on the count value output from one or both of the right wheel encoder 16 and the left wheel encoder 17. However, each code | symbol is as follows. τe is a torque generated in the drive unit 40. I is the moment of inertia of the main body 50. q is an angle value calculated from the output value of the encoder unit 15. D is the viscosity coefficient of the motor (the characteristic characteristic value of the drive unit 40). τa is an external force (external force torque).
... (1)

エンコーダ部１５から出力されるカウント値は、図８に示すように時間軸に沿って一定の割合で増加する。これは、上述のように、センシング期間、車輪５１を回転中心として本体部５０は一定の速度で回転するためである。各時点におけるカウント値から上述の角度値ｑが定まる。なお、角度値は、相対的な角度情報である。 The count value output from the encoder unit 15 increases at a constant rate along the time axis as shown in FIG. This is because, as described above, the main body 50 rotates at a constant speed with the wheel 51 as the rotation center during the sensing period. The angle value q described above is determined from the count value at each time point. The angle value is relative angle information.

駆動部に発生するトルクτｅは、倒立制御演算部２２で生成される指令値から算出される。本体部５０の慣性モーメントＩ、モータの粘性係数Ｄは、予め既知である。 Torque τe generated in the drive unit is calculated from a command value generated by the inverted control calculation unit 22. The moment of inertia I of the main body 50 and the viscosity coefficient D of the motor are known in advance.

上述の値を式（１）に代入して算出処理することによって、外力τａが算出される。なお、カウント値は、時系列順に式（１）に代入するものとする。これによって外力τａの時間変化を計算することができる。つまり、図８に示すカウント値の時間変化に基づいて、図９に示す外力の時間変化を検出することができる。 The external force τa is calculated by substituting the above-described value into the expression (1) and performing calculation processing. It is assumed that the count value is substituted into equation (1) in time series order. Thereby, the time change of the external force τa can be calculated. That is, it is possible to detect the time change of the external force shown in FIG. 9 based on the time change of the count value shown in FIG.

外力の推定後、オフセット値を算出する。上述の外力の推定処理によって、図９に示す外力の時間変化を検出することができる。外力の値が零のとき、本体部５０は、鉛直方向に沿う軸線上に位置することが判定できる。 After the external force is estimated, an offset value is calculated. With the above-described external force estimation process, it is possible to detect the temporal change of the external force shown in FIG. When the value of the external force is zero, it can be determined that the main body 50 is located on the axis along the vertical direction.

他方、上述のセンシング時、図１０に示すように角速度センサ部１１及び加速度センサ部１２の出力値に基づいて、姿勢角度演算部２１は、姿勢角度を算出している。そして、姿勢角度演算部２１の出力値は、オフセット値演算実行部３２に供給される。 On the other hand, at the time of sensing described above, the posture angle calculation unit 21 calculates the posture angle based on the output values of the angular velocity sensor unit 11 and the acceleration sensor unit 12 as shown in FIG. Then, the output value of the posture angle calculation unit 21 is supplied to the offset value calculation execution unit 32.

オフセット値演算実行部３２は、図１１に模式的に示すように、外力推定実行部３１の出力値から外力が零になる時刻ｔ１を算出し、また、時刻ｔ１時の姿勢角度演算部２１の出力値を算出する。オフセット値演算実行部３２は、算出した時刻ｔ１時の姿勢角度演算部２１の出力値（図１１では、−５度）をオフセット値とする。このようにして、エンコーダ部１５及びセンサ部１０の各出力値に基づいて、オフセット値が算出される。なお、オフセット値の算出方法は任意である。外力τａが零になる時刻と姿勢角度が０度になる時刻間の差分の算出に基づいてオフセット値を算出しても良い。 As schematically shown in FIG. 11, the offset value calculation execution unit 32 calculates a time t1 when the external force becomes zero from the output value of the external force estimation execution unit 31, and the posture angle calculation unit 21 at the time t1 Calculate the output value. The offset value calculation execution unit 32 uses the calculated output value (−5 degrees in FIG. 11) of the posture angle calculation unit 21 at time t1 as an offset value. In this way, the offset value is calculated based on the output values of the encoder unit 15 and the sensor unit 10. The offset value calculation method is arbitrary. The offset value may be calculated based on the calculation of the difference between the time when the external force τa becomes zero and the time when the posture angle becomes 0 degrees.

次に、オフセット値を格納する（ｓ２０５）。具体的には、オフセット値演算実行部３２は、算出したオフセット値をオフセット値格納部３５へ転送する。オフセット値格納部３５は、オフセット値演算実行部３２から伝達したオフセット値を保持する。オフセット値格納部３５にオフセット値が格納されることによって、姿勢角度演算部２１は、上述のように、オフセット値によって角速度センサ部１１の出力値に含まれる誤差を補正した条件で姿勢角度を算出する。 Next, the offset value is stored (s205). Specifically, the offset value calculation execution unit 32 transfers the calculated offset value to the offset value storage unit 35. The offset value storage unit 35 holds the offset value transmitted from the offset value calculation execution unit 32. By storing the offset value in the offset value storage unit 35, the posture angle calculation unit 21 calculates the posture angle on the condition that the error included in the output value of the angular velocity sensor unit 11 is corrected by the offset value as described above. To do.

上述の説明から明らかなように、本実施形態では、角速度センサ部１１の出力値が経時的に劣化するおそれがあることに鑑みて、角速度センサ部１１に含まれる誤差を補正した条件で指令を生成して駆動部を駆動する。これによって、安価な角速度センサ部１１を採用することができ、ロボット１００の低コスト化を図ることができる。 As is apparent from the above description, in the present embodiment, in consideration of the possibility that the output value of the angular velocity sensor unit 11 may deteriorate with time, the command is issued under the condition that the error included in the angular velocity sensor unit 11 is corrected. Generate and drive the drive unit. Thereby, the inexpensive angular velocity sensor unit 11 can be employed, and the cost of the robot 100 can be reduced.

また、本実施形態では、鉛直方向に沿う軸線上に本体部５０が位置するときに外力の値が零に近付くことに鑑みて、エンコーダ部１５の出力値に基づいて外力を推定し、外力が零になる時刻に基づいて姿勢角度の時間変化を評価してオフセット値を算出する。これによって、既存のエンコーダ部１５を活用して、オフセット値を算出することができ、別途新たなハードウェアを用意する必要性を解消することができる。この点は、コンパクト性も要求される倒立二輪型の移動体においては特に有利である。なお、姿勢角度が角速度に基づいて定まる点を考慮すれば、外力が零になる時刻に基づいて角速度センサ部１１の出力値の変動を評価してオフセット値を算出する、とも把握することができる。 Further, in the present embodiment, in view of the fact that the value of the external force approaches zero when the main body 50 is positioned on the axis along the vertical direction, the external force is estimated based on the output value of the encoder unit 15, and the external force is The offset value is calculated by evaluating the time change of the posture angle based on the time when it becomes zero. As a result, the offset value can be calculated by utilizing the existing encoder unit 15, and the necessity of separately preparing new hardware can be eliminated. This is particularly advantageous in an inverted two-wheeled mobile body that also requires compactness. If the point at which the posture angle is determined based on the angular velocity is taken into account, it can be understood that the offset value is calculated by evaluating the fluctuation of the output value of the angular velocity sensor unit 11 based on the time when the external force becomes zero. .

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。角速度センサ部１１の具体的な構造は任意である。加速度センサ部１２の具体的な構造も任意である。指令値算出部２０の具体的な算出方法は任意である。オフセット値算出部３０の具体的な算出方法は任意である。オフセット値格納部３５は、着脱可能なメモリデバイスであっても良い。オフセット値算出部３０をロボット１００に組み込む必要はなく、それを外部のパソコンに格納し、外部のパソコンで実行しても良い。本体部５０と車輪５１間は、複数関節のリンク機構を介して接続しても良い。タイミングコントローラの搭載は任意である。オフセット値の算出動作を複数回実行し、これらの平均値を算出し、これをオフセット値格納部３５に格納しても良い。また、上述の説明によれば、補正値をコンピュータに算出させるプログラムの内容も自明である。 Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. The specific structure of the angular velocity sensor unit 11 is arbitrary. The specific structure of the acceleration sensor unit 12 is also arbitrary. The specific calculation method of the command value calculation unit 20 is arbitrary. The specific calculation method of the offset value calculation unit 30 is arbitrary. The offset value storage unit 35 may be a removable memory device. The offset value calculation unit 30 does not need to be incorporated in the robot 100, but may be stored in an external personal computer and executed on the external personal computer. You may connect between the main-body part 50 and the wheel 51 via the link mechanism of multiple joints. The timing controller is optional. The offset value calculation operation may be executed a plurality of times, an average value of these may be calculated, and stored in the offset value storage unit 35. Further, according to the above description, the contents of the program that causes the computer to calculate the correction value are also obvious.

１００倒立二輪型ロボット

１０センサ部
１１角速度センサ部
１２加速度センサ部

１５エンコーダ部
１６右輪エンコーダ
１７左輪エンコーダ

２０指令値算出部
２１姿勢角度演算部
２２倒立制御演算部

３０オフセット値算出部
３１外力推定実行部
３２オフセット値演算実行部

３５オフセット値格納部

４０駆動部
４１右輪駆動部
４２左輪駆動部

５０本体部
５０ａ起立部
５０ｂ着座部
５０ｃ足台

５１車輪
５１ａ車輪
５１ｂ車輪

５３モータ部
５４コントローラ
５５電源
５６入力部

６０治具
６１ベース板
６２柱
６３凸部 100 Inverted two-wheeled robot

DESCRIPTION OF SYMBOLS 10 Sensor part 11 Angular velocity sensor part 12 Acceleration sensor part

15 Encoder section 16 Right wheel encoder 17 Left wheel encoder

20 Command value calculator 21 Attitude angle calculator 22 Inverted control calculator

30 Offset value calculation unit 31 External force estimation execution unit 32 Offset value calculation execution unit

35 Offset value storage

40 Driving part 41 Right wheel driving part 42 Left wheel driving part

50 Main body 50a Standing part 50b Seating part 50c Footrest

51 Wheel 51a Wheel 51b Wheel

53 Motor section 54 Controller 55 Power supply 56 Input section

60 Jig 61 Base plate 62 Column 63 Projection

Claims

車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、
前記本体部の角速度を検出する検出部と、
前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、
前記検出部及び前記計測部の各出力に基づいて予め算出された補正値に応じて前記検出部の出力値に含まれる誤差を補正した条件で、前記車輪部と前記本体部間の相対位置を制御するための指令を生成する指令生成部と、
を備える移動体。 A moving body that moves in space in a state in which the relative position of the main body is controlled with respect to the wheel,
A detector for detecting an angular velocity of the main body;
A measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit;
The relative position between the wheel unit and the main body unit is obtained under the condition that the error included in the output value of the detection unit is corrected according to the correction value calculated in advance based on the outputs of the detection unit and the measurement unit. A command generation unit that generates a command to control;
A moving body comprising:

前記計測部の前記出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の前記出力値の変動を評価して前記補正値を算出する補正値算出部を更に備えることを特徴とする請求項１に記載の移動体。 Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the variation in the output value of the detection unit is evaluated to calculate the correction value. The mobile body according to claim 1, further comprising a correction value calculation unit for performing the correction.

前記補正値算出部は、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に取得した前記検出部及び前記計測部の各出力に基づいて前記補正値を算出することを特徴とする請求項２に記載の移動体。 The correction value calculation unit calculates the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center. The moving body according to claim 2.

前記補正値算出部は、少なくとも、前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて、前記本体部に与えられる外力を推定することを特徴とする請求項３に記載の移動体。 The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a drive source that relatively moves between the wheel unit and the main body, a torque generated by the drive source, and an output value of the measurement unit The moving body according to claim 3, wherein an external force applied to the main body portion is estimated based on an angle value determined from

前記補正値算出部は、算出した前記外力の値が零になるタイミングを前記本体部が鉛直方向上に位置するタイミングとして検出することを特徴とする請求項２乃至４のいずれか一項に記載の移動体。 5. The correction value calculation unit detects a timing at which the calculated value of the external force becomes zero as a timing at which the main body portion is positioned in the vertical direction. 6. Moving body.

前記検出部は、前記本体部の加速度も検出し、
前記指令生成部は、前記検出部の出力に基づいて、前記本体部の姿勢を評価して前記本体部の姿勢角度を算出する第１演算部と、前記第１演算部及び前記計測部の各出力に基づいて、前記指令を生成する第２演算部と、を備えることを特徴とする請求項１乃至５のいずれか一項に記載の移動体。 The detection unit also detects acceleration of the main body,
The command generation unit is configured to evaluate a posture of the main body unit based on an output of the detection unit and calculate a posture angle of the main body unit, and each of the first calculation unit and the measurement unit. The mobile body according to claim 1, further comprising: a second calculation unit that generates the command based on an output.

前記補正値算出部は、算出した前記タイミングに基づいて、前記第１演算部の出力値の変動を評価して前記補正値を算出することを特徴とする請求項６に記載の移動体。 The mobile object according to claim 6, wherein the correction value calculation unit calculates the correction value by evaluating a variation in an output value of the first calculation unit based on the calculated timing.

前記検出部は、コリオリの力を検出する機構を備えることを特徴とする請求項１乃至７のいずれか一項に記載の移動体。 The mobile unit according to claim 1, wherein the detection unit includes a mechanism that detects Coriolis force.

前記補正値を保持する補正値保持部を更に備えることを特徴とする請求項１乃至８のいずれか一項に記載の移動体。 The mobile body according to claim 1, further comprising a correction value holding unit that holds the correction value.

前記本体部は、実質的に一定の速度で前記車輪部を回転中心として回転することを特徴とする請求項１乃至９のいずれか一項に記載の移動体。 The moving body according to any one of claims 1 to 9, wherein the main body portion rotates at a substantially constant speed with the wheel portion as a rotation center.

車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、
前記本体部の角速度を検出する検出部と、
前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、
前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の出力値の変動を評価して、前記検出部の出力値に含まれる誤差を補正するための補正値を算出する補正値算出部と、
を備える、移動体。 A moving body that moves in space in a state in which the relative position of the main body is controlled with respect to the wheel,
A detector for detecting an angular velocity of the main body;
A measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit;
Based on the output of the measurement unit, the timing at which the main body unit is positioned in the vertical direction is calculated, and based on the calculated timing, the fluctuation of the output value of the detection unit is evaluated, and the output value of the detection unit A correction value calculation unit for calculating a correction value for correcting an error included in
A moving object comprising:

前記補正値に応じて前記検出部の出力値に含まれる誤差を補正した条件で、前記車輪部と前記本体部間の相対位置を制御するための指令を生成する指令生成部を更に備えることを特徴とする請求項１１に記載の移動体。 A command generation unit configured to generate a command for controlling a relative position between the wheel unit and the main body unit under a condition in which an error included in the output value of the detection unit is corrected according to the correction value; The moving body according to claim 11, wherein the moving body is characterized.

前記補正値算出部は、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に取得した前記検出部及び前記計測部の各出力に基づいて前記補正値を算出することを特徴とする請求項１１又は１２に記載の移動体。 The correction value calculation unit calculates the correction value based on outputs of the detection unit and the measurement unit acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center. The moving body according to claim 11 or 12.

前記補正値算出部は、少なくとも、前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて、前記本体部に与えられる外力を推定することを特徴とする請求項１３に記載の移動体。 The correction value calculation unit includes at least a moment of inertia of the main body, a characteristic value of a driving source that relatively moves between the wheel unit and the main body, a torque generated by the driving source, and an output value of the measuring unit The moving body according to claim 13, wherein an external force applied to the main body portion is estimated based on an angle value determined from:

車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれた角速度センサ部の出力値を補正するための補正値の算出方法であって、
固定状態の前記車輪部を回転中心として前記本体部が回転する期間に前記角速度センサ部の出力を取得し、
固定状態の前記車輪部を回転中心として前記本体部が回転する期間に、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力を取得し、
前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、
算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して、前記角速度センサ部の出力値を補正するための補正値を算出する、補正値の算出方法。 A method of calculating a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially in a state where the relative position of the main body unit is controlled with respect to the wheel unit,
The output of the angular velocity sensor unit is acquired during a period in which the main body unit rotates with the wheel unit in a fixed state as a rotation center,
The output of the measuring unit that measures the change in the relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit is acquired during the period in which the main body unit rotates with the wheel unit in the fixed state as the rotation center. And
Calculate the timing at which the main body is positioned in the vertical direction based on the output of the measurement unit,
A correction value calculation method for evaluating a variation in an output value of the angular velocity sensor unit based on the calculated timing and calculating a correction value for correcting the output value of the angular velocity sensor unit.

前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて前記本体部に与えられる外力を推定することを特徴とする請求項１５に記載の補正値の算出方法。 The main body based on an inertial moment of the main body, a characteristic value of a driving source that relatively moves between the wheel portion and the main body, a torque generated by the driving source, and an angle value determined from an output value of the measuring section. The correction value calculation method according to claim 15, wherein an external force applied to the unit is estimated.

前記角速度センサ部は、コリオリの力を検出する機構を備えることを特徴とする請求項１５又は１６に記載の補正値の算出方法。 The correction value calculation method according to claim 15, wherein the angular velocity sensor unit includes a mechanism for detecting Coriolis force.

車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれる角速度センサ部の出力値を補正するための補正値をコンピュータに算出させるプログラムであって、
前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力値に基づいて、前記本体部が鉛直方向上に位置するタイミングを算出させ、
算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して前記補正値を算出させる、プログラム。 A program for causing a computer to calculate a correction value for correcting an output value of an angular velocity sensor unit incorporated in a moving body that moves spatially with a relative position of a main body unit controlled with respect to a wheel unit,
Based on an output value of a measurement unit that measures a change in relative position between the main body unit and the wheel unit in the rotation direction of the wheel unit, the timing at which the main body unit is positioned in the vertical direction is calculated.
A program for evaluating the fluctuation of the output value of the angular velocity sensor unit based on the calculated timing and calculating the correction value.