JP2009107033A

JP2009107033A - Legged mobile robot and its control method

Info

Publication number: JP2009107033A
Application number: JP2007278727A
Authority: JP
Inventors: Hiroshi Shimada; 宏史嶋田; Yuji Tsusaka; 祐司津坂
Original assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Current assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Priority date: 2007-10-26
Filing date: 2007-10-26
Publication date: 2009-05-21

Abstract

<P>PROBLEM TO BE SOLVED: To provide a legged mobile robot stably moving on an uneven ground, and also to provide its control method. <P>SOLUTION: The legged mobile robot 1 includes: a trunk gait data corrective calculation part 112 for calculating acceleration deviation defined as deviation between intended target trunk acceleration and current trunk acceleration; and a correction amount change part 116 for calculating deviation of the center of gravity defined as deviation between an intended target center-of-gravity position and a current center-of-gravity position and correcting a target gait data value by the deviation of the center of gravity and the acceleration deviation. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

本発明は、脚式移動型のロボット及びその制御方法に関し、足先センサを用いずに安定した歩行が可能な脚式移動ロボット及びその制御方法に関する。 The present invention relates to a legged mobile robot and a control method thereof, and more particularly to a legged mobile robot capable of stable walking without using a foot sensor and a control method thereof.

近年、歩行するための脚部を備え、この脚部を駆動し、脚部の下端に設けられた足平部分を所定の歩容データに基づいて床面上に配置することで歩行動作を行う脚式移動型のロボットが開発されている。 In recent years, a leg for walking is provided, the leg is driven, and a foot portion provided at the lower end of the leg is placed on the floor surface based on predetermined gait data to perform a walking motion. Legged mobile robots have been developed.

このような脚式移動型のロボットは、まず、脚部の足平部分を床面に接触させて支持脚とし、その後に足平の裏面で床面を押して脚部全体（ロボット全体）を上げるように脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このように、遊脚と支持脚とを交互に繰り返して切り換えることで、歩行動作を行うことが可能になる。 In such a legged mobile robot, first, the foot portion of the leg is brought into contact with the floor surface to form a supporting leg, and then the floor surface is pushed by the back of the foot to raise the entire leg portion (the entire robot). Thus, the next walking motion is performed by driving the legs. The driven leg portion is a free leg, while the other leg portion is a support leg. In this way, a walking motion can be performed by alternately switching the free leg and the support leg.

このような歩行動作を安定して行う際には、ロボット全体の重心位置を制御して、脚部を駆動する必要がある。すなわち、左右に各々脚部を有する２足歩行タイプの脚式歩行型ロボットの場合、歩行する床面に接地する支持脚の足平部分の接地面内部に、ロボット全体の重心によるモーメントが作用しない点（ＺＭＰ＝ＺｅｒｏＭｏｍｅｎｔＰｏｉｎｔ）を位置させる。 In order to stably perform such a walking motion, it is necessary to control the position of the center of gravity of the entire robot and drive the leg portion. That is, in the case of a biped walking type legged walking type robot having leg portions on the left and right sides, the moment due to the center of gravity of the entire robot does not act on the ground contact surface of the foot portion of the supporting leg that contacts the floor surface to walk. A point (ZMP = Zero Moment Point) is located.

そのような脚式移動型のロボットとして、特許文献１には、予期しない凹凸がある路面でも安定して歩行することを目的とした脚式移動ロボットが開示されている。この特許文献１に記載の脚式移動ロボットは、予め設定された各関節の目標角度を所定時刻ごとに各関節に配置したサーボモータに出力し、各関節の実角度が目標角度に追従するように制御するロボットにおいて、ロボットの姿勢を検出する検出手段及び検出された姿勢に基づいてロボットが所定の姿勢にあるか否かを判定する姿勢判定手段を備え、胴体の傾斜角、傾斜角速度を検出し、その値が所定範囲外である場合に、歩容を時間方向に変更する。例えば、前傾している場合には、早く着地させる。 As such a legged mobile robot, Patent Document 1 discloses a legged mobile robot intended to walk stably on a road surface with unexpected unevenness. The legged mobile robot described in Patent Document 1 outputs a preset target angle of each joint to a servo motor arranged in each joint at a predetermined time so that the actual angle of each joint follows the target angle. The robot to be controlled is equipped with a detecting means for detecting the posture of the robot and a posture determining means for determining whether the robot is in a predetermined posture based on the detected posture, and detects the tilt angle and tilt angular velocity of the body. If the value is out of the predetermined range, the gait is changed in the time direction. For example, if the vehicle is tilted forward, land early.

また、特許文献２には、足先センサを用いずに安定した歩行を行う脚式ロボットとその歩行制御方法が開示されている。図６は、特許文献２に記載のロボットの制御システムを示す図である。図６に示すように、脚式ロボット２０１は、体幹と、体幹に対して揺動可能に連結されている脚リンクと、目標とする足先運動の経時的変化を記述する足先歩容データ、目標足先運動の変化に追従して歩行を可能とする目標とする体幹運動の経時的変化を記述する体幹歩容データを記憶する歩容データ記憶装置２１１、関節角群計算装置２１３、アクチュエータ制御部２１４、加速度センサ２１７、実際の体幹加速度を検出する体幹加速度検出装置２１５、体幹歩容データ補正演算部２１２を有する。 Patent Document 2 discloses a legged robot that performs stable walking without using a foot tip sensor and a walking control method thereof. FIG. 6 is a diagram showing a robot control system described in Patent Document 2. As shown in FIG. As shown in FIG. 6, the legged robot 201 includes a trunk, a leg link that is swingably connected to the trunk, and a footstep that describes a change over time in a target foot movement. Gait data storage device 211 that stores trunk gait data that describes changes over time in target trunk motion that enables walking while following changes in target data, target toe movement, joint angle group calculation A device 213, an actuator control unit 214, an acceleration sensor 217, a trunk acceleration detection device 215 that detects actual trunk acceleration, and a trunk gait data correction calculation unit 212 are included.

体幹歩容データ補正演算部２１２は、体幹位置姿勢目標値を２階微分して目標体幹加速度を算出する装置２３１と、目標とする体幹運動と実際の体幹運動の偏差を算出する偏差算出部２３２と、算出された偏差から所定の伝達関数に基づいて補正量を求め、その補正量に基づいて体幹歩容データ記憶装置２１１に記憶されている体幹歩容データを補正する外乱力算出装置２３３、目標補正量算出装置２３４及び体幹位置補正装置２３５とを備えている。目標とする体幹加速度と、実際の体幹加速度との偏差に応じて、歩容の体幹位置姿勢目標値を補正する。主に、加速度の偏差に基づき体幹位置を補正している。
特許３１６７４０７号公報特開２００５−２１２０１２号公報 The trunk gait data correction calculation unit 212 calculates a target trunk acceleration by calculating the target trunk acceleration by second-order differentiation of the trunk position / posture target value, and calculates a deviation between the target trunk exercise and the actual trunk exercise. And calculating a correction amount based on a predetermined transfer function from the calculated deviation, and correcting the trunk gait data stored in the trunk gait data storage device 211 based on the correction amount A disturbance force calculating device 233, a target correction amount calculating device 234, and a trunk position correcting device 235. The trunk position / posture target value of the gait is corrected according to the deviation between the target trunk acceleration and the actual trunk acceleration. Mainly, the trunk position is corrected based on the deviation of acceleration.
Japanese Patent No. 3167407 JP 2005-212012 A1

しかしながら、特許文献１に記載の技術は、歩容を時間方向に補正する手段について、たとえば前傾している場合には、早く着地させるために各関節の速度が速くなる。このためモータの速度がより必要になってしまい、関節の粘性摩擦の影響などによって、モータのトルクが必要となる、すなわちエネルギー効率が悪いという問題点がある。また、例えば高速に歩行している場合には、前傾とするとモータの速度を速くする方向に補正されるので、モータの性能以上の目標関節角度が要求される場合があり得る。さらにまた、胴体が前傾した状態のまま、時間方向に歩容を早めて、早く着地させようとすると、ロボット全体が前傾しているため、遊脚の着地位置目標は床よりも下面に設定されている。このため、着地時にロボットが床よりもさらに鉛直下方に遊脚足先を動作させようとして床を強く蹴ってしまう。すなわち、時間方向の補正が入ると、前傾している場合には、着地時に遊脚で路面を鉛直方向に強く蹴ってしまい、その後、不安定になりやすい。 However, in the technique described in Patent Document 1, when the gait is corrected in the time direction, for example, when tilting forward, the speed of each joint is increased in order to land quickly. For this reason, the speed of the motor becomes more necessary, and there is a problem that the torque of the motor is required due to the influence of viscous friction of the joint, that is, the energy efficiency is poor. For example, when walking at a high speed, if the vehicle is tilted forward, the motor speed is corrected in a direction that increases, so that a target joint angle that exceeds the performance of the motor may be required. Furthermore, if the body is tilted forward and the gait is advanced in the time direction to try to land earlier, the entire robot is tilted forward, so the landing position target of the free leg is lower than the floor. Is set. For this reason, at the time of landing, the robot strongly kicks the floor in an attempt to move the free leg toe further vertically below the floor. That is, when correction in the time direction is entered, if the vehicle is tilted forward, the landing surface is strongly kicked in the vertical direction by the free leg at the time of landing, and then becomes unstable.

