JP5035005B2 - Legged robot, its control method, and its control system - Google Patents

Description

  The present invention relates to a legged robot that operates by changing a joint angle, a control method thereof, and a control system thereof.

Many techniques for calculating the walking trajectory of a legged robot have been proposed. As a walking trajectory calculation technique, a technique for calculating the center of gravity position trajectory of a robot is well known (for example, Patent Document 1). Patent Document 1 discloses a technique for calculating the center of gravity trajectory so that the ZMP calculated by the ZMP equation matches the target ZMP. In this specification, “orbit” refers to data describing a change in position over time.
JP 2004-167676 A

  In solving the ZMP equation, generally, the center of gravity trajectory of the robot is calculated using the angular momentum trajectory of the rotational motion around the center of gravity of the robot. However, the prior art focuses on determining the center of gravity trajectory of the robot, and does not consider a method for setting an appropriate angular momentum while satisfying the condition that the robot does not fall. . In the prior art, for example, it is assumed that the angular momentum of the robot is zero, or the angular momentum is treated as a disturbance. When the angular momentum is set to 0, the trajectory calculation is performed assuming that the angular momentum is preserved.

  Assuming that the angular momentum during movement is zero, the calculated center of gravity trajectory is actually an unnatural trajectory in which the upper body of the robot rotates, and the robot's overturning is caused by the rate of change of angular momentum over time. It is.

  In a biped robot, for example, when the trajectory calculation of the stepping motion on the spot is performed assuming that the angular momentum is 0, when the center of gravity of the robot moves to the left, the angular momentum is stored as 0. The body rotates to the right. That is, since the robot tries to maintain the angular momentum with respect to the movement of the upper body, the upper body rotates greatly and the posture is lost. In addition, even when a running operation is performed, the robot turns its upper body backward each time it steps forward, and its posture is warped.

  Such an operation of the robot is not only unnatural in appearance, but also has a problem that a large torque is generated on the hip joint and an excessive load is applied. When a human performs a stepping motion, the motion can be realized while maintaining the posture without rotating the upper body. From this, it is considered that some appropriate value should be set instead of setting the trajectory of the angular momentum to 0 in order for the robot to realize the motion with a natural posture like a human.

  As described above, in the conventional operation in which the angular momentum is set to 0, there is a problem that the upper body of the robot moves unnaturally.

  In view of the above-mentioned problems, the present applicant has proposed a legged robot capable of calculating a trajectory in which the upper body moves naturally in a previously filed application (Japanese Patent Application No. 2007-08585) and a control method thereof. is doing. In the legged robot proposed in the application (Japanese Patent Application No. 2007-08585) and its control method, the angular momentum parameter is defined so that the angular momentum is proportional to the moving speed of the center of gravity and the relationship between the angular momentum and the center of gravity speed is specified. Is used to calculate the center of gravity trajectory. According to the legged robot proposed in the application (Japanese Patent Application No. 2007-08585) and its control method, for example, when the center of gravity of the robot moves to the left, the upper body also moves to the left according to the movement of the center of gravity. Since it rotates, it is possible to prevent the movement of the robot based on the calculated trajectory from becoming unnatural.

  However, in the legged robot proposed in the application (Japanese Patent Application No. 2007-08585) and its control method, it is necessary to adjust the optimum value of the angular momentum parameter and the like in advance while confirming the posture of the robot. In addition, since the parameters change depending on the weight distribution of the robot and the position of the center of gravity, it is necessary to readjust the parameters for each robot, which is troublesome. Further, in the legged robot proposed in the application (Japanese Patent Application No. 200708585) and its control method, the robot's body posture is determined from the rotational momentum set in advance, so the speed of the robot changes. In some cases, the center-of-gravity trajectory that is a natural posture angle cannot be calculated satisfactorily with the rotational momentum corresponding to the change.

  As described above, in the conventional operation in which the angular momentum is set to 0, there is a problem that the upper body of the robot moves unnaturally. In addition, unnecessary processing such as readjustment of parameters may occur, and it may not be possible to cope with a transient situation such as when the speed changes.

  The present invention has been made to solve such a problem, and it is possible to easily calculate center-of-gravity trajectory data with a stable posture angle without performing unnecessary processing such as parameter adjustment, and a natural posture. It is an object of the present invention to provide a legged robot capable of realizing walking / running operation, a control method thereof, and a control system thereof.

  The legged robot according to the present invention is a legged robot that operates by changing a joint angle, and the posture angle of the upper body of the legged robot does not vary according to a target speed commanded to the legged robot. Means for calculating a rotational momentum around the center of gravity of the legged robot; means for calculating a center of gravity trajectory of the legged robot by solving a ZMP equation using the calculated rotational momentum; And a means for rotating the joint based on the center of gravity trajectory.

  Thus, according to the target speed, the rotational momentum when the posture angle of the upper body is stable without fluctuation is calculated, and the center of gravity trajectory is calculated using the rotational momentum. Without executing the process, it is possible to easily calculate the center-of-gravity trajectory data with a stable posture angle, and to realize a walking / running operation with a natural posture. Further, since the optimum rotational momentum is calculated according to the target speed, the center of gravity trajectory that assumes a natural posture can be calculated even in a transient situation where the speed of the robot changes.

  Further, the apparatus further comprises means for storing the calculated rotational momentum, and the means for calculating the center of gravity trajectory calculates the center of gravity trajectory of the legged robot by solving a ZMP equation using the stored rotational momentum. You may make it calculate. Thereby, in the calculation process of the center-of-gravity trajectory, the ZMP equation can be solved using the stored rotational momentum, so that the number of rotational momentum calculation processes can be reduced. For this reason, even when the rotational momentum calculation process for realizing a natural posture is executed, it is possible to easily calculate the center-of-gravity trajectory data with a stable posture angle while suppressing an increase in the amount of calculation. .

  Furthermore, the means for calculating the rotational momentum calculates the rotational momentum around the center of gravity of the legged robot in a predetermined cycle unit, stores the rotational momentum in the means for storing the rotational momentum, and means for calculating the center of gravity trajectory Calculates the rotational momentum between the cycles from the stored rotational momentum in the predetermined cycle unit according to the target speed, and solves the ZMP equation using the calculated rotational momentum. By doing so, the center-of-gravity trajectory of the legged robot may be calculated. Thereby, the data amount of the rotational momentum to be stored can be further reduced, and the desired rotational momentum can be easily acquired even when the one-step time is different.

  In addition, the means for calculating the rotational momentum is the position around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the target speed commanded to the legged robot and the duration of one step. You may make it calculate the rotational momentum of.

  Furthermore, the means for calculating the rotational momentum is based on the target speed commanded to the legged robot, the duration of one step, and the transition time for transition of the state of the legged robot. The rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body does not change may be calculated.

  The means for calculating the rotational momentum includes data indicating a left foot position, a right foot position, and a trunk posture of the legged robot according to a target speed commanded to the legged robot, and the position and posture. Means for inputting and storing data indicating the trunk position to be realized, and modeling the legged robot from the data group, “mass at left knee position, right knee position, left foot vicinity position, and right foot position. It is also possible to provide means for calculating the rotational momentum around the center of gravity of the legged robot by calculating the position and velocity of each mass point using a “dynamic system in which there are concentrated”.

  Furthermore, the means for generating the center of gravity trajectory is established between a matrix having a coefficient calculated based on the vertical direction trajectory and the calculated rotational momentum, a column of the center of gravity trajectory, and a column of the target ZMP. You may make it calculate a gravity center orbit by solving simultaneous equations.

  The legged robot control method according to the present invention includes a step of calculating a rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to a commanded target speed. The step of calculating the center of gravity trajectory of the legged robot by solving the ZMP equation using the rotational momentum and the step of rotating the joint of the legged robot based on the calculated center of gravity trajectory. It is to be prepared.

  A control system for a legged robot according to the present invention is a control system for a legged robot that operates by changing a joint angle, and the left leg of the legged robot according to a target speed commanded to the legged robot. The legged robot is modeled from the data group, the data indicating the flat position, the right foot position, and the trunk posture, the input data storage unit that stores the data indicating the trunk position that realizes the position and posture, and the data group. The above-mentioned legged robot calculates the position and velocity of each mass point using the "Left Knee Position, Right Knee Position, Left Foot Position and Right Foot Position". A rotational momentum calculation unit that calculates the rotational momentum around the center of gravity of the robot, and a center of gravity trajectory calculation unit that calculates the center of gravity trajectory of the legged robot by solving the ZMP equation using the calculated rotational momentum. Before Based on the trajectory of the center of gravity, in which and a joint driving unit for rotating the joint.

  According to the present invention, it is possible to easily calculate center-of-gravity trajectory data with a stable posture angle without executing unnecessary processing such as parameter adjustment, and to realize a walking / running motion in a natural posture. An object is to provide a robot, a control method thereof, and a control system thereof.

