JPH05245780A - Controller of leg type mobile robot - Google Patents

Abstract

PURPOSE:To maintain a stable position at all times even at the time of walking a road surface on which unexpected roughness is found, by installing an operational means, operating a second manipulated variable in order to compensate a detected variation according to it, adding the second manipulated variable operated into this specified manipulated variable, and giving it to a servomotor. CONSTITUTION:An actual angle, a desired angle and a deviation in a leg part link 2 are detected each with an absolute angle to the gravitational direction by a detecting means 36. According to this detected deviation, a second manipulated variable is operated in order to compensate it by an operational means 26. Next, a value added with the second manipulated variable operated into the specified manipulated variable is given to a servomotor, through which a stable walk is secured even on a road surface where unexpected roughness is found, in a biped mobile robot.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control system for a legged mobile robot, and more particularly to a bipedal mobile robot capable of stably walking even on a road surface having unexpected irregularities.

[0002]

2. Description of the Related Art In the case of a humanoid bipedal robot consisting of a base and two leg links connected to the base, the two leg links are alternately swung forward and landed while walking. Generally, a walking pattern that assumes a flat road is designed in advance, and the walking is realized by converting the joint pattern into a joint angle trajectory and controlling the robot joint angle so as to follow the trajectory without delay. .. The same applies to known stairs and slopes. An example thereof is, for example, Japanese Patent Laid-Open No. 62-970.
The technique described in Japanese Patent Publication No. 06-06 can be mentioned.

[0003]

However, in this method, when the road surface has unexpected irregularities, the angle of the foot (foot) of the supporting leg is not constant as when walking on a known road surface. Therefore, there may be a case where a person cannot walk stably according to a walking pattern designed on the basis of a known road surface, and in the worst case, the vehicle falls. Therefore, as one of the methods to make a person walk stably without falling even on a road surface with unknown irregularities, it is possible to design a posture above the foot with a walking pattern and control the actual posture to follow it. That is, instead of controlling the joint angle of the robot, it is conceivable to control the absolute angle of each link above the foot of the supporting leg, in other words, the posture of the robot in the direction of gravity. This is because, regardless of the angle of the foot of the support leg, if the posture above it is the target posture (the posture that does not fall), stable walking can be secured. The applicant of the present invention has previously proposed in Japanese Patent Application No. 2-259839 (filed on Sep. 28, 1990) using this idea.

However, since the technique proposed by the present applicant approximates the motion of the robot by a linear model, the walking pattern or the stride of the joint of the supporting leg corresponding to the knee moves extremely and the linearity is impaired. In order to realize smooth walking even in walking patterns, it was necessary to compensate for non-linearity, as is commonly done with manipulators. However, to compensate for the non-linearity, a high-speed computer must be installed in the robot controller,
Since such a high speed computer is large and heavy, it is not preferable to mount it on a mobile robot.

Therefore, an object of the present invention is to control the posture of a robot on the basis of a walking pattern designed in advance, and to perform nonlinear compensation using a high-speed computer for the walking pattern that impairs the linearity. It is an object of the present invention to provide a control device for a legged mobile robot that realizes a smooth walking without performing and can always ensure a stable posture even when unexpected bumps are encountered.

[0006]

In order to solve the above-mentioned problems, the present invention provides a servomotor arranged at each joint of a leg link of a legged mobile robot with a predetermined operation amount as shown in claim 1, for example. In the control device for controlling the angle of each joint to follow the preset target angle, the detecting means for detecting the deviation between the actual angle of the leg link and the target angle by the absolute angle with respect to the gravity direction, and the detected deviation According to the above, the control device calculates a second manipulated variable to correct it, and the control device adds the calculated second manipulated variable to the predetermined manipulated variable and adds the calculated second manipulated variable to the servo motor. Configured to give.

[0007]

[Function] Since the deviation between the actual angle of the leg link and the target angle is obtained as an absolute angle and the second operation amount is calculated and added to the operation amount of the joint angle servo system, the unevenness of the unevenness is obtained. Even when walking on a certain road surface, it is possible to always ensure stable walking. Further, it is possible to always realize smooth posture control even when walking on a flat road.

[0008]

Embodiments of the present invention will be described below by taking a bipedal robot as an example of a legged mobile robot. Figure 1
Is an explanatory skeleton diagram showing the robot 1 as a whole, and each of the left and right leg links 2 has six joints (axes) (for convenience of understanding, each joint (axis) is driven by an electric motor). Motor is illustrated). The six joints (axes) are, in order from the top, joints (axes) 10R and 10L for rotating the legs of the waist.
(R on the right side and L on the left side. The same applies hereinafter.) Joints (axes) 12R and 12L in the waist pitch direction (x direction), joints (axes) 14R and 14L in the roll direction (y direction), and knees. Pitch direction joints (axes) 16R and 16L, ankle pitch direction joints (axes) 18R and 18L, and roll direction joints (axes) 20R and 20L. 22R and 22L are attached, and a body portion (base body) 24 is provided at the uppermost position, and a control unit 26 is housed inside the body portion. In the above, the hip joint is composed of joints (axes) 10R (L), 12R (L), 14R (L), and the ankle joints are joints (axes) 18R (L), 2
It consists of 0R (L). Further, the hip joint and the knee joint are connected by thigh links 28R and 28L, and the knee joint and the ankle joint are connected by lower leg links 30R and 30L.

