JP4492395B2 - Legged robot and its motion control method - Google Patents

Description

  The present invention relates to a legged robot and a method for controlling its operation.

A robot has been developed that includes at least one leg link, swings out the leg link in the direction of movement, and moves repeatedly by using the leg link that has been swung out and grounded as a support leg.
It is known that the legged robot as described above can realize stable walking by controlling using a ZMP (Zero Moment Point). ZMP means a point on the floor where the sum of moments due to external force (including inertial force) acting on the robot becomes zero.
Even when only one leg link is in contact with the ground, if the ZMP exists in the foot of the support leg, the robot will not fall down. For example, in the case of a robot having two leg links, the robot moves by swinging forward with one leg link as a supporting leg and the other leg link as a free leg. If the ZMP is present in the foot of the support leg during this one-foot grounding state, the robot will not fall down. If the leg link that was used as a free leg is grounded and the two leg links are grounded, the ZMP will move from the foot of the leg link that was the supporting leg to the foot of the newly grounded leg link. The robot does not fall down, and the leg link that has been the supporting leg can be swung forward. If the ZMP is present in the foot of the new support leg when the leg link that has been the support leg is swung forward, the robot will not fall down.
By repeating the above operation, the robot can continue walking without falling down.

In addition to walking, there are various modes of human actions. There is a long-awaited robot that can run, jump, and balance.
Each of the operations described above has an aerial phase (a phase in which the robot is floating in the air). In the air phase, only gravity acts on the robot, and ZMP cannot be defined. There is a need for technology to stabilize the robot before and after the aerial phase.

Techniques have been developed to allow legged robots to perform motions including the aerial phase. For example, in Patent Document 1, in the ground phase before and after the aerial phase (phase in which the robot is grounded on the floor), ZMP calculated by the ZMP equation (equation for calculating ZMP from the posture of the robot and its temporal change) A technique for calculating a center-of-gravity trajectory that matches a target ZMP is disclosed. In this technique, the trajectory of the position / posture of the toe of the leg link is set, the robot motion is divided into a plurality of motion sections, and the center of gravity trajectory is calculated for each of the divided motion sections. Among the divided motion sections, in the phase (grounding phase) in which any one leg link is grounded and used as the support leg (grounding phase), (1) the ZMP trajectory has no acceleration, (2) the center of gravity height is (3) Analytical solution of the center of gravity trajectory derived based on the assumption that the external moment is constant is prepared in advance, and the center of gravity trajectory is determined by giving the coefficient of the analytical solution in real time. calculate. When the center of gravity trajectory is calculated, a joint angle target value that realizes the center of gravity trajectory is calculated, and the joint is driven based on the calculated joint angle target value, so that the legged robot realizes the operation.
In addition, in order to feed back the actual robot motion state, the technique described in Patent Document 1 detects the actual motion state of the robot for each control cycle, and the actual position and speed of the robot's actual center of gravity and the target center of gravity. The position / velocity deviation is calculated and the subsequent center-of-gravity trajectory is updated to match the actual position / speed of the center of gravity.
In this specification, “orbit” refers to data that describes a change in position over time.

Patent Document 2 also discloses a technique for calculating a center-of-gravity trajectory in which the ZMP calculated by the ZMP equation matches the target ZMP. Also in this technique, the trajectory of the position / posture of the foot tip of the leg link is set, the robot motion is divided into a plurality of motion sections, and the center of gravity trajectory is calculated for each of the divided motion sections. In the calculation of the center of gravity trajectory, the ground phase is derived on the assumption that (1) the ZMP trajectory has no acceleration, (2) the height of the center of gravity is constant, and (3) the external moment is constant. An analytical solution for the center of gravity trajectory to be performed is prepared in advance, and parameters of the analytical solution are given in real time. Also, in the vicinity of the boundary between the no-ground state and the ground state, the above analytical solution is corrected to ensure the continuity of the center of gravity trajectory.
JP 2004-142095 A JP 2004-167676 A

  According to the techniques described in Patent Documents 1 and 2, in a robot that realizes an operation including an aerial phase, stable operation can be achieved by feeding back the actual motion state of the robot and replanning the subsequent operation of the robot. Can be realized.

However, the technique described above leaves room for further improvement.
In the techniques described in Patent Documents 1 and 2, only the center-of-gravity trajectory is updated based on the actual motion state, and the position / posture trajectory of the foottip of the leg link is not updated. The foot position and posture trajectory is set based on the gait parameters such as stride, walking period, turning angle, and environment shape, regardless of the actual robot motion state. Such motion control allows the robot to achieve the desired stride, walking cycle, and turning angle even when the actual robot motion state is slightly different from the target motion state. It is effective in.
However, when the actual robot motion state deviates significantly from the target motion state, the above-described technique cannot stabilize the robot operation. In the above-described technology, for example, even when an unexpected external force acts on the robot and the actual center-of-gravity trajectory deviates greatly from the target center-of-gravity trajectory, Not changed. Therefore, even if the robot is in an unreasonable posture, the robot tends to realize the target position / posture of the toes, and the subsequent operation tends to be unstable.
The above situation is particularly noticeable when the robot jumps from the ground phase, jumps, floats in the air, then lands and transitions to the ground phase again. When floating in the air, no matter how much the robot's joint angle is driven, the trajectory of the center of gravity of the robot cannot be changed. Even if the actual trajectory deviates from the target trajectory, the center of gravity trajectory cannot be changed until landing. However, the position where the robot will land the foot is determined in advance and is not changed to reflect the actual center of gravity trajectory. As a result, the robot takes an unreasonable posture in an attempt to maintain the ZMP after landing at the target toe position and makes the subsequent behavior unstable, or the ZMP after landing becomes the target toe position. It cannot be maintained and falls immediately after landing.

  In a robot that lands from a state of floating in the air, it is extremely important to ensure the stability of the operation immediately after landing. To do this, measure the actual motion state of the robot when it is floating in the air, and based on the measured motion state, the center of gravity trajectory of the robot until landing, the trajectory of the position / posture of the toes, and after landing A technology that can update the trajectory of the center of gravity of the robot and the trajectory of the position and posture of the toes is awaited.

  The present invention solves the above problems. The present invention relates to a legged robot that realizes a crossing, floating and landing motion, in which the actual robot motion state while floating in the air deviates significantly from the target robot motion state. However, a technique capable of stabilizing the operation after landing is provided.

The robot embodied in the present invention is a legged robot that realizes the operation of crossing, floating in the air, and landing by changing the joint angle. That robot includes means for detecting a velocity of the center of gravity of the case railroad crossing, and means for generating a scheduled trajectory of the center of gravity to the landing from takeoff based on a previously given landing time and velocity of the center of gravity of the case takeoff detected, it is generated Next, landing is made so that the planned ZMP trajectory after landing matches the target ZMP trajectory given in advance with the planned center of gravity position and planned speed at the scheduled landing time obtained from the planned center of gravity trajectory up to the final landing as initial values. Means for determining a planned landing position of a toe that is scheduled to be generated and generating a planned center-of-gravity trajectory after landing, and a foot that is scheduled to land next until landing so as to be the planned landing position at the scheduled landing time means for generating a previous scheduled trajectory, means for generating a chronological data of target values of the joint angles of up to landing based on calendar appointments pathway of the tip of the foot of the center of gravity trajectory and leg link to the generated landing Means for generating a chronological data of target values of the joint angle after landing on the basis of the generated predetermined landing position of the planned trajectory of the center of gravity and foot after landing, chronological data of the generated target values of the joint angles And a means for rotating the joint based on. The time-lapse data of the target value of the joint angle until landing and after landing is generated while the legged robot is floating in the air after the crossing. In the following description, the planned center of gravity trajectory and the planned toe trajectory may be simply referred to as the center of gravity trajectory and the toe trajectory.

The legged robot described above detects the center of gravity speed at the time of crossing, and generates a center of gravity trajectory from the level crossing to the landing based on the detected center of gravity speed. Thereby, even if the actual motion of the robot in the aerial phase is significantly different from the target motion, it is possible to generate a center of gravity trajectory that matches the actual center of gravity speed.
Then, the legged robot described above generates a center of gravity trajectory after landing based on the generated center of gravity trajectory from the crossing to the landing. Since the center of gravity trajectory after landing is generated based on the center of gravity trajectory based on the measured actual center of gravity speed, it is possible to realize stable operation that allows the center of gravity to sink to alleviate the landing impact. Become.
The legged robot described above generates a trajectory of the foot of the leg link up to the landing based on the generated center of gravity trajectory after landing. As a result, even if the actual motion of the robot in the aerial phase is significantly different from the target motion, the center of gravity trajectory in the aerial phase that matches the actual center of gravity velocity and the center of gravity trajectory after landing that matches the center of gravity trajectory From, the trajectory of the foot of the leg link in the aerial phase is generated. The landing position of the leg link is generated based on the center of gravity trajectory at the time of the actually measured center of gravity speed, so the robot will not take an excessive posture and will continue to operate stably after landing. Can do.
The above legged robot generates temporal data of joint angle target values of each joint in the aerial phase based on the center of gravity trajectory and the leg link foot trajectory in the aerial phase generated as described above. Then, based on the center of gravity trajectory after landing generated as described above, the temporal data of the joint angle target value of each joint after landing is generated, and based on the temporal data of the generated joint angle target value Rotate the joints. In the ground contact phase after landing, the foot links of the leg links are in contact with the ground and do not move relative to the floor.

  The legged robot described above updates the center-of-gravity trajectory from the level crossing to the landing and the center-of-gravity trajectory after landing based on the center-of-gravity velocity detected at the level crossing, and the leg link of the leg link to the landing according to the updated center-of-gravity trajectory. The toe trajectory can be updated. As a result, even if the actual center-of-gravity orbit in the aerial phase deviates significantly from the target center-of-gravity orbit, the position at which the leg link lands is updated according to the actual center-of-gravity orbit and the operation after landing is stabilized. Can be realized.

  In the legged robot described above, the means for generating the center of gravity trajectory after landing is generated based on the vertical trajectory of the center of gravity after landing from the level crossing to the landing, and means for generating a vertical trajectory of the center of gravity after landing Based on the vertical trajectory of the center of gravity after landing, the ternary equation obtained by discretizing the ZMP equation, the target relative ZMP after landing, and the horizontal speed of the center of gravity at the time of landing and a predetermined time after landing And means for calculating the horizontal trajectory of the center of gravity after landing.

  The legged robot includes a means for generating a vertical trajectory of the center of gravity after landing, and can define a vertical movement that realizes a sinking operation after landing. In the legged robot according to the present invention, the horizontal trajectory in which the ZMP matches the target ZMP is calculated in consideration of the vertical trajectory. If a vertical trajectory is defined and a horizontal trajectory is assumed, a change in the posture of the robot with time is defined, inertial force can be calculated, and ZMP can be calculated. Utilizing this relationship, mathematically, it is possible to calculate a horizontal trajectory in which the ZMP calculated from the above relationship matches the target ZMP. In order to calculate the ZMP from the vertical trajectory and the horizontal trajectory, vertical acceleration and velocity, and horizontal acceleration and velocity are required. If the vertical trajectory and the horizontal trajectory are discretized at unit time intervals, the velocity can be calculated from two position coordinates at the start and end of the unit time. The acceleration can be calculated from three position coordinates at the start, middle and end of two consecutive unit times. For example, the times discretized by the unit time Δt are t1, t2, t3,. The speed at time t3 can be calculated from the two position coordinates at time t2 and time t3. The acceleration at time t3 can be calculated from the three position coordinates at time t1, time t2, and time t3. However, the above is an example, and the speed at time t2 may be calculated from the two position coordinates at time t2 and time t3, and the acceleration at time t2 is calculated from the three positions at time t1, time t2, and time t3. You may calculate from a coordinate. What is important is that the ZMP can be calculated from three position coordinates. In the above example, ZMP at time t3 can be calculated from three position coordinates at time t1, time t2, and time t3. Alternatively, the ZMP at the time t2 can be calculated from the three position coordinates at the time t1, the time t2, and the time t3 depending on how to take the reference. It can also be calculated from two position coordinates. What is important is that the ZMP can be calculated from the position coordinates at three times.

