JP3674787B2 - Robot apparatus motion control device, motion control method, and computer program - Google Patents

Description

  The present invention relates to an operation control apparatus and an operation control method for a robot apparatus including at least a plurality of movable parts, and a computer program. In particular, in a mobile robot apparatus including movable legs, the stability determination of ZMP is performed. The present invention relates to an operation control apparatus and operation control method for a robot apparatus that performs posture stabilization control using a norm, and a computer program.

  More particularly, the present invention relates to an operation control device and operation control method for a robot apparatus that corrects an unknown external force moment and an unknown external force by correcting a target (desired) trajectory for each part on the body, and a computer program.

  A mechanical device that uses an electrical or magnetic action to perform a movement resembling human movement is called a “robot”. It is said that the word “robot” comes from the Slavic word “ROBOTA (slave machine)”. In Japan, robots started to spread from the end of the 1960s, but many of them are industrial robots such as manipulators and transfer robots for the purpose of automating and unmanned production operations in factories. Met.

  Recently, research and development on legged mobile robots simulating the body mechanisms and movements of biped upright walking such as humans and monkeys has progressed, and expectations for practical use are also increasing. Leg type movement with two legs standing up is unstable and difficult to control posture and walking, compared to crawler type, four or six legs type, etc., but walking with irregularities on the work path such as rough terrain and obstacles It is excellent in that it can realize flexible movement work, such as being able to cope with discontinuous walking surfaces such as up and down of surfaces and stairs and ladders.

  A legged mobile robot that reproduces a human biological mechanism and operation is particularly called a “humanoid” or “humanoid robot”. The humanoid robot can provide, for example, life support, that is, support for human activities in various situations in daily life such as a living environment.

  Most of the human working space and living space are formed according to the human body mechanism and behavioral style of biped upright walking, and the current mechanical system using wheels and other driving devices as moving means moves. There are many barriers. Therefore, in order for the mechanical system, that is, the robot to perform various human tasks and penetrate deeply into the human living space, it is preferable that the movable range of the robot is substantially the same as that of the human. This is also why the practical application of legged mobile robots is highly expected.

  Many techniques relating to posture control and stable walking related to a biped legged mobile robot have already been proposed. Stable “walking” as used herein can be defined as “moving with legs without falling down”. Robot posture stabilization control is very important in avoiding robot overturning. This is because a fall means that the robot is interrupting the work being executed, and a considerable amount of labor and time are spent to get up from the fall state and resume the work. In addition, there is a danger of causing fatal damage to the robot body itself or the object on the other side that collides with the falling robot.

  Many proposals related to posture stabilization control of legged mobile robots and prevention of falls during walking use ZMP (Zero Moment Point) as a norm for determining the stability of walking. The standard for discriminating the stability by ZMP is the principle of d'Alembert that gravity and inertia force from the walking system to the road surface, and these moments balance with the floor reaction force and the floor reaction force moment as a reaction from the road surface to the walking system. based on. As a result of mechanical reasoning, there is a point where the pitch axis and roll axis moments become zero, that is, ZMP, inside the support polygon (that is, the ZMP stable region) formed by the sole contact point and the road surface (for example, non-patented) (Ref. 1).

  In summary, the ZMP norm is: “At every moment of walking, the ZMP exists inside the support polygon formed by the foot and the road surface, and the force in the direction in which the robot pushes the road surface acts. “The robot can walk stably without falling down (the body is rotating)”.

  According to the biped walking pattern generation based on the ZMP norm, there is an advantage that a foot landing point can be set in advance and it is easy to consider a kinematic constraint condition of a foot according to the road surface shape. In addition, using ZMP as a stability determination criterion means that a trajectory is treated as a target value in motion control instead of force, and thus technically feasible.

  For example, a legged mobile robot can perform a stable walk by making a point on the floor where ZMP becomes zero coincide with a target value (see, for example, Patent Document 1).

  Further, the legged mobile robot can be configured such that the ZMP is in the support polyhedron (polygon) or at a position where the ZMP has at least a predetermined margin from the end of the support polygon when landing or getting off ( For example, see Patent Document 2). In this case, even if a disturbance or the like is received, there is a ZMP margin for a predetermined distance, and the stability of the aircraft during walking is improved.

  Also, the walking speed of the legged mobile robot can be controlled by the ZMP target position (see, for example, Patent Document 3). That is, using the walking pattern data set in advance, the leg joint is driven so that the ZMP matches the target position, the inclination of the upper body is detected, and the walking pattern set according to the detected value Change the data discharge rate. When the robot leans forward, for example, by stepping on unknown irregularities, the posture can be recovered by increasing the discharge speed. Further, since the ZMP is controlled to the target position, there is no problem even if the discharge speed is changed during the both-leg support period.

  Further, the landing position of the legged mobile robot can be controlled by the ZMP target position (see, for example, Patent Document 4). That is, the legged mobile robot described in the publication detects a deviation between the ZMP target position and the actually measured position and drives one or both of the legs so as to eliminate it, or a moment around the ZMP target position. Is detected, and the leg is driven so that it becomes zero, thereby realizing stable walking.

  Further, the tilt posture of the legged mobile robot can be controlled by the ZMP target position (see, for example, Patent Document 5). That is, the moment around the ZMP target position is detected, and when the moment is generated, the leg is driven so that the moment becomes zero, thereby performing stable walking.

  As described above, the robot posture stabilization control using ZMP as a stability determination criterion searches for a point where the pitch and roll axis moment become zero inside the support polygon formed by the sole contact point and the road surface. There is.

  However, as a result of a priori verification by the present inventors, when the robot is legged at high speed, not only the moment about the pitch axis and the roll axis but also the yaw axis, that is, the Z axis It was also found that a moment was generated.

  FIG. 6 shows a relationship (example) between the walking speed [second / step] of the biped legged mobile robot and the moment Nm generated in the yaw axis direction. As can be seen from the figure, the time required for the legged mobile robot to take one step is shortened, that is, as the walking speed increases, the yaw moment increases significantly.

  Such a yaw-axis moment will eventually cause the aircraft to turn, causing slippage around the yaw axis between the bottom of the robot and the road surface, which greatly affects the stability of walking. This is an obstacle to realizing the formula work stably and accurately. Furthermore, if the influence of the yaw axis moment becomes significant, it may lead to the robot falling over and the situation where the airframe and the collision object are damaged.

  In addition, a legged mobile robot is generally composed of a plurality of part groups such as left and right upper limbs, lower limbs, and trunk, and has various degrees of freedom of joints. It is unclear how these multiple parts should be coordinated to cancel the moment error applied to the aircraft around the axis.

  In addition, robot posture stabilization control using ZMP as a stability criterion is to search for a point where the pitch and roll axis moment become zero inside the support polygon formed by the sole contact point and the road surface. However, since the support polygon itself does not exist at the time of getting off, such as when jumping or jumping off a hill, there is a problem that the conventional method of controlling the ZMP inside the support polygon cannot be applied.