一方、特許文献２では、以下の問題が生じる。ここで、ロボットが凹部と凸部に着地した場合について説明する。図７（ｂ）に示すように、ロボットが凸部３０１に着地した場合は、もともとの重心位置３０２が目標重心位置に対して遅れているので補正後重心位置３０３のように大きく補正しても問題にならない。これに対し、図７（ａ）に示すように、ロボットが凹部３０４に着地した場合、重心位置３０５が補正後重心位置３０６へ補正され、補正が大きいと目標重心位置より外側に大きく外れてしまい、その後に回復することが困難になる。特許文献２に記載の技術では、凹部３０６に着地した場合、凹部３０４であることによるロボットの姿勢偏差分と加速度フィードバック制御によるロボットの胴体補正分の和が胴体のずれ分になるが、凹部３０４の場合、そのずれ分が大きくなってしまい、支持脚からみた重心位置の水平方向の偏差が大きくなってしまう。すなわち、路面外乱に対して、足裏の接地性能が良くなるように制御されるが、凹部３０６に着地した場合、胴体は実際の重心位置が目標重心位置より大きく外れる側に補正されてしまうため、重心位置がその外側に移動してしまい、その後、重心位置を戻すだけの床反力モーメントを発生させられないという問題点がある。 On the other hand, in Patent Document 2, the following problems occur. Here, a case where the robot has landed on the concave portion and the convex portion will be described. As shown in FIG. 7B, when the robot has landed on the convex portion 301, the original center of gravity position 302 is delayed with respect to the target center of gravity position. It doesn't matter. On the other hand, as shown in FIG. 7A, when the robot has landed in the recess 304, the center of gravity position 305 is corrected to the corrected center of gravity position 306, and if the correction is large, the center of gravity is greatly deviated from the target center of gravity position. Then it becomes difficult to recover. In the technique described in Patent Document 2, when landing on the concave portion 306, the sum of the posture deviation of the robot due to the concave portion 304 and the body correction of the robot by acceleration feedback control becomes the amount of deviation of the trunk. In this case, the deviation becomes large, and the deviation in the horizontal direction of the center of gravity as viewed from the support leg becomes large. In other words, the ground contact performance of the sole is controlled to improve against the road surface disturbance, but when landing on the recess 306, the trunk is corrected to the side where the actual center of gravity position deviates greatly from the target center of gravity position. However, there is a problem that the center of gravity position moves to the outside, and thereafter, the floor reaction force moment for returning the center of gravity position cannot be generated.

本発明は、このような問題点を解決するためになされたものであり、凹凸がある地面であっても安定して移動が可能な脚式移動ロボット及びその制御方法を提供することを目的とする。 The present invention has been made to solve such problems, and an object of the present invention is to provide a legged mobile robot that can move stably even on uneven ground and a control method thereof. To do.

上述した目的を達成するために、脚式移動ロボットにおいて、目標とする目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出手段と、目標とする目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出手段と、前記第１偏差及び第２偏差により歩容データ目標値を補正する補正手段とを有するものである。 In order to achieve the above-described object, in a legged mobile robot, an acceleration deviation calculating means for calculating a first deviation which is a deviation between a target trunk acceleration and a target trunk acceleration, and a target target gravity center A center-of-gravity position deviation calculating unit that calculates a second deviation that is a deviation between the position and the current center-of-gravity position; and a correcting unit that corrects the gait data target value based on the first and second deviations.

本発明においては、第１偏差及び第２偏差により体幹歩容データ目標値を補正する補正手段を有するので、歩容データ目標値を的確に補正することができる。 In the present invention, the gait data target value can be accurately corrected because the gait data target value is corrected by the first deviation and the second deviation.

また、前記補正手段は、前記歩容データ目標値を前記第１偏差に基づき補正する体幹歩容データ補正演算手段と、前記第１偏差及び第２偏差に基づき、前記体幹歩容データ補正演算手段の補正の度合を変更する補正量変更手段とを有することができる。これにより、第１偏差により歩容データを補正するのみならず、その補正の度合を調整するため、より精密な補正が可能となる。 The correcting means corrects the gait data target value based on the first deviation, and corrects the trunk gait data based on the first deviation and the second deviation. Correction amount changing means for changing the degree of correction of the calculating means. Accordingly, not only the gait data is corrected by the first deviation, but also the degree of correction is adjusted, so that more precise correction is possible.

さらに、前記体幹歩容データ補正演算手段は、前記第１偏差から補正量を生成する補正量生成手段と、前記補正量生成手段が生成した補正量で前記歩容データ目標値を補正する目標値補正手段とを有することができる。 Further, the trunk gait data correction calculating means includes a correction amount generating means for generating a correction amount from the first deviation, and a target for correcting the gait data target value with the correction amount generated by the correction amount generating means. Value correction means.

さらにまた、前記補正量変更手段は、前記第１偏差及び第２偏差が同符号の場合にのみ前記歩容データ目標値の補正量を調整する調整係数を生成することができる。第１偏差及び第２偏差が同符号の場合は、凹部に着地したと判断でき、歩容データ目標値を第１偏差の値に応じて補正する際、その補正量を修正する調整係数を生成する。第１偏差及び第２偏差の符号が異符号の場合は、凸部に着地したと判断することができ、この場合は、第１偏差の値に応じて生成された補正量で歩容データ目標値を補正する。 Furthermore, the correction amount changing means can generate an adjustment coefficient for adjusting the correction amount of the gait data target value only when the first deviation and the second deviation have the same sign. When the first deviation and the second deviation have the same sign, it can be determined that the landing has occurred in the recess, and when the gait data target value is corrected according to the value of the first deviation, an adjustment coefficient for correcting the correction amount is generated. To do. When the signs of the first deviation and the second deviation are different signs, it can be determined that the landing has occurred on the convex part. In this case, the gait data target is generated with the correction amount generated according to the value of the first deviation. Correct the value.

また、前記補正量生成手段は、前記第１偏差に予め定められた比例ゲイン及び伝達関数、並びに前記補正量変更手段が生成した調整係数を乗算して補正量を生成し、前記補正量変更手段は、前記第２偏差が大きいほど前記補正量が小さくなるよう前記調整係数を変更することができる。調整係数を小さくすることで補正量を減少させ、凹部に着地した際にロボットの重心位置が大きく移動してしまうことを防止する。 The correction amount generation means generates a correction amount by multiplying the first deviation by a predetermined proportional gain and transfer function and an adjustment coefficient generated by the correction amount change means, and the correction amount change means The adjustment coefficient can be changed so that the correction amount decreases as the second deviation increases. By reducing the adjustment coefficient, the amount of correction is reduced, and the position of the center of gravity of the robot is prevented from moving greatly when landing on the recess.

さらにまた、前記補正量変更手段は、前記第２偏差が第１の閾値より小さい場合は調整係数を１とし、前記第１の閾値乃至第２の閾値の間は１から０への単調減少関数とし、前記第２の閾値以上の場合は前記調整係数をゼロとすることができる。重心位置偏差の値に応じて調整係数を適宜変更することができる。 Furthermore, the correction amount changing means sets the adjustment coefficient to 1 when the second deviation is smaller than the first threshold, and a monotonically decreasing function from 1 to 0 between the first threshold and the second threshold. If the value is equal to or greater than the second threshold, the adjustment coefficient can be set to zero. The adjustment coefficient can be changed as appropriate according to the value of the gravity center position deviation.

さらに、前記重心位置偏差算出手段の代わりに、目標胴体位置と、現在の胴体位置との偏差である第３偏差を算出する胴体位置偏差算出手段、又は目標胴体姿勢と、現在の胴体姿勢との偏差である第４偏差を算出する胴体姿勢位置偏差算出手段を設けるものとしてもよい。ロボットの質量のうち、大半がロボットの体幹に集中していることから、重心位置の代わりに胴体位置や胴体姿勢位置を使用してもよい。 Further, instead of the center-of-gravity position deviation calculating means, a fuselage position deviation calculating means for calculating a third deviation which is a deviation between the target body position and the current body position, or a target body posture and a current body posture. It is good also as providing the body posture position deviation calculation means which calculates the 4th deviation which is a deviation. Since most of the mass of the robot is concentrated on the trunk of the robot, the body position or the body posture position may be used instead of the position of the center of gravity.

さらにまた、前記歩容データ目標値は、体幹位置姿勢目標値及び足先位置姿勢目標値であるとすることができる。体幹位置姿勢は、絶対座標系に対するロボットの胴体に固定された座標系の位置及び傾きで表わされる。また、足先位置姿勢は、絶対座標系に対するロボットの足先に固定された座標系の位置及び傾きで表わされる。 Furthermore, the gait data target values may be a trunk position / posture target value and a toe position / posture target value. The trunk position / posture is represented by the position and inclination of a coordinate system fixed to the body of the robot with respect to the absolute coordinate system. Also, the foot position / posture is represented by the position and inclination of a coordinate system fixed to the robot's foot with respect to the absolute coordinate system.

