JP6149791B2 - Center of gravity estimation device and center of gravity estimation method - Google Patents

Center of gravity estimation device and center of gravity estimation method Download PDF

Info

Publication number
JP6149791B2
JP6149791B2 JP2014089943A JP2014089943A JP6149791B2 JP 6149791 B2 JP6149791 B2 JP 6149791B2 JP 2014089943 A JP2014089943 A JP 2014089943A JP 2014089943 A JP2014089943 A JP 2014089943A JP 6149791 B2 JP6149791 B2 JP 6149791B2
Authority
JP
Japan
Prior art keywords
robot
center
gravity
angular velocity
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2014089943A
Other languages
Japanese (ja)
Other versions
JP2015208790A (en
Inventor
将弘 土井
将弘 土井
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Priority to JP2014089943A priority Critical patent/JP6149791B2/en
Publication of JP2015208790A publication Critical patent/JP2015208790A/en
Application granted granted Critical
Publication of JP6149791B2 publication Critical patent/JP6149791B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)

Description

本発明は、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法に関するものである。   The present invention relates to a center-of-gravity estimation device and a center-of-gravity estimation method that can accurately estimate the center of gravity position and / or speed of a robot.

環境とロボットとが接触する接触力を検出し、該検出した接触力に基づいてＺＭＰ（ZeroMomentPoint）方程式等により幾何学的にロボットの重心位置や速度を推定するロボットが知られている（例えば、特許文献１参照）。   A robot that detects a contact force between an environment and a robot and geometrically estimates the position and speed of the center of gravity of the robot by a ZMP (ZeroMomentPoint) equation based on the detected contact force is known (for example, Patent Document 1).

特開２００９−２８５８１６号公報JP 2009-285816 A

しかしながら、上記ロボットにおいては、幾何学的な方法を用いているため、関節部の撓みなどによる関節角度の誤差の影響を受け、高精度にロボットの重心位置や速度を推定するのが困難となる。
本発明は、このような問題点を解決するためになされたものであり、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法を提供することを主たる目的とする。 However, since the above-described robot uses a geometric method, it is difficult to estimate the position and speed of the center of gravity of the robot with high accuracy due to the effects of joint angle errors caused by the bending of the joints. .
The present invention has been made to solve such problems, and has as its main object to provide a center of gravity estimation device and a center of gravity estimation method capable of estimating the center of gravity position and / or speed of a robot with high accuracy. To do.

上記目的を達成するための本発明の一態様は、環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出する力検出手段と、前記ロボットの胴体部に作用する角速度を検出する角速度検出手段と、前記ロボットの胴体部に作用する加速度を検出する加速度検出手段と、前記ロボットの各関節の角度を検出する角度検出手段と、前記ロボットの各接触点の姿勢及び位置を取得する取得手段と、前記角速度検出手段により検出された角速度と、前記加速度検出手段により検出された加速度と、前記角度検出手段により検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出する角運動量算出手段と、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定する第１推定手段と、を備える、ことを特徴とする重心推定装置である。
この一態様において、前記第１推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出されてもよい。
この一態様において、前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第２推定手段を更に備え、前記角運動量算出手段は、前記第２推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備えていてもよい。
この一態様において、前記力検出手段は、前記各接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力と、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメント力を検出してもよい。
上記目的を達成するための本発明の一態様は、環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、前記ロボットの胴体部に作用する角速度を検出するステップと、前記ロボットの胴体部に作用する加速度を検出するステップと、前記ロボットの各関節の角度を検出するステップと、前記ロボットの各接触点の姿勢及び位置を取得するステップと、前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法であってもよい。 In order to achieve the above object, one aspect of the present invention includes a force detection unit that detects forces acting on a plurality of contact points of an articulated robot that contacts an environment, and an angular velocity that acts on a body portion of the robot. Angular velocity detection means for detecting the acceleration, acceleration detection means for detecting acceleration acting on the body of the robot, angle detection means for detecting the angle of each joint of the robot, and posture and position of each contact point of the robot The center of gravity of the robot based on the acquisition means for acquiring the angular velocity detected by the angular velocity detection means, the acceleration detected by the acceleration detection means, and the angle of each joint detected by the angle detection means and angular momentum calculating means for calculating an angular momentum around the force of each contact point detected by the force detecting means, the posture of the contact point obtained by the obtaining means and Comprises a location, and the angular momentum of the center of gravity around calculated by the angular momentum calculating means, and a first estimating means for estimating at least one of the center-of-gravity position and velocity of the robot by using a Kalman filter based on, This is a center-of-gravity estimation device.
In this one aspect, the first estimation means includes the force of each contact point detected by the force detection means, the posture and position of each contact point acquired by the acquisition means, and the angular momentum calculation means. Based on the calculated angular momentum around the center of gravity, a state equation with the position and speed of the center of gravity of the robot as state variables, and an observation equation with the angular momentum around the center of gravity of the robot as observations, A Kalman filter is applied to estimate the position and velocity of the center of gravity of the robot, and the equation of state and the observation equation may be derived based on a motion equation of force acting in a translational direction and a rotational direction with respect to the center of gravity of the robot. Good.
In this aspect, the robot is an articulated robot having a plurality of the joints and a plurality of links connected to each other via the joints, and each joint detected by the angle detection unit. A joint angular velocity calculating means for calculating an angular velocity of each joint based on the angle of the angle; the angular momentum calculating means calculated by the joint angular velocity calculating means and the angle of each joint detected by the angle detecting means; Based on the angular velocity of each joint, the posture angle and angular velocity of the body part, the mass, position and moment of inertia of each link, and the gravity center position of the robot, the angular momentum due to the translational motion of each link of the robot And the angular momentum due to the rotational motion may be added to calculate the angular momentum around the center of gravity of the robot.
In this one aspect, the posture value offset value of the body part of the robot is defined using a quaternion, and the state equation including system noise with the quaternion and the angular velocity offset value of the angular velocity detecting means as state variables And applying an Kalman filter to the observation equation including the observation noise with the acceleration of the body part as an observation value, estimating the state variable, and based on the estimated state variable, the attitude angle of the body part And a second estimating means for estimating the angular velocity, wherein the angular momentum calculating means includes the posture angle and angular velocity of the body portion estimated by the second estimating means, and the angle of each joint detected by the angle detecting means. When the angular velocity of each joint, the mass and moment of inertia of each link, and the position of the center of gravity of the robot, based on the angular momentum about the center of gravity of the robot is calculated It may be.
In this aspect, a high-pass filter that removes low-frequency noise from the angular velocity of the robot body detected by the angular velocity detection means, and high-frequency noise from the acceleration of the robot body detected by the acceleration detection means. A low-pass filter, and the angular momentum calculating means includes: an angular velocity of the trunk portion from the high-pass filter; an acceleration of the trunk portion from the low-pass filter; and an angle of each joint detected by the angle detection means. the angular velocity of each joint, the mass and moment of inertia of each link, and the position of the center of gravity of the robot, based on, may be calculated angular momentum about the center of gravity of the robot.
In this aspect, based on the posture angle of each contact point acquired by the acquisition unit, the conversion unit converts the force in the coordinate system of the force detection unit output from the force detection unit into a force in the global coordinate system. May be further provided.
In this aspect, the force detection means detects the forces in the x-axis direction, the y-axis direction, and the z-axis direction and the moment forces around the x-axis, the y-axis, and the z-axis at each contact point. May be.
In order to achieve the above object, one aspect of the present invention is to detect a force acting on a plurality of contact points of an articulated robot that comes into contact with the environment, and to detect an angular velocity acting on the body of the robot. Detecting the acceleration acting on the body of the robot, detecting the angle of each joint of the robot, obtaining the posture and position of each contact point of the robot, and the detection Calculating an angular momentum about the center of gravity of the robot based on the detected angular velocity, the detected acceleration, and the detected angle of each joint; and the force of each detected contact point; Of the position and speed of the center of gravity of the robot using a Kalman filter based on the obtained posture and position of each contact point and the calculated angular momentum around the center of gravity Comprising the steps of: estimating one even without, or may be a centroid estimation method characterized by.