Embodiment 1 of the Invention
The legged robot according to the first embodiment first calculates the rotational momentum around the center of gravity where the posture angle of the upper body of the legged robot does not vary according to the commanded target speed. Next, the legged robot calculates the center-of-gravity trajectory of the robot by solving the ZMP equation using the calculated rotational momentum. The legged robot operates by changing the joint angle based on the calculated center of gravity trajectory.

Hereinafter, the robot control method according to the first embodiment will be described with reference to the drawings. FIG. 1 is a diagram showing an outline of the legged robot according to the first embodiment. The robot 2 includes a trunk 4, a left leg link 6, a right leg link 8, a control unit 10, and a controller 12. One end of the left leg link 6 is swingably connected to the trunk 4 via a hip joint. The left leg link 6 further includes a knee joint and an ankle joint, and a foot at the tip. One end of the right leg link 8 is swingably connected to the trunk 4 via a 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 according to instructions from the control unit 10. Reference points L ₀ and R ₀ are provided at the centers of the feet of the left leg link 6 and the right leg link 8, respectively. The reference points L ₀ and R ₀ are points that serve as a reference when generating an operation pattern of the robot 2. G in the figure indicates the position of the center of gravity of the robot 2.

  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.

  In the following, a coordinate system (x, y, z) fixed to the floor outside the robot 2 is used as a coordinate system for expressing 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 according to the first embodiment, as shown in FIG. 1, the foot center of the support leg is set as a reference point of the coordinate system (x ′, y ′, z ′). In the following, the position, velocity, acceleration, and trajectory described in the coordinate system (x ′, y ′, z ′) fixed to the reference point of the robot 2 are respectively represented by the relative position, relative velocity, relative acceleration, and relative trajectory. It expresses.

  The robot 2 according to the first embodiment generates and stores an operation pattern from the start to the end of the operation, and sequentially reads the stored operation pattern to realize the operation. 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 according to the first embodiment walks while generating an operation pattern in real time.

  Next, details of the operation of the control unit 10 will be described. FIG. 2 is a functional block diagram illustrating a functional configuration of the control unit 10. The control unit 10 includes a rotational momentum calculation unit 21, a rotational momentum storage unit 22, a trajectory calculation unit 30, a target posture calculation unit 40, and a joint drive unit 50. Here, since the center of gravity trajectory is calculated and generated, the terms “calculation”, “generation”, and “calculation” are used interchangeably.

  The rotational momentum calculation unit 21 calculates the rotational momentum around the center of gravity of the robot 2 where the posture angle of the upper body 4 of the robot 2 does not vary according to the target speed commanded by the user to the robot 2. The calculated rotational momentum is stored in the rotational momentum storage unit 22. Details of the calculation method of the rotational momentum will be described later.

  The trajectory calculation unit 30 includes a target speed determination unit 31, a target rotational momentum determination unit 32, a temporary center of gravity trajectory calculation unit 33, a ZMP compensation calculation unit 34, and a free leg trajectory calculation unit 37. The trajectory calculation unit 30 calculates the center of gravity trajectory of the robot 2 and the toe trajectory of the free leg by solving the ZMP equation using the rotational momentum stored in the rotational momentum storage unit 22. The target posture calculation unit 40 calculates the target posture of the robot 2 that realizes the calculated center of gravity trajectory and free leg trajectory. The joint drive unit 50 drives each joint of the robot 2 based on the calculated target posture.

  Next, a method for calculating the rotational momentum will be described. With reference to FIG. 3, the calculation method of the rotational momentum in which the robot 2 can stably operate the body posture will be described. The rotational momentum calculation unit 21 is used when the robot 2 is activated, and calculates the rotational momentum that can be operated while stabilizing the upper body of the robot 2. The rotational momentum data calculated by the rotational momentum calculation unit 21 is stored in the rotational momentum storage unit 22. As will be described later, the robot 2 calculates trajectory data for walking using the stored rotational momentum data. The rotational momentum calculator 21 can calculate the rotational momentum sufficiently fast. For example, when the duration of one step is about 1 second, the rotational momentum of that step can be calculated in 5 msec or less, and in 2 seconds, the rotational momentum of 400 cases or more can be calculated. While watching the robot 2 walking, the user specifies the next step with the joystick etc. Since rotational momentum data for one step can be created, rotational momentum data can be created substantially in real time. The rotational momentum may be calculated when the robot 2 is activated, and thereafter, the rotational momentum calculation process may be repeated as a periodic motion.

  First, the user inputs data such as a course on which the robot 2 is desired to walk according to the target speed of the robot 2 to the rotational momentum calculator 21. Specifically, five vectors and one scalar quantity shown in the lower left (A) of FIG. 3 are input. A vector C is a left foot position vector, which indicates the left foot position of the robot 2 as viewed from the coordinate origin O. A vector D is a left foot posture vector and indicates the direction of the left foot. A vector E is a right foot position vector, which indicates the right foot position of the robot 2 as viewed from the coordinate origin O. A vector F is a right foot posture vector and indicates the direction of the right foot. A vector R is a trunk posture vector and indicates the direction of the trunk of the robot 2. A direction in which a member corresponding to a human spine extends is shown. When the torso of the robot 2 is separated into a waist (lower torso) and an upper torso, the waist corresponds to the trunk, and the trunk posture vector R defines a direction perpendicular to the waist. Q is the trunk height, which corresponds to the height of the waist position that humans say. The user successively inputs these five types of vectors and one type of scalar quantity in time series. These five types of vectors and one type of scalar are calculated so as to realize the target speed of the robot 2 and are calculated so that the posture angle of the upper body of the robot 2 does not fluctuate. .

  Further, when these five types of vectors and one type of scalar quantity are input in time series, these five types of vectors and one type of scalar quantity are time series data corresponding to the duration of one step of the robot 2. Or time-series data corresponding to the transition time during which the state of the robot 2 transitions may be input. Here, the transition time refers to the time for transitioning to the aerial phase, which is the period during which both legs of the robot 2 are leaving the floor, and the transition to the both leg phase where both legs are landing on the floor. It is time for. Data input support software for this purpose has been developed. By specifying data at main timing, intermediate data in time series is complementarily calculated. In addition, the input amount can be reduced by instructing to repeatedly use the input data for one step.

  Furthermore, the user may instruct a walking course, a target speed, etc. with a joystick or the like. If the standard foot posture vector change pattern, trunk posture vector change pattern, and trunk height change pattern for one step are stored, the user can specify the walking course and target speed with a joystick, etc. Can generate and store the five types of vectors C (t), D (t), E (t), F (t), R (t) and one type of scalar quantity Q (t) for one step. . Five types of vectors C (t), D (t), E (t), F (t), R (t) and one type of scalar quantity Q (t) necessary for creating rotational momentum data are: It is input to the rotational momentum calculation unit 21 and stored in the storage device 292.

  The user further inputs a target ZMP position vector according to a time series. In this case, since the distinction between the left foot contact state, the two foot contact state, and the right foot contact state is known from the vectors C to F, the position in the left foot contact surface is specified in the left foot contact state, and the right foot contact surface in the right foot contact state. Specify the position within. By using the vectors C to F, a target ZMP position vector (ZMP * vector) can be designated. By controlling the actual ZMP observed during walking so as to match the target ZMP, the robot 2 can walk without falling.

  First, the rotational momentum calculation unit 21 calculates ZMP that will actually occur based on the five types of vectors and one type of scalar input by the user, obtains the deviation from the target ZMP, and calculates the deviation from the ZMP deviation. A trunk position vector P that yields the target ZMP is calculated. Then, the rotational momentum calculation unit 21 calculates rotational momentum data based on the five types of vectors, one type of scalar amount, and the calculated trunk position vector P input by the user. The means 294 generates data indicating one trunk position that realizes the position and posture of the robot 2 defined by five types of vectors and one type of scalar. Since there are a plurality of trunk positions that realize the position and posture of the robot 2 defined by five types of vectors and one type of scalar, it cannot be uniquely determined. Here, an arbitrary one is adopted. When the trunk position vector P is generated, six types of vectors C, D, E, F, P, and R are prepared, so that the position and posture of the robot 2 are uniquely determined.

  The trunk vector P used here is temporarily used and is corrected by the means 310 described later, and may be rough. It may be specified by a person, input to the rotational momentum calculation unit 21 and stored, or may be generated by the rotational momentum calculation unit 21.

  The trunk posture vector R, the toe position vectors C and E, the toe posture vectors D and F, the trunk height Q, and the target ZMP vector can be standardized with respect to the trunk position vector P. If the change pattern for one step is standardized, a person can input the five types of vectors and one type of scalar required by the rotational momentum calculation unit 21 by designating the trunk position vector P. it can. The trunk position vector P can be specified using a joystick or the like. It is possible to input the trunk position vector P in real time by operating a joystick or the like while observing the walking robot 2. The trunk position vector P can be inputted in real time, the five types of vectors and one scalar quantity required by the rotational momentum calculating unit 21 can be inputted in real time, and the rotational momentum calculating unit 21 can be inputted in real time. If the rotational momentum data can be calculated at a higher speed in time for the control, the walking trajectory of the robot 2 can be specified in real time.

  If the trunk position P and posture R are determined, the positions of the left and right hip joints are determined. If the left foot position C and the left foot posture D are determined, the position of the left ankle is determined. If the position of the left hip joint and the position of the left ankle are determined, the position G of the left knee is determined. Since the length of the left thigh and the length of the left shin are known and the left knee joint has only one degree of freedom of rotation, the position of the left knee is unique if the position of the left hip joint and the position of the left ankle are determined. To be determined. Similarly, the position of the right ankle is determined from the right foot position E and the right foot posture F. The right knee position H is determined from the position of the right hip joint and the position of the right ankle. The means 296 calculates the left knee position vector G and the right knee position vector H using the above geometric relationship. The means 298 calculates the ZMP position of the robot 2. Here, the 6-mass system model shown in FIG. 4 is used.

  In the dynamic system having six mass points, the total mass M1 of the mass of the lower left shin half 324, the mass of the left ankle 322, and the mass of the left foot 320 is concentrated in the vicinity of the left foot. The total mass M3 of the mass of the lower right shin half 340, the mass of the right ankle 338, and the mass of the right foot 336 is concentrated in the vicinity of the right foot. The total mass M2 of the mass of the left upper shin half 326, the mass of the left knee joint 328, and the mass of the left lower leg half 330 is concentrated at the left knee position. A total mass M4 of the mass of the right upper shin half 342, the mass of the right knee joint 344, and the mass of the right lower leg half 346 is concentrated at the right knee position. The total mass M5 of the mass of the left upper thigh half 332, the mass of the left hip joint 334, the mass of the right upper thigh half 348, the mass of the right hip joint 350, and the mass of the lower trunk half 352 is concentrated on the lower trunk reference point. Exists. It is assumed that the mass M6 of the upper half 354 of the trunk is concentrated on the upper trunk reference point.

  The position of the mass point M1 may be in the vicinity of the left foot, may be deviated from the left ankle joint position, or may be deviated from the end point of the left foot position vector C. The position of the mass point M3 may be in the vicinity of the right foot, may be deviated from the right ankle joint position, or may be deviated from the end point of the right foot position vector E. On the other hand, it is extremely effective that the position of the mass point M2 is at the left knee joint placement, and since the mass is concentrated on the left knee joint placement, the calculation of the ZMP position is possible. The calculation of the correction amount of the trunk position is greatly simplified. Similarly, it is extremely effective that the position of the mass point M4 is at the right knee joint placement. The mass points M5 and M6 exist on the human spine. The height should be set so that the value obtained by calculating the moment of inertia around the horizontal axis of the trunk using the concentrated masses M5 and M6 matches the actual moment of inertia around the horizontal axis of the trunk. Is preferred.

  The means 298 in FIG. 3 uses the speed and acceleration of the mass points M1 to M6 and the moment of inertia about the horizontal axis due to the concentrated masses M5 and M6 from the change in the position and posture of the robot 2 determined by the means 296. The ZMP position generated in the robot 2 is calculated.

  The rotational momentum calculation unit 21 shown in FIG. 3 includes means 292 for inputting and storing data indicating the target ZMP position, and the ZMP position calculated by the means 298 and the target ZMP position stored in the means 292. Can be calculated (means 300).

  The rotational momentum calculation unit 21 calculates a trunk position vector correction amount necessary to eliminate the ZMP deviation calculated by the means 300, and a trunk position vector P from which a ZMP position equal to the target ZMP position is calculated. Means 310 is calculated. The means 310 corrects how much the data indicating the trunk position is corrected, how much the data indicating the left knee position and the right knee position is changed, and how much the data indicating the ZMP position is changed. The correction amount of the data indicating the position is obtained. According to the correction means 310, it is not necessary to repeatedly calculate and converge the correction amount, and the ZMP deviation can be eliminated by a single correction. In calculating the movement amount of the left knee position and the right knee position from the correction amount of the trunk position, the movement amount of the left knee position and the right knee position is calculated by multiplying the correction amount ΔP of the trunk position by a coefficient. This further simplifies the calculation. By using the proportional coefficient between the trunk height and the knee position height, the movement amounts of the left knee position and the right knee position can be easily calculated from the trunk position correction amount ΔP.

  As described above, a ZMP position that substantially coincides with the target ZMP position is calculated. However, there is a case where the ZMP deviation is not sufficiently solved by correcting the trunk position vector P once. In this case, the ZMP deviation can be further eliminated if the means 298 and the subsequent steps are used again and corrected twice. As described above, it has been verified that gait data including the trunk position vector P that closely follows the target ZMP can be calculated in a short time with a small amount of calculation.

  In the means 312, gait data (5 types of vectors and 1 type of scalar quantity calculated according to the target speed commanded to the robot 2, the duration of one step, and the transition time during which the state of the robot 2 changes) , Including the calculated trunk position vector P), the rotational momentum around the center of gravity of the robot 2 where the posture angle of the upper body of the robot 2 does not vary is calculated. The means 312 calculates the position and velocity of each mass point from these gait data, and calculates the angular momentum around the center of each mass point. The means 312 calculates the rotational momentum around the center of gravity of the robot 2 from the angular momentum around the center of each mass point using the positional relationship between the center position of each mass point and the center of gravity position.

  The control unit 10 stores the rotational momentum calculated in this way in the rotational momentum storage unit 22. The stored rotational momentum data may be stored as rotational momentum data discretized for each predetermined cycle unit.

  The trajectory calculation unit 30 calculates the center of gravity trajectory of the robot 2 and the toe trajectory of the free leg by solving the ZMP equation using the rotational momentum. First, the target speed determination unit 31 determines the target speed of the robot 2 according to the target speed commanded by the user to the robot 2, and outputs the target speed to the target rotational momentum determination unit 32 and the provisional gravity center trajectory calculation unit 33. To do. The target speed can be arbitrarily given by the user as long as the performance of the actuator of the robot 2 allows.

  The target rotational momentum determination unit 32 reads the rotational momentum stored in the rotational momentum storage unit 22 according to the input target speed. The target rotational momentum determination unit 32 reads the rotational momentum calculated in the situation closest to the current traveling state according to the input target speed, the duration of one step, and the traveling conditions such as the transition time. When the rotational momentum data discretized in a predetermined cycle unit is stored in the rotational momentum storage unit 22, the target rotational momentum determination unit 32 calculates a polynomial from the stored rotational momentum data in a predetermined cycle unit. The rotational momentum between these periods is calculated by interpolation processing. Since the rotational momentum around the center of gravity of the robot 2 is unlikely to fluctuate greatly during one step, the data amount of the rotational momentum to be stored is further reduced by storing only the data discretized in a predetermined cycle unit. In addition, the desired rotational momentum can be flexibly acquired even when the duration of one step is different.

  The temporary center of gravity trajectory calculation unit 33 calculates the temporary center of gravity trajectory of the robot 2 according to the input target speed. The relative trajectory of the center of gravity is determined based on the conditions of the target relative ZMP trajectory, the rotational momentum around the center of gravity, and the horizontal speed (target speed) of the center of gravity at the start and end of the operation. The relative trajectory of the center of gravity is calculated so that the actual relative ZMP trajectory when the relative trajectory of the center of gravity is realized matches the target relative ZMP trajectory.

  The target relative ZMP trajectory is time-dependent data of the position of the target ZMP with respect to the foot reference point of the support leg, and can be arbitrarily given by the user. 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 according to the first embodiment, the relative ZMP trajectory 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 may be any trajectory as long as it is maintained inside the foot. For example, a track that moves from the rear to the front inside the foot of the support leg may be used. Even when such a relative ZMP trajectory is realized, the robot 2 can stably operate without falling down.

