JP5099733B2 - Control device for legged mobile robot - Google Patents

Description

The present invention relates to an apparatus for generating a motion of a legged mobile biped walking robot used for a home robot, various service robots, and the like, and for performing feedback control of the motion.

  Conventionally, the movement of a legged mobile robot has been generated based on the ZMP (Zero-Moment Point), which is the center of action of the reaction force that the robot receives from the floor (Takanishi, Ishida, Yamazaki, Kato: The Robotics Society of Japan Vol.3, no.4, pp.325-336, 1985, JP 2002-326173). Here, since ZMP is an action center point of the floor reaction force, it may be called a floor reaction force center point.

 When a legged mobile robot walks on a horizontal floor, the robot is guaranteed to continue walking without falling as long as the ZMP is contained within the support polygon formed by the contact point between the sole and the floor. Is done. On the other hand, when the ZMP comes to the end of the support polygon, the contact between the sole of the robot and the floor surface is separated, which may cause the robot to fall.

  However, as described above, since ZMP is a center point of action of floor reaction force, it cannot be determined whether or not the sole and the floor surface slip even if ZMP is used. Further, when the hand of the legged mobile robot comes into contact with the environment, an internal force is generated between the contact points, but the influence of the internal force does not appear at the ZMP position. That is, in this case as well, using ZMP, it is impossible to determine the transition of the contact state such as slippage or separation at the contact point between the robot and the environment.

  In the specification of Japanese Patent No. 3132156, a motion in which a legged mobile robot climbs stairs is generated based on ZMP. However, as described above, when ZMP is used, when the contact point is not on a single surface or when slip occurs at the contact point, the transition of the contact state cannot be determined. This is because the robot receives a three-dimensional force and a three-dimensional moment from the floor, but ZMP uses only two components of the moment parallel to the floor surface.

JP 2002-326173 A Japanese Patent No. 3132156

A first object of the present invention is to consider a three-dimensional force and a three-dimensional moment applied to the robot in a legged mobile robot, and these three-dimensional force and three-dimensional moment are formed in a force-moment space. An object of the present invention is to provide a control device for a legged mobile robot having a function of determining a transition of a contact state using a contact force convex polyhedral cone.

Furthermore, a second object of the present invention is to consider a three-dimensional force and a three-dimensional moment applied to the robot in a legged mobile robot, and these three-dimensional force and three-dimensional moment are force-moment space. It is an object of the present invention to provide an apparatus for generating a motion of a legged mobile robot in which a contact state does not change using a contact force convex polyhedral cone formed in the above.

A third object of the present invention is a legged mobile robot, taking into account the three-dimensional force and three-dimensional moment applied to the robot, the operation control of a legged mobile robot which feedback controls the posture of the robot in real time To provide an apparatus.

In order to achieve the first object described above, in a control device for a legged mobile robot comprising at least an upper body and a plurality of legs connected to the upper body , an inertial force is applied when the robot operates. and obtains a three-dimensional force robot receives (f _G) and three-dimensional moment (tau _G) by gravity, each contact point where the robot is in contact with the environment in (1 · · · · K), the robot In order to approximate a friction cone, which is a set of three-dimensional forces that can be generated, as an L pyramid, for each side (1... L) in the L pyramid, a unit that becomes each side of the L pyramid. and vector, the product of the non-negative scalar defining the size of those said side edges force, defines the possible occurrence force each side direction, these sums can be generated with respect to the robot at each contact point as well as a set of three-dimensional forces, At the point of contact, said defining the outer product of the L pyramid of the position vector of each side direction can be generated force and the contact point, three-dimensional moment that can be generated with respect to the robot in these sums each contact point A set of the three-dimensional force set at each contact point and the sum of the three-dimensional moment set for all contact points (1... K), The contact force is defined as a convex polyhedral cone that is a set of moments, and the force (f _G ) and moment (τ _G ) that the robot receives by inertial force and gravity when the robot moves in a three-dimensional force and moment space. reverse forces and moments (-f _G, -τ _G) to cancel the can, if not included in the contact force PCC, the contact state in which the contact leaves at least part of the contact points Qian It was set to comprise a means for determining.

Furthermore, in order to achieve the second object, the three-dimensional force (f _G ) and the three-dimensional moment (τ _G ) received by the robot due to the inertial force and gravity corresponding to the generated movement of the robot. The reverse force and moment (−f _G , −τ _G ) that cancels the three-dimensional force (f _G ) and the three-dimensional moment (τ _G ) are included in the contact force convex polyhedral cone. A means for generating a robot motion is provided.

