Robot drive joint control system

A system for controlling the locomotion of a biped walking robot by absorbing the footfall impact such that the robot walks stably. In advance, constraint conditions for the robot such as a coordinates position of the robot's center of gravity for the robot to assume a predetermined attitude are preestablished and in walking, robot's joint angles are determined inverse kinematically from the preestablished attitude constraint conditions. The ground reaction force generated at the footfall is detected and a correction amount required for shifting the center of gravity in the impact force absorbing direction is calculated. The attitude constraint conditions are recalculated in response to the correction amount and then the robot's joint angles are recalculated from the corrected attitude constraint conditions in three-dimensional, or at least in one of two-dimensional planes. Instead of the center of gravity, the robot's hip section can be used.

BACKGROUND OF THE INVENTION 
1. Field of the Invention 
This invention relates to a system for controlling the drive joints in a 
robot, more particularly to a system for controlling drive joints in a 
legged mobile robot such as a biped walking robot so as to soften footfall 
impact. 
2. Description of the Prior Art 
As shown in FIGS. 19A and 19B, so long as the feet of an autonomous biped 
walking robot or other type legged mobile robot are in contact with the 
ground, the robot is acted on by forces from the ground surface. Such an 
external force will hereinafter be referred to as a "ground reaction 
force." One of the ground reaction forces received by a legged mobile 
robot during locomotion acts as a vibration-producing impact over a short 
period following the landing (footfall) of the foot of a free leg. This 
will hereafter be referred to as a "footfall impact." A large footfall 
impact may act as a disturbance which destabilizes the robot's locomotion. 
In the worse case it may cause the robot to tip over. 
It was for this reason that, earlier, in Japanese Patent Application No. 
1(1989)-297,199 (Japanese Laid-Open Patent Publication No. 
3(1991)-161,290), the assignee proposed a technology for softening 
footfall impact by controlling the ankle drive joints in response to 
footfall impact. If the capability of absorbing and softening footfall 
impact can be enhanced still further, there can be expected to be obtained 
even more stable robot locomotion. In particular, there can be expected to 
be obtained more stable locomotion in situations where the footfall impact 
tends to be high, as during walking over rough terrain or walking at high 
speed. 
Footfall impact can be treated as a force acting on the robot's center of 
gravity. As shown in FIG. 20, however, the earlier technology softened 
footfall impact solely through control of the ankle joints (indicated by 
hatching). Since it was therefore unable to achieve large shifts in the 
center of gravity, the absorption of footfall impact was inadequate. 
Another problem with the earlier technology is that it cannot perform an 
absorption operation when the whole sole of the foot is in contact with 
the ground. 
As just noted, the locomotion of a legged mobile robot becomes more 
unstable as the footfall impact gets larger. To say that a footfall impact 
is large means that its maximum value is high and the period over which it 
acts (its duration) is long. Since there is a strong tendency for the 
duration of a footfall impact to grow longer with increasing maximum 
value, reduction of the footfall impact impulse (the integral of the force 
over an interval of time) can be achieved by holding down the maximum 
value of the footfall impact. In particular, since a close relationship 
can be observed between the locomotion stability of a legged mobile robot 
and the maximum value of the footfall impact, one of the key aspects of 
stabilizing the locomotion of a legged mobile robot is that suppressing 
the maximum value of footfall impact is sufficient for achieving the 
stabilization. Moreover, since, as was pointed out earlier, a footfall 
impact can be deemed to be a force acting on the robot's center of 
gravity, locomotion can be stabilized by holding the maximum acceleration 
of the center of gravity to a low level. 
SUMMARY OF THE INVENTION 
One object of the present invention is to provide a robot drive joint 
control system which responds to an external force acting on the robot by 
positively moving the center of gravity or another specified portion or 
section of the robot from its prescribed position in a direction which 
softens the impact of the external force. 
Another object of the invention is to provide a robot drive joint control 
system which conducts the operation for softening an impact over a 
prescribed period of time so as to disperse the impact over said period. 
For realizing these objects, the present invention provides a system for 
controlling the locomotion of a legged walking robot having a body and 
legs each connected to the body, comprising first means for detecting 
external force or moment acting on the robot when the leg is landed on the 
ground, second means for calculating a correction amount required for a 
predetermined robot's portion to shift in a direction in which the 
detected external force or moment reduces, and third means for determining 
robot's joint angles inverse kinematically in response to the calculated 
correction amount.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
Embodiments of the invention will now be explained based on a biped robot 
as an example of a robot. 
An overall skeleton view of a biped robot 1 is shown in FIG. 1. The robot 1 
has left and right legs each having six joints (axes). (To make the 
arrangement easier to understand, the joints (axes) are represented as the 
electric motors by which they are driven.) The six joints (axes) are, 
starting at the top, joints (axes) 10R, 10L for swiveling (generally 
horizontal rotation) of the legs at the hip (R and L indicating the right 
and left legs), joints (axes) 12R, 12L for rotation at the hip in the 
pitch direction (x direction), joints (axes) 14R, 14L for rotation at the 
hip in the roll direction (y direction), joints (axes) 16R, 16L for 
rotation at the knee in the pitch direction, joints (axes) 18R, 18L for 
rotation at the ankle in the pitch direction and joints (axes) 20R, 20L 
for rotation at the ankle in the roll direction. Foot members 22R, 22L are 
provided at the lower end of this arrangement and a body (main unit) 24 
housing the control unit 26 is provided at the upper end. 
The hip joints in the foregoing configuration are constituted by the joints 
(axes) 10R(L), 12R(L) and 14R(L) and the ankle joints by the joints (axes) 
18R(L) and 20R(L). The leg links of the respective legs thus have six 
degrees of freedom, so that during locomotion the legs as a whole can be 
caused to execute the desired movement by driving the 6.times.2=12 joints 
(axes) to appropriate angle. The robot is thus capable of walking freely 
within three-dimensional space. It will be noted that the hip and knee 
joints are connected by thigh links 28R, 28L and the knee and ankle joints 
by crus links 30R, 30L. The joints are provided mainly with electric 
motors as was mentioned earlier and reduction gear mechanisms for 
increasing motor torque. The structure of the joints is described in the 
assignee's earlier Japanese Patent Application No. 1(1989)324,218 
(Japanese Laid-open Patent Publication No. 3(1991)184,782) etc., and since 
it is not an essential aspect of the present invention, will not be 
explained further here. 
The individual ankles of the robot 1 shown in FIG. 1 are provided with a 
six dimensional force and torque sensor 36 of conventional design. By 
measuring the x, y and z force components Fx, Fy and Fz transmitted to the 
robot through the foot members and also measuring the moment components 
Mx, My and Mz around the three axes, the six-dimensional force and torque 
sensor 36 detects whether or not the associated foot member has landed and 
the magnitude and direction of the forces acting on the supporting leg. 
The sole of each foot member is equipped at its four corners with 
touch-down switches 38 of conventional design for detecting whether or not 
the foot is in contact with the ground. The top of the body 24 is provided 
with a pair of inclination sensors 40, 42 for detecting the robot's 
inclination angle and angular velocity relative to the z axis in the x-z 
and y-z planes. The outputs of the sensors 36 and the like are sent to the 
control unit 26 in the body. 
As shown in the block diagram of FIG. 2, the control unit 26 is constituted 
as a microcomputer. The outputs from the inclination sensors 40, 42 etc. 
are converted into digital signals by an A/D converter 50 and the 
resulting digital values are sent via a bus 52 to a RAM (random access 
memory) 54 for storage. In addition, the outputs of encoders disposed 
adjacent to the respective motors (only two encoders 56, 58 are shown in 
FIG. 1 by way of example) are input to the RAM 54 through a counter 60, 
while the outputs of the touchdown switches 38 and the like are stored in 
the RAM 54 via a waveform shaper 62. The control unit has a CPU (central 
processing unit) 64 which fetches locomotion data from a ROM (read-only 
memory) 66 and computes current command values from the deviation relative 
to the measured values received from the counter 60, and sends the same to 
a servo amplifier 70 of the respective motors via a D/A converter 68. As 
illustrated, the output of the individual encoders is forwarded to the 
associated servo amplifier 70 such that velocity feedback control is 
realized, as shown in FIG. 3, in addition to the positional feedback 
control. Reference numeral 76 designates a joy stick for inputting 
commands to change the robot's stride and other aspects of its gait, 
reference numeral 78 a zero reference switch for setting the robot's 
beginning attitude (upright), and reference numeral 80 a limit switch for 
preventing overruns. And, although it will be possible to carry out an 
additional posture control from the detected inclination angle or angular 
velocity, the gist of the invention does not reside therein so that no 
explanation will be made on the posture control based on the inclination 
angle or angular velocity detected. 
The operation of the control system according to the invention will now be 
explained with reference to the flow chart of FIG. 4 and the following 
Figures. The flow chart of FIG. 4 shows the procedure used for designing 
the robot's basic gait. This work is conducted offline in advance. It will 
be explained briefly in the following. 
The walking speed is set in step S1 and then the time series data of twelve 
constraint conditions (equations) are set in step S2 as required for the 
robot to assume an attitude to obtain the walking speed. The conditions 
are set not on joint angles, but on positions in coordinates such as the 
center of gravity, the toe of the robot 1, for example. Control then 
passes to step S3 in which control flag data Cf(t) is set ("t" indicating 
time). This flag determines the footfall impact control period and the 
like. (As shown in FIG. 5, the footfall impact control period in the 
control according to this invention extends from just before the time that 
the part of the sole 22R (L) analogous to the heel contacts the ground 
(indicated as t-HC in FIG. 5) to an appropriate time after the whole sole, 
including the part analogous to the toe, comes in contact with the ground 
(indicated as t-TC).) Next, in step S4, employing Newton's method 
(Newton-Raphson formula) the constraint conditions (equations) are inverse 
kinematically converted to time series data .theta.com(t,i) (hereinafter 
called the "standard joint angles") for the twelve joint angles of the 
robot 1 ("t" indicating time and "i" indicating the joint). Finally, in 
step S5, the joint angle time series data converted in step S4 and the 
control flag set are stored in the ROM 66 in the control unit of the robot 
1. 
The preset time series data is used to conduct real-time locomotion control 
in accordance with the flow chart of FIG. 6 (in, for example, 5-ms control 
cycles). After loading of the stored data in step S10, a check is made in 
step S12 as to whether or not walking has begun. If it has, control passes 
to step S14 in which the value of timer t is initialized to zero. Next, in 
step S16, whether or not impact absorption control is to be conducted is 
decided on the basis of the bit of the flag Cf(t). 
If the result in step S16 is that the impact absorption control period is 
running, control passes to step S18 of the flow chart of FIG. 7, in which 
the center of gravity G and other constraint condition data are read. (As 
shown in FIGS. 8A and 8B, the center of gravity G is indicated as the 
absolute coordinate positions Gx, Gy and Gz in a three dimensional space 
in which the vertical direction is designated Z, the direction of robot 
advance is designated X and the direction perpendicular to these 
directions is designated Y. 
Next, in step S20, the ground reaction forces (footfall impact forces) Fx, 
Fy and Fz acting on the robot 1 are obtained. This is done by geometric 
calculation in accordance with the equations set out below using the 
forces F.sub.x1,2, F.sub.y1,2 and F.sub.z1,2, which are detected by the 
six-dimensional force and torque sensor 36, and the rotation angles 
.theta..sub.1,2 (about the Y axis), .PHI..sub.1,2 (about the X axis) and 
P.sub.1,2 (about the Z axis). As shown in FIG. 8, the subscripts 1, 2 
indicate the left and right legs, respectively. Since the six-dimensional 
force and torque sensor 36 rotates with varying attitude of the robot 1, 
its outputs become as illustrated. In the illustrated example, the values 
of .PHI..sub.1, P.sub.1, .PHI..sub.2 and P.sub.2 are all zero. 
Fx=fx(.theta.1,.PHI.1,P1,Fx1,Fy1,Fz1)+fx(.theta.2,.PHI.2,P2,Fx2,Fy2,Fz2) 
Fy=fy(.theta.1,.PHI.1,P1,Fx1,Fy1,Fz1)+fy(.theta.2,.PHI.2,P2,Fx2,Fy2,Fz2) 
Fz=fz(.theta.1,.PHI.1,P1,Fx1,Fy1,Fz1)+fz(.theta.2,.PHI.2,P2,Fx2,Fy2,Fz2) 
Control then passes to step S22 in which an attitude correction amount 
(amount of center of gravity movement) delta G is calculated using the 
following equations. 
EQU delta Gx=fg (Fx).multidot.k 
EQU delta Gy=fg (Fy).multidot.k 
EQU delta Gz=fg (Fz).multidot.k 
More specifically, as shown in FIG. 9, delta G is calculated from a primary 
function established with respect to the ground reaction forces (footfall 
impact forces) obtained in the preceding step and the result is multiplied 
by a coefficient k defined to vary with time as, for example, shown in 
FIG. 5. In other words, the correction amount is determined so as to move 
the center of gravity in the direction in which the ground reaction force 
acts and the magnitude thereof is reduced with the passage of time. As 
shown in FIG. 9, upper and lower limits of lmt are placed on the value of 
delta G. This is because setting the correction value too large relative 
to the ground reaction force may destabilize rather than stabilize the 
robot attitude. 
Control next passes to step S24 in which the correction amount delta G is 
added to the preset center of gravity G coordinates and then to step S26 
in which any of the twelve joint angles requiring correction under the 
corrected constraint condition (constraint conditions other than the 
center of gravity coordinates remain the same) are recalculated in real 
time. 
In other words, as shown in FIG. 10, this embodiment computes corrected 
joint angles .theta.'com(t,i) for those among the twelve joints (the 
joints 10, 12 etc.) whose angles have to be corrected for enabling the 
robot 1 to assume the attitude in three-dimensional (X,Y,Z) space 
determined under the corrected constraint condition. As in step S4 of the 
flow chart of FIG. 4 explained earlier, the computation is again conducted 
employing Newton's method (Newton-Raphson formula). Since the joint angles 
are calculated from twelve constraint conditions, the matrix calculation 
is of the twelfth order. Control then passes to step S28 in which the 
standard joint angles are replaced with the corrected joint angles. 
Control then passes to step S30 in the flow chart of FIG. 6, in which the 
joint number counter is initialized to zero (i=0), to step S32 in which 
the actual joint angle .theta.act(i) is detected, to step S34 in which the 
motor velocity command value Vcom(t,i) is determined by obtaining the 
product of gain kp and the deviation between the target value 
.theta.com(t,i) shown in FIG. 3 and the actual value, to step S36 in which 
the determined value is output, to step S38 in which the joint number 
counter value is incremented, to step S40 which repeats steps S32 to S38 
until the count value becomes 12, and when it does, to step S42 in which 
the timer t is incremented, and to step S44 which returns control to step 
S16 for repeating the same operations at time t+1 (provided that 
locomotion has not been terminated). 
As is clear from the foregoing, the present embodiment absorbs and softens 
footfall impact by using all twelve joints for conducting feedback to the 
robot state quantities so as to modify the robot's attitude as required 
for moving its center of gravity in the direction in which the ground 
reaction force acts. Therefore, as is apparent from a comparison of FIGS. 
10 and 20, it becomes possible to achieve a much greater center of gravity 
shift than is possible by the conventional technique involving use of only 
the ankle joints. This in turn means a much larger impact absorption 
capacity. In addition, even when the whole sole is in contact with the 
ground, the embodiment can still achieve absorption and softening of 
footfall impact by, for example, a bending movement of the knees etc. 
Moreover, since all of the joints are controlled at such times, no 
interference arises between the different joints. 
FIG. 11 shows a second embodiment of the invention which differs from the 
first embodiment in the method of calculating the corrected joint angles. 
Specifically, step S26 of the flow chart of FIG. 7 is replaced by step 
S26a of FIG. 11, in which the attitude of the robot 1 in three-dimensional 
space is separated into attitudes projected on the XZ plane and the YZ 
plane as shown in FIG. 10. That is to say, the three-dimensional attitude 
is modeled in two-dimensional representations. The footfall impact 
absorption operation in the up/down and fore/aft directions is controlled 
by determining the joint angles from the attitude projected on the XZ 
plane (the XZ model of FIG. 12), while that in the left/right direction is 
controlled by determining the joint angles from the attitude projected on 
the YZ plane (the YZ model of FIG. 13). This expedient makes it possible 
to complete the calculations in a shorter time than in the first 
embodiment because the matrix computations for calculating the joint 
angles from the constraint conditions are of only the sixth and fourth 
orders in the XZ model and the YZ model, respectively (=6+4). Since the 
second embodiment provides substantially the same degree of center of 
gravity shift as the first, it achieves the same level of increase in 
footfall impact absorption and freedom from impact absorption period 
limitation as the first. Although the two-dimensional modeling of the 
robot's attitude in three-dimensional space may lead to interference 
between joints, the interference is of such small magnitude as to be 
negligible. The remainder of the configuration is the same as in the first 
embodiment. 
FIG. 14 shows a third embodiment of the invention which differs from the 
first and second embodiments in that, as shown in step S26b of the same 
figure, the calculation of the corrected joint angles is further 
simplified by halting the impact absorption control for the left/right 
direction. This expedient is usable because the amount of footfall impact 
in the right/left direction is smaller than that in the fore/aft 
direction, which means that no substantial destabilization of the 
locomotion occurs even if the left/right direction impact absorption is 
not conducted. Therefore, while like the second embodiment the third 
embodiment also produces a two-dimensional model, it conducts impact 
absorption operation using only the XZ model. Specifically, it corrects 
the control values for only the joints 12, 16 and 18, which are the ones 
which drive in the X direction. It is therefore possible to complete the 
calculations in a shorter time than in the second embodiment because the 
matrix computations for calculating the joint angles are of only the sixth 
order. Since the third embodiment provides substantially the same degree 
of center of gravity shift as the first and second, it is able to achieve 
the intended purpose. The remainder of the configuration is the same as in 
the first embodiment. 
FIG. 15 shows a fourth embodiment of the invention. The points of 
difference between this embodiment and the earlier ones are that the hip 
section is used in place of the center of gravity as a basic gait 
parameter and that the footfall impact force is fed back not to the center 
of gravity but to the hip section H (the hip section H is defined as being 
located on the central vertical section line at the bottom of the body 24; 
see FIG. 10) Specifically, when a footfall impact force (ground reaction 
force) F is received, the hip section, which is initially in the standard 
hip position H for standard gait, is shifted in the direction for 
absorbing the force by an amount delta H proportional to the magnitude of 
the force F. Feedback to the hip section is possible since in the case of 
the robot 1 shown in FIG. 10 the center of gravity remains around the hip 
section irrespective of what attitude the robot 1 assumes during 
locomotion. Because of this, the basic object of the invention (to 
increase footfall impact absorption amount) can be achieved by moving the 
hip section instead of the center of gravity. Moreover, one advantage this 
arrangement provides is that it eliminates the need to conduct the 
relatively large volume center of gravity position calculation otherwise 
required. 
FIG. 16 shows a comparison between the case where the coordinates (X, Y) of 
the tip of a second link in a two-link model are calculated and the case 
where the center of gravity (Gx, Gy) of the entire link is calculated. 
From this it will be understood that the amount of calculation required 
for determining the center of gravity, which involves dynamic 
computations, is much greater than the amount of calculation required for 
determining the hip section, which involves only geometric calculations, 
and that the magnitude of this difference becomes extremely large in the 
case of the 13-1ink model (the robot 1) of this embodiment. Another 
advantage is that since a fixed position that does not change with 
attitude is used as a reference, it becomes possible to treat the right 
and left legs independently as will be explained later, which results in 
an additional reduction in the amount of computation required. 
The fourth embodiment will now be explained primarily with reference to the 
differences between it and the earlier embodiments. The hip section is 
included among the attitude constraint conditions defined with respect to 
coordinates of step S2 of the flow chart of FIG. 4 beforehand, and the 
time series data (standard joint angles) are set in advance in steps S4 
and S5. Following this, the constraint conditions, including the hip 
section coordinates H are read in step S100 of the flow chart of FIG. 15, 
the ground reaction force is detected in step S102 in the same manner as 
in the first embodiment, and the amount of correction of the hip section 
is computed in step S104. Taking the X direction as an example, similarly 
to in the first embodiment this is computed as, for instance (See FIG. 9); 
EQU delta Hx=fh(Fx).multidot.k 
Control then passes to step S106 in which the correction amount delta H is 
added to the standard position H and to step S108 in which, as in the 
first embodiment, any of the twelve joint angles requiring correction are 
recalculated. As mentioned earlier, no dynamic computations are required. 
Since as a standard it is possible to use the hip section, which can be 
determined solely by geometric calculation, and further since the left and 
right legs can be treated independently, the relative positions of the hip 
section and sole as well as the rotation angle relationship can be taken 
into account independently and, as a result, the matrix computations for 
calculating the joint angles from the constraint conditions become sixth 
order +sixth order. Thus the amount of computation is much less and the 
computation time much shorter than in the first embodiment. In addition, 
since the distance between the center of gravity and the hip section is 
very small, the shifting of the hip section in response to and in the 
direction of a footfall impact force enables the object of the invention 
to be achieved to substantially the same degree as by the first 
embodiment. 
FIG. 17 shows a fifth embodiment of the invention. In this embodiment, step 
S108 of the flow chart of FIG. 15 is replaced by step S108a, in which the 
joint angles are calculated using two-dimensional models. As in the second 
embodiment, the matrix computation for each of the left and right legs is 
thus of the third order with respect to the attitude projected on the XY 
plane and of the second order with respect to the attitude projected on 
the YZ plane. Since the orders of the overall computation are thus 
(3+2)+(3+2), a reduction in the amount of computation is realized. Since 
the hip section is used instead of the center of gravity, the computation 
is even further simplified. The remainder of the configuration is the same 
as in the fourth embodiment. 
FIG. 18 shows a sixth embodiment of the invention. As indicated in step 
S108b, in this embodiment, as in the third embodiment, the joint angles 
are calculated using two-dimensional models and no impact absorption 
control is conducted with respect to the left/right direction. Aside from 
the point that it is the hip section that is shifted, the effect of this 
embodiment is the same as that of the third embodiment. The remainder of 
the configuration is the same as in the fourth embodiment. 
In the fourth, fifth and sixth embodiments, the hip section was defined as 
being located on the central vertical section line at the bottom of the 
body 24, but, needless to say, the invention is not limited to this 
arrangement. Results similar to those of the foregoing embodiments can 
also be achieved using any other section or portion that, unlike the 
center of gravity, does not vary with the robot attitude and thus does not 
have to be determined by dynamic calculations involving relatively large 
amounts of computation, namely, that can be determined solely by geometric 
calculation, and that is in the vicinity of the center of gravity. 
In addition, while only the ground reaction force (footfall impact force) 
was mentioned as an external force, the invention can also be used to 
correct for other detected forces, including external forces acting on the 
body and forces produced by shifting of internal members. 
Further, the footfall impact control period need not necessarily be set as 
shown in FIG. 5 but can be defined in various other ways. 
While the invention was explained with respect to legged mobile robots, 
specifically biped walking robots, it can also be applied to 
fixed-position robots and autonomous mobile robots falling in the class 
generally referred to as "industrial robots" as well as to legged mobile 
robots with three or more legs. 
The present invention has thus been shown and described with reference to 
the specific embodiments. However, it should be noted that the present 
invention is in no way limited to the details of the described 
arrangements, changes and modifications may be made without departing from 
the scope of the appended claims.