Dead reckoning in a dynamic quadruped robot based ...

Dead reckoning in a dynamic quadruped robot based on multimodal proprioceptive sensory information Michal Reinstein, Member, IEEE, and Matej Hoffmann

In mobile robots, a popular solution lies in the combination of a proprioceptive component in the form of an inertial navigation system (INS) and an external reference system. INS combines information from accelerometers and gyroscopes composed in an inertial measurement unit (IMU) and integrates them into relative changes of the robot’s pose. If long-term precision is to be guaranteed, absolute information needs to be provided. External references (landmarks) can be sensed using vision (e.g., [1]) or ultrasonic sensors [2], or absolute position information can be directly obtained from a GPS (e.g., [3]). However, all these solutions have limitations. For instance, signal robustness, availability (indoor/outdoor), and accuracy issues (attenuation through atmosphere, multipath errors) can be listed among the drawbacks of a GPS; computer vision has typically high computational requirements and is vulnerable to lighting conditions and the presence and saliency of landmarks in the environment. Inspired by the animal kingdom, in this paper, we are aiming to solve the classical dead reckoning (path integration) problem by proposing a system that does not rely on any external reference. Studies on mammals and arthropods (e.g. [4], [5], [6]) have accumulated evidence that motion and position can be reliably estimated relying on socalled self-motion cues (such as inertial and joint information) only. Since neither absolute position nor heading can be sensed directly, drift is eventually inevitable. The challenge lies in achieving a graceful degradation of the estimates. In our mobile robot, we have decided to use a low-cost inertial measurement unit (IMU) as our primary sensor. However, to prevent a major drift in estimates that would result from double integration of raw noisy inertial data, we were seeking another information source (other than external reference) that would provide aiding. In general, there are two possibilities. First, a kinematic model of a vehicle can be used, typically to provide velocity constraints. Second, aiding information can be provided by an additional egomotion sensor: an odometer. In wheeled robots, the solutions are relatively straightforward. A vehicle’s motion is restricted to two dimensions. Furthermore, additional constraints regarding its turning radius, for instance, can be applied, giving rise to the vehicle’s motion model. An odometer can be obtained by simply measuring the rotations of the wheels. Both aiding sources (model and odometer) were fused with an inertial

Abstract — It is an important ability for any mobile robot to be able to estimate its posture and to gauge the distance it travelled. In this work, we have addressed this problem in a dynamic quadruped robot by combining traditional state estimation methods with machine learning. We have designed and implemented a navigation algorithm for full body state (position, velocity, and attitude) estimation that does not use any external reference, but relies on multimodal proprioceptive sensory information only. Extended Kalman Filter was used to provide error estimation and data fusion from two independent sources of information: (1) strapdown mechanization algorithm processing raw inertial data and (2) legged odometry. We have devised a novel legged odometer that combines information from a multimodal combination of sensors (joint and pressure). We have shown our method to work for a dynamic turning gait and we have also successfully demonstrated how it generalizes to different velocities and terrains. Furthermore, our solution proved to be immune to substantial slippage of the robot’s feet.

KEYWORDS: legged robots, odometry, dead reckoning, extended Kalman filter, path integration, slippage I. INTRODUCTION

I

T is a strong requirement for any mobile agent, animal or robot, to be able to estimate its posture and to gauge the distance it travelled. The former is usually crucial for successful locomotion; the latter for navigation. The information can be – often indirectly – obtained from various sensors: proprioceptive (inertial measurements, joint sensors on legs or motor encoders) and exteroceptive (primarily visual information and compass measurements). Exteroceptive sensors – which acquire information from the robot’s environment – can be also used to perceive external references (landmark detection using visual sensors, for instance) that are necessary for long-term precision in a navigation task. M.R. was supported by the project FP7-ICT-247870 NIFTi. M.H. was supported by the project FP7-ICT-270212 “eSMCs”'. M. Reinstein is with the Dept. of Cybernetics, Centre for Machine Perception, Faculty of Electrical Engineering, Czech Technical University in Prague, Prague, 166 27 Czech Republic (e-mail: [email protected]). M. Hoffmann is with the Artificial Intelligence Laboratory, University of Zurich, Andreasstrasse 15, 8050 Zurich, Switzerland (e-mail: [email protected]).

1

robot’s change in position in a statistical, data-driven fashion. Concretely, we trained a regression function relating a pool of features over the sensory data to the robot’s stride length (relative position increment over a gait period), giving rise to a Legged Odometer (LO). Finally, this position increment (with appropriate noholonomic velocity constraints) obtained from the LO was used to aid integrated inertial measurements. Data fusion was realized by an Extended Kalman filter (EKF). Our contributions are: First, we propose a novel datadriven odometer for a dynamic legged robot that relies on the exploitation of multimodal proprioceptive sensory information. Second, we develop an appropriate data fusion algorithm based on the EKF. Third, we demonstrate that the proposed architecture not only generalizes to different velocities of motion (0.08 – 0.21 m/s), but also to different grounds (five surfaces of different friction). Fourth, we assess the relative contributions of the different components of the system. More than 120 experimental runs were conducted. This article is an extension of our previous work [19], expanded by the following: (i) additional experiments on three new grounds to demonstrate generalization to different terrains; (ii) new approach to the legged odometer design which reduces the designer bias as well as, on average, the error in estimation to less than half compared to [19], (iii) an analysis of the contributions of individual sensory modalities; (iv) more elaborate description of the data fusion. The paper is structured as follows. Section II explains our data fusion approach. Section III briefly describes the experimental setup. Section IV is devoted to the legged odometer. Section V demonstrates the performance of our legged odometer-aided inertial navigation system (INS-LO). Section VI concludes our contribution.