A target three-dimensional force and a three-dimensional moment are calculated, a contact force convex polyhedral cone formed in a force-moment space corresponding to a given robot posture is calculated, and the calculated 3 When the target value of the three-dimensional force and the three-dimensional moment is not included in the contact force convex polyhedral cone , the robot has a means for correcting the target value of the three-dimensional force and the three-dimensional moment so that the contact between the robot and the environment It is possible to generate a motion of a stable legged mobile robot that has no contact state transition , such as leaving.

Further, in order to achieve the above third object, when the robot operates, the three-dimensional force (f _G ) and the three-dimensional moment (τ _G ) that the robot receives due to inertial force and gravity are applied to the current robot. Calculated based on the joint angle information and position / orientation information, and in the walking on the plane, the three-dimensional force (f _G ) and the three-dimensional moment (τ _G ) calculated in real time are reversed and Means for feedback controlling the posture of the robot is provided so that a moment (-f _G , -τ _G ) is included in the contact force convex polyhedral cone.

Considering 3D force and 3D moment, it has a means for feedback control of posture in real time, so stable motion control of legged mobile robot can be realized in real time without contact state transition .

Since the present invention is configured as described above, Te legged mobile robot odor, can determine the transition of the state of contact between the robot and the environment, and the operation of the robot not to transition the contact state can be easily obtained, the robot It becomes possible to perform feedback control of the attitude in real time.

FIG. 2 is a diagram showing a legged mobile robot standing on the floor and touching the environment with a hand. It is a figure showing the contact of the sole of a legged mobile robot and a floor surface. It is a figure which shows the sole support area | region and the area | region of a force and a moment in the walk on the flat floor surface with sufficient friction. It is a figure which shows the area | region of force in the walk on the floor surface which is slippery. It is a figure which shows the range of the moment of the surroundings of a vertical axis in the walk on a slippery floor surface. It is a figure where a legged mobile robot walks while pushing an object in a two-dimensional plane. It is a figure which shows the orthogonal projection to the convex polyhedron of the force and moment which are not contained in a contact force convex polyhedron. It is a figure which shows operation | movement production | generation of the legged mobile robot using a contact force convex polyhedral cone. It is a figure of the real-time control system which considered force and moment.

The invention Te legged mobile robot odor, robot and can determine the transition state of contact with the environment, and can be the operation of the robot not to transition the contact state obtained easily, feedback control of the posture of the robot in real time order to be able to, calculating at least a body, in the control system of a legged mobile robot comprising a plurality of legs coupled to the body, a three-dimensional force and three-dimensional moment applied to the robot by, Te contacts smell between the sole and the floor surface at walking on a plane, it was achieved by providing a means for determining a transition of contact away contact in some points of contact even without low.

  Hereinafter, a control apparatus for a legged mobile robot according to an embodiment of the present invention will be described with reference to the accompanying drawings. An example of a legged mobile robot is a biped walking robot.

First, as a first embodiment of the present invention, when a legged mobile robot performs an action such as walking, a means for calculating a three-dimensional force and a three-dimensional moment received by the robot from the environment, a given robot posture The means for calculating the contact force convex polyhedral cone formed in the force-moment space corresponding to is shown.

のように定義されるとする。さらに、ｚ０はロボットが水平面上を歩行する場合の床面の高さを表すものとする。ｆ_Ｈｉ（＝［（ｆ_Ｈｉ）ｘ（ｆ_Ｈｉ）ｙ（ｆ_Ｈｉ）ｚ］^Ｔ）、τ_Ｈｊ（ｊ＝１，２）は各手先が受ける力・モーメント、ｆ_Ｆｊ，τ_Ｆｊ（ｊ＝１，２）は各足裏が受ける力・モーメントである。同様に、ｆ_ｋは点ｐ_ｋで受ける力とする。なお、図ではモーメントを太矢印で表している。ｎ_ｋは、点ｐ_ｋにおける単位拘束法線ベクトルを表す。Ｉ_ｉ、ω_ｉはそれぞれ第iリンクの基準座標系に関する慣性テンソル、角速度ベクトルを表す。以上述べた位置ベクトル、力、モーメントは、全て基準座標系Σ_Ｒで表すものとする。 FIG. 1 is a schematic diagram generally showing a legged mobile robot, more specifically a biped walking robot, to which the control device according to this embodiment is applied. ΣR is a reference coordinate system, ΣB is a coordinate system fixed to the robot waist, and ΣLi is a coordinate system fixed to the center of gravity of the i-th link (i = 1,..., N). For the position vector related to ΣR, P _Hj (= [x _Hj y _Hj z _Hj ] ^T (j = 1, 2) is each hand, p _Fj (= [x _Fj y _Fj z _Fj ] ^T ) (j = 1, 2) ) Is a fixed point on the link of each sole, p _Li (= [x _Li y _Li z _Li ] ^T ) is the origin of ΣLi, p _B (= [x _B y _B z _B ] ^T ) is the origin of ΣB In addition, it is assumed that p _k (k = 1,..., K) is used as the apex of the polygon that forms the contact area of each hand and the support area of each sole, and pG _{(= [x G y G z} G] T) is a position vector representing the center of gravity of the robot,