Here, a method for calculating the relative trajectory of the center of gravity will be described. The relative ZMP position (q _{x ′} , q _{y ′} ) realized by the robot 2 includes the relative center of gravity position (x ′, y ′, z ′) of the robot 2 and the rotational momentum (r _{x ′} , r _{y ′} ). The following equation for calculating the ZMP actually generated from the relative center of gravity position of the robot 2 and the angular momentum around the center of gravity of the robot 2 is called a ZMP equation.

Here, ⁽¹⁾ shows the first order derivative with respect to time t, and ⁽²⁾ shows the second order derivative with respect to time t. Further, m is the mass of the robot 2 and g is the gravitational acceleration. 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. In the above equation, r _{x ′} ⁽¹⁾ and r _{y ′} ⁽¹⁾ indicate differential components of rotational momentum around the center of gravity.

The provisional center-of-gravity trajectory calculation unit 33 determines a time-series change in the rotational momentum (r _{x ′} , r _{y ′} ) around the center of gravity of the robot 2 and the vertical trajectory z ′ of the center of gravity based on a command from the user or the like. To do. Then, the horizontal trajectory (x ′, y ′) of the center of gravity of the robot 2 is calculated by solving the above ZMP equation. The horizontal trajectory (x ′, y ′) of the center of gravity of the robot 2 can be calculated by solving a ternary equation obtained by discretizing the ZMP equation. In order to calculate the center of gravity trajectory at high speed, for example, simultaneous equations established between a matrix having a coefficient calculated based on the vertical direction trajectory and the calculated rotational momentum, a column of the center of gravity trajectory, and a column of the target ZMP The center of gravity trajectory may be calculated by solving A method for calculating the horizontal trajectory of the center of gravity using this ternary equation is described in the specification of Japanese Patent Application No. 2007-08585 filed earlier by the present applicant. However, it should be noted that this patent application has not yet been published at the time of this application.

  The center of gravity trajectory calculation unit 35 of the ZMP compensation calculation unit 34 actually calculates the center of gravity of the robot 2 from the target rotational momentum input from the target rotational momentum determination unit 32 and the temporary center of gravity trajectory calculated by the temporary center of gravity trajectory calculation unit 33. Calculate the center of gravity trajectory to follow. The center-of-gravity trajectory calculation unit 35 determines the vertical center-of-gravity z ′ of the center of gravity based on a command from the user and the like, and is calculated by the target rotational momentum input from the target rotational momentum determination unit 32 and the provisional center-of-gravity trajectory calculation unit 33. The ZMP position is calculated by solving the above ZMP equation from the calculated temporary center of gravity position. Then, the center-of-gravity trajectory calculation unit 35 calculates a deviation between the calculated ZMP position and the target ZMP trajectory stored in the means 292. The center-of-gravity trajectory calculation unit 35 calculates a correction amount of the center-of-gravity position necessary for eliminating the calculated ZMP deviation, and calculates a center-of-gravity position where a ZMP position equal to the target ZMP position is calculated. By realizing the calculated center-of-gravity trajectory, the robot 2 can realize a motion in a natural posture while maintaining the ZMP inside the foot of the support leg.

  The landing position calculation unit 36 of the ZMP compensation calculation unit 34 calculates the landing position of the toe of the free leg from the calculated gravity center trajectories for a plurality of steps. The free leg trajectory calculation unit 37 calculates a relative trajectory of the free leg's toe from the calculated landing position of the toe. The relative trajectory of the toes of the free leg is calculated so as to smoothly connect with the movement of the leg link before and after the movement. The relative trajectory of the free leg's foot can be calculated by using, for example, polynomial interpolation.