navigation system in wheeled vehicles in [7], [8]. Compared to wheeled applications, there is much less work addressing legged platforms. This problem is much harder. First, it has more dimensions: unlike in a wheeled vehicle, a legged platform’s motion involves pitching and rolling motions, making the body motion inherently three dimensional and most kinematic representations nonlinear [9]. Consequently, compared to wheeled vehicles, there are much fewer constraints that could be used for aiding. Nevertheless, using joint encoders, the advancement of the Center of Mass (COM) can be calculated through kinematic transformations – as demonstrated in statically stable walking robots in [10], [11]. Imprecision comes mainly from foot slippage and link and joint flexion. Dynamic locomotion, which may include aerial phases, poses even more difficulties, as the excursion of the COM during the flight phase is not observable from the kinematic configuration. Hybrid models that switch between contact and aerial phases need to be introduced (see [12]). In hexapod robots, the tripod gait is typically used and the assumption of the model is that three non-collinear feet are in contact with the ground at all times [13] [14] [15]. In [16], aiding of inertial sensors was provided by an attitude estimator using strain-based leg sensors and the authors could handle even jogging gaits by splitting the modeling into tripod stance phase, liftoff transient phase, aerial phase and touchdown transient phase. Using a quadruped robot, Bloesch et al. [17] devised a kinematic model that can handle intermittent ground contacts without taking any assumptions on the gait used. However, none of the methods reviewed above could deal with substantial slippage of the robot’s feet. In our case, we were dealing with an underactuated compliant dynamic quadruped platform, in which slippage is an integral part of the robot’s locomotion. Furthermore, we addressed ground of different friction. There, because of slip, the same kinematic trajectory of the robot results in substantially different displacement1 (see the multimedia attachment). Modeling different grounds and the robot’s interaction with them would be an option, but this would first require prior knowledge about the grounds and second, modeling of contacts is a hard problem. Therefore, we were seeking an odometer that does not require a model of the robot’s interaction with the ground. One option that fulfills these criteria is visual odometry, as demonstrated by the Mars exploration rovers, for instance [18]. The requirements there are a camera, some moderate assumptions on the scene characteristics, and reasonable computational power. In our contribution we demonstrate an alternative solution. We employ a multimodal sensory set that consists of angular position sensors on active joints (controlled by servomotors), passive compliant joints, and pressure sensors on the robot’s feet, and establish a relationship with the

II. THEORY AND METHODOLOGY A. Inertial Navigation Systems A conventional IMU is composed of three accelerometers and three angular rate sensors, mounted perpendicularly to each other to create an orthogonal measurement frame [20, p. 12]. Therefore, each accelerometer is able to detect the specific force that is defined as the time rate of change of velocity relative to local gravitational field [21], [22]. For navigation in any frame, it is important to track directions, in which the accelerometers sensing axes are oriented. This is done by sensing the rotational motion using angular rate sensors and integrating them [21], [22]. When the angular orientation of the IMU is known, compensation for projected local gravity value, Coriolis, and centripetal acceleration is done. Resulting acceleration signals are integrated consecutively to obtain velocity and position vectors. B. Strapdown Mechanization Algorithm Strapdown mechanization is an algorithm that converts raw inertial data into navigation variables, i.e. velocity, position, and attitude [20, pp. 29-39]. We have developed a strapdown mechanization by combining the approaches

1 For instance when running with the gait used in this work at motor frequency 1 Hz, the robot travels on average 11.5 cm/s on low-friction plastic foil, but 18 cm/s on cardboard or Styrofoam.

2

described in [21], [22], [23, pp. 61-75], with enhancements proposed in [24]. Our implementation determines the navigation variables from raw inertial measurements by integrating differential equations describing the motion dynamics [24, p. 29]. The algorithm proceeds with the following steps: compensation of sensor errors, numerical integration of sensor outputs, velocity update, position update, and attitude update. In our implementation, we have employed a quaternion approach. Experimental evaluation of our strapdown mechanization can be found in [25], [26].

where two independent sources of information (position and velocity computed from raw inertial data via strapdown mechanization, and relative position increment obtained from the LO) are combined to compensate for each other’s limitations [33]. In addition, very simple nonholonomic constraints are exploited (modeled as zero mean lateral and vertical body frame velocities with variances determined from analysis of the referential trajectory). The algorithm of the legged odometer aided inertial navigation system INS-LO is initialized with calibration parameters for the inertial sensors; coarse alignment proceeds to estimate the initial heading. While the system is static during initialization, fine alignment algorithm is executed using the same error model in order to estimate the biases of the six inertial sensors used; these biases are then compensated as the navigation proceeds (for details see previous work [34]). Fig. 2 consists of several blocks. Strapdown Mechanization block: the output of the strapdown mechanization are the updated velocity, position, and attitude vectors obtained from the inertial data processed at 100 Hz. Legged Odometer block: the LO processes the joint and pressure data measured at 50 Hz. Using a regression function the stride length is computed for each gait period. The output of the LO over one gait period is converted into position aiding signal that enters the EKF. Error Model Update block: The output of the strapdown mechanization together with the sensors noise models are used to update the error model at 100 Hz. Extended Kalman Filter block: Since aiding proceeds at 20Hz, in the meantime the EKF runs as a predictor with Kalman gain set to zero. If at least one EKF update has occurred, feedback to the strapdown mechanization is provided. Each time this feedback occurs, the errors are corrected and the state vector is reset to zero to prevent unbounded error growth.