Is defined as follows. Further, z0 represents the height of the floor surface when the robot walks on a horizontal plane. f _Hi (= [(f _Hi ) x (f _Hi ) y (f _Hi ) z] ^T ), τ _Hj (j = 1, 2) is the force / moment received by each hand, f _Fj , τ _Fj (j = 1, 2) are the force and moment that each sole receives. Similarly, _{f k} is the force applied at the point _{p k.} In the figure, moments are indicated by thick arrows. n _k represents a unit constraint normal vector at the point p _k . I _i and ω _i represent an inertia tensor and an angular velocity vector related to the reference coordinate system of the i-th link, respectively. Above mentioned position vectors, forces, moments, is intended to refer in all the reference coordinate system sigma _R.

ここで、

はロボットの質量、ｇ＝［０ ０ −ｇ］Ｔは重力ベクトルを表す。また、

は重心まわりの角運動量を表す。 When the robot is operated, inertial and those representing the forces robot receives the reference coordinate system sigma _R by gravity f _G, the moment about the reference coordinate system in which the robot is subjected to tau _G. The force / moment applied to the robot is given by equations (1) and (2).

here,

Is the mass of the robot, and g = [0 0 -g] T is the gravity vector. Also,

Represents the angular momentum around the center of gravity.

さらに、各接触点がロボットに対して発生可能な力の集合は、摩擦コーンをL角錐で近似した場合、数式（５）により与えられる。

ここに、μ_ｋは各点の摩擦係数、

が各点の摩擦コーンを近似するL角錐の側辺となる様な単位ベクトルで、

は各点に加わる接線方向の力の大きさにより定まる非負のスカラである。数式（５）を数式（３）、数式（４）に代入すると、（ｆ_Ｃ，τ_Ｃ）がとり得る値の集合は、数式（６）、数式（７）により得られる。

（ｆ_Ｃ，τ_Ｃ）はベクトルの非負１次結合で表されていることから、これらが力・モーメントの空間でとり得る値の集合は接触力凸多面錐になる。 When a force f _G and moment τ _G are applied to the robot, the robot receives a reaction force / moment from the environment in contact with the robot. When the reaction force received from the environment in contact with the robot is expressed in the reference coordinate system as f _C , and the moment around the reference coordinate system received by the robot as τ _C , these are the contact surfaces of the hand, sole, and environment. Equations (3) and (4) are obtained from the geometrical shape.

Furthermore, the set of forces that each contact point can generate on the robot is given by Equation (5) when the friction cone is approximated by an L pyramid.

Where μ _k is the coefficient of friction at each point,

Is a unit vector that is the side of an L pyramid that approximates the friction cone of each point,

Is a non-negative scalar determined by the magnitude of the tangential force applied to each point. Equation (5) Equation (3), are substituted into Equation (4), a set of values that can take _{(f C,} tau _C) is Equation (6) is obtained by Equation (7).

(F _C, τ _C) from that represented by nonnegative linear combination of vectors, the set of these can take the space of a force and moment values ing the contact force convex polyhedral cone.

When force f _G and moment τ _G are applied to the robot, if (−f _G , −τ _G ) is not included in the contact force convex polyhedral cone represented by Equation (6) and Equation (7) , This relationship is not established, and the contact state always changes.

ここで、

は任意の非負数である。一方、鉛直方向の力と水平方向のモーメントは、数式（１１）、数式（１２）、数式（１３）で表されるが、これらは常に釣り合いが成立するとは限らない。

