JP5198035B2 - Legged robot and control method thereof - Google Patents

Description

  The present invention relates to a legged robot in which two legs are connected to a torso and a control method thereof, and more particularly to a legged robot that performs copying control and inversion control and a control method thereof.

  In recent years, a leg for walking is provided, the leg is driven, and a foot portion provided at the lower end of the leg is placed on the floor surface based on predetermined gait data to perform a walking motion. Legged mobile robots have been developed.

  In such a legged mobile robot, first, the foot portion of the leg portion is brought into contact with the floor surface to form a support leg, and then the floor surface is pushed by the back surface of the foot to propel the robot while supporting its own weight. The following walking motion is performed by driving the leg. The driven leg portion is a free leg, while the other leg portion is a support leg. In this way, a walking motion can be performed by alternately switching the free leg and the support leg.

  The legged mobile robot has a trunk and two legs connected to the trunk. The leg portion has a plurality of links and a plurality of joints, and adjacent links are slidably connected via the joints. The joint is provided with an actuator, and the posture of the entire robot can be controlled by controlling the joint angle. A link that is a link at the tip of the leg and is in contact with the ground contact surface is referred to as a foot. The bottom surface of the foot, that is, the surface in contact with the ground contact surface is referred to as the foot back surface.

  Then, target values such as the positions of the trunk and the foot are given, and the joint angles are controlled so that the actual positions and the like of the trunk and the feet coincide with the respective target values. A target value such as the position of the trunk or the foot is referred to as gait data. The gait data typically includes a target body position, a target body position / posture angle, a target foot position, and a target foot position / posture angle. The gait data is created using computer simulation or the like so as to satisfy the condition that the robot does not fall.

  Particularly in a biped robot, it is important that the actual body posture angle (relative position shift (mainly in the XY direction) between the support foot and the center of gravity (body)) follows the target body position / posture angle. This is because the mismatch between the actual body posture angle and the target body position / posture angle is often caused by the fact that the foot of the leg in contact with the ground is inclined unexpectedly. This is because if the foot of the leg that is in contact with the ground is tilted unexpectedly, the entire robot tilts and may fall. Note that an unplanned inclination of the foot is caused by an unexpected inclination or unevenness of the ground contact surface. Therefore, it is important to make the actual body posture angle follow the target body position / posture angle in order to walk the ground contact surface with unexpected inclination and unevenness without falling down.

  Japanese Patent Application Laid-Open No. 2004-228561 has a servo function for controlling the robot joint to follow a target position, and in a joint control device that changes the operation amount according to an external force acting on the robot and performs a copying operation. This is a joint that includes the ankle joint of a legged walking robot equipped with a gait, detects the external force acting on the sole, adjusts the gain of the control device so that the external force is reduced, and makes the sole land while following the ground A joint control device for a legged directional robot configured as described above is disclosed.