JP-A-5-305579 JP-A-5-305581 JP-A-5-305583 JP-A-5-305585 JP-A-5-305586 Miomir Vukobratovic's “LEGGED LOCATION ROBOTS” (Ichiro Kato's “Walking Robot and Artificial Foot” (Nikkan Kogyo Shimbun))

  An object of the present invention is to provide an excellent robot apparatus motion control device, motion control method, and computer program capable of correcting an unknown external force moment and an unknown external force by correcting a target trajectory for each part on the airframe. There is to do.

  A further object of the present invention is to suitably operate a moment error applied to the aircraft around each of the roll, pitch, and yaw axes during exercise such as walking by operating each part group constituting the aircraft in a coordinated manner. An object of the present invention is to provide an excellent operation control device and operation control method for a robot apparatus and a computer program that can be canceled out.

  A further object of the present invention is that the moment error applied to the airframe around the roll, pitch, and yaw axes can be suitably canceled even when leaving the floor, such as when jumping or jumping off a hill. An object of the present invention is to provide an operation control apparatus and operation control method for a robot apparatus, and a computer program.

The present invention has been made in consideration of the above problems, and a first aspect thereof is an operation control device or an operation control method for a robot apparatus including at least a plurality of movable parts,
ZMP balance equation generating means or step for generating a ZMP balance equation describing a balance relationship of each moment applied to the robot body based on desired motion data consisting of trajectories for each part;
A moment error calculation means or step for calculating a moment error appearing on the ZMP balance equation generated by the ZMP balance equation generation means or step;
A priority setting means or step for dividing the airframe into a plurality of parts and setting the priority of the part for correcting the target (desired) trajectory in order to cancel the moment error;
A trajectory correcting means or step for correcting a target (desired) trajectory for each part in order according to the set priority order to compensate for a moment error;
An operation control apparatus or an operation control method for a robot apparatus, comprising:

  As described in the “Prior Art” section, in particular, in a bipedal legged mobile robot, the yaw moment applied to the aircraft increases remarkably as the walking speed increases. Such a yaw-axis moment is an obstacle to realizing a stable and accurate leg work such as turning of the aircraft, slipping around the yaw axis with respect to the road surface, and falling of the aircraft due to loss of balance.

  According to the motion control apparatus and the motion control method of the robot apparatus according to the first aspect of the present invention, when executing the motion pattern of the airframe composed of the motions of the lower limbs, the trunk, and the upper limbs, Moment errors such as the applied yaw moment can be canceled by correcting the target (desired) trajectory for each part according to a sequence according to a predetermined priority.

  In addition, according to the present invention, the moment error applied to the aircraft around the roll, pitch, and yaw axes during movement such as walking can be suitably performed by cooperatively operating each part group constituting the aircraft. Can be countered. For example, stability during walking or other legged work can be maintained by movement of the upper limb while continuing legged work by the lower limbs or trunk.

  More specifically, the movement of the upper limb is a movement using driving of a shoulder joint and an elbow joint. Of course, the left and right upper limbs generally move in opposite phases, but it is not necessary to limit to this in order to realize the present invention.

  According to the body design of a general upright biped robot, the shoulder joints and elbow joints have a wider movable angle than the trunk (eg trunk yaw axis). It is possible to achieve high attitude stability by canceling the yaw moment of the aircraft efficiently and with high accuracy.

  Further, such an upper limb movement has an effect of showing the motion of the upper body of the robot with rich expressive power.

  Here, the priority order setting means or step may give priority for correcting the target (desired) trajectory to each part in the order of the mass operation amount or the moment operation amount.

  Alternatively, the priority order setting means or step may refer to a predetermined action plan and give a priority order for correcting or achieving a desired trajectory to each part. For example, the leg trajectory cannot be corrected when a legged motion such as walking is performed, so it is lower (in the case of priority to correct the desired trajectory) or higher (priority without correcting the desired trajectory). If degree) give priority. Also, a hand grabbing an object cannot correct the trajectory and therefore gives a lower or higher priority. Further, when walking in a narrow passage sideways, if the waist trajectory is corrected, there is a high possibility of colliding with the wall, so a lower or higher priority is given.

  The ZMP balance equation generating means or step derives a ZMP equation for each of the pitch, roll, and yaw axes, and the trajectory correcting means or step determines the target (desired) trajectory of each part for each of the pitch, roll, and yaw. It may be corrected for each axis to compensate for the moment error.

  In addition, there is a problem that the conventional method of controlling the ZMP inside the support polygon cannot be applied because the support polygon itself does not exist at the time of getting off, such as when the aircraft jumps or jumps off the hill. According to the present invention, it is necessary to solve the equation of motion instead of the ZMP equation when there is no ZMP inside the support polygon, such as when leaving the bed, or when there is no support action point for the outside world. For this reason, it is possible to further include a center-of-gravity motion setting means or step for setting a center-of-gravity motion related to translation and / or rotation of the airframe when the legs of the legged mobile robot are getting out of bed.

  In such a case, the moment error calculating means calculates a force compensation amount at the target center of gravity in addition to the moment error appearing on the ZMP balance equation, and the priority setting means calculates the moment error and the force compensation amount. In order to cancel, a priority order of parts for correcting the target (desired) trajectory is set, and the trajectory correcting means corrects the target trajectory for each part in the order according to the set priority order, thereby generating a moment error. The force compensation amount may be compensated.

According to a second aspect of the present invention, there is provided a computer program written in a computer-readable format so as to process operation control for a robot apparatus having at least a plurality of movable parts on a computer system.
A ZMP balance equation generation step for generating a ZMP balance equation describing a balance relationship of each moment applied to the body of the robot based on desired motion data consisting of trajectories for each part;
A moment error calculation step for calculating a moment error appearing on the ZMP balance equation generated by the ZMP balance equation generation step;
A priority order setting step for setting the priority order of parts for dividing the airframe into a plurality of parts and correcting the target (desired) trajectory in order to cancel the moment error;
A trajectory correcting step for correcting a target (desired) trajectory for each part in order according to the set priority order to compensate for a moment error;
A computer program characterized by comprising:

  The computer program according to the second aspect of the present invention defines a computer program described in a computer-readable format so as to realize predetermined processing on a computer system. In other words, by installing the computer program according to the second aspect of the present invention in the computer system, a cooperative action is exhibited on the computer system, and the robot apparatus according to the first aspect of the present invention. The same effects as those of the motion control apparatus or motion control method can be obtained.

  According to the present invention, it is possible to provide an excellent operation control device and operation control method for a robot apparatus, and a computer program capable of correcting an unknown external force moment and an unknown external force by correcting a target trajectory for each part on the airframe. can do.

  Further, according to the present invention, the moment error applied to the airframe around each of the roll, pitch, and yaw axes at the time of exercise such as walking can be suitably performed by operating each part group constituting the airframe in a coordinated manner. Therefore, it is possible to provide an excellent motion control apparatus and control method for a robot apparatus, and a computer program.

  In addition, according to the present invention, it is possible to suitably cancel the moment error applied to the airframe around each axis of roll, pitch, and yaw even when leaving the floor such as when jumping or jumping off a hill. It is possible to provide an operation control apparatus and operation control method for a robot apparatus, and a computer program.

  Other objects, features, and advantages of the present invention will become apparent from more detailed description based on embodiments of the present invention described later and the accompanying drawings.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

  FIG. 1 schematically shows a configuration of the degree of freedom of a legged mobile robot that is used to implement the present invention.

  The robot shown in the figure is a humanoid robot having two legs and two arms. This robot has limbs attached to the airframe, and the left and right arms have 7 degrees of freedom: shoulder joint pitch axis, shoulder joint roll axis, upper arm yaw axis, elbow joint pitch axis, forearm yaw axis, wrist roll axis, wrist pitch axis. And left and right leg portions having six degrees of freedom, that is, a hip joint yaw axis, a hip joint roll axis, a hip joint pitch axis, a knee pitch axis, an ankle pitch axis, and an ankle roll axis.

  Each of these joint degrees of freedom is actually realized by an actuator / motor. In the present embodiment, a small AC servo actuator of a gear direct connection type and a servo control system integrated into one chip and built in a motor unit is mounted. This type of AC servo actuator is disclosed in, for example, Japanese Patent Laid-Open No. 2000-299970 (Japanese Patent Application No. 11-33386) already assigned to the present applicant.

  The legged mobile robot according to the present embodiment uses ZMP (Zero Moment Point) as a standard for determining the stability of walking. The stability criterion for ZMP is that the system forms an appropriate ZMP space, and if there is ZMP inside the support polygon, the system does not generate rotational or translational motion, and solves the equations of motion related to rotation and translation. There is no need. However, when there is no ZMP inside the support polygon or when there is no support action point for the outside world, it is necessary to solve the equation of motion instead of the ZMP equation (described later).

  FIG. 2 schematically shows a control system configuration of the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 includes each of the mechanism units 30, 40, 50R / L, 60R / L representing human limbs, and adaptive control for realizing a cooperative operation between the mechanism units. (Where R and L are suffixes indicating right and left, respectively, and so on).

  The entire operation of the legged mobile robot 100 is controlled by the control unit 80 in an integrated manner. The control unit 80 exchanges data and commands between a main control unit 81 configured by main circuit components (not shown) such as a CPU (Central Processing Unit) and a memory, and each component of the power supply circuit and the robot 100. The peripheral circuit 82 includes an interface (none of which is shown).

  In realizing the present invention, the installation location of the control unit 80 is not particularly limited. Although it is mounted on the trunk unit 40 in FIG. 2, it may be mounted on the head unit 30. Alternatively, the control unit 80 may be provided outside the legged mobile robot 100 so as to communicate with the body of the legged mobile robot 100 by wire or wirelessly.