C. Extended Kalman Filter The Kalman Filter (KF) [27] has been widely used in INS state estimation and filtering. It combines all available measurements with prior knowledge about the sensing device and the system to produce an optimal state estimate that statistically minimizes the error in a least square sense. In navigation applications, the KF appears in many variations, which deal with non-linearities of the system model equations, e.g. the EKF; for details see [24], [28]. For a long time, the EKF has been playing a key role in navigation software design. Whereas the KF estimates states of a linear system model only, the EKF can estimate states of a nonlinear system model, but only up to a certain level of nonlinearity [29, pp. 176-182]. The choice of an appropriate model is crucial since only small errors are allowed to be delivered to the EKF. Because of the first-order approximations, large values can cause biased solutions and inconsistency of the covariance update [24]. Therefore, instead of using state-vector representation for actual navigation variables we implemented the EKF to estimate only the errors given by an error model. Detailed description and derivation of the EKF equations that we implemented for data fusion can be found in [29].

III. EXPERIMENTAL SETUP

D. Error Model for Data Fusion To develop an error model for data fusion as shown in Fig. 2, we considered the theory regarding sensor error propagation [29]. Our error model is based on the classical 15-state concept described in [30, pp. 35-41], [31]. We have taken the uncoupled approach to the classical perturbation analysis [30] to obtain equations capturing the error dynamics. These perturbation equations were linearized and errors of the second order and higher were neglected. Then, the equations were expressed in terms of position, velocity, attitude, and sensor errors to form the desired linearized error model. The appropriate parts of the covariance matrix of measurement noise were determined by statistically evaluating the precision of the stand-alone LO with respect to reference in velocity and position over more than 100 runs. In order to model the random sensor errors, we employed Allan variance analysis evaluated on sensor noise data. We explored this method in detail in [32]. The original model with appropriate proof can be found in [30]. Due to the nature of the EKF, we constructed the data fusion scheme as a centralized processing approach combined with closed loop state vector estimation. The architecture therefore corresponds to a complementary filter,

The robot used is 19 cm long and has four identical legs driven by position-controlled servomotors in the hips. In addition, there are four passive compliant joints at the “knees”. A photograph of the robot can be seen in Fig. 1. The mechanical design (weight distribution, proportions, springs used, etc.) is a result of previous research (e.g., [35], [36]). New material (an adhesive skin used for ski touring from Colltex), which has asymmetrical friction properties, was added onto the robot’s feet. This allows the robot to get a good grip during leg retraction (stance), but enables sliding during protraction (swing).

Fig. 1. Quadruped robot “Puppy” and its sensory set [19, p. 619].

3

Correction Feedback (20Hz) , where

Extended Kalman Filter Algorithm 1. Time update

are attitude errors:

2. Observation update If measurements unavailable, set Kk = 0

LEGGED ODOMETER (50 Hz) Measurements update (100 Hz) Measurements vector zk is defined as:

INPUT: 4xpressure, 4x knee and 4x hips angles OUTPUT: stride length

Error Model Discretization (100 Hz) System transition matrix F and process noise covariance matrix Q are discretised: , , where I is unit matrix of appropriate size, is the expected mean value operator

STRAPDOWN MECHANIZATION (100Hz) INS-LO OUTPUT (100 Hz) ,

INPUT: 3x acceleration, 3x angular rate OUTPUT: 3D velocity, position, orientation

Error Model Update (100 Hz) System transition matrix F defines the state-space model as follows: where

,

,

,

,

,

F, Φ – The system transition matrix continuous time and discrete G – The process noise coupling matrix H – The measurement sensitivity matrix B – The control matrix Q – The covariance matrix of process noise R – The covariance matrix of measurement noise x – The system state vector (15 states: errors in position, velocity, attitude, errors of accelerometers and angular rate sensors) z – The measurement vector k – The discrete time-step , – Sensor noise models

,

,

,

RN – Radius of curvature in the prime vertical RM – Meridian radius of curvature RM, – Roll, pitch and yaw angles – Quaternion and corresponding Direction Cosine Matrix (DCM) for transformation from body frame (BF) to navigation (NED) frame – Quaternion and corresponding DCM for transformation from navigation (NED) frame to Earth-centered Earth-fixed (ECEF) frame – The position vector [latitude, longitude, altitude] – The velocity vector due to North, East, Down – Navigation variables computed by mechanization – Errors estimated by the EKF as the part of state vector

Fig. 2. Data fusion scheme for the legged odometer aided inertial navigation system INS-LO.

* i (t )

Ai sin 2 ft i Bi (1) where the oscillation can be varied by changing the amplitude Ai, offset Bi, frequency f, and phase θi parameters. We have investigated two different gaits in [19]. In this work, we wanted to specifically address generalization to different grounds and contributions of individual sensory channels. We have thus used a single gait only: a dynamic right-turning gait derived from a bounding gait, since this