Japanese Patent No. 2819323

  By the way, when a legged robot walks on an uneven surface or slope of a road, it is necessary to make the sole of the foot follow the ground in order to be able to walk on the uneven surface. For this reason, it is desirable that the relative positional relationship between the leg and the trunk be in accordance with the original command. In the present specification, control for copying the sole of the foot to the ground is called copying control, and control for maintaining the position and posture from the foot to the trunk at the target position and posture is called inverted control. In general, a legged robot uses both inversion control and copying control. Here, when trying to correct the inversion control by feeding back the tilt angle of the trunk, the inversion control may cause the sole to come off the ground and become unstable. This is because if the torque applied by the correction by the inversion control is too large, the effect of the ground imitation control of the sole is diminished, and the sole of the sole is bent. Once you get off the ground, the amount of correction may diverge and fall.

  FIG. 9 and FIG. 10 are diagrams showing problems of a robot that uses both inversion control and copying control. As shown in FIG. 9A, when the robot 201 lands on the convex portion 202, as shown in FIG. 9B, the robot 201 causes the convex portion 202 to drive the driving direction by the scanning control and the driving direction by the inversion control. Works in the opposite direction. The motor is driven in an attempt to correct the tilt angle of the torso by inversion control, but the sole may come off first. As shown in FIG. 9 (c), if the inversion control is continued in a state where the sole is separated from the ground, the sole continues to float. And as shown to Fig.10 (a), a robot may perform the operation | movement which falls down depending on own weight, and an impact is very large. In this case, as shown in FIG. 10 (b), it falls down as it is. Or, the soles come off the ground again. Then, as shown in FIG. 10C, by performing the inversion control to reestablish the system, the operation of further lifting the sole is repeated.

  The present invention has been made to solve such problems, and a legged robot capable of performing a stable walk by executing an inverted control and a copying control in a balanced manner even on an uneven surface and its An object is to provide a control method.

  In order to achieve the above-described object, the legged robot according to the present invention includes a posture angle deviation calculating unit that calculates a posture angle deviation between target gait data and actual gait data, and the posture angle deviation calculating unit includes: An inversion control means for calculating a correction amount based on the calculated posture angle deviation, a correction means for correcting the target gait data with the correction amount calculated by the inversion control means, and a distance sensor provided on the back surface of the foot A contact degree calculation means for calculating a contact degree with respect to the ground of the foot based on a value of the distance sensor, wherein the contact degree calculation means calculates the correction amount calculated by the inversion control means based on the calculated contact degree. Is to be corrected.

  Here, the target gait data includes a target body position / posture angle or a target foot position / posture angle. That is, the legged robot according to the present invention includes a posture angle deviation calculating means for obtaining a deviation between a target body position / posture angle and an actual body posture angle, and a body portion based on the posture angle deviation calculated by the posture angle deviation calculating means. An inversion control means for calculating a correction amount, a torso correction means for correcting the target torso position / posture angle with the torso correction amount calculated by the inversion control means, a distance sensor provided on the back surface of the foot, A contact degree calculation means for calculating a contact degree of the foot with respect to the ground based on a value of the distance sensor, wherein the contact degree calculation means calculates the trunk part correction amount calculated by the inversion control means based on the calculated contact degree. Is to be corrected.

  Alternatively, posture angle deviation calculating means for obtaining a deviation between the desired foot position / posture angle and the actual foot posture angle, and inversion for calculating a foot portion correction amount based on the foot angle deviation calculated by the posture angle deviation calculating means. Control means; foot part correction means for correcting the target foot position / posture angle with the foot part correction amount calculated by the inversion control means; a distance sensor provided on the back surface of the foot; Contact degree calculation means for calculating a contact degree of the foot with respect to the ground based on the value, wherein the contact degree calculation means corrects the foot portion correction amount calculated by the inversion control means based on the calculated contact degree. To do.

  In the present invention, the degree of ground contact with the ground of the foot is calculated for each distance sensor based on the value of the distance sensor, and the correction amount of the inversion control means is corrected according to the degree of ground contact. Control in consideration of the state becomes possible. Here, the degree of ground contact is a value obtained by setting a state where the sole is not in contact with the ground as 0, and a state where the sole is sufficiently in contact as 1, and connecting these with a linear function or the like.

  Further, a foot position / posture deviation calculating means for obtaining a deviation between a relative target foot position / posture viewed from the ground and an actual foot position / posture, and the foot deviation calculated by the foot position / posture deviation calculating means A copy control unit that calculates a leg correction amount based on the image, and a foot part correction unit that corrects a desired foot position / posture angle with the leg correction amount calculated by the copy control unit; The gains of the inversion control means and the copying control means can be adjusted based on the calculated degree of contact. Thereby, the correction amount of the inversion control can be increased or the correction amount of the copying control can be increased according to the degree of contact.

  Furthermore, a correction amount adjusting unit is provided for determining a contact direction of the foot with respect to the ground from the contact degree calculated by the contact degree calculating unit, and adjusting the trunk portion correction amount based on the contact direction. The torso correction amount can be corrected in consideration of the direction of contact of the foot.

  Furthermore, a foot position / posture deviation calculating means for obtaining a deviation between a relative target foot position / posture viewed from the ground and an actual foot position / posture, and a foot deviation calculated by the foot position / posture deviation calculating means. And a foot correction unit that corrects the desired foot position / posture angle with the leg correction amount calculated by the scanning control unit.

  Further, target acceleration calculation means for calculating a target body acceleration from a target body position, an acceleration sensor, an acceleration deviation calculation means for obtaining a deviation between the current acceleration acquired from the acceleration sensor and the target acceleration, and the acceleration deviation The apparatus may further include an acceleration control unit that calculates an acceleration correction amount based on the body, and a body position correction unit that corrects the target body position based on the acceleration correction amount calculated by the acceleration control unit.

  Furthermore, based on the degree of contact, the body correction amount is converted into a ground coordinate system that is a coordinate system whose main axis is the direction in which the foot is in contact with the ground, and the ground coordinate system is converted into the ground coordinate system. Correction amount correction means for correcting the fuselage correction amount in accordance with a grounding state indicating the degree of ground contact, and the coordinate conversion means uses the absolute coordinate system to correct the trunk correction amount corrected by the correction amount correction means. The body correction means can correct the target body position / posture angle with the body correction amount converted into the absolute coordinate system. Since the correction amount is corrected by converting to the ground coordinate system, the correction amount can be calculated in a direction according to the ground contact state of the sole.

  The legged robot control method according to the present invention is based on the posture angle deviation calculating step for obtaining a deviation between the target body position / posture angle and the actual body posture angle, and the posture angle deviation calculated in the posture angle deviation calculating step. An inversion control step for calculating a torso correction amount, a torso correction step for correcting the target torso position / posture angle with the torso correction amount calculated in the inversion control step, and a distance sensor provided on the back of the foot A contact degree calculation step of calculating a contact degree with respect to the ground for each distance sensor based on the value of the distance sensor. In the contact degree calculation step, the body part correction calculated in the inversion control step based on the calculated contact degree Correct the amount.

  ADVANTAGE OF THE INVENTION According to this invention, even if it is an uneven surface, the legged robot which can perform a stable walk by performing inversion control and copying control with good balance, and its control method can be provided.

Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In this embodiment, the present invention is applied to a legged mobile robot that performs copying control and inversion control.
Embodiment 1.

  FIG. 1 is a schematic view schematically showing a state in which the robot 1 is viewed from the front, and shows a state in which the robot 1 walks on the floor F. In FIG. The x direction is the direction in which the robot travels (front-rear direction), the y-axis is the direction orthogonal to the horizontal direction (left-right direction) in the direction in which the robot 1 travels, and the direction (vertical direction) extends in the vertical direction from the plane on which the moving body moves. A description will be given using the coordinate system consisting of these three axes as the z-axis. That is, in FIG. 1, the x-axis indicates the depth direction of the paper surface, the y-axis indicates the left-right direction toward the paper surface, and the z-axis indicates the vertical direction in the paper surface. This coordinate system is set to an absolute coordinate system (world coordinate system) Og-XgYgZg.

  As shown in FIG. 1, the robot 1 has a head 2, a trunk 3, a waist 4 coupled to the trunk 3, a right arm 5, a left arm 6, and a waist 4 connected to the trunk 3. And a leg 10 that is pivotably fixed. Details will be described below.

  The head 2 includes a pair of left and right imaging units (not shown) for visually imaging the environment around the robot 1, and the head 2 is parallel to the trunk 3 in the vertical direction. By rotating around the axis, the surrounding environment is imaged widely. The captured image data indicating the surrounding environment is transmitted to the control unit 130 and used as information for determining the operation of the robot 1.

  The trunk 3 houses therein a control unit 130 that controls the operation of the robot 1, a battery (not shown) for supplying power to a leg motor and the like. The control unit 130 includes a storage area that stores gait data for driving the leg 10 and moving the robot 1, an arithmetic processing unit that reads gait data stored in the storage area, and the leg 10. A motor driving unit for driving the motor. Each of these components operates by being supplied with electric power from a battery (not shown) provided inside the trunk 3. Here, the trunk coordinate system Ob-XbYbZb is set for the trunk. A later-described target body position is represented by the position of the origin Ob of the absolute coordinate system Og-XgYgZg. Further, a target body position / posture angle, which will be described later, is represented by an inclination of the body coordinate system Ob-XbYbZb with respect to the absolute coordinate system Og-XgYgZg.

  The arithmetic processing unit reads out the gait data stored in the storage area and calculates the joint angle of the leg 10 necessary to realize the posture of the robot 1 specified by the read out gait data. And the signal based on the joint angle computed in this way is transmitted to a motor drive part.

  The motor drive unit specifies the drive amount of each motor for driving the leg portion based on the signal transmitted from the arithmetic processing unit, and the motor drive signal for driving the motor with these drive amounts is sent to each motor. Send to. As a result, the driving amount at each joint of the leg 10 is changed, and the movement of the robot 1 is controlled.

  In addition to instructing the motor to be driven based on the read gait data, the arithmetic processing unit receives a signal from a sensor (not shown) such as a gyroscope or an accelerometer built in the robot 1, Adjust the drive amount of the motor. Further, a distance to the floor surface F may be detected by providing a laser sensor or the like. Various sensors such as a gyro sensor, an accelerometer, and a laser sensor are provided on the trunk 3 and the waist 4, for example. Thus, the robot 1 can be maintained in a stable state by adjusting the joint angle of the leg 10 according to the external force acting on the robot 1 detected by the sensor, the posture of the robot 1, and the like.

  The right arm 5 and the left arm 6 are pivotally connected to the trunk 3, and perform the same movement as a human arm portion by driving joint portions provided at the elbow portion and the wrist portion. Can do. Further, the hand part connected to the tip of the wrist part has a hand structure for gripping an object (not shown), and by driving a plurality of finger joints incorporated in the hand structure, various kinds of parts are provided. It becomes possible to grip an object having a shape.

  The lumbar part 4 is connected to the trunk 3 so as to rotate, and the driving energy necessary for driving the leg part 10 can be obtained by combining the rotational action of the lumbar part 4 when performing a walking action. Can be reduced.

  A leg 10 (right leg 20 and left leg 30) for bipedal walking is composed of a right leg 20 and a left leg 30. Specifically, as shown in FIG. 1, the right leg 20 includes a right hip joint 21, an upper right thigh 22, a right knee joint 23, a right lower thigh 24, a right ankle joint 25, and a right foot 26. A left hip joint 31, a left upper thigh 32, a left knee joint 33, a left lower thigh 34, a left ankle joint 35, and a left foot 36 are provided.

  The right leg 20 and the left leg 30 are each driven to a desired angle by transmitting a driving force from a motor (not shown) via a pulley and a belt (not shown). A desired movement can be made to a leg part. Here, for the foot, the foot coordinate system Of-XfYfZf is set. A later-described target foot position is represented by the position of the origin of the foot coordinate system Of-XfYfZf with respect to the absolute coordinate system Og-XgYgZg. The desired foot position / posture angle is represented by the angle of the sole surface with respect to the ground contact surface.

  The gait data is associated with the amount of movement of the leg 10 specified by a signal sent from the operation unit (not shown), and the tip (foot) of the foot of the leg 10 (the right foot 26 and the left foot 36). The previous position and the position of the main body of the moving body are indicated with time in a coordinate system (for example, an xyz coordinate system) that defines a space in which the robot 1 moves. The gait data includes a target body position, a target body position / posture angle, a target foot position, a target foot position / posture angle, and the like. The gait data is created so that the robot 1 can walk stably, that is, without falling down, by simulation or the like before actually operating the robot 1. The created gait data is stored in the control unit (controller) 130 of the robot 1. The control unit controls each joint so that the actual body position or the like follows the target body position or the like included in the gait data.

  Furthermore, the robot according to the present embodiment is provided with a distance sensor (not shown) that measures the distance to the floor surface F on the right foot 26 and the left foot 36. Then, the distance to the floor measured by the distance sensor is input to the control unit 130 as a sensor signal. Based on this sensor signal, the control unit performs feedback control. For example, the ankle joints (the right ankle joint 25 and the left ankle joint 35) are driven so that the soles of the foot portions follow the floor surface F. Thereby, the robot can walk stably.

  Next, gait data will be described. The target foot position is represented by the position of the origin Of of the foot coordinate system with respect to the absolute coordinate system. The desired foot position / posture angle is represented by time-dependent data of the angle of the back surface of the foot with respect to the ground contact surface F. The desired foot position / posture angle may be represented by time-dependent data of the inclination of the foot coordinate system with respect to the absolute coordinate system. Whether the desired foot position / posture angle is represented by time-dependent data of the angle of the back surface of the foot with respect to the ground contact surface F or the inclination of the foot coordinate system relative to the absolute coordinate system, both are substantially equivalent. It is. This is due to the following reason. The contact surface when creating the desired foot position / posture angle of the gait data is a virtual contact surface set in the simulation. The inclination of the virtual ground plane in the absolute coordinate system may be set based on the actual slope of the ground plane, or may be set to a temporary slope regardless of the actual slope of the ground plane. In any case, the inclination of the virtual ground plane in the simulation with respect to the absolute coordinate system is set. Therefore, even if the desired foot position / posture angle is expressed by the angle of the foot back surface with respect to the ground contact surface or by the inclination of the foot coordinate system with respect to the absolute coordinate system, both are substantially equivalent. Hereinafter, the actual position and posture angle of the foot are referred to as the actual foot position and the actual foot posture angle, respectively.

  The actual foot posture angle is represented by an angle (inclination) between the actual ground contact surface and the back surface of the foot, and can be detected by a distance sensor provided on the foot as will be described later. For easy comparison with the actual foot posture angle, it is assumed that the target foot position / posture angle is represented by the inclination of the virtual foot bottom surface with respect to the virtual ground contact surface.

  The target body position is represented by time-dependent data of the relative position of the origin Ob of the body coordinate system with respect to the target foot position. The target body position may be represented by the position of the origin Ob of the body coordinate system with respect to the absolute coordinate system. Both are substantially equivalent. This is because, in creating gait data, it is equivalent to directly expressing the target body position as a position with respect to the absolute coordinate system and indirectly expressing it through the target foot position.

  The target body position / posture angle is represented by time-dependent data of the inclination of the body coordinate system with respect to the absolute coordinate system. The actual trunk posture angle can be detected by a posture angle sensor provided on the trunk. The actual position and posture angle of the body are referred to as the actual body position and the actual body posture angle, respectively. The posture angle sensor includes, for example, a gyro that detects the angular velocity of the body, an integrator that integrates the output (angular velocity) of the gyro, and a triaxial acceleration sensor that detects the magnitude and direction of gravitational acceleration.