The degree of freedom of each joint in the legged mobile robot 100 shown in FIG. 1 is realized by the corresponding actuator / motor M. That is, the head unit 30 includes a neck joint yaw axis actuator M ₁ , a neck joint pitch axis actuator M _2A representing the neck joint yaw axis, the first and second neck joint pitch axes, and the neck joint roll axis. M _2B and a neck joint roll shaft actuator M ₃ are disposed.

The trunk unit 40 is provided with a trunk pitch axis actuator M ₁₁ and a trunk roll axis actuator M ₁₂ representing the trunk pitch axis and trunk roll axis.

Further, the arm unit 50R / L is subdivided into an upper arm unit 51R / L, an elbow joint unit 52R / L, and a forearm unit 53R / L, but the shoulder joint pitch axis, shoulder joint roll axis, upper arm yaw axis , Elbow joint pitch axis, elbow joint yaw axis, wrist roll axis, shoulder joint pitch axis actuator M ₄ representing the wrist joint pitch axis, shoulder joint roll axis actuator M ₅ , upper arm yaw axis actuator M ₆ , elbow joint pitch An axis actuator M ₇ , an elbow joint yaw axis actuator M ₈ , a wrist joint roll axis actuator M ₉ , and a wrist joint pitch axis actuator M ₁₀ are provided.

The leg unit 60R / L is subdivided into a thigh unit 61R / L, a knee unit 62R / L, and a shin unit 63R / L. The leg yaw axis, hip pitch axis, hip roll axis , Knee joint pitch axis, ankle joint pitch axis, hip joint yaw axis actuator M ₁₃ representing ankle joint roll axis, hip joint pitch axis actuator M ₁₄ , hip joint roll axis actuator M ₁₅ , knee joint pitch axis actuator M ₁₆ , ankle A joint pitch axis actuator M ₁₇ and an ankle joint roll axis actuator M ₁₈ are provided.

The actuators M ₁ , M ₂ , M ₃ ... Used for each joint are more preferably a small AC servo actuator of the type directly mounted on a gear and mounted in a motor unit with a servo control system integrated into a single chip. ).

  For each mechanism unit such as the head unit 30, trunk unit 40, arm unit 50, and leg unit 60, sub-control units 35, 45, 55, and 65 for actuator drive control are provided.

  The body trunk 40 is provided with an acceleration sensor A1 and a posture sensor G1 including a gyro sensor and the like. The acceleration sensor A1 is disposed in each of the X, Y, and Z axis directions, for example. By locating the acceleration sensor A1 on the lumbar part of the body, the lumbar part, where the mass manipulated variable is large, is set as the control target point, and the posture and acceleration at that position are directly measured, and the posture stabilization based on ZMP Control can be performed.

  In addition, floor reaction force sensors F1 to F4 and F5 to F8, acceleration sensors A2 and A3, and attitude sensors G2 and G3 are disposed on the legs 60R and L, respectively. The floor reaction force sensors F <b> 1 to F <b> 8 are configured by, for example, mounting pressure sensors on the soles, and can detect whether or not the soles have landed based on the presence or absence of the floor reaction force. The acceleration sensors A2 and A3 are arranged at least in the X and Y axial directions. By disposing the acceleration sensors A2 and A3 on the left and right feet, the ZMP balance equation can be directly assembled with the feet closest to the ZMP position.

  When an acceleration sensor is placed only on the waist where the mass manipulated variable is large, only the waist is set as the control target point, and the foot state must be relatively calculated based on the calculation result of this control target point. It is necessary to satisfy the following conditions between the foot and the road surface.

(1) The road surface does not move no matter what force or torque is applied.
(2) The friction coefficient with respect to translation on the road surface is sufficiently large, and no slip occurs.

  On the other hand, in the present embodiment, a reaction force sensor system (such as a floor reaction force sensor) that directly applies a force to the ZMP is provided on the foot that is a contact portion with the road surface, and the local coordinates used for the control and the coordinates thereof An acceleration sensor for directly measuring is provided. As a result, the ZMP balance equation can be assembled directly at the foot closest to the ZMP position. Therefore, more rigorous posture stabilization control can be realized at high speed. As a result, the fuselage can be used on gravel or on carpets with long bristle feet that cause the surface to move when force or torque is applied, or on residential tiles that are prone to slip due to insufficient translational friction coefficient. Can guarantee stable walking (movement).

  The main control unit 80 can dynamically correct the control target in response to the outputs of the sensors A1 to A3, G1 to G3, and F1 to F8. More specifically, the whole body movement pattern in which each of the sub-control units 35, 45, 55, and 65 is adaptively controlled, and the upper limbs, trunk, and lower limbs of the legged mobile robot 100 are cooperatively driven. Is realized.

