Fig. 3. Comparisonbetween measured and predictedtractive force usingthe New- ton Raphson methodand the prediction error force predicted fromthe identified lumped soil parameter, ( Ac + Wtanφ)and K .The prediction errorranges from -0.7% to 1%. Thisreflects avery good prediction accuracy of the tractiveforce. Thus theidentified soil parameters can be used for UGV traversabilityprediction and trajectoryplanning in real time based on accuratepredicted tractiveforce. Thisisbeneficial for autonomy purposes of UGVs. 6Conclusion andFutureWork The multi-solution problemofthe track-terrain interactiondynamicsmodel is acknowledgedbythe random test. The investigation andanalysis show thatthis problem originates fromthe term ( Ac + Wtanφ)inwhich c and φ compensate eachother in anumberofways(multi-solutions) to make the same value of ( Ac+ Wtanφ). This occurrenceinitiates the idea to treatthistermas asingle soil parameter called“Lumped soilparameter” to solve multi-solution problem. The Newton Raphson methodisapplied as soil parameter identification technique forthe modified track-terrain interaction dynamics model to iden- tify lumped soil parameter, ( Ac + Wtanφ)and sheardeformationmodulus, K .The Newton Raphsonmethodisshown to be excellentinall aspects in- cluding parameteridentification accuracy,robustnesstoawiderange of initial conditions, robustness to noise, and computational speed. 526 S. Hutangkabodee et al. The future work will focus on the soil parameter identification of a tracked UGV traversing different terrain categories illustrated in appendix A. The hy- brid among different track-terrain interaction dynamics models will be carried out to benefit the soil parameter identification in any terrain. Also, research on traversability prediction based on the use of the identified soil parameters will be carried out. 7Acknowledgement The authorsthank J.Y. Wong for providing useful experimental information. Also,the authors would liketoacknowledge EPSRC (GR/S31402/01), Minis- tryofDefense (MoD),QinetiQ Ltd. and DSTL for fundingthis project. References 1. ZweiriYH, Seneviratne LD, Althoefer K(2003) JournalofSystems and Control Engineering 217:259–274 2. BekkerG(1956) Theory of Land Locomotion. UniversityofMichigan Press 3. Bekker G(1969) Introduction of Terrain-VehicleSystems. UniversityofMichi- gan Press 4. Wong JY (2001) Theory of Ground Vehicles (3 rd Edition). JohnWiley &Sons, USA 5. Wong JY (1989) Terramechanics andOff-Road Vehicles.Springer,Elsevier Sci- ence Publishers B.V. ,Netherlands 6. Yoshida K, HamanoH(2002)IEEE International Conference on Roboticsand Automation 3:3155–3160 7. Le AT,Rye DC, Durrant-Whyte HF (1997)IEEE International Conference on Roboticsand Automation 2:1388–1393 8. Zweiri YH, Seneviratne LD,Althoefer K(2004) IEEE TransactionsonRobotics 20:762–767 9. TanC,Zweiri YH, Seneviratne LD, Althoefer K(2003) IEEE International ConferenceonRobotics and Automation 1:121–126 10. IagnemmaK,Golda D, SpenkoM,Dubowsky S(2004) IEEE Transactions on Robotics 20:5:921–927 11. IagnemmaK,Dubowsky S(2002) SPIE Conference on Unmanned Ground Ve- hicleTechnologyIV4715:256–266 12. Song Z, Hutangkabodee S, Zweiri YH, SeneviratneLD, Althoefer K(2004) SICE Annual Conference 2255–2260 13. Hutangkabodee S, Zweiri YH,SeneviratneLD, Althoefer K(2004) MECHROB Conference 3:889–895 Multi-solution Problem for Track-Terrain Interaction Dynamics 527 Appendix A Shear-based track-terrain interaction dynamicsmodels fordifferent categories of terrains(from the one used in this paper) are described below. A.1 Organic terrain (muskeg) withamat of livingvegetation on thesurface and saturatedpeat beneath it The shear stress -sheardisplacement relationship forthis type of terrain exhibits characteristicsshown in Fig. 4(a) and itsshearing behaviorcan be described by τ = τ max ( j/K ω )e (1− j/K ω ) , (11) where K ω is the shear displacementwhere τ max occurs. A.2 Compact sand, silt and loam, and frozen snow The shear stress -sheardisplacement relationship forthis type of terrain exhibits characteristicsshown in Fig. 4(b) and its shearing behavior can be described by τ = τ max K r 1+[1 / ( K r (1 − 1 / e)) − 1] e (1− j/K ω ) 1 − e ( − j/K ω ) , (12) where K r is the ratio of the residualshear stress τ r to the maximum shear stress τ max ,and K is the shear displacementwhere τ max occurs. (a) (b) Fig. 4. (a) and (b) showplotsofshear stressagainst shear displacementfor a trackedvehicletravellingonorganicterrain (muskeg) andoncompact sand, siltand loam,and frozen snow, respectively[4] 528S. Hutangkabodee et al. 3D Position TrackinginChallenging Terrain Pierre Lamonand Roland Siegwart Ecole PolytechniqueF´e d´erale de Lausanne { firstname.lastname} @epfl.ch Summary. The intentofthis paper is to showhow the accuracy of 3D position tracking can be improvedbyconsidering roverlocomotion in rough terrainasa holistic problem.Anappropriate locomotion concept endowedwith acontroller min- imizing slipi mpro ve st he climb ing pe rformance, thea ccuracy of od ometry and the signal/noise ratioofthe onboard sensors. Sensor fusion involving an inertial mea- surementunit, 3D-Odometry, and visual motion estimation is presented. The exper- imen talr esults sho wc learly ho we ac hs ensorc on tributes to increaset he accuracy of the 3D pose estimationinrough terrain. 1Introduction In order to acquire knowledge about the environment, amobile robot uses differen tt yp es of sensors,w hic ha re error prone and whose measurement s are uncertain. In office-likeenvironments, the interpretationofthis data is facilitated thanks to the numerous assumptions that can be formulated e.g. the soil is flat, the wa lls are pe rp endiculart ot he ground, etc. In natural scenes, the problem is much more tedious because of limited apriori knowledgeabout the environmentand the difficultyofperception. In rough terrain, the change in ligh ting conditionsc an strongly affectt he qualit yo ft he acquired images and the vibrations due to uneven soils lead to noisy sensor signals. When the robot is overcoming an obstacle, the field of view can change significantly be tw een tw od ata acquisitions, increasing thed ifficult yo ft rac king featuresi n the scene. To get arobust estimate of therobots position, the measurements acquired by several complementary sensors have to be fused accounting for their relative variance.Inthe literature, the localization task generally involves two types of sensorsand is divided into twophases a) the first step consists in the inte- gration of ahigh frequency deadreckoning sensor to predict vehicle location b) the second phase, whic hi su sually activ ated at am uc hs lo we rr ate,u ses an absolute sensing mechanism for extracting relevantfeatures in the envi- ronmentand updating the predicted position. In [1], an inertial measurement P. Corke and S. Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp. 529–540, 2006. © Springer-Verlag Berlin Heidelberg 2006 530 P. Lamon and R. Siegwart unit is used for the prediction and an omnicam is used as the exteroceptive sensor. The pair of sensors composed of an inertial measurement unit and a GPS is used in [2]. Even if sensor fusion can be applied to combine the mea- surements acquired by any number of sensors, most of the applications found in the literature generally use only two types of sensors and only the 2D case is considered (even for terrestrial rovers). In challenging environments, the six degrees of freedom of the rover have to be estimated (3D case) and the selection of sensors must be done care- fully because of the aformentioned difficulties of perception in rough terrain. However, the accuracy of the position estimates does not only depend on the quality and quantity of sensors mounted onboard but also on the specific lo- comotion characteristics of the rover and the way it is driven. Indeed, the sensor signals might not be usable if an unadapted chassis and controller are used in challenging terrain. For example, the ratio signal/noise is poor for an inertial measurement unit mounted on a four-wheel drive rover with stiff suspensions. Furthermore, odometry provides bad estimates if the controller does not include wheel-slip minimization or if the kinematics of the rover is not accounted for. The intent of this paper is to show how the accuracy of 3D position tracking can be improved by considering rover locomotion in rough terrain as a holistic problem. Section 2 describes the robotic platform developed for conducting this research. In Sect. 3, a method for computing 3D motion increments based on the wheel encoders and state sensors is presented. Because it accounts for the kinematics of the rover, this method provides better results than the standard method. Section 4 proposes a new approach for slip-minimization in rough terrain. Using this controller, both the climbing performance of the rover and the accuracy of the odometry are improved. Section 5 presents the results of the sensor fusion using 3D-Odometry, an Inertial Measurement Unit (IMU) and Visual Motion Estimation based on stereovision (VME). The experiments show clearly how each sensor contributes to increase the accuracy and robustness of the 3D pose estimation. Finally, Sect. 6 concludes this paper. 2 Research Platform The Autonomous System Lab (at EPFL) developed a six-wheeled off-road rover called Shrimp, which shows excellent climbing capabilities thanks its passive mechanical structure [3]. The most recent prototype, called SOLERO, has been equipped with sensors and more computational power (see Fig. 1). The parallel architecture of the bogies and the spring suspended fork provide a high ground clearance while keeping all six motorized wheels in ground-contact at any time. This ensures excellent climbing capabilities over obstacles up to two times the wheel diameter and an excellent adaptation to all kinds of ter- rains. The ability to move smoothly across rough terrain has many advantages when dealing with onboard sensors: for example, it allows limited wheel slip 3D Position Tracking in Challenging Terrain531 and reduces vibration. The quality of the odometric information and the ratio signal/noise for the inertial sensors are significantly improved in comparison with rigid structures such as four-wheel drive rovers. Thus, both odometry and INS integration techniques can be accounted for position estimation. a e d b c f j g h k i Front Fig. 1: Sensors, actuators and electronics of SOLERO. a) steering servo mechanism b) passively articulated bogie and spring suspended front fork (equipped with an- gular sensors) c) 6 motorized wheels (DC motors) d) omnidirectional vision system e) stereo-vision module, orientable around the tilt axis f) laptop (used for image processing) g) low power pc104 (used for sensor fusion) h) energy management board i) batteries (NiMh 7000 mAh) j) I 2 C slave modules (motor controllers, angu- lar sensor module, servo controllers etc.) k) IMU (provides also roll and pitch) 33 D-Odometry Odometry is widelyused to trackthe position and the orientation ([x, y, ψ ] T ) of ar ob ot in ap lane π .T his ve ctor is up dated by in tegrating small motion increments between twosubsequentrobot poses. This 2D odometry method can be extended in order to accountfor slopechanges in the environmentand to estimate the 3D po sition in ag lobalc oo rdinate system i.e. [ x, y, z, φ, θ, ψ ] T . This technique uses typically an inclinometer for estimating the roll ( φ )and pitch(θ )angles relativetothe gravityfield [4]. Thus, the orientation of the plane π ,onwhichthe robot is currently moving, can be estimated. The, z coordinate is computed by projecting the robot displacements in π into the global coordinate system.This method, whichwill be referred later as the standard method ,workswell under the assumption that the ground is relatively smo oth and do es not ha ve to om an ys lop ed iscont in uities. Indeed, the system accumulateserrors during transitionsbecause of the planar assumption. In 532 P. Lamon and R. Siegwart rough terrain, this assumption is not verified and the transitions problem must be addressed properly. This section briefly describes a new method, called 3D-Odometry, which takes the kinematics of the robot into account and treats the slope discontinuity problem. The main reference frames and some of the variables used for 3D-Odometry are introduced in Fig. 2 Z w X w Z r r X Y w Z r Z w Y r L F O F R L O Δ η OX w Y w Z w global reference frame L projection of O in the bogie plane OX r Y r Z r robots frame Δ, η norm/angle of L ’s displacement Fig. 2: Reference frames definition The norm Δ and the direction of motion η of each bogie can be computed by considering the kinematics of the bogie, the incremental displacement of the Rear/Front bogie wheels (wheel encoders) and the angular change of the bogie (angular sensor) between two data acquisition cycles. Then, the displacement of the robot’s center O , i.e. [ x, y, z,ψ ] T , can be computed using Δ and η of the left and the right bogie, whereas the attitude [ φ , θ ] T is directly given by the inclinometer 1 . Experimental results The robot has been driven across obstacles of known shape and the trajectory computed online with both 3D-Odometry and the standard method. In all the experiments, the 3D-Odometry produced much better results than the stan- dard method because the approach accounts for the kinematics of the rover. The difference between the two techniques becomes bigger as the difficulty of the obstacles increases (see Fig. 3). In Fig. 4, an experiment testing the full 3D capability of the method is depicted. The position error at the goal is only x = 1 . 4%, y = 2%, z = 2 . 8%, ψ = 4% for a total path length of around 2 m . SOLERO has a non-hyperstatic mechanical structure that yields a smooth trajectory in rough terrain. As a consequence wheel slip is intrinsically mini- mized. When combined with 3D-Odometry, such a design allows to use odom- 1 The reader can refer to the originalpaper [5] for moredetails about3D-Odometry. In particular, the methodalso computes the wheel-ground contact angles. 3D Position Tracking in Challenging Terrain533 etry as a mean to track the rover’s position in rough terrain. Moreover, the quality of odometry can still be significantly improved using a ”smart” con- troller minimizing wheel slip. Its description is presented in the next section. Fig. 3: Sharp edges experiment (b) (a) Only the right bogie wheels climbed obstacle (a). Then, the rover has been driven over obstacle (b) (with an incident angle of approximatively 20 ◦ ) Fig. 4: Full 3D experiment 4 Wheel Slip Minimization For wheeled rovers, the motion optimization is somewhat related to mini- mizing wheel slip. Minimizing slip not only limits odometric error but also increases the robot’s climbing performance and efficiency. In order to fulfill this goal, several methods have been developed. Methods derived from the Anti-lock Breaking System can be used for rough terrain rovers. Because they adapt the wheel speeds when slip already occurred, they are referred to as reactive approaches. A velocity synchroniza- tion algorithm, which minimizes the effect of the wheels fighting each other, has been implemented on the NASA FIDO rover [6]. The first step of the method consists in detecting which of the wheels are deviating significantly from the nominal velocity profile. Then a voting scheme is used to compute the required velocity set point change for each individual wheel. However, per- formance might be improved by considering the physical model of the rover and wheel-soil interaction models for a specific type of soil. Thus, the traction of each wheel is optimized considering the load distribution on the wheels and the soil properties. Such approaches are referred to as predictive approaches. In [7], wheel-slip limitation is obtained by minimizing the ratio T/N for each wheel, where T is the traction force and N the normal force. Reference [8] proposes a method minimizing slip ratios and thus avoid soil failure due to ex- cessive traction. These physics-based controllers assume that the parameters of the wheel-ground interaction models are known. However, these parameters are difficult to estimate and are valid only for a specific type of soil and condi- tion. Reference [9] proposes a method for estimating the soil parameters as the 534 P. Lamon and R. Siegwart robot moves, but it is limited to a rigid wheel travelling through deformable terrain. In practice, the rover wheels are subject to roll on different kind of soils, whose parameters can change quickly. Thus, physics-based controllers are sensitive to soil parameters variation and difficult to implement on real rovers. In this section, a predictive approach considering the load distribu- tion on the wheels and which does not require complex wheel-soil interaction models is presented. More details about the controller can be found in [10]. Quasi-static model The speed of an autonomous rover is limited in rough terrain because the nav- igation algorithms are computationally expensive (limited processing power) and for safety reasons. In this range of speeds, typically smaller than 20cm/s, the dynamic forces might be neglected and a quasi-static model is appropri- ate. To develop such a model, the mobility analysis of the rover’s mechanical structure has to be done. It ensures to produce a consistent physical model with the appropriate degrees of freedom at each joints. Then the forces are introduced and the equilibrium equations are written for each part composing the rover’s chassis. Because we have no interest in implicitly calculating the internal forces of the system, it is possible to reduce this set of independent equations. The variables of interest are the 3 ground contact forces on the front and the back wheel, the 2 ground contact forces on each wheel of the bogies and the 6 wheel torques. This makes 20 unknowns of interest and the system can be reduced to 15 equations. This leads to the following equation M 15x 20 · U 20x 1 = R 15x 1 (1) where M is the model matrix depending on the geometric parameters and the state of the robot, U a vector containing the unknowns and R a constant vector. It is interesting to note that there are more unknowns than equations in 1. That means that there is an infinite set of wheel-torques guaranteeing the static equilibrium. This characteristic is used to control the traction of each wheel and select, among all the possibilities, the set of torques minimizing slip. The optimal torques are selected by minimizing the function f = max( i T i /N i ) i =1 6(2) where T i and N i are thetractionand the normalforceapplied to wheel i . Rover motion Astatic model balances the forcesand momentsonasystem to remain at rest or maintain aconstantspeed. Suchasystem is an ideal case and does not include resistance to movement. Therefore,anadditional torque compensating the rolling resistance torque mu st be added on thew heels in order to complete 3D Position Tracking in Challenging Terrain535 the model and guarantee motion at constant speed. This results in a quasi- static model. Unlike the other approaches, we don’t use complex wheel-soils interaction models. Instead, we introduce a global speed control loop, in order to estimate the rolling resistance as the robot moves. The final controller, minimizing wheel slip and including rolling resistance, is depicted in Fig. 5. M r M w PID d V + − M c r V Robot Model & s Optimization N Distribution Correction + + o M V d desired rover velocity M o vector of optimal torques V r measured rover velocity N vector of normal forces M r rolling resistance torque s rover state M c correction torque M w vector of wheel correction torques Fig. 5: Rover motion control loop. The kernel of the control loop is a PID controller. It allows to estimate the additional torque to apply to each wheel in order to reach the desired rover’s velocity V d and thus, minimizes the error V d − V r . M c is actually an estimate of the global rolling resistance torque M r , which is considered as a perturbation by the PID controller. The rejection of the perturbation is guaranteed by the integral term I of the PID. We assume that the rolling resistance is proportional to the normal force, thus the individual corrections for the wheels are calculated by M w i = N i N m · M c (3) where N i is the normalforceonwheel i and N m the average of all the normal forces. The derivativeterm D of thePID allowstoaccountfor non modeled dynamiceffects and helps to stabilizethe system. The parameters estimation for the controller is not critical because we are more interested in minimizing slip than in reaching thedesired velocityvery precisely. For locomotion in rough terrain, aresidual error on the velocitycan be accepted as longasslip is minimized. Experimentalresults Asimulationphase using Open Dynamics Engine 2 has been initiated in order to test the approac ha nd ve rify the theoreticalc onceptsa nd assumptions. The 2 this librarysimulates rigid bodydynamicsinthreedimensions, including ad- vanced joint typesand collision detection with friction. [...]... terrain such approach will P Corke and S Sukkarieh (Eds.): Field and Service Robotics, STAR 25, pp 541–552, 2006 © Springer-Verlag Berlin Heidelberg 2006 542 M Pivtoraiko, A Kelly, and P Rander either result in slow, inefficient traversal, or may cause a failure of the path planner to generate an admissible path 1.1 Prior Work Great overviews of automobile off-road mobility and approaches to soil modeling... terrestrial soil types and in a wide range of natural landscapes and vehicle velocities This model was developed empirically, it is simple yet accurate and can be readily used to improve model-predictive planning and control The model encapsulates the specifics of wheel-terrain interaction, offers a good compromise between accuracy and real-time computational efficiency, and allows straight-forward consideration... Sendai, Japan 12 Jung I-K, Lacroix S (2003) Simultaneous Localization and Mapping with Stereovision, International Symposium on Robotics Research, Siena Efficient Braking Model for Off-Road Mobile Robots Mihail Pivtoraiko, Alonzo Kelly, and Peter Rander Robotics Institute, Carnegie Mellon University mihail@cs.cmu.edu, alonzo@ri.cmu.edu, rander@rec.ri.cmu.edu Summary In the near future, off-road mobile robots... functions and integration, International Journal of Robotics Research 5 Lamon P, Siegwart R (2003) 3D-Odometry for rough terrain - Towards real 3D navigation, IEEE International Conference on Robotics and Automation, Taipei, Taiwan 6 Baumgartner E.T, Aghazarian H, Trebi-Ollennu A, Huntsberger T.L, Garrett M.S (2000) State Estimation and Vehicle Localization for the FIDO Rover, Sensor Fusion and Decentralized... Conference on Robotics and Automation, Washington D.C, USA 10 Lamon P, Siegwart R (2005) Wheel torque control in rough terrain - modeling and simulation, IEEE International Conference on Robotics and Automation, Barcelona, Spain, in press 11 Lamon P, Siegwart R (2004) Inertial and 3D-odometry fusion in rough terrain Towards real 3D navigation, IEEE/RSJ International Conference on Intelligent Robots and Systems,... on-line and adapted as the robot moves into different type of terrain This formulation of the model was shown to work well on off-road robots operating on a wide variety of terrain types, such as clay, soil with sod cover, gravel, coarse sands, and packed snow, as well as at various speeds and on natural slopes (typical to mid-West region, the plains and the desert) This model can be used in model-predictive... through “smarter” operation during both dig and swing by eliminating overloads and collisions with crawlers and trucks Robotic excavation has been investigated by a number of authors on different machine types but focused around digging, weight estimation and motion planning Singh[11] provides a good review of the field and discusses state-of-the-art in sensing and machine/ground interaction models He... 3DOdometry only, whereas the second part involves all the three sensors i.e 3D-Odometry, inertial sensor and VME Inertial and 3D-Odometry: The experimental results show that the inertial navigation system helps to correct odometric errors and significantly improves the pose estimate The main contributions occur locally when the robot overcomes sharp-shaped obstacles (Fig 9) and during asymmetric wheel slip... Advanced Robotics, Portugal 2 Nebot E, Sukkarieh S, Durrant-Whyte H (1997) Inertial navigation aided with GPS information, In the proceedings of the Fourth Annual Conference of Mechatronics and Machine Vision in Practice 3 Siegwart R, Lamon P, Estier T, Lauria M, Piguet R (2000) Innovative design for wheeled locomotion in rough terrain, Journal of Robotics and Autonomous Systems, Elsevier, vol 40/ 2-3 p15 1-1 62... significant role in coal uncovery and production, but are not achieving optimum performance Problems include operator variability, truck positioning and timing, and sub-optimal digging paths which can result in either partly filled buckets1 or a time-consuming stall of the machine Automation offers the potential to factor out this variability and to provide consistent and optimised performance Further . applications. For example, [7] and [11] present approaches that model the soil as a mass-spring system. These models provide fairly good results in describing compression, shear and plastic deformations. ahead,given its slope and ground characteristics, presentsrisks suchastip-over, and provideapreciseestimate of the stoppingdistance. Precisionofthe model is very important,but it shouldalso be very. K ω is the shear displacementwhere τ max occurs. A.2 Compact sand, silt and loam, and frozen snow The shear stress -sheardisplacement relationship forthis type of terrain exhibits characteristicsshown