FIG. 2 is a control block diagram of the robot according to the present embodiment. As shown in FIG. 2, the robot control system according to the present embodiment includes a storage unit 111 that stores gait data 111a, a target distance calculation unit 112, a distance deviation calculation unit 113, and foot deviations (Δφ, Δθ, Δz). ) Calculation unit 114, scanning control unit 115, foot coordinate position / posture angle correction unit (adder) 116, ^second- order differentiator (s ² ) 117, acceleration deviation calculation unit (adder) 118, acceleration control unit 119, posture Angular deviation calculation unit (adder) 120, inversion control unit 121, coordinate conversion unit 122, correction amount correction unit 123, body position correction unit (adder) 124, body position / posture angle correction unit (adder) 125, joint angle A conversion unit 126 and each axis control unit 127 are provided. Further, a sole distance sensor 131 is provided on the back surface of the foot of the robot, and a posture sensor 132, an acceleration sensor 133, and the like are provided on the trunk.

  First, an outline of the control performed in such a control system will be given. The target position / posture angle of the foot and the target position / posture angle of the torso of the gait data 111a are input to the joint angle conversion unit 126 after various correction amounts are added. The correction amount will be described later. The joint angle conversion unit 126 converts the target position / posture angle of the foot and the target position / posture angle of the torso part into joint angles of the joints of the legs (inverse kinematics conversion). This conversion is called so-called inverse conversion. The joint angle obtained by the joint angle conversion unit 126 is sent as a joint angle command value to each axis control unit 127 which is a motor driver. Each axis control unit 127 drives a motor 141 attached to each joint of the leg and controls so that the joint angle of each joint matches the joint angle command value sent from the joint angle conversion unit 126. As a result, the legged robot 1 can walk while following the target position / posture of the foot and torso of the gait data.

  Next, each process in this control system will be described. First, the control of the copying control unit 115 will be described. Of the gait data, the foot target position / posture angle is sent to the target distance calculation unit 112. The target distance calculation unit 112 obtains the distance between the walking surface and the foot on the gait data from the foot target position / posture angle data. More specifically, the target distance calculation unit 112 calculates a target distance that is a distance from the back surface of the foot to which the distance sensor is attached to the walking surface. The foot target position / posture angle is a target position and posture angle at a reference point installed (fixed) on the foot. Since the positional relationship between the reference point and the distance sensor is known, the distance between the attachment point of the distance sensor and the virtual walking surface can be calculated from the foot target position / posture angle of the reference point. In the present embodiment, since four distance sensors are attached to one foot, the target distance calculation unit 112 calculates four target positions for one foot.

  In the present embodiment, the walking plane at the time of target value calculation is a plane that coincides with the xy plane of the absolute coordinate system. Accordingly, the value of the position in the z-axis direction of the foot target position / posture angle data expressed in the absolute coordinate system xyz is directly the target distance between the walking surface and the foot at the foot reference point. .

  The target distance obtained by the target distance calculation unit 112 is sent to the distance deviation calculation unit 113. Further, the measured value from the sole distance sensor 131 is also input to the distance deviation calculation unit 113. The distance deviation calculation unit 113 calculates, for each distance sensor 131, the deviation between the actual distance between the walking surface measured by the distance sensor 131 and the back of the foot and the above-described target distance. Here, the sole distance sensor according to the present embodiment uses a distance sensor that can accurately measure the distance to the ground only after contact, but until it approaches a predetermined distance, the robot The force that influences the posture of the child is virtually not received. Non-contact measurement may be performed by an optical method or the like, and an equivalent effect can be obtained.

  Foot tracking control is not performed over the entire length of the free leg period of the foot. Therefore, when the distance target value and the measured value between the foot and the ground are outside the measurable range of the distance sensor, the copying control is not performed and the distance deviation is set to zero. Further, when the foot is landing, if a small steady-state deviation occurs in the distance deviation due to, for example, a rounding error at the time of calculation, the integral amount of the steady-state deviation by the integral term of the scanning control unit 115 that performs the control is a correction amount. Will join. Even if the rounding error at the time of calculation is small, the integration amount may become so large that it cannot be ignored for control. Therefore, it is necessary to prevent such steady deviation from affecting the correction amount. Here, various types of control are possible, such as PID control, PID with various compensations, feedback control based on transfer functions, and control based on modern control theory.

  Therefore, in the present embodiment, a dead zone is provided so as to limit the target value and the measured value of the distance between the foot and the ground to a predetermined measurable range. That is, the amount of divergence of the scanning control correction amount due to a minute error in the distance deviation between the foot and the ground is prevented.

  The foot deviation calculation unit 114 receives a deviation of the sole distance sensor. This is converted into foot position / posture deviations Δφ, Δθ, Δz. Δφ and Δθ are deviations in posture angle around two straight lines that define a plane parallel to the ground, and Δz is a positional deviation in a direction perpendicular to the ground.

  The obtained foot position / posture deviations Δφ, Δθ, Δz are input to the copying control unit 115. The copying control unit 115 is constructed to properly converge the following feedback loop. That is, the measured value of the distance sensor 131 attached to the leg is a distance deviation calculation unit 113, a foot deviation calculation unit 114, a copying control unit 115, a foot coordinate position / posture angle correction unit 116, a joint angle conversion unit 126, each axis. This is a loop that affects the motor 141 that drives the joint of the leg via the control unit 127. Since the specific function of the copying control unit 115 is the same as that of a general PID controller, description thereof is omitted here.

  The correction amount calculated by the scanning control unit 115 is input to the foot coordinate position / posture angle correction unit 116. The foot coordinate position correction unit 116 receives the foot target position / posture angle from the storage unit 111 and corrects the foot target position / posture angle by adding the correction amount calculated by the copying control unit 115 to the foot target position / posture angle. Input to the joint angle conversion unit 126. The actual foot posture angle can be matched with the desired foot position / posture angle by the control by the copying control unit 115. That is, for a leg that is in contact with the ground, control is performed so that the back surface of the foot is in surface contact with the ground surface and the back surface of the foot is made to follow the ground.

  Next, the acceleration control unit 119 and the inversion control unit 121 will be described. The second-order differentiator 117 calculates the target body acceleration by performing the second-order differentiation of the target body position among the target body position / posture angle. Since the control system is configured as a digital control system, the differentiation of the target body position is more accurately obtained as an approximate value by the difference of the time series data of the target body position. The acceleration deviation calculation unit 118 receives the target body acceleration and the measurement value from the acceleration sensor 133 attached to the body part, and outputs these deviations. The acceleration control unit 119 performs acceleration feedback control of the body part based on the acceleration deviation. For example, a control system based on a transfer function such as a proportional coefficient and a second-order delay element is configured in the acceleration control unit. The acceleration control unit 119 converts the acceleration deviation into a position dimension, that is, a body position correction amount, and outputs it.

  Next, the posture angle deviation calculation unit 120 receives the target body posture angle from the storage unit 111 and the body posture angle from the posture sensor 132 attached to the body unit. The posture angle deviation calculation unit 120 calculates the deviation between the two and inputs it to the inversion control unit 121. The inversion control unit 121 calculates a correction amount by feedback control based on the posture angle deviation. The inversion control unit 121 is a control unit for reducing the body position deviation and the body posture angle deviation, that is, for matching the actual body position and posture angle with the target body position and posture angle. The presence of the body posture angle deviation, that is, the mismatch between the actual body position / posture angle and the target body position / posture angle is often caused by an unexpectedly inclined foot. When the foot is inclined, the horizontal deviation between the position of the torso and the position of the foot becomes large and the robot is likely to fall down. Therefore, the correction amount of the body position is calculated by geometric calculation so that the actual body posture angle matches the target body position posture angle and the horizontal deviation between the body position and the foot position is also reduced. By correcting the position, the robot can be prevented from falling over. Therefore, the target body position / posture angle is corrected based on the body posture angle deviation. However, if the body posture angle is simply corrected based on the posture angle deviation, if the deviation is large (even on flat ground), the output of the inversion control will be too large, the sole will be lifted off the ground, and will eventually fall There is.

  Therefore, in the present embodiment, instead of feedback control for correcting the target body position / posture angle with the correction amount calculated by the inversion control unit 121, the correction amount is further corrected based on the degree of contact with the ground of the foot, The body position / posture angle is corrected by the corrected correction amount. In this way, by further correcting the correction amount generated by the inversion control unit 121, the body inversion control is limited when the soles are likely to come off, and the state where the soles are always along the ground is given priority. Can be controlled.

  First, the contact degree calculation unit 129 will be described. The sensor value from the sole distance sensor 131 is input to the contact degree calculation unit 129. And the value of this distance sensor is normalized to 0-1. This value is referred to as the degree of ground contact s. The degree of ground contact s indicates that the larger the value is, the more grounded the ground. FIG. 3 is a graph showing the relationship between the degree of ground contact and the value from the distance sensor. It can be set to 1 when it is sufficiently grounded, and 0 when it is not grounded, which can be a continuously connected function. In this example, a linear function within the measurement range is shown, but a sigmoid function or the like may be used. However, in the present embodiment, as will be described later, in order to prevent the case where the zero division or the axis is not determined in the coordinate conversion procedure to the ground coordinate system described later, the minimum value of the ground contact level is not set to 0 but is a minute value. ε (0 <ε <1) was set.