The whole body movement on the body of the robot 100 sets the foot movement, the ZMP trajectory, the trunk movement, the upper limb movement, the waist height, and the like, and commands for instructing the movement in accordance with these setting contents. Forward to the units 35, 45, 55, 65. Then, in each of the sub-control section 35, 45 ... interprets the command received from the main control unit 81 outputs a drive control signal to each actuator _{M 1, M 2, M 3} .... Here, “ZMP” refers to a point on the floor where the moment due to floor reaction force during walking is zero, and “ZMP trajectory” refers to, for example, ZMP during the walking motion period of the robot 100. It means the trajectory that moves.

  Next, a description will be given of the procedure of the posture stabilization process in the legged mobile robot 100 according to the present embodiment at the time of legged work, that is, at the time of executing the whole body cooperative movement pattern including the foot, waist, trunk, lower limb movement, etc. .

  The legged mobile robot according to the present embodiment uses ZMP as a norm for determining the stability of walking. The stability criterion for ZMP is that the system forms an appropriate ZMP space, and if there is ZMP inside the support polygon, the system does not generate rotational or translational motion, and solves the equations of motion related to rotation and translation. There is no need. On the other hand, when there is no ZMP inside the support polygon or when there is no support action point for the outside world, it is necessary to solve the equation of motion instead of the ZMP equation. For example, since a support polygon does not exist at the time of getting off, such as when jumping or jumping off a hill, in this embodiment, the equation of motion is solved instead of the ZMP equation (described later).

The airframe ZMP equation describes the balance of moments on the target ZMP. Represents aircraft in a number of mass points m _i, when the these control target point, the formula for obtaining the orbit of each control point total is zero moment in all of the control target point on the target ZMP generated in m _i It is a ZMP balance equation.

  The ZMP balance equation of the aircraft described in the world coordinate system (O-XYZ) and the ZMP balance equation of the aircraft described in the local coordinate system of the aircraft (O-X'Y'Z ') are as follows. .

Each of the above formulas represents the sum of moments around the ZMP (radius r _i −P _zmp ) generated by the acceleration component applied at each mass point (or control target point) m _i and the applied j-th external force moment. the sum of M _j, describes that the sum of the moments of ZMP around produced by an external force F _k (a point of action of k-th external force F _k and S _k) is balanced.

  This ZMP balance equation includes a floor reaction force moment (moment error component) T in the target ZMP. By suppressing this moment error to zero or within a predetermined allowable range, the attitude stability of the aircraft is maintained. In other words, correcting body movements (such as foot movements and trajectories of each part of the upper body) so that the moment error is zero or less than the allowable value is a means of posture stabilization control using ZMP as a stability criterion. It is the essence.

  In this embodiment, since the acceleration sensor is provided on the foot sole that is a contact portion with the road surface, the local coordinate system of the real robot is set with respect to the world coordinate system, and the foot foot sole as the origin is obtained. The ZMP balance equation can be directly derived. Furthermore, by locating acceleration sensors at control target points where the mass manipulated variable is large including the waist, the moment amount around the ZMP for each control target point can be derived directly using the acceleration sensor output value. it can.

  Here, since a real robot as a control target is a mobile device, it is difficult to obtain a position vector on the world coordinates at each control target point. As an alternative, the position vector of the control target point on the local coordinates can be obtained by a relatively easy calculation method such as inverse kinematics calculation. Therefore, the actual posture stabilization processing may be performed using the latter ZMP balance equation described in the local coordinate system (O-X′Y′Z ′) of the aircraft. (Since it is difficult to accurately measure the ZMP trajectory in the world coordinate system with the state detector mounted on the robot, an external measuring instrument fixed to the world coordinate system is generally required. On the other hand, a ZMP trajectory can be obtained accurately and directly in local coordinates by measuring the ZMP trajectory by placing a weighted sensor on the ground contact portion, such as walking (walking, etc.). By disposing the acceleration sensor near the local coordinate origin, it is possible to directly measure information that is dominant in high-speed motion.For this reason, in this embodiment, the ZMP balance equation in the local coordinate system is used. Is used.)

  Next, a procedure of posture stabilization processing in the legged mobile robot 100 according to the present embodiment, that is, generation processing of a whole body cooperative motion pattern for canceling a moment generated by motion of the waist, trunk, lower limbs, etc. will be described. In this embodiment, the body of the legged mobile robot 100 is divided into a plurality of parts, the priority of trajectory correction is given to each part, and the moment (error) is canceled by correcting the target (desired) trajectory for each part. There is a first feature.

  FIG. 3 shows an example of a procedure for stabilizing the whole body movement pattern of the aircraft in the form of a flowchart.

  First, exercise is set at a ground contact portion such as a foot (step S1). The foot movement is motion data obtained by connecting two or more poses of the aircraft in time series. The motion data is data created and edited in advance using a motion editing device, for example, and includes joint space information representing the displacement of each joint angle of the foot and Cartesian space information representing the joint position. Composed.

  Next, the ZMP stable region is calculated based on the set motion on the ground contact part such as the foot (step S2). Alternatively, the ZMP stable region is calculated based on the desired trajectory with priority set for each part group (step S14: described later). If the point where the robot contacts the road surface, such as floor motion or handstand motion, is not the sole, the ZMP stable region is calculated based on the support polygon formed by the ground contact point other than the sole and the road surface.

  ZMP is a point at which the moment applied to the aircraft becomes zero (as described above), and basically exists inside the support polygon formed by the ground contact point and the road surface. The ZMP stable region is a region set further inside the support polygon, and the aircraft can be brought into a highly stable state by accommodating the ZMP in the region.

  Then, a ZMP trajectory during the foot movement is set based on the foot movement and the ZMP stable region (step S3). However, when the point where the robot touches the road surface, such as floor motion or handstand motion, is not the sole, the ZMP trajectory is set based on the support polygon formed by the grounding point other than the sole and the road surface.

  In addition, each part of the aircraft is set as a group such as a waist, a trunk, an upper limb, and a head (step S11).

FIG. 7 shows an example in which a body part group is set. In the example shown in the figure, each part group of 1 to 7 parts is set in the order of waist, trunk, right arm, head, left arm, left leg, and right leg. Each part group is composed of a plurality of mass points. The mass point setting method may be either manual input by the designer or automatically generated according to a predetermined rule (for example, a part having a large mass manipulated variable such as a joint actuator is set as the mass point). ) When the number of mass points of the i-th part is n _i , the total mass m _i of the i-th part group (part i) is expressed by the following equation.

  Then, a desired trajectory is set for each part group (step S12). The setting of the desired trajectory for each part group of the airframe is performed offline using, for example, a motion editing device.

  Next, the group setting of each part is adjusted (regrouping) (step S13), and priority is given to these groups (step S14). The priority order mentioned here is the order of input to the processing calculation for the attitude stability control of the aircraft, and is assigned according to, for example, the mass operation amount or the moment operation amount. As a result, a desired trajectory group with priority for each part of the aircraft is completed. At this time, connection conditions for each part (local coordinate connection, world coordinate position fixed connection for a specific part, world coordinate posture connection for a specific part, etc.) are also set. The priority order may be changed at time t.

  In addition to the mass operation amount or the moment operation amount, the priority order for each part is determined with reference to a previously planned action plan. For example, since a trajectory of a grounded foot cannot be corrected when a legged motion such as walking is performed, a lower priority is given to the grounded foot. Also, a hand grabbing an object cannot give a lower priority because it cannot correct the trajectory. Also, when walking in a narrow passage sideways, a lower priority is given because the possibility of a collision with the wall is high if the waist trajectory is corrected.

Further, a mass that can be used for moment compensation is calculated for each part group of the machine body (step S15). In the example shown in FIG. 7, since the right leg is the support leg and the ZMP position P _ZMP exists in the right foot, the mass M _i that can be used for moment compensation in each part group is as follows.