なお、本実施例における数式（８）〜（１３）は、実施例１において行った（−ｆＧ，−τＧ）が数式（６）と数式（７）によって表される接触力凸多面錐に含まれるか否かの評価を、水平面方向の力と鉛直軸周りのモーメントについては任意の反力が発生することを前提にして、力、モーメントの関係式で表したものである。
ここでは、より具体的には、数式（１１）、数式（１２）、数式（１３）が成立するためには、右辺に含まれる非負のスカラー

が存在しなくてはならない。逆に言うと、与えられたロボットの動作に対して、非負のスカラー

が存在するかどうかによって、接触状態が遷移するかどうかが判定される。 Next, as a second embodiment of the present invention, means for determining transition of the contact state when a legged mobile robot walks on a plane with sufficient friction is shown. The contact state transitions on a floor surface having sufficient friction is when the contact leaves at a certain contact point. Consider the case where only the sole of the robot is in contact with the horizontal plane as shown in FIG. Assuming that a sufficiently large friction acts between the robot foot and the horizontal plane, an arbitrary reaction force is generated for the force in the horizontal plane direction and the moment about the vertical axis, and the formulas (8), (9), The balance relationship of Formula (10) is always established.

here,

Is any non-negative number. On the other hand, the vertical force and the horizontal moment are expressed by Equation (11), Equation (12), and Equation (13), but these are not always balanced.

In addition, numerical formula (8)-(13) in a present Example is contained in the contact force convex polyhedral cone whose (-fG,-(tau) G) performed in Example 1 is represented by numerical formula (6) and numerical formula ( 7 ). The evaluation of whether or not the force is generated is expressed by a relational expression of force and moment on the premise that an arbitrary reaction force is generated for the force in the horizontal plane direction and the moment about the vertical axis.
Here, more specifically, in order for Formula (11), Formula (12), and Formula (13) to hold, a non-negative scalar included in the right side is used.

Must exist. Conversely, a non-negative scalar for a given robot motion

It is determined whether or not the contact state transitions depending on whether or not there exists.

FIG. 3 shows an appropriate contact area between the robot and the floor on a flat floor surface, and the contact force convex polyhedral cones expressed by the corresponding equations (11), (12), and (13) ( The cross section at the value of f _C ) z is shown.

次に、この接触力凸多面錐に加えて、内力を考える。ロボットの手が環境と接する場合、ロボットが持つ運動量や角運動量には影響を及ぼさない接触力の成分である内力が存在する。この場合、内力の関係式は数式（１５）のように表すことができる。

ここで、φは内力に関するパラメータである。数式（１４）は式が三つに対して、変数が

の四つである。しかしながら、数式（１４）に加えて数式（１５）を考えることで、式の数と変数の数が合い、変数を一意に求めることができる。このとき、これらの変数に負のものが含まれるならば、接触状態が遷移することを判定できる。つまり、手先が環境と接触する場合は、接触力凸多面錐に加えて内力の関係式を用いることで、接触状態の遷移を判定する。 As the third embodiment of the present invention, assuming the legged mobile robot having an arm, a case where the arm is in contact with the environment. Unlike the previous section, in this case, the contact between the robot and the environment is not necessarily included in a single plane. In this case, the influence of the internal force that acts between the contact points of the hand and the toe becomes significant. The embodiment will be described by taking the two-dimensional case shown in FIG. 6 as an example. Since a motion in a two-dimensional plane is assumed, a two-dimensional force and a one-dimensional moment may be considered. Summarizing the relational expression of force and moment balance, the following formula (14) is obtained.

Then, in addition to the contact force PCC this, consider the internal force. When the robot's hand comes into contact with the environment, there is an internal force that is a component of contact force that does not affect the momentum or angular momentum of the robot. In this case, the internal force relational expression can be expressed as in Expression (15).

Here, φ is a parameter relating to internal force. In Equation (14), there are three variables and variables

There are four. However, considering Formula (15) in addition to Formula (14), the number of formulas matches the number of variables, and the variables can be uniquely determined. At this time, if these variables include negative values, it can be determined that the contact state transitions. That is, when the hand touches the environment, the transition of the contact state is determined by using the internal force relational expression in addition to the contact force convex polyhedral cone.

As a fourth embodiment of the present invention, showing a technique for generating an operation of the legged mobile robot so as not to shift the contact state. The method for calculating the force / moment received by the robot and the method for calculating the contact force convex polyhedral cone with respect to the motion of the robot given by the first embodiment of the present invention are shown. Here, a method of correcting the operation when the calculated force / moment is not included in the contact force convex polyhedral cone is shown.

と表記する。（ｆ_Ｇ，τ_Ｇ）を数式（１５）の連立不等式で表される凸多面錐に正射影する。