本発明にかかる脚式移動ロボットの制御方法は、脚式移動ロボットの制御方法であって、目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出工程と、目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出工程と、前記第１偏差及び第２偏差により歩容データ目標値を補正する補正工程とを有する。 A legged mobile robot control method according to the present invention is a legged mobile robot control method, comprising: an acceleration deviation calculating step of calculating a first deviation which is a deviation between a target trunk acceleration and a current trunk acceleration; A gravity center position deviation calculating step of calculating a second deviation which is a deviation between the target gravity center position and the current gravity center position, and a correcting step of correcting the gait data target value by the first deviation and the second deviation.

本発明によれば、凹凸がある地面であっても安定して移動が可能な脚式移動ロボット及びその制御方法を提供することができる。 According to the present invention, it is possible to provide a legged mobile robot that can move stably even on uneven ground and a control method thereof.

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。図１は、ロボット１を正面から見た様子を概略的に表す概略図であり、床面Ｆ上をロボット１が歩行する様子を表しているなお、図１においては、説明の便宜上、ロボット１が進行する向き（前後方向）をｘ軸、ロボット１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面の奥行方向、ｙ軸は紙面に向かって左右方向、ｚ軸は紙面中の上下方向を示す。この座標系を、絶対座標系Ｏｇ−ＸｇＹｇＺｇに設定する。 Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. FIG. 1 is a schematic view schematically showing a state in which the robot 1 is viewed from the front, and shows a state in which the robot 1 walks on the floor F. In FIG. The x direction is the direction in which the robot travels (front-rear direction), the y-axis is the direction orthogonal to the horizontal direction (left-right direction) in the direction in which the robot 1 travels, and the direction (vertical direction) extends in the vertical direction from the plane on which the moving body moves A description will be given using the coordinate system consisting of these three axes as the z-axis. That is, in FIG. 1, the x-axis indicates the depth direction of the paper surface, the y-axis indicates the left-right direction toward the paper surface, and the z-axis indicates the vertical direction in the paper surface. This coordinate system is set to the absolute coordinate system Og-XgYgZg.

図１に示すように、ロボット１は、頭部２と、体幹３と、体幹３に結合された腰部４と、体幹３に接続された右腕５、左腕６と、腰部４に対して回動自在に固定される脚部１０と、を備えた２足歩行型のロボットである。以下、詳細に説明する。 As shown in FIG. 1, the robot 1 has a head 2, a trunk 3, a waist 4 coupled to the trunk 3, a right arm 5, a left arm 6, and a waist 4 connected to the trunk 3. And a leg 10 that is pivotably fixed. This will be described in detail below.

頭部２は、ロボット１の周囲の環境を視覚的に撮像するための左右一対の撮像部（図示せず）を備えているとともに、体幹３に対して頭部２を鉛直方向に平行な軸周りに回動させることで、周囲の環境を広く撮像する。撮像した周囲の環境を示す画像データは、制御部１３０に送信され、ロボット１の動作を決定するための情報として用いられる。 The head 2 includes a pair of left and right imaging units (not shown) for visually imaging the environment around the robot 1, and the head 2 is parallel to the trunk 3 in the vertical direction. By rotating around the axis, the surrounding environment is imaged widely. The captured image data indicating the surrounding environment is transmitted to the control unit 130 and used as information for determining the operation of the robot 1.

体幹３は、その内部にロボット１の動作を制御する制御部１３０や、脚部のモータ等に電力を供給するためのバッテリー（図示せず）等を収容するものである。制御部１３０は、脚部１０を駆動し、ロボット１を動かすための歩容データを記憶する記憶領域と、この記憶領域に記憶された歩容データを読み出す演算処理部と、脚部１０に含まれるモータを駆動するモータ駆動部と、を備えている。これらの各構成要素は、体幹３の内部に設けられたバッテリー（図示せず）から電力を供給されることで動作する。ここで、体幹部に対しては、胴体座標系Ｏｂ−ＸｂＹｂＺｂを設定する。後述する目標胴体位置は、絶対座標系Ｏｇ−ＸｇＹｇＺｇの原点Ｏｂの位置で表わされる。また、後述する目標胴体姿勢角は、絶対座標系Ｏｇ−ＸｇＹｇＺｇに対する胴体座標系Ｏｂ−ＸｂＹｂＺｂの傾きで表わされる。 The trunk 3 houses therein a control unit 130 that controls the operation of the robot 1, a battery (not shown) for supplying power to a leg motor and the like. The control unit 130 includes a storage area that stores gait data for driving the leg 10 and moving the robot 1, an arithmetic processing unit that reads gait data stored in the storage area, and the leg 10. A motor driving unit for driving the motor. Each of these components operates by being supplied with electric power from a battery (not shown) provided inside the trunk 3. Here, the trunk coordinate system Ob-XbYbZb is set for the trunk. A later-described target body position is represented by the position of the origin Ob of the absolute coordinate system Og-XgYgZg. In addition, a target body posture angle, which will be described later, is expressed by an inclination of the body coordinate system Ob-XbYbZb with respect to the absolute coordinate system Og-XgYgZg.

また、演算処理部は、記憶領域に記憶された歩容データを読み出すとともに、読み出した歩容データによって特定されるロボット１の姿勢を実現するために必要な脚部１０の関節角を算出する。そして、このように算出した関節角に基づく信号をモータ駆動部に送信する。 The arithmetic processing unit reads out the gait data stored in the storage area and calculates the joint angle of the leg 10 necessary to realize the posture of the robot 1 specified by the read out gait data. And the signal based on the joint angle computed in this way is transmitted to a motor drive part.

モータ駆動部は、演算処理部より送信された信号に基づいて、脚部を駆動するための各モータの駆動量を特定し、これらの駆動量でモータを駆動させるためのモータ駆動信号を各モータに送信する。これによって脚部１０の各関節における駆動量が変更され、ロボット１の動きが制御される。 The motor drive unit specifies the drive amount of each motor for driving the leg portion based on the signal transmitted from the arithmetic processing unit, and the motor drive signal for driving the motor with these drive amounts is sent to each motor. Send to. As a result, the driving amount at each joint of the leg 10 is changed, and the movement of the robot 1 is controlled.

また、演算処理部は、読み出した歩容データに基づいてモータの駆動を行うように指令するほか、ロボット１に組み込まれたジャイロや加速度計などセンサ（図示せず）からの信号を受けて、モータの駆動量を調整する。また、レーザセンサなどを設けて、床面Ｆまでの距離を検出してもよい。ジャイロセンサや加速度計やレーザセンサなどの各種センサは、例えば、体幹３や腰部４に設けられる。このように、センサにより検出したロボット１に作用する外力や、ロボット１の姿勢などに応じて脚部１０の関節角を調整することで、ロボット１が安定した状態を維持することができる。 In addition to instructing the motor to be driven based on the read gait data, the arithmetic processing unit receives a signal from a sensor (not shown) such as a gyroscope or an accelerometer built in the robot 1, Adjust the drive amount of the motor. Further, a distance to the floor surface F may be detected by providing a laser sensor or the like. Various sensors such as a gyro sensor, an accelerometer, and a laser sensor are provided on the trunk 3 and the waist 4, for example. Thus, the robot 1 can be maintained in a stable state by adjusting the joint angle of the leg 10 according to the external force acting on the robot 1 detected by the sensor, the posture of the robot 1, and the like.

右腕５および左腕６は、体幹３に対して回動自在に接続されており、肘部分および手首部分に設けられた関節部分を駆動することにより、人間の腕部と同様の動きを行うことができる。また、手首部分の先端に接続された手先部は、図示を省略するが物体を把持するためのハンド構造を備えており、ハンド構造に組み込まれた複数の指関節を駆動することで、様々な形状の物体を把持することが可能となる。 The right arm 5 and the left arm 6 are pivotally connected to the trunk 3, and perform the same movement as a human arm portion by driving joint portions provided at the elbow portion and the wrist portion. Can do. Further, the hand part connected to the tip of the wrist part has a hand structure for gripping an object (not shown), and by driving a plurality of finger joints incorporated in the hand structure, various kinds of parts are provided. It becomes possible to grip an object having a shape.

腰部４は、体幹３に対して回動するように接続されており、歩行動作を行う際に腰部４の回動動作を組み合わせることで、脚部１０を駆動するために必要な駆動エネルギーを低減させることができる。 The lumbar part 4 is connected to the trunk 3 so as to rotate, and the driving energy necessary for driving the leg part 10 can be obtained by combining the rotational action of the lumbar part 4 when performing a walking action. Can be reduced.

２足歩行を行うための脚部１０（右脚２０、左脚３０）は、右脚２０と左脚３０とから構成されている。詳細には、図１に示すように、右脚２０は右股関節２１、右上腿２２、右膝関節２３、右下腿２４、右足首関節２５、右足平２６を備え、同様に、左脚３０は左股関節３１、左上腿３２、左膝関節３３、左下腿３４、左足首関節３５、左足平３６を備えている。 A leg 10 (right leg 20 and left leg 30) for bipedal walking is composed of a right leg 20 and a left leg 30. Specifically, as shown in FIG. 1, the right leg 20 includes a right hip joint 21, an upper right thigh 22, a right knee joint 23, a right lower thigh 24, a right ankle joint 25, and a right foot 26. A left hip joint 31, a left upper thigh 32, a left knee joint 33, a left lower thigh 34, a left ankle joint 35, and a left foot 36 are provided.