The target posture calculation unit 40 calculates a target value of the joint angle that realizes the calculated center of gravity trajectory and free leg trajectory. The target value of the joint angle can be calculated by so-called inverse kinematics calculation. The relative translational momentum P _g of the robot _2, the relative angular momentum L _g of the robot _2, the relative velocity v _f and the free leg foot of the angular velocity omega _f of the free leg foot, and the angular velocity theta ⁽¹⁾ of the joint angle, respectively Jacobi Using the matrices J _g (θ), K _g (θ), J _f (θ), and K _f (θ), they are expressed as follows.

In Equation 2, 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 joint angle group (θ ₁ , θ ₂ ,..., Θ _n−1 ) of the robot 2 as shown below.

The relative translational momentum P _g of the robot _2, the relative angular momentum L _g of the robot _2, the relative velocity v _f and the free leg foot of the angular velocity omega _f of the free leg foot is less, respectively.

There are as many Jacobian matrices at the tip of the free leg as there are free legs. The robot 2 according to the first embodiment includes two leg links, and calculates a joint value target value using 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 according to the first embodiment, the target relative speed of the center of gravity is realized as the first subtask, the target angular momentum is realized as the second subtask, and the target free leg as the third subtask. Data on the target joint angle over time is calculated on condition that a relative trajectory of the toes is realized. 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. .

  By performing the above-described processing, the target value of the joint angle can be calculated using the trajectory of the center of gravity, the target angular momentum, and the trajectory of the free leg foot 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 of the calculated joint angle target value is stored in the control unit 10. For example, temporal data of the joint angle target value corresponding to one step is calculated and stored. In parallel with the above processing, the control unit 10 sequentially drives each joint of the robot 2 by the joint driving unit 50 based on the temporal data of the joint angle target value. As a result, the robot 2 realizes a stable posture operation based on the user command value.

  As described above, the robot 2 according to the first embodiment calculates the rotational momentum around the center of gravity where the posture angle of the upper body of the robot 2 does not vary according to the target speed commanded to the robot 2. Then, the calculated rotational momentum is stored, and the center of gravity trajectory of the robot 2 is calculated by solving the ZMP equation using the stored rotational momentum. The robot 2 calculates the center of gravity trajectory of the robot 2 by solving the ZMP equation using the stored rotational momentum, and rotates the joint based on the calculated center of gravity trajectory.

  Thus, according to the target speed, the rotational momentum when the posture angle of the upper body is stable without fluctuation is calculated, and the center of gravity trajectory is calculated using the rotational momentum. Without executing the process, it is possible to easily calculate the center-of-gravity trajectory data with a stable posture angle, and to realize a walking / running operation with a natural posture. In the calculation process of the center of gravity trajectory, the number of rotational momentum calculation processes can be reduced by solving the ZMP equation using the stored rotational momentum. Therefore, even when such a rotational momentum calculation process is executed, it is possible to easily calculate the center-of-gravity trajectory data with a stable posture angle while suppressing an increase in the calculation amount.