All joints (active hips, passive knees) were equipped with angular position sensors, and every foot had a pressure sensor underneath, sampled at 50 Hz. Xsens IMU was used to measure the inertial data. We applied a simple oscillatory position control of the motors. The target joint angle of each motor i* (and hence of each leg) was determined as: 4

gait is stable on a bigger range of terrains (see multimedia attachment).

distance and distance measured by the Tracker software between two markers placed on the robot. For examples of the trajectories and raw sensory signal plots see [19]. B. Feature Computation In line with our previous approach [19], we have used a pool of features that compress the raw sensory signals in each period of locomotion (these can then be seamlessly related to stride length, which is also defined over a locomotion period). However, in an effort to reduce the designer’s bias, we have enriched this feature set over [19]. For each sensory channel, the amplitude, mean, variance, and numerical integral were computed. These features were denoted as a single-channel feature set (with 12 sensory channels and 4 feature types this gives 48 base features). Second, powers (squares and cubes) of these features were also added. Then, a cross-channel feature set was formed by combining the single-channel features with each other by means of linear combinations, ratios and multiplications of features of the same modality (e.g. sum of the means of amplitudes of the left knees and hips was one feature, or ratio between the variances of the front and hind pressure signals another). For numerical reasons, all the features were normalized.

Fig. 3. Experimental setup and tracking software: The robot was running in an arena and was tracked from a single camera providing a top view (left). Free tracking software Tracker, http://cabrillo.edu/~dbrown/tracker/AAPT _video_modeling_2009.pdf) was used to obtain the referential trajectory.

During the experiments the robot was running in an arena of about 2.5 x 2.5 m and was tethered. An overhead camera attached to the ceiling was used to record the robot’s movement. Later, the position of a red-colored marker placed roughly above the robot’s COM was tracked together with a green marker above the robot’s head (for heading tracking). Fig. 3 is a screenshot from the free video tracking software Tracker depicting the robot in the arena from above. Individual terrains were selected to cover a range of different friction; Table I shows experimentally determined estimates of the static friction coefficients between the adhesive skin on the robot’s feet and the different substrates. Since the “skin” has asymmetrical friction properties, coefficients in both directions had to be determined.

C. Training the Regression Function In our previous work [19], we have first looked for correlations of individual features with stride length, then picked few (4 or 5) features that performed best, while ensuring that the features are not overlapping (are not based on the same base features). In this work, we have devised a more automatic procedure. We have defined a new linear regression function , which processes the features and outputs the estimated stride length. It is defined as follows:

TABLE I STATIC FRICTION COEFFICIENT ESTIMATES BETWEEN ADHESIVE SKIN AND DIFFERENT SUBSTRATES Linoleum Foil Cardboard Styrofoam Rubber 0.31±0.05 0.39±0.00 0.64±0.05 0.74±0.07 0.76±0.04 0.40±0.03 0.39±0.02 1.10±0.06 1.06±0.04 0.91±0.06 Upper row: coefficients measured in low-friction sliding direction; lower row: high-friction sliding direction.

(2) where n is the number of features, is a normalized feature value, is corresponding parameter of the regression function, is the intercept term. The original full feature set (composed of single- and crosschannel features) was pruned through experimentation with forward and backward feature selection, resulting in around 200 best features in the end. Training of the regression function is based on a wellknown batch gradient descent algorithm which minimizes in least squares sense the cost function defined as follows:

IV. LEGGED ODOMETER DEVELOPMENT Stride length is defined as the distance from initial contact of one foot to the following initial contact of the same foot, which essentially corresponds to the distance travelled by the robot in one period of locomotion. This section is devoted to the development of a new LO, superior to the one we presented in [19]. The development proceeds in the following steps: (i) data collection, (ii) feature computation, and (iii) training a regression function.

(3)

A. Data Collection We have reused the data set collected in [19]. The robot was left to run with the same gait, but at different frequencies of the periodic motor signal (from 0.25 to 1.75 Hz) on different grounds. The sensory data as well as position and orientation reference from the overhead camera were recorded. The ground truth trajectory with Root Mean Square Error (RMSE) precision of 0.0024m ± 0.0017m was used to compute the stride length reference. The precision was determined statistically as mean difference between the real

where m is the number of training examples, is the vector of features, is the stride length ground truth value for the training example. The update rule for the batch gradient descent is defined as follows (repeated for all i until convergence): (4)

5

friction coefficients associated with the new substrates lie outside of the range that was in the training set (Table I), and, in addition, stride length has a strongly nonlinear relationship with ground friction in our platform (data not shown here).

where is the learning rate determined experimentally to assure convergence after a reasonable number of iterations. This procedure can now be universally applied and only the composition of the training set will determine the final form of the regression function. Our goal was to show that the multimodal odometer can generalize to different speeds and previously unseen grounds. Therefore, we have included only three grounds in the training set: plastic foil, cardboard, and Styrofoam. In addition, one frequency of the motor signal – 1 Hz – was also excluded from the training set. With this setting, we have created the Regression – StyroFoilCard odometer. Later, when assessing the respective influence of different modalities, the same training procedure is applied, but only features from individual modalities are available. For comparison, we have trained another regression function that uses a single feature – the frequency of locomotion. This will allow us to contrast the performance of the sensorybased odometer with a simple estimator of the robot’s stride length at different controller settings: different frequencies.