In this specification, an equation for calculating ZMP from the position coordinates of three times discretized in unit time is referred to as “a ternary equation obtained by discretizing a ZMP equation”. By using this ternary equation, it is possible to calculate a horizontal trajectory that realizes a ZMP that matches the target ZMP. It becomes possible to calculate the horizontal position of the center of gravity for each time discretized in unit time.
The robot of the present invention includes means for calculating a horizontal trajectory in consideration of a vertical trajectory, and if a vertical motion that realizes a sinking operation after landing is defined, the center of gravity assuming the vertical motion Calculate the trajectory. When realizing the operation of sinking after landing, it is possible to prevent the ZMP from deviating from the target ZMP and the robot falling.
Boundary conditions are required to calculate a horizontal trajectory that realizes a ZMP that matches the target ZMP. In the present invention, the horizontal speed of the center of gravity at the start of the ground phase after landing (at the time of landing) and at the time of completion (a predetermined time after landing) is used as the boundary condition. A horizontal trajectory that satisfies this boundary condition is calculated, and a horizontal trajectory in which the center of gravity speed continues before and after landing can be calculated.
The predetermined time point after landing may be any time point as long as the landing leg link is continuously grounded as a support leg from the landing. For example, in the case of realizing an operation of sinking after landing, the predetermined time after landing can be the time of completion of the sinking operation. When realizing the operation of sinking after landing, it is possible to prevent the ZMP from deviating from the target ZMP and the robot falling.
According to the legged robot described above, it is possible to define a vertical movement that realizes a sinking operation performed after landing in order to reduce the landing impact.

  As described above, when the “ternary equation obtained by discretizing the ZMP equation” is used, it is possible to calculate a horizontal trajectory that realizes a ZMP that matches the target ZMP. In this case, a tridiagonal matrix having coefficients calculated from the vertical orbit of the center of gravity after landing, a sequence of horizontal orbits of the center of gravity after landing, the target relative ZMP after landing, and the time of landing Means for calculating a horizontal trajectory of the center of gravity after landing by solving simultaneous equations established with a sequence of distances calculated from the horizontal speed of the center of gravity at the completion of the operation at a predetermined time after landing It is preferable.

  For simultaneous equations expressed using a tridiagonal matrix, a technique for solving at high speed with a small calculation load has been developed. Therefore, in order to calculate the horizontal trajectory of the center of gravity of the legged robot, a simultaneous equation expressed using a tridiagonal matrix is generated, and the horizontal trajectory of the center of gravity is calculated by solving the simultaneous equations. Then, the horizontal trajectory can be calculated at high speed with a small calculation load.

The present invention is also embodied as a motion control method for a legged robot.
The method of the present invention is a method for controlling the operation of a legged robot that realizes the operation of crossing, floating in the air, and landing by changing the joint angle. The method includes the steps of detecting the velocity of the center of gravity of the case railroad crossing, and generating an expected trajectory of the center of gravity to the landing from takeoff based on a previously given landing time and velocity of the center of gravity of the case takeoff detected, it is generated Next, landing is made so that the planned ZMP trajectory after landing matches the target ZMP trajectory given in advance with the planned center of gravity position and planned speed at the scheduled landing time obtained from the planned center of gravity trajectory up to the final landing as initial values. Determining a planned landing position of a toe to be scheduled and generating a planned center-of-gravity trajectory after landing, and a toe that is scheduled to land next until landing so that the planned landing position is the planned landing position generating a schedule orbit, and generating a chronological data of target values of the joint angles of up to landing based on planned trajectory plan centroid trajectory and feet to the generated landing, generated landing Rotation generating a temporal data scheduled trajectory of the center of gravity and a target value of the joint angle after landing on the basis of the landing position of the foot, the joint on the basis of the time data of the generated target values of the joint angles of the And a process of performing. The time-lapse data of the target value of the joint angle until landing and after landing is generated while the legged robot is floating in the air after the crossing.

  By using the technology of the present invention, in a legged robot that realizes a crossing, floating and landing motion, the actual robot motion state while floating in the air is determined from the target robot motion state. Even if it is far off, the operation after landing can be realized stably.

The main features of the embodiments described below are listed below.
(Mode 1) A legged robot that realizes a crossing, floating in the air, and landing by changing the joint angle,
Means for detecting the center of gravity speed and the angular momentum around the center of gravity at the crossing;
Means for generating a center-of-gravity trajectory from the crossing to the landing based on the detected center-of-gravity speed at the crossing;
Means for generating a center of gravity trajectory after landing based on the generated center of gravity trajectory from crossing to landing;
Means for generating a trajectory of the foot of the leg link to the landing based on the generated center of gravity trajectory after landing;
Means for generating temporal data of the target value of the joint angle to the landing based on the generated center of gravity trajectory to the landing and the foot link of the leg link and the angular momentum around the detected center of gravity;
Means for generating temporal data of the target value of the joint angle after landing based on the generated center of gravity trajectory after landing;
A means for rotating the joint based on the generated temporal data of the target value of the joint angle;
A legged robot characterized by comprising:

With reference to the drawings, an operation control technique of the robot 2 according to an embodiment of the present invention will be described.
FIG. 1 shows an outline of the robot 2 of this embodiment. The robot 2 includes a trunk 4, a left leg link 6, a right leg link 8, a control unit 10, a controller 12, and a sensor 14.
The left leg link 6 is swingably connected to the trunk 4 at one end via a hip joint. The left leg link 6 further includes a knee joint and an ankle joint, and a foot at the tip.
The right leg link 8 is swingably connected to the trunk 4 via the hip joint. The right leg link 8 further includes a knee joint and an ankle joint, and a foot at the tip.
Each joint of the robot 2 includes actuators (not shown), and these actuators are rotationally driven by instructions from the control unit 10. Reference points L0 and R0 are provided at the centers of the feet of the left leg link 6 and the right leg link 8, respectively. The reference points L0 and R0 are points that serve as a reference when generating an operation pattern of the robot 2. In the figure, G indicates the position of the center of gravity of the robot 2.
Each joint of the robot 2 includes an encoder (not shown), and these encoders output a change in the joint angle over time to the control unit 10.
The control unit 10 is a computer device having a CPU, ROM, RAM, hard disk and the like. The control unit 10 can communicate with the controller 12 and inputs a command value from the controller 12 operated by the user. The control unit 10 generates or calculates an operation pattern of the robot 2 based on a command value input from the user. The control unit 10 stores the generated motion pattern and drives each joint so as to realize the stored motion pattern. Details of the control unit 10 will be described later.
The sensor 14 includes an acceleration sensor (not shown) and a gyro sensor (not shown) whose positions and postures are fixed to the trunk 4. The acceleration sensor can measure the triaxial acceleration, and the gyro sensor can measure the angular velocity around the three axes. The positions of the measurement points of the acceleration sensor and the gyro sensor are adjusted so as to substantially coincide with the position G of the center of gravity when the robot 2 is upright and stationary. The sensor 14 is connected to the control unit 10, and measures acceleration and angular velocity according to instructions from the control unit 10, and transmits them to the control unit 10.

FIG. 2 illustrates an outline of the operation realized by the robot 2 of the present embodiment. The robot 2 according to the present embodiment operates in a grounding phase using the left leg link 6 as a support leg from the posture A1, swings the right leg link 8 forward, and shifts to the posture A2. The robot 2 takes off in the posture A2 and shifts to the aerial phase. The robot 2 lands from the right leg link 8 in the posture A4 through the posture A3. The robot 2 operates from the posture A4 in the ground phase with the right leg link 8 as the supporting leg, swings the left leg link 6 forward, and moves to the posture A5. The robot 2 takes off at posture A5 and moves to the aerial phase. The robot 2 lands from the left leg link 6 in the posture A7 through the posture A6. The robot 2 operates from the posture A7 in the ground phase using the left leg link 6 as the supporting leg, swings the right leg link 8 forward, and shifts to the posture A8. In posture A8, the support leg is switched from the left leg link 6 to the right leg link 8 and operates in the grounding phase from the posture A8 with the right leg link 8 as the support leg, and the left leg link 6 is swung forward to move to the posture A9. To do.
A broken line in FIG. 2 indicates a rough trajectory of the center of gravity of the robot 2 in the above-described operation mode. In the contact phase immediately before the robot 2 jumps (that is, the contact phase from posture A1 to posture A2 and the contact phase from posture A4 to posture A5), the center of gravity is sunk in preparation for the subsequent jump. Also, in the ground contact phase immediately after landing of the robot 2 (that is, the ground contact phase from posture A4 to posture A5 and the ground contact phase from posture A7 to posture A8), the center of gravity is submerged to reduce the landing impact caused by jumping. Yes. The operation in the contact phase from the posture A4 to the posture A5 serves as both the sinking of the center of gravity for mitigating the landing impact and the sinking of the center of gravity for preparation for jumping.
The above-described operation mode is shown for illustrative purposes only, and the robot 2 of the present embodiment can realize various operations combining the ground phase and the air phase. The operation mode realized by the robot 2 can be arbitrarily set by the user. For example, it may include an action of performing a fight with one leg.