Here, the leg link 2 is provided with 6 degrees of freedom for each of the left and right legs, and these 6 degrees of freedom are provided during walking.
By driving each of the x2 = 12 joints (axes) to an appropriate angle, the desired movement can be given to the entire foot,
It is configured so that it can walk in a three-dimensional space arbitrarily. As mentioned above, the above-mentioned joint is composed of an electric motor, and is further provided with a speed reducer that boosts the output of the electric motor.
For details, refer to the application previously proposed by the applicant (Japanese Patent Application No.
No. 324218, Japanese Patent Laid-Open No. 3-184782) and the like, which are not the gist of the present invention, so further description will be omitted.

Here, in the robot 1 shown in FIG.
A well-known 6-axis force sensor 36 is provided on the ankle portion, and a force component F in the x, y, z directions is transmitted to the robot via the foot.
x, Fy, Fz and the moment component Mx around that direction,
By measuring My and Mz, the presence or absence of landing of the foot and the magnitude and direction of the force applied to the supporting leg are detected. Further, known grounding switches 38 are provided at the four corners of the foot to detect the presence or absence of grounding. Further, a pair of inclination sensors 40 and 42 are installed on the upper part of the body portion 24, and the inclination with respect to the z axis in the xz plane and its angular velocity, as well as the inclination with respect to the z axis in the yz plane and its angular velocity. To detect. Further, the electric motor of each joint is provided with a rotary encoder for detecting the amount of rotation thereof (only an electric motor for the ankle joint is shown in FIG. 1). Outputs of the sensors 36 and the like are sent to the control unit 26 in the body portion 24 described above.

FIG. 2 is a block diagram showing the details of the control unit 26, which is composed of a microcomputer. The outputs of the tilt sensors 40 and 42 are A
It is converted into a digital value by the / D converter 50, and its output is sent to the RAM 54 via the bus 52. The output of the encoder arranged adjacent to each electric motor is input into the RAM 54 via the reversible counter 56, and the outputs of the grounding switch 38 and the like are also stored in the RAM 54 via the waveform shaping circuit 62. .. An arithmetic unit 64 is provided in the control unit, reads the walking pattern stored in the ROM 66, calculates the speed command value of the electric motor from the deviation from the actual measurement value sent from the reversible counter 60, and calculates D / It is sent to the servo amplifier via the A converter 68. Further, as shown in the figure, the encoder output is sent to the servo amplifier via the F / V conversion circuit, and the speed feedback control as a minor loop is realized as shown in FIG.

Next, the operation of this control device will be described.

FIG. 3 shows the robot 1 shown in FIG.
It is a block diagram showing the case where a known joint angle servo system is constituted. Converting a walking pattern, which is designed off-line in advance on the premise of a flat road, into time series data of 12 joint angles qr and angular velocities qr dots, that is, a joint trajectory,
It is output to the servo amplifier of each joint as a command value at regular intervals. As a result, each joint angle follows the command value with almost no delay, and the robot 1 walks while being driven according to the designed walking pattern. As a result, walking on a flat road will not hinder, but when an unexpected bumpy road surface is encountered, it is not always possible to secure stable walking because the angle of the foot of the supporting leg is not constant, as described above. ..

Therefore, in the present invention, as shown in FIG. 4, an attempt is made to stabilize the uneven road surface by adding an operation amount u described later to the joint angle servo system shown in FIG. Hereinafter, the calculation of the manipulated variable u and the resulting stabilization of walking will be described.

First, a mathematical model is created for the robot 1 having the joint angle servo system shown in FIG. At that time, the following assumptions are made. [Assumption 1] Mutual interference between the front-back movement and the left-right movement of the robot 1 is sufficiently small and can be separated by the stabilization control. (FIG. 5 shows the movement in the front-back direction and FIG. 6 shows the movement in the left-right direction.) [Assumption 2] One foot 2 of the two leg links
No. 2 is completely grounded on the road surface. Also, the leg whose foot is completely in contact with the road surface shall be the supporting leg. [Assumption 3] The foot of the supporting leg shall not move (slide) with respect to the road surface. (Generally, the walking pattern is designed so that the foot of the supporting leg does not move with respect to the road surface during one foot supporting period. For example, so-called ZMP (zero moment poin).
ZMP using the concept of t) so that the foot does not separate from the road surface.
Designed by deciding the orbit of. Further, even in the walking pattern using the concept of the inverted pendulum, the foot does not physically separate from the road surface when it rotates around the ankle. With respect to slippage, it is possible to prevent slippage by properly treating the bottom of the foot with rubber or the like. )

