JP3568527B2 - Mobile device - Google Patents

Description

  The present invention relates to a mobile device having a base and a movable portion connected to the base, and more particularly to a mobile device that performs posture stabilization control using ZMP as a stability determination criterion.

  More specifically, the present invention relates to a movement that identifies an unknown external force moment and an unknown external force using a ZMP equation or a motion equation derived based on a measurement value from a sensor installed in each part on the body and performs motion control. The present invention relates to a body apparatus, and more particularly, to a mobile body apparatus in which sensor systems dispersed in a plurality of parts on a body are arranged to efficiently measure a motion parameter necessary for deriving a ZMP equation or a motion equation.

  A mechanical device that performs a motion resembling a human motion using an electric or magnetic action is called a “robot”. It is said that the robot is derived from the Slavic word "ROBOTA (slave machine)". In Japan, robots began to spread from the late 1960's, but most of them were industrial robots (industrial robots) such as manipulators and transfer robots for the purpose of automation and unmanned production work in factories. Met.

  Recently, research and development on legged mobile robots that imitate the body mechanisms and movements of animals such as humans and monkeys that walk upright on two legs have been progressing, and expectations for their practical use have increased. Leg-based movement with two feet standing upright is more unstable than the crawler type, four-legged or six-legged type, and makes posture control and walking control difficult, but walking with irregularities on the work route such as uneven terrain and obstacles It is excellent in that it can realize flexible moving work, for example, it can respond to discontinuous walking surfaces such as surfaces, stairs and ladders.

  In addition, a legged mobile robot that reproduces a human biological mechanism or motion is particularly called a “humanoid” or “humanoid” robot. The humanoid robot can perform, for example, life support, that is, support for human activities in various situations in a living environment and other daily lives.

  Most of the working space and living space of human beings are formed according to the human body mechanism and behavior style of bipedal upright walking, and the current mechanical system using wheels and other driving devices as moving means will move. There are many barriers. Therefore, in order for the mechanical system, that is, the robot, to perform various human tasks and penetrate deep into the human living space, it is preferable that the movable range of the robot is almost the same as that of a human. This is the reason why practical use of the legged mobile robot is greatly expected.

  The significance of researching and developing a two-legged upright legged mobile robot called a humanoid or humanoid will be understood from the following two viewpoints, for example.

  One is a human science perspective. That is, through a process of creating a robot having a structure similar to a human lower limb and / or upper limb, devising a control method thereof, and simulating a human walking motion, a mechanism of natural human motion such as walking can be achieved. Can be elucidated by engineering. These findings could be greatly reduced to advances in various other disciplines that address human motor mechanisms, such as ergonomics, rehabilitation engineering, or sports science.

  The other is the development of a practical robot that supports life as a human partner, that is, supports human activities in various situations in a living environment and other daily lives. In various aspects of the human living environment, this type of robot needs to learn from humans to learn how to adapt to humans or the environment, each of which has a different personality, and to further grow in functionality. At this time, it is considered that the robot having the "human form", that is, the same shape or the same structure as a human, functions more effectively in performing smooth communication between the human and the robot.

  For example, when teaching a robot how to get through a room while avoiding obstacles that should not be stepped on, the partner who teaches like a crawler type or quadruped type robot has a completely different structure than myself. A bipedal walking robot having a similar appearance should be much easier for the user (operator) to teach and for the robot to learn (for example, see Non-Patent Document 1).

  There have already been proposed a number of techniques relating to posture control and stable walking for a type of robot that performs legged movement by biped walking. Stable “walking” here can be defined as “moving using the legs without falling over”.

  The posture stability control of the robot is very important in avoiding the falling of the robot. This is because falling means that the robot interrupts the work being performed, and considerable effort and time is spent in getting up from the falling state and resuming the work. In addition, above all, there is a risk that the fall may cause fatal damage to the robot body itself or the object on the other side that collides with the fallen robot. Therefore, in the design and development of a legged mobile robot, posture stability control during walking or other legged work is one of the most important technical issues.

  During walking, gravity, inertial force, and these moments act on the road surface from the walking system due to gravity and acceleration caused by the walking motion. According to the so-called "Dalambert principle", they balance the floor reaction force and the floor reaction force moment as a reaction from the road surface to the walking system. As a consequence of the mechanical inference, there is a point where the pitch and roll axis moments are zero, that is, “ZMP (Zero Moment Point)”, inside the supporting polygon formed by the sole and the road surface.

  Many proposals relating to posture stability control of a legged mobile robot and fall prevention during walking use this ZMP as a criterion for determining walking stability (for example, see Non-Patent Document 2). The bipedal walking pattern generation based on the ZMP standard has an advantage that a sole landing point can be set in advance, and the kinematic constraint condition of the toe according to the road surface shape can be easily considered. In addition, using ZMP as a stability determination criterion means that a trajectory, not a force, is treated as a target value in motion control, so that technical feasibility is increased.

  In general, a bipedal walking robot such as a humanoid has a higher center of gravity and a narrower ZMP stable area during walking than a quadrupedal walking. Therefore, the problem of the posture change due to the change of the road surface condition is particularly important in a bipedal walking robot.

  There have already been some proposals using ZMP as a posture stability determination standard for a bipedal walking robot.

  For example, a legged mobile robot can perform stable walking by matching a point on the floor at which ZMP becomes zero with a target value (for example, see Patent Literature 1).

  Further, the legged mobile robot can be configured so that the ZMP is located inside the supporting polyhedron (polygon) or at a position having at least a predetermined margin from the end of the supporting polygon at the time of landing or leaving the floor. In this case, there is a margin of ZMP for a predetermined distance even when a disturbance or the like is received, and the stability of the body during walking is improved (for example, see Patent Document 2).

  Further, the walking speed of the legged mobile robot can be controlled by the ZMP target position. That is, using the previously set walking pattern data, the leg joints are driven so that the ZMP coincides with the target position, and at the same time, the inclination of the upper body is detected, and the walking pattern set according to the detected value is used. Change the data ejection speed. 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 two-leg supporting period (for example, see Patent Document 3).

  Further, the landing position of the legged mobile robot can be controlled by the ZMP target position. 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 to eliminate the deviation, or a moment about the ZMP target position. , And by driving the legs so that it becomes zero, stable walking is realized (for example, refer to Patent Document 4).

  Further, the inclination posture of the legged mobile robot can be controlled by the ZMP target position. 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 to perform stable walking (for example, refer to Patent Document 5).

  The posture stability control of a robot using ZMP as a stability discrimination standard is basically to search for a point where the moment is zero on or inside the support polygon formed by the sole and the road surface. It is in.

  That is, a ZMP equation describing the balance relationship of each moment applied to the robot body may be derived, and the target trajectory of the body may be corrected so as to cancel the moment error appearing on the ZMP equation.

  In order to establish the ZMP equation, it is necessary to obtain the position and acceleration at a control target point on the airframe. Many conventional robot body control systems that use ZMP as a stability discrimination criterion use only position data at a control target point as a sensor input, and calculate acceleration data by performing second order differentiation of this position data in the control system. Then, the ZMP equation was derived.

  However, depending on such a calculation method, the amount of calculation increases, the processing load increases, and the calculation time increases. Furthermore, since acceleration data is obtained indirectly, accurate acceleration data cannot be used, and it is difficult to realize an operation that requires high-speed real-time trajectory correction of the aircraft, such as jumping or running. Further, when the strictness of the attitude control of the airframe is pursued, it is preferable to take a plurality of control target points, but the calculation time becomes excessively long, which leads to an increase in cost.

  Considering strictly controlling the motion of a mobile machine such as a legged robot in accordance with the ZMP equation, the acceleration in the world coordinates of the local coordinate origin used for the control, the position (posture) of each part of the body in the local coordinate system, By measuring the acceleration, ZMP position, external force and external force moment, and introducing the measured values to the ZMP equation, it is most strict to control the position and acceleration of each part while identifying the unknown external force moment and unknown external force. Motion control will be performed.

  For example, an inclinometer (or an accelerometer) and a gyro may be placed on each axis (pitch, roll, yaw (X, Y, Z)) one by one. Motion control can be performed with a minimum number of sensor configurations at positions distant from the actual operation position for each assumed part.

  However, in the motion control method based on such sensor arrangement, it is difficult to directly measure and control the positions and accelerations of all parts in addition to the local coordinate origin acceleration used for control.