TABLE II INS-LO OVERALL PERFORMANCE ON DIFFERENT TERRAINS Mean Absolute Error in Stride Position Terrain / Odometer Type Position Length (% traject. (m) ± 1σ length) (m) Foil – 296 gait periods over 2 experiments Frequency only Universal [ICRA 2011] Regression - StyroFoilCard Cardboard – 174 gait periods over 3 experiments Frequency only Universal [ICRA 2011] Regression - StyroFoilCard Styrofoam – 88 gait periods over 5 experiments Frequency only Universal [ICRA 2011] Regression - StyroFoilCard Linoleum – 148 gait periods over 5 experiments Frequency only Universal [ICRA 2011] Regression - StyroFoilCard Rubber – 232 gait periods over 4 experiments Frequency only Universal [ICRA 2011] Regression - StyroFoilCard

V. EXPERIMENTS AND RESULTS We have conducted more than 150 navigation experiments to analyze the performance of the proposed navigation system – IMU aided by different types of legged odometers using the EKF for data fusion and error estimation. First, we will show the performance of the system with the new odometer and compare with aiding by the frequency-only estimator and our solution from [19]. Second, we will test odometers that encompass features from individual modalities only and assess their performance. For each terrain type, a series of navigation experiments with the right-turning bounding gait were evaluated at gait frequency of 1 Hz which was absent from the training set. This subset of all the experiments was used as testing data to compute the statistics presented in Tables II and III. The results are presented in tabular form, starting with the grounds that were in the training set (but not with the robot running at this frequency of the periodic motor signal), followed by the ones that were completely absent from the training set: linoleum and rubber. Due to the limited size of the arena and different turning radii of the robot on different grounds, duration of the experiments had to be adjusted accordingly. Therefore, the total length of the trajectories varied for each case.

0.207±0.013 0.219±0.012 0.104±0.012

0.949 1.001 0.473

0.015 0.014 0.015

0.318±0.015 0.242±0.016 0.126±0.020

2.744 2.091 1.100

0.054 0.038 0.013

0.315±0.016 0.287±0.017 0.180±0.013

6.364 5.811 3.634

0.056 0.048 0.019

0.203±0.015 0.216±0.015 0.097±0.020

3.258 3.515 1.509

0.030 0.033 0.015

0.187±0.019 0.201±0.020 0.149±0.016

1.485 1.631 1.197

0.023 0.022 0.023

When the LO component is switched off (only inertial data are processed), the precision in position degrades rapidly in all experiments: exceeding 0.5m RMSE in position after 15s of dynamic motion and 30m RMSE after 1 minute. If the INS component is switched off, the standalone LO cannot provide 3D trajectory since no attitude angles are available. An example of INS-LO performance is presented in the following figures and is based on the new regression odometer, i.e. the multimodal odometer trained on Styrofoam, foil, and cardboard terrains. The actual run was performed on linoleum using right-turning bounding gait of motor frequency 1 Hz. Fig. 4 shows a 2D projection of the estimated trajectory (INS-LO) compared to the ground truth reference (REF); in this case the robot was running for approx. 30s. The estimated stride length values per gait period estimated by the LO are presented in Fig. 5 (top) and compared to the stride length computed from the reference. The estimated Euler angles are presented in Fig. 5 (bottom); the estimated velocity and position vectors are shown in Fig. 6; for the errors in position estimated by the EKF see Fig. 7. To compare the performance across different settings (aiding type and terrain), the Mean Absolute Error (MAE) in position was computed (see Table II). We define the MAE as absolute error in position at the end of each gait period (due to referential trajectory synchronized with the INS-LO) averaged over all the gait periods over all the testing experiments in the same condition. However, MAE is not a perfect measure, since in dead reckoning the error accumulates with the travelled distance. Therefore, in addition, we express the MAE in position as percentage of

A. Performance Analysis Table II summarizes the overall performance of the proposed navigation system (INS-LO) over all of the 5 surfaces. The results for the newly developed odometer (Regression - StyroFoilCard) are preceded by the “control” experiments with the frequency-only model, and Universal (ICRA2011) odometer – our previous “heuristic” approach based on correlations and a smaller feature set [19]2. We see that the motion estimation system aided by the new odometer significantly outperforms the other solutions and gives good results even on new grounds. Please note that the 2

Please note that the Universal (ICRA2011) odometer was trained on two grounds – Foil and Styrofoam – only.

6

Altitude Error (m)

Pos. Err. East (m)

Pos. Err. North (m)

the total trajectory length. Precision in attitude was not evaluated with respect to ground truth; however, by inspection, the attitude angles were stable. This can be best seen by comparing the attitude at the beginning and end of a trial where the robot had the same posture (in roll and pitch).

0.02 0 -0.02 0

5

10

15

20

25

30

35

40

0

5

10

15

20

25

30

35

40

0

5

10

15

20 Time (s)

25

30

35

40

0.02 0 -0.02

0.01 0 -0.01 -0.02

Fig. 7. Errors in 3D position as estimated by the EKF and corrected in the INS-LO feedback.

B. Modality Analysis After examining the influence of the different system components – INS, LO and their fusion –, in this section, we look more closely at the workings of the LO. By testing three new odometers that rely on features from a single sensory modality (hip joint encoders, knee joint encoders, and pressure sensors), we can assess their respective contributions. They are contrasted with the Regression – StyroFoilCard odometer, which we will denote here as simply “multimodal”. An overview of the results is presented in Table III.