Based on the above assumptions, the robot 1 shown in FIG.
The equation of motion of can be expressed as in Equation 1. As is clear from the equation of Equation 1, if Assumption 3 holds, the robot 1
Movement is independent of the angle of the foot.

[0017]

[Equation 1]

By the way, in this control, since the joint angle servo system shown in FIG. 3 is provided, the torque generated by each joint (represented by the symbol i) (motor-generated torque in FIG. 3).
Is calculated from the joint angle qi and the joint angle command value qir using the equation shown in Equation 2. (Note that kp and k in the equation of Equation 2
d and fv are used to mean values including kv, kc, and kt in kp and kd in FIG. 3, respectively. )

[0019]

[Equation 2]

Here, as shown in FIGS. 5 and 6, the relationship between the joint angle qi and the link angle θi can be expressed by Equation 3. still,
In this specification, “joint angle” or “q” is the angle between links (ie, relative angle), and “link angle” or “θ” is the angle of link (in absolute coordinates) with respect to the direction of gravity (ie, absolute). Angle) is used to mean.

[0021]

[Equation 3]

When the expression of the expression 2 is expressed by the link angle by the expression of the expression 3 and summarized, it becomes as shown in the expression 4.

[0023]

[Equation 4]

The disturbance d (θ0, θ0r) of the equation 4, that is, the value generated by the actual value and the target value of the foot angle θ0 can be expressed as in the equation 5.

[0025]

[Equation 5]

Here, by substituting the equation of the equation (4) into the equation of the equation (1) and rearranging, the equation (6) is obtained.

[0027]

[Equation 6]

The equation (6) is the equation of motion of the robot by the joint angle servo system. In the formula of Formula 1, the operation amount for controlling the motion is the joint torque T, but in the formula of Formula 6, the target posture Θr (absolute angle) and Θr dot (absolute angular velocity). Further, in the formula of the formula 1, the term of the foot angle θ0 (absolute angle) does not appear in the formula, but it appears in the formula of the formula 6. For this reason, when the joint angle servo system walks on an unexpected uneven road, the foot angle θ0 randomly fluctuates and becomes a disturbance that disturbs the motion of the formula (6), which makes the walking unstable. Therefore,
The target posture capable of walking, that is, the walking pattern can be designed only when the shape of the road surface is known from Assumption 3.
Here, considering walking on a flat road, the foot angle is zero, and therefore, Equation 7 is obtained.

[0029]

[Equation 7]

That is, the target postures Θr and Θr dots form a walking pattern on a flat road. At this time, the equation of motion of the robot (equation of equation 6) can be expressed by equation 8.

[0031]

[Equation 8]

On the other hand, when the constant Kr of the joint angle servo is set to an optimum value, the actual joint trajectory follows the target joint trajectory with almost no delay, and therefore, the equation 9 is obtained. That is, it becomes as shown in the equation 10.

[0033]

[Equation 9]

[0034]

[Equation 10]

As a result, the equation of the equation 11 is approximately established from the equation of the equation 8.

[0036]

[Equation 11]

Here, as often described, considering the case where unknown unevenness exists on a flat road, it is difficult to create a walking pattern that allows walking because the foot angle θ0 is not zero and unknown. Is. Therefore, it is assumed that the unevenness of the road surface is sufficiently small.
It is easy to detect if a visual means is provided in the above), and if a walking pattern Θr on a flat road is used (that is, disturbance d (θr, θ0) =
0), when the deviation (deviation) of the posture due to the unevenness is ΔΘ, the posture Θ of the robot can be expressed as in Expression 12.

[0038]

[Equation 12]

Therefore, in order to correct this deviation, Equation 4
The correction amount U is introduced into the joint torque in the equation (3) as shown in the equation (13).

[0040]

[Equation 13]

The equation (13) is used for walking on a flat road.
That is, it can be seen that the correction amount U is added to the torque generated by the joint angle servo system in the state of (θ0r, θ0 = 0).

The calculation of the correction amount U will be described below. By substituting the equations (12) and (13) into the equation (1) which is the equation of motion of the robot, the equation (14) is obtained. That is, the equation of motion is represented by the flat road walking pattern Θr and the deviation ΔΘ.

[0043]

[Equation 14]

If the unevenness of the road surface is sufficiently small, the deviation Δ
Since Θ is also sufficiently small, it can be transformed into the equation (15).

[0045]

[Equation 15]

Here, ignoring the second-order and higher-order terms of the deviation Δθ, the equation 16 is obtained.

[0047]

[Equation 16]

From the equation shown in equation 11 which holds when the joint angle servo follows at high speed, the equation of equation 16 approximates to equation 17
As shown in.

[0049]

[Equation 17]

Furthermore, since the change in J (Θr) due to Θ is small, the value in X (Θr) is small, and the values of the joint angle servo gains Fp and Fv are large, the magnitude relationship as shown in equation 18 is obtained. Holds.