そして、右脚２０および左脚３０とは、図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。ここで、足平に関しては、足平座標系Ｏｆ−ＸｆＹｆＺｆを設定する。後述する目標足平位置は、絶対座標系Ｏｇ−ＸｇＹｇＺｇに対する足平座標系Ｏｆ−ＸｆＹｆＺｆの原点の位置で表わされる。目標足平姿勢角は、接地面に対する足裏面の角度で表わされる。 The right leg 20 and the left leg 30 are each driven to a desired angle by transmitting a driving force from a motor (not shown) via a pulley and a belt (not shown). A desired movement can be made to a leg part. Here, for the foot, the foot coordinate system Of-XfYfZf is set. A later-described target foot position is represented by the position of the origin of the foot coordinate system Of-XfYfZf with respect to the absolute coordinate system Og-XgYgZg. The desired foot posture angle is represented by the angle of the sole surface with respect to the ground contact surface.

歩容データは、操作部（図示せず）から送られる信号で特定される脚部の移動量に対応づけて、脚部１０の足平（右足平２６、左足平３６）の先端（足先）の位置と、移動体本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。歩容データとしては、目標胴体位置、目標胴体姿勢角、目標足平位置、目標足平姿勢角などがある。歩容データは、ロボット１を実際に動作させる前に、シミュレーション等によってロボット１が安定して歩行できるよう、すなわち転倒せずに歩行できるように作成されている。作成された歩容データは、ロボット１の制御部（コントローラ）１３０に記憶される。制御部１３０は、実際の胴体位置等を歩容データに含まれる目標胴体位置等に追従させるように各関節を制御する。 The gait data is associated with the amount of movement of the leg specified by the signal sent from the operation unit (not shown), and the tip of the foot (the right foot 26, the left foot 36) of the leg 10 (the tip of the foot). ) And the position of the moving body main body are indicated with time in a coordinate system (for example, xyz coordinate system) that defines a space in which the robot 1 moves. The gait data includes a target body position, a target body posture angle, a target foot position, a target foot posture angle, and the like. The gait data is created so that the robot 1 can walk stably, that is, without falling down, by simulation or the like before actually operating the robot 1. The created gait data is stored in the control unit (controller) 130 of the robot 1. The control unit 130 controls each joint so that the actual body position or the like follows the target body position or the like included in the gait data.

図２は、ロボットの制御システムの機能を示すブロック図である。図２に示す要素のうち、ロボットの機械系および体幹加速度センサ１７を除いた部分が制御システムの構成要素である。制御システムは、物理的に１つの装置に含まれていてもよいし、物理的に分離された装置ごとに分けて収容されていてもよい。 FIG. 2 is a block diagram showing functions of the robot control system. Of the elements shown in FIG. 2, the parts excluding the robot mechanical system and the trunk acceleration sensor 17 are components of the control system. The control system may be physically included in one device, or may be accommodated separately for each physically separated device.

ロボット１は、歩容データ記憶部１１１、体幹歩容データ補正演算部１１２、関節角変換部１１３、各軸制御器１１４、体幹加速度計算部１１５、補正量変更部１１６、体幹加速度センサ１１７ａ、各種センサ及び関節エンコーダ（以下、まとめてセンサ１１７という。）を有する。 The robot 1 includes a gait data storage unit 111, a trunk gait data correction calculation unit 112, a joint angle conversion unit 113, each axis controller 114, a trunk acceleration calculation unit 115, a correction amount change unit 116, a trunk acceleration sensor. 117a, various sensors and joint encoders (hereinafter collectively referred to as sensors 117).

歩容データ記憶装置１１１には、予め計算されたロボット１の歩容データが記憶されている。歩容データとは、腕先軌道データ（腕先歩容データ）、足先軌道データ（足先歩容データ）及び体幹軌道データ（体幹歩容データ）のことをいう。予め記憶されている歩容データのうち、腕先軌道データ及び足先軌道データは、関節角変換部１１３へ直接に入力される。体幹歩容データは、体幹歩容データ補正演算部１１２へ入力され、ロボット１の歩行に伴いその都度補正され、補正された体幹軌道を示すデータが関節角変換部１１３へ入力される。また、関節角等から演算により歩容データをリアルタイムに生成してもよい。足先軌道データと腕先軌道データは、補正されないで直接に関節角変換部１１３に入力される。一方、体幹位置姿勢目標値は体幹歩容データ補正演算部１１２に入力される。 The gait data storage device 111 stores gait data of the robot 1 calculated in advance. Gait data refers to arm tip trajectory data (arm tip gait data), foot tip trajectory data (foot tip gait data), and trunk trajectory data (trunk gait data). Of the gait data stored in advance, the arm tip trajectory data and the foot tip trajectory data are directly input to the joint angle conversion unit 113. The trunk gait data is input to the trunk gait data correction calculation unit 112, corrected each time the robot 1 walks, and data indicating the corrected trunk trajectory is input to the joint angle conversion unit 113. . Further, the gait data may be generated in real time by calculation from the joint angle or the like. The toe trajectory data and the arm tip trajectory data are directly input to the joint angle conversion unit 113 without being corrected. On the other hand, the trunk position / posture target value is input to the trunk gait data correction calculation unit 112.

関節角変換部１１３は、両足位置姿勢目標値及び、補正された体幹位置姿勢目標値からなる歩容データが入力される。この歩容データに基づいて、いわゆる逆キネマティクスを解くことでロボット１の各関節角θ１（ｔ）、θ２（ｔ）、θ３（ｔ）、・・・を計算する。計算された関節角群データは、各軸制御器１１４に入力される。 The joint angle conversion unit 113 receives gait data composed of both foot position / posture target values and the corrected trunk position / posture target values. Based on the gait data, the joint angles θ1 (t), θ2 (t), θ3 (t),... Of the robot 1 are calculated by solving so-called inverse kinematics. The calculated joint angle group data is input to each axis controller 114.

各軸制御器１１４は、図１のロボット１の関節群を回転させるアクチュエータ群を制御する。アクチュエータ群はロボットの機械系に存在している。アクチュエータ群を制御すると、ロボット１の関節角を計算された関節角に調整できる。これにより、ロボット１は歩容データに従って歩行する。 Each axis controller 114 controls an actuator group that rotates the joint group of the robot 1 of FIG. The actuator group exists in the mechanical system of the robot. By controlling the actuator group, the joint angle of the robot 1 can be adjusted to the calculated joint angle. Thereby, the robot 1 walks according to the gait data.

ロボット１の体幹部には、センサの１つとして、体幹加速度センサ１１７ａが搭載されている。体幹加速度センサ１１７ａにより、体幹のｘ方向（歩行方向）とｙ方向（体側方向）とｚ方向（高さ方向）の加速度を検出する。体幹加速度計算部１１５は、体幹加速度センサにて測定された体幹の加速度を、目標体幹加速度を算出する際に使用した体幹部の位置に対応する位置の体幹加速度に変換する。この実際の体幹加速度ａｘｒ、ａｙｒ、ａｚｒを体幹歩容データ補正演算部１１２へ出力する。 A trunk acceleration sensor 117a is mounted on the trunk of the robot 1 as one of the sensors. The trunk acceleration sensor 117a detects accelerations of the trunk in the x direction (walking direction), the y direction (body side direction), and the z direction (height direction). The trunk acceleration calculation unit 115 converts the trunk acceleration measured by the trunk acceleration sensor into a trunk acceleration at a position corresponding to the position of the trunk used when calculating the target trunk acceleration. The actual trunk accelerations axr, ayr, and azr are output to the trunk gait data correction calculation unit 112.

体幹歩容データ補正演算部１１２は、歩容データ記憶部１１１に記憶されている体幹軌道データ（体幹歩容データ）と、体幹加速度計算部１１５で計算されたロボット１の実際の体幹加速度（以下、実体幹加速度という。）に基づいて、体幹軌道データの補正を行う。具体的には、実体幹加速度と、目標となる目標体幹加速度の偏差である第１偏差に基づき、歩容データ目標値のうち、体幹位置姿勢目標値を補正する。補正された体幹位置姿勢目標値は、関節角変換部１１３へ入力される。 The trunk gait data correction calculation unit 112 includes the trunk trajectory data (trunk gait data) stored in the gait data storage unit 111 and the actual robot 1 calculated by the trunk acceleration calculation unit 115. Based on the trunk acceleration (hereinafter referred to as the actual trunk acceleration), the trunk trajectory data is corrected. Specifically, the trunk position / posture target value is corrected among the gait data target values based on the first deviation which is the deviation between the actual trunk acceleration and the target trunk trunk acceleration. The corrected trunk position / posture target value is input to the joint angle conversion unit 113.