Conventional motion control method
(1) The external environment of the robot does not move even if any force or torque acts.
(2) The coefficient of friction against translation in the external environment of the robot is sufficiently large, and no slip occurs.
(3) The robot is not deformed by any force or torque.
It is based on the condition that: For this reason, stable walking on gravel or carpets with long bristle feet, on which the road surface moves when force or torque is applied, and on tiles in dwellings where slippage is likely to occur due to insufficient friction coefficient of translation ( It does not guarantee the robot's motion control aiming at realizing the whole-body motion with jumping by giving the robot its own structure and flexibility.

JP-A-5-305579 JP-A-5-305581 JP-A-5-305558 JP-A-5-305585 JP-A-5-305586 Takanishi, "Control of Biped Robots" (Automotive Society of Japan Kanto Branch <Taka Plastics> No. 25, 1996 APRIL) "LEGGED LOCOMATION ROBOTS" by Miomir Vukobratovic (Ichiro Kato, "Walking Robots and Artificial Feet" (Nikkan Kogyo Shimbun))

  An object of the present invention is to provide an excellent mobile device capable of stabilizing and controlling the attitude of an aircraft during exercise using ZMP as an attitude stability determination criterion.

  It is a further object of the present invention to provide an excellent mobile device capable of performing more precise attitude stabilization control by deriving the ZMP equation at high speed and with high accuracy.

  A further object of the present invention is to identify the unknown external force moment and unknown external force using the ZMP equation or the equation of motion introduced based on the measurement values from the sensors installed at various parts on the fuselage, and to suitably perform the motion control. And to provide an excellent mobile device.

The present invention has been made in consideration of the above problems, and is a mobile device including a base and one or more movable units connected to the base,
A first acceleration sensor installed on the movable unit having the local coordinate system set, wherein the first acceleration sensor sets one local coordinate system of the movable unit;
A second acceleration sensor installed at one or more control target points of the mobile device in the local coordinate system;
Control for controlling a movable portion of the mobile device by controlling acceleration at each control point of the mobile device based on at least acceleration information obtained from the first acceleration sensor and the second acceleration sensor. Means,
A mobile device characterized by comprising:

  Here, the mobile device is a robot device having a movable part including at least an upper limb, a lower limb, and a trunk, and a control target point is provided for at least the upper limb, the lower limb, and the trunk. .

  Then, the local coordinate system and the first acceleration sensor are provided at a contact portion between the mobile device and a road surface. For example, the local coordinate system and the first acceleration sensor are provided on the foot. Alternatively, the local coordinate system and the first acceleration sensor are provided on the waist.

  In such a case, using the ZMP equation or the equation of motion derived based on the acceleration information obtained from at least the first acceleration sensor and the second acceleration sensor, the unknown moment applied to the mobile device or An unknown external force can be calculated. Therefore, the control means can control the movable portion of the mobile device based on the calculated unknown moment or unknown external force.

  In order to secure the posture stability of the robot device, for example, introduce an equation which is a stability determination criterion such as a ZMP equation or a motion equation, and perform motion control so as to cancel an unknown moment or an unknown external force applied to the device body. I need to do it. If there is a ZMP inside the supporting polygon, there is no rotation or translational motion in the system, and there is no need to solve the equation of motion related to rotation and translation, and the ZMP equation is calculated using the appropriate ZMP space formed by the system. By doing so, posture stability control is performed. When there is no ZMP inside the support polygon or when there is no support action point with respect to the external world, the posture stability control is performed by solving a motion equation instead of the ZMP equation. In addition, when the priority is set to be uniformly high for the trajectories of all the parts, such as in a dance involving jumping, both the ZMP equation and the equation of motion may be solved.

  Here, in order to formulate an equation, it is necessary to obtain the position and acceleration at each control target point on the airframe. However, in the case of a control system using only the position data at the control target point as a sensor input, the equation must be derived after calculating the acceleration data by performing second-order differentiation of the position data. In this case, the amount of calculation is large, and there is a problem of an increase in processing load and calculation time. In addition, since acceleration data is obtained indirectly, accurate acceleration data cannot be used. Therefore, it is difficult to implement an operation that requires high-speed real-time trajectory correction of the body.

  On the other hand, in the case of the robot device according to the present invention, the acceleration sensor is provided for each control target point set at a plurality of locations of the device main body, and the robot and the outside world, such as a sole that touches the floor, are placed on the floor. Since the reaction force sensor that measures the reaction force at the contact part of is provided, it is possible to derive the ZMP equation and the equation of motion by directly using the accurate acceleration data and reaction force data, The amount of calculation can be reduced. As a result, it is possible to preferably correct the trajectory even in an operation requiring high speed such as jumping or running.

  For example, an acceleration at a control target point on the body of the robot and a reaction force at a contact portion between the robot and the outside world are measured. Then, based on these measurement results, a ZMP equation describing a balance relation of each moment applied to the robot body is generated, and a target trajectory of the body is corrected so as to cancel a moment error appearing on the ZMP equation. can do.

  The posture stability control of a robot using ZMP as a stability discrimination criterion basically consists in searching for a point where the moment becomes zero inside a support polygon formed by a sole and a road surface. That is, a ZMP equation describing a balance relationship of each moment applied to the robot body is derived, and the target trajectory of the body is corrected so as to cancel a moment error appearing on the ZMP equation.

  For example, the acceleration in world coordinates of the origin of the local coordinates of the aircraft used for control, the position (posture) and acceleration of each control target point of the aircraft in the local coordinate system, the ZMP position and the external force moment are measured, and the position at each point is measured. By controlling the acceleration and the acceleration, the aircraft control can be performed most strictly.

  On the other hand, in accordance with such a principle, in addition to the local coordinate origin acceleration used for the control, directly calculating the positions and accelerations of all the parts to perform the aircraft control is excessively expensive, and the measurement system is arranged. There is a problem with the accommodation space for the work.

  Therefore, according to the present invention, a portion where the mass operation amount is maximum, for example, the waist, is set as the local coordinate origin as a control target point on the robot body. Then, a measuring means such as an acceleration sensor is arranged at the control target point, and the posture and acceleration at that position are directly measured, so that posture stability control based on ZMP can be performed.

  On the other hand, when a part having a large mass operation amount is set as the control target point, the state of the foot is not directly measured in the world coordinate system, but is relatively calculated based on the calculation result of the control target point. Things. Therefore, it is premised that the following conditions are satisfied between the foot and the road surface.

(1) The road surface does not move under any force or torque.
(2) The coefficient of friction against translation on the road surface is sufficiently large and no slip occurs.

  For example, stable walking (moving on a gravel or a carpet with long hairs, on which the road surface moves when force or torque is applied, or on a tile in a dwelling where the translational friction coefficient cannot be sufficiently secured and slipping is likely to occur. ) Cannot be guaranteed.

  Therefore, in the present invention, a reaction force sensor system (for example, a floor reaction force sensor) for directly measuring ZMP and force is provided on the foot, which is a contact portion with the road surface, and the local coordinates and the coordinates used for control are set. An acceleration sensor for direct measurement is provided.

  As a result, the ZMP equation can be assembled directly with the foot closest to the ZMP position, and more precise posture stabilization control that does not depend on the preconditions described above can be realized at high speed.

  In addition, a larger amount of mass operation can be incorporated into the control system, and the cooperative operation with the direct measurement results by the acceleration sensor and the posture sensor, which are arranged mainly at the part (waist) used for stability of operation, In addition, it is possible to realize the posture stabilization control of the legged mobile robot that does not depend on the preconditions described above.

  In addition, by connecting the acceleration sensors dispersedly arranged on the fuselage in series, a moment term in the ZMP equation or the equation of motion calculated at each control point based on the acceleration information of the acceleration sensor, By sequentially adding the external force term at each control target point according to the connection path, the sum of these can be efficiently calculated. Therefore, by sequentially repeating such addition processing according to the connection path, when the calculation result reaches the central controller, the moment term constituting the ZMP equation or the translation force term constituting the motion equation has already been obtained. Therefore, stable attitude control of the aircraft using these equations can be realized efficiently and at high speed.

  ADVANTAGE OF THE INVENTION According to this invention, the outstanding mobile body apparatus which can stabilize and control the attitude | position of the body at the time of exercise | movement using ZMP as attitude | position stability determination criteria can be provided.

  Further, according to the present invention, it is possible to provide an excellent moving body device that can perform more precise posture stability control by deriving a ZMP equation or a motion equation with high speed and high accuracy.

  Further, according to the present invention, it is preferable to identify the unknown external force moment and the unknown external force using the ZMP equation or the equation of motion introduced based on the measurement values from the sensors installed in the various parts on the body, and to suitably perform the motion control. It is possible to provide an excellent mobile device capable of performing the operation.

  Further objects, features, and advantages of the present invention will become apparent from more detailed descriptions based on embodiments of the present invention described below and the accompanying drawings.

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

A. Mechanical Structure of Legged Mobile Robot FIG. 1 and FIG. 2 show that the “humanoid” or “humanoid” legged mobile robot 100 used in the embodiment of the present invention is standing upright and forward. The view from above is shown. As shown in the figure, the legged mobile robot 100 includes a torso, a head, left and right upper limbs, and two left and right lower limbs performing legged movement. For example, a control unit built in the torso (Not shown) controls the operation of the aircraft in a comprehensive manner.

  Each of the left and right lower limbs includes a thigh, a knee joint, a shin, an ankle, and a foot, and is connected at a substantially lowermost end of the trunk by a hip joint. Each of the left and right upper limbs is composed of an upper arm, an elbow joint, and a forearm, and is connected at the left and right side edges above the trunk by a shoulder joint. The head is connected to the center of the uppermost end of the trunk by a neck joint.

  The control unit includes a controller (main control unit) that processes drive control of each joint actuator constituting the legged mobile robot 100 and processes an external input from each sensor (described later) and a power supply circuit and other peripheral devices. It is the case which did. The control unit may include a communication interface and a communication device for remote control.

  The legged mobile robot 100 configured as described above can realize bipedal walking by controlling the whole body cooperatively by the control unit. Such bipedal walking is generally performed by repeating a walking cycle divided into the following operation periods. That is,

(1) Right leg lifted, left leg supports single leg (2) Right leg touches both legs support period (3) Left leg lifts, right leg supports single leg period (4) Left leg touches left leg Support period

  Walking control in the legged mobile robot 100 is realized by planning a target trajectory of the lower limb in advance and correcting the planned trajectory in each of the above periods. That is, in the two-leg support period, the correction of the lower limb trajectory is stopped, and the waist height is corrected to a constant value using the total correction amount for the planned trajectory. In the single leg support period, a corrected trajectory is generated so that the relative positional relationship between the ankle and the waist of the corrected leg is returned to the planned trajectory.

  In general, the attitude stabilization control of the fuselage, including the correction of the trajectory of the walking motion, is performed by interpolation calculation using a fifth-order polynomial so that the position, velocity, and acceleration for reducing the deviation from ZMP are continuous. . ZMP is used as a criterion for determining the stability of walking. The stability discrimination standard based on ZMP is based on the principle of “Dallambert” that gravity and inertia force from the walking system to the road surface and these moments balance 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 the mechanical inference, there is a ZMP in which the pitch axis and the roll axis moment become zero inside the supporting polygon formed by the sole contact point and the road surface.

  FIG. 3 schematically shows a configuration of the degrees of freedom of the joints included in the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 connects an upper limb including two arms and a head 1, a lower limb including two legs for realizing a moving operation, and an upper limb and a lower limb. This is a structure including a plurality of limbs and a trunk.

  The neck joint (Neck) that supports the head has three degrees of freedom: a neck joint yaw axis 1, first and second neck joint pitch axes 2a and 2b, and a neck joint roll axis 3.

  Each arm has, as its degrees of freedom, a shoulder joint pitch axis 4 at the shoulder (Shoulder), a shoulder joint roll axis 5, an upper arm yaw axis 6, an elbow joint pitch axis 7 at the elbow (Elbow), and a wrist ( (Wrist) and a hand part. The hand is actually a multi-joint / multi-degree-of-freedom structure including a plurality of fingers.

  The trunk (Trunk) has two degrees of freedom: a trunk pitch axis 9 and a trunk roll axis 10.

  Further, each leg constituting the lower limb includes a hip joint yaw axis 11, a hip joint pitch axis 12, a hip joint roll axis 13, a knee joint pitch axis 14 in a knee (Knee), and an ankle (Ankle) in a hip joint (Hip). ), An ankle joint pitch axis 15, an ankle joint roll axis 16, and a foot.

  However, it does not mean that the legged mobile robot 100 for entertainment must be equipped with all the degrees of freedom described above, or that the present invention is not limited to this. It goes without saying that the degree of freedom, that is, the number of joints, can be appropriately increased or decreased according to design / manufacturing constraints and required specifications.

  Each degree of freedom of the legged mobile robot 100 as described above is actually implemented using an actuator. It is preferable that the actuator is small and light because of requirements such as removing excess bulges from the appearance and approximating the human body shape, and performing posture control on an unstable structure such as bipedal walking. . In this embodiment, a small AC servo actuator of a type directly connected to a gear and of a type in which the servo control system is integrated into a single motor control unit and incorporated in a motor unit is mounted (for this type of AC servo actuator, for example, Japanese Patent Application Laid-Open No. 2000-299970, which has already been assigned to the applicant). In the present embodiment, the passive characteristics of the drive system itself required for the robot 100 of the type that attaches importance to physical interaction with humans are obtained by adopting the reduced speed gear as the directly connected gear.

B. Control System Configuration of Legged Mobile Robot FIG. 4 schematically shows a control system configuration of the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 has mechanism units 30, 40, 50R / L and 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. The same applies hereinafter).

  The operation of the entire legged mobile robot 100 is totally controlled by the control unit 80. The control unit 80 transmits and receives data and commands to and from a main control unit 81 composed of main circuit components (not shown) such as a CPU (Central Processing Unit) and a memory, and a power supply circuit and each component of the robot 100. A peripheral circuit 82 including an interface (both not shown) is provided.

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