[0051]

[Equation 18]

Therefore, the equation of the equation 17 can be expressed as the equation 19.

[0053]

[Formula 19]

That is, the equation (19) represents the equation of motion for the deviation ΔΘ from the walking pattern, and the correction amount U for stabilization is the operation amount for controlling the movement of the deviation ΔΘ. Therefore, by deriving an operation amount U that makes the deviation ΔΘ zero, the posture of the robot converges on a walking pattern on a flat road even on a road surface having unevenness, so that the robot walks stably.

Next, the calculation of the manipulated variable U for making the deviation ΔΘ zero will be described. Here, the manipulated variable U is expressed as shown in Equation 20. Here, ΔF is a variation of the external force acting on the free leg, which can be detected by the 6-axis force sensor 36 described above.

[0056]

[Equation 20]

Substituting the equation of the equation 20 into the equation of the equation 19, the equation 21 is obtained.

[0058]

[Equation 21]

Therefore, if U is calculated using the equation of the equation 20, the equation of the equation 21 becomes a linear equation relating to the deviation ΔΘ,
Also, the control value can be determined optimally. However, if the external force fluctuation ΔF is sufficiently small and can be regarded as a disturbance in the formula of Formula 21, ΔF≈0 is set, and the following manipulated variable Uθ is calculated without calculating the second term of the formula of Formula 20. By doing so, some stabilization is possible. This is, for example, the case where ΔF≈0 can be achieved because the external force does not act on the free leg during the one-leg support period.

The calculation of the manipulated variable Uθ will be described below.

Since the expression of Expression 21 is a linear equation related to ΔΘ, it can be expressed as Expression 22 by using a so-called state equation expression.

[0062]

[Equation 22]

Here, by the optimum regulator theory, u that minimizes the quadratic form evaluation function J shown in the equation (23) can be expressed as in equation (24).

[0064]

[Equation 23]

[0065]

[Equation 24]

Minimizing the evaluation function J is to make the state variable x as small as possible with the smallest possible manipulated variable u, as is clear from the equation (23). That is, the deviation Δθ due to the unevenness of the road surface is minimized. Therefore, stable walking can be realized by calculating the correction amount (manipulation amount) u from the equation (24).

On the other hand, external force fluctuation ΔF and matrix A,
Considering the modeling error of B as the disturbance d, the formula shown in the formula 22 can be expressed as the formula 25.

[0068]

[Equation 25]

Further, according to the optimal regulator theory, u that minimizes the quadratic evaluation function J shown in Expression 26 is expressed by Expression 2
It can be calculated like 7.

[0070]

[Equation 26]

[0071]

[Equation 27]

The equation (27) is also a method of calculating the correction amount (operation amount) u for realizing stable walking, like the equation (24).

Here, to calculate u, the gain K
It is necessary to obtain x, Ki and the state quantity x of the robot, but the gains Kx, Ki are obtained by solving the Riccati equation by the optimal regulator theory as is well known.
Further, the state quantity x is the angle and angular velocity of the link, which are the inclination sensors 40, 4 attached to the body 24 of the robot.
2 and a rotary encoder arranged at each joint. So, for example, in the case of forward-backward movement,
Equation 3 can be written as Equation 28.

[0074]

[Equation 28]

Here, the inclination angle θ3 of the body portion (base body) 24
Can be detected by the tilt sensors 40 and 42. Therefore, assuming that the detected value is ψ, the formula of Formula 29 is established from the formula of Formula 28 using the joint angle qi.

[0076]

[Equation 29]

Therefore, the deviation ΔΘ can be expressed by the equation (30).

[0078]

[Equation 30]

Therefore, the expression of the expression 30 can be expressed as the expression 31 in the matrix expression.

[0080]

[Equation 31]

Therefore, for example, the formula of the formula 24 can be expressed as the formula 32 by using the conversion formula shown in the formula 31.

[0082]

[Equation 32]

The equation (32) shows that the operation amount u is calculated from the deviation between the tilt angle of the body 24 and the joint angle. Here, looking at the constituent elements of the matrix E, there are many 1 elements in the columns of the tilt angle and the tilt angular velocity, and the deviation of the joint angle is equivalent to the deviation of the servo system, and the servo system reduces this deviation. In consideration of the fact that the operation is performed in the same manner, the operation amount u can be considered by the formula of Expression 33, which is simply calculated from only the tilt angle and the tilt angular velocity of the body portion 24.

[0084]

[Expression 33]