In addition to the lumbar part itself, one part, that is, the lumbar part, can use the supporting trunk, right arm part, head part, and left arm part for moment compensation. In addition, the two parts, that is, the trunk part, can use the supporting right arm part, head part, and left arm part for moment compensation in addition to the trunk part itself. Also, 3-6 parts, that is, the right arm part, the head part, the left arm part, and the left leg part as a free leg have no other supporting part group, so that only their own part should be used for moment compensation. It becomes the mass that can be. Further, since the 7 parts as the standing legs, that is, the right leg parts support all the part groups, the total mass M ₇ of all the parts of the aircraft can be used for moment compensation. The gravity center position of the mass M _{7 in} this case corresponds to the gravity center position of the airframe, not the foot (see FIG. 8).

  Then, based on the movement of the ground contact part such as the foot, the ZMP trajectory, and the desired trajectory group for each part group of the airframe, the movement pattern of each part group is stabilized according to the priority set in step S14. Input into processing.

  In this posture stabilization process, first, an initial value 1 is substituted for the process variable i (step S20). Then, the moment amount on the target ZMP, that is, the total moment compensation amount Ω when the target trajectory is set for all the part groups is calculated (step S21). To the total moment compensation amount Ω, the unknown external force moment and the unknown external force identified by the above equation are added as known terms to the sum of the moment amounts on the target ZMP of all the site groups.

  Further, the desired trajectory is used for a part for which the target trajectory has not been calculated.

Next, the absolute moment compensation amount coefficient α _i is set using the mass M _i that can be used for moment compensation of the part i calculated in step S15 (step S22), and the moment compensation amount Ω _i in the part group i is set. Is calculated (step S23).

Next, using the calculated moment compensation amount Ω _i of the i-th part, a ZMP equation (E1) relating to part i is derived (step S24).

However, since the above ZMP equation has many unknown variables, it is difficult to obtain a solution both analytically and numerically. Therefore, here, the desired center of gravity orbit r _Mi of the mass that can be used for moment compensation of the i-th part is a known variable, and the change amount Δr _Mi for the desired center of gravity orbit of the mass that can be used for moment compensation of the i-th part is an unknown variable. We decided to derive the following approximate ZMP equation (E2). Here, r _Mi is a position vector of the center of gravity of the mass M _{i that} can be used for moment compensation in the i-th region group.

First, by solving this approximate ZMP equation, a change amount Δr _Mi for the desired center-of-gravity trajectory of the mass that can be used for moment compensation of the i-th part is calculated, and the target mass that can be used for moment compensation of the i-th part. Calculate the center of gravity trajectory using the following formula.

  In the case of a legged mobile robot in which each link is connected by a rotating joint, the above equation (E2) generally shares a motion in the Z direction, and thus becomes a nonlinear second-order differential equation with interference and is obtained analytically. It is difficult. Therefore, as shown below, it is assumed that the motion in the Z direction is not shared, and linearity / non-interference is performed in Expression (E2).

Then, to calculate an approximate solution of the target barycentric trajectory of the mass available for the moment compensation of the i th site of the change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution as strictly ZMP equation The operation of substituting into the equation (E1) to obtain the moment error, accumulating the inverted sign of this error on the right side of the linear / decoupled equation (E2), and obtaining the approximate solution again is the error The moment compensation motion of the part can be calculated by repeating until the value becomes equal to or less than a predetermined allowable value (step S25). In this way, it is possible to obtain the target trajectory for the part from the top to the i-th priority.

In order to calculate the trajectory of each mass point of the i-th part from the approximate solution of the target gravity center trajectory of the mass that can be used for moment compensation of the i-th part, an operation point is arranged at an arbitrary i-th part, Deriving a centroid vector C (X, Y, Z, θ _x , θ _y , θ _z ) with the translation position (X, Y, Z) or rotation angle (θ _x , θ _y , θ _z ) as an unknown variable, This centroid vector C (X, Y, Z, θ _x , θ _y , θ _z ) is set as an unknown variable on the left side, and the position vector r _Mi of the center of mass M _i is set as a known variable on the right side. Y, Z, θ _x , θ _y , θ _z ) = r _iMi, and the solution of this equation is calculated using a numerical search method or the like, or obtained analytically, so that The trajectory of each mass point can be determined.

  By performing such processing for all the site groups, a whole body motion pattern capable of stable motion (for example, walking) is generated.

  According to the processing procedure shown in FIG. 3, the body of the legged mobile robot 100 is divided into a plurality of parts, the priority of trajectory correction is given to each part, and the target (desired) trajectory for each part is corrected. Thus, the moment error in the whole body cooperative movement can be canceled. Here, the posture stabilization process using ZMP as a stability criterion is basically a point where the pitch and roll axis moment become zero inside the support polygon formed by the ground contact point such as the sole and the road surface. On the other hand, there is a problem that the moment error generated around the yaw axis, that is, the Z axis of the aircraft is not explicitly dealt with.

  Therefore, as a modification of the present embodiment, in a whole body cooperative movement pattern such as walking, a moment error applied to the airframe around the roll, pitch, and yaw axes is preferably canceled. FIG. 4 is a flowchart showing a procedure for stabilizing the whole body motion pattern by canceling out moment errors around the roll, pitch, and yaw axes of the aircraft.

  First, the foot movement is set (step S31). The foot movement is motion data obtained by connecting two or more poses of the aircraft in time series. The motion data is composed of joint space information representing the displacement of each joint angle of the foot and Cartesian space information representing the joint position (same as above).

  Next, a ZMP stable region is calculated based on the movement at the set contact portion such as the foot (step S32). Alternatively, the ZMP stable region is calculated based on the desired trajectory with priority set for each part group (step S44: described later). If the point where the robot contacts the road surface, such as floor motion or handstand motion, is not the sole, the ZMP stable region is calculated based on the support polygon formed by the ground contact point other than the sole and the road surface.

  ZMP is a point at which the moment applied to the aircraft becomes zero (as described above), and basically exists inside the support polygon formed by the ground contact point and the road surface. The ZMP stable region is a region set further inside the support polygon, and the aircraft can be brought into a highly stable state by accommodating the ZMP in the region.

  Then, based on the foot motion and the ZMP stable region, a ZMP trajectory during the foot motion is set (step S33). However, when the point where the robot touches the road surface, such as floor motion or handstand motion, is not the sole, the ZMP trajectory is set based on the support polygon formed by the grounding point other than the sole and the road surface.

  In addition, groups of the body parts are set such as the waist, trunk, upper limbs, and head (step S41). The setting example of the body part group is as shown in FIG. 7, and each part group of 1 to 7 parts in the order of the waist, trunk, right arm, head, left arm, left leg, and right leg. Is set.

  Then, a desired trajectory is set for each part group (step S42). The setting of the desired trajectory for each part group of the aircraft is performed offline using, for example, a motion editing device.

Next, the group setting of each part is adjusted (regrouping) (step S43), and priority is given to these groups (step S44). The priority order mentioned here is the order of input to the processing calculation for the attitude stability control of the aircraft, and is assigned according to, for example, the mass operation amount or the moment operation amount. As a result, a desired trajectory group with priority for each part of the aircraft is completed. Here, it is assumed that the priority order ρ is composed of three elements: a pitch axis priority order ρ _p , a roll axis priority order ρ _r , and a yaw axis priority order ρ _yaw . In addition to the mass operation amount or the moment operation amount, the priority order for each part is determined with reference to a previously planned action plan. At this time, connection conditions for each part (local coordinate connection, world coordinate position fixed connection for a specific part, world coordinate posture connection for a specific part, etc.) are also set. The priority order may be changed at time t.