この射影を

とすると、所望の射影は、

と求めることができる。このアルゴリズムの説明図を図７に示す。 When (−f _G , −τ _G ) that is not included in the contact force convex polyhedral cone is given, the distance that is included in the contact force convex polyhedral cone and defined by the Euclidean norm is closest by the following algorithm. Can be converted into a thing. The converted value

Is written. (F _G , τ _G ) is orthogonally projected onto the convex polyhedral cone represented by the simultaneous inequality of Expression (15).

This projection

Then the desired projection is

It can be asked. An explanatory diagram of this algorithm is shown in FIG.

FIG. 8 shows a motion generation method for a legged mobile robot using Equation (15). That is, first advance generates an operation of the robot, the formula (1), 3-dimensional forces, the moment calculation applied to the robot on the basis of the equation (2). When this is not included in the contact force convex polyhedral cone, the operation is corrected by projecting (−f _G , −τ _G ) based on the equations (17) and (18).

In the second to fourth embodiments of the present invention, the transition of the contact state was determined for walking on the floor surface with sufficient friction or when the hand touches the environment. What can be said in common in all these cases is the use of convex polyhedral cones. That is, in each case, by using the fifth embodiment of the present invention, it is possible to generate an action of a legged mobile robot in which the contact state does not change.

As a fifth embodiment of the present invention, a technique for feedback control of the posture of a robot in real time is shown. In this method, the force / moment received by the robot is calculated from the current joint angle information of the robot and the position and orientation information of the aircraft in real time. Then, a comparison is made with the target values of the force and moment received by the robot, and the operation of the robot is controlled so as to converge to the target value. A block diagram of this control system is shown in FIG.

In the second and third embodiments of the present invention, the transition of the contact state was determined for walking on the floor surface with sufficient friction or when the hand touches the environment. In all these cases, the formulas (1) and (2) are used in common. That is, in each case, the robot operation can be controlled in real time by using the fifth embodiment of the present invention.

Claims (3)

In a control device for a legged mobile robot comprising at least an upper body and a plurality of legs connected to the upper body,
When the robot moves, a three-dimensional force (f _G ) and a three-dimensional moment (τ _G ) that the robot receives due to inertial force and gravity are obtained,
Each contact point of the robot is in contact with the environment in (1 · · · · K), to approximate the friction cone is a set of 3-dimensional forces that can be generated with respect to the robot as L pyramid, the L pyramids each side in (1 · · · L) each, the L and the unit vectors, which are each side of the pyramid, the product of the non-negative scalar defining the size of those said side edges directed force, each side of the defining a possible occurrence force in a square direction, with the sum of these is a set of 3-dimensional forces that can be generated with respect to the robot at each contact point,
At each contact point, the outer product of the force that can be generated in each side direction of the L pyramid and the position vector of each contact point is defined, and the sum of these is a three-dimensional moment that can be generated for the robot at each contact point. A set of
A set of reaction forces and moments that can be generated with respect to the robot by adding the sum of the set of three-dimensional forces and the set of three-dimensional moments at all contact points for all contact points (1... K). The contact force is defined as a convex polyhedral cone ,
In the space of a three-dimensional forces and moments, the robot receives from the inertial force and gravity when the robot is operated force (f _G) and moment reverse forces and moments to cancel the _{(τ G) (-f G,} - A control device for a legged mobile robot, comprising: means for determining a transition of a contact state in which contact is separated at least at some contact points when τ _G ) is not included in the contact force convex polyhedral cone . Corresponding to the generated movement of the robot, a three-dimensional force (f _G ) and a three-dimensional moment (τ _G ) received by the robot due to inertial force and gravity are obtained, and the three-dimensional force (f _G ) and three-dimensional reverse forces and moments cancel the moment _{(τ G) (-f G,} -τ G) as is included in the contact force PCC, characterized in that it comprises means for generating a motion of the robot The control device for a legged mobile robot according to claim 1. When the robot moves, the three-dimensional force (f _G ) and the three-dimensional moment (τ _G ) received by the robot due to inertial force and gravity are calculated based on the current joint angle information and position / orientation information of the robot. in walking on a plane, a three-dimensional force calculated in real time _(f G) and reverse forces and moments to cancel the three-dimensional moment _{(τ G) (-f G,} -τ G) is the contact force 3. The control apparatus for a legged mobile robot according to claim 2, further comprising means for feedback control of the posture of the robot so as to be included in the convex polyhedral cone.

Images