7 to 11 show the experimental data, the control system is stable even when the manipulated variable u is calculated from these by the equation 33, and the eigenvalue that determines the responsiveness of the control system is also expressed by the equation 32. It was confirmed that it was almost the same as when it was used. That is, FIGS. 7 to 9 are experimental data showing the response of each link angle when the ankle joint angle of the supporting leg is changed stepwise forward by 3 degrees while the robot is supported on one leg, of which FIG. In the case of only the joint angle servo, FIG.
FIG. 9 shows the case of the stabilization control by the equation 3 and FIG. 9 shows the case of the stabilization control by the equation 32. Further, FIGS. 10 and 11 show experimental data when tilted in the same manner in the left-right direction by 3 degrees. Of these, FIG. 10 shows the case of joint angle control only, and FIG. 11 shows the case of stabilization control by the formula 32. Indicates. As is clear from the results of this experiment, no significant difference was observed in the response between the case of the equation of Formula 32 and the case of the formula of Equation 33. In addition, whichever formula is used, the robot did not fall down by performing this stabilization control, but it was confirmed that the robot would fall down excessively when only the joint angle servo was used.

The above is also applicable to the equation shown in Expression 27, and the equation can be simplified in exactly the same manner.

Based on the above, the determination of the control value will be described with reference to the flow charts of FIGS.
It is to be noted that these flow charts assume that in the walking state transition diagram shown in FIG. 15, the walking starts from the right foot, transitions from the third step to the steady walking, and ends with the left foot. It is divided into the one-leg support period, the compliance control period (when landing on the free leg), and the two-leg support period.

FIG. 12 is a control value determination flow chart for the first one-leg supporting period. First, in S10, the joint angle command values qr and qr dots are read. That is, the ROM 66 in the control unit 26 of the robot 1 stores each link angle Θr (absolute angle as described above) as a walking pattern, as shown in FIG. The coordinates are converted into qr and angular velocity qr dots (both are relative angles), and S
In 10, the converted value (at time t) is
The first joint of the joints is read.

Next, in S12, the actual angle q for the joint is input, and in S14, the operation amount of the joint angle servo system is calculated from the equation shown. Specifically, this is calculated in the form of the speed command value of the servo motor described above. Next, in S16, the inclination angle ψ and the inclination angular velocity ψ dot are input, the joint angle deviation Δq is calculated from the actual angle q and the target angle qr in S18, and the correction is performed using either the equation 32 or the equation 33 in S20. The quantity u is calculated (it should be noted that S18 jumps when the expression of Expression 33 is used). This correction amount u is also calculated in the form of the speed command value of the servo motor. Subsequently, in S22, the value calculated in S20 is added to the value calculated in S14, and the value is output to the servo amplifier. Subsequently, the control value is similarly determined for the next joint until it is determined in S24 that the free leg has landed.
When the control values for the joint No. 2 are determined, the next time t
The same operation is repeated for +1. The landing of the free leg is detected by the landing switch 38 described above.

It should be noted that what should be noted here is S22.
In the above, when outputting to the servo amplifier, the joints are given priority. That is, the priorities of the joints are as follows: the supporting leg ankle joints 18 and 20R (L), the supporting leg knee joints 16R.
(L), supporting leg hip joints 10, 12, 14R (L), swing leg hip joints 10, 12, 14L (R), swing leg knee joint 1
6L (R) and the ankle joints 18 and 20L (R) of the swing leg, and the operation amounts are added in that order. In other words, the joints that make a large contribution to maintaining a stable posture are prioritized. As a result, when there is no time to spare, for example, the operation amount of only the ankle joint and the knee joint of the support leg is reduced. u will be added. Needless to say, when there is enough time, add to all joints in this order.

When it is determined in S24 of the flow chart of FIG. 12 that the landing is on the free leg, the flow goes to compliance control at the time of landing according to the flow chart of FIG. Explaining with reference to the figure, first, after similarly calculating the servo control values in S100 to S104, the process proceeds to S106, in which the ankle torque applied to the free leg landed is input from the output of the 6-axis force sensor 36 described above. , S108 calculates the compliance amount m. This is done by using a so-called virtual compliance control method that realizes impedance control by speed decomposition control. Subsequently, in S110, a value obtained by adding the servo control value vc, the virtual compliance control value m, and the correction amount u is output to the servo amplifier, and the respective joints are time-lapsed until the compliance control is determined to be completed in S112. repeat. It should be noted that this end determination is made based on whether or not a predetermined time has elapsed since landing. Also S11
The value of the correction amount u of 0 is S20 in the flow chart of FIG.
Save and use the value calculated in.

S112 in the flow chart of FIG.
When it is determined that the compliance control is completed in step S1 of FIG.
The control values for the two-leg supporting period are determined according to S200 to S214 of the flow chart, except that the calculation of the correction amount U in S210 is determined by the equation (20).
It is similar to the 2 flow chart. It should be noted that, here, the operation amount is precisely calculated by using the formula shown in Formula 20,
If ΔF is considered to be a disturbance, it may be calculated from Equation 32 or Equation 33. Conversely, when an external force acts on the free leg, it is calculated from the equation including Equation 20 in S20 of the flow chart of FIG. Is also good.

Incidentally, in addition to the above-mentioned determination of the weight of the evaluation function J, in general, when the evaluation function J of the equation (23) is specifically expressed in the case of the forward / backward movement, It becomes like the number 34.