Specifically, when the degree of ground contact is 0 <ε ≦ s ≦ 1 and the sensor distance h is 0 <h _low <h <h _high , if the sensor distance h is equal to or lower than h _low , the degree of ground contact is 1 and the sensor distance is A monotonically decreasing function of 1 ≧ s ≧ ε during h _low ≦ h ≦ h _high , and a contact degree ε when the sensor distance h is greater than h _high .

  The grounding degree calculation unit 128 receives the grounding degree s calculated by the grounding degree calculation unit 129. Then, by using the contact degree s, a coordinate system (hereinafter referred to as a contact coordinate system) having the most grounded direction as a main axis and its contact degree are expressed using eigenvectors and eigenvalues. An eigenvector and an eigenvalue (hereinafter referred to as a ground state) are input to the coordinate conversion unit 122 and the correction amount correction unit 123.

  A plane representing the slope of the foot with respect to the ground can be represented by the direction of the maximum slope and the direction of the minimum slope of the foot with respect to the ground. Since the gradient is represented by a plane, the direction of the maximum gradient and the direction of the minimum gradient are orthogonal. The ground coordinate system is a coordinate system having the maximum gradient direction and the minimum gradient direction as two axes.

In order to simplify the description, it is assumed that the ground plane coincides with the XwYw plane in the ground coordinate system Ow-XwYwZw. Then, the ground contact coordinate system can be expressed as a coordinate system obtained by rotating the Xw axis and the Yw axis of the absolute coordinate system by a predetermined angle around the vertical axis (Zw axis). The one axis of the ground coordinate system is represented by V _1, the other axis and V _2. How to obtain the axes V ₁ and V ₂ will be described later. Absolute coordinate system Xw axis, V ₁ axis and Yw axis ground coordinate system, the relation of V ₂ axis in FIG. 4 schematically represents. The plane in FIG. 4 represents the ground plane. One axis V ₁ of the ground coordinate system is a position obtained by rotating the Yw axis of the absolute coordinate system clockwise by an angle β. Other axis V ₂ of the ground coordinate system is a position where the Xw axis of the absolute coordinate system is rotated by an angle β in the clockwise direction.