さらに、本実施の形態においては、体幹加速度偏差及びロボット１の実際の重心位置（以下、実重心位置という。）と目標となる目標重心位置との偏差である第２偏差である重心位置偏差に基づき、前記体幹位置姿勢目標値の補正の度合を変更する補正量変更部１１６を有する。補正量変更部１１６は、体幹加速度偏差及び重心位置偏差の値が同符号の場合に体幹位置姿勢目標値の補正量を修正するが、体幹加速度偏差及び重心位置偏差の値が異符号である場合は補正量の修正はしない。具体的には、重心位置偏差が大きいほど体幹位置姿勢目標値の補正量が小さくなるよう、当該補正量に乗算する後述する調整係数を出力する。ここで、体幹歩容データ補正演算部１１２及び補正量変更部１１６から体幹加速度偏差及び重心位置偏差により体幹位置姿勢目標値を補正する補正手段が構成される。 Furthermore, in this embodiment, the trunk acceleration deviation and the center of gravity position deviation which is the second deviation which is the deviation between the actual center of gravity position of the robot 1 (hereinafter referred to as the actual center of gravity position) and the target center of gravity position. And a correction amount changing unit 116 for changing the degree of correction of the trunk position / posture target value. The correction amount changing unit 116 corrects the correction amount of the trunk position / posture target value when the trunk acceleration deviation and the centroid position deviation have the same sign, but the trunk acceleration deviation and the centroid position deviation have different signs. If it is, the correction amount is not corrected. Specifically, an adjustment coefficient (to be described later) for multiplying the correction amount is output so that the correction amount of the trunk position / posture target value decreases as the gravity center position deviation increases. Here, the trunk gait data correction calculation unit 112 and the correction amount change unit 116 constitute correction means for correcting the trunk position / posture target value based on the trunk acceleration deviation and the gravity center position deviation.

次に、体幹歩容データ補正演算部１１２について説明する。体幹歩容データ補正演算部１１２は、２階微分演算部１３１、加速度偏差算出部１３２、補正量生成部１３３及び目標値補正手段としての目標体幹位置姿勢補正部１３４を有する。ここでは、図示の明瞭化のために、ｘ方向の事象のみを図示している。ｙ方向とｚ方向についても同様である。 Next, the trunk gait data correction calculation unit 112 will be described. The trunk gait data correction calculation unit 112 includes a second-order differential calculation unit 131, an acceleration deviation calculation unit 132, a correction amount generation unit 133, and a target trunk position / posture correction unit 134 as target value correction means. Here, only events in the x direction are shown for clarity of illustration. The same applies to the y direction and the z direction.

２階微分演算部１３１は、歩容データ記憶部１１１に記憶されている体幹歩容データが示す目標体幹位置ｘｏ、ｙｏ、ｚｏ（それぞれは経時的に変化する）を、時間に関して２階微分演算を行って、目標体幹加速度ａｘｏ、ａｙｏ、ａｚｏを算出する。加速度偏差算出部３２は、実際の体幹加速度ａｘｒ、ａｙｒ、ａｚｒから、実際の体幹加速度ａｘｏ、ａｙｏ、ａｚｏを減じて、加速度偏差Δａｘ、Δａｙ、Δａｚを算出する。 The second-order differential calculation unit 131 converts the target trunk positions xo, yo, and zo (each of which changes with time) indicated by the trunk gait data stored in the gait data storage unit 111 to the second floor with respect to time. A differential calculation is performed to calculate target trunk accelerations axo, ayo, and azo. The acceleration deviation calculation unit 32 calculates acceleration deviations Δax, Δay, Δaz by subtracting the actual trunk accelerations axo, ayo, azo from the actual trunk accelerations axr, ayr, azr.

補正量算出部１３３は、算出された加速度偏差Δａｘ、Δａｙ、Δａｚに、比例ゲイン、調整係数ｋ、伝達関数１／Ｇ（ｘ）を乗じて補正量ｘｄ、ｙｄ、ｚｄを算出する。ここで、調整係数ｋは、後述するように、補正量の値を調整するための係数である。本実施の形態においては、加速度偏差Δａｘ、Δａｙ、Δａｚに対し、−Ｍ／（Ｍｉ×ｓ^２＋Ｃｉ×ｓ＋Ｋｉ）を乗算している。 The correction amount calculation unit 133 calculates correction amounts xd, yd, and zd by multiplying the calculated acceleration deviations Δax, Δay, and Δaz by a proportional gain, an adjustment coefficient k, and a transfer function 1 / G (x). Here, the adjustment coefficient k is a coefficient for adjusting the value of the correction amount, as will be described later. In the present embodiment, the acceleration deviations Δax, Δay, Δaz are multiplied by −M / (Mi × s ² + Ci × s + Ki).

ここで、伝達関数１／Ｇ（ｓ）は、２次の項の係数をＭｉ、１次の項の係数をＣｉ、０次の項の係数をＫｉとする遅れ要素からなる。Ｍｉ、Ｃｉ、Ｋｉは所望のパラメータであり、予め指定されている。ｓはラプラス変換子、伝達関数のゲインは１／Ｋｉで、比例ゲインとしては−Ｍを用いる。ここで、Ｍはロボットの質量であり、加速度偏差Δａｘ、Δａｙ、Δａｚに比例ゲイン−Ｍを乗じることで外乱力Ｄｒｘ、Ｄｒｙ、Ｄｒｚを算出する。補正量算出部１３３の処理は、たとえばｘ方向について、
Ｄｒｘ＝Ｍｉ・ｘｄ（２）＋Ｃｉ・ｘｄ（１）＋Ｋｉ・ｘｄ（０）
の微分方程式を解いてｘｄを求めることに相当する。ここで、（２）は時間に関する２階微分を示し、（１）は時間に関する１階微分を示し、（０）はｘｄ自身を示す。なお、補正量ｘｄの符号の取り方によっては、
−Ｄｒｘ＝Ｍｉ・ｘｄ（２）＋Ｃｉ・ｘｄ（１）＋Ｋｉ・ｘｄ（０）
と表現する方が正しいこともある。 Here, the transfer function 1 / G (s) is composed of a delay element in which the coefficient of the second-order term is Mi, the coefficient of the first-order term is Ci, and the coefficient of the 0th-order term is Ki. Mi, Ci, and Ki are desired parameters and are designated in advance. s is a Laplace converter, the gain of the transfer function is 1 / Ki, and -M is used as the proportional gain. Here, M is the mass of the robot, and disturbance forces Drx, Dry, Drz are calculated by multiplying the acceleration deviations Δax, Δay, Δaz by a proportional gain −M. For example, the correction amount calculation unit 133 performs processing in the x direction.
Drx = Mi · xd (2) + Ci · xd (1) + Ki · xd (0)
Is equivalent to finding xd by solving the differential equation. Here, (2) indicates the second derivative with respect to time, (1) indicates the first derivative with respect to time, and (0) indicates xd itself. Depending on how to sign the correction amount xd,
−Drx = Mi · xd (2) + Ci · xd (1) + Ki · xd (0)
Sometimes it is more correct to say.

なお、これらの係数、Ｍｉ、Ｃｉ、Ｋｉのうち、０次の係数Ｋｉは不可欠であるが、１次の遅れ要素の係数Ｃｉ及び２次の遅れ要素の係数Ｍｉの一方又は双方をゼロとしてもよい。 Of these coefficients, Mi, Ci, and Ki, the zeroth order coefficient Ki is indispensable, but one or both of the first order delay element coefficient Ci and the second order delay element coefficient Mi are set to zero. Good.

この伝達関数は、ばね−質点−ダンパ系を記述する伝達関数であり、補正後の体幹歩容データに従って動作するロボットの体幹は、補正後の体幹歩容データが表現する目標体幹運動の経時変化に従って緩やかに収束する。ロボットには、関節等にガタがあったり、制御に応答遅れがあったり、外界から外力が加わると、実際の体幹加速度が目標とする体幹加速度にならず、予期せぬ外乱力によって加速度偏差Δａｘ、Δａｙ、Δａｚが生じる。補正量生成部１３３は、この加速度偏差とロボット（体幹部）の質量から、外乱力を推定し、この外乱力と、「ばね−質点−ダンパ系」を記述する伝達関数に基づいて、外乱力を補償するための体幹運動に関する補正量を求めることができる。なお、パラメータＭｉ、Ｃｉ、Ｋｉは、ロボットに発揮させる復元力の大きさに影響するが、「ばね−質点−ダンパ系」を記述するものであればよいので、適宜設定すればよい。 This transfer function is a transfer function describing the spring-mass point-damper system, and the trunk of the robot that operates according to the corrected trunk gait data is the target trunk expressed by the corrected trunk gait data. It converges slowly according to the change of movement over time. If the robot has looseness in the joints, response delay in control, or external force applied from the outside world, the actual trunk acceleration will not become the target trunk acceleration, and acceleration due to unexpected disturbance force Deviations Δax, Δay, Δaz occur. The correction amount generation unit 133 estimates the disturbance force from the acceleration deviation and the mass of the robot (trunk), and based on the disturbance force and the transfer function describing the “spring-mass point-damper system”, the disturbance force It is possible to obtain a correction amount related to trunk movement for compensating for the above. The parameters Mi, Ci, and Ki affect the magnitude of the restoring force exerted on the robot, but may be set as appropriate as long as they describe a “spring-mass point-damper system”.