本発明によれば、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法を提供することができる。   ADVANTAGE OF THE INVENTION According to this invention, the gravity center estimation apparatus and gravity center estimation method which can estimate the gravity center position and / or speed of a robot with high precision can be provided.

多点接触動作を行う際のロボット制御系のフレームワークを説明するための図である。It is a figure for demonstrating the framework of the robot control system at the time of performing multipoint contact operation | movement. 環境と各接触点で接触するロボットの２次元モデルを概略的に示す図である。It is a figure which shows roughly the two-dimensional model of the robot which contacts an environment at each contact point. 実施の形態１に係る重心推定装置の概略的なシステム構成を示すブロック図である。1 is a block diagram showing a schematic system configuration of a centroid estimation device according to Embodiment 1. FIG. 実施の形態１に係る多関節型ロボットに搭載された各センサを示す図である。FIG. 3 is a diagram illustrating each sensor mounted on the articulated robot according to the first embodiment. ロボットの重心周りの角運動量を説明するための図である。It is a figure for demonstrating the angular momentum around the gravity center of a robot. 実施の形態１に係る重心推定方法の処理フローを示すフローチャートである。4 is a flowchart showing a processing flow of a center-of-gravity estimation method according to Embodiment 1. 実施の形態２に係る重心推定装置の概略的なシステム構成を示すブロック図である。6 is a block diagram illustrating a schematic system configuration of a center-of-gravity estimation apparatus according to Embodiment 2. FIG.

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。
本発明の実施の形態１に係る重心推定装置は、ロボットの重心位置及び速度のうち少なくとも一方を高精度に推定できる。重心推定装置は、例えば、ロボットに搭載されている。ロボットは、重心推定装置により推定されたロボットの重心位置及び速度に基づいて、その手足先等と環境との各接触点に作用する接触力を正確に制御できる。 Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings.
The center-of-gravity estimation apparatus according to Embodiment 1 of the present invention can estimate at least one of the center-of-gravity position and speed of the robot with high accuracy. The center-of-gravity estimation device is mounted on, for example, a robot. Based on the position and speed of the center of gravity of the robot estimated by the center-of-gravity estimation device, the robot can accurately control the contact force acting on each contact point between its limbs and the environment and the environment.

ここで、ロボットが環境と複数の接触点で接触して動作（多点接触動作）を行う際の、ロボット制御系のフレームワークについて説明する。ロボットは、腕部、脚部、胴体部等で構成された、例えば、多関節型のヒューマノイドロボットとして構成されている。ロボットは、ロボットの重心軌道を生成する軌道生成部と、ロボットの力制御を行う制御部と、実機とモデルとのずれを推定する状態推定部と、を備えている。   Here, the framework of the robot control system when the robot performs an operation (multi-point contact operation) by contacting the environment at a plurality of contact points will be described. The robot is configured as, for example, an articulated humanoid robot including arms, legs, and a torso. The robot includes a trajectory generation unit that generates a center-of-gravity trajectory of the robot, a control unit that performs force control of the robot, and a state estimation unit that estimates a deviation between the actual machine and the model.

（１）接触点計画
軌道生成部は、設定された一連のタスクを実行する際に環境とロボットの手、足等との接触点の位置及び姿勢（時系列の接触点情報）を算出する。例えば、図１に示すように、ロボット１００が、壁に手を接触させつつ机の前まで移動し、机上に手を接触させつつ奥にあるペットボトルを把持するタスクを実行する。このとき、軌道生成部は、どの位置にロボット１００のどのような姿勢で手、足等を接触させて胴体部を支持するか等を、動作計画ソフトウェア（プランナー）、あるいは予めユーザに設定された設定情報を用いて計画する。 (1) Contact point plan The trajectory generation unit calculates the position and posture (time-series contact point information) of the contact point between the environment and the hand, foot, etc. of the robot when executing the set series of tasks. For example, as shown in FIG. 1, the robot 100 moves to the front of a desk while bringing a hand into contact with the wall, and executes a task of gripping a plastic bottle in the back while bringing the hand into contact with the desk. At this time, the trajectory generation unit is set by the motion planning software (planner) or the user in advance to determine the position at which the robot 100 is brought into contact with the hand, foot, etc. to support the torso. Plan using configuration information.

（２）重心軌道生成
軌道生成部は、上記計画した時系列の接触点情報に基づいて、現在を初期状態として所定時間経過後までの安定性が維持できるロボット１００の重心軌道を生成する。同時に、軌道生成部は、各接触点で発生させる反力（接触力）を算出する。 (2) Center of gravity trajectory generation The trajectory generation unit generates the center of gravity trajectory of the robot 100 that can maintain the stability until a predetermined time has elapsed from the current state as an initial state based on the planned time-series contact point information. At the same time, the trajectory generator calculates a reaction force (contact force) generated at each contact point.

（３）接触力制御
制御部は、軌道生成部により算出された接触力が各接触点で発生するようにロボット１００の各関節部の力制御を行う。 (3) Contact Force Control The control unit performs force control of each joint portion of the robot 100 so that the contact force calculated by the trajectory generation unit is generated at each contact point.

（４）全身協調
軌道生成部は、制御部の力制御の結果に基づいて重心軌道を修正する。制御部は、軌道生成部により修正された重心軌道に基づいて、ロボット１００全身の姿勢及び各関節角度を算出し、ロボット１００の制御を行う。 (4) Whole body coordination The trajectory generator corrects the center of gravity trajectory based on the result of force control by the controller. The control unit calculates the posture of the whole body of the robot 100 and each joint angle based on the center of gravity trajectory corrected by the trajectory generation unit, and controls the robot 100.

（５）状態推定
最後に、状態推定部は、ジャイロセンサ、加速度センサ等により検出された実際のセンサ値に基づいて算出した実機の重心軌道と、上記修正したモデルの重心軌道と、のずれを推定する。そして、状態推定部は、推定したずれに基づいて、モデルの重心軌道が実機の重心軌道に一致するように制御パラメータを設定する。 (5) State Estimation Finally, the state estimation unit calculates a deviation between the center of gravity trajectory of the actual machine calculated based on the actual sensor value detected by the gyro sensor, the acceleration sensor, etc., and the center of gravity trajectory of the corrected model. presume. Then, the state estimation unit sets the control parameters based on the estimated deviation so that the center of gravity trajectory of the model matches the center of gravity trajectory of the actual machine.

ロボット１００は、上述した（２）から（５）のプロセスを高速周期で実行することで、安定的な多点接触動作を実現する。本実施の形態１に係る重心推定装置は、上述した（５）状態推定に相当する機能を有している。   The robot 100 realizes a stable multipoint contact operation by executing the above-described processes (2) to (5) at a high speed cycle. The center-of-gravity estimation apparatus according to the first embodiment has a function corresponding to the above-described (5) state estimation.

図２は、環境と複数の接触点で接触するロボットの２次元モデルを概略的に示す図である。なお、図２に示すロボットにおいて、説明の簡略化のために各接触点でモーメント力は作用しないものとする。すなわち、各接触点は夫々点接触、もしくは線接触しているものとする。   FIG. 2 is a diagram schematically showing a two-dimensional model of a robot that contacts the environment at a plurality of contact points. In the robot shown in FIG. 2, it is assumed that moment force does not act at each contact point for simplification of description. That is, it is assumed that each contact point is in point contact or line contact.