Each joint degree of freedom in the legged mobile robot 100 shown in FIG. 3 is realized by an actuator corresponding to each joint. That is, the head unit 30 includes a neck joint yaw axis actuator A ₁ , a neck joint pitch axis actuator A ₂ expressing each of the neck joint yaw axis 1, the neck joint pitch axis 2 and the neck joint roll axis 3, a neck joint roll. axis actuator A ₃ is arranged.

Further, the trunk unit 40 is provided with a trunk pitch axis actuator A ₉ and a trunk roll axis actuator A ₁₀ representing the trunk pitch axis 9 and the trunk roll axis 10, respectively.

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. The shoulder joint pitch axis 4, shoulder joint roll axis 5, upper arm The shoulder pitch axis actuator A ₄ , the shoulder roll axis actuator A ₅ , the upper arm yaw axis actuator A ₆ , and the elbow joint pitch axis actuator A ₇ representing the yaw axis 6, the elbow joint pitch axis 7 and the wrist joint yaw axis 8, respectively. , wrist joint yaw axis actuator A ₈ is arranged.

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 hip joint yaw axis 11, the hip joint pitch axis 12, and the hip joint A hip joint yaw axis actuator A ₁₁ , a hip joint pitch axis actuator A ₁₂ , a hip joint roll axis actuator A ₁₃ that expresses each of the roll axis 13, the knee joint pitch axis 14, the ankle joint pitch axis 15, and the ankle joint roll axis 16, the knee joint pitch An axis actuator A ₁₄ , an ankle joint pitch axis actuator A ₁₅ , and an ankle joint roll axis actuator A ₁₆ are provided.

The actuators A ₁ , A ₂ , A ₃ ... Used for the respective joints are more preferably small-sized AC servo actuators of the type directly connected to a gear and integrated in a motor unit with a single-chip servo control system (described above). ).

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

  An acceleration sensor 95 and a posture sensor 96 are provided on the trunk 40 of the body. The acceleration sensor 95 is disposed in each of the X, Y, and Z axis directions. By arranging the acceleration sensor 95 on the waist of the body, the waist, which is a part where the mass operation amount is large, is set as the control target point, and the posture and acceleration at that position are directly measured, and the posture stability control based on ZMP is performed. Can be performed.

  In addition, grounding confirmation sensors 91 and 92 and acceleration sensors 93 and 94 are disposed on the legs 60R and 60L, respectively. The ground contact confirmation sensors 91 and 92 are configured by attaching a pressure sensor to the sole, for example, and can detect whether the sole has landed on the basis of the presence or absence of a floor reaction force. The acceleration sensors 93 and 94 are arranged at least in the X and Y axis directions. By arranging the acceleration sensors 93 and 94 on the left and right feet, the ZMP equation can be assembled directly with the feet closest to the ZMP position.

  When the acceleration sensor is placed only on the waist, which is a part where the mass operation amount is large, only the waist is set as the control target point, and the state of the foot must be calculated relatively 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 under any force or torque.