Fig. 4. 2D trajectory plot as estimated by the legged odometer aided inertial navigation system (INS-LO), output of the legged odometer (LO) used for aiding (stride length projected to the direction of motion using the yaw angle from the IMU), and referential ground truth trajectory (REF).

0.1

LO REF INS-LO

0.05

Roll / Pitch (deg)

0

5

10

15

20 Time (s)

25

30

20

Roll Pitch 200 Yaw

10

100

0

0

-10 -20

TABLE III INS-LO PERFORMANCE WITH RESPECT TO DIFFERENT MODALITIES Mean Absolute Error in Stride Position Terrain / Odometer Type Position Length (% traject. (m) ± 1σ length) (m)

35

Yaw (deg)

Stride length (m)

0.2 0.15

Foil – 296 gait periods over 2 experiments Regression – multimodal Regression – knees only Regression – hips only Regression – pressure only Cardboard – 174 gait periods over 3 experiments Regression – multimodal Regression – knees only Regression – hips only Regression – pressure only Styrofoam – 88 gait periods over 5 experiments Regression – multimodal Regression – knees only Regression – hips only Regression – pressure only Linoleum – 148 gait periods over 5 experiments Regression – multimodal Regression – knees only Regression – hips only Regression – pressure only Rubber – 232 gait periods over 4 experiments Regression – multimodal Regression – knees only Regression – hips only Regression – pressure only

-100

0

5

10

15

20 Time (s)

25

30

35

-200 40

Fig. 5. (top) Estimates of the stride length per gait period as computed by the legged odometer (LO, used for aiding), by the legged odometer aided inertial navigation system (INS-LO), and as obtained from the referential ground truth trajectory (REF); (bottom) estimated pitch, roll, and yaw angles corresponding to the motion dynamics. Velocity North Velocity East Velocity Down

Velocity (m/s)

0.5

0

-0.5

0

5

10

15

20

25

30

35

Position North Position East Position Down

0.5 Position (m)

40

0 -0.5 -1 -1.5

0

5

10

15

20 Time (s)

25

30

35

40

0.107±0.013 0.068±0.013 0.168±0.014 0.241±0.013

0.490 0.311 0.772 1.111

0.015 0.017 0.018 0.016

0.136±0.013 0.112±0.011 0.269±0.015 0.191±0.015

1.193 0.976 2.334 1.665

0.015 0.016 0.044 0.026

0.194±0.013 0.166±0.011 0.271±0.017 0.228±0.016

3.936 3.346 5.482 4.611

0.022 0.018 0.046 0.031

0.118±0.011 0.120±0.011 0.149±0.014 0.134±0.012

1.842 1.852 2.439 2.133

0.015 0.015 0.024 0.017

0.114±0.017 0.131±0.018 0.248±0.021 0.153±0.018

0.913 1.060 1.959 1.217

0.021 0.024 0.031 0.017

The results show very good performance of the Regression knees-only which lags only slightly behind the Regression – multimodal odometer. The Regression – pressure-only

Fig. 6. Components of the 3D velocity vector (top) and 3D position vector (bottom) as estimated by the INS-LO.

7

odometer has substantially worse performance and the Regression – hips-only odometer shows the weakest performance. We offer the following explanation: the hip encoders largely follow the trajectory of the hip motors and are thus not very sensitive to the interaction with the ground. Pressure sensors on the robot’s feet reflect the force vectors at the robot’s feet and thus carry information on the strength of each leg’s push-off and may, indirectly, contain information about slip too. However, they are sensitive to the exact way a foot hits the ground and to the ground texture (the cardboard and rubber have ridges). The joint encoders at the passive compliant knees are the most effective sensors: they also sense the push-off that each leg can exert when interacting with the ground, but do not suffer from the drawbacks of the pressure sensors. On grounds with higher friction, during leg retraction, the feet slip less and hence effectively propel the body forward, while bending the passive knee joint at the same time, which can be detected.

legged odometer providing relative position aiding, complemented by nonholonomic velocity constraints. Whereas the former component was a largely standard white-box model, for the latter we have devised a novel data-driven solution (the legged odometer). We argue that this is the best solution in our situation: whereas knowledge regarding the physics of inertial measurements is readily available, odometry in our dynamic legged platform is extremely hard to model analytically. Without using any external reference, our architecture ensures MAE error in position between 1% and 1.5% of the trajectory length, which corresponds to approximately half the body length (0.1 m) after 1-2 minutes of dynamic locomotion. It has to be noted that this performance was on terrains with different friction than those included in the odometer training data set. Moreover, the robot was tested for dynamics of locomotion (given by the gait frequency) different to the dynamics recorded for the odometer training data set. Therefore, we have successfully demonstrated generalization to unknown terrains and speeds. Furthermore, it has to be noted that substantial slippage (accounting for up to 50% difference in speed for the same motor commands on grounds of different friction) – a bottleneck of typical approaches to odometry – is an intrinsic part of the locomotion patterns we studied. An analysis of the contributions of the individual system components to the overall performance was conducted. In addition, the respective influence of individual sensory modalities that comprise the legged odometer was assessed. If different gaits are used, the odometer’s generalization is limited. This is to be attributed to the very different dynamics of the interaction with the ground and hence different sensory stimulation that arises. However, the same methodology can be applied to train odometers for a different gait, as we have shown previously [19]. For a different quadruped platform, the particular odometer (regression function) proposed here would not work. However, what we propose as a general approach is to equip any robot with a rich multimodal sensor set. Then, using a data-driven approach, it is very likely that information about many variables of interest can be delivered. For example, in [40], we have shown how the same sensory set can be employed for successful terrain classification in our platform. This contrasts with traditional approaches that add sensors specifically targeted at measuring desired quantities.