In addition, the mass that can be used for moment compensation is calculated for each part group of the aircraft (step S45). In the example shown in FIG. 7, since the right leg is the supporting leg and the ZMP position P _ZMP is present in the right foot, the mass M _i that can be used for moment compensation in each part group is as described above.

  Then, based on the motion of the ground contact part such as the foot, the ZMP trajectory, and the desired trajectory group for each part group, the motion pattern of each part group is subjected to posture stabilization processing according to the priority set in step S44. throw into.

  In this posture stabilization process, first, an initial value 1 is substituted for the process variable i (step S50). Then, the moment amount on the target ZMP, that is, the total moment compensation amount Ω when the target trajectory is set for all the part groups is calculated (step S51). For the total moment compensation amount Ω, the unknown external force moment and the unknown external force identified by the above formula are added as known terms to the moment amounts on the target ZMP of all the site groups.

  Further, the desired trajectory is used for the part where the target trajectory is not calculated.

Next, the absolute moment compensation amount coefficient α _i is set using the mass M _i that can be used for moment compensation of the part i calculated in step S45 (step S52), and the moment compensation amount Ω _i in the part group i is set. Is calculated by the following equation (step S53).

Next, using the calculated moment compensation amount Ω _i of the i-th part, a ZMP equation (E1) relating to part i is derived (step S54).

However, since the above ZMP equation has many unknown variables, it is difficult to obtain a solution both analytically and numerically. Accordingly, here, the desired center of gravity orbit r _Mi of the mass that can be used for moment compensation of the i-th part is set as a known variable, and the change amount Δr _Mi for the desired center of gravity orbit of the mass that can be used for moment compensation of the i-th part is unknown. We decided to derive the following approximate ZMP equation (E2) as variables. Here, r _Mi is a position vector of the center of gravity of the mass M _{i that} can be used for moment compensation in the i-th region group.

First, by solving this approximate ZMP equation, a change amount Δr _Mi for the desired center-of-gravity trajectory of the mass that can be used for moment compensation of the i-th part is calculated, and the target mass that can be used for moment compensation of the i-th part. Calculate the center of gravity trajectory using the following formula.

  In the case of a legged mobile robot in which each link is connected by a rotating joint, the above equation (E2) generally shares a motion in the Z direction, and thus becomes a nonlinear second-order differential equation with interference and is obtained analytically. It is difficult. Therefore, as shown below, it is assumed that the motion in the Z direction is not shared, and linearity / non-interference is performed in Expression (E2).

Then, to calculate an approximate solution of the target barycentric trajectory of the mass available for the moment compensation of the i th site of the change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution as strictly ZMP equation The operation of substituting into the equation (E1) to obtain the moment error, accumulating the inverted sign of this error on the right side of the linear / decoupled equation (E2), and obtaining the approximate solution again is the error The moment compensation motion of the part can be calculated by repeating until the value becomes equal to or less than the predetermined allowable value (step S55). In this way, it is possible to obtain the target trajectory for the part from the top to the i-th priority.

In order to calculate the trajectory of each mass point of the i-th part from the approximate solution of the target gravity center trajectory of the mass that can be used for moment compensation of the i-th part, an operation point is arranged at an arbitrary i-th part, Deriving a centroid vector C (X, Y, Z, θ _x , θ _y , θ _z ) with the translation position (X, Y, Z) or rotation angle (θ _x , θ _y , θ _z ) as an unknown variable, An equation C (X, Y) in which the centroid vector C (X, Y, Z, θ _x , θ _y , θ _z ) is arranged on the left side as an unknown variable, and the position vector r _Mi of the centroid of the mass Mi is arranged on the right side as a known variable. , Z, θ _x , θ _y , θ _z ) = r _iMi, and the solution of this equation is calculated using a numerical search method or the like, or is obtained analytically, so that each i th part is calculated. The trajectory of the mass point can be determined.

Also, depending on the degree of freedom configuration of the i-th part, there may be no motion satisfying the priority order ρ (ρ _p , ρ _r , ρ _yaw ) (for example, the yaw axis having little interference with pitch motion and roll motion) Such as when there is no freedom of movement). In such a case, the process returns to step S44 to reset the priority order.

  By performing such processing for all the site groups, a whole body motion pattern capable of stable motion (for example, walking) is generated.

  According to the processing procedure shown in FIG. 4, the airframe of the legged mobile robot 100 is divided into a plurality of parts, the priority order of trajectory correction is given to each part, and the roll, pitch, and yaw axes are applied to the airframe. The whole body motion pattern can be stabilized so as to preferably cancel the moment error. Here, robot posture stabilization control using ZMP as a stability criterion is to search for a point where the pitch and roll axis moment become zero inside the support polygon formed by the ground contact point and the road surface. However, there is also a problem that the conventional method of controlling the ZMP inside the support polygon cannot be applied because the support polygon itself does not exist at the time of getting off, such as when jumping or jumping off a hill.

  Therefore, as a modification of the present embodiment, it is assumed that there is no ZMP inside the support polygon, such as when leaving the bed, or a case where there is no support action point for the outside world. FIG. 5 shows still another example of the procedure for stabilizing the whole body movement pattern of the aircraft in the form of a flowchart. In the processing procedure shown in the figure, it is assumed that there is no ZMP inside the support polygon, such as when leaving the bed, or the case where there is no support action point for the outside world, so that the equation of motion is solved in addition to the ZMP equation. It has become.

  First, the foot movement is set (step S61). The foot movement is motion data obtained by connecting two or more poses of the aircraft in time series. The motion data is composed of joint space information representing the displacement of each joint angle of the foot and Cartesian space information representing the joint position (same as above).

  Next, the ZMP stable region is calculated based on the foot movement at the set contact portion such as the foot (step S62). Alternatively, the ZMP stable region is calculated based on the desired trajectory with priority set for each part group (step S74: described later). If the point where the robot contacts the road surface, such as floor motion or handstand motion, is not the sole, the ZMP stable region is calculated based on the support polygon formed by the ground contact point other than the sole and the road surface.

  ZMP is a point at which the moment applied to the aircraft becomes zero (as described above), and basically exists inside the support polygon formed by the ground contact point and the road surface. The ZMP stable region is a region set further inside the support polygon, and the aircraft can be brought into a highly stable state by accommodating the ZMP in the region.

  Then, a ZMP trajectory during the foot movement is set based on the foot movement and the ZMP stable region (step S63). However, when the point where the robot touches the road surface, such as floor motion or handstand motion, is not the sole, the ZMP trajectory is set based on the support polygon formed by the grounding point other than the sole and the road surface.

In addition, it is assumed that there is no ZMP inside the support polygon, such as when jumping or jumping off a hill, or when there is no support action point for the outside world. Since it is necessary to solve the equation of motion, the center of gravity (translation, rotation) is set (step S91).
In addition, it is assumed that there is no ZMP inside the support polygon, such as when jumping or jumping off a hill, or when there is no support action point for the outside world. Since it is necessary to solve the equation of motion, the center of gravity (translation, rotation) is set (step S91).

  In addition, after the jump or jump off the hill, when suppressing the change in the attitude of the aircraft at the moment of landing, immediately after step S91, the virtual ZMP is calculated, and within the support polygon formed at the time of landing. What is necessary is just to set the desired trajectory of the movement regarding the landing site so that the virtual ZMP exists. The “virtual ZMP” here refers to the result of calculating ZMP under the assumption that a very large support polygon is formed on the planned landing road surface, although a support polygon cannot actually be formed. It is a ZMP that can.

  In addition, each part of the machine body is set as a group such as a waist, a trunk, an upper limb, and a head (step S71). The setting example of the body part group is as shown in FIG. 7, and each part group of 1 to 7 parts in the order of the waist, trunk, right arm, head, left arm, left leg, and right leg. Is set.

  Then, a desired trajectory is set for each part group (step S72). The setting of the desired trajectory for each part group of the aircraft is performed offline using, for example, a motion editing device.