図２（１）に示す如く、ロボットの各接触点には環境から水平方向の反力（接触力）ｆ１及び鉛直方向の反力ｆ２が夫々作用している。そして、図２（２）に示す如く、この各接触点の水平方向及び鉛直方向の接触力の合力ｆ３、ｆ４がロボットの重心Ｇに作用する。したがって、このロボットの重心Ｇに作用する合力ｆ３、ｆ４から重心Ｇに作用する重力ｍｇを減算した値が、ロボットの重心Ｇの並進力となり、この並進力をロボットの質量で除算した値がロボットの重心Ｇの並進加速度となる。このように、各接触点に作用する接触力を正確に計測できれば、ロボットの重心Ｇの並進加速度を正確に算出できる。そして、算出したロボットの重心Ｇの並進加速度を逐次積分することで重心位置及び速度を推定することができる。   As shown in FIG. 2A, a horizontal reaction force (contact force) f1 and a vertical reaction force f2 are applied to each contact point of the robot from the environment. 2 (2), the resultant force f3, f4 of the contact force in the horizontal direction and the vertical direction at each contact point acts on the center of gravity G of the robot. Therefore, the value obtained by subtracting the gravity mg acting on the center of gravity G from the resultant forces f3 and f4 acting on the center of gravity G of the robot becomes the translational force of the center of gravity G of the robot, and the value obtained by dividing this translational force by the mass of the robot Is the translational acceleration of the center of gravity G. Thus, if the contact force acting on each contact point can be accurately measured, the translational acceleration of the center of gravity G of the robot can be accurately calculated. Then, the position of the center of gravity and the speed can be estimated by successively integrating the calculated translational acceleration of the center of gravity G of the robot.

しかしながら、力センサなどにより計測される各接触点の接触力は一般にノイズや誤差を含んでいる。したがって、このノイズや誤差を含む各接触点の接触力を用いて算出した重心Ｇの並進加速度も誤差を含むこととなる。さらに、この誤差を含む重心Ｇの並進加速度を積分することで誤差が累積し重心位置及び速度は真値から外れた推定値となる虞がある。   However, the contact force at each contact point measured by a force sensor or the like generally includes noise and errors. Therefore, the translational acceleration of the center of gravity G calculated using the contact force of each contact point including noise and error also includes an error. Further, by integrating the translational acceleration of the center of gravity G including this error, the error accumulates, and the center of gravity position and speed may become an estimated value deviating from the true value.

ところで、図２（３）に示す如く、ロボットの重心Ｇには上述した並進力と共にモーメント力Ｍも作用している。これは、ロボットの重心Ｇから各接触点までの距離に各接触力を乗算し、各乗算結果を加算した合モーメント力として表される。   By the way, as shown in FIG. 2 (3), the moment force M acts on the center of gravity G of the robot together with the translational force described above. This is expressed as a resultant moment force obtained by multiplying the distance from the center of gravity G of the robot to each contact point by each contact force and adding each multiplication result.

そこで、本実施の形態１に係る重心推定装置は、ロボットの重心Ｇに作用する並進力に基づいた重心の推定に、重心周りのモーメントの関係式を組合せると共に、重心周り角速運動量を観測値としてカルマンフィルタを構成している。これにより、ロボットのセンサによるノイズや誤差を除去し、ロボットの重心位置及び速度を高精度かつリアルタイムに推定できる。   Therefore, the center-of-gravity estimation apparatus according to the first embodiment combines the relational expression of the moment around the center of gravity with the estimation of the center of gravity based on the translational force acting on the center of gravity G of the robot, and observes angular velocity momentum around the center of gravity. A Kalman filter is configured as a value. As a result, noises and errors due to the robot sensor can be removed, and the center of gravity position and speed of the robot can be estimated with high accuracy in real time.

図３は、本実施の形態１に係る重心推定装置の概略的なシステム構成を示すブロック図である。本実施の形態１に係る重心推定装置１は、複数の力センサ２と、複数のエンコーダ３と、ジャイロセンサ４と、加速度センサ５と、データ取得部６と、６軸力算出部７と、第２カルマンフィルタ８と、微分演算部９と、角運動量算出部１０と、第１カルマンフィルタ１１と、を備えている。   FIG. 3 is a block diagram illustrating a schematic system configuration of the centroid estimation apparatus according to the first embodiment. The center-of-gravity estimation device 1 according to the first embodiment includes a plurality of force sensors 2, a plurality of encoders 3, a gyro sensor 4, an acceleration sensor 5, a data acquisition unit 6, a six-axis force calculation unit 7, A second Kalman filter 8, a differential calculation unit 9, an angular momentum calculation unit 10, and a first Kalman filter 11 are provided.

重心推定装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。   The center-of-gravity estimation device 1 includes, for example, a CPU (Central Processing Unit) that performs arithmetic processing and the like, a memory that includes a ROM (Read Only Memory) and a RAM (Random Access Memory) that store arithmetic programs executed by the CPU, and the like. And a hardware configuration centering on a microcomputer comprising an interface unit (I / F) for inputting and outputting signals. The CPU, memory, and interface unit are connected to each other via a data bus or the like.

ロボット１００は、例えば、複数の関節部と、各関節部を介して相互に連結された複数のリンクと、を有する多関節型ロボットとして構成されている（図４）。ロボット１００は、二足歩行ロボットなどのヒューマノイドロボットとして構成されているが、これに限らず、複数の関節部及びリンクを有し、環境と複数点で接触する任意の多関節型ロボットに適用可能である。   The robot 100 is configured as, for example, an articulated robot having a plurality of joints and a plurality of links connected to each other through the joints (FIG. 4). The robot 100 is configured as a humanoid robot such as a biped robot, but is not limited to this, and can be applied to any articulated robot that has a plurality of joints and links and contacts the environment at a plurality of points. It is.

各力センサ２は、力検出手段の一具体例であり、例えば、ロボット１００の手先、足先、臀部、膝部、肘部などの路面、壁面などの環境と接触する部分に夫々設けられている。各力センサ２は、ロボット１００と環境とが接触する接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力Ｆｉと、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメントτｉ（接触６軸力）を夫々検出する。各力センサ２は、検出した接触６軸力（Ｆｉ、τｉ）を６軸力算出部７に出力する。

Figure 0006149791
Each force sensor 2 is a specific example of a force detection unit, and is provided at each portion of the robot 100 that comes into contact with an environment such as a hand surface, a toe, a buttocks, a knee, an elbow, or a road surface such as a wall surface. Yes. Each force sensor 2, moment around the contact point and the robot 100 and the environment are in contact, the x-axis direction, y-axis direction, and the z-axis direction force F i, around the x axis, about the y-axis, and z-axis τ i (contact 6-axis force) is detected. Each force sensor 2 outputs the detected contact 6-axis force (F i , τ i ) to the 6-axis force calculation unit 7.
Figure 0006149791

なお、ロボット１００には、例えば、Ｍ個の力センサ２が設けれており、ｉ番目の力センサ２の接触６軸力を（Ｆｉ、τｉ）とする。
各エンコーダ３は、角度検出手段の一具体例であり、例えば、ロボット１００の各関節部に設けられている。各エンコーダ３は、各関節部の関節角度θを検出する。各エンコーダは、検出した各関節部の関節角度θを角運動量算出部１０及び微分演算部９に出力する。なお、ロボット１００には例えば、Ｎ個の関節部が設けられているものとする。

Figure 0006149791
The robot 100 is provided with, for example, M force sensors 2, and the 6-axis contact force of the i-th force sensor 2 is (F i , τ i ).
Each encoder 3 is a specific example of an angle detection unit, and is provided at each joint portion of the robot 100, for example. Each encoder 3 detects a joint angle θ of each joint part. Each encoder outputs the detected joint angle θ of each joint to the angular momentum calculation unit 10 and the differential calculation unit 9. Note that the robot 100 is provided with N joint portions, for example.
Figure 0006149791

ジャイロセンサ４は、角速度検出手段の一具体例であり、例えば、ロボット１００の胴体部に設けれている。ジャイロセンサ４は、ロボット１００の胴体部の角速度ωを検出する。ジャイロセンサ４は、検出した胴体部の角速度ωを第２カルマンフィルタ８に出力する。   The gyro sensor 4 is a specific example of an angular velocity detection unit, and is provided, for example, on a body part of the robot 100. The gyro sensor 4 detects the angular velocity ω of the body part of the robot 100. The gyro sensor 4 outputs the detected angular velocity ω of the body part to the second Kalman filter 8.