[0094]

[Equation 34]

This evaluation function J represents the weighted sum of squares of the posture deviation Δθi and the operation amount ui. According to the optimal regulator theory, the feedback gain is calculated so that the deviation of the posture of the term having a large weight becomes a value smaller than that of the term having a small weight. Therefore, different feedback gains are calculated depending on the weighting method, and the time response for correcting the deviation of the posture becomes different. In the optimal regulator theory, there is no fixed selection method for weighting. Generally, it is selected by simulation or experiment.

Therefore, in the present invention, the weights Q and R are selected in consideration of the characteristics of bipedal walking. In this regard, a case where it is desired to accurately control the landing position of the free leg as when going up and down stairs will be described as an example.

Consider a case where deviations Δx and Δy occur in the target foot position (x, y) of the free leg as shown in FIG. These can be shown as in equations (35) to (38).

[0098]

[Equation 35]

[0099]

[Equation 36]

[0100]

[Equation 37]

[0101]

[Equation 38]

As shown in FIG. 16, the deviation Δp on the position of the deviation Δx, Δy from the target foot position (x, y) of the free leg is shown.
Can be expressed by the sum of squares as shown in Expression 39.

[0103]

[Formula 39]

As is clear from this equation, the deviation Δp ² from the target foot position is proportional to the coefficient concerning the posture deviation Δθi of each link. That is, if the posture deviation Δθi is reduced in the order of increasing coefficient, the position deviation Δp ² can be efficiently reduced. On the other hand, the evaluation function J can be made smaller as the posture deviation Δθi having a larger weight is increased. Therefore, by selecting the evaluation function J as in the formula (40), it is possible to optimally correct the displacement of the foot position. That is, when it is desired to accurately land on a predetermined position, the weights for all link angles are not made equal, but the weights are changed according to the degree of contribution to the displacement of the toe position. For example, in Expression 34, the weight q1 may be increased to reduce the fluctuation of Δθ1, the maximum torque generated by the actuator may be considered, and the r2 may be decreased to increase the torque generated by u2. In this way, in addition to the stabilization control, depending on the position (here, the position in the work coordinate system, which is not the position in the joint coordinate system such as the joint angle position) where the weights Q and R are to be optimally controlled, It is possible to optimally (effectively) control the position of the toes, for example. It should be noted that this selection method can be applied not only to the position of the tip of the foot, but also to the positions of other parts such as the center of gravity.

[0105]

[Formula 40]

In this embodiment, as described above, in the controller of the bipedal legged mobile robot having the joint angle servo system, the deviation between the preset target angle of the walking pattern and the actual angle is expressed in absolute angle. Since the operation amount u is calculated and added to the servo operation amount in accordance with the calculation, it is possible to always maintain a stable posture even when walking on an uneven road surface where the walking pattern is unexpected, and it is possible to achieve linearity. It is not necessary to use a high-speed computer to perform non-linear compensation even for a walking pattern that is impaired, and smooth walking can always be realized. Further, when the operation amount u is calculated, in addition to the method of finely determining it according to the variation of the external force acting on the swing leg, the operation amount u is configured to be simply calculated from the tilt angle and the tilt angular velocity of the body. In the case of the simple calculation method, it can be calculated easily.

FIG. 17 is a block diagram showing a second embodiment of the present invention, and shows a stabilizing control system similarly to FIG. 4 of the first embodiment. In this example, the above-mentioned gain Kx or Kq, Kψ is given a frequency characteristic. That is, in the first embodiment, as shown in FIG. 18, the gain is fixed to a constant value with respect to the frequency, whereas in FIG.
As shown in, it is configured to reduce according to the frequency.

To explain this, it is necessary to raise the gain to speed up the reaction of the system and improve the stability against disturbance in case of occurrence of disturbance. However, if the gain is increased to increase the responsiveness of the links, high-frequency vibration is generated in each link, and if the gain is further increased, the high-frequency vibration is amplified in the feedback loop, causing oscillation. This is because the mathematical model expressed by the equation (1) described above is established only in the low frequency region such as the stiffness model, and the state of the high frequency region in which the influences of the softness of the link, the rattling, the bending, etc. appear This is because it cannot be expressed accurately. However, it is extremely difficult to accurately represent the state including the high frequency region with a mathematical model,
Even if it could be expressed, it would be a very complicated model, and would require a large-capacity and expensive computer, making it almost impossible to realize.

The cause of this oscillation is that the high frequency signal is not attenuated,
Since this is due to amplification, this high frequency signal may be attenuated. Therefore, in this embodiment, the feedback gain Kx or Kq, Kψ is configured to have frequency characteristics. That is, as shown in FIG.
The gain is relatively high in the low frequency region of the command signal level, and is low in the high frequency region where the link is elastic. Specifically, as shown in FIG. 17, a high frequency cutoff filter is inserted in the feedback loop. The equation of state of this filter can be expressed by the equation 41,
The cutoff frequency can be arbitrarily determined by appropriately setting Af, Bf, and Cf at the design stage.