目標体幹位置姿勢補正部１３４は、歩容データ記憶部１１１に記憶されている補正前の目標体幹位置ｘｏ、ｙｏ、ｚｏに、補正量ｘｄ、ｙｄ、ｚｄを加算して、補正後の目標体幹位置ｘｒｅｆ、ｙｒｅｆ、ｚｒｅｆを算出し、関節角変換部１１３へ指示する。 The target trunk position / posture correction unit 134 adds the correction amounts xd, yd, zd to the target trunk positions xo, yo, zo before correction stored in the gait data storage unit 111, and then corrects them. Target trunk positions xref, yref, and zref are calculated and instructed to the joint angle conversion unit 113.

次に、補正量変更部１１６について説明する。補正量変更部は、目標重心位置計算部、補正値調整部１２２、実重心位置計算部１２３を有する。補正量変更部１１６は、加速度偏差のフィードバック制御に用いる補正値を重心位置の偏差に応じて調整するための調整係数を生成する。 Next, the correction amount changing unit 116 will be described. The correction amount changing unit includes a target centroid position calculating unit, a correction value adjusting unit 122, and an actual centroid position calculating unit 123. The correction amount changing unit 116 generates an adjustment coefficient for adjusting a correction value used for acceleration deviation feedback control according to the deviation of the gravity center position.

目標重心位置計算部１２１、両足位置姿勢目標値及び体幹位置姿勢目標値から目標重心位置を算出する。なお、歩容データを生成する際に、この目標重心位置を計算しておいてもよい。その場合は、目標重心位置計算部は不要である。 A target center-of-gravity position is calculated from the target center-of-gravity position calculation unit 121, both foot position / posture target value and trunk position / posture target value. Note that the target barycentric position may be calculated when generating gait data. In that case, the target center-of-gravity position calculation unit is unnecessary.

実重心位置計算部１２３は、姿勢センサ及び関節エンコーダの値に基づき、現在の重心位置を算出する。ここで、本実施の形態においては、現在の重心位置を算出するものとして説明するが、重心位置ではなく、胴体位置を使用することも可能である。この場合、目標重心位置ではなく、目標胴体位置を算出しておく。ロボットの質量のうち、大半がロボットの体幹に集中している。そこで、重心位置偏差の代わりに胴体位置偏差を使用してもよい。これによって重心位置を求める演算を省略することができ、計算時間を削減して処理スピードを向上することができる。または、同様の理由から重心位置偏差の代わりに胴体姿勢偏差を使用してもよい。 The actual center-of-gravity position calculation unit 123 calculates the current center-of-gravity position based on the values of the posture sensor and the joint encoder. Here, in the present embodiment, description is made assuming that the current center-of-gravity position is calculated, but it is also possible to use the body position instead of the center-of-gravity position. In this case, the target body position is calculated instead of the target center of gravity position. Most of the mass of the robot is concentrated on the trunk of the robot. Therefore, the trunk position deviation may be used instead of the gravity center position deviation. As a result, the calculation for obtaining the center of gravity position can be omitted, the calculation time can be reduced, and the processing speed can be improved. Alternatively, the body posture deviation may be used instead of the center-of-gravity position deviation for the same reason.

補正値調整部１２２は、目標重心位置と、現在の重心位置との偏差である重心位置偏差を求める。そして、重心位置偏差と加速度偏差とが同符号である場合のみ、重心位置偏差の値に応じて、補正量生成部１３３の補正量を調整する調整係数ｋを求める。加速度偏差と重心位置偏差の値の符号が同符号であると、ロボットは凹部に着地したものと判断でき、重心位置偏差が大きくなりすぎないよう補正量を調整する必要がある。ここで、本実施の形態においては、補正係数生成部１２２は、補正値生成部１３３の補正値を変更・調整するものとして説明するが、補正量を調整ができればよく、例えば加速度偏差Δａｘ、Δａｙ、Δａｚを調整・変更してもよい。 The correction value adjustment unit 122 obtains a gravity center position deviation that is a deviation between the target gravity center position and the current gravity center position. Then, only when the gravity center position deviation and the acceleration deviation have the same sign, an adjustment coefficient k for adjusting the correction amount of the correction amount generation unit 133 is obtained according to the value of the gravity center position deviation. If the signs of the values of the acceleration deviation and the centroid position deviation are the same, it can be determined that the robot has landed in the recess, and it is necessary to adjust the correction amount so that the centroid position deviation does not become too large. Here, in the present embodiment, the correction coefficient generation unit 122 is described as changing / adjusting the correction value of the correction value generation unit 133, but it is only necessary to be able to adjust the correction amount. For example, acceleration deviations Δax, Δay , Δaz may be adjusted / changed.

次に、補正量変更部の動作、すなわち、実重心位置の計算方法、重心位置偏差の計算方法、調整係数の生成方法について説明する。図３は、目標歩容と重心位置及び実際の歩容と重心位置を示す図である。Σ_ｗ０は、基準座標系、Σ_ｆｄは、ロボットの脚部の目標座標系、Σ_ｂｄは、ロボットの体幹部の目標座標系、Σ_ｆｒは、ロボットの脚部の実際の座標系、Σ_ｂｒはロボットの体幹部の実際の座標系を示している。 Next, the operation of the correction amount changing unit, that is, the calculation method of the actual centroid position, the calculation method of the centroid position deviation, and the adjustment coefficient generation method will be described. FIG. 3 is a diagram illustrating a target gait and a gravity center position, and an actual gait and a gravity center position. Σ _w0 is the reference coordinate system, Σ _fd is the target coordinate system of the robot leg, Σ _bd is the target coordinate system of the robot trunk, Σ _fr is the actual coordinate system of the robot leg, and Σ _br Indicates the actual coordinate system of the trunk of the robot.

実重心位置の計算方法
ロボットの体幹部に備えられた姿勢センサから得られた姿勢角、各関節のサーボ目標値、ロボットのリンクの質量、長さなどを含むモデルから重心位置を算出する。ここで得られる重心位置は、目標の歩容座標系を生成する基準座標系Σ_ｗ０とは異なる座標系基準となっている。例えば、図３（ａ）に示すように、目標の歩容と重心位置は基準座標系Σ_ｗ０を基準にしており、また、実際の歩容と重心位置は、図３（ｂ）に示すように、任意の別の座標系Σ_ｗｒを基準に求められる。図３（ａ）、図３（ｂ）において、Σの添え字のｂは「体幹部」、ｆは「脚部」、ｄは「目標座標」、ｒは「実際の座標」、ｗは、基準座標系Σ_ｗ０を基準とした座標系であることを示す。 Calculation method of the actual center of gravity position The center of gravity position is calculated from a model including the posture angle obtained from the posture sensor provided in the trunk of the robot, the servo target value of each joint, the mass of the link of the robot, the length, and the like. The position of the center of gravity obtained here is a coordinate system reference different from the reference coordinate system _Σw0 for generating the target gait coordinate system. For example, as shown in FIG. 3A, the target gait and the position of the center of gravity are based on the reference coordinate system _Σw0 , and the actual gait and the position of the center of gravity are as shown in FIG. In addition, any other coordinate system Σ _wr is obtained as a reference. 3 (a) and 3 (b), the subscript b of Σ is “trunk”, f is “leg”, d is “target coordinates”, r is “actual coordinates”, w is The coordinate system is based on the reference coordinate system _Σw0 .

重心位置偏差の計算
ロボットの体幹に固定された姿勢センサから得られる情報から胴体の回転行列^ｗ０Ｒ_ｂｒが得られる。これを利用して、ロボットの支持脚足先位置からみた重心位置を基準座標系で求める。先ず、ロボットの目標支持脚足先位置からみた目標重心位置座標は、
^ｗ０Ｐ_ｇｄ−^ｗ０Ｐ_ｆｄ
^ｗ０Ｐ_ｇｄ：基準座標系Σ_ｗ０からみた目標重心位置座標
_ｗ０Ｐ^ｆｄ：基準座標系Σ_ｗ０からみた目標支持脚足先位置座標
次に、ロボットの実際（座標系Σ_ｗｒ）の支持脚足先位置座標からみた重心位置座標は、^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒであり、座標系Σ_ｗｒを座標系Σ_ｂｒに変換する回転行列^ｗｒＲ_ｂｒ、座標系Σ_ｗｒを座標系Σ_ｗ０に変換する回転行列^ｗ０Ｒ_ｂｒを使用し、基準座標系に変換した実際の重心位置座標を現すと、下記となる。
^ｗ０Ｒ_ｂｒ ^ｗｒＲ_ｂｒ ^Ｔ（^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒ）
^ｗｒＰ_ｇｒ：座標系Σ_ｗｒからみた実重心位置座標
^ｗｒＰ_ｆｒ：座標系Σ_ｗｒからみた実支持脚足先位置座標
^ｗｒＲ_ｂｒ：座標系Σ_ｗｒからみた実胴体姿勢
^ｗ０Ｒ_ｂｒ：姿勢センサから得られた実胴体姿勢 Calculation of center-of-gravity position deviation A torso rotation matrix ^w0 R _br is obtained from information obtained from a posture sensor fixed to the trunk of the robot. Using this, the position of the center of gravity as seen from the position of the support leg foot of the robot is obtained in the reference coordinate system. First, the target center-of-gravity position coordinates as seen from the target support leg toe position of the robot are:
^w0 P _gd - ^w0 P _fd
^w0 P _gd : target center-of-gravity position coordinates _viewed from the reference coordinate system _Σw0
_w0 P ^fd : Target support leg toe position coordinate viewed from the reference coordinate system Σ _w0 Next, the center of gravity position coordinate viewed from the actual support leg toe position coordinate of the robot (coordinate system Σ _wr ) is ^wr P _gr ^−wr P is _fr, actually converted coordinate system sigma _wr rotation matrix ^wr R _br be converted into the coordinate system sigma _br, using a rotating matrix ^w0 R _br converting the coordinate system sigma _wr to the coordinate system sigma _w0, the reference coordinate system The center-of-gravity position coordinates are expressed as follows.
^{_{^{_{^{w0 R br wr R br T (}}}}} wr P gr - wr P fr)
^wr P _gr : Actual center-of-gravity position coordinates _viewed from the coordinate system Σ _wr
^wr P _fr : Actual support leg toe position coordinate viewed from coordinate system Σ _wr
^wr R _br : real torso posture _viewed from coordinate system Σ _wr
^w0 R _br : real torso posture obtained from posture sensor