Other embodiments.
In the first embodiment described above, the rotational momentum calculation unit 21 calculates the rotational momentum in which the posture angle of the upper body does not vary using the six mass point model, but the present invention is not limited to this. For example, The rotational momentum may be calculated using a dynamic system of 12 joints and 13 links.

  In the first embodiment described above, the trajectory calculation unit 30 calculates trajectory data corresponding to one step. However, the present invention is not limited to this, and the trajectory data to be calculated is shorter than that corresponding to one step. Orbital data of a portion corresponding to the time segment may be acquired.

  Furthermore, the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the gist of the present invention described above.

It is a block diagram of the robot concerning embodiment of this invention. It is a functional block diagram which shows the structure of the control part of the robot concerning embodiment of this invention. It is a functional block diagram which shows the structure of the rotational momentum calculation part which concerns on embodiment of this invention. It is a conceptual diagram which shows the 6 mass point type | system | group model which concerns on embodiment of this invention.

Explanation of symbols

2 legged robot, 4 trunk, 6 left leg link, 8 right leg link, 10 control unit,
12 controller,
21 rotational momentum calculation unit, 22 rotational momentum storage unit, 30 orbit calculation unit,
31 target speed determination unit, 32 target rotational momentum determination unit, 33 temporary center of gravity trajectory calculation unit,
34 ZMP compensation calculation unit, 35 Center of gravity trajectory calculation unit, 36 Landing position calculation unit,
37 free leg trajectory calculation unit, 40 target posture calculation unit, 50 joint drive unit,
328 Left knee joint, 334 Left hip joint, 344 Right knee joint, 350 Right hip joint,
320 left foot, 322 left ankle, 324 left shin lower half, 326 left shin upper half,
330 left lower thigh, 332 left upper thigh, 336 right foot, 338 right ankle,
340 Lower right shin half, 342 Upper right shin upper half, 346 Lower right thigh half, 348 Upper right thigh half,
352 Lower trunk half, 354 Upper trunk half