[0110]

[Formula 41]

In the case of the present embodiment, in calculating the correction amounts u and U in S20 of the flow chart of FIG. 12 and S210 of the flow chart of FIG. 14, the cutoff frequency is made variable according to the transported weight. That is, when the robot mounts an object on the upper body and carries it, the natural frequency of the mechanism of the robot changes, and thus the oscillation frequency also changes.
Needless to say, this adjustment may use an electric filter instead of the software method.

In the present embodiment, the feedback gain of the stabilizing control system (optimal regulator) is set to be small in the high frequency range, and the deviation between the preset target angle of the walking pattern and the actual angle is obtained as an absolute angle. Since the operation amount u calculated accordingly is added, in addition to the case where the filter is not added, the stability can be further improved in the limit that the oscillation due to the elasticity of the joint link does not occur. The walking pattern can always maintain a stable posture even when walking on an uneven bumpy road surface, and it is not necessary to use a high-speed computer for non-linear compensation for walking patterns that impair linearity. A smooth walk can be realized. Further, when the operation amount u is calculated, in addition to the method of finely determining it according to the variation of the external force acting on the swing leg, the operation amount u is configured to be simply calculated from the tilt angle and the tilt angular velocity of the body. In the case of the simple calculation method, it can be calculated easily.

In the first and second embodiments described above, the manipulated variable u is added to the portion indicated by the symbol (a) in FIG. 3, but the symbol (b) or ( You may add at the points shown in (c).

Further, as shown in FIG. 4, the walking pattern is designed to have an absolute angle, but the invention is not limited to this, and the walking pattern may be designed to have a relative angle. However, in that case, since the actual angle is detected as an absolute angle, it is necessary to appropriately perform coordinate conversion to obtain the absolute angle.

Furthermore, the present invention has been described with respect to a bipedal legged mobile robot, but the present invention is not limited to this, and is also applicable to a legged mobile robot having one leg or three or more legs.

[0116]

According to the first aspect of the present invention, a predetermined operation amount is given to the servomotors arranged at the respective joints of the leg links of the legged mobile robot so that the angles of the respective joints follow the preset target angles. A control device for controlling detects a deviation between a real angle of the leg link and a target angle by an absolute angle with respect to a gravity direction, and calculates a second operation amount to correct the deviation according to the detected deviation. Since the control device is configured to add the calculated second operation amount to the predetermined operation amount and give the servo motor with the calculated second operation amount, the controller walks on a road surface having an unexpected unevenness. It is possible to always maintain a stable posture even when doing, and it is possible to always realize a smooth posture control even when walking on a flat road.

In the apparatus according to claim 2, the legged mobile robot comprises a base body and a plurality of leg links connected to the base body, and the computing means comprises the inclination angle of the base body. Since the second operation amount is calculated from the inclination angular velocity and / or the second operation amount, the second operation amount can be easily calculated.

According to the apparatus of claim 3, the legged mobile robot comprises a base body and two leg links connected to the base body via a first joint, and the leg links are provided. A second joint is provided near the tip thereof, and a third joint is provided between the second joint and the first joint, and walking while alternately supporting the own weight with the two leg links. A bipedal robot, wherein the control device comprises a second joint of the support leg, a third joint of the support leg, a first joint of the support leg, and a first joint of the free leg.
Since the second operation amount is added to the predetermined operation amount at least partially in accordance with the priority order of the joint, the third joint of the free leg, and the second joint of the free leg, the control is performed. Even when there is no time margin, the stabilization control can be effectively realized.

In the apparatus according to the fourth aspect, the calculating means calculates the second manipulated variable by using a state equation, and the state feedback gain thereof is changed according to the posture of the robot determined from the angle. In addition to the stabilization control, it is possible to optimally control the landing position and the like.

According to a fifth aspect of the present invention, the legged mobile robot has two legs, which are connected to a base body through a first joint and have a second joint near the tip thereof. A two-legged walking robot which is composed of a partial link and which alternately walks while supporting its own weight by the two leg links.
A means for detecting an external force acting on the swing leg near the joint is provided,
Since the calculation means is configured to calculate the second manipulated variable based on the detected external force, the second manipulated variable can be precisely calculated, and thus a more stable posture is always realized. be able to.

In the device according to the sixth aspect, since the feedback gain is provided with the frequency characteristic,
In addition to the effects described above, stable high-speed walking can be realized while suppressing oscillation.

According to the apparatus of claim 7, since the cutoff frequency is variable according to the payload, in addition to the above-mentioned effect, stable high-speed walking can be realized while suppressing oscillation. You can

[Brief description of drawings]

FIG. 1 is an overall schematic view of a control device for a legged mobile robot according to the present invention.

FIG. 2 is an explanatory block diagram of a control unit shown in FIG.

FIG. 3 is a block diagram showing a joint angle servo system in the control of the control unit shown in FIG.

FIG. 4 is a block diagram showing a state in which a second operation amount calculated by the present invention is added to the joint angle servo system shown in FIG.

5A and 5B are explanatory diagrams showing a front-back movement of the robot shown in FIG.

FIG. 6 is an explanatory diagram showing a horizontal movement of the robot shown in FIG.

FIG. 7 is an experimental data diagram showing the response when the ankle joint angle of the supporting leg of the robot is tilted stepwise 3 degrees in the front-rear direction when only the joint angle servo system shown in FIG. 3 is provided.

FIG. 8 is a diagram when the second manipulated variable shown in FIG. 4 is added.
It is an experimental data figure similar to.

FIG. 9 is an experimental data diagram similar to FIG. 8 in the case where a second manipulated variable calculated by another method is added.

FIG. 10 is an experimental data diagram similar to FIG. 7 and showing responsiveness when the ankle joint angle of the supporting leg of the robot is tilted 3 degrees in a stepwise manner in the left-right direction.

11 is an experimental data diagram similar to FIG. 10 in the case where the second manipulated variable shown in FIG. 4 is added.

12 is a flow chart showing an operation of the control device of FIG. 2, which is a flow chart showing determination of a control value in one foot supporting period.

FIG. 13 is a flowchart similar to FIG. 12, showing the control value determination in the compliance control period at the time of landing.

FIG. 14 is a flowchart similar to FIG. 12, showing the control value determination in the both-foot supporting period.

FIG. 15 is a walking state transition diagram illustrating the control timing shown in FIGS. 12 to 14;

16 is an explanatory diagram illustrating a method of determining a weight of an evaluation function when calculating the second operation amount illustrated in FIG.

FIG. 17 is an explanatory block diagram similar to FIG. 4, showing a second embodiment of the present invention.

FIG. 18 is an explanatory characteristic diagram showing the frequency characteristic of the feedback gain of the first embodiment.

FIG. 19 is an explanatory characteristic diagram showing the frequency characteristic of the feedback gain of the second embodiment.

[Explanation of symbols]

1 Leg type mobile robot (bipedal walking robot) 2 Leg links 10R, 10L Joints for pivoting legs (axes) 12R, 12L Joints in the pitch direction of the crotch (axis) 14R, 14L Joints in the roll direction of the crotch (Axis) 16R, 16L Joint in the pitch direction of the knee (axis) 18R, 18L Joint in the pitch direction of the ankle (axis) 20R, 20L Joint in the roll direction of the ankle (axis) 22R, 22L Foot (foot) ) 24 body section 26 control unit 36 6-axis force sensor 40, 42 tilt sensor