VI. DISCUSSION In the Introduction, we discussed the challenges that dynamic locomotion of a quadruped robot poses to analytical modeling. For these reasons, we decided to adopt a data-driven strategy to legged odometry by exploiting multimodal proprioceptive sensory information. Black-box models often employ complex, nonlinear relationships (e.g., [37]). However, we have taken a different approach. We have used a big number of features coming from individual sensory channels and then their simple combinations (sums or ratios). A linear regression function (even though the dynamic relationships are nonlinear) was then sufficient to obtain a good estimate of the stride length. That is, the robot body was used to preprocess and prestructure the sensory signals (see e.g., [38]). In other words, complex dynamics that presents itself as a difficulty to motion modeling may at the same time prove as a benefit for sensing and can be exploited: sensing occurs through the body dynamics [35]. This is also confirmed by the modality analysis we have performed. For instance, from the single modalities, the potentiometers in the passive compliant knee joints – which are excited by the interaction with the environment – outperform the potentiometers in the motordriven hip joints. A detailed analysis of information flows between motor and sensory channels in our platform is provided in [39].

ACKNOWLEDGMENT The authors would like to thank F. Iida for the development of the robot, H. Marques for the IMU, D. Brown for the Tracker free software, R. Pfeifer for continuous support, F. Iida and D. Scaramuzza for reviewing an earlier version of the manuscript, and the anonymous reviewers for their constructive suggestions that greatly improved the manuscript.

VII. CONCLUSION We have developed a navigation algorithm for full body state (position, velocity, and attitude) estimation for a quadruped robot. Our method rests on a unique exploitation of a multimodal proprioceptive sensory set of our legged robot. We have split the sensory modalities into two parts: inertial measurements on one side, and joint and pressure information from legs on the other. The extended Kalman filter was used to provide error estimation and data fusion from two independent sources of information: the strapdown mechanization algorithm processing raw inertial data and

REFERENCES [1]

8

S. Se, D. Lowe and J. Little, “Vision-based global localization and mapping for mobile robots,” IEEE Transactions on Robotics, vol. 21, no. 3, pp. 364-375, 2005.

[2]

[3]

[4] [5]

[6] [7]

[8]

[9] [10]

[11]

[12]

[13]

[14]

[15]

[16]

[17]

[18]

[19]

[20] [21]

[22]

[23] [24]

[25]

L. Moreno, J. Armingol, S. Garrido, A. Escalera and M. Salichs, “A genetic algorithm for mobile robot localization using ultrasonic sensors,” J. Intell. Robot. Syst., vol. 34, no. 2, pp. 135-154, 2002. P. Goel, S. I. Roumeliotis and G. S. Sukhatme, “Robust localization using relative and absolute position estimate,” in IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS), 1999. A. Etienne and K. Jeffery, “Path integration in mammals,” Hippocampus, vol. 14, pp. 180-92, 2004. F. H. Durgin, M. Akagi, C. R. Gallistel and W. Haiken, “The precision of locometry in humans,” Exp Brain Res, vol. 193, pp. 429436, 2009. M. Wittlinger, R. Wehner and H. Wolf, “The ant odometer: stepping on stilts and stumps,” Science, vol. 312, 2006. G. Dissanayake, S. Sukkarieh, E. Nebot and H. Durrant-Whyte, “The Aiding of a Low-Cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications,” IEEE Transactions on Robotics and Automation, vol. 17, no. 5, pp. 731 747, 2001. J. Yi, H. Wang, J. Zhang, D. Song, S. Jayasuriya and J. Liu, “Kinematic modeling and analysis of skid-steered mobile robots with applications to low-cost inertial measurement-unit-based motion estimation,” IEEE Transactions on Robotics, vol. 25, no. 5, 2009. H. Rehbinder and H. Xiaoming, “Drift-free attitude estimation for accelerated rigid bodies,” Automatica, vol. 40, 2004. J. A. Cobano, J. Estremera and P. Gonzalez de Santos, “Location of legged robots in outdoor environments,” Robotics and Autonomous Systems, vol. 56, pp. 751–761, 2008. G. P. Roston and E. P. Krotkov, “Dead reckoning navigation for walking robots,” in IEEE/RSJ Int. Conf on Intelligent Robots and Systems (IROS), 1992. O. Gur and U. Saranli, “Model-based proprioceptive state estimation for spring-mass running,” in Robotics: Science and Systems (RSS VII), 2011. S.-H. Liu, T.-S. Huang and J.-Y. Yen, “Comparison of sensor fusion methods for an SMA-based hexapod biomimetic robot,” Robotics and Autonomous Systems, vol. 58, no. 5, pp. 737-744, 2010. A. Chilian, H. Hirschmuller and M. Gorner, “Multisensor data fusion for robust pose estimation of a sixlegged walking robot,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2497–2504, 2011. P.-C. Lin, H. Komsuoglu and D. E. Koditschek, “A Leg Configuration Measurement System for Full Body Posture Estimates in a Hexapod Robot,” IEEE Transactions on Robotics, vol. 21, no. 3, pp. 411-422, 2005. P.-C. Lin, H. Komsuoglu and D. E. Koditschek, “Sensor data fusion for body state estimation in a hexapod robot with dynamical gaits,” IEEE Transactions on Robotics, vol. 22, no. 5, pp. 932-943, 2006. M. Bloesch, M. Hutter, M. Hoepflinger, S. Leutenegger, C. Gehring, D. Remy and R. Siegwart, “State estimation for legged robots consistent fusion of leg kinematics and IMU,” in Robotics: Science and Systems (RSS VIII), 2012. M. Maimone, Y. Cheng and L. Matthies, “Two years of visual odometry on the mars exploration rovers: Field reports,” Journal of Field Robotics, vol. 24, no. 3, pp. 169-186, 2007. M. Reinstein and M. Hoffmann, “Dead reckoning in a dynamic quadruped robot: inertial navigation system aided by a legged odometer,” in Proc. IEEE Int. Conf. Robotics and Autonomation (ICRA), pp. 617-624, 2011. D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, Lavenham, UK: The Lavenham Press ltd, 1997. P. G. Savage, “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms,” Journal of Guidance, Control, and Dynamics, vol. 21, no. 1, pp. 19 - 28, 1998. P. G. Savage, “Strapdown Inertial Navigation Integration Algorithm Design Part 2: Velocity and Position Algorithms,” Journal of Guidance, Control, and Dynamics, vol. 21, no. 2, pp. 208 - 221, 1998. O. Salychev, Applied Inertial Navigation: Problems and Solutions, Bauman MSTU Press, 2004. E.-H. Shin, “Estimation Techniques for Low-Cost Inertial Navigation”, Ph. D. dissertation, Dept. of Geomatics Engineering, University of Calgary, 2005. M. Sipos, P. Paces, M. Reinstein and J. Rohac, “Flight Attitude Track Reconstruction Using Two AHRS Units under Laboratory Conditions,” in IEEE Sensors, pp. 675-678, 2009.