ここで添え字のｆは「脚部」、ｇは「重心位置」、ｗは、「基準座標系Σ_ｗ０を基準としたベクトルの要素」、ｒは「実際の座標」を示している。以上の結果から、ロボットの支持脚からみた重心位置偏差^ｗ０ΔＰ_ｇ−ｆは、基準座標系Σ_ｗ０を基準とすると下記となる。
^ｗ０ΔＰ_ｇ−ｆ＝^ｗ０Ｒ_ｂｒ ^ｗｒＲ_ｂｒ ^Ｔ（^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒ）−（^ｗ０Ｐ_ｇｄ−^ｗ０Ｐ_ｆｄ） Here, the subscript f is “leg”, g is “center of gravity”, w is “vector element based on the reference coordinate system _Σw0 ”, and r is “actual coordinates”. From the above results, the center-of-gravity position deviation ^w0 ΔP _g−f viewed from the support leg of the robot is as follows with reference to the reference coordinate system _Σw0 .
^{_{^{_{^{w0 ΔP g-f = w0 R}}}}} br wr R br T (wr P gr - wr P fr) - (w0 P gd - w0 P fd)

重心位置偏差に応じたゲイン変更
重心位置偏差と、補正量生成部１３３の補正量の符号を比較し、符号が同じ場合には、補正量が小さくなるような調整係数ｋを乗算する。これを式であらわすと下記となる。 Gain change according to barycentric position deviation The barycentric position deviation is compared with the sign of the correction amount of the correction amount generating unit 133. If the signs are the same, the adjustment coefficient k is multiplied so that the correction amount becomes small. This is expressed by the following formula.

このとき、調整係数は、図４のように表すことができる。図４は、ロボットの胴体位置補正及び加速度偏差の向きを示す図である。図４は、調整係数ｋと重心位置の偏差の大きさとの関係を示す図である。加速度偏差の符号（補正量の符号）と重心位置偏差の符号が同じ場合には、ロボットが凹部に着地したと判断して胴体加速度フィードバック制御の補正量を図４に示す変調係数で調整する。 At this time, the adjustment coefficient can be expressed as shown in FIG. FIG. 4 is a diagram illustrating the direction of the body position correction and acceleration deviation of the robot. FIG. 4 is a diagram illustrating the relationship between the adjustment coefficient k and the magnitude of the deviation of the center of gravity position. When the sign of the acceleration deviation (sign of the correction amount) and the sign of the center of gravity position deviation are the same, it is determined that the robot has landed in the recess, and the correction amount of the trunk acceleration feedback control is adjusted by the modulation coefficient shown in FIG.

図４に示す例では、重心位置偏差がある閾値Δ１以下である場合は、調整係数ｋを最大値の１とする。重心位置偏差が閾値Δ１乃至閾値Δ２（Δ１＜Δ２）の間は、調整係数ｋは１から０への単調減少関数とし、閾値Δ２でｋ＝０とする。重心位置偏差が閾値Δ２以上の場合は、調整係数ｋ＝０とし、補正量を０とする。 In the example shown in FIG. 4, when the center-of-gravity position deviation is equal to or less than a certain threshold value Δ1, the adjustment coefficient k is set to 1 which is the maximum value. While the center-of-gravity position deviation is between threshold value Δ1 and threshold value Δ2 (Δ1 <Δ2), the adjustment coefficient k is a monotonically decreasing function from 1 to 0, and k = 0 at the threshold value Δ2. When the gravity center position deviation is greater than or equal to the threshold Δ2, the adjustment coefficient k = 0 and the correction amount is set to zero.

これにより、上述の図６に示したように、凹部に着地したと判断できる場合は、補正量が小さくなるように補正することで、重心位置が大きく外れてしまうことを防止する。そして、凸部に着地したと判断できる場合は、胴体の加速度フィードバックによる補正量はそのままとする。これにより、十分に外力を吸収し、足裏の路面に対する接地性を確保する。本実施の形態においては、凸部と凹部を判定するため、重心位置偏差と加速度偏差の符号を比較するのである。 As a result, as shown in FIG. 6 described above, when it can be determined that the ground has landed in the recess, the center of gravity position is prevented from greatly deviating by correcting the correction amount to be small. And when it can be judged that it has landed on a convex part, the correction amount by the acceleration feedback of a fuselage is left as it is. Thereby, external force is fully absorbed and the grounding property with respect to the road surface of a sole is ensured. In the present embodiment, the signs of the gravity center position deviation and the acceleration deviation are compared in order to determine the convex part and the concave part.

図６は、本実施の形態における補正量変更部の動作を示すフローチャートである。図５に示すように、先ず、実重心位置計算部１２３が胴体の実姿勢角と各関節サーモ目標値から、ロボットの実重心位置を算出する(ステップＳ１)。次に、調整係数生成部１２２は、ロボットの重心位置偏差を求める(ステップＳ２)。そして、調整係数生成部１２２は、ロボットの重心位置偏差と、加速度フィードバック制御に使用する体幹加速度偏差の符号を比較する(ステップＳ３)。符号が同じ、すなわち、体幹の補正方向と重心位置偏差の方向が同じであれば（ステップＳ４：Ｙｅｓ）、重心位置偏差に応じて調整係数を生成する (ステップＳ５)。符号が異なる場合は、調整係数は出力せず、そのままの補正量で体幹位置姿勢目標値の補正を行わせる。 FIG. 6 is a flowchart showing the operation of the correction amount changing unit in the present embodiment. As shown in FIG. 5, first, the actual center-of-gravity position calculation unit 123 calculates the actual center-of-gravity position of the robot from the actual posture angle of the body and each joint thermo target value (step S1). Next, the adjustment coefficient generation unit 122 obtains the center-of-gravity position deviation of the robot (step S2). Then, the adjustment coefficient generation unit 122 compares the center-of-gravity position deviation of the robot with the sign of the trunk acceleration deviation used for acceleration feedback control (step S3). If the signs are the same, that is, if the trunk correction direction and the centroid position deviation direction are the same (step S4: Yes), an adjustment coefficient is generated according to the centroid position deviation (step S5). When the signs are different, the adjustment coefficient is not output, and the trunk position / posture target value is corrected with the correction amount as it is.

なお、本実施の形態においては、体幹位置姿勢目標値をフィードバック制御する場合について説明したが、両足位置姿勢目標値も同様にフィードバック制御してもよいことは勿論である。例えば、足裏距離センサから足平と接地面の距離を求める。より正確には足平に固定された座標原点Ｏｆと接地面の間の距離を求める。この距離は、接地面に対する実足平位置を表す。目標足平位置と実足平位置の偏差に基づいて目標足平位置の補正量を算出する。算出された補正量を目標足平位置に加算した値、すなわち目標足平位置の補正量で補正された目標足平位置を関節角変換部１１３へ入力する。そのような制御系によって、実足平位置を目標足平位置に一致させることができる。例えば、実際のロボットの遊脚が実際の接地面に接地するタイミングを、歩容データ上で遊脚が接地面に着地するタイミングに一致させることができる。 In the present embodiment, the case where the trunk position / posture target value is feedback-controlled has been described, but it is needless to say that the both foot position / posture target value may also be feedback-controlled in the same manner. For example, the distance between the foot and the contact surface is obtained from the sole distance sensor. More precisely, the distance between the coordinate origin Of fixed to the foot and the ground plane is obtained. This distance represents the actual foot position with respect to the ground plane. A correction amount for the desired foot position is calculated based on the deviation between the desired foot position and the actual foot position. A value obtained by adding the calculated correction amount to the target foot position, that is, the target foot position corrected by the target foot position correction amount is input to the joint angle conversion unit 113. With such a control system, the actual foot position can be matched with the target foot position. For example, the timing at which the free leg of the actual robot touches the actual ground plane can be matched with the timing at which the free leg lands on the ground plane on the gait data.