FIG. 4 also shows a state in which the foot of the right leg and the foot of the left leg are both in contact with the ground. The center of the circle drawn in each sole shows the position of the distance sensor. The size of the circle schematically represents the size of the contact degree s at the position of each distance sensor. As will be described later, the axis V ₁ of the ground coordinate system indicates the direction in which the sole is in contact with the ground most (the slope of the sole is small), and the axis V ₂ is the sole on the ground. On the other hand, the direction where the ground is not touched most (the slope of the sole is large) is shown. A small gradient of the degree of ground contact s means that the bottom surface of the foot is grounded equally to the ground surface. Therefore, when the robot 1 changes the body posture angle in a plane including the axis V ₁ and the vertical line, it can be said that the bottom surface of the foot is hardly lifted from the ground contact surface. When changing the body posture angle in a plane containing the axis V ₂ and the vertical line is sole surface may be said to be prone state floating from the ground plane. Accordingly, if the correction amount of the target body position / posture angle is corrected using the axes V ₁ and V ₂ of the ground coordinate system as follows, the back surface of the foot can be made difficult to lift from the ground surface.

First, components in the directions of the axes V ₁ and V ₂ are obtained from the correction amount of the target body position / posture angle. The component in each direction is decreased as the gradient of the corresponding axis increases. By doing so, the component in the direction of the correction amount of the target body position / posture angle can be reduced in the direction in which the slope of the foot is large with respect to the ground, that is, the direction in which the sole is not in contact with the ground. By doing so, it is possible to effectively reduce the back surface of the foot from being lifted from the ground contact surface when the body posture angle is changed.

A method for obtaining the axes V ₁ and V ₂ of the ground coordinate system will be described. Axis _{V 1} and _{V 2} of the ground coordinate system, because it contains the XwYw plane definitive absolute coordinate system Ow-XwYwZw, for vertical Zw in the absolute coordinate system Ow-XwYwZw is omitted below. Here, the processing of the ground state calculation unit 128 will be described in the case where n / 2 sensors are arranged on one foot. The sensor position (x, y) in absolute coordinates is obtained.
^W r _i = ^W T _r ^r r _i (i = 0, 1,..., N / 2-1)
^W r _i = ^W T _l ^l r _i (i = n / 2,..., N−1)
However,
^W r _{i: i-th} sensor position in the absolute coordinate
^r r _i , ^l r _i : i-th sensor position in the right foot / left foot coordinate system
^W T _r, ^W T _l: right foot / target position and posture matrix left foot coordinate system origin ground center position as the weight of the ground level s of each sensor _x m, determine the _{y m.}