Next, the group setting of each part is adjusted (regrouping) (step S73), and priority is given to these groups (step S44). The priority order mentioned here is the order of input to the processing calculation for the attitude stability control of the aircraft, and is assigned according to, for example, the mass operation amount or the moment operation amount. As a result, a desired trajectory group with priority for each part of the aircraft is completed. Here, the priority ρ is the pitch axis priority ρ _p , the roll axis priority ρ _r , the yaw axis priority ρ _yaw , the X direction priority ρ _x , the Y direction priority ρ _y , and the Z direction priority ρ _z. It is assumed to be composed of 6 elements. In addition to the mass operation amount or the moment operation amount, the priority order for each part is determined with reference to a previously planned action plan. At this time, connection conditions for each part (local coordinate connection, world coordinate position fixed connection for a specific part, world coordinate posture connection for a specific part, etc.) are also set. The priority order may be changed at time t.

Further, a mass that can be used for moment compensation and force compensation is calculated for each part group of the aircraft (step S75). In the example shown in FIG. 7, since the right leg is the supporting leg and the ZMP position P _ZMP is present in the right foot, the mass M _i that can be used for moment compensation in each part group is as described above.

  Then, based on the motion of the ground contact part such as the foot, the ZMP trajectory, and the desired trajectory group for each part group, the motion pattern of each part group is subjected to posture stabilization processing according to the priority set in step S74. throw into.

  In this posture stabilization process, first, an initial value 1 is substituted into the process variable i (step S80). Then, the moment amount on the target ZMP, that is, the total moment compensation amount Ω and the force compensation amount Γ when the target trajectory is set for all the part groups is calculated (step S81). For the total moment compensation amount Ω and force compensation amount Γ, the unknown external force moment and the unknown external force identified by the above formula are added as known terms to the sum of the moment amount and force compensation amount on the target ZMP of all the site groups. .

  Further, the desired trajectory is used for the part where the target trajectory is not calculated.

Next, the absolute moment compensation amount coefficient α _i and the absolute force compensation amount coefficient χ _i are set using the mass M _i that can be used for moment compensation and force compensation of the part i calculated in step S75 (step S82). The moment compensation amount Ω _i and the force compensation amount Γ _i are calculated by the following equations (step S83).

Next, using the moment compensation amount Ω _i and the force compensation amount Γ _i of the i-th part to be calculated, the ZMP equation (E1) relating to the part i or the equation of motion relating to rotation at the center of gravity (E1 ′), and the equation of motion relating to translation (E3) is derived (step S84).

However, since these equations have many unknown variables, it is difficult to obtain a solution both analytically and numerically. Therefore, here, the desired center of gravity trajectory r _Mi and the desired center of gravity acceleration μ _Mi that can be used for moment compensation and force compensation of the i-th part are used as known variables, and the desired mass that can be used for moment compensation of the i-th part. Approximate ZMP equation (E2) using the change amount Δr _{Mi and the} change acceleration amount (second-order derivative over time of Δμ _Mi ) for the center of gravity trajectory as unknown variables, or the approximate motion equation (E2 ′) for rotation at the center of gravity, and the approximate motion for translation Equation (E4) is derived. Here, r _Mi is a position vector of the center of gravity of the mass M _{i that} can be used for moment compensation in the i-th region group.

  Next, the derived equations must be solved simultaneously, but in the case of a legged mobile robot in which each link is connected by a rotating joint, each of the above equations shares a motion in the Z direction with each other. It becomes a second-order differential equation with interference, and it is difficult to obtain analytically.

Therefore, first, assuming that the motion in the Z direction is not shared, the above equation (E4) is linearized and _decoupled, and an approximate solution r _iMi of the target centroid trajectory of mass that can be used for force compensation of the i-th part is calculated. substitutes change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution to the exact equations of motion (E3), to calculate the force compensation error, linear and those obtained by inverting the sign of the error Accumulation is performed on the right side of the non-interfering equation (E4).