加速度センサ５は、加速度検出手段の一具体例であり、例えば、ロボット１００の胴体部に設けられている。加速度センサ５は、胴体部の並進方向の加速度（並進加速度）αを検出する。加速度センサ５は、検出した胴体部の並進加速度αを第２カルマンフィルタ８に出力する。

Figure 0006149791
The acceleration sensor 5 is a specific example of acceleration detection means, and is provided, for example, on the body of the robot 100. The acceleration sensor 5 detects an acceleration (translation acceleration) α in the translational direction of the body part. The acceleration sensor 5 outputs the detected translational acceleration α of the body part to the second Kalman filter 8.
Figure 0006149791

データ取得部６は、取得手段の一具体例であり、各接触点の姿勢を示す回転行列（以下、姿勢行列Ｒｃ ｉ）及び各接触点の位置を示すグローバル座標系の座標（以下、位置座標ｐｉ）を上記メモリなどから取得する。例えば、ロボット１００は環境上の予め決められた位置に手や足を接触させて運動するものとし、メモリには、そのときの各接触点の姿勢及び位置が記憶されている。 The data acquisition unit 6 is a specific example of an acquisition unit, and includes a rotation matrix (hereinafter, attitude matrix R c i ) indicating the attitude of each contact point, and coordinates (hereinafter, position) of the global coordinate system indicating the position of each contact point. The coordinates p i ) are acquired from the memory or the like. For example, it is assumed that the robot 100 moves by bringing a hand or foot into contact with a predetermined position on the environment, and the memory stores the posture and position of each contact point at that time.

データ取得部６は、取得した各接触点の姿勢行列Ｒｃ ｉを６軸力算出部７に出力し、取得した各接触点の位置座標ｐｉを第１カルマンフィルタ１１に出力する。なお、データ取得部６は、例えば、ロボット１００に設けられたセンサを用いて、各接触点の姿勢行列Ｒｃ ｉ及び／又は位置座標ｐｉを幾何学的に算出してもよい。また、データ取得部６は、取得した各接触点の姿勢行列Ｒｃ ｉ及び／又は位置座標ｐｉを、ロボット１００の外界情報を検出する外界センサ（レーザレンジファインダ、カメラなど）を用いて幾何学的に修正してもよい。これにより、各接触点の姿勢行列Ｒｃ ｉ及び位置座標ｐｉをより高精度に求めることができる。 The data acquisition unit 6 outputs the acquired posture matrix R c i of each contact point to the six-axis force calculation unit 7, and outputs the acquired position coordinates p i of each contact point to the first Kalman filter 11. Note that the data acquisition unit 6 may geometrically calculate the posture matrix R c i and / or the position coordinate p i of each contact point using, for example, a sensor provided in the robot 100. Further, the data acquisition unit 6 uses the external sensor (laser range finder, camera, etc.) that detects the external environment information of the robot 100 for the acquired posture matrix R c i and / or position coordinate p i of each contact point. It may be corrected scientifically. Thereby, the posture matrix R c i and the position coordinates p i of each contact point can be obtained with higher accuracy.

６軸力算出部７は、変換手段の一具体例であり、データ取得部６から出力された各接触点ｉの姿勢行列Ｒｃ ｉに基づいて、各力センサ２から出力された力センサ座標系の接触６軸力（Ｆｉ、τｉ）をグローバル座標系の接触６軸力（ｆｉ、ｎｉ）に下記式を用いて変換する。

Figure 0006149791
The 6-axis force calculation unit 7 is a specific example of the conversion unit, and based on the posture matrix R c i of each contact point i output from the data acquisition unit 6, the force sensor coordinates output from each force sensor 2. The contact 6-axis force (F i , τ i ) of the system is converted into the contact 6-axis force (f i , n i ) of the global coordinate system using the following formula.
Figure 0006149791

なお、上記式において、ｉ番目の接触点の姿勢行列をＲｃ ｉとし位置座標をｐｉとしている。
６軸力算出部７は、変換した接触６軸力（ｆｉ、ｎｉ）を第１カルマンフィルタ１１に出力する。 In the above equation, the posture matrix of the i-th contact point is R c i and the position coordinates are p i .
The 6-axis force calculation unit 7 outputs the converted contact 6-axis force (f i , n i ) to the first Kalman filter 11.

第２カルマンフィルタ８は、第２推定手段の一具体例であり、ジャイロセンサ４から出力されたロボット１００の胴体部の角速度ωと、加速度センサ５から出力された胴体部の並進加速度αと、に基づいて、周知のカルマンフィルタを用いて、ロボット１００の胴体部の姿勢角（行列）Ｒｂｏｄｙおよび角速度（ベクトル）ωｄを高精度に算出する。
ここで、ロボット１００の胴体部の姿勢角のオフセット（バイアス）値を示す行列を四元数（クォータニオン）を用いて下記式を用いて定義する。

Figure 0006149791
The second Kalman filter 8 is a specific example of the second estimating means, and includes an angular velocity ω of the body portion of the robot 100 output from the gyro sensor 4 and a translational acceleration α of the body portion output from the acceleration sensor 5. Based on this, a posture angle (matrix) R body and an angular velocity (vector) ω d of the body part of the robot 100 are calculated with high accuracy using a known Kalman filter.
Here, a matrix indicating the offset (bias) value of the posture angle of the body portion of the robot 100 is defined using the following equation using a quaternion.
Figure 0006149791

ロボット１００の胴体部の角速度のオフセット値を示すベクトルを下記式を用いて定義する。但し、βドット（βの微分値）＝０とする。

Figure 0006149791
A vector indicating the offset value of the angular velocity of the body portion of the robot 100 is defined using the following equation. However, β dot (differential value of β) = 0.
Figure 0006149791

状態変数ηを下記式で定義する。

Figure 0006149791
次に、四元数の時間微分の関係式は、下記式のようになる。
Figure 0006149791
 上記式を用いて下記式が導出される。
Figure 0006149791
 上記式を離散化することで下記状態方程式（１）が導出される。
Figure 0006149791
 The state variable η is defined by the following formula.
Figure 0006149791
 Next, the relational expression of the time differentiation of the quaternion is as follows.
Figure 0006149791
 The following formula is derived using the above formula.
Figure 0006149791
 The following equation of state (1) is derived by discretizing the above equation.
Figure 0006149791

さらに、ロボット１００の胴体部の並進加速度を観測値に選ぶことで下記観測方程式（２）が導出される。但し、下記観測方程式（２）において、加速度センサ５が取り付けられたロボット１００の胴体部の並進加速度が十分に小さいものと仮定する。

Figure 0006149791
Furthermore, the following observation equation (2) is derived by selecting the translational acceleration of the body of the robot 100 as an observation value. However, in the following observation equation (2), it is assumed that the translational acceleration of the body portion of the robot 100 to which the acceleration sensor 5 is attached is sufficiently small.
Figure 0006149791

上記観測方程式（２）において、回転変換の回転行列Ｒ（ｑ）を下記式で表すことができる。

Figure 0006149791
In the observation equation (2), the rotation transformation rotation matrix R (q) can be expressed by the following equation.
Figure 0006149791

上記導出した状態方程式（１）及び観測方程式（２）にシステム雑音ｂηｖη[ｋ]及び観測雑音ｗΓ[ｋ]を導入することで、下記（３）式を導出することができる。

Figure 0006149791
By introducing the system noise b η v η [k] and the observation noise w Γ [k] into the derived state equation (1) and observation equation (2), the following equation (3) can be derived.
Figure 0006149791

第２カルマンフィルタ８は、加速度センサ５からのロボット１００の胴体部の並進加速度αに基づいて、例えば、上記（３）式に拡張カルマンフィルタを適用することで、高精度に状態変数η（ｑ、β）を推定する。そして、第２カルマンフィルタ８は、推定した状態変数ηと、に基づいて下記式を用いて、ロボット１００の胴体部の姿勢角Ｒｂｏｄｙおよび角速度ωｄを算出する。 The second Kalman filter 8 applies the extended Kalman filter to the above equation (3) based on the translational acceleration α of the body of the robot 100 from the acceleration sensor 5, for example, so that the state variables η (q, β ). Then, the second Kalman filter 8 calculates the posture angle R body and the angular velocity ω d of the body portion of the robot 100 using the following equations based on the estimated state variable η.