In the robot 2 of the present embodiment, the mode of motion to be generated is divided into several motion sections, and motion patterns in each motion section are generated and stored in advance. In the robot 2 according to the present embodiment, in the mode of motion to be generated, the leg link serving as the support leg is grounded and the leg link is grounded after the leg link serving as the support leg is grounded and the ZMP is positioned in the foot. Up to the time when the ZMP is located in the plane is defined as one operation section. In such a case, one operation section is constituted by the ground phase and the subsequent air phase or only the ground phase.
In the case of dealing with the operation mode shown in FIG. 2, from the start time of the first ground phase (that is, the time point of the posture A1 and the start of the sinking operation), the completion time of the air phase following the ground phase (that is, the time point) Section 1 is set to section 1 until the time of posture A4, and section 2 is from the start time of the ground phase following section 1 (that is, the time of position A4) to the completion time of the aerial phase following that ground phase (that is, the position of posture A7). The section from the start time of the ground phase following section 2 (ie, the time of attitude A7) to the completion time of the ground phase (ie, the time of attitude A8) is section 3, and the start time of the ground phase following section 3 (ie, attitude) A section 4 is defined from the time point A8) to the time point when the grounding phase is completed (that is, the time point of the posture A9).

  In the following, a coordinate system (x, y, z) fixed to the floor outside the robot 2 is used as a coordinate system representing the trajectory of the robot 2. A coordinate system fixed to the reference point on the robot 2 is expressed by (x ′, y ′, z ′). In the robot 2 of the present embodiment, as shown in FIG. 2, the center of the foot of the support leg is set as a reference point of the coordinate system (x ′, y ′, z ′). In the aerial phase, no leg link is grounded, and there is no leg link that supports the robot 2, but for convenience, a leg link that becomes a support leg when landing is called a support leg in the aerial phase. In the robot 2 of the present embodiment, the center point of the foot of the support leg is set as the reference point in the aerial phase as in the ground phase. In the aerial phase, not only the center of gravity of the robot 2 but also the reference point moves with respect to the floor. In the following, the position, velocity, acceleration and trajectory described in the coordinate system (x ', y', z ') fixed to the reference point on the robot 2 are respectively referred to as relative position, relative velocity, relative acceleration and relative trajectory. Express. A ZMP described in the coordinate system (x ′, y ′, z ′) is expressed as a relative ZMP.

The robot 2 of the present embodiment generates and stores motion patterns for each motion section, and sequentially reads the stored motion patterns to realize the motion.
When a command value is input from the user during the operation of the robot 2, the robot 2 generates a subsequent operation pattern again and stores the operation pattern corresponding to the newly input command value. The stored new operation pattern is reflected in subsequent operations.
As described above, the robot 2 of the present invention continues the operation while updating the operation pattern every time a command value from the user is input.
Further, when the robot 2 of this embodiment switches from the ground phase to the aerial phase, it detects the actual center-of-gravity speed and angular momentum around the center of gravity, generates the subsequent motion pattern again, and determines the actual center-of-gravity speed and An operation pattern corresponding to the angular momentum around the center of gravity is stored. The stored operation pattern is reflected in subsequent operations.

Details of the operation of the control unit 10 will be described below with reference to the block diagram of FIG. 3 and the flowcharts of FIGS. 4 and 7.
FIG. 3 is a block diagram illustrating a functional configuration of the control unit 10. The control unit 10 includes a user command value storage unit 302, an aerial phase centroid vertical trajectory generation unit 304, a ground phase centroid vertical trajectory generation unit 306, a ground phase centroid horizontal trajectory calculation unit 308, and an aerial phase centroid horizontal trajectory calculation unit 310. And a gravity center relative trajectory calculation unit 314, a free leg relative trajectory calculation unit 316, a joint angle target value calculation unit 318, a joint drive unit 320, a sensor control unit 354, and a target angular momentum update unit 362. . Here, since the center of gravity trajectory is calculated and generated, the terms “calculation”, “generation”, and “calculation” are used without distinction. Since the vertical trajectory of the center of gravity is calculated directly from the user command value, the generated word is used, and the horizontal trajectory of the center of gravity is calculated so as to satisfy the ZMP equation described later. However, technically, there is no particular distinction between generation and calculation.

The user command value storage unit 302 stores a user command value 352 input from the user. The user command value storage unit 302 includes (1) an operation mode 321, (2) a jump height 322 in the aerial phase, (3) a center of gravity height 324 in switching the operation, and (4) a ground phase duration 326. , (5) Relative ZMP trajectory 328 in the ground phase, (6) Time-dependent data 330 of the target angular momentum, (7) Horizontal velocity 332 of the center of gravity in switching the operation, (8) Free leg toe at crossing Data such as position and speed 334 are stored. Details of these data will be described later.
When a new user command value 352 is input during the operation of the robot 2, the control unit 10 replaces the user command value stored in the user command value storage unit 302 with the newly input user command value. When a user command value is newly input, the control unit 10 performs a process of generating a subsequent operation pattern. The new command value input from the user is reflected in the generation of the motion of the robot 2 thereafter. The user can control the operation of the robot 2 in real time by inputting a user command value using the controller 12 such as a joystick while visually recognizing the motion of the robot 2.

  The aerial phase centroid vertical trajectory generation unit 304, the ground phase centroid vertical trajectory generation unit 306, the ground phase centroid horizontal trajectory calculation unit 308, and the aerial phase centroid horizontal trajectory calculation unit 310 are stored in the user command value storage unit 302. The trajectory of the center of gravity of the robot 2 in each operation section is generated based on the user command value, the center-of-gravity vertical speed 356 at the crossing measured by the sensor control unit 354, and the center-of-gravity horizontal speed 358 at the crossing. The trajectory of the center of gravity generated here is described in the coordinate system (x, y, z) fixed outside the robot 2 in each motion section, but in the ground phase, the reference point of the robot 2 Since it does not move with respect to the floor (located on the foot of the support leg), it can be handled as data described in the coordinate system (x ′, y ′, z ′) fixed to the reference point of the robot 2 . Details of processing performed in each trajectory generation unit and calculation unit will be described later.

  The aerial phase center of gravity vertical trajectory 336 is data describing the vertical direction trajectory of the center of gravity in the aerial phase of the robot 2 in each motion section, and a code indicating the order of the motion sections and the center of gravity in the aerial phase in the motion section. A vertical trajectory is associated. The aerial phase centroid vertical trajectory 336 is updated each time the aerial phase centroid vertical trajectory generation unit 304 generates a vertical centroid trajectory in a certain motion section.

The aerial phase center of gravity horizontal trajectory 342 is also data similar to the aerial phase center of gravity vertical trajectory 336 and describes the horizontal direction trajectory of the center of gravity in the aerial phase of the robot 2 in each motion section, and a code indicating the order of the motion sections; The horizontal trajectory of the center of gravity in the aerial phase in the operating section is associated. The aerial phase centroid horizontal trajectory 342 is updated each time the aerial phase centroid horizontal trajectory calculation unit 310 calculates the horizontal trajectory of the centroid in a certain operation section.
The ground phase centroid vertical trajectory 338 and the ground phase centroid horizontal trajectory 340 also describe the trajectory of the center of gravity in the ground phase of the robot 2 in each operation section in the same manner as the air phase centroid vertical trajectory 336 and the air phase centroid horizontal trajectory 342 described above. It is data to be. In these data, a code indicating the order of the operation sections is associated with the trajectory of the center of gravity in the ground phase in the operation sections. These data are updated every time a trajectory of the center of gravity in a certain motion section is generated by the corresponding trajectory generator or calculator.

  The center-of-gravity relative trajectory calculation unit 314 is generated by the aerial phase center-of-gravity vertical trajectory generation unit 304, the ground-phase center-of-gravity vertical trajectory generation unit 306, the ground-phase center-of-gravity horizontal trajectory calculation unit 308, and the aerial phase center-of-gravity horizontal trajectory calculation unit 310. Based on the trajectory of the center of gravity of the robot 2 and the free leg relative trajectory 348 calculated by the free leg relative trajectory calculation unit 316, the center of gravity relative trajectory 346 of the robot 2 is calculated.

  The center-of-gravity relative trajectory 346 describes the center-of-gravity trajectory in a coordinate system (x ′, y ′, z ′) with the reference point provided at the tip of the support leg of the robot 2 as the origin. The center-of-gravity relative trajectory 346 is associated with a code indicating the order of the motion sections and the relative trajectory of the center of gravity in the motion sections. The center-of-gravity relative trajectory calculation unit 314 updates the center-of-gravity relative trajectory 346 every time the center-of-gravity relative trajectory is calculated in a certain motion section.

The free leg relative trajectory calculation unit 316 is based on the center of gravity relative trajectory 346 of the robot 2 calculated by the center of gravity relative trajectory calculation unit 314 and the user command value stored in the user command value storage unit 302. A relative trajectory 348 of the leg tip is calculated. The relative trajectory 348 of the toe of the free leg calculated here is described in a coordinate system (x ′, y ′, z ′) with the reference point provided at the toe of the supporting leg of the robot 2 as the origin. Is.
The free leg relative trajectory 348 is data describing the relative trajectory of the reference point of the toe of the free leg of the robot 2 in each motion section, and includes a code indicating the order of the motion sections and the leg of the free leg in the motion section. The relative trajectory of the previous reference point is associated. The free leg relative trajectory 348 is updated each time the free leg relative trajectory calculation unit 316 calculates the relative trajectory of the free leg in a certain motion section.

The joint angle target value calculation unit 318 includes a reference point of the toe of the free leg of the robot 2 calculated by the relative gravity center trajectory 346 of the robot 2 calculated by the relative gravity track calculation unit 314 and the free leg relative trajectory calculation unit 316. Based on the relative trajectory 348, the temporal data 350 of the joint angle target value of each joint of the robot 2 is calculated.
The joint angle target value temporal data 350 is temporal data of the target joint angle of each joint of the robot 2 and is associated with a code indicating the order of the corresponding motion sections. Each time the joint angle target value calculation unit 318 performs the calculation, the joint angle target value temporal data 350 is updated.

  The joint driving unit 320 drives each joint of the robot 2 based on the temporal data 350 of the joint angle target value.

  The sensor control unit 354 transmits an instruction to the sensor 14 when the robot 2 takes a step from the ground phase and shifts to the aerial phase, and causes the acceleration and the angular velocity at the time of the crossing to be measured. When measured values of acceleration and angular velocity are input from the sensor 14, the sensor control unit 354 calculates the center-of-gravity vertical velocity 356, the center-of-gravity horizontal velocity 358, and the angular momentum 360 around the center of gravity at the time of the crossing.

  The target angular momentum update unit 362 updates the target angular momentum temporal data 330 stored in the user command value storage unit 302 based on the angular momentum 360 at the time of crossing output from the sensor control unit 354.

  With reference to FIG. 4, a motion generation process performed by the control unit 10 of the robot 2 of the present invention will be described. The process of FIG. 4 can handle an operation of an arbitrary mode. Hereinafter, a case of handling the mode of operation shown in FIG. 2 will be described as an example.

  First, in step S402, a variable k indicating the number of an operation section for generating an operation is set to 1.

Next, in step S404, a trajectory of the center of gravity of section 1 which is the first motion section is generated.
The generation of the center-of-gravity trajectory of the section 1 performed in step S404 is the same as the generation of the center-of-gravity trajectory in the general section k described later in steps S408 to S420, and step S404 performs the processing from step S408 to step S420. It is expressed collectively. Details of the process of generating the center of gravity trajectory will be described later.
By executing the process of step S404, the vertical trajectory and the horizontal trajectory of the center of gravity in the section 1 are generated. As shown in FIG. 2, the section 1 has a ground phase and an air phase. The generated center-of-gravity orbits of the section 1 are reflected in the aerial phase center-of-gravity vertical track 336, the ground-phase center-of-gravity vertical track 338, the ground-phase center-of-gravity horizontal track 340, and the aerial phase center-of-gravity horizontal track 342.

  In step S406, 1 is added to the number k indicating the section for generating the motion.

In step S <b> 408, the control unit 10 reads the user command value related to the operation in the section k in which the operation is to be generated from the user command value storage unit 302. The following parameters can be given as user command values.
(1) Operation mode 321 of the robot 2
(2) The jump height 322 of the center of gravity in the air phase
(3) Height of center of gravity (vertical position) 324 in switching of operation
(4) Duration of ground phase 326
(5) Relative ZMP trajectory 328 in the ground phase
(6) Target angular momentum temporal data 330 in the ground phase and the air phase
(7) Horizontal velocity 332 of the center of gravity in switching the operation
(8) Position and velocity 334 of the free leg foot at the time of crossing

  The operation mode 321 of the robot 2 in (1) above shows the characteristics of the operation of the robot 2 in each operation section. The operation mode 321 of the robot 2 includes a code indicating the order of the motion sections, a code indicating the presence / absence of an aerial phase in each motion section, and a code for identifying a leg link that is a support leg in the ground phase of each motion section. And can be given arbitrarily by the user.

  The jump height of the center of gravity in the aerial phase (2) is the maximum height at which the center of gravity of the robot 2 reaches when the operation of the robot 2 includes the aerial phase, and the performance of the actuator of the robot 2 permits. Within the range, the user can arbitrarily give.

  The height (vertical direction position) 324 of the center of gravity in the switching of the operation of (3) above is determined when the operation of the robot 2 is switched between the ground phase and the air phase or between the ground phase and the ground phase. The height can be realized by the center of gravity of 2, and can be arbitrarily given by the user within a range that the mechanism of the robot 2 allows geometrically.

  The duration 326 of the ground phase in (4) above is a time during which the ground phase continues in each operation section, and can be arbitrarily given by the user.

The relative ZMP trajectory in the grounding phase of (5) above is the time-dependent data of the position of the target ZMP in the grounding phase with respect to the foot reference point of the supporting leg, and is present in the foot of the leg link serving as the supporting leg. Can be given arbitrarily by the user under constraints. In the grounding phase, if the ZMP exists in the foot of the leg link serving as the support leg, the robot 2 can continue the operation without falling down.
In the robot 2 of the present embodiment, the relative ZMP trajectory in the ground phase is set to be fixed at the center of the foot of the support leg. When such a relative ZMP trajectory is realized, the robot 2 can stably operate without falling down. The relative ZMP trajectory in the ground phase may be any trajectory as long as it is maintained within the foot between the ground phases. For example, a track that moves from the rear to the front inside the foot of the support leg during the ground phase may be used. Even when such a relative ZMP trajectory is realized, the robot 2 can stably operate without falling down.

The target angular momentum time-dependent data 330 in the ground phase and the air phase in (6) above is the time-dependent data of the angular momentum of the rotational motion around the center of gravity realized by the robot 2 and within the range permitted by the performance of the actuator. Can be given arbitrarily by the user. The angular momentum about the roll axis (x ′ axis) and the pitch axis (y ′ axis) of the robot 2 can be set so that the robot 2 realizes a desired posture. The angular momentum around the yaw axis (z ′ axis) can be set so that the robot 2 realizes a desired turning motion.
In the robot 2 of the present embodiment, the angular momentum about the roll axis, the pitch axis, and the yaw axis is set to 0 in all the motion sections.

  The horizontal direction velocity 332 of the center of gravity in the switching of the operation of (7) above is determined when the operation of the robot 2 is switched between the ground phase and the air phase, or when the operation of the robot 2 is switched between the ground phase and the ground phase. The horizontal speed achieved by the center of gravity can be arbitrarily given by the user within a range allowed by the performance of the actuator of the robot 2.

  The position and speed 334 of the free leg toe at the time of the crossing in (8) above are the position and speed of the free leg toe as seen from the supporting leg toe when the robot 2 transitions from the ground phase to the aerial phase. Thus, the user can arbitrarily give it as long as the mechanism of the robot 2 geometrically allows.

In step S410 of FIG. 4, it is determined whether or not the operation in the section k includes an aerial phase. If section k includes an aerial phase (YES in step S410), the process proceeds to step S412 to generate an operation of the aerial phase in section k. If the section k does not include the aerial phase (NO in step S410), the process proceeds to step S414 to generate the operation of the ground phase of the section k.
For example, when k is 2, since the operation in the section 2 includes an aerial phase, the process proceeds to step S412.

In step S412, a vertical orbit of the center of gravity in the aerial phase of section k is generated.
In the aerial phase, the robot 2 is floating in the air, and only gravity acts on the robot 2 as an external force. Therefore, no matter how the joint of the robot 2 is driven in the air phase, the trajectory of the center of gravity is the trajectory shown below.

In the above equation, g is the gravitational acceleration, v _{z0 in} the above equation is the vertical speed of the center of gravity at the time of crossing in the section k, and z ₀ is the vertical position of the center of gravity at the time of crossing. By giving the above v _z0 and z ₀ , the vertical trajectory of the center of gravity in the air phase can be generated.
The above z ₀ is the height of the center of gravity at the time of the crossing, and is given as a user command value in step S408.
Also, the above-mentioned v _z0 is the vertical speed of the center of gravity at the time of crossing. This v _z0 can be calculated from the jump height of the user command value given in step S408. The vertical velocity v _z0 of the center of gravity at the time of the crossing is calculated as follows using the jump height h and the gravitational acceleration g.

Instead of inputting the jump height h as a user command value, v _z0 may be directly given as a user command value.

By performing step S412, the vertical trajectory of the center of gravity of the robot 2 in the aerial phase of the section k can be generated.
FIG. 5A schematically shows the vertical trajectory of the center of gravity of the robot 2. For example, if k is 2, the vertical phase trajectory 102 of the aerial phase of section 2 is generated in step S412.
Further, the time during which the aerial phase continues can be calculated from the vertical trajectory 102 of the center of gravity of the robot 2 in the aerial phase generated in step S412. The calculated duration of the aerial phase is used for processing in later steps S420 to S422.

  In step S414, a vertical trajectory of the center of gravity in the ground phase of section k is generated. The vertical trajectory of the center of gravity in the ground phase of the section k is generated so as to be smoothly connected to the vertical trajectory of the center of gravity in the operation before and after the ground phase.

The vertical position and velocity of the center of gravity at the start of the grounding phase of section k are based on the vertical trajectory of the center of gravity of section k-1 that has already been generated, and the vertical position of the center of gravity at the completion of section k-1 And match the position and speed. For example, when k is 2, the vertical position and speed of the center of gravity at the time of completion 108 of the section 1 are specified based on the already generated vertical trajectory 112 of the center of gravity in the aerial phase of the section 1 and specified. The determined position and speed are the conditions at the start of the ground phase in section k.
Further, in the case of generating the vertical trajectory of the center of gravity of the section 1 in step S404, the condition at the start of the ground phase of the section k can be arbitrarily given. In the robot 2 according to the present embodiment, the vertical position of the center of gravity is set as the user-instructed height 324 as the vertical position of the center of gravity as the condition at the start of the grounding phase in the section 1, and the vertical speed of the center of gravity is set to zero.

The vertical position and speed of the center of gravity at the time of completion of the ground phase of the section k are matched with the vertical position and speed of the center of gravity at the start of the subsequent aerial phase or ground phase.
When the section k includes the aerial phase, the vertical position and velocity of the center of gravity at the completion of the ground phase can be set from the vertical orbit of the center of gravity in the aerial phase already generated in step S412. it can. For example, if k is 2, the vertical position and speed of the center of gravity at the time of completion 110 of the ground phase are calculated from the vertical direction trajectory 102 of the center of gravity in the aerial phase already generated in step S412. The vertical position and speed of the center of gravity at 110 are specified, and the specified position and speed are used as the conditions for completion of the ground phase.
If the section k does not have an aerial phase and the completion of the ground phase is connected to the start of the ground phase of the section k + 1, the vertical position of the center of gravity at the completion of the section k is determined in step S408. It can be set from the user command value given by. In this case, the vertical speed of the center of gravity can be arbitrarily given. For example, when k is 3, section 3 does not have an aerial phase, and when the vertical trajectory 114 of the ground phase center of gravity 114 is completed, the vertical trajectory of the ground phase of the ground phase of section 4 is the next operation section. 115 (not generated yet). In such a case, the center-of-gravity height 324 given as the user command value is used as it is as the vertical position of the center of gravity. The vertical speed of the center of gravity can be arbitrarily given.

Based on the start and completion conditions set as described above, a vertical trajectory of the center of gravity in the ground phase is generated using polynomial interpolation. For example, a vertical trajectory of the center of gravity in the ground phase of the section k is generated by interpolation using a quadratic or cubic polynomial with the start and completion positions and velocities as constraints. As a result, for example, when the vertical velocity at the start of the ground phase is 0 and the vertical velocity at the end of the ground phase is v _z0 , the velocity increases after sinking during the ground phase, A vertical velocity with a directional velocity of v _z0 can be generated.

  By performing the process of step S414, the vertical trajectory of the center of gravity in the ground phase of the section k can be generated. The vertical direction trajectory of the center of gravity generated by the above process is described in a coordinate system (x, y, z) fixed outside the robot 2. However, since the foot of the support leg on which the reference point on the robot 2 exists does not move with respect to the floor in the ground contact phase, the above trajectory is described as it is in the coordinate system (x ', y', z '). It can be easily replaced with something. In FIG. 5A, the trajectory described in the coordinate system (x, y, z) is indicated by a broken line, and the trajectory described in the coordinate system (x ′, y ′, z ′) is indicated by a solid line. .

In step S416, the horizontal trajectory of the center of gravity in the ground phase of the section k is calculated. The horizontal trajectory of the center of gravity is the vertical trajectory of the center of gravity in the ground phase, the target relative ZMP trajectory, the temporal data of the target angular momentum, and the center of gravity at the start and completion of the ground phase. It is determined based on the horizontal speed condition. In step S416, the horizontal trajectory of the center of gravity is calculated so that the actual relative ZMP trajectory when the vertical trajectory and the horizontal trajectory of the center of gravity coincide with the target relative ZMP trajectory.
The vertical trajectory of the center of gravity in the ground phase of the section k has already been generated in step S414. The time-dependent data of the target relative ZMP trajectory and angular momentum are given as user command values in step S408. The horizontal speed of the center of gravity at the start of the ground phase is given by specifying the horizontal speed of the center of gravity at the completion of the section k-1 from the center of gravity trajectory of the section k-1 that has already been generated. The horizontal speed of the center of gravity at the completion of the ground phase is given as a user command value in step S408. In step S420, the horizontal trajectory of the center of gravity in the ground phase is calculated so as to satisfy these conditions.

Hereinafter, calculation of the horizontal trajectory of the center of gravity in the ground phase will be described.
The relative ZMP position (p _{x ′} , p _{y ′} ) realized by the robot 2 is viewed from the coordinate system with the origin of the relative center of gravity position (x ′, y ′, z ′) of the robot 2 and the tip of the support leg. It can be calculated from the angular velocity (ω _{x ′} , ω _{y ′} ) around the center of gravity of the robot 2. The following equation for calculating the actual ZMP from the relative center of gravity position of the robot 2 and the angular velocity around the center of gravity of the robot 2 is called a ZMP equation.

Here, ⁽¹⁾ indicates the first derivative with respect to time t, and ⁽²⁾ indicates the second derivative with respect to time t. M is the mass of the robot 2.
Z ′ and z ′ ^{(2) in the} above expression are the vertical position and vertical acceleration of the center of gravity with the toe of the support leg of the robot 2 as the origin. Since the toes of the support legs of the robot 2 do not move relative to the floor in the grounding phase, z ′ and z ′ ⁽²⁾ can be specified from the vertical trajectory of the center of gravity generated in step S414.
The above formulas I _{x ′} ω _{x ′} ⁽¹⁾ and I _{y ′} ω _{y ′} ⁽¹⁾ indicate the influence of the rotational inertia of the robot 2. In this embodiment, the angular momentum about the roll axis (x ′ axis) and the pitch axis (y ′ axis) is set to 0 in all the motion sections, and I _{x ′} ω _{x ′} ⁽¹⁾ and I _{y ′} ω _{y ′.} ⁽¹⁾ is 0.

  By discretizing the above ZMP equation, an equation for calculating temporal data of the horizontal position (x ′, y ′) of the center of gravity can be derived. Discretization here is performed using the unit time Δt with respect to the time t as described above. For example, for the x ′ direction trajectory of the center of gravity, the following relational expression is obtained.

Here, x ′ _i and p _x′i are variables obtained by discretizing the _x′- direction trajectory x ′ (t) of the center of gravity and the relative ZMP trajectory p _{x ′} (t) in the x ′ direction, respectively. i indicates the order of time divided by unit time. Equation 4 shows that the x ′ direction coordinate p _x′i of the ZMP at time i is the immediately preceding x ′ direction coordinate x ′ _i−1 , the x ′ direction coordinate x ′ _{i at} that time, and the immediately following x ′ direction coordinate. It can be calculated from x ′ _{i + 1} . The coefficients a _i , b _i , and c _i are coefficients calculated as follows.

Δt is a time width used for discretization of the ZMP equation, and z ′ _i and z ′ ⁽²⁾ _i are variables obtained by discretizing the vertical position and acceleration of the center of gravity, respectively. In this embodiment, Δt is a time width that divides the duration of the ground phase into n equal parts.

The coefficient d _i is a coefficient calculated by the following.

Here, I _{y ′} ω _{y ′} ⁽¹⁾ _i is a variable obtained by discretizing the angular momentum about the target pitch axis (y ′ axis). In the robot 2 of the present embodiment, the target angular momentum is 0, so d _i is all 0 in the process shown in FIG.

In the robot 2 of the present embodiment, the operation of the robot 2 is calculated so that the trajectory of the center of gravity changes smoothly at the start and completion of the ground phase. By calculating the motion in this way, for example, even when the robot 2 performs a crossing or landing, or when the support leg is replaced, the center of gravity of the robot 2 changes smoothly, and the robot 2 performs a stable motion. Realize.
The _x′- direction velocity v _x′0 of the center of gravity at the start of the section k is expressed by the following using the division time width Δt and the positions of the center of gravity x ′ ₀ and x ′ ₁ .

The _x′- direction velocity v _x′0 of the center of gravity at the start of the section k is set from the orbit of the center of gravity of the section k−1 that has already been calculated. For example, when k is 2, the horizontal speed v _x′0 of the center of gravity at the start 108 of the section 2 is calculated as the horizontal speed of the center of gravity at the completion 108 of the section 1 from the horizontal trajectory 120 of the section 1. Identify.

_{Similarly, the x′-} direction velocity v _x′n−1 of the center of gravity at the completion of the ground phase of the section k is the division time width Δt, the positions of the center of gravity x ′ _n−2 , x ′ _n−1, and so on. There is a relationship.

The _x′- direction velocity v _x′n−1 of the center of gravity at the completion of the section k is set as a user command value in step S408.

When the above relations are arranged, after all, the realized relative ZMP trajectory becomes equal to the target relative ZMP trajectory, and the x ′ direction trajectory (x ′ ₀ , x ′ ₁ ,. .., X ′ _n−1 ) can be calculated by solving an equation (ternary equation) represented by the following tridiagonal matrix.

In the above equation, the left side is expressed by the product of a matrix and a column vector, and the matrix is a tridiagonal matrix having coefficients calculated from the vertical trajectory. The tridiagonal matrix refers to a matrix in which only a diagonal component and a sub-diagonal component adjacent to the diagonal component have significant values and the other components are zero. The column vector on the left side is a column vector indicating the x ′ direction trajectory (x ′ ₀ , x ′ ₁ ,..., X ′ _n−1 ) of the center of gravity. The column vector on the right-hand side is obtained by multiplying the target relative ZMP trajectory with correction based on the target angular momentum, and multiplying the _x′- direction velocity v _x′0 of the center of gravity at the start of the ground phase by the unit time Δt. The value v _x'0 Δt converted to distance and the value v _x'n-1 Δt converted to distance by multiplying the _x'- direction velocity v _x'n-1 of the center of gravity at the end of the ground phase by unit time Δt Is a column vector.

In Equation 8, an unknown matrix (x ′ ₀ , x ′ ₁ ,..., X ′ _n− is calculated by calculating the inverse matrix of the tridiagonal matrix on the left side and calculating the product of the inverse matrix and the right side. ₁ ) can be calculated. Therefore, the x ′ direction trajectory of the center of gravity can be calculated.
In the above calculation, the horizontal trajectory of the center of gravity that directly satisfies the target relative ZMP trajectory can be calculated without calculating the horizontal trajectory of the center of gravity by trial and error. The above-described calculation can be performed with a small amount of calculation, and the ground phase center-of-gravity horizontal trajectory calculation unit 308 can perform the calculation at high speed.

FIG. 5B shows the horizontal trajectory of the center of gravity of the robot 2. For example, when k is 2, the horizontal trajectory 118 of the ground phase in section 2 is calculated in step S416.
FIG. 6 shows details of the x′-direction trajectory (x ′ ₀ , x ′ ₁ ,..., X ′ _n−1 ) calculated in step S416 when k is 2.

'Center of gravity pathway in the direction (y' y in the ground phase of Interval _{k 1, ···, y 'n} -1) for even, y' direction relative ZMP position _{(p y'1, ···, p y} ' _n-2 ), the angular momentum around the target roll axis (x 'axis), and the ground phase starting speed v _y'0 and completion speed v _{y'n-1 in} the section k, in the x' direction Can be calculated in the same manner as the center-of-gravity trajectory (x ′ ₀ , x ′ ₁ ,..., X ′ _n−1 ).

Users as a command value | Note the above-mentioned horizontal velocity v _x'0, instead of v _y'0, unit vector in the horizontal direction indicating the direction in which the center of gravity is moved n _{x ',} n _y' and speed absolute value | v May give. In such a case, the horizontal velocities v _x′0 and v _y′0 are calculated as follows.

By performing the processing of step S414 and step S416, the trajectory of the center of gravity in the ground phase can be generated. For example, when k is 2, the trajectory of the center of gravity in the ground phase in section 2 can be generated. In the contact phase of the section 2, it is possible to realize an operation of sinking the center of gravity of the robot 2 in preparation for the subsequent jump in the air phase of the section 2. Moreover, in the ground contact phase of the section 2, the operation | movement which relieve | moderates the landing impact resulting from the aerial phase of the section 1 immediately before by subtracting a gravity center is also implement | achieved.
By realizing the trajectory of the center of gravity generated by the above processing, the robot 2 can realize an operation of moving the center of gravity up and down while maintaining the ZMP inside the foot of the support leg.

  In the case of the center of gravity trajectory generated by the above-described method, it is not always guaranteed that the ZMP calculated by the ZMP equation coincides with the target ZMP at the start and completion of the ground phase. However, the center-of-gravity velocity is smoothly connected at the start and completion of the ground phase, and the period during which the ZMP equation is not satisfied is very short (about twice the time width Δt used for discretization of the ZMP equation). Therefore, the robot 2 that realizes the operation generated above can perform a stable operation without falling down.

In step S418, it is determined whether or not the operation in the section k includes an aerial phase. When the section k includes the aerial phase (YES in step S418), the process proceeds to step S420 to calculate the operation of the aerial phase of the section k. When the section k does not have an aerial phase (NO in step S418), the process proceeds to step S424.
For example, when k is 2, since the operation in section 2 has an aerial phase, the process proceeds to step S420.

  In step S420, the horizontal trajectory of the center of gravity in the aerial phase of section k is calculated. In the aerial phase, the robot 2 is floating in the air, and only gravity acts on the robot 2 as an external force. Therefore, in the aerial phase, no matter how the joints of the robot 2 are driven, the center of gravity of the robot 2 performs a constant-velocity linear motion in the horizontal direction as shown below.

In the above equation, v _x0 and v _y0 are horizontal speeds of the center of gravity at the time of the crossing of the robot 2, and x ₀ and y ₀ are horizontal positions of the center of gravity at the time of the crossing.
The above v _x0 and v _y0 are also the horizontal speed of the center of gravity at the time of the crossing of the robot 2 and the horizontal speed of the center of gravity at the time of landing of the robot 2. v _x0 and v _y0 are equal to the horizontal speed of the center of gravity in switching _the operation, and are given as user command values in step S408.
The above x ₀ and y ₀ are horizontal positions of the center of gravity at the time of the crossing of the robot 2, and the horizontal position of the center of gravity when the ground phase of the section k already calculated in step S416 is completed. equal.
By giving v _x0 , v _y0 , x ₀ and y ₀ described above, the horizontal trajectory of the center of gravity can be calculated.

In step S422, the relative trajectory of the center of gravity in the section k-1 is calculated. Regarding the ground contact phase, the toes of the support legs do not move with respect to the floor, so that the gravity center trajectory generated in steps S410 to S420 can be easily replaced with the relative trajectory of the gravity center. The relative trajectory of the center of gravity of the aerial phase is calculated from the positional relationship between the center of gravity and the feet of the support legs. Since the support legs are also moved relative to the floor in the aerial phase, calculating the relative trajectory of the center of gravity is equivalent to calculating the trajectory of the foot of the support leg relative to the center of gravity. The trajectory of the foot of the support leg in the aerial phase needs to be calculated so that it smoothly connects with the trajectory of the foot of the support leg in the ground phase before and after the aerial phase at the time of crossing and landing. . The relative position between the center of gravity in the aerial phase and the toes of the support legs must be smoothly connected to the relative position of the center of gravity and the toes of the support legs in the subsequent grounding phase. After calculating the relative orbit of, calculate the relative orbit of the center of gravity of the aerial phase of interval k-1.
For example, when k is 2, after the relative trajectories 106 and 118 of the center of gravity in the ground phase in the section 2 in FIGS. 5A and 5B are generated, the relative trajectories 128 in the center of the air phase in the section 1 are generated. , 120 is calculated.
When the section k-1 does not include the aerial phase, the relative trajectory of the center of gravity is the relative trajectory of the toes of the support leg as viewed from the center of gravity, and thus the process proceeds to step S424 without performing any particular processing in step S422. .
In FIGS. 5A and 5B, since the support legs are switched when switching from the ground phase to the aerial phase, the relative trajectory of the center of gravity (the curve indicated by the solid line) changes discontinuously. . On the other hand, since the support legs are not interchanged when switching from the aerial phase to the ground phase, the relative trajectory of the center of gravity changes continuously. However, the trajectory of the center of gravity viewed from the outside of the robot 2 (the curve shown by the broken line in FIG. 5; in the contact phase, it coincides with the relative trajectory of the center of gravity) and the trajectory of the foot of the support leg are continuous trajectories. Note that there are.

At the time of the crossing, the support legs may or may not be replaced. For example, when the robot 2 crosses one leg link as a supporting leg and passes through the aerial phase and then lands as another leg link as a supporting leg, the supporting leg is switched at the time of the crossing. Unlike the above, when the robot 2 crosses one leg link as a support leg and passes through the aerial phase and then lands as the same leg link as a support leg, the support leg is not replaced at the time of the crossing.
If the support legs change at the time of the crossing, the relative trajectory of the support legs from the center of gravity at the start of the aerial phase of section k-1 (ie, the relative trajectory of the center of gravity) is Set to connect smoothly with the relative trajectory seen from the center of gravity of the free leg. The position and speed seen from the center of gravity of the toe of the free leg at the time of completion of the grounding phase of the section k-1 are the position and speed 334 of the toe of the free leg at the time of the crossing of the user command value, and the section k-1 It can be calculated from the relative trajectory of the center of gravity at the completion of the grounding phase.
If the support legs do not change at the level crossing, the relative trajectory of the support legs at the start of the aerial phase of section k-1 (ie, the relative trajectory of the center of gravity) is the relative trajectory of the support legs in the ground phase of section k-1 (Ie, the relative trajectory of the center of gravity). That is, the relative position and speed of the center of gravity at the completion of the ground phase of section k-1 are identified from the relative trajectory of the center of gravity at the ground phase of section k-1, and the center of gravity at the start of the aerial phase of section k-1 is determined. Set as relative position and speed.
For example, when k is 2, the support leg is switched from the left leg link 6 to the right leg link 8 at the time of the crossing of the aerial phase of the section 1. Therefore, in calculating the relative orbits 128 and 120 of the center of gravity of the aerial phase in section 1 in FIGS. 5A and 5B, the relative position and speed of the center of gravity at the start 126 of the aerial phase are determined by the user command. It is calculated from the position and speed 334 of the toe of the free leg at the time of the crossing of the value, and the relative trajectories 122 and 124 of the center of gravity at the completion of the grounding phase of the section 1.

At the time of landing, the relative trajectory of the center of gravity of the aerial phase is calculated so as to be smoothly connected to the relative trajectory of the center of gravity at the start of the ground phase in the section k. The relative trajectory of the center of gravity in the ground phase of section k has already been given in steps S414 and S416. That is, the relative position and speed of the center of gravity at the start of the ground phase of section k is determined from the relative trajectory of the center of gravity in the ground phase of section k, and the relative position and speed of the center of gravity at the completion of the aerial phase of section k-1 Set as.
For example, when k is 2, in calculating the relative trajectories 128 and 120 of the center of gravity in the aerial phase of the section 1 in FIGS. 5A and 5B, the relative position of the center of gravity at the time when the aerial phase is completed 108 is calculated. And the speed are matched with the relative position and speed of the center of gravity at the start time 108 of the ground phase based on the relative trajectories 106 and 118 of the center of gravity in the ground phase of section 2.

  The relative trajectory of the center of gravity in the aerial phase of the section k-1 can be calculated by using, for example, polynomial interpolation based on the above conditions at the start time and completion time.

In step S424 in FIG. 4, the relative trajectory of the toe of the free leg in the section k-1 is calculated.
The relative trajectory of the toe of the free leg in the section k-1 is calculated so as to be smoothly connected to the movement of the leg link before and after the section k-1.

  The relative position and speed of the toes of the free leg at the start of the grounding phase in section k-1 are the relative trajectory of the center of gravity in section k-2 (that is, the relative trajectory of the support leg as viewed from the center of gravity), and in section k-2 Based on the relative trajectories of the center of gravity and the free leg, the toes of the leg link that is the free leg at the start of the section k-1 are set to move smoothly. When k is 2, the relative position and speed of the toes of the free leg at the start of the grounding phase of the section 1 are used as Use speed.

The relative trajectory of the toe of the free leg at the completion of the ground phase of the section k-1 is handled differently depending on whether the section k-1 has the aerial phase or not.
When the section k-1 has an aerial phase, the position and speed of the free leg's toe at the time of the crossing are given as user command values in step S408. Therefore, the toe of the free leg at the time of completion of the ground phase of the section k-1 is set to match the user command value.
When section k-1 does not have an aerial phase, when the ground phase of section k-1 is completed, connection is made with the start of the ground phase of section k. When transitioning from the ground phase to the ground phase, the leg links that are the support legs are switched. Therefore, in the robot 2 of this embodiment, in such a case, the position and speed of the free leg when the ground phase of the section k-1 is completed are the position and speed of the center of gravity when the ground phase of the section k-1 is completed. Based on the relative position and speed of the center of gravity at the start of the section k, setting is made so that the movement of the toes of the leg link is smooth.

When the section k-1 has an aerial phase, the relative trajectory of the free leg's foot in the aerial phase is calculated as follows. The relative trajectory of the toes of the free leg in the aerial phase is handled differently depending on whether or not the supporting leg is replaced at the time of the crossing.
When the support leg is switched at the time of the crossing, the position and speed viewed from the center of gravity of the toe of the free leg at the start of the aerial phase of section k-1 Match the position and speed as seen from the center of gravity. The position and speed of the toe of the free leg at the completion of the aerial phase of the section k-1 can be arbitrarily given.
If the support legs do not change at the level crossing, the position and speed seen from the center of gravity of the toes of the free leg at the start of the aerial phase of section k-1 Match the position and speed as seen from the center of gravity of the leg. The position and speed of the toe of the free leg at the completion of the aerial phase of the section k-1 can be arbitrarily given.

  The above-mentioned calculation of the relative trajectory of the toes of the free leg is calculated by using, for example, polynomial interpolation based on the position and speed conditions at the start and completion of the ground phase and, if necessary, the aerial phase. Can do.

In step S426, a target value of the joint angle that realizes the center of gravity and the relative trajectory of the free leg calculated by the above-described processing is calculated. The target value of the joint angle can be calculated by so-called inverse kinematics calculation.
The relative translational momentum P _{g of} the center of gravity, the relative angular momentum L _{g of the} center of gravity, the relative velocity v _f of the free leg toe and the angular velocity ω _f of the free leg toe are the angular velocity θ ⁽¹⁾ of the joint angle and the Jacobian matrix J, respectively. It is expressed as follows using _g (θ), K _g (θ), J _f (θ), and K _f (θ).

In the above equation, the angular velocity θ ⁽¹⁾ of the joint angle is a first-order differentiation of the angle θ of the joint angle with respect to time. The joint angle θ is a group of joint angles (θ ₁ , θ ₂ ,..., Θ _n-1 ) of the robot 2 as shown below.

The relative translational momentum P _{g of} the center of gravity, the relative angular momentum L _{g of the} center of gravity, the relative speed v _f of the free leg foot, and the angular speed ω _f of the free leg foot are as follows.

However, in the above, (θ _{gx ′} , θ _{gy ′} , θ _{gz ′} ) is the relative rotation angle of the center of gravity viewed from the coordinate system fixed to the toes of the support leg, and (x ′ _f , y ′ _f , z ' _f ) is the relative position of the toe of the free leg, and (θ _x'f , θ _y'f , θ _z'f ) is the relative rotation angle of the toe of the free leg.

There are as many Jacobian matrices at the tip of the free leg as there are free legs. The robot 2 of this embodiment includes two leg links, and calculates a target value of the joint angle with the free leg as one leg link.
The joint angular velocity θ ⁽¹⁾ satisfying the motions P _g , L _g , v _f , and ω _f of the target robot 2 is a redundant system having an infinite number of solutions. In the robot 2 of the present embodiment, a target relative speed of the center of gravity is realized as the first subtask, a target angular momentum is realized as the second subtask, and the target of the free leg toe as the third subtask is achieved. On the condition that the relative trajectory is realized, the temporal data of the target joint angle is calculated. A method for obtaining an optimal solution as long as the redundancy allows, while giving priority to each of the constraint conditions represented by a plurality of Jacobian matrices as described above, has been proposed so far and will not be described. .

  Among the above, the target relative speed of the center of gravity is given in step S422. The target angular momentum is given as a user command value. In the robot 2 of the present embodiment, the target angular momentum is zero. The target relative trajectory of the free leg foot is given in step S424.

By performing the above processing, the target value of the joint angle can be calculated using the relative trajectory of the center of gravity, the target angular momentum, and the relative trajectory of the free leg toe as constraint conditions. Thereby, a stable operation pattern of the robot 2 can be generated. By adopting such a method, it becomes possible to generate a motion pattern using the degrees of freedom of all robots 2 such as arms and torso as well as legs, and a more natural and flexible motion pattern can be created. .
The temporal data 350 of the joint angle target value calculated in step S426 is stored in the control unit 10.

  In step S428, it is determined whether or not the generation of the center-of-gravity trajectory has been completed for all the motion sections. This determination is made by comparing the number k of the motion section to be processed with the number kmax of the last motion section. In the robot 2 of the present embodiment, the operation mode shown in FIG. 2 is assumed, and the number kmax of the last operation section is 4. If k is equal to or greater than kmax (YES in step S428), it is determined that the generation of the center of gravity trajectory has been completed for all the motion sections, and the process proceeds to step S430. If k is less than kmax (NO in step S428), the process proceeds to step S406 to generate a centroid trajectory for the motion section for which a centroid trajectory has not yet been generated, and the processes from step S406 to step S428 are performed. Repeatedly.

  In step S430, the time-dependent data of the relative trajectory of the center of gravity, the relative trajectory of the free leg, and the joint angle target value is generated for the last motion section k. The process performed in step S430 is the same as the process described in detail in steps S422, S424, and S426, and thus the description thereof is omitted.

  By the above-described processing of FIG. 4, the temporal data 350 of the joint angle target value that realizes the mode of operation shown in FIG. 2 is generated. The generated temporal data 350 of the joint angle target value is stored in the control unit 10.

  When the robot 2 actually starts the operation, the control unit 10 sequentially drives each joint of the robot 2 by the joint driving unit 320 based on the stored temporal data 350 of the joint angle target value. As a result, the robot 2 realizes a stable operation based on the user command value.

  In the robot 2 according to the present embodiment, the control unit 10 watches the switching of the motion section while reading the stored temporal data 350 of the joint angle target value. When detecting the switching of the operation section, the control unit 10 performs an operation update process shown in FIG.

Hereinafter, an operation update process performed by the control unit 10 will be described with reference to FIG.
In step S702, the motion section k of the motion realized by the robot 2 at that time is specified.
In step S704, it is determined whether or not the section k includes an aerial phase. If the section k includes an aerial phase (YES in step S704), the process proceeds to step S706, and an operation pattern update process is performed. When the section k does not include the aerial phase (NO in step S704), the control unit 10 ends the process without updating the operation pattern.

  In step S706, the speed of the center of gravity and the angular momentum around the center of gravity at the time of crossing in the section k are measured. The control unit 10 specifies the timing at which the robot 2 steps based on the ground phase gravity center vertical trajectory 338 for the section k, and instructs the sensor control unit 354 to measure the center of gravity speed and the angular momentum at the timing at which the robot 2 steps.

The sensor control unit 354 instructs the sensor 14 to measure acceleration every predetermined time interval after the robot 2 actually starts to operate, and stores the change in acceleration over time.
The acceleration measured by the sensor 14 is the acceleration at the measurement point of the sensor 14 and not the acceleration at the center of gravity of the robot 2. The sensor control unit 354 calculates the acceleration of the center of gravity from the acceleration at the measurement point of the sensor 14. For the acceleration of the center of gravity, for example, the actual temporal change of the joint angle of each joint of the robot 2 is stored from the output of the encoder, and the center of gravity and the measurement point of the sensor 14 are relative to each other from the temporal change of the joint angle of each joint. The acceleration can be calculated by adding the relative acceleration to the acceleration at the measured measurement point of the sensor 14.
The sensor control unit 354 stores the temporal change in the acceleration of the center of gravity calculated above. Further, by integrating the acceleration of the center of gravity from the time when the robot 2 actually starts operation to the present, the center of gravity speed is calculated and the change with time is stored.
When instructed to measure the center of gravity speed, the sensor control unit 354 outputs the center of gravity speed at that time as the center of gravity vertical speed 356 at the time of crossing and the center of gravity horizontal speed 358 at the time of crossing.

When instructed to measure angular momentum, the sensor control unit 354 instructs the sensor 14 to measure the angular velocity at the measurement point.
The angular velocity measured by the sensor 14 is an angular velocity around the measurement point of the sensor 14 and is not an angular velocity around the center of gravity of the robot 2. The sensor control unit 354 calculates the angular velocity around the center of gravity from the angular velocity around the measurement point of the sensor 14. As the angular velocity around the center of gravity, for example, the actual temporal change of the joint angle of each joint is stored from the output of the encoder, the angular velocity of the measurement point of the sensor 14 with respect to the center of gravity is calculated, and the sensor 14 measured as the result is calculated. It can be calculated by adding the angular velocity around the measurement point.
The sensor control unit 354 determines the angular momentum 360 at the level crossing from the calculated angular velocity around the center of gravity and the inertia moment of the robot 2 in the posture at the level crossing.

In step S706, the target angular momentum temporal data 330 is also updated.
In step S706, regarding the aerial phase of the section k, the angular momentum 360 at the time of crossing output from the sensor control unit 354 is set as a new target angular momentum. As the target angular momentum for the section k + 1 and thereafter, the value input as the user command value is used as it is without being changed.

In step S708, a vertical trajectory of the center of gravity in the aerial phase of section k is generated. The process performed in step S708 is the same as the process in step S412 of FIG. However, in step S412 in FIG. 4, a value calculated from the jump height 322, which is a user command value, is used as the initial velocity of the center of gravity. However, in step S708 in FIG. 7, the initial velocity of the center of gravity is output from the sensor control unit 354. The vertical speed 356 of the actual center of gravity of the robot 2 is used.
The vertical trajectory of the center of gravity in the aerial phase of the section k newly generated in step S708 is replaced with the stored data and used for the generation of the subsequent operation.
FIG. 8A shows the vertical trajectory of the center of gravity before the update process is performed, and FIG. 8B shows the vertical trajectory of the center of gravity after the update process is performed.
In the process of step S708, the vertical trajectory 112 of the center of gravity in the aerial phase shown in (a) is based on the vertical velocity 356 of the actual center of gravity indicated by the arrow 830 in FIG. Updated to the vertical trajectory 812 of the center of gravity.

In step S710, a horizontal trajectory of the center of gravity in the aerial phase of section k is generated. The process performed in step S710 is the same as the process in step S420 of FIG. However, in step S420 in FIG. 4, the user command value is used as the initial velocity of the center of gravity, but in step S710 in FIG. 7, the horizontal velocity 358 of the actual center of gravity of the robot 2 measured as the initial velocity of the center of gravity is used.
By the processes in steps S708 and S710, the trajectory of the center of gravity until the landing is updated in the aerial phase of the section k. In the processing after step S712, the update processing is performed for the operation after landing.

In step S712, the section k to be processed is advanced by 1.
In the processing from step S714 to step S726, the barycentric trajectory in the section k is generated again. The generation of the center-of-gravity trajectory in the section k is the same as the processing from step S408 to step S420 in FIG.
By the processing from step S714 to step S726, the trajectory of the center of gravity in the section k after landing is updated. In the processing after step S728, the trajectory of the foot of the leg link in the section k-1 is updated. In the update process of FIG. 7, when the process after step S728 is first performed, the process of updating the foot link trajectory of the leg link until landing in the operation section currently realized by the robot 2 is performed. Equivalent to.

  In step S728, a relative trajectory of the center of gravity in the section k-1 is generated. In step S730, the relative trajectory of the free leg in the section k-1 is generated. These processes are the same as the processes in steps S422 and S424 in FIG.

  In step S732, a joint angle target value in section k-1 is generated. The processing in step S732 is the same as that in step S426 in FIG.

  In step S734, it is determined whether or not the update of the center of gravity trajectory has been completed for all the motion sections. In this determination, k is compared with kmax. In the case of the operation mode shown in FIG. If k is equal to or greater than kmax (YES in step S734), it is determined that the update of the center of gravity trajectory has been completed for all the motion sections, and the process proceeds to step S736. If k is less than kmax (NO in step S734), the process proceeds to step SS712 to further update the center of gravity trajectory, and the processes from step S712 to step S734 are repeatedly performed.

  In step S736, the time-dependent data of the relative trajectory of the center of gravity, the relative trajectory of the free leg, and the joint angle target value is generated for the last motion section k. The processing performed in step S734 is the same as the processing described in detail in steps S728, S730, and S732, and thus description thereof is omitted.

The operation pattern of the robot 2 is updated by the process of FIG. 8A shows the vertical trajectory of the center of gravity before the update process of FIG. 7 is performed, and FIG. 8B shows the vertical trajectory of the center of gravity after the update process of FIG. 7 is performed. .
Even when the measured vertical velocity 356 of the center of gravity indicated by the arrow 830 in the drawing does not match the vertical track 112 of the target center of gravity at the crossing of the section 1, it is shown in (b). As described above, the center-of-gravity vertical trajectory 812 that matches the measured center-of-gravity vertical speed 356 is updated. Then, the vertical trajectories 802 and 806 of the center of gravity after the section 2 are updated in accordance with the update of the vertical trajectory 812 of the center of gravity in the aerial phase of the section 1. Then, the relative trajectory 828 of the center of gravity in the aerial phase of the section 1 is updated so as to match the updated vertical track 806 of the center of gravity in the ground phase of the section 2. The relative trajectory of the center of gravity in the air phase indicates the trajectory of the foot of the support leg with respect to the center of gravity in the air phase.
Although the relative trajectory of the free leg toe is not shown in FIG. 8, the relative trajectory of the free leg toe is adapted to the actual motion state of the robot 2, similarly to the relative trajectory 828 of the center of gravity. Updated.
Although FIG. 8 illustrates the vertical trajectory, the horizontal trajectory is similarly updated.

In the above processing, the center-of-gravity trajectory until landing is updated between the time the robot 2 crosses and the landing, the center-of-gravity trajectory after the landing is updated, and the foot link of the leg link until landing is updated. The trajectory is updated. As a result, the position where the leg link lands is updated according to the actual movement state of the robot 2 as in the case of the center of gravity trajectory.
Therefore, even when the actual motion state of the robot 2 deviates significantly from the target motion state, the robot 2 does not take an unreasonable posture and stably realizes the operation after landing. be able to.

  In the present embodiment, the presence / absence of an aerial phase is evaluated every time the motion section is switched, and the time point when the robot 2 crosses is detected based on the temporal data of the joint angle target value generated in advance. The detection of the crossing time is not limited to this. For example, a force sensor or a distance measuring sensor is arranged on the foot of the robot 2 to detect whether the foot of the supporting leg is away from the floor. A time point may be detected.

  In the above embodiment, the center-of-gravity velocity is detected by calculating the acceleration of the center of gravity from the measurement value of the acceleration sensor provided on the trunk 4 and the temporal change in the joint angle of each joint, and integrating the acceleration of the center of gravity. An example was explained. The detection of the center-of-gravity acceleration is not limited to this. For example, a plurality of acceleration sensors may be mounted on the robot 2 and the acceleration of the center of gravity may be calculated from the positions and measurement values of the measurement points of these acceleration sensors. Further, the acceleration of the center of gravity may be calculated from the measurement value of the acceleration sensor and the measurement value of the gyro sensor.

In the above description, the example in which the center of gravity speed is measured based on the acceleration measured from the sensor 14 has been described. However, the center of gravity speed is not limited to this.
For example, the position of the representative point of the robot 2 is measured using a distance measuring sensor capable of knowing the distance to the target whose position is known, and the relative positional relationship between the representative point and the center of gravity is determined as the joint angle of each joint. The speed of the center of gravity can also be obtained by calculating the position of the center of gravity from the relative position of the center of gravity and the representative point and the position of the representative point to be measured, and calculating the rate of change over time. Alternatively, a GPS receiver may be mounted on the trunk 4 as the sensor 14, the position of the center of gravity may be calculated from the acquired position and the joint angle of each joint, and the center of gravity speed may be calculated from the change over time of the center of gravity position.
Alternatively, the center-of-gravity velocity may be calculated directly using only the temporal change in the joint angle of each joint.

  In the above embodiment, the example in which the angular momentum around the center of gravity is detected using the angular velocity sensor provided on the trunk 4 has been described. The detection of the angular momentum around the center of gravity is not limited to this. For example, the temporal change in the joint angle of each joint may be measured by an encoder or the like, and the angular momentum around the center of gravity may be directly calculated.

  In the above embodiment, the legged robot 2 having two leg links has been described. However, the present invention can be embodied as a legged robot having three or more leg links, or a leg having only one leg link. It can also be embodied as a type robot.

As mentioned above, although embodiment of this invention was described in detail, these are only illustrations and do not limit a claim. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
In addition, the technical elements described in the present specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.

FIG. 1 is a diagram showing an overview of a robot 2 according to the present invention. FIG. 2 is a diagram showing an outline of the operation of the robot 2 of the present invention. FIG. 3 is a block diagram showing the configuration of the control unit 10 of the robot 2 of the present invention. FIG. 4 is a flowchart showing a motion generation process performed by the control unit 10 of the robot 2 of the present invention. FIG. 5 is a diagram showing a relative trajectory of the center of gravity of the robot 2 generated by the control unit 10. FIG. 6 is a diagram illustrating a part of the relative trajectory of the center of gravity of the robot 2 generated by the control unit 10. FIG. 7 is a flowchart showing the operation update process performed by the control unit 10. FIG. 8 is a diagram showing the relative trajectory of the center of gravity of the robot 2 updated by the control unit 10.

Explanation of symbols

2 ... leg type robot 4 ... trunk 6 ... left leg link 8 ... right leg link 10 ... control unit 12 ... controller 14 ... sensors 102, 106, 114, 115 , 122, 128... Vertical trajectory of the center of gravity 104, 118, 120, 124... Horizontal trajectory of the center of gravity 108, 110, 116, 126... Boundary of movement section 802, 806, 812, 828. Vertical trajectory 830 ... arrow

Claims (4)

It is a legged robot that realizes the operation of crossing, floating in the air and landing by changing the joint angle,
Means for detecting the center of gravity speed at the crossing;
Means for generating a planned center-of-gravity trajectory from the level crossing to the landing based on the detected center-of-gravity speed at the level crossing and a predetermined landing scheduled time ;
The planned ZMP trajectory after landing is obtained by using the planned center of gravity trajectory and planned speed at the scheduled landing time obtained from the generated planned center of gravity trajectory up to the landing so that the planned ZMP trajectory after landing coincides with a predetermined target ZMP trajectory. Means for determining a planned landing position of a toe that is scheduled to land on and generating a planned center-of-gravity trajectory after landing;
Means for generating a planned trajectory of a foot to be landed next until landing, so as to be the planned landing position at the planned landing time ;
Based on the expected trajectory of the planned trajectory of the center of gravity and foot to the generated landing, it means for generating a chronological data of target values of the joint angles of up to landing,
Means for generating temporal data of the target value of the joint angle after landing based on the generated planned center of gravity trajectory after landing and the planned landing position of the toe ;
A means for rotating the joint based on the generated temporal data of the target value of the joint angle;
Equipped with a,
Legged robot of up landing chronological data of target values of the joint angles after landing, characterized that you generated while the legged robot is floating in the air after takeoff said. Means for generating a planned center of gravity trajectory after landing,
Means for generating a vertical trajectory of the center of gravity after landing based on the vertical trajectory of the center of gravity from the crossing to the landing;
The generated vertical trajectory of the center of gravity after landing, the ternary equation obtained by discretizing the ZMP equation, the target relative ZMP after landing, the scheduled landing time, and the horizontal speed of the center of gravity at a predetermined time after landing And a means for calculating a horizontal trajectory of the center of gravity after landing based on
The legged robot according to claim 1, further comprising:   The means for calculating the horizontal trajectory of the center of gravity after landing includes a triple diagonal matrix having a coefficient calculated from the vertical trajectory of the center of gravity after landing, a sequence of horizontal trajectories of the center of gravity after landing, Solving the simultaneous equations established between the target relative ZMP and the sequence of distances calculated from the horizontal speed of the center of gravity at a predetermined time after landing and the horizontal trajectory of the center of gravity after landing The legged robot according to claim 2, further comprising means for calculating A method of controlling the movement of a legged robot that realizes a crossing, floating in the air, and landing by changing the joint angle,
Detecting the center-of-gravity speed at the crossing;
Generating a planned center-of-gravity trajectory from the level crossing to landing based on the detected center-of-gravity speed at the level crossing and a predetermined landing scheduled time ;
The planned ZMP trajectory after landing is obtained by using the planned center of gravity trajectory and planned speed at the scheduled landing time obtained from the generated planned center of gravity trajectory up to the landing so that the planned ZMP trajectory after landing coincides with a predetermined target ZMP trajectory. Determining a planned landing position of a toe that is scheduled to land on and generating a planned center-of-gravity trajectory after landing;
Generating a planned trajectory of a foot to be landed next until landing, so as to be the planned landing position at the planned landing time ;
Based on the expected trajectory of the planned trajectory of the center of gravity and foot to the generated landing, and generating a chronological data of target values of the joint angles of up to landing,
Generating temporal data of the target value of the joint angle after landing based on the generated planned center-of-gravity trajectory after landing and the planned landing position of the toe ;
A step of rotating the joint based on the generated temporal data of the target value of the joint angle;
Equipped with a,
Chronological data of target values of the joint angles after landing until landing, generated to an operation control method of the legged robot according to claim Rukoto while the legged robot is floating in the air after takeoff said.

Images