Next, Δr _Mi (unknown variable) is _replaced with r _iMi (calculated known variable) −r _Mi (known variable) + Δr _Mi (unknown variable), and moment compensation of the i-th part from the previously calculated target center of gravity trajectory is performed. The approximate solution of the change amount Δr _Mi to the target center-of-gravity trajectory of the mass that can be used for the calculation is calculated. Specifically, it is assumed that the motion in the Z direction is not shared, the above equation (E2) or (E2 ′) is linearized / decoupled, and the mass center of gravity trajectory that can be used for moment compensation of the i-th part Approximate solution is calculated. Then, by substituting the change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution strictly motion equation (E1) or (E1 '), and calculates a moment compensation error, the sign of the error The inverted one is stored in the right side of the equation (E2) or the equation (E2 ′) in which linearization / decoupling is performed.

Then, again i th site of calculating the approximate solution of the target barycentric trajectory of the mass available for force compensation, rigorous motion equation of the change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution ( Substituting into E3), a force compensation error is calculated, and the result of inverting the sign of this error is stored again on the right side of the equation (E4) in which linearization / decoupling is performed.

Next, Δr _Mi (unknown variable) is _replaced with r _iMi (calculated known variable) −r _Mi (known variable) + Δr _Mi (unknown variable), and the target gravity center trajectory of mass that can be used for moment compensation of the i-th part. calculating an approximate solution of the substitutes on the exact equations of motion the change amount [Delta] r _i of the mass points r _i of the i th site can be calculated from the approximate solution (E1) or (E1 '). Then, a moment compensation error is calculated, and the inverted sign of this error is stored again on the right side of the expression (E2) or (E2 ′) in which linearization / decoupling is performed.

  By repeating the above operation until the error becomes less than the allowable value, it is possible to calculate a moment compensating motion in addition to the force of the part (step S85). Then, the target trajectory can be obtained for the part with the priority order from the head to the i-th part.

  In general, a motion that satisfies either the ZMP equation or the motion equation may be calculated. However, for example, when jumping, getting off at the time of traveling, or landing, the above formula (E1) and formula (E3) may have to be satisfied at the same time.

In calculating the trajectory of each mass point of the i-th part from the approximate solution of the target gravity center trajectory of the mass that can be used for moment compensation or force compensation of the i-th part, an operation point is arranged at an arbitrary i-th part. Deriving a centroid vector C (X, Y, Z, θ _x , θ _y , θ _z ) with the translation position (X, Y, Z) or rotation angle (θ _x , θ _y , θ _z ) as an unknown variable, the centroid vector C (X, Y, Z, θ x, θ y, θ z) of the left side as unknown variables, the equation C (X disposed on the right side of the r _iMi as known _{variables, Y, Z, θ x,} θ By deriving _y , θ _z ) = r _iMi and calculating the solution of this equation, the trajectory of each mass point of the i-th part can be determined.

In addition, depending on the degree of freedom configuration of the i-th part, there may be no motion satisfying the priority order ρ (ρ _p , ρ _r , ρ _yaw , ρ _x , ρ _y , ρ _z ) (for example, each of XY (For example, when there is no degree of freedom of pitch / roll axis movement with little interference with direction movement). In such a case, it is necessary to return to step S61, S72, or S74 to reset the setting items.

In addition, depending on the posture of the i-th part, there is a difference in the convergence speed of the error in the iterative calculation of each element of the priority order ρ (ρ _p , ρ _r , ρ _yaw , ρ _x , ρ _y , ρ _z ). In some cases, an exact solution of an equation of an element having a slow convergence speed cannot be obtained, and an exact solution of only an equation of an element having a fast convergence speed is obtained first. Therefore, in such a case, the calculation is performed using an exact equation. Each moment compensation amount error or force compensation amount error is multiplied by the convergence speed adjustment coefficient μ (μ _p , μ _r , μ _yaw , μ _x , μ _y , μ _z ) and then accumulated on the right side of each approximate equation It is necessary to make the convergence speed uniform.

  By performing such processing for all the site groups, a whole body motion pattern capable of stable motion (for example, walking) is generated.

[Supplement]
The present invention has been described in detail above with reference to specific embodiments. However, it is obvious that those skilled in the art can make modifications and substitutions of the embodiment without departing from the gist of the present invention.

  The gist of the present invention is not necessarily limited to a product called a “robot”. That is, if it is a mechanical device or other general mobile device that performs a movement resembling human movement using electrical or magnetic action, it is a product belonging to another industrial field such as a toy. Even if it exists, this invention can be applied similarly.

  In short, the present invention has been disclosed in the form of exemplification, and the description of the present specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims section described at the beginning should be considered.

FIG. 1 is a diagram schematically showing a configuration of a degree of freedom of a legged mobile robot used for carrying out the present invention. FIG. 2 is a diagram schematically showing a control system configuration of the legged mobile robot 100. FIG. 3 is a flowchart showing an example of a procedure for stabilizing the whole body motion pattern of the aircraft. FIG. 4 is a flowchart showing another example of the procedure for stabilizing the whole body movement pattern of the aircraft. FIG. 5 is a flowchart showing still another example of the procedure for stabilizing the whole body motion pattern of the aircraft. FIG. 6 is a diagram showing a relationship (example) between the walking speed [second / step] of the biped legged mobile robot and the moment Nm generated in the yaw axis direction. FIG. 7 is a diagram schematically showing an example in which a body part group is set. FIG. 8 is a diagram showing the position of the center of gravity of the mass M ₇ .

Explanation of symbols

DESCRIPTION OF SYMBOLS 30 ... Head unit 40 ... Trunk unit 50 ... Arm unit 51 ... Upper arm unit 52 ... Elbow joint unit 53 ... Forearm unit 60 ... Leg unit 61 ... Thigh unit 62 ... Knee joint unit 63 ... Tibial unit 80 ... Control unit 81 ... Main control unit 82 ... Peripheral circuit 100 ... Legged mobile robot

Claims (11)

An operation control device for a robot apparatus having at least a plurality of movable parts,
ZMP balance equation generating means for generating a ZMP balance equation describing a balance relationship of moments applied to the body of the robot apparatus based on desired motion data consisting of trajectories for each part;
A moment error calculation means for calculating a moment error appearing on the ZMP balance equation generated by the ZMP balance equation generation means;
Priority setting means for dividing the airframe into a plurality of parts and setting the priority order of parts for correcting the target trajectory in order to cancel the moment error;
Trajectory correcting means for correcting the target trajectory for each part in order according to the set priority order to compensate for moment error;
An operation control device for a robot apparatus, comprising: The priority order setting means gives a priority order for target trajectory correction to each part in order of the magnitude of the mass manipulated variable or the moment manipulated variable.
The motion control apparatus for a robot apparatus according to claim 1. The priority order setting means gives a priority order for correcting a target trajectory to each part with reference to a pre-planned action plan,
The motion control apparatus for a robot apparatus according to claim 1. The ZMP balance equation generating means derives a ZMP equation for each of pitch, roll and yaw axes,
The trajectory correcting means corrects the moment trajectory by correcting the target trajectory of each part for each axis of pitch, roll, and yaw,
The motion control apparatus for a robot apparatus according to claim 1. At the time of getting off both legs of the robot apparatus, the robot apparatus further comprises a center of gravity motion setting means for setting a center of gravity motion related to translation or rotation of the airframe,
The moment error calculation means calculates a force compensation amount at the target center of gravity in addition to the moment error appearing on the ZMP balance equation,
The priority order setting means sets the priority order of the part for correcting the target trajectory in order to cancel the moment error and the force compensation amount,
The trajectory correcting means corrects a moment error and a force compensation amount by correcting a target trajectory for each part in an order according to the set priority order.
The motion control apparatus for a robot apparatus according to claim 1. An operation control method for a robot apparatus having at least a plurality of movable parts,
A ZMP balance equation generation step for generating a ZMP balance equation describing a balance relationship of each moment applied to the body of the robot based on desired motion data consisting of trajectories for each part;
A moment error calculation step for calculating a moment error appearing on the ZMP balance equation generated by the ZMP balance equation generation step;
A priority setting step for setting the priority of the part for correcting the target trajectory in order to divide the airframe into a plurality of parts and cancel the moment error;
A trajectory correction step for correcting a target trajectory for each part in order according to the set priority order to compensate for a moment error;
An operation control method for a robot apparatus, comprising: In the priority order setting step, each part is given a priority order for target trajectory correction in the order of the mass manipulated variable or the moment manipulated variable.
The operation control method of the robot apparatus according to claim 6. In the priority order setting step, a priority order for correcting the target trajectory is given to each part with reference to an action plan prepared in advance.
The operation control method of the robot apparatus according to claim 6. In the ZMP balance equation generation step, a ZMP equation is derived for each pitch, roll, and yaw axis,
In the trajectory correction step, the target trajectory of each part is corrected for each of the pitch, roll, and yaw axes to compensate for moment errors.
The operation control method of the robot apparatus according to claim 6. At the time of getting out of both legs of the robot apparatus, the robot apparatus further comprises a gravity center setting step for setting a gravity movement related to translation or rotation of the aircraft,
In the moment error calculation step, in addition to the moment error appearing on the ZMP balance equation, a force compensation amount at the target center of gravity is calculated,
In the priority order setting step, the priority order of the part for correcting the target trajectory to cancel the moment error and the force compensation amount is set,
In the trajectory correction step, the target trajectory is corrected for each part in order according to the set priority order to compensate for the moment error and the force compensation amount.
The operation control method of the robot apparatus according to claim 6. A computer program written in a computer readable format for processing motion control for a robot apparatus having at least a plurality of movable parts on a computer system,
A ZMP balance equation generation step for generating a ZMP balance equation describing a balance relationship of each moment applied to the body of the robot based on desired motion data consisting of trajectories for each part;
A moment error calculation step for calculating a moment error appearing on the ZMP balance equation generated by the ZMP balance equation generation step;
A priority setting step for setting the priority of the part for correcting the target trajectory in order to divide the airframe into a plurality of parts and cancel the moment error;
A trajectory correction step for correcting a target trajectory for each part in order according to the set priority order to compensate for a moment error;
A computer program comprising:

Images