上記第２カルマンフィルタを用いることで、ジャイロセンサ４の低周波ノイズ及び加速度センサ５の高周波ノイズを除去し、ロボット１００の胴体部の姿勢角および角速度を高精度に算出できる。

Figure 0006149791
By using the second Kalman filter, the low frequency noise of the gyro sensor 4 and the high frequency noise of the acceleration sensor 5 can be removed, and the posture angle and angular velocity of the body portion of the robot 100 can be calculated with high accuracy.
Figure 0006149791

第２カルマンフィルタ８は、算出した胴体部の姿勢角Ｒｂｏｄｙおよび角速度ωｄを角運動量算出部１０に出力する。
微分演算部９は、関節角速度算出手段の一具体例であり、各エンコーダ３から出力された各関節部の関節角度θに対し微分演算を行い各関節部の関節角速度（θ上辺にドットをθ（・）とする）を算出する。微分演算部９は、算出した各関節部の関節角速度を角運動量算出部１０に出力する。 The second Kalman filter 8 outputs the calculated posture angle R body and angular velocity ω d of the body part to the angular momentum calculation unit 10.
The differential calculation unit 9 is a specific example of a joint angular velocity calculation unit, performs differential calculation on the joint angle θ of each joint output from each encoder 3, and performs a joint calculation on the joint angular velocity (θ (·) . The differential calculation unit 9 outputs the calculated joint angular velocity of each joint to the angular momentum calculation unit 10.

角運動量算出部１０は、角運動量算出手段の一具体例であり、第２カルマンフィルタ８から出力される胴体部の姿勢角Ｒｂｏｄｙ及び角速度ωｄと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ（・）と、第１カルマンフィルタ１１から出力されるロボット１００の重心位置ｒと、に基づいて、ロボット１００の重心周りの角運動量Ｌを算出する。 The angular momentum calculation unit 10 is a specific example of the angular momentum calculation unit, and the posture angle R body and angular velocity ω d of the body portion output from the second Kalman filter 8 and the joint portions output from the encoders 3. Based on the joint angle θ, the joint angular velocity θ (•) of each joint portion output from the differential calculation unit 9 , and the center-of-gravity position r of the robot 100 output from the first Kalman filter 11, Is calculated.

ここで、ロボット１００の重心周りの角運動量Ｌは、ロボットの各リンクが重心周りに運動することによって発生する角運動量である。例えば、図５に示すように、ｊ番目のリンクの質量をｍｊ、その位置をｃｊとすると、このリンクが並進運動することによって生じる重心周りの角運動量は、下記式を用いて算出される。

Figure 0006149791
Here, the angular momentum L around the center of gravity of the robot 100 is an angular momentum generated when each link of the robot moves around the center of gravity. For example, as shown in FIG. 5, when the mass of the j-th link is m j and its position is c j , the angular momentum around the center of gravity caused by the translational movement of this link is calculated using the following equation: The
Figure 0006149791

さらに、上記式に各リンクの回転運動による角運動量を考慮して求めた式を、全リンクについて足し合わせることで下記ロボット１００の重心周りの角運動量Ｌの下記式を導出できる。但し、下記式において、ｊ番目のリンクの慣性モーメントをＩｊとし、角速度をωｊとする。

Figure 0006149791
Furthermore, the following formula of the angular momentum L around the center of gravity of the robot 100 can be derived by adding the formula obtained in consideration of the angular momentum due to the rotational motion of each link to the above formula for all links. In the following equation, the moment of inertia of the j-th link is I j and the angular velocity is ω j .
Figure 0006149791

角運動量算出部１０は、第２カルマンフィルタ８から出力される胴体部の姿勢角Ｒｂｏｄｙ及び角速度ωｄと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ（・）と、に基づいて、周知の再帰的な演算を行い、各リンクの位置ｃｊ及び角速度ωｊとを高速に算出する。 The angular momentum calculating unit 10 outputs the posture angle R body and angular velocity ω d of the body portion output from the second Kalman filter 8, the joint angle θ of each joint portion output from each encoder 3, and the output from the differential calculation unit 9. Based on the joint angular velocity θ (•) of each joint portion, a known recursive calculation is performed to calculate the position c j and the angular velocity ω j of each link at high speed.

角運動量算出部１０は、算出した各リンクの位置ｃｊ及び角速度ωｊと、第１カルマンフィルタ１１からフィードバックされたロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。角運動量算出部１０は、算出した重心周りの角運動量Ｌを第１カルマンフィルタ１１に出力する。 Based on the calculated position c j and angular velocity ω j of each link and the gravity center position r of the robot 100 fed back from the first Kalman filter 11, the angular momentum calculation unit 10 uses the above formula to calculate the gravity center of the robot 100. The surrounding angular momentum L is calculated. The angular momentum calculation unit 10 outputs the calculated angular momentum L around the center of gravity to the first Kalman filter 11.

第１カルマンフィルタ１１は、６軸力算出部７から出力された接触６軸力（ｆｉ、ｎｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、ロボットの接触点の位置座標ｐｉと、に基づいて、周知のカルマンフィルタを用いてロボット１００の重心位置ｒ及び速度（ｒ上辺にドットをｒ（・）とする）を推定する。 The first Kalman filter 11 includes a contact 6-axis force (f i , n i ) output from the 6-axis force calculator 7, an angular momentum L around the center of gravity output from the angular momentum calculator 10, and a contact point of the robot Based on the position coordinates p i , the center of gravity position r and the speed of the robot 100 are estimated using a known Kalman filter (a dot on the upper side of r is r (·)) .

ここで、ロボット１００の重心に対して並進方向及び回転方向に作用する力の運動方程式は、下記式で表される。但し、下記式において、Ｐは並進方向の運動量であり、ｇは重力加速度ベクトルである。

Figure 0006149791
Here, the equation of motion of the force acting in the translation direction and the rotation direction with respect to the center of gravity of the robot 100 is represented by the following equation. In the following equation, P is the momentum in the translation direction, and g is the gravitational acceleration vector.
Figure 0006149791

下記新しい変数ｓを導入する。

Figure 0006149791
上記変数ｓの式に上記並進方向及び回転方向の運動方程式を代入することで下記式が導出される。
Figure 0006149791
 状態変数ｘを下記式で定義する。
Figure 0006149791
 上記式を離散化することで下記状態方程式（４）が導出される。
Figure 0006149791
 The following new variable s is introduced.
Figure 0006149791
 The following equation is derived by substituting the equation of motion in the translation direction and the rotation direction into the equation of the variable s.
Figure 0006149791
 The state variable x is defined by the following formula.
Figure 0006149791
 The following equation of state (4) is derived by discretizing the above equation.
Figure 0006149791

ロボット１００の重心周りの角運動量Ｌを観測値に選択することで下記観測方程式（５）が導出される。

Figure 0006149791
The following observation equation (5) is derived by selecting the angular momentum L around the center of gravity of the robot 100 as an observation value.
Figure 0006149791

上記状態方程式（４）及び観測方程式（５）にシステム雑音ｂｘｖｘ[ｋ]及び観測雑音ｗｈ[ｋ]を導入することで下記式が導出される。

Figure 0006149791
The following equation is derived by introducing the system noise b x v x [k] and the observation noise w h [k] into the state equation (4) and the observation equation (5).
Figure 0006149791

第１カルマンフィルタ１１は、第１推定手段の一具体例であり、６軸力算出部７から出力された接触６軸力（ｆｉ、ｎｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、ロボット１００の接触点の位置座標ｐｉと、に基づいて、上記導出した（６）式に拡張カルマンフィルタを適用し、状態変数ｘを高精度に推定する。 The first Kalman filter 11 is a specific example of the first estimating means, and the contact 6-axis force (f i , n i ) output from the 6-axis force calculation unit 7 and the center of gravity output from the angular momentum calculation unit 10. and angular momentum L around the position coordinates p i of contact points of the robot 100, based on, and apply extended Kalman filter and the derived equation (6), to estimate the state variable x with high precision.

図６は、本発明の実施の形態１に係る重心推定方法の処理フローを示すフローチャートである。
各力センサ２は、ロボット１００と環境との各接触点における接触６軸力（Ｆｉ、τｉ）を夫々検出し（ステップＳ１０１）、検出した接触６軸力を６軸力算出部７に出力する。 FIG. 6 is a flowchart showing a processing flow of the center-of-gravity estimation method according to Embodiment 1 of the present invention.
Each force sensor 2 detects a contact 6-axis force (F i , τ i ) at each contact point between the robot 100 and the environment (step S 101), and the detected contact 6-axis force is sent to the 6-axis force calculation unit 7. Output.

各エンコーダ３は、ロボット１００の各関節部の関節角度θを検出し（ステップＳ１０２）、検出した各関節部の関節角度θを角運動量算出部１０及び微分演算部９に出力する。   Each encoder 3 detects the joint angle θ of each joint of the robot 100 (step S102), and outputs the detected joint angle θ of each joint to the angular momentum calculation unit 10 and the differential calculation unit 9.

ジャイロセンサ４は、ロボット１００の胴体部の角速度ωを検出し（ステップＳ１０３）、検出した胴体部の角速度ωを第２カルマンフィルタ８に出力する。
加速度センサ５は、ロボット１００の胴体部の並進加速度αを検出し（ステップＳ１０４）、検出した胴体部の並進加速度αを第２カルマンフィルタ８に出力する。 The gyro sensor 4 detects the angular velocity ω of the body of the robot 100 (step S103), and outputs the detected angular velocity ω of the body to the second Kalman filter 8.
The acceleration sensor 5 detects the translational acceleration α of the body part of the robot 100 (step S104), and outputs the detected translational acceleration α of the body part to the second Kalman filter 8.

６軸力算出部７は、データ取得部６から出力された各接触点ｉの姿勢行列Ｒｃ ｉに基づいて、各力センサ２から出力された力センサ座標系の接触６軸力（Ｆｉ、τｉ）をグローバル座標系の接触６軸力（ｆｉ、ｎｉ）に変換し（ステップＳ１０５）、変換した接触６軸力を第１カルマンフィルタ１１に出力する。 The 6-axis force calculation unit 7 is based on the posture matrix R c i of each contact point i output from the data acquisition unit 6, and the contact 6-axis force (F i of the force sensor coordinate system output from each force sensor 2. , Τ i ) are converted into contact 6-axis forces (f i , n i ) in the global coordinate system (step S 105), and the converted contact 6-axis forces are output to the first Kalman filter 11.

第２カルマンフィルタ８は、ジャイロセンサ４から出力されたロボット１００の胴体部の角速度ωと、加速度センサ５から出力されたロボット１００の胴体部の並進加速度αと、に基づいて、カルマンフィルタを用いて、ロボット１００の胴体部の姿勢角Ｒｂｏｄｙおよび角速度ωｄを算出する（ステップＳ１０６）。第２カルマンフィルタ８は、算出した胴体部の姿勢角Ｒｂｏｄｙおよび角速度ωｄを角運動量算出部１０に出力する。 The second Kalman filter 8 uses a Kalman filter based on the angular velocity ω of the body of the robot 100 output from the gyro sensor 4 and the translational acceleration α of the body of the robot 100 output from the acceleration sensor 5. The posture angle R body and the angular velocity ω d of the body part of the robot 100 are calculated (step S106). The second Kalman filter 8 outputs the calculated posture angle R body and angular velocity ω d of the body part to the angular momentum calculation unit 10.

微分演算部９は、各エンコーダ３から出力された各関節部の関節角度θに対し微分演算を行い各関節部の関節角速度θ（・）を算出し（ステップＳ１０７）、算出した各関節部の関節角速度を角運動量算出部１０に出力する。 The differential calculation unit 9 performs differential calculation on the joint angle θ of each joint output from each encoder 3 to calculate the joint angular velocity θ (•) of each joint (step S107), and calculates the calculated joint The joint angular velocity is output to the angular momentum calculation unit 10.

角運動量算出部１０は、第２カルマンフィルタ８から出力されるロボット１００の胴体部の姿勢角及び角速度と、各エンコーダ３から出力される各関節部の関節角度と、微分演算部９から出力される各関節部の関節角速度θ（・）と、フィードバックされるロボット１００の重心位置ｒと、に基づいて、ロボット１００の重心周りの角運動量Ｌを算出する（ステップＳ１０８）。角運動量算出部１０は、算出した重心周りの角運動量Ｌを第１カルマンフィルタ１１に出力する。 The angular momentum calculation unit 10 outputs the posture angle and angular velocity of the body portion of the robot 100 output from the second Kalman filter 8, the joint angle of each joint unit output from each encoder 3, and the differential calculation unit 9. An angular momentum L around the center of gravity of the robot 100 is calculated based on the joint angular velocity θ (•) of each joint and the center of gravity position r of the robot 100 fed back (step S108). The angular momentum calculation unit 10 outputs the calculated angular momentum L around the center of gravity to the first Kalman filter 11.

第１カルマンフィルタ１１は、６軸力算出器７から出力された接触６軸力（ｆｉ、ｎｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、データ取得部６から出力されるロボット１００の接触点の位置座標ｐｉと、に基づいて、カルマンフィルタを用いてロボットの重心位置ｒ及び速度ｒ（・）を推定する（ステップＳ１０９）。 The first Kalman filter 11 includes a contact 6-axis force (f i , n i ) output from the 6-axis force calculator 7, an angular momentum L around the center of gravity output from the angular momentum calculator 10, and a data acquisition unit 6. Based on the position coordinates p i of the contact point of the robot 100 output from, the center-of-gravity position r and the velocity r (·) of the robot are estimated using the Kalman filter (step S109).

以上、本実施の形態１に係る重心推定装置１及び重心推定方法において、各力センサ２により検出された各接触点の接触６軸力と、各接触点の姿勢行列及び位置座標と、ロボット１００の重心周りの角運動量と、に基づいて、ロボット１００の重心位置及び速度を状態変数とした状態方程式、および、ロボット１００の重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用してロボット１００の重心位置及び速度を推定する。これにより、ロボット１００の力センサ２などによるノイズや誤差を除去し、ロボット１００の重心位置及び速度を高精度に推定できる。   As described above, in the center-of-gravity estimation apparatus 1 and the center-of-gravity estimation method according to the first embodiment, the contact 6-axis force of each contact point detected by each force sensor 2, the posture matrix and position coordinates of each contact point, and the robot 100 The Kalman filter is applied to the state equation using the position and speed of the center of gravity of the robot 100 as state variables and the observation equation using the angular momentum around the center of gravity of the robot 100 as an observation value. Apply to estimate the center of gravity position and speed of the robot 100. Thereby, noise and error due to the force sensor 2 of the robot 100 and the like can be removed, and the center of gravity position and speed of the robot 100 can be estimated with high accuracy.

実施の形態２
本実施の形態２に係る重心推定装置２０は、上記実施の形態１の第２カルマンフィルタ８の代わりに、ハイパスフィルタ２１及びローパスフィルタ２２を備える点を特徴とする。図７は、本実施の形態２に係る重心推定装置の概略的なシステム構成を示すブロック図である。 Embodiment 2
The center-of-gravity estimation apparatus 20 according to the second embodiment is characterized in that a high-pass filter 21 and a low-pass filter 22 are provided instead of the second Kalman filter 8 of the first embodiment. FIG. 7 is a block diagram showing a schematic system configuration of the center-of-gravity estimation apparatus according to the second embodiment.

ジャイロセンサ４は、検出した胴体部の角速度ωをハイパスフィルタ２１に出力する。ハイパスフィルタ２１は、ジャイロセンサ４の角速度ωから低周波ノイズを除去する。ハイパスフィルタ２１は、低周波ノイズを除去した角速度を角運動量算出部１０に出力する。   The gyro sensor 4 outputs the detected angular velocity ω of the body part to the high pass filter 21. The high pass filter 21 removes low frequency noise from the angular velocity ω of the gyro sensor 4. The high pass filter 21 outputs the angular velocity from which low frequency noise has been removed to the angular momentum calculation unit 10.

加速度センサ５は、検出した胴体部の並進加速度αをローパスフィルタ２２に出力する。ローパスフィルタ２２は、加速度センサ５の並進加速度αから高周波ノイズを除去する。ローパスフィルタ２２は、高周波ノイズを除去した並進加速度を角運動量算出部１０に出力する。   The acceleration sensor 5 outputs the detected translational acceleration α of the body part to the low-pass filter 22. The low pass filter 22 removes high frequency noise from the translational acceleration α of the acceleration sensor 5. The low-pass filter 22 outputs the translational acceleration from which high-frequency noise has been removed to the angular momentum calculation unit 10.

角運動量算出部１０は、ハイパスフィルタ２１から出力される胴体部の角速度と、ローパスフィルタ２２から出力された胴体部の並進加速度と、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ（・）と、に基づいて、周知の再帰的な演算を行い、各リンクの位置ｃｊ及び角速度ωｊとを高速に算出する。 The angular momentum calculation unit 10 includes an angular velocity of the trunk portion output from the high-pass filter 21, a translational acceleration of the trunk portion output from the low-pass filter 22, a joint angle θ of each joint portion output from each encoder 3, and A well-known recursive calculation is performed based on the joint angular velocity θ (•) output from the differential calculation unit 9 to calculate the position c j and the angular velocity ω j of each link at high speed.

角運動量算出部１０は、算出した各リンクの位置ｃｊ及び角速度ωｊと、ロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。これにより、ジャイロセンサ及び加速度センサのノイズを除去し、高精度にロボット１００の重心周りの角運動量Ｌを算出できる。 The angular momentum calculation unit 10 calculates the angular momentum L around the center of gravity of the robot 100 using the above formula based on the calculated position c j and angular velocity ω j of each link and the center of gravity position r of the robot 100. . Thereby, the noise of the gyro sensor and the acceleration sensor is removed, and the angular momentum L around the center of gravity of the robot 100 can be calculated with high accuracy.

なお、ジャイロセンサ４及び加速度センサ５の計測精度が高い場合、ジャイロセンサ４及び加速度センサ５は、夫々、ハイパスフィルタ及びローパスフィルタを介することなく、角運動量算出部１０に直接的に角速度ω及び並進加速度αを出力してもよい。   In addition, when the measurement accuracy of the gyro sensor 4 and the acceleration sensor 5 is high, the gyro sensor 4 and the acceleration sensor 5 directly pass the angular velocity ω and the translation to the angular momentum calculation unit 10 without using a high-pass filter and a low-pass filter, respectively. The acceleration α may be output.

角運動量算出部１０は、ジャイロセンサ４から出力される胴体部の角速度ωと、加速度センサ５から出力された胴体部の並進加速度αと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ（・）と、に基づいて、各リンクの位置ｃｊ及び角速度ωｊとを算出し、該算出した各リンクの位置ｃｊ及び角速度ωｊと、ロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。これにより、構成を簡略化しつつ、高精度にロボット１００の重心周りの角運動量Ｌを算出できる。
本実施の形態２において、上記実施の形態１と同一部分に同一符号を付して詳細な説明は省略する。 The angular momentum calculation unit 10 includes an angular velocity ω of the trunk portion output from the gyro sensor 4, a translational acceleration α of the trunk portion output from the acceleration sensor 5, and a joint angle θ of each joint portion output from each encoder 3. And the position c j and the angular velocity ω j of each link based on the joint angular velocity θ (·) of each joint output from the differential calculation unit 9, and the calculated position c j of each link Then, based on the angular velocity ω j and the gravity center position r of the robot 100, the angular momentum L around the gravity center of the robot 100 is calculated using the above formula. Thereby, the angular momentum L around the center of gravity of the robot 100 can be calculated with high accuracy while simplifying the configuration.
In the second embodiment, the same parts as those in the first embodiment are denoted by the same reference numerals, and detailed description thereof is omitted.

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。 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.
In the present invention, for example, the processing shown in FIG. 6 can be realized by causing a CPU to execute a computer program.

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭを含む。   The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer readable media are magnetic recording media (eg flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg magneto-optical disks), CD-ROM, CD-R, CD-R / W. Semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, and RAM are included.

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。   The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

１ 重心推定装置、２ 力センサ、３ エンコーダ、４ ジャイロセンサ、５ 加速度センサ、６ データ取得部、７ ６軸力算出部、８ 第２カルマンフィルタ、９ 微分演算部、１０ 角運動量算出部、１１ 第１カルマンフィルタ、２１ ハイパスフィルタ、２２ ローパスフィルタ、１００ ロボット   DESCRIPTION OF SYMBOLS 1 Center of gravity estimation apparatus, 2 Force sensor, 3 Encoder, 4 Gyro sensor, 5 Acceleration sensor, 6 Data acquisition part, 7 6-axis force calculation part, 8 2nd Kalman filter, 9 Differential operation part, 10 Angular momentum calculation part, 11 1st 1 Kalman filter, 21 high-pass filter, 22 low-pass filter, 100 robot

Claims (8)

環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出する力検出手段と、
前記ロボットの胴体部に作用する角速度を検出する角速度検出手段と、
前記ロボットの胴体部に作用する加速度を検出する加速度検出手段と、
前記ロボットの各関節の角度を検出する角度検出手段と、
前記ロボットの各接触点の姿勢及び位置を取得する取得手段と、
前記角速度検出手段により検出された角速度と、前記加速度検出手段により検出された加速度と、前記角度検出手段により検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出する角運動量算出手段と、
前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定する第１推定手段と、を備える、ことを特徴とする重心推定装置。 Force detecting means for detecting forces acting on a plurality of contact points of an articulated robot in contact with the environment;
Angular velocity detection means for detecting angular velocity acting on the body of the robot;
Acceleration detecting means for detecting acceleration acting on the body of the robot;
Angle detection means for detecting the angle of each joint of the robot;
Obtaining means for obtaining the posture and position of each contact point of the robot;
Based on the angular velocity detected by the angular velocity detection means, the acceleration detected by the acceleration detection means, and the angle of each joint detected by the angle detection means, the angular momentum around the center of gravity of the robot is calculated. Angular momentum calculation means;
Based on the force of each contact point detected by the force detection means, the posture and position of each contact point acquired by the acquisition means, and the angular momentum around the center of gravity calculated by the angular momentum calculation means. And a first estimating means for estimating at least one of the position and speed of the center of gravity of the robot using a Kalman filter. 請求項１記載の重心推定装置であって、
前記第１推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、
前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出される、ことを特徴とする重心推定装置。 The center-of-gravity estimation apparatus according to claim 1,
The first estimating means includes the force of each contact point detected by the force detecting means, the posture and position of each contact point acquired by the acquiring means, and the center of gravity calculated by the angular momentum calculating means. The Kalman filter is applied to the state equation using the position and speed of the center of gravity of the robot as state variables and the observation equation using the angular momentum around the center of gravity of the robot as an observation value based on the angular momentum of Estimating the center of gravity and speed of the robot;
The center-of-gravity estimation apparatus, wherein the state equation and the observation equation are derived based on a motion equation of force acting in a translational direction and a rotational direction on the center of gravity of the robot. 請求項１又は２記載の重心推定装置であって、
前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、
前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、
前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 The center-of-gravity estimation device according to claim 1 or 2,
The robot is an articulated robot having a plurality of the joints and a plurality of links connected to each other through the joints.
Joint angular velocity calculating means for calculating the angular velocity of each joint based on the angle of each joint detected by the angle detecting means;
The angular momentum calculating means includes an angle of each joint detected by the angle detecting means, an angular velocity of each joint calculated by the joint angular velocity calculating means, a posture angle and an angular speed of the body part, and an angle of each link. Based on the mass, position, and moment of inertia, and the center of gravity of the robot, the angular momentum about the center of gravity of the robot is obtained by adding the angular momentum due to the translational motion and the angular momentum due to the rotational motion of each link of the robot. A centroid estimation device characterized by: 請求項１乃至３のうちいずれか１項記載の重心推定装置であって、
前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第２推定手段を更に備え、
前記角運動量算出手段は、前記第２推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 The center-of-gravity estimation device according to any one of claims 1 to 3,
An attitude angle offset value of the body part of the robot is defined using a quaternion, a state equation including system noise using the quaternion and the angular velocity offset value of the angular velocity detection means as a state variable, and the body The state variable is estimated by applying a Kalman filter to the observation equation including the observation noise and the observation acceleration including the observation noise, and the posture angle and the angular velocity of the body part are estimated based on the estimated state variable. A second estimating means;
The angular momentum calculating means includes a posture angle and an angular velocity of the body portion estimated by the second estimating means, an angle of each joint detected by the angle detecting means, an angular velocity of each joint, and each link. A center-of-gravity estimation apparatus that calculates an angular momentum around the center of gravity of the robot based on a mass and a moment of inertia and a position of the center of gravity of the robot. 請求項１乃至３のうちいずれか１項記載の重心推定装置であって、
前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、
前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、
前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 The center-of-gravity estimation device according to any one of claims 1 to 3,
A high-pass filter for removing low-frequency noise from the angular velocity of the body of the robot detected by the angular velocity detection means;
A low-pass filter for removing high-frequency noise from the acceleration of the body of the robot detected by the acceleration detecting means,
The angular momentum calculation means includes: an angular velocity of the trunk portion from the high-pass filter; an acceleration of the trunk portion from the low-pass filter; an angle of each joint detected by the angle detection means; and an angular velocity of each joint ; A center-of-gravity estimation device that calculates an angular momentum around the center of gravity of the robot based on a mass and a moment of inertia of each link and a position of the center of gravity of the robot. 請求項１乃至５のうちいずれか１項記載の重心推定装置であって、
前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備える、ことを特徴とする重心推定装置。 The center-of-gravity estimation device according to any one of claims 1 to 5,
Further comprising conversion means for converting the force in the coordinate system of the force detection means output from the force detection means into a force in the global coordinate system based on the attitude angle of each contact point acquired by the acquisition means. A centroid estimation device characterized by 請求項１乃至６のうちいずれか１項記載の重心推定装置であって、
前記力検出手段は、前記各接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力と、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメント力を検出する、ことを特徴とする重心推定装置。 The centroid estimation device according to any one of claims 1 to 6,
The force detecting means detects forces in the x-axis direction, y-axis direction, and z-axis direction, and moment forces around the x-axis, y-axis, and z-axis at each contact point. Centroid estimation device. 環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、
前記ロボットの胴体部に作用する角速度を検出するステップと、
前記ロボットの胴体部に作用する加速度を検出するステップと、
前記ロボットの各関節の角度を検出するステップと、
前記ロボットの各接触点の姿勢及び位置を取得するステップと、
前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、
前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法。 Detecting each of the forces acting on a plurality of contact points of an articulated robot in contact with the environment;
Detecting an angular velocity acting on the body of the robot;
Detecting acceleration acting on the body of the robot;
Detecting an angle of each joint of the robot;
Obtaining the posture and position of each contact point of the robot;
Calculating an angular momentum around the center of gravity of the robot based on the detected angular velocity, the detected acceleration, and the detected angle of each joint;
Based on the detected force of each contact point, the obtained posture and position of each contact point, and the calculated angular momentum around the center of gravity, a Kalman filter is used to determine the position and speed of the center of gravity of the robot. A center of gravity estimation method comprising: estimating at least one of them.
JP2014089943A 2014-04-24 2014-04-24 Center of gravity estimation device and center of gravity estimation method Active JP6149791B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2014089943A JP6149791B2 (en) 2014-04-24 2014-04-24 Center of gravity estimation device and center of gravity estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2014089943A JP6149791B2 (en) 2014-04-24 2014-04-24 Center of gravity estimation device and center of gravity estimation method

Publications (2)

Publication Number Publication Date
JP2015208790A JP2015208790A (en) 2015-11-24
JP6149791B2 true JP6149791B2 (en) 2017-06-21

Family

ID=54611450

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2014089943A Active JP6149791B2 (en) 2014-04-24 2014-04-24 Center of gravity estimation device and center of gravity estimation method

Country Status (1)

Country Link
JP (1) JP6149791B2 (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113433338B (en) * 2021-07-02 2024-07-12 乐聚(深圳)机器人技术有限公司 Robot centroid speed calculation method, device and medium

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH02298489A (en) * 1989-05-10 1990-12-10 Hitachi Ltd Operation device for moving robot
JP3672426B2 (en) * 1996-12-19 2005-07-20 本田技研工業株式会社 Posture control device for legged mobile robot
JP2007007797A (en) * 2005-07-01 2007-01-18 Toyota Motor Corp Walking robot
JP4753068B2 (en) * 2005-09-15 2011-08-17 独立行政法人産業技術総合研究所 POSITION DETECTION DEVICE FOR MOBILE BODY AND MOBILE ROBOT
JP2009285816A (en) * 2008-05-30 2009-12-10 Toyota Motor Corp Leg type robot and control method of the same
JP5540850B2 (en) * 2010-04-09 2014-07-02 トヨタ自動車株式会社 Attitude estimation apparatus, method and program
JP5569681B2 (en) * 2010-04-23 2014-08-13 国立大学法人 東京大学 POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER

Also Published As

Publication number Publication date
JP2015208790A (en) 2015-11-24

Similar Documents

Publication Publication Date Title
JP5509226B2 (en) Kinematic and dynamic calibration method and system for a legged robot with force control joint and the robot
EP3751434B1 (en) Information processing device, information processing method, and storage medium
CN110662635B (en) Collision handling for robots
JP6380828B2 (en) Robot, robot system, control device, and control method
JP2015199155A (en) Information processing device, information processing method, and program
JP6795540B2 (en) Devices, methods and programs for estimating load weight and center of gravity position using a robot
Kim et al. On-line initialization and extrinsic calibration of an inertial navigation system with a relative preintegration method on manifold
JP2012137421A (en) Control device of robot performing force control using triaxial sensor
Danilov et al. Zmp trajectory from human body locomotion dynamics evaluated by kinect-based motion capture system
CN109983299A (en) The measuring system and method for industrial robot
KR20190022198A (en) Method for calibrating posture of lower body using wearable sensors, and computer readable medium for performing the method
JP2013018075A (en) Robot, robot controller, robot control method, and program
JP2013013987A (en) Robot system
JP6149791B2 (en) Center of gravity estimation device and center of gravity estimation method
Hawley et al. External force observer for medium-sized humanoid robots
JP2017120575A (en) Posture operation detector
JP2014087922A (en) Robot control device and method
CN108874146B (en) Moving human body mass center displacement calculation method applied to virtual reality system
JP2011255500A (en) Control device for legged mobile robot
JP2014200882A (en) Method for detecting gripping pattern of object to be gripped and robot
JP2017144490A (en) Control device, control system, control method and program
JP2020160594A (en) Self-position estimating method
JP4915934B2 (en) Control device for legged mobile robot
Yang et al. Novel walking assist device based on mobile manipulator and inertial measurement unit
Yunardi Marker-based motion capture for measuring joint kinematics in leg swing simulator

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20160513

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20170222

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20170307

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20170403

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20170425

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20170508

R151 Written notification of patent or utility model registration

Ref document number: 6149791

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151