(2) The coefficient of friction against 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 (a floor reaction force sensor or the like) for directly measuring ZMP and force is provided on a foot portion that is a contact portion with a road surface, and the local coordinates used for control and the An acceleration sensor for directly measuring coordinates is provided. As a result, the ZMP equation can be assembled directly with the foot closest to the ZMP position, and more precise posture stabilization control that does not depend on the preconditions described above can be realized at high speed. As a result, even on a gravel or a carpet with long fluff that the road surface moves when force or torque is applied, or on a residential tile where slippage tends to occur due to a lack of sufficient friction coefficient for translation, Stable walking (exercise) can be guaranteed.

  The main control unit 80 can dynamically correct the control target in response to the outputs of the sensors 91 to 93. More specifically, a whole body motion pattern in which the upper limb, the trunk, and the lower limb of the legged mobile robot 100 are driven in a coordinated manner by performing adaptive control on each of the sub-control units 35, 45, 55, and 65. To achieve.

The whole body movement on the body of the robot 100 sets foot movement, trajectory, trunk movement, upper limb movement, waist height, and the like, and issues a command for instructing an operation in accordance with these setting contents to each sub-control unit. Transfer to 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 the actuators _{A 1, A 2, A 3} .... Here, the ZMP is a point on the floor at which the moment due to the floor reaction force during walking becomes zero, and the “ZMP trajectory” means, for example, a trajectory where the ZMP moves during the walking operation of the robot 100. .

C. Posture stability control of the legged mobile robot Next, in the legged mobile robot 100 according to the present embodiment, the posture stability during the legged work, that is, the execution of the whole body cooperative movement including the foot, waist, trunk, and lower limb movements. The procedure of the conversion process will be described.

  The posture stability control according to the present embodiment uses ZMP as a stability determination criterion. The posture stability control of a robot using ZMP as a stability discrimination criterion basically consists in searching for a point where the moment becomes zero inside a support polygon formed by a sole and a road surface. That is, a ZMP equation describing a balance relationship of each moment applied to the robot body is derived, and the target trajectory of the body is corrected so as to cancel a moment error appearing on the ZMP equation.

  In the present embodiment, a region where the mass operation amount is maximum, for example, a waist, is set as a local coordinate origin as a control target point on the robot body. Then, a measuring means such as an acceleration sensor is arranged at the control target point, and the posture and acceleration at that position are directly measured to perform posture stability control based on ZMP. Further, by arranging an acceleration sensor on the foot which is a contact portion with the road surface, the local coordinates used for control and the coordinates are directly measured, and the ZMP equation is directly assembled with the foot closest to the ZMP position.

