Vol. 29, No. 12, 785–799, http://dx.doi.org/10.1080/01691864.2015.1011694
FULL PAPER
Dead reckoning for biped robots that suffers less from foot contact condition based on anchoring pivot estimation
Ken Masuya∗and Tomomichi Sugihara
Department of Adaptive Machine Systems, Graduate School of Engineering, Osaka University, 2-1 Yamadaoka, Suita, Osaka, Japan
(Received 25 July 2014; revised 4 November 2014; accepted 16 January 2015)
A novel technique of dead reckoning for high-rate feedback control of biped robots is proposed. A fast position estimation of a robot is achieved by fusing information only from internal sensors including joint angle encoders, inertial sensors and force sensors. It combines the kinematics computation and the double integral of acceleration in a complementary way in order to improve the accuracy. The kinematics computation takes the movement of supporting foot, particularly, rotation about a fixed point and rolling on the terrain into consideration. The weights on each information are adjusted automatically based on the reaction force from the ground as it is expected to reflect the certainty of the contact condition of each foot. The validity of the proposed method is verified through computer simulations.
Keywords:dead reckoning; biped robot; self-localization; internal sensor
1. Introduction
A fast and accurate estimation of the current position is cru- cial for high-rate control of mobile robots including legged robots. The global localization of a robot requires external sensors such as cameras,[1–3] a laser range finder (LRF),[4]
GPS and a combination of them [5] in principle. However, their sampling rates are not so high that they are available for high-rate feedback controls. On the other hand, internal sensors such as joint angle encoders and an accelerometer allow higher sampling rate than the above sensors. Hence, the position estimation only by the internal sensors, namely, dead reckoning, could be a practical option.
The dead reckoning is a well-known technique in the field of wheeled robots. The number of rotation of each wheel counted by a rotary encoder provides the position and the orientation.[6] The estimation accuracy can be improved by combining with other sensors such as cameras,[7] LRF [8]
and GPS.[9,10] On the other hand, in the field of legged robots, the relative motion of the body with respect to the supporting foot is often utilized.[11–13] The supporting foot is assumed to be fixed on the ground during one step, so that the position of the body is obtained by the kinematics computation rooted to the supporting foot (KCSF) in the conventional methods. KCSF is not very reliable since the foot often rotates about the contact point, rolls on the terrain or even leaves the ground in more dy- namic locomotions. Another idea is the double integral of the acceleration (DIA),[14] which is free from the ground
∗Corresponding author. Email: [email protected]
contact. While it responds well to quick motion, it substan- tially accumulates the error particularly due to the drift.
As well as for wheeled robots, some methods were pro- posed to combine them in the field of the multilegged robots.
[3,15–18] Some of them [15–17] used Kalman filter for a robot with small hemispherical feet. However, it is often a problem of Kalman filter to tune parameters that model system and observation noises. Apart from the difficulty of parameter tuning, they assumed that each foot contacts to the ground at a point on the hemisphere, so that they are not directly applicable to biped robots with relatively complex geometry at soles. Oriolo et al. [18] proposed an extended Kalman filter for biped robots which combines the dead reckoning with a vision sensor, in which the position of the supporting foot is still considered to stay at the same location during a step. Ahn et al. [3] dealt with the position estimation in human-like walking motions including heel- strike and toe-off. The supporting foot is judged when the heel-contact onto the ground is detected, whereas the detec- tion of contact based on sensors is not as certain as expected, besides the kinematics computation holds the same problem as the conventional methods. Furthermore, it is difficult to know the detailed ground profile in advance in practical situations.
The paper proposes a dead reckoning for biped robots which suffers less from the movement of the supporting foot. It combines the kinematics computation and the ac- celeration measurement, where the complementary filter is
© 2015 The Author(s). Published by Taylor & Francis.
This is an Open Access article distributed under the terms of the Creative Commons Attribution-NonCommercial-NoDerivatives License (http://creativecommons.org/licenses/
by-nc-nd/4.0/), which permits non-commercial re-use, distribution, and reproduction in any medium, provided the original work is properly cited, and is not altered, transformed, or built upon in any way.
based on the following three assumptions, namely, 1) AP is a point which has the instantaneous minimum velocity with respect to the inertial frame in the foot, 2) AP is likely to stay at the same position and 3) AP is near the point of action of the net ground reaction force (GRF). The assumption 2) works for regularizing the condition. Also, the likelihood of the foot contact condition is evaluated in accordance with the information of GRF. Even a floating situation, where the robot loses contact with the ground, can be handled. This paper not only summarizes our previous works [20,21] but also improves the estimation of AP by taking the constraint about moment, namely, the assumption 3), into account.
Section2shows the overview of the proposed dead reck- oning. Section3describes the estimation of AP which is an essential part of the proposed method. Section4shows the adjustment of weights on the kinematics computation and DIA in accordance with GRF. Comparisons with other methods through simulations are shown in Section 5.
Section6concludes the paper.
2. A complementary filter for dead reckoning of a biped robot
Legged robots locomote by alternating the supporting feet on the ground and transferring equilibratory postures. The forward kinematics and its time derivative between the body and the supporting foot are represented as:
pS= p0+R00pS, (1) vS=v0+ω0×R00pS+R00vS, (2)
RS= R00RS, (3)
ωS=ω0+R00ωS, (4) where ∗1p∗2 ∈ R3, ∗1v∗2 ∈ R3, ∗1R∗2 ∈ S O(3) and
∗1ω∗2 ∈ R3denote the position, velocity, attitude and an- gular velocity of a frame∗2 with respect to a frame∗1, respectively.Sand 0 for both the superscripts and the sub- scripts mean the supporting foot frameS and the body frame0shown in Figure1, respectively, and the super- script of the inertial frame is omitted. Hereafter, the estimates of a value∗is denoted by ˆ∗.0ˆpS,0RˆS,0vˆS and
0ωˆS are available through the kinematics computation, if the joint angleqand its time-derivativeq˙are measured by encoders attached to each joint.Rˆ0andωˆ0can be obtained
by an attitude estimator using inertial measurement unit, [22,23] so that
RˆS= ˆR00RˆS (5) ˆ
ωS= ˆω0+ ˆR00ωˆS. (6) The remaining unknowns arepˆ0,vˆ0, ˆpSandvˆS.
Suppose the supporting foot is stationarily in contact with the ground during one step as well as the conventional methods,[11–13,15–18] namely, ˆpSis known andvˆS=0, where0∈R3means the zero vector, so that ˆp0andvˆ0are obtained from Equations (1) and (2) as
ˆ
p0= ˆpS− ˆR00ˆpS, (7) ˆ
v0= − ˆω0× ˆR00ˆpS− ˆR00vˆS. (8) However, the supporting foot often rotates about a contact point, rolls on the terrain or even leaves the ground in dynamic locomotions and the above assumption does not work. DIA,[14] which the foot contact condition does not concern, is another option. Although it responds well to quick motion at high frequency, it suffers from the error accumulation particularly due to the drift. Some proposed state estimation techniques [3,15–18] combine kinematics and inertial information based on Kalman filter for multi- legged robots including bipeds. However, Kalman filter has a common problem that it needs stochastic parameters to be tuned. Another drawback of those methods is that they assumed that each sole has a small hemispherical shape and contacts at a point, so that they are not necessarily suitable for robots which are equipped with rather large feet to support themselves.
The proposed dead reckoning suffers less from the move- ment of the supporting foot. It combines the kinematics computation and the acceleration in a complementary way.
It is easier to tune the parameters in the frequency domain, which the complementary filter deals with, than in the time domain because the frequency response of an accelerometer and accordingly the working range of DIA is empirically identified. The proposed method uses the high-pass filter (HPF) for DIA so as to reduce the effect of the drift and the
Figure 2. An idea of updating the supporting foot position based on AP.
low-pass filter (LPF) was designed complementarily for the other.
In order to improve the accuracy of the kinematics com- putation itself, AP on the supporting foot, which is an esti- mate of the contact point, is used as the known pivot of the kinematics instead of pS. The forward kinematics between the supporting foot and AP is written as:
pS A = pS+RSSpS A, (9) vS A =vS+ωS×RSSpS A, (10) where∗1pS A and∗1vS Arepresent the position and veloc- ity of AP onS with respect to a frame ∗1. Note that AP is instantaneously determined at any moment, so that
SvS A =0. If AP SˆpS Ais estimated, it provides the current ˆ
pS A, which also works as the pivot at the next instance as shown in Figure2. SupposevS A0andˆpS Ais invariant in a short duration,ˆpSandvˆSafter the duration are estimated as:
ˆ
pS= ˆpS A− ˆRSSˆpS A, (11) ˆ
vS= − ˆωS× ˆRSSˆpS A, (12) Then ˆp0andvˆ0are estimated based on Equations (7) and (8). The first question is how to estimateSˆpS A, where the assumptionvS A0would be a criterion.
AP is computable regardless of the associated link, so that it does not suggest any clue that the link is really the supporting foot. Even if the supporting foot is assumed to be either the left or right foot, it is not easy to determine it due to the complex ground profile and uncertainty of contact state, which is the second question. An idea is to measure the possibility of each foot to be the supporting foot by the magnitude of GRF and to adjust the weights for estimates in accordance with it, expecting that the force reflects the certainty of contact condition.
Figure 3 shows the overview of the proposed method.
The sequence consists of the following steps:
(I) The velocity estimator with reaction-force- dependent adjustment of the crossover frequency:
ˆ
v0is estimated by fusing ˙ˆp0and the integral of acceleration.
(II) AP estimator: LˆpL A and RˆpR A are estimated based on the minimum velocity criterion, where the superscripts and the subscriptsLandR, which denote the left foot frameL and the right foot frameR, respectively, are used instead ofS.
(III) AP-rooted forward kinematics computation: ˜p0 is calculated by reaction-force-dependent sum of
˜
p0L and ˜p0R, which are obtained from ˜p0L and
˜
p0R, respectively.
(IV) The position estimator with reaction-force- dependent adjustment of the crossover frequency:
ˆ
p0is estimated by fusingp˜0and DIA.
As shown in the sequence, the proposed method is a dual- stage complementary filter consisting of the velocity and position estimator. The details of each step are described in the following sections.
3. Estimation of AP based on the differential kinemat- ics and the moment equation and AP-rooted kinematics computation
The robot feet and the ground are assumed to be unde- formable. Let us consider the case that the left foot is as- sumed to be the supporting foot. In order to determine
LpL A, the following hypotheses are employed:
1) AP is a point with the instantaneous minimum velocity with respect to in the foot, namely, vL A0,
2) AP is likely to stay at the same position in the foot, namely,δLpL A 0,
3) AP is near the point of action of the net GRF.
Let LFLand LτLbe the force and torque acting to the left foot with respect toL, respectively, which are measured at the position of the force sensor on the left foot LpL F, the hypothesis 3) is represented as
LτL+
LpL F− LˆpL A
× LFL 0. (13)
The hypothesis 1) is supported by the kinematics of AP. In reality, AP is not necessarily stationary due to a possible slip, so that a hypothesisvL A =0does not work. Furthermore, AP is not uniquely determined only from 1) since ωˆL× is not a regular matrix, which causes the ill-posedness. The hypothesis 2) is necessary in order to regularize the problem.
On the other hand, the hypothesis 3) is supported from the mechanical aspect. Such a fusion of multimodal information as the above works for mutual compensation of reliabilities of sensors. The above is mathematically interpreted as a
Figure 3. The overview of the proposed dead reckoning.
minimization of the following weighted sum of evaluation functions with respect toLpL A:
E=α1E1+α2E2+α3E3, (14) where αi > 0 is the weight on Ei (i = 1,2,3) which corresponds to the hypothesisi) as
E1=1
2ˆvL+ ˆωL× ˆRLLˆpL A2, (15) E2= 1
2ζ2δLpL A2, (16)
E3= 1
2ζ3LτL+
LpL F− LˆpL A
× LFL2, (17)
whereζ2andζ3are positive constant values for dimension transformation.α2is set for 1.0, hereafter, sinceE2is always necessary for regularization. RˆL andωˆL are computed as well as Equations (5) and (6).vˆLis computed from Equation (2) as
ˆ
vL = ˆv0+ ˆω0× ˆR00ˆpL+ ˆR00vˆL, (18) where vˆ0 is obtained by the velocity estimator. LˆpL A is estimated from the stationary condition
∂E
∂LˆpL A T
=0. (19)
By putting this LˆpL A into Equations (9) and (11), a tentative position of the left foot˜pLis estimated. Then, the position estimation of the body with respect to the left foot
˜
p0Lis also tentatively obtained from Equation (7) and ˜pL. It is merged with a tentative position estimation of the body with respect to the right foot ˜p0R, which is computed as well as˜p0L, to be ˜p0as described in the following section.
˜
pLand ˜pRare corrected topˆLandˆpR, respectively, based on ˜p0.
The above idea is implemented on a computer in a dis- cretized form. LetT be the sampling interval and∗[k]be a variable∗at the timekT. Equation (14) is rewritten as
E[k] =α1E1[k] +E2[k] +α3E3[k], (20) E1[k] =1
2 ˆvL[k] + ˆωL[k] × ˆRL[k]LˆpL A[k]2, (21) E2[k] = 1
2ζ2LpˆL A[k] − LˆpL A[k−1]2, (22) E3[k] = 1
2ζ3LτL[k] +
LpL F− LˆpL A[k]
×LFL[k]2. (23) ζ2andζ3are defined, for instance, asT2and(MgT)2, respectively, where M[kg] is the robot’s total mass and g =9.8m/s2is the acceleration due to the gravity.
Equation (19) turns to the following equation:
AL[k]LˆpL A[k] =bL[k], (24) where
AL[k] = 1 ζ2
1−α1
Lω[ˆ k]×2
−α3
ζ3
LFL[k]×2
, (25) bL[k] = 1
ζ2
LpL A[k−1] +α1
Lω[ˆ k]×
RˆTL[k]ˆvL[k] +α3
ζ3
LFL[k]×
LτL[k]
−
LFL[k]×2
LpL F[k]
. (26)
and Lωˆ = ˆRTLωˆL.1∈R3×3means the identity matrix. The regularity of AL[k]is proven as follows. Letx =0∈R3, a quadratic form is written as
xTAL[k]x= 1 ζ2
xTx−α1xT
Lω[ˆ k]×2
x
−α3
1 ζ3
xT
LFL[k]×2
x. (27)
The first term of the right-hand side is clearly positive and the second term of that is
−α1xT
Lω[ˆ k]×2
x=α1xT
Lω[ˆ k]×T
Lω[ˆ k]×
x
=α1
Lω[ˆ k]×
x2≥0. (28) Similarly, the third term is also non-negative. Therefore, xTAL[k]x > 0 for∀x = 0, so that AL[k]is a positive definite matrix. Since the positive definite matrix is always regular,AL[k]is proved to be regular. Therefore, LpL A[k] is always computable.
Equations (9), (11) and (7) are also discretized as
˜
pL[k] = ˆpL[k−1] + ˆRL[k−1]LˆpL A[k]
− ˆRL[k]LˆpL A[k], (29)
˜
p0L[k] = ˜pL[k] − ˆR0[k]0ˆpL[k]. (30) Likewise, RˆpR A[k],˜pR[k],˜p0R[k]andˆpR[k]can be com- puted.
It is worth noticing thatEis independent of the geometry of both the terrain and the robot sole. This implies that AP- rooted kinematics possibly improves the estimation accu- racy even on uneven terrains. It is evaluated in Section5.
4. Reaction-force-adaptive fusion of independently estimated positions
4.1. Reaction-force-dependent sum of the position with respect to each foot
The position of the body with respect to each foot is calcu- lated at every time based on AP-rooted kinematics compu- tation as described in the previous section. However, it is not trivial how to vote each of the more possible estimation since it always suffers from sensor noises when determining the supporting foot. An idea to avoid such a binary choice is to merge them as
˜
p0=wL˜p0L +wR˜p0R, (31) wherewSis a non-negative weight on˜p0S, which represents the certainty that the footSis in contact with ground. An idea is that the normal GRF measureswSas
wS= FˆSz+
FˆL z+ ˆFRz+2, (S=L or R), (32) whereis a positive small constant to avoid zero-division.
ˆ
FSzis a truncated value of the normal component of GRF acting to each footFSzdefined as
FˆSz=
⎧⎨
⎩
Mg(Mg≤FSz) FSz (0≤ FSz<Mg)
0 (FSz<0) (S=L or R). (33) 4.2. Reaction-force-dependent adjustment of the crossover frequencies of filters
The contact condition of the feet to the ground varies during the biped locomotion. Even the robot possibly hops and
loses AP. In such situations, AP-rooted kinematics does not appropriately work and DIA is more reliable. Whether the robot is on the ground or in the air is not clearly dis- criminated but measured by GRF as well as the supporting foot. The reliability of AP-rooted kinematics and DIA is balanced by the crossover frequency of the complementary filter. This idea is embodied by the following reaction-force- dependence crossover frequency fp:
fp=
⎧⎨
⎩
fpmin (Fz ≤Fzmin)
fˆp(Fz) (Fzmin≤Fz<Fzmax)
fpmax (Fzmax<Fz) , (34) whereFzis the vertical component of the total GRF, namely, Fz = FL z +FRz. Fzmin and Fzmax mean the lower and upper bound, respectively. fpminand fpmaxare the mini- mum and maximum crossover frequencies of the position estimator, respectively. fˆp(Fz)is a monotone increasing function which satisfies
ˆ
fp(Fzmin)= fpmin, fˆp(Fzmax)= fpmax. (35) The following linear function is available, for instance:
ˆ
fp(Fz)=(fpmax− fpmin) Fz−Fzmin
Fzmax−Fzmin + fpmin (36) Likewise, the crossover frequency of the velocity esti- mator fvis also adjusted adaptively, for instance, as
fv=
⎧⎨
⎩
fvmin (Fz≤ Fzmin)
fˆv(Fz) (Fzmin≤ Fz <Fzmax)
fvmax (Fzmax<Fz) , (37) where
fˆv(Fz)=(fvmax− fvmin) Fz−Fzmin
Fzmax−Fzmin + fvmin. (38) fvminandfvmaxare the minimum and maximum crossover frequency, respectively.
The position is estimated by a complementary filter which combines the position by DIA with that by the AP-rooted kinematics. The reliability of the former signal increases in the high frequency domain, so that HPF is applied. The double integral operator 1/s2has to be cancelled in order to avoid the instability, so that the filter is required to have at least s2. On the other hand, the latter signal is filtered by LPF in a complementary way. Therefore, the position estimator is designed as
ˆ p0= 1
s2Fp1(s) (a0−g)+Fp2(s)˜p0, (39) where a0 is the body acceleration measured by the ac- celerometer andg = [0 0g]T.Fp1(s)andFp2(s)are the filters designed as
Figure 4. The robot model. (a) The exterior. (b) The structure. (c) The shape of the foot.
Table 1. The parameters of the proposed method.
fpmin[Hz] fpmax[Hz] fvmin[Hz] fvmax[Hz] [N] Fzmin[N] Fzmax[N] α1 α3
0.001 1 0.001 1 0.1 0.0 19.6 1 0.01
Fp1(s)= s2
4π2fpf¯p+2π(fp+ ¯fp)s+s21, (40) Fp2(s)= 4π2fpf¯p+2π(fp+ ¯fp)s
4π2fpf¯p+2π(fp+ ¯fp)s+s21, (41) where fp is adjusted by Equations (34) and (36) and
f¯p =
fpminfpmax is a constant crossover frequency.
Obviously,Fp1(s)andFp2(s)satisfy the following com- plementary condition:
Fp1(s)+Fp2(s)=1. (42) The velocity is estimated by combining the velocity by the integral of the acceleration with that by the derivative of the estimated position also in a complementary way. The velocity estimator is designed as
vˆ0= 1
sFv1(s) (a0−g)+sFv2(s)ˆp0. (43) Fv1(s)andFv2(s)are the filters designed as
Fv1(s)= s
2πfv+s, (44)
Fv2(s)= 2πfv
2πfv+s, (45)
where fv is adjusted by Equations (37) and (38). Fv1(s) is designed to have the differential operators in order to cancel the integral operator 1/s. Fv1(s)and Fv2(s)also satisfy the following complementary condition:
Fv1(s)+Fv2(s)=1. (46) In the implementation, the filters are converted in a dis- cretized way by the bilinear transformation.
5. Evaluation of the proposed method through simulations
5.1. Simulation set up
In order to evaluate the validity of the proposed method, dynamic simulations were conducted on OpenHRP3.[24]
Figure 4 shows the supposed robot model, which is equipped with a gyroscope and an accelerometer on the trunk and force sensors at each ankle. M andT are set for 10.0 kg and 0.002 s, respectively. The joint input torques were defined by a PD controller based on the reference of joint angles and its time-derivative which were calculated by Yamamoto et al.’s method,[25] in advance. Both static and kinetic friction coefficients between the ground and robot model were set for 1.0.
The following methods were compared:
• KCSF
• DIA with HPF (DIA+HPF)
• Complementary filter combining KCSF with DIA (KCSF+DIA)
• Kalman filter combining KCSF with DIA (KF)
• The proposed dead reckoning (Proposed)
The parameters for Proposed are shown in Table 1. The same frequency filter with Proposed was applied to KCSF+
DIA in order to clarify the effect of AP-rooted kinematics.
The crossover frequency of that filter was also adjusted by Equations (34) and (36). HPF used in DIA+HPF was a second-order HPF with cut-off frequency 0.001 Hz. The system of Kalman filter is represented as
d dt
p0 v0
= O 1
O O p0 v0
+
O 1
(a0−g) , (47)
Figure 5. The snapshots of walking on the horizontal ground.
Table 2. The estimation error for walking on the horizontal ground without the attitude error.
Position [mm] Velocity [mm/s]
Method x y z 3D x y z 3D
KCSF 21.37 6.116 36.47 42.71 50.62 35.00 115.0 130.4
DIA+HPF 23.87 33.61 44.54 60.69 31.61 44.08 62.13 82.47
KCSF+DIA 18.77 6.026 36.10 41.13 27.01 18.33 64.67 72.44
KF 19.84 5.441 36.37 41.79 21.62 29.28 50.15 61.97
Proposed 9.881 5.406 15.93 19.51 14.16 13.31 28.70 34.66
Figure 6. A result of position estimation for walking on the horizontal ground without the attitude error.
p0=
1 O p0 v0
, (48)
where O ∈ R3×3means the zero matrix. The covariance matrices in KF to model the process and observation noises
are represented as diagonal matrices and tuned through about 100 times of trials and errors.
In order to simulate the sensor noise on the accelerometer, the following erroreawas added to the true acceleration:
ea∼N(μa,0.121), μa∼N(0,0.0421), (49)
Figure 7. A result of velocity estimation for walking on the horizontal ground without the attitude error.
Table 3. The estimation error for walking on the horizontal ground with the attitude error.
Position [mm] Velocity [mm/s]
Method x y z 3D x y z 3D
KCSF 38.81 35.88 41.08 66.94 1029.4 1010.3 163.8 1451.6
DIA+HPF 123.3 105.7 52.69 170.7 142.1 136.9 72.75 210.3
KCSF+DIA 33.17 36.27 42.52 64.99 185.3 186.9 73.13 273.2
KF 29.74 30.12 42.08 59.68 100.9 103.5 60.48 156.7
Proposed 31.04 35.40 21.61 51.80 101.4 109.9 36.60 153.9
whereN(μ,)denotes the normal distribution composed of the meanμand the covariance matrix. The meanμa
was initialized at the beginning of each simulation. Also, noises on the force sensor were simulated as
ef ∼N(0,1.021), eτ ∼N(0,0.0121), (50) whereef andeτ denote the error of the force and torque, respectively. In terms of the error of the attitude estimator, the following erroreR was added to Euler angles of the current attitude:
eR = 3.0
(1+(1/10π)s)2wR, wR∼N(0,0.121) (51) wherewRwas filtered by a second-order LPF. Ten patterns of noises were prepared for each of the accelerometer, the force sensor and the attitude estimator, so that the total
number of combinations of noises is 1000, meaning that the simulation ran 1000 times per each motion.
5.2. Simulation 1: forward walking
First, forward walking on a horizontal ground with rotation and rolling of the supporting foot was examined. In the sim- ulation, the robot takes two steps forward with the rotation and rolling of the supporting foot during the phase between heel-strike and toe-off, as shown in Figure5. The left and right foot were, respectively, planned to lift up and rotate in the first step from 0.0 to 1.0 s. In the second step from 1.0 to 2.0 s, those foot were, respectively, planned to move in contrast to the first step. In order to evaluate the influence of the attitude estimation error on the proposed method, the result without noises is also compared.
Figure 8. A result of position estimation for walking on the horizontal ground with the attitude error.
Figure 9. A result of velocity estimation for walking on the horizontal ground with the attitude error.
Figure 10. Locus of AP on the foot (the foot is rendered in wire frame).
Figure 11. The snapshots of jumping.
Table 4. The estimation error for jumping.
Position [mm] Velocity [mm/s]
Method x y z 3D x y z 3D
KCSF 37.27 48.50 331.3 336.9 858.2 1124.7 1669.8 2188.5
DIA+HPF 342.5 366.3 199.1 539.5 240.0 262.4 181.1 399.0
KCSF+DIA 84.76 87.27 175.8 213.8 166.3 194.0 227.2 341.9
KF 34.77 41.52 337.3 341.6 140.7 151.6 255.7 328.8
Proposed 85.61 86.61 74.46 142.7 160.8 167.7 117.7 260.5
Table 5. The estimation error for walking on uneven terrain.
Position [mm] Velocity [mm/s]
Method x y z 3D x y z 3D
KCSF 34.87 45.44 39.80 69.75 1038.9 1006.9 172.6 1457.0
DIA+HPF 123.7 105.4 52.81 170.9 141.6 137.5 72.69 210.3
KCSF+DIA 32.47 39.30 42.57 66.41 186.8 197.4 67.73 280.1
KF 26.37 38.65 41.69 62.66 100.8 108.3 59.83 159.6
Proposed 32.72 35.53 21.59 52.90 101.9 110.5 34.14 154.1
The root-mean-square error (RMSE) of the position and velocity estimation without the attitude error are tabulated in Table2. Notice that those results are obtained from 100 simulations except for the cases with the attitude error. An example set of those estimations are shown in Figures6 and7. From the result, the accuracy of KCSF, particularly
inz-direction, is degraded due to the rotation and rolling of the supporting foot. On the other hand, that of DIA+HPF suffers from the error accumulation with the integration.
Although KCSF+DIA and KF show better results, the rota- tion and rolling of the supporting foot still affect. Compared to those methods, Proposed reduces RMSE, particularly
Figure 12. A result of position estimation for jumping.
Figure 13. A result of velocity estimation for jumping.
Figure 15. A result of position estimation for walking on uneven terrain.
less than half in the position estimation compared with KCSF+DIA.
On the other hand, RMSE of estimations with the attitude error are shown in Table3. Figures8and9show an example set of those estimations. The result shows that the attitude error lowered the accuracy of all estimations. Even the error of Proposed has increased about two and a half times in the position estimation and about five times in the velocity estimation. However, RMSE of Proposed is the minimum among the all methods.
AP on each foot is plotted in Figure10. It is expected that AP works as the approximation of the real contact point, which should be on the sole. The figure indicates, however, that AP does not always exist on the sole due to the slippage
with the rotation. On the other hand, it is seen that the attitude estimation error does not significantly perturb the estimation of AP thanks to the information of GRF.
5.3. Simulation 2: jumping
Next, a jumping motion was examined. Snapshots of the tested motion are shown in Figure11. The robot first squats down and jumps by quick vertical motion. RMSE of the position and velocity estimation are shown in Table4. An example set of those estimations are plotted in Figures12 and13, respectively.
Inz-direction, KCSF cannot follow the true locus during the aerial phase from about 2.6 s to about 3.5 s. DIA+HPF
Figure 16. A result of velocity estimation for walking on uneven terrain.
rather worked during that period but was affected by the error accumulation. Compared with those methods, KCSF+
DIA and Proposed showed better accuracy inz-direction.
Even when either foot was in contact, the accuracy of the two was also better. The efficacy of the reaction-force- dependent adjustment technique is confirmed. Furthermore, the accuracy of Proposed is better than that of KCSF+DIA, through which the efficacy of the AP-rooted kinematics is also verified.
5.4. Simulation 3: Walking on uneven terrain
A forward walking on an uneven terrain was examined. As shown in Figure14, the robots takes two steps forward as well as on the horizontal plane, but the unexpected turn happens due to the unevenness. The unevenness was mod- elled by tetrahedra randomly located in the range from 0.0 to 0.014 m in height with respect to the ground level and 0.02×0.02 m2 bottom face. The referential motion trajectory was the same with that in simulation 1. Table 5 shows RMSE of the position and velocity estimations.
Figures15and16show an example set of those estimations, respectively.
Obviously, the accuracy of KCSF was degraded due to the irregular movement of the supporting foot, which remains even in KCSF+DIA and KF. On the other hand, Proposed showed the same level of accuracy with that in simulation
1. Therefore, it is confirmed that Proposed works even on an uneven terrain.
6. Conclusion
A novel dead reckoning for biped robots was proposed. It is peculiarized by (i) a combination of the double-integration of acceleration and AP-rooted kinematics in a complemen- tary way, (ii) the estimation of AP based on both kinematics and force information and (iii) reaction-force-dependent adjustment of reliability.
It is shown through simulations that the proposed method improves the accuracy of the position estimation compared with the conventional methods, particularly in situations where the supporting foot moves with respect to the ground and even lacks ground contact. It is worth noting that the proposed method can estimate inclination and turning of the robot body which is not expected in the motion planning stage. Such rotational movements are caused since the robot is essentially underactuated and should be compensated by a feedback control. It only uses high-rate internal sensors and is expected to work for feedback controls of quick motions.
Evaluation on a real robot is the future work.
The paper does not claim that the proposed technique overrides the use of external sensors. It is mandatory to combine the dead reckoning and the global localization for dependable long-term autonomy.
at the Department of adaptive machine systems, Graduate School of Engineering, Osaka University. He received his BS and MS degrees in mechanical engineering from the University of Tokyo, Japan, in 1999 and 2001, respectively. He also received his PhD from the University of Tokyo in 2004. He was an academic research assistant from 2004 to 2005 at the University of Tokyo, and became a research associate. He worked at Kyushu University as a guest associate professor from 2007 to 2010. He moved to Osaka University in 2010 and held the current position. He is the principal investigator of Motor Intelligence Laboratory. His research interests include kinematics and dynamics computation, motion planning, control, hardware design and software development of anthropomorphic robots. He also studies human motor control based on robotic technologies.
He is a member of IEEE.
References
[1] Thompson S, Kagami S. Humanoid robot localisation using stereo vision. In: Proceedings of the 2005 IEEE-RAS International Conference on humanoid robots; Tsukuba, Japan; 2005. p. 19–25.
[2] Kwak N, Stasse O, Foissotte T, Yokoi K. 3D grid and particle based SLAM for a humanoid robot. In: Proceedings of the 9th IEEE-RAS International Conference on humanoid robots; Paris, France; 2009. p. 62–67.
[3] Ahn S, Yoon S, Hyung S, Kwak N, Roh KS. On- board odometry estimation for 3D vision-based SLAM of humanoid robot. In: Proceedings of the 2012 IEEE/RSJ International Conference on intelligent robots and systems;
Vilamoura, Algarve, Portugal; 2012. p. 4006–4012.
[4] Hornung A, Wurm KM, Bennewitz M. Humanoid robot localization in complex indoor environments. In:
Proceedings of the 2010 IEEE/RSJ International Conference on intelligent robots and systems; Taipei, Taiwan; 2010. p.
1690–1695.
[5] Oßwald S, Hornung A, Bennewitz M. Improved proposals for highly accurate localization using range and vision data. In: Proceedings of the 2012 IEEE/RSJ International Conference on intelligent robots and systems; Vilamoura, Algarve, Portugal; 2012. p. 1809–1814.
[6] Crowley JL. Asynchronous control of orientation and displacement in a robot vehicle. In: Proceedings of the 1989 IEEE International Conference on robotics and automation;
Scottsdale, AZ; 1989;3:1277–1282.
[7] Chenavier F, Crowley JL. Position estimation for a mobile robot using vision and odometry. In: Proceedings of the 1992 IEEE International Conference on robotics and automation;
Vol. 3, Nice, France; 1992. p. 2588–2593.
of the 2003 IEEE International Conference on robotics and automation; Taipei, Taiwan; 2003. p. 1978–1984.
[11] Roston GP, Krotkov EP. Dead reckoning navigation for walking robots. In: Proceedings of the 1992 IEEE/RSJ International Conference on Intelligent Robots and systems;
Raleigh, NC, USA; 1992. p. 607–612.
[12] Masuda T, Mae Y, Arai T, Inoue K. Dead reckoning of multi-legged robot – error analysis –. In: Human friendly mechatronics, selected papers of the International Conference on machine automation; Osaka, Japan; 2000.
p. 351–356.
[13] Mae Y, Masuda T, Arai T, Inoue K. Error analysis of dead reckoning of multi-legged robots. In: Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems; Maui, HI, USA; 2001. p. 1558–1563.
[14] Marques L, Lobo J, Dias J, Nunes U, de Almeida AT.
Sensors for legged mobile robots. In: Proceedings of 2nd International workshop on climbing & walking robots (Clawar’99); Portsmouth, UK; 1999. p. 33–58.
[15] Chilian A, Hirschmüller H, Görner M. Multisensor data fusion for robust pose estimation of a six-legged walking robot. In: Proceedings of the 2011 IEEE/RSJ international conference on intelligent robots and systems; San Francisco, CA, USA; 2011. p. 2497–2504.
[16] Bloesch M, Hutter M, Hoepflinger M, Leutenegger S, Gehring C, Remy CD, Siegwart R. State estimation for legged robots – consistent fusion of leg kinematics and IMU.
In: Proceedings of robotics: science and systems; Sydney, Australia; 2012.
[17] Reinstein M, Hoffmann M. Dead reckoning in a dynamic quadruped robot based on multimodal proprioceptive sensory information. IEEE Trans. Rob. 2013;29:563–571.
[18] Oriolo G, Paolillo A, Rosa L, Vendittelli M. Vision-based odometric localization for humanoids using a kinematic EKF. In: Proceedings of the 12th IEEE-RAS International Conference on Humanoid Robots; Osaka, Japan; 2012. p.
153–158.
[19] Higgins WT. A Comparison of complementary and Kalman filtering. IEEE Trans. Aerosp. Electron. Syst. 1975;AES- 11:321–325.
[20] Masuya K, Sugihara T. A Dual-stage complementary filter for dead reckoning of a biped robot via estimated contact point. In: Proceedings of the 2013 IEEE-RAS International Conference on Humanoid Robots; Atlanta, GA, USA; 2013.
p. 112–117.
[21] Masuya K, Sugihara T. Dead reckoning of biped robots with estimated contact points based on the minimum velocity criterion. In: Proceedings of the 2013 IEEE/RSJ international conference on intelligent robots and systems;
Tokyo, Japan; 2013. p. 3637–3642.
[22] Masuya K, Sugihara T, Yamamoto M. Design of complementary filter for high-fidelity attitude estimation based on sensor dynamics compensation with decoupled
properties. In: Proceedings of the 2012 IEEE International Conference on Robotics and Automation; Saint Paul, MN, USA; 2012. p. 606–611.
[23] Mahony R, Hamel T, Pflimlin JM. Nonlinear complementary filter on the special orthogonal group. IEEE Trans. Autom.
Control. 2008;53:1203–1218.
[24] Nakaoka S, Hattori S, Kanehiro F, Kajita S, Hirukawa H.
Constraint-based dynamics simulator for humanoid robots
with shock absorbing mechanisms. In: Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems; 2007; San Diego, CA. p. 3641–3647.
[25] Yamamoto K, Sugihara T, Nakamura Y. Gait planning including toe contact with boundary condition relaxation.
In: Proceedings of the 17th CISM-IFToMM Symposium (RoManSy17); 2008; Tokyo, Japan. p. 409–416.