[26] M. Reinstein, J. Rohac and M. Sipos, “Algorithms for Heading Determination using Inertial Sensors,” Przeglad Elektrotechniczny, vol. 86, no. 9, pp. 243-246, 2010. [27] R. E. Kalman, “A new approach to linear filtering and prediction problems,” ASME Journal of Basic Engineering, vol. 82, pp. 34-45, 1960. [28] W. Abdel-Hamid, “Accuracy Enhancement of Integrated MEMSIMU/GPS Systems for Land Vehicular Navigation Applications”, Ph.D. dissertation, Dept. of Geomatics Engineering, University of Calgary, 2005. [29] M. S. Grewal and A. P. Andrews, Kalman Filtering - Theory and Practice using Matlab, New York: Wiley-Interscience, 2001. [30] E.-H. Shin, “Accuracy Improvement of Low Cost INS/GPS for Land Applications”, M.S. thesis, Dept. of Geomatics Engineering, University of Calgary, 2001. [31] N. Vikas Kumar, “Integration of Inertial Navigation System and Global Positioning System Using Kalman Filtering”, M.S. thesis, Indian Institute of Technology, Dept. of Aerospace Engineering, 2004. [32] M. Reinstein, M. Sipos and J. Rohac, “Error Analyses of Attitude and Heading Reference Systems,” Przeglad Elektrotechniczny, vol. 85, no. 8, pp. 115-118, 2009. [33] S. B. Lazarus, I. Ashokaraj, A. Tsourdos, R. Zbikowski, P. Silson, N. Aouf and B. A. White, “Vehicle Localization Using Sensors Data Fusion Via Integration of Covariance Intersection and Interval Analysis,” IEEE Sensors Journal, vol. 2, no. 2, pp. 107-119, 2007. [34] M. Reinstein, “Evaluation of Fine Alignment Algorithm for Inertial Navigation,” Przeglad Elektrotechniczny, vol. 87, no. 7, pp. 255-258, 2011. [35] F. Iida and R. Pfeifer, “Sensing through body dynamics,” Robotics and Autonomous Systems, vol. 54, no. 8, pp. 631-640, 2006. [36] F. Iida, G. Gomez and R. Pfeifer, “Exploiting body dynamics for controlling a running quadruped robot,” in Int. Conf. on Advanced Robotics (ICAR), 2005. [37] U. Forssell and P. Lindskog, “Combining Semi-Physical and Neural Network Modeling: An Example of,” in 11th IFAC Symposium on System Identification (SYSID'97), 1997. [38] M. Lungarella and O. Sporns, “Mapping information flow in sensorimotor networks,” PLoS Comput Biol, pp. 1301-12, 2006. [39] N. Schmidt, M. Hoffmann, K. Nakajima and R. Pfeifer, “Bootstrapping perception using information theory: Case studies in a quadruped robot running on different grounds,” Advances in Complex Systems J., vol. 16, 2012. [40] M. Hoffmann, N. Schmidt, R. Pfeifer, A. Engel and A. Maye, “Using sensorimotor contingencies for terrain discrimination and adaptive walking behavior in the quadruped robot Puppy,” in From animals to animats 12: Proc. Int. Conf. Simulation of Adaptive Behaviour (SAB), 2012.

9