C-1. Introduction of ZMP Equation The legged mobile robot 100 according to the present embodiment is an aggregate of infinite, that is, continuous mass points. In this case, however, the amount of calculation for stabilization processing is reduced by replacing the model with an approximate model having a finite number of discrete mass points. More specifically, the legged mobile robot 100 physically having the multi-joint degree of freedom configuration shown in FIG. 3 is handled by replacing it with a multi-mass point approximation model as shown in FIG. The illustrated approximation model is a linear and non-interfering multi-mass approximation model.

  In FIG. 5, the O-XYZ coordinate system represents the roll, pitch, and yaw axes in the absolute coordinate system, and the O′-X′Y′Z ′ coordinate system represents the roll, pitch, Each yaw axis is shown. However, the meanings of the parameters in the figure are as follows. It should also be understood that symbols with a dash (') describe a motion coordinate system.

The multi-mass point model shown in FIG, i is a subscript representing a mass given to i-th, m _i is the i-th material point mass, r _'i is the i-th material point of the position vector (where motion coordinates System). The machine center of gravity of the legged mobile robot 100 according to the present embodiment exists near the waist. That is, the waist is a mass point where the mass operation amount becomes the maximum, in FIG. 5, the mass m _h, its position vector (although moving coordinate system) is _{r 'h (r' hx,} r 'hy, r' _hz ). The ZMP position vector (movement coordinate system) of the aircraft is r ′ _zmp (r ′ _zmpx , r ′ _zmpy , r ′ _zmpz ).

The world coordinate system O-XYZ is an absolute coordinate system and is invariable. In the legged mobile robot 100 according to the present embodiment, acceleration sensors 93, 94, and 96 are disposed on the lower back and the legs of both legs, respectively. _q is detected. On the other hand, in the motion coordinate system, that is, in the local coordinate system of the body, OX'Y'Z 'moves with the robot.

  The multi-mass model is a representation of a robot in the form of a wireframe model. As can be seen from FIG. 5, the multi-mass point approximation model is set with each of the shoulders, both elbows, both wrists, trunk, waist, and both ankles as mass points. In the illustrated non-rigid multi-mass approximation model, the moment equation is described in the form of a linear equation, which does not interfere with the pitch and roll axes. The multi-mass point approximation model can be generally generated by the following processing procedure.

(1) The mass distribution of the entire robot 100 is obtained.
(2) Set a mass point. The method of setting the mass may be either manual input by a designer or automatic generation according to a predetermined rule.
(3) for each region i, obtains the center of gravity, it is applied to the mass to the appropriate position of the center of gravity and mass m _i.
(4) the mass points m _i, centered on the mass point position r _i, to display as a sphere having a radius proportional to its mass.
(5) The masses that are actually connected, that is, the spheres are connected.

In the multi-mass model shown in FIG. 6, each rotation angle (θ _hx , θ _hy , θ _hz ) in the base, that is, the waist information, indicates the posture of the waist in the legged mobile robot 100, that is, the rotation of the roll, pitch, and yaw axes. It is specified.

The ZMP equation of the fuselage describes the balance of each moment applied at the control target point. As shown in FIG. 6 represents the body in a number of mass points m _i, when the these control target points, obtaining the sum of the moments applied at all the controlled object points m _i Formula is ZMP equation.

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

The above equation, the sum of the moments of ZMP around produced by the acceleration component applied at each mass point m _i (radius r _i -r _ZMP), the sum of external force moment M _i applied to each of the mass points m _i, It describes that the sum of moments around the ZMP generated by the external force F _k (the point of action of the k-th external force F _k is s _k ) is balanced.

  This ZMP balance equation includes a total moment compensation amount, that is, a moment error component T. By keeping this moment error at zero or within a predetermined tolerance, the attitude stability of the aircraft is maintained. In other words, correcting the body motion (foot motion or the trajectory of each part of the upper body) so that the moment error becomes zero or less than the allowable value is the essence of the posture stability control using the ZMP as a stability determination criterion. is there.

  In the present embodiment, since the acceleration sensors 96, 93 and 94 are provided on the waist and the left and right feet, respectively, the ZMP is directly and accurately performed using the acceleration measurement results at these control target points. A balance equation can be derived. As a result, high-speed and more strict posture stability control can be realized.

C-2. Whole Body Cooperative Posture Stability Control FIG. 7 is a flowchart showing a processing procedure for generating a body motion capable of stably walking in the legged mobile robot 100. However, in the following description, the joint positions and motions of the legged mobile robot 100 are described using a linear / non-interfering multi-mass point approximation model as shown in FIGS.

  First, the foot motion is set (step S1). The foot motion is motion data in which two or more aircraft poses are connected in chronological order.

  The motion data is composed of, for example, joint space information indicating displacement of each joint angle of the foot and Cartesian space information indicating joint positions. Motion data can be manually input on a console screen, direct teaching (direct teaching) to an airframe, or constructed on an authoring system for motion editing.

  Next, a ZMP stable area is calculated based on the set foot motion (step S2). The ZMP is a point where the moment applied to the airframe becomes zero, and basically exists inside the support polygon formed by the sole contact point and the road surface. The ZMP stable region is a region set further inside the supporting polygon, and the ZMP can be accommodated in this region to make the aircraft highly stable.

  Then, a ZMP trajectory during the foot motion is set based on the foot motion and the ZMP stable area (step S3).

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

  Then, a desired trajectory is set for each part group (step S12). The desired trajectory for the upper body is set, as in the case of the feet, by manual input on the console screen, direct teaching (direct teaching) to the fuselage, or on an authoring system for motion editing. be able to.

  Next, the group setting of each part is adjusted (re-grouping) (step S13), and priorities are given to these groups (step S14).

  Here, the priority refers to the order of input to the processing calculation for stabilizing the attitude of the aircraft, and is assigned according to, for example, the mass operation amount. As a result, a desired trajectory group with priority for each part of the upper body of the aircraft is completed. The priority may be changed between the target trajectories according to the posture of the robot.

  In addition, a mass that can be used for moment compensation is calculated for each of the body groups of the upper body (step S15).

  Then, based on the foot motion, the ZMP trajectory, and the desired trajectory group for each upper body part group, the exercise pattern of each part group is input to the posture stabilization process in accordance with the priority set in step S14.

  In this posture stabilization processing, first, an initial value 1 is substituted for a processing variable i (step S20). Then, the moment amount on the target ZMP, that is, the total moment compensation amount at the time of setting the target trajectory for the i-th part group from the top is calculated (step S21). For a part for which the target trajectory has not been calculated, the desired trajectory is used.

  Next, the amount of moment compensation is set using the mass that can be used for the moment compensation of the part calculated in step S15 (step S22), and the moment compensation amount is calculated (step S23).

  Next, using the calculated moment compensation amount of the i-th part, a ZMP equation for the i-th part is derived (step S24), and the moment compensation movement of the part is calculated (step S25). It is possible to obtain the target trajectory for the i-th part from the top in the priority order.

  By performing such processing for all the part groups, a whole body movement pattern capable of stable movement (for example, walking) is generated. That is, by correcting all or a part of each target trajectory according to the solution of the ZMP equation (or the equation of motion (to be described later)) and the priority given to each part, the whole body motion pattern becomes Generated.

  In the processing procedure for generating the body motion pattern shown in FIG. 7, first, the foot motion is set, the stable region is calculated, the ZMP trajectory is set, and then the priority order of the desired trajectory in each part of the upper body is set. , But the processing order is not limited to this. For example, the priority of the desired trajectory in each part of the upper body may be set first, and then the calculation of the ZMP stable area and the setting of the ZMP trajectory may be performed. In the former case, the priority order of the desired trajectory in each part of the upper body is set according to the previously set ZMP trajectory, whereas in the latter case, the desired trajectory of each part of the upper body set earlier is determined. The calculation of the stable region and the ZMP trajectory are set so as to be maintained.

  FIG. 8 shows, in the form of a flowchart, a processing procedure for generating a body motion for calculating the ZMP stable area and setting the ZMP trajectory after setting the priority order of the desired trajectory of each part first.

  First, for each part of the upper body of the body (above the hip joint), a group is set such as a waist, a trunk, an upper limb, and a head (step S31).

  Then, a desired trajectory is set for each part group (step S32). The desired trajectory for the upper body is set, as in the case of the feet, by manual input on the console screen, direct teaching (direct teaching) to the fuselage, or on an authoring system for motion editing. be able to.

  Next, the group setting of each part is adjusted (re-grouping) (step S33), and priorities are given to these groups (step S34).

  Here, the priority refers to the order of input to the processing calculation for stabilizing the attitude of the aircraft, and is assigned according to, for example, the mass operation amount. As a result, a desired trajectory group with priority for each part of the upper body of the aircraft is completed.

  Next, a ZMP stable area is calculated based on the set priority order of the desired orbit in the upper body (step S35). Then, a ZMP trajectory during the foot motion is set based on the ZMP stable region (step S36).

  In addition, a mass that can be used for moment compensation is calculated for each of the body groups of the upper body (step S45).

  Then, based on the desired trajectory group and the ZMP trajectory for each upper body part group, the motion pattern of each part group is input to the posture stabilization processing in accordance with the priority set in step S34.

  In this posture stabilization processing, first, an initial value 1 is substituted for a processing variable i (step S37). Then, the moment amount on the target ZMP, that is, the total moment compensation amount when the target trajectory is set for the i-th part group from the top is calculated (step S38). For a part for which the target trajectory has not been calculated, the desired trajectory is used.

  Next, the moment compensation amount is set using the mass that can be used for the moment compensation of the part calculated in step S45 (step S39), and the moment compensation amount is calculated (step S40).

  Next, the ZMP equation for the i-th part is derived using the calculated moment compensation amount for the i-th part (step S41), and the moment compensation movement for the part is calculated (step S42). It is possible to obtain the target trajectory for the i-th part from the top in the priority order.

  Here, a method of setting the priority order of the desired trajectory in step S14 in FIG. 7 will be described.

The total moment compensation amount is Ω [Nm], the mass available for the moment compensation of the i portion and _{M i [N] (i =} 1,2,3, ..., n), the moment compensation amount of i portions α _i × Ω [Nm]. Here, α _i is an absolute moment compensation amount coefficient, and is expressed by the following equation using the relative moment compensation amount coefficient β _i .

  The priority of the desired trajectory decreases as the compensation amount coefficient becomes larger than 0. The positive direction affects the motion stabilization, and the negative direction acts in the opposite direction to the motion stabilization.

  Hereinafter, a method of setting the priority order of the desired trajectory in the upper body will be described with reference to specific examples.

  As shown in FIG. 9, in a movement pattern in which a cart is carried by hand, the priority of the hand trajectory increases. As an example of setting the priority, α of the hand portion is set to 0.0 and the sum of α of the remaining portions is set to 1.0.

  As shown in FIG. 10, in an exercise pattern in which a golf club (or a baseball bat) is swung with both hands, the priorities are set in the order of the trajectory of the hand and the foot. As an example of the setting of the priority, α of the hand is set to 0.0, α of the foot is set to 0.1, and the sum of α of the remaining parts is set to 0.9.

  As shown in FIG. 11, in an exercise pattern such as performing an horse competition in mechanical exercises, since the body is supported only by both hands and the posture of the legs is regarded as important, the hands, the trunk, and the lower limbs are emphasized. The priority of the trajectory of the relative relationship is set high. As an example of setting the priority, α of the hand is set to 0.0, α of the trunk and the lower limb (trajectory at the shoulder) is set to 0.0, and the sum of α of the remaining parts is set to 1.0.

  As shown in FIG. 12, in an exercise pattern in which a tray on which bottles, glasses, and the like are placed with one hand and walking while maintaining a balance, priority is given in the order of the trajectories of the hand, trunk, waist, and foot. Is set.

  As shown in FIG. 13, in the exercise pattern in which a handstand is performed, the priority is set in the order of the trajectory of the hand, the bodily sensation, and the waist because the body is supported by both hands and the posture is stabilized. As an example of setting the priority, α of the hand is set to 0.0, α of the trunk is set to 0.2, α of the waist is set to 0.3, and the sum of α of the remaining parts is set to 0.5.

  As shown in FIG. 14, in the exercise pattern in which a bar is placed on the bottom of a tray on which a plurality of cups are placed, and the lower end of the bar is placed on the forehead for balancing, the priority of starting the head is set high. Is done. As an example of setting the priority, it is assumed that α of the head is 0.0 and the sum of α of the remaining parts is 1.0.

  As shown in FIG. 15, in a motion pattern in which a plurality of hula hoops are supported by the rotation motion of the waist and trunk, the priority of the trajectory of the trunk is set high. As an example of setting the priority, α of the trunk is set to 0.0, and the sum of α of the remaining parts is set to 1.0.

  As shown in FIG. 16, in an exercise pattern of running with a long pole and performing a pole vault over a bar set at a high place, the lower limb, the waist, the trunk, the upper limb, etc. are given priority over time. The degree changes. As an example of setting the priority, α of the foot is set to 0.0 in the above-mentioned trial, α of the waist and the trunk is set to 0.0 in the middle stage, α of the upper limb is set to 0.0 in the second half, and the rest in each period is set. Is set to 1.0.

  As shown in FIG. 17, in exercise patterns such as rhythmic gymnastics, ball riding, and ballet dancing with a ribbon, the trajectory of all parts is set to have a higher priority. As an example of setting the priority, it is assumed that α of each unit is common and the sum of α is 1.0.

  As shown in FIG. 18, in an exercise pattern in which a tightrope is stretched while keeping balance by spreading both arms, the priority is set in order of the trajectory of the foot, the upper limb and the trunk. As an example of setting the priority, α of the foot is set to 0.0, α of the upper limb and the trunk is set to 0.1, and the sum of α of the remaining parts is set to 0.9.

  As shown in FIG. 19, in the exercise pattern of walking on a scaffold assembled along the outer wall of the building under construction, the priority is set in the order of the upper limb / trunk and foot trajectory. As an example of setting the priority, α of the upper limb and the trunk is set to 0.1, α of the foot is set to 0.2, and the sum of α of the remaining parts is set to 0.7.

D. Motion control in consideration of mechanical hardware deformation Previous legged mobile robots and their mechanical handling were premised on that the deformation was very small even when subjected to external force or torque, and could be ignored for the entire robot motion. . That is, since it is assumed that the distance between the joints of the robot does not change, one configuration is sufficient for each element of the state detection sensor of the robot system.

  However, in the future, in order to realize high-dynamic-level motion using running and acceleration more continuously and more aggressively, a shock-absorbing function that utilizes the deformation of mechanical hardware itself will be required, and higher Must be solved in real time and at high speed.

  Therefore, this section proposes a sensor system configuration method that does not require the precondition that the distance between the joints of the robot does not change, and a distributed high-speed motion control system using the same.

  In this specification, the following definitions are used (see, for example, “Dynamics of Mechanical System” edited by The Japan Society of Mechanical Engineers, pp. 31-33, Ohmsha, March 25, 1991). thing).

Translational motion: Inertial force =-(weight / gravity acceleration) x acceleration Rotational motion: Moment of inertia =-polar moment of inertia x angular acceleration Polar moment of inertia: moment of inertia at the axis of rotation

  The legged mobile robot according to the present embodiment uses ZMP as a criterion for determining the stability of walking. The stability discrimination criterion by ZMP is that if the system forms an appropriate ZMP space and there is a ZMP inside the supporting polygon, no rotational or translational motion occurs in the system, and the equation of motion for rotation or translation is solved. No need. When there is no ZMP inside the supporting polygon or when there is no supporting action point with respect to the external world, it is necessary to solve a motion equation instead of the ZMP equation.

The ZMP equation of the fuselage describes the balance of each moment applied at the control target point. Represents aircraft in a number of mass points m _i, when the these control target points, obtaining the sum of the moments applied at all the controlled object points m _i Formula is ZMP equilibrium 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 (OX-Y'Z ') are as follows. .

Each the above formula, the sum of the moments of the mass points (or control point) ZMP around produced by the acceleration component applied at m _i (radius r _i -P _ZMP), was applied to the mass points m _i external force the sum of the moments M _i, 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 total moment compensation amount, that is, a moment error component T. By keeping this moment error at zero or within a predetermined tolerance, the attitude stability of the aircraft is maintained. In other words, correcting the body motion (foot motion or the trajectory of each part of the upper body) so that the moment error becomes zero or less than the allowable value is the essence of the posture stability control using the ZMP as a stability determination criterion. is there.

  The legged mobile robot according to the present embodiment is provided with a reaction force sensor system for directly measuring ZMP and force at a contact portion with the outside world, and for directly measuring local coordinates used for motion control and the coordinates. By disposing the acceleration sensor and angular velocity sensor, and by arranging the acceleration sensor and the attitude sensor at each vector position used in the calculation model, the control parameters necessary to introduce the ZMP equation (or motion equation) can be directly calculated. By making the measurement possible, strict motion control is realized with good responsiveness without assuming the condition that the body is rigid and does not deform due to the application of external force or the like.

  An example of the arrangement of the reaction sensor system according to the present embodiment will be described below.

(1) An acceleration sensor, an angular acceleration sensor, and an angular velocity sensor are mounted on a portion where mass is concentrated.
(2) An acceleration sensor, an angular acceleration sensor, and an angular velocity sensor are mounted near the center of gravity of each link.
(3) An acceleration sensor, an angular acceleration sensor, and an angular velocity sensor are mounted near the center of gravity of each actuator.
(4) An acceleration sensor, an angular acceleration sensor, and an angular velocity sensor are mounted near the center of gravity of each actuator and near the center of gravity of the link excluding the actuator.
(5) An acceleration sensor, an angular acceleration sensor, and an angular velocity sensor are mounted near the center of gravity of each actuator, near the center of gravity of the battery, and near the center of gravity of the link excluding the battery and the actuator.

  According to (1), an acceleration component applied at each control point is directly measured for each control point, with a portion where mass is concentrated as a control point, and a moment term around the ZMP at the control point generated thereby is generated. The external force moment term applied to the control point and the moment term around the ZMP generated by the external force applied to the control point can be directly calculated for each part. Then, in the central control unit, these moment terms collected from each control point are sequentially added and their sum is taken, whereby a more exact ZMP balance equation can be directly introduced. Further, since the moment term is directly measured for each control point, strict motion control can be realized with good responsiveness without assuming the condition that the body is rigid and does not deform due to application of external force or the like.

  Here, the portion where the mass is concentrated corresponds to the center of gravity of the battery, the center of gravity of the control unit, the center of gravity of the link, the center of gravity of the actuator, the joint axis, and other mass concentrated objects. FIG. 20 shows a state in which acceleration, angular acceleration, and angular velocity sensors are mounted on a portion of the body of the legged mobile robot where the mass is concentrated. As shown in the figure, an external force sensor and an external force moment sensor are mounted on a palm and a sole as main contact portions with the outside world.

  According to (2), the acceleration component applied at each control point is directly measured for each control point, with the vicinity of the center of gravity of each link connecting the joints as a control point, and the ZMP at the control point generated by this is measured. The surrounding moment term, the external force moment term applied to the control point, and the ZMP moment term generated by the external force applied to the control point can be directly calculated for each part. Then, in the central control unit, these moment terms collected from each control point are sequentially added and their sum is taken, whereby a more exact ZMP balance equation can be directly introduced. Further, since the moment term is directly measured for each control point, strict motion control can be realized with good responsiveness without assuming the condition that the body is rigid and does not deform due to application of external force or the like.

  FIG. 21 shows a state in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each link on the body of the legged mobile robot. As shown in the figure, an external force sensor and an external force moment sensor are mounted on a palm and a sole as main contact portions with the outside world.

  Also, according to (3), the acceleration component applied at each control point is directly measured for each control point, with the vicinity of the center of gravity of each actuator as a main mass concentration portion on the fuselage as a control point. The moment term around the ZMP at the controlled point, the external force moment term applied to the control point, and the ZMP moment term generated by the external force applied to the control point can be directly calculated for each part. Then, in the central control unit, these moment terms collected from each control point are sequentially added and their sum is taken, whereby a more exact ZMP balance equation can be directly introduced. Further, since the moment term is directly measured for each control point, strict motion control can be realized with good responsiveness without assuming the condition that the body is rigid and does not deform due to application of external force or the like.

  FIG. 22 shows a state in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each actuator on the body of the legged mobile robot. As shown in the figure, an external force sensor and an external force moment sensor are mounted on a palm and a sole as main contact portions with the outside world.

  According to the distributed force sensor system as described in the above (1) to (5), the actual rotation center is determined based on the sensor information from the acceleration sensor measured at each control point. Can be measured. Therefore, unlike the case of the center of gravity uniquely determined from the design information of the aircraft, even when the aircraft such as a link is deformed by external force, it is possible to dynamically calculate a more accurate center of gravity position of the aircraft. it can.

  FIG. 23 is a flowchart illustrating a schematic processing procedure of motion control of the legged mobile robot according to the present embodiment.

  First, the stability of the body of the legged mobile robot is determined (step S51). The stability can be determined based on whether the ZMP position is in the stable region with reference to the support polygon of the fuselage.

  When the ZMP is inside the supporting polygon, no rotational motion or translational motion occurs in the system, and there is no need to solve the equation of motion related to rotation and translation. Then, the process proceeds to step S52, in which posture stability control is performed by solving the ZMP equation using an appropriate ZMP space formed by the system (described later).

  On the other hand, when there is no ZMP inside the support polygon or when there is no support action point with respect to the external world, posture stability control is performed by solving a motion equation instead of the ZMP equation (step S53) (described later). .

  If the priorities of the trajectories of all parts are set to be uniformly high, such as in a dance involving jumping, both the ZMP equation and the equation of motion may be solved.

  FIG. 24 is a flowchart showing the procedure of the aircraft body stability control based on the solution of the ZMP equation in step S52.

  First, ZMP is measured based on sensor information from acceleration sensors, angular acceleration sensors, and angular velocity sensors arranged at each control point, such as near the center of gravity of each link, near the center of gravity of each link, and near the center of gravity of each actuator. Is measured (step S61). When the body is deformed due to an external force or the like, it is necessary to dynamically measure the center of gravity based on the measured value of the acceleration sensor.

  Next, in the processing loop formed by steps S62 to S69, the moment terms around the ZMP for each control point, the external force moment term applied to the control point, and the A moment term around the ZMP generated by the external force is directly calculated based on information from a sensor disposed at the control point, and these moment terms are sequentially added to obtain a total sum of the moment terms.

  As a result, the moment error T can be calculated using the ZMP equation (step S70).

  Next, the ZMP trajectory or the rotational trajectory of the center of gravity and the trajectory of each part are re-planned using the measured state quantities of the respective parts and the identified external force moment as initial values (step S71).

  Then, the target value based on the replanning result is transmitted to the actuator system group, and the present processing routine ends.

  In the processing procedure shown in FIG. 24, i-series processing for calculating the generated moment at the control point, j-series processing for calculating the external force moment applied at the control point, and ZMP generated by the external force at the control point Including the k-system processing for calculating the surrounding moment, the i, j, and k-system processing proceeds serially, but may proceed in parallel (described later).

  FIG. 25 is a flowchart illustrating a processing procedure of aircraft stability control based on the solution of the equation of motion in step S53.

First, measure the ground reaction force F _r of the ZMP (step S81).

  Next, in the processing loop formed by steps S82 to S89, the translational force applied to each control point, the translational force applied by the moment around the ZMP, and the external force are sequentially determined from the vicinity of the ZMP or the center of gravity. Is calculated directly based on the information from the sensors disposed in the, and these translational force terms are sequentially added to obtain the sum of these.

  As a result, the unknown external force F can be calculated based on the d'Alembert principle (step S90).

  Next, the ZMP trajectory or the center of gravity trajectory and the trajectory of each part are re-planned using the measured information amount of each part and the identified unknown external force as initial values (step S91).

  Then, the target value based on the replanning result is transmitted to the actuator system group, and the present processing routine ends.

  In the processing procedure shown in FIG. 25, the i-system processing for calculating the translational force at the control point, the j-system processing for calculating the translational force generated by the external force moment at the control point, and the processing applied at the control point Including the k-system processing for calculating the external force, the i, j, and k-system processing proceeds serially, but may proceed in parallel (described later).

  In the processing loop formed in steps S62 to S69 in the flowchart shown in FIG. 24, the moment terms around ZMP for each control point, the external force moment term applied to the control point, and A moment term around the ZMP generated by an external force applied to the control point is directly calculated based on information from a sensor disposed for each control point, and these moment terms are sequentially added. , The ZMP equation can be efficiently introduced.

  Similarly, in the processing loop formed in steps S82 to S89 in the flowchart shown in FIG. 25, the translation force applied to each control point and the translation applied by the moment around the ZMP in order from the vicinity of the ZMP or the vicinity of the center of gravity. The force and the external force are directly calculated based on the information from the sensors arranged for each control point, and these translational force terms are sequentially added to obtain the sum of the translational force terms to obtain the translational / rotational motion. Equations can be introduced efficiently.

  As described with reference to FIGS. 20 to 22, in the legged mobile robot according to the present embodiment, the local coordinates used for control and the acceleration sensor or the angular velocity sensor for directly measuring the coordinates are controlled by the control points. It is configured to directly measure the control parameter values required for introducing the ZMP equation (or equation of motion) by arranging the acceleration sensor and the attitude sensor at each vector position used in the calculation model and further arranging the acceleration sensor and the attitude sensor. ing.

  When sensors distributed on these aircraft are connected in series, moment terms and external force terms calculated based on sensor information at each control point are sequentially added at each control point according to the connection path. By going, the sum of these can be calculated efficiently.

  FIG. 22 shows a state in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each actuator on the body of the legged mobile robot (described above). FIG. Are connected in series.

  As shown in the figure, the sensors disposed on the left and right upper limbs and the left and right lower limbs are independently connected in series so that the central control unit becomes the start point and the end point. In such a case, the calculation results based on the sensor information of the control points are sequentially added for each limb, and these are returned to the central control unit to obtain the sum, where the equations can be introduced.

  FIG. 27 shows another example for connecting sensors in series. In the example shown in the figure, the sensors arranged on the whole body are connected in a line so that the central control unit is a start point and an end point in a so-called "one-stroke" form. In the case of such a wiring configuration, the calculation result based on the sensor information at each control point is sequentially added for each control target point, and when the data is returned to the central control unit, the sum of each term has already been obtained. Thus, the control unit can easily introduce equations.

  Also, an example in which local coordinates used for control and an acceleration sensor or angular velocity sensor for directly measuring the coordinates are arranged for each control target point, and an acceleration sensor and a posture sensor are arranged at each vector position used in the calculation model As an example, we have already introduced an implementation example in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each actuator where mass is concentrated.

  FIG. 28 shows a configuration example of a joint actuator having an acceleration, angular acceleration, and angular velocity sensor mounted near the center of gravity of the unit.

  The joint actuator shown in FIG. 1 includes a motor unit including a rotor magnet, a stator having a plurality of phases of magnetic coils, a gear unit (GU) for accelerating and decelerating rotation output from the motor unit, and a motor unit. It is composed of a control unit that controls the power supplied to the power supply.

  The control unit is formed of, for example, a printed wiring board, and a sensor unit is mounted substantially at the center thereof.

  The sensor unit is arranged near the position of the two-dimensional center of gravity of the actuator unit.

  The sensor unit includes a combination of a one-axis to three-axis acceleration sensor, a one- or two-axis angular velocity sensor, and a three-axis angular velocity sensor.

  FIG. 29 schematically shows the functional configuration of the joint actuator shown in the figure. As shown in FIG. 1, the actuator 10 includes an interface unit 11, a command processing unit 12, a motor control unit 13, and a sensor signal processing unit 14.

  The interface unit 11 implements an interface protocol with the host controller.

  The command processing unit 12 processes the host command received via the interface unit 12 and transmits the processed host command to the motor control unit 13, or performs arithmetic processing on sensor information from the motor control unit 13 and the sensor signal processing unit 14 to perform interface processing. The data is returned to the host controller via the unit 12.

  The motor control unit 13 outputs a current signal for realizing the rotation of the motor according to the host command to the motor coil 15 by PWM (Pulse Width Modulation), and detects the rotational position of a rotor (not shown). The angle information from one sensor 16 is acquired.

  The sensor signal processing unit 14 processes sensor information from acceleration sensors (X to Y) and gyro sensors (pitch, roll, yaw) included in the sensor unit.

  In the present embodiment, based on information from a sensor disposed for each control point, a moment term around the ZMP for each control point and an external force moment term applied to the control point in order from near the ZMP or near the center of gravity. , And the moment term around the ZMP generated by the external force applied to the control point can be directly calculated. Similarly, a translation force applied to each control point, a translation force applied by a moment around the ZMP, and an external force, in order from the vicinity of the ZMP or the center of gravity, based on information from sensors provided for each control point. Can be calculated directly.

  Furthermore, when sensors distributed on the aircraft are connected in series, the moment term and the external force term calculated based on the sensor information at each control point are sequentially added at each control point according to the connection path. By doing so, the sum of these can be calculated efficiently.

  In the joint actuator with a built-in sensor described with reference to FIGS. 28 and 29, the command processing unit 12 includes an acceleration sensor (X to Y), a gyro sensor (pitch, Using the sensor information from the roll and yaw, the moment term and the external force term can be sequentially added at each control point according to the connection path.

  FIG. 30 shows, in the joint actuator of each control point, a moment term around the ZMP, an external force moment term applied to the control point, and a moment term around the ZMP generated by the external force applied to the control point are sequentially added. The following figure illustrates the configuration.

  As shown in the figure, the joint actuator includes, from the joint actuator at a higher rank in the connection path, the sum of the moment terms around the ZMP at the (i-1) th control point and the external force moment term at the j-1th control point. And the sum of moment terms around the ZMP generated by the external force at the (k-1) th control points. Then, based on the sensor information detected in the joint actuator, a moment term around the ZMP at the control point, an external force moment term applied to the control point, and a ZMP around the ZMP generated by the external force applied to the control point The moment terms are calculated and added to the respective sums, so that the sum of the moment terms around the ZMP at the i-th control point, the sum of the external force moment terms at the j-th control point, and the k-th control point Is output to the lower joint actuator of the connection path as the sum of the moment terms around the ZMP generated by the external force at the control point. Therefore, by sequentially repeating such addition processing according to the connection path, when the calculation result reaches the central controller, since each moment term constituting the ZMP balancing equation has already been obtained, The attitude stabilization control of the aircraft based on the ZMP stability determination standard can be realized efficiently and at high speed.

  The introduction of the ZMP balance equation includes the processing of the i-system for calculating the generated moment at the control point, the processing of the j-system for calculating the external force moment applied at the control point, and the ZMP around the ZMP generated by the external force at the control point. A k-system process for calculating a moment is included, but in the illustrated example, i-, j-, and k-system processes proceed in parallel. In a system in which i, j, and k processes proceed in parallel, there is an advantage that the number of wirings is small. In particular, it is not necessary for each control point to have all the elements of the i, j, and k systems, and only the operation of the i system or the operation of the i-1 system without the operation of the i system is simply passed through. Design is also possible.

  FIG. 31 illustrates a configuration in which a translation force term applied to a control point, a translation force term applied by a moment around ZMP, and an external force term are sequentially added in the joint actuator at each control point. are doing.

  As shown in the figure, the joint actuator includes, from the joint actuator on the upper side of the connection path, the sum of the translational force terms applied to the (i-1) th control points, the ZMP rotation at the j-1th control points. , And the sum of the external force terms applied to the (k−1) th control points. Then, based on the sensor information detected in the joint actuator, a translation force term applied to the control point, a translation force term applied by a moment around the ZMP, and an external force term are calculated, and these are respectively calculated. The summation is performed on the summation, and the summation of the translational force terms applied to the i-th control point, the summation of the translational force terms applied by the moment around ZMP at the jth control point, and the control up to the kth control point The sum of the external force terms applied to the points is output to the lower joint actuator of the connection path. Therefore, by sequentially repeating such addition processing according to the connection path, when the calculation result reaches the central controller, since each translational force term constituting the equation of motion has already been obtained, It is possible to realize efficient and high-speed attitude stabilization control of the aircraft using the equation of motion.

  To introduce the equation of motion, i-system processing for calculating the translation force at the control point, j-system processing for calculating the translation force generated by the external force moment at the control point, and calculation of the external force applied at the control point However, in the illustrated example, the i, j, and k processes proceed in parallel. In a system in which i, j, and k processes proceed in parallel, there is an advantage that the number of wirings is small. In particular, it is not necessary for each control point to have all the elements of the i, j, and k systems, and only the operation of the i system or the operation of the i-1 system without the operation of the i system is simply passed through. Design is also possible.

  The present invention has been described in detail with reference to the specific embodiments. However, it is obvious that those skilled in the art can modify or substitute the embodiment without departing from the spirit of the present invention.

  The gist of the present invention is not necessarily limited to products called “robots”. That is, if it is a mechanical device or other general mobile device that performs a motion similar to a human motion by using an electric or magnetic action, a product belonging to another industrial field such as a toy, for example. However, the present invention can be similarly applied.

  In short, the present invention has been disclosed by way of example, and the contents described in this specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims described at the beginning should be considered.

FIG. 1 is a view showing a state in which a legged mobile robot provided for carrying out the present invention stands upright, as viewed from the front. FIG. 2 is a view showing a state in which the legged mobile robot provided for carrying out the present invention stands upright, as viewed from the rear. FIG. 3 is a diagram schematically illustrating a joint degree of freedom configuration included in the legged mobile robot. FIG. 4 is a diagram schematically illustrating a control system configuration of the legged mobile robot 100. FIG. 5 is a diagram illustrating a multi-mass approximation model of the legged mobile robot 100. FIG. 6 is an enlarged view of the periphery of the waist of the multi-mass model. FIG. 7 is a flowchart illustrating a processing procedure for generating a body motion capable of stably walking in the legged mobile robot 100. FIG. 8 is a flowchart illustrating a modified example of a processing procedure for generating a body motion capable of stably walking in the legged mobile robot 100. FIG. 9 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 10 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 11 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 12 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 13 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 14 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 15 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 16 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 17 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 18 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 19 is a diagram for explaining a method of setting the priority order of the desired trajectory in the upper body. FIG. 20 is a diagram illustrating a state in which acceleration, angular acceleration, and angular velocity sensors are mounted on a portion of the body of the legged mobile robot where the mass is concentrated. FIG. 21 is a diagram showing a state in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each link on the body of the legged mobile robot. FIG. 22 is a diagram illustrating a state in which acceleration, angular acceleration, and angular velocity sensors are mounted near the center of gravity of each actuator on the body of the legged mobile robot. FIG. 23 is a diagram showing a schematic processing procedure of the motion control of the legged mobile robot in the form of a flowchart. FIG. 24 is a flowchart showing a processing procedure of aircraft stability control based on the solution of the ZMP equation. FIG. 25 is a flowchart showing a processing procedure of the aircraft stability control based on the solution of the equation of motion. FIG. 26 is a diagram showing a configuration example in which sensors arranged near the center of gravity of each actuator on the body of the legged mobile robot are connected in series. FIG. 27 is a diagram showing a configuration example in which sensors arranged near the center of gravity of each actuator on the body of the legged mobile robot are connected in series. FIG. 28 is a diagram illustrating a configuration example of a joint actuator having an acceleration, an angular acceleration, and an angular velocity sensor mounted near the center of gravity of the unit. FIG. 29 is a diagram schematically showing a functional configuration of the joint actuator shown in FIG. FIG. 30 shows that the moment term around ZMP, the external force moment term applied to the control point, and the ZMP moment term generated by the external force applied to the control point are sequentially added in the joint actuator at each control point. FIG. 4 is a diagram showing a configuration for performing the operation. FIG. 31 is a diagram illustrating a configuration in which a translational force term applied to a control point, a translational force term applied by a moment around ZMP, and an external force term are sequentially added in the joint actuator at each control point. is there.

Explanation of reference numerals

DESCRIPTION OF SYMBOLS 1 ... Neck joint yaw axis 2A ... 1st neck joint pitch axis 2B ... 2nd neck joint (head) pitch axis 3 ... Neck joint roll axis 4 ... Shoulder joint pitch axis 5 ... Shoulder joint roll axis 6 ... Upper arm yaw axis 7: Elbow joint pitch axis 8: Wrist joint yaw axis 9 ... Trunk pitch axis 10 ... Trunk roll axis 11 ... Hip joint yaw axis 12 ... Hip joint pitch axis 13 ... Hip joint roll axis 14 ... Knee joint pitch axis 15 ... Ankle joint pitch Axis 16: Ankle joint roll axis 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: Shin unit 80: Control unit, 81: Main control unit 82: Peripheral circuit 91, 92: Ground contact confirmation sensor 93, 94: Acceleration sensor 95: Attitude sensor 9 ... acceleration sensor 100 ... legged mobile robot

Claims (13)

A mobile device having one or more movable parts,
A first acceleration sensor that sets a local coordinate system on the mobile device and measures an acceleration at the origin of the local coordinate system;
A second acceleration sensor installed at one or more control target points in the local coordinate system;
Relative acceleration calculating means for calculating a relative acceleration of the control target point with respect to an acceleration of the origin of the local coordinate system, based on acceleration information obtained from at least the first acceleration sensor and the second acceleration sensor;
Control means for controlling the control target point based on the relative acceleration,
A mobile device comprising: Providing the local coordinate system and the first acceleration sensor at a contact portion between the mobile device and a road surface;
The mobile device according to claim 1, wherein: The mobile device is a robot device having a foot,
Providing the local coordinate system and the first acceleration sensor on the foot;
The mobile device according to claim 1, wherein: The mobile device is a robot device having a waist,
Providing the local coordinate system and the first acceleration sensor on the waist;
The mobile device according to claim 1, wherein: An unknown moment or unknown external force applied to the mobile device is calculated using a ZMP equation or a motion equation derived based on acceleration information obtained from at least the first acceleration sensor and the second acceleration sensor. Further comprising means,
The control means controls the control target point based on the calculated unknown moment or unknown external force,
The mobile device according to claim 1, wherein: The mobile device is a robot device having a movable portion including at least upper limb, lower limb, and trunk,
At least the upper limb, the lower limb, a control target point is provided for each torso,
The mobile device according to claim 1, wherein: The acceleration sensors distributed and arranged on the mobile device are connected in series, and a moment term and an external force on the ZMP equation or the equation of motion calculated at each control point based on acceleration information of the acceleration sensor. Term is sequentially added at each control target point according to the connection path,
The mobile device according to claim 6, wherein: A method of controlling a mobile device having one or more movable parts,
A first acceleration measurement step of setting a local coordinate system on the mobile device and measuring a first acceleration at the local coordinate system origin;
A second acceleration measurement step of measuring a second acceleration at one or more control target points installed in the local coordinate system;
A relative acceleration calculating step of calculating a relative acceleration of the control target point with respect to an acceleration of the local coordinate system origin based on at least the first acceleration and the second acceleration;
A control step of controlling the control target point based on the relative acceleration,
A method for controlling a mobile device, comprising: In the first acceleration measuring step, the first acceleration is measured by setting the local coordinate system at a contact portion between the mobile device and a road surface,
The method of controlling a mobile device according to claim 8, wherein: The mobile device is a robot device having a foot,
In the first acceleration measuring step, the local coordinate system is provided on the foot to measure the first acceleration,
The method of controlling a mobile device according to claim 8, wherein: The mobile device is a robot device having a waist,
In the first acceleration measurement step, the local coordinate system is provided on the waist to measure the first acceleration,
The method of controlling a mobile device according to claim 8, wherein: An unknown moment or unknown external force calculating step of calculating an unknown moment or unknown external force applied to the mobile device by using a ZMP equation or an equation of motion derived based on at least the first acceleration and the second acceleration. Further comprising
In the control step, the control target point is controlled based on the calculated unknown moment or unknown external force,
The method of controlling a mobile device according to claim 8, wherein: The mobile device is a robot device having a movable portion including at least upper limb, lower limb, and trunk,
At least the upper limb, the lower limb, a control target point is provided for each torso,
The method of controlling a mobile device according to claim 8, wherein:

Images