本実施の形態においては、インピーダンス制御において、遊脚着地時に大きな外乱が生じると、胴体に加速度が発生し、その結果胴体位置が補正される。しかし、遊脚着地位置が凹部の場合には、その補正によって、胴体が大きく外側に補正されることになり、結果として衝撃は吸収されるが重心位置がより大きくずれてしまい、その重心位置を回復させることができなくなる。そこで、重心位置偏差と加速度偏差を利用し、凹部に着地した場合を判定する。上述のように重心位置偏差と加速度偏差の符号が同一であれば、凹部に着地したと判定でき、この場合には、加速度偏差のフィードバック制御に用いる補正量を小さくしすることで、体幹位置姿勢目標値の補正量を小さくする。このことにより、凹部に着地した場合に補正量が大きくなって重心位置が外側に大きくなり回復できなくなるようなことを防止する。凹部に着地したと想定でき、補正量を小さくする。 In the present embodiment, in the impedance control, if a large disturbance occurs when landing on the free leg, acceleration occurs in the trunk, and as a result, the trunk position is corrected. However, when the free leg landing position is a concave portion, the torso is greatly corrected by the correction, and as a result, the impact is absorbed but the center of gravity position is further shifted, and the center of gravity position is changed. It cannot be recovered. Therefore, the case where the robot has landed on the recess is determined using the gravity center position deviation and the acceleration deviation. If the signs of the gravity center position deviation and the acceleration deviation are the same as described above, it can be determined that the vehicle has landed in the recess. In this case, the trunk position can be reduced by reducing the correction amount used for feedback control of the acceleration deviation. Decrease the correction amount of the posture target value. This prevents the amount of correction from increasing when landing on the recess and the position of the center of gravity from increasing outside, making it impossible to recover. It can be assumed that it has landed in the recess, and the correction amount is reduced.

これに対し、凸部に着地した場合、もともとの重心位置が目標に対して遅れているので、補正しても問題にならない。よって、凸部に着地した場合は、補正量をそのままとし、十分に外力を吸収し、足裏の路面に対する接地性を確保させる。 On the other hand, when landing on the convex part, the original position of the center of gravity is delayed with respect to the target. Therefore, when landing on the convex portion, the correction amount is left as it is, the external force is sufficiently absorbed, and the grounding property to the road surface of the sole is ensured.

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。 It should be noted that the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention. For example, in the above-described embodiment, the hardware configuration has been described. However, the present invention is not limited to this, and arbitrary processing may be realized by causing a CPU (Central Processing Unit) to execute a computer program. Is possible. In this case, the computer program can be provided by being recorded on a recording medium, or can be provided by being transmitted via the Internet or another transmission medium.

本発明の実施の形態にかかるロボットを正面から見た様子を概略的に表す概略図であり、It is a schematic diagram schematically showing a state of the robot according to an embodiment of the present invention when viewed from the front, 本発明の実施の形態にかかるロボットの制御システムの機能を示すブロック図である。It is a block diagram which shows the function of the control system of the robot concerning embodiment of this invention. 目標歩容と重心位置及び実際の歩容と重心位置を示す図である。It is a figure which shows a target gait and a gravity center position, and an actual gait and a gravity center position. ロボットの胴体位置補正及び加速度偏差の向きを示す図である。It is a figure which shows the direction of the body position correction | amendment and acceleration deviation of a robot. 重心位置偏差と調整係数の関係を示すグラフ図である。It is a graph which shows the relationship between a gravity center position deviation and an adjustment coefficient. 本発明の実施の形態における調整係数生成部の動作を示すフローチャートである。It is a flowchart which shows operation | movement of the adjustment coefficient production | generation part in embodiment of this invention. 特許文献２に記載のロボットの制御システムを示す図である。It is a figure which shows the control system of the robot of patent document 2. FIG.

Explanation of symbols

１ロボット、２頭部、３体幹、
４腰部、５右腕、６左腕、
１０脚部、１１載置部、１７体幹加速度センサ、
２０右脚、２１右股関節、２２右上腿、
２３右膝関節、２４右下腿、２５右足首関節、
２６右足平、３０左脚、３１左股関節、
３２左上腿、３２加速度偏差算出部、３３左膝関節、
３４左下腿、３５左足首関節、３６左足平、
１１１歩容データ記憶部、１１２体幹歩容データ補正演算部、
１１３関節角変換部、１１４各軸制御器、
１１５体幹加速度計算部、１１６補正量変更部、１１７センサ、
１２１目標重心位置計算部、１２２調整係数生成部、
１２３実重心位置計算部、１３１階微分演算部、
１３２加速度偏差算出部、１３３補正量算出部、１３４目標体幹位置姿勢補正部 1 robot, 2 heads, 3 trunks,
4 waist, 5 right arm, 6 left arm,
10 legs, 11 placement parts, 17 trunk acceleration sensor,
20 right leg, 21 right hip joint, 22 right upper leg,
23 right knee joint, 24 right lower leg, 25 right ankle joint,
26 right foot, 30 left leg, 31 left hip joint,
32 left upper thigh, 32 acceleration deviation calculator, 33 left knee joint,
34 left lower leg, 35 left ankle joint, 36 left foot,
111 gait data storage unit, 112 trunk gait data correction calculation unit,
113 joint angle conversion unit, 114 each axis controller,
115 trunk acceleration calculation unit, 116 correction amount change unit, 117 sensor,
121 target center-of-gravity position calculation unit, 122 adjustment coefficient generation unit,
123 actual center-of-gravity position calculation unit, 131st-order differential calculation unit,
132 Acceleration deviation calculation unit, 133 Correction amount calculation unit, 134 Target trunk position / posture correction unit

Claims

In a legged mobile robot,
Acceleration deviation calculating means for calculating a first deviation which is a deviation between a target trunk acceleration and a current trunk acceleration;
Centroid position deviation calculating means for calculating a second deviation which is a deviation between the target centroid position and the current centroid position;
A legged mobile robot having correction means for correcting a gait data target value based on the first deviation and the second deviation.

The correction means includes a trunk gait data correction calculation means for correcting the gait data target value based on the first deviation, and the trunk gait data correction calculation means based on the first deviation and the second deviation. The legged mobile robot according to claim 1, further comprising: a correction amount changing unit that changes the degree of correction.

The trunk gait data correction calculation means includes a correction amount generation means for generating a correction amount from the first deviation, and a target value correction for correcting the gait data target value with the correction amount generated by the correction amount generation means. The legged mobile robot according to claim 2, further comprising: means.

4. The correction amount changing means generates an adjustment coefficient for adjusting the correction amount of the gait data target value only when the first deviation and the second deviation have the same sign. Legged mobile robot.

The correction amount generating means generates a correction amount by multiplying the first deviation by a predetermined proportional gain and transfer function, and an adjustment coefficient generated by the correction amount changing means,
The legged mobile robot according to claim 3, wherein the correction amount changing unit changes the adjustment coefficient so that the correction amount decreases as the second deviation increases.

The correction amount changing means sets the adjustment coefficient to 1 when the second deviation is smaller than a first threshold, and uses a monotonically decreasing function from 1 to 0 between the first threshold and the second threshold, The legged mobile robot according to claim 5, wherein the adjustment coefficient is set to zero when the second threshold value is exceeded.

7. A body position deviation calculating means for calculating a third deviation, which is a deviation between the target body position and the current body position, is provided instead of the center-of-gravity position deviation calculating means. The legged mobile robot according to item 1.

7. A body posture position deviation calculating means for calculating a fourth deviation which is a deviation between the target body posture and the current body posture is provided in place of the center-of-gravity position deviation calculating means. A legged mobile robot according to claim 1.

The legged mobile robot according to any one of claims 1 to 8, wherein the gait data target values are a trunk position / posture target value and a toe position / posture target value.

A control method for a legged mobile robot,
An acceleration deviation calculating step of calculating a first deviation which is a deviation between the target trunk acceleration and the current trunk acceleration;
A centroid position deviation calculating step of calculating a second deviation which is a deviation between the target centroid position and the current centroid position;
And a correction step of correcting a gait data target value based on the first deviation and the second deviation.

The correcting step includes a trunk gait data correction calculation step for correcting the gait data target value based on the first deviation, and the trunk gait data correction calculation step based on the first deviation and the second deviation. The method for controlling a legged mobile robot according to claim 10, further comprising: a correction value changing step of changing a degree of correction at the point.

The trunk gait data correction calculation step includes a correction amount generation step for generating a correction amount from the first deviation, and a target value correction for correcting the gait data target value with the correction amount generated by the correction amount generation means. The method for controlling a legged mobile robot according to claim 11, further comprising: a step.

The legged movement according to claim 11 or 12, wherein, in the correction value changing step, the degree of correction of the gait data target value is changed only when the first deviation and the second deviation have the same sign. Robot control method.

In the correction amount generation step, a correction amount is generated by multiplying the first deviation by a predetermined proportional gain and transfer function, and an adjustment coefficient in the correction value changing step,
The method of controlling a legged mobile robot according to claim 13, wherein the correction amount changing step changes the adjustment coefficient so that the correction amount decreases as the second deviation increases.

15. A body position deviation calculating step for calculating a third deviation, which is a deviation between the target body position and the current body position, is provided instead of the center-of-gravity position deviation calculating step. A method for controlling a legged mobile robot according to claim 1.

15. The body posture position deviation calculating step for calculating a fourth deviation which is a deviation between the target body posture and the current body posture is provided instead of the gravity center position deviation calculating step. A control method for a legged mobile robot according to claim 1.