Claims (10)

A legged robot that operates by changing the joint angle,
Means for calculating a rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to a target speed commanded to the legged robot ;
Means for storing the calculated rotational momentum;
By solving the ZMP equation using the pre-Symbol rotational momentum stored, means for calculating a centroid trajectory of the legged robot,
Means for rotating the joint based on the calculated center of gravity trajectory ,
The means for calculating the rotational momentum is:
Calculate the rotational momentum around the center of gravity of the legged robot in a predetermined cycle unit, and store it in the means for storing the rotational momentum,
The means for calculating the center of gravity trajectory is:
According to the target speed, a rotational momentum between the periods is calculated from the stored rotational momentum of the predetermined period by polynomial interpolation processing, and a ZMP equation is solved using the calculated rotational momentum. To calculate the center of gravity trajectory of the legged robot,
Legged robot. A legged robot that operates by changing the joint angle,
Means for calculating a rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to a target speed commanded to the legged robot;
Means for calculating a center-of-gravity trajectory of the legged robot by solving a ZMP equation using the calculated rotational momentum;
Means for rotating the joint based on the calculated center of gravity trajectory ,
The means for calculating the rotational momentum is:
Calculating the rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the target speed commanded to the legged robot and the duration of one step;
Legged robot. The means for calculating the rotational momentum is:
The legged robot in which the posture angle of the upper body of the legged robot does not vary according to the target speed commanded to the legged robot, the duration of one step, and the transition time during which the state of the legged robot transitions claim 1 or 2 have Zureka 1 wherein the legged robot, wherein calculating the rotational momentum around the center of gravity. The means for calculating the rotational momentum is:
According to the target speed commanded to the legged robot, data indicating the left foot position, right foot position and trunk posture of the legged robot, and data indicating the trunk position realizing the position and posture are input. Means for memorizing and
From the data group, the position of each mass point is modeled using a "dynamic system in which mass concentrates at the left knee position, right knee position, left foot position, and right foot position" that models the legged robot. and calculates the speed, the legged claim 1乃optimum 3 have Zureka 1 wherein the legged robot according to, characterized in that it comprises means for calculating the rotational momentum around the center of gravity of the robot. The means for generating the center of gravity trajectory is:
Calculating a centroid trajectory by solving a matrix having a coefficient calculated based on the vertical trajectory and the calculated rotational momentum, a centroid trajectory sequence, and a target ZMP sequence. claim 1乃optimum 4 have Zureka 1 wherein the legged robot according to features. Calculating a rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the commanded target speed;
By solving the ZMP equation using the pre-Symbol rotational momentum stored, calculating the center of gravity pathway of the legged robot,
Rotating the joint of the legged robot based on the calculated center of gravity trajectory ,
In the step of calculating the rotational momentum,
Calculate the rotational momentum around the center of gravity of the legged robot in a predetermined cycle unit, store the rotational momentum,
In the step of calculating the center of gravity trajectory,
According to the target speed, a rotational momentum between the periods is calculated from the stored rotational momentum of the predetermined period by polynomial interpolation processing, and a ZMP equation is solved using the calculated rotational momentum. To calculate the center of gravity trajectory of the legged robot,
Control method for legged robot. Calculating a rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the commanded target speed;
Calculating a center-of-gravity trajectory of the legged robot by solving a ZMP equation using the calculated rotational momentum;
Rotating the joint of the legged robot based on the calculated center of gravity trajectory ,
In the step of calculating the rotational momentum,
Calculating the rotational momentum around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the commanded target speed and the duration of one step;
Control method for legged robot. In the step of calculating the rotational momentum,
Rotation around the center of gravity of the legged robot in which the posture angle of the upper body of the legged robot does not vary according to the commanded target speed, the duration of one step, and the transition time during which the state of the legged robot transitions claim 6 or 7 have Zureka 1 wherein the legged robot control method, wherein the calculating the momentum. The step of calculating the rotational momentum comprises:
The position and speed of each mass point are calculated using the “dynamic system in which mass is concentrated at the left knee position, right knee position, left foot position, and right foot position” that models the legged robot. Te, the control method of the legged legged robot according to claim 6 to 8 have Zureka preceding claim, characterized in that it comprises the step of calculating the rotational momentum around the center of gravity of the robot. In the step of generating the center of gravity trajectory,
Calculating a centroid trajectory by solving a matrix having a coefficient calculated based on the vertical trajectory and the calculated rotational momentum, a centroid trajectory sequence, and a target ZMP sequence. control method for a legged robot according to claim 6 to 9 have Zureka preceding claim, characterized.

Images