但し、
ｘ_ｍ，ｙ_ｍ：接地中心位置
ｘ_ｉ，ｙ_ｉ：ｉ番目のセンサ位置（ｘ，ｙ）
ｓ_ｉ：ｉ番目のセンサの接地度
各センサの接地度を重みとして分散、共分散を求める。

However,
x _m , y _m : grounding center position x _i , y _i : i-th sensor position (x, y)
s _i : Grounding degree of the i-th sensor The variance and covariance are obtained using the grounding degree of each sensor as a weight.

分散共分散行列Ｓの固有値λ、固有ベクトルＶを求める。
但し、下記を満たす。

The eigenvalue λ and eigenvector V of the variance-covariance matrix S are obtained.
However, the following is satisfied.

Here, the minimum value of the contact degree s is set to ε that is not zero in order to ensure that the variance-covariance matrix S is a regular matrix. Since the covariance matrix S is a regular matrix, it is guaranteed that there are _two eigenvalues λ ₁ and λ _{2 and} that the eigenvectors V ₁ and V ₂ are independent. That is, the eigenvectors V ₁ and V ₂ are orthogonal. Assuming λ ₁ and λ _{2 in} descending order of eigenvalues and the corresponding eigenvectors as V ₁ and V ₂ , the respective eigenvectors obtained here are
V ₁ : A direction in which the slope of the sole is minimum V ₂ : A direction in which the slope of the sole is maximum is expressed, and the contact degree is represented by eigenvalues λ ₁ and λ ₂ . Xw axis of the ground plane, and as schematic diagram of a Yw-axis, V ₁ vector, indicating the relationship between V ₂ vectors FIG.
Normalization is performed in order to use eigenvectors for coordinate transformation, and sign inversion is appropriately performed so that the x coordinate of V ₁ is positive and the y coordinate of V ₂ is positive.