Claims (7)

[Claims] 1. A control device for giving a predetermined operation amount to a servomotor arranged at each joint of a leg link of a legged mobile robot to control the angle of each joint so as to follow a preset target angle. Detecting means for detecting a deviation between the actual angle of the leg link and the target angle by an absolute angle with respect to the direction of gravity; and b. And a calculation means for calculating a second manipulated variable in order to correct the detected deviation in accordance with the detected deviation. A control device for a legged mobile robot, characterized in that it is applied to the servomotor. 2. The legged mobile robot comprising a base body and a plurality of leg links connected to the base body, wherein the computing means calculates the second from the inclination angle and / or the inclination angular velocity of the base body. The control device for a legged mobile robot according to claim 1, wherein the control amount is calculated. 3. The legged mobile robot comprises a base body and two leg links connected to the base body via a first joint, and the leg link has a second joint near its tip. A bipedal robot that comprises a third joint between the second joint and the first joint, and walks while alternately supporting its own weight with the two leg links. The control device is provided in the order of the second joint of the support leg, the third joint of the support leg, the first joint of the support leg, the first joint of the free leg, the third joint of the free leg, and the second joint of the free leg. The legged mobile robot controller according to claim 1 or 2, wherein a second operation amount is added to the predetermined operation amount according to at least part of the determined priority order. 4. The calculating means calculates the second manipulated variable by using a state equation, and changes the state feedback gain according to the posture of the robot determined from the angle. Item 4. A control device for a legged mobile robot according to any one of items 1 to 3. 5. The legged mobile robot comprises a base body and two leg links which are connected to the base body via a first joint and have a second joint near the tip thereof. Is a two-legged walking robot that alternately walks while supporting its own weight with its leg links, and is provided with means for detecting an external force acting on the idle leg near the second joint, and the arithmetic means is detected. The control device for a legged mobile robot according to claim 1, wherein the second operation amount is calculated based on an external force. 6. In the calculation of the predetermined manipulated variable, the feedback gain is made variable according to the frequency characteristic of the feedback signal, and the gain in a band exceeding the predetermined frequency is set as a gain in a band below the predetermined frequency. The control device for a legged mobile robot according to claim 1, wherein the control device is made smaller. 7. The control device for a legged mobile robot according to claim 6, wherein the predetermined frequency is variable according to a loadable weight.