When the correction amount of the target body position / posture angle is expressed by a vector Δθ, the vector θ is expressed by each axis component (Δθ _x , Δθ _y , Δθ _z ) of the absolute coordinate system Ow-XwYwZw. Here, the correction amount Δθ _z around the vertical axis Zw does not affect the fact that the back surface of the foot rises from the ground contact surface. Therefore, the in-ground surface components (Δθ _x , Δθ _y ) of the correction amount of the target body position / posture angle are converted into the V ₁ axis and V ₂ axis components described above. The converted V ₁ axis direction component is represented by Δθ ₁ , and the V ₂ axis direction component is represented by Δθ ₂ . In order to convert the correction amount (Δθ _x , Δθ _y ) represented by the Xw axis direction component and the Yw axis direction component of the absolute coordinate system into the V ₁ axis direction component Δθ ₁ and the V ₂ axis direction component Δθ ₂ (absolute coordinate system The conversion matrix ^w A _{grd (} for converting from the ground coordinate system to the ground coordinate system) can be expressed by the following equation using the eigenvectors V ₁ and V ₂ described above.

Next, processing of the coordinate conversion unit 122 will be described. If the posture correction amount (controller output) of the inverted control in the absolute coordinate system (world coordinate system) is (Δθ _x , Δθ _y ) ^T , the posture correction amount (Δθ ₁ , Δθ ₂ ) ^T of the inverted control in the ground coordinate system Can be obtained as follows.

Next, the correction amount correcting unit 123 limits the correction amount in each direction of the ground coordinate system in accordance with the degree of ground contact. Here, a function that limits the correction amount is represented by f and described as follows.
Δθ _1f = f (λ ₁ , Δθ ₁ )
Δθ _2f = f (λ ₂ , Δθ ₂ )
As the function f for limiting the correction amount, for example, the following function may be employed.
f (λ, Δθ) = α (λ) · Δθ
However, α represents a correction magnification used for restriction, and a function of λ can be, for example, as shown in the graph of FIG. That is, when the eigenvalue λ is larger than the threshold value _λth , that is, when sufficiently grounded, there is no need to limit the correction amount, and when the eigenvalue λ is smaller than the threshold value _λth , that is, not sufficiently grounded. In this case, the correction amount is limited to the correction magnification α.

This is because when the gradient of the sole is greater than a predetermined value (i.e., if the eigenvalues lambda ₁ is predetermined magnitude lambda _th smaller), the larger the slope of V ₁ axial soles (i.e., the eigenvalues lambda ₁ The smaller the value, the smaller the V ₁ axial component Δθ ₁ of the correction amount Δθ. Similarly, when the sole gradient is larger than a predetermined value (that is, when the eigenvalue λ ₂ is smaller than the predetermined size λ _th ), the larger the sole gradient in the V ₂ axis direction is (that is, the eigenvalue λ _2). The smaller the value, the smaller the V ₂ direction component Δθ ₂ of the correction amount Δθ.

The corrected posture correction amount for the inverted control is again converted into absolute coordinates by the coordinate converter 122. The coordinate conversion unit 122 multiplies the inverse matrix of the conversion matrix in order to return from the ground coordinate system to the absolute coordinate system. ^Here, W _{A grd} it is because orthogonal matrix, is equal to the transposed matrix and inverse matrix.

以上の手順で計算した（Δθ_ｘｆ，Δθ_ｙｆ）^Ｔを実際の倒立制御の補正量として用いれば、十分に接地が保たれている方向に倒立制御を利かせ、逆に接地が保たれていない場合には、足裏の路面倣い制御を優先して働かせることとなる。

If (Δθ _xf , Δθ _yf ) ^T calculated by the above procedure is used as a correction amount for the actual inversion control, the inversion control is used in the direction in which the grounding is sufficiently maintained, and conversely, the grounding is not maintained. In this case, the road surface copying control of the sole is preferentially performed.

  The correction amount returned to the absolute coordinates is input to the body position correction unit 124 and added with the correction amount from the acceleration control unit 119. Further, the added correction amount is input to the body position / posture angle correction unit 125 and added to the target body position / posture angle. The corrected target body position and target body position / posture angle are input to the joint angle conversion unit 126.

  The foot target position / posture angle includes a target foot position and a target foot position / posture angle. Although the point at which the desired foot posture angle is corrected by the copying control unit 115 has been described above, the target foot position can also be corrected. The target foot position can be corrected as follows. From the measured value of the sole distance sensor, the distance between the foot coordinate system origin Of and the ground plane is obtained. This distance indicates the actual foot position relative to the ground, the target foot position (the distance between the virtual ground plane and the virtual foot on the simulation when creating gait data) and the actual foot position. Based on the position deviation, the correction amount of the target foot position is calculated, and the target foot position is corrected by adding the calculated correction amount to the target foot position, and is input to the joint angle conversion unit 126. By such a control system, the actual foot position can be matched with the target foot position. For example, the timing at which the free leg of the actual robot contacts the actual ground plane can be matched with the timing at which the free leg lands on the ground plane on the gait data.

  FIG. 6 is a diagram for explaining the effect of the present embodiment. If the robot lands on the convex part and the sole is likely to come off, the control input of the inversion control is limited. As a result, the control that always keeps the sole on the ground is given priority.

Next, a method for correcting the body posture angle in accordance with the degree of contact will be described with reference to the flowchart shown in FIG. First, the distance from the sole to the ground is measured for each sole distance sensor (step S1). Then, the degree of ground contact s is obtained from the graph shown in FIG. 3 based on the measured sensor distance (step S2). When the contact degree s is obtained, a variance covariance matrix S is obtained, and its eigenvalues λ ₁ and λ ₂ and eigenvectors V ₁ and V ₂ are obtained. Then, a transformation matrix from the absolute coordinate system to the ground coordinate system is obtained from this eigenvector (step S3).

  Next, the correction amount calculated by the inversion control unit 121 is converted into the ground coordinate system using the conversion matrix obtained in step S3 (step S4). Then, this is corrected based on the correction magnification shown in FIG. 5 according to the grounding state (step S5). Finally, the corrected posture correction amount of the inversion control unit 121 is returned to the absolute coordinate system using the inverse matrix of the above-described conversion matrix (step S6). By correcting the target body position / posture angle using this correction amount, it is possible to control so as not to leave the sole of the foot from the ground.

In the present embodiment, the grounding direction of the foot is obtained based on the degree of grounding of the foot with respect to the ground. Then, by converting the correction amount calculated by the inversion control unit 121 into the ground coordinate system and correcting it according to the ground contact state of the foot, the sole can be grounded to the ground as much as possible. In other words, in order to correct the correction amount according to the grounding state, inversion control is applied in the direction where the grounding is sufficiently maintained, and on the contrary, the scanning control of the sole is given priority in the direction where the grounding is not maintained. By making it work, you can control so that the sole of the foot always follows the ground.
Embodiment 2.

  Next, a second embodiment of the present invention will be described. FIG. 8 is a block diagram showing a robot control system according to the present embodiment. In the present embodiment shown in FIG. 8, the same components as those in the first embodiment shown in FIG. 2 are denoted by the same reference numerals, and detailed description thereof is omitted. In the above-described embodiment, the correction amount calculated by the inversion control unit 121 is corrected according to the contact degree of the sole, but in the present embodiment, the calculation by the contact degree calculation unit 152 is performed. The gains of the copying control unit 115 and the inversion control unit 121 are adjusted based on the degree of contact.

  As a gain control method of the contact degree calculation unit 152, when the contact degree is larger than a predetermined threshold and it can be determined that the sole is sufficiently grounded with respect to the ground, the control gain of the inversion control unit 121 is increased, and the reverse In addition, control is performed so that the gain of the sole copying control unit 115 is lowered. On the other hand, when the degree of ground contact is smaller than a predetermined threshold and the sole is not sufficiently grounded, the gain of the inversion control unit 121 is increased, and conversely, control is performed to increase the gain of the copying control unit 115.

  In the present embodiment, the gain of the copying control and the inversion control is adjusted using the degree of contact obtained based on the measured value of the sole distance sensor. Specifically, when the degree of ground contact is large, the gain of the inverted control is increased to increase the correction amount, and when the degree of ground contact is small, the gain of the inverted control is reduced to reduce the correction amount. As a result, the road surface copying control of the sole can be preferentially applied and the sole can be brought into close contact with the ground.

  It should be noted that the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention.

  For example, in the above-described embodiment, the hardware configuration has been described. However, the present invention is not limited to this, and arbitrary processing may be realized by causing a CPU (Central Processing Unit) to execute a computer program. Is possible. In this case, the computer program can be provided by being recorded on a recording medium, or can be provided by being transmitted via the Internet or another transmission medium.

It is the schematic showing a mode that the robot was seen from the front. It is a block diagram which shows the control system of the robot concerning embodiment of this invention. It is a graph which shows the relationship between a contact degree and the value from a distance sensor. Absolute coordinate system Xw axis is a schematic diagram showing the relationship of V ₁ axis, V ₂ axes and Yw axis ground coordinate system. It is a figure which shows the correction magnification (alpha) used for a restriction | limiting, and the function of eigenvalue (lambda). It is a figure explaining the effect of embodiment of this invention. It is a flowchart which shows the method which corrects the fuselage attitude | position angle in embodiment of this invention according to a contact degree. It is a block diagram which shows the control system of the robot concerning Embodiment 2 of this invention. It is a figure which shows the problem of the conventional robot which uses inversion control and copying control together. Similarly, it is a figure which shows the problem of the conventional robot which uses inversion control and copying control together.

Explanation of symbols

1 robot, 2 heads, 3 trunks,
4 waist, 5 right arm, 6 left arm,
10 legs, 20 right leg, 21 right hip joint,
22 right upper leg, 23 right knee joint, 24 right lower leg,
25 Right ankle joint, 26 Right foot, 30 Left leg,
31 left hip joint, 32 left upper thigh, 33 left knee joint,
34 left lower leg, 35 left ankle joint, 36 left foot,
111 storage unit, 111a gait data, 112 target distance calculation unit,
113 distance deviation calculation unit, 114 foot deviation calculation unit, 115 copying control unit,
116 foot coordinate position correction unit, 117th-order differentiator, 118 acceleration deviation calculation unit,
119 acceleration control unit, 120 posture angle deviation calculation unit, 121 inverted control unit,
122 coordinate conversion unit, 123 correction amount correction unit, 124 fuselage position correction unit,
125 body position / posture angle correction unit, 126 joint angle conversion unit, 127 each axis control unit,
128 grounding state calculation unit, 129, 152 grounding degree calculation unit, 130 control unit,
131 distance sensor, 132 attitude sensor, 133 acceleration sensor, 141 motor

Claims (6)

Posture angle deviation calculating means for obtaining a deviation between the target body position and posture angle and the actual body posture angle;
An inversion control means for calculating a body part correction amount based on the attitude angle deviation calculated by the attitude angle deviation calculating means;
A body part correcting unit that corrects the target body position / posture angle with the body part correction amount calculated by the inversion control unit;
Foot position / posture deviation calculating means for obtaining a deviation between the target foot position / posture and the actual foot position / posture;
A scanning control means for calculating a leg correction amount based on the foot deviation calculated by the foot position / posture deviation calculating means;
Foot part correcting means for correcting the desired foot position / posture angle with the leg correction amount calculated by the copying control means;
Joint angle conversion means for calculating a joint angle command value of each joint angle based on outputs of the trunk part correction means and the foot part correction means;
A distance sensor provided on the back of the foot;
Ground calculation means to calculate the ground level relative to the ground of the distance values in based-out the foot of the sensor flat,
Grounding state calculation for obtaining the direction of contact of the foot with respect to the ground, the direction in which the foot is best grounded and the direction in which the foot is not grounded from the grounding degree calculated by the grounding degree calculation means Means,
Coordinate conversion means for converting the torso correction amount into a ground coordinate system that is a coordinate system having a principal axis in a direction in which the foot is in contact with the ground best, based on the ground contact degree calculated by the ground contact degree calculation means;
Correction amount correcting means for correcting the body correction amount converted into the ground coordinate system according to a grounding state indicating a degree of grounding,
The coordinate conversion means converts the body correction amount corrected by the correction amount correction means into an absolute coordinate system,
The body portion correcting means legged robot for correcting the target torso position and orientation angle fuselage correction amount is converted into the absolute coordinate system. Target acceleration calculating means for calculating the target body acceleration from the target body position;
An acceleration sensor;
An acceleration deviation calculating means for obtaining a deviation between the current acceleration acquired from the acceleration sensor and the target acceleration;
Acceleration control means for calculating an acceleration correction amount based on the acceleration deviation;
The torso correction means corrects the target torso position / posture angle by the torso correction amount converted into the absolute coordinate system and the acceleration correction amount calculated by the acceleration control means.
The legged robot according to claim 1. Posture angle deviation calculating means for obtaining a deviation between the target body position and posture angle and the actual body posture angle;
An inversion control means for calculating a body part correction amount based on the attitude angle deviation calculated by the attitude angle deviation calculating means;
A body part correcting unit that corrects the target body position / posture angle with the body part correction amount calculated by the inversion control unit;
Foot position / posture deviation calculating means for obtaining a deviation between the target foot position / posture and the actual foot position / posture;
A scanning control means for calculating a leg correction amount based on the foot deviation calculated by the foot position / posture deviation calculating means;
Foot part correcting means for correcting the desired foot position / posture angle with the leg correction amount calculated by the copying control means;
Joint angle conversion means for calculating a joint angle command value of each joint angle based on outputs of the trunk part correction means and the foot part correction means;
A distance sensor provided on the back of the foot;
Contact degree calculation means for calculating a contact degree of the foot with respect to the ground based on a value of the distance sensor;
It said ground calculation hand stage legged robot that adjusts the gain of the inversion control means and the scanning control means based on the calculated ground level. A posture angle deviation calculating step for obtaining a deviation between the target body posture angle and the actual body posture angle;
An inversion control step of calculating a body part correction amount based on the posture angle deviation calculated in the posture angle deviation calculating step;
A body part correction step of correcting the target body position / posture angle with the body part correction amount calculated in the inversion control step;
A foot position / posture deviation calculating step for obtaining a deviation between the target foot position / posture and the actual foot position / posture;
A scanning control step of calculating a leg correction amount based on the foot deviation calculated in the foot position / posture deviation calculating step;
A foot portion correction step of correcting the desired foot position / posture angle with the leg correction amount calculated in the scanning control step;
A joint angle conversion step of calculating a joint angle command value of each joint angle based on the correction amount output in the trunk portion correction step and the foot portion correction step;
And more ground calculating Engineering for calculating the ground level to the value of the distance sensor provided on the back surface of the foot relative to the ground of based-out the foot,
A grounding state for obtaining a grounding direction with respect to the ground of the foot, the direction in which the foot is grounded best and the direction in which the foot is not grounded from the grounding degree calculated in the grounding degree calculation step Calculation process;
A coordinate conversion step of converting the torso correction amount into a ground coordinate system that is a coordinate system whose main axis is a direction in which the foot is in contact with the ground best, based on the ground contact degree calculated in the ground contact degree calculation step; ,
A correction amount correction step of correcting the body correction amount converted into the ground coordinate system according to a ground state indicating a degree of ground contact,
In the coordinate conversion step, the body correction amount corrected in the correction amount correction step is converted into an absolute coordinate system,
Wherein the fuselage correction process, the control method of a legged robot for correcting the target torso position and orientation angle fuselage correction amount is converted into the absolute coordinate system. A target acceleration calculation step of calculating a target body acceleration from the target body position;
An acceleration deviation calculating step for obtaining a deviation between the current acceleration acquired from the acceleration sensor and the target acceleration;
An acceleration control step of calculating an acceleration correction amount based on the acceleration deviation;
In the body part correction step, the target body position is corrected by the acceleration correction amount calculated in the acceleration control step.
The method for controlling a legged robot according to claim 4. A posture angle deviation calculating step for obtaining a deviation between the target body posture angle and the actual body posture angle;
An inversion control step of calculating a body part correction amount based on the posture angle deviation calculated in the posture angle deviation calculating step;
A body part correction step of correcting the target body position / posture angle with the body part correction amount calculated in the inversion control step;
A foot position / posture deviation calculating step for obtaining a deviation between the target foot position / posture and the actual foot position / posture;
A scanning control step of calculating a leg correction amount based on the foot deviation calculated in the foot position / posture deviation calculating step;
A foot portion correction step of correcting the desired foot position / posture angle with the leg correction amount calculated in the scanning control step;
A joint angle conversion step of calculating a joint angle command value of each joint angle based on the correction amount output in the trunk portion correction step and the foot portion correction step;
Have a higher ground calculation engineering to calculate the ground level to the value of the distance sensor provided on the back surface with respect to ground based-out the foot of the foot,
And in the ground level calculating step, based on the ground level, the inversion control process and tracking controlling method of a legged robot that adjusts the gain in calculating the amount of correction in the control process.

Images