- Email: [email protected]
A ZMP based interval criterion for rollover-risk diagnosis
9th IFAC Symposium on Fault Detection, Supervision and 9th IFAC Symposium on 9th IFAC on Fault Fault Detection, Detection, Supervision Supervision and and Safety of Symposium Technical Processes 9th IFAC on Fault Detection, Supervision and Safety of Symposium Technical Processes Available online at www.sciencedirect.com September 2-4, 2015. Arts et Métiers ParisTech, Paris, France Safety of Technical Processes Safety of Technical Processes September 2-4, 2015. Arts et Métiers ParisTech, Paris, France September September 2-4, 2-4, 2015. 2015. Arts Arts et et Métiers Métiers ParisTech, ParisTech, Paris, Paris, France France
ScienceDirect
A A A A Xu Xu Xu Xu
IFAC-PapersOnLine 48-21 (2015) 277–282
ZMP based interval criterion ZMP based interval criterion ZMP based interval criterion ZMP based interval criterion rollover-risk diagnosis rollover-risk diagnosis rollover-risk diagnosis rollover-risk diagnosis
for for for for
HAN HAN HAN HAN
Joanny STEPHANT Gilles MOURIOUX Joanny STEPHANT Gilles MOURIOUX Joanny STEPHANT Gilles MOURIOUX Dominique MEIZEL Joanny STEPHANT Gilles MOURIOUX Dominique MEIZEL Dominique MEIZEL Dominique MEIZEL XLIM, XLIM, UMR UMR CNRS/ CNRS/ Limoges Limoges University University #7252, #7252, 16 16 rue rue Atlantis, Atlantis, XLIM, UMR CNRS/ Limoges University #7252, 16 rue 87068 Limoges, France (e-mail: [email protected]). XLIM, UMR CNRS/France Limoges University #7252, 16 rue Atlantis, Atlantis, 87068 Limoges, (e-mail: [email protected]). 87068 Limoges, France (e-mail: [email protected]). 87068 Limoges, France (e-mail: [email protected]). Abstract: This paper presents an interval criterion used to anticipate rollover accidents of Abstract: This This paper paper presents presents an an interval interval criterion criterion used used to to anticipate anticipate rollover rollover accidents accidents of of Abstract: all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid Abstract: This paper presents an interval criterion used to anticipate rollover accidents of all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid robots, is presented the case of 4 wheels vehicle, first in an ideal case of aa perfectly ground all-terrain wheeled in vehicles. Moment Point initially forflat humanoid robots, is is presented presented in the case caseThe of aa aZero 4 wheels wheels vehicle, firstconcept, in an an ideal ideal case of ofdesigned perfectly flat ground robots, in the of 4 vehicle, first in case a perfectly flat ground and finally in the off-road context. This adaptation is done by assuming that the tyre/ground robots, is presented in the case of a 4 wheels vehicle, first in an ideal case of a perfectly flat ground and finally finally in in the the off-road off-road context. context. This This adaptation adaptation is is done done by by assuming assuming that that the the tyre/ground tyre/ground and contact is no more punctual but distributed in set enclosed in computable interval). and finally the off-road This adaptation is done by thatbox the(3D tyre/ground contact is no noinmore more punctualcontext. but distributed distributed in aaa set set enclosed enclosed in aaaassuming computable box (3D interval). contact is punctual but in in computable box (3D interval). It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is contact is no more punctual but distributed in a set enclosed in a computable box (3D interval). It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is experimented by using data collected during a real experiment done with a vineyard harvester It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is experimented by using data collected during a real experiment done with a vineyard harvester experimented by using data collected during experiment done with harvester that is typical wheeled reconfigurable vehicle. The interval gives flexibility experimented by case usingof collected during aa real real experiment done criterion with aa vineyard vineyard harvester that is is aaa typical typical case ofdata wheeled reconfigurable vehicle. The interval interval criterion gives aaa flexibility flexibility that case of wheeled reconfigurable vehicle. The criterion gives for its interpretation that is more adapted to the complex and varying all-terrain context. that is interpretation a typical case that of wheeled reconfigurable vehicle. Theand interval criterion givescontext. a flexibility for its is more adapted to the complex varying all-terrain for its that is to complex and all-terrain context. for its interpretation interpretation that Federation is more more adapted adapted to the theControl) complex and varying varying all-terrain context. © 2015, IFAC (International of Automatic Hosting by Elsevier Ltd. All rights reserved. Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Assistance Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Assistance Assistance Assistance 1. INTRODUCTION 1. INTRODUCTION INTRODUCTION 1. 1. INTRODUCTION The ultimate motivation of the study is societal: it aims to The ultimate ultimate motivation motivation of of the the study study is is societal: societal: it it aims aims to to The reduce the number of fatal accidents caused by the reversal The ultimate motivation of the study is societal: it aims to reduce the number of fatal accidents caused by the reversal reduce the number of fatal fatal accidents accidents caused by the theas reversal of heavy vehicles operating in an off-road context it was was reduce the number of caused by reversal of heavy heavy vehicles operating in an an off-road off-road context as it of vehicles operating in context as it was reported in CCMSA (2010) for agriculture. There is a need of heavy vehicles operating off-road context was reported in CCMSA CCMSA (2010) in foran agriculture. There as is aaitneed need reported in (2010) for agriculture. There is for an Advanced Driver Assistant System (ADAS) that reported in CCMSA (2010) for agriculture. There is a need for an Advanced Driver Assistant System (ADAS) that for andiagnose Advanced Driver Assistant System (ADAS) that could the rollover risk and warn the driver to take for an Advanced System that could diagnose theDriver rolloverAssistant risk and and warn warn the (ADAS) driver to to take take could diagnose the rollover risk the driver a corrective action or even alters its command when the could diagnose the rollover risk and warn the driver to take a corrective corrective action action or or even even alters alters its its command command when when the the a proximity of the danger is too close. In the road context, aproximity corrective or even alters its In command the of action the danger danger is too too close. the road roadwhen context, proximity of the is close. In the context, this problematic been for trucks, for instance proximity of the has danger is studied too close. the road context, this problematic problematic has been studied forIntrucks, trucks, for instance instance this has been studied for for in (Bouteldja et al. (2004) ) and for cars (Doumiati et al. al. Fig. 1. The Grégoire G7-240 vineyard harvester (left: view this problematic has been studied for trucks, for instance in (Bouteldja (Bouteldja et et al. al. (2004) (2004) )) and and for for cars cars (Doumiati (Doumiati et et 1. The Grégoire G7-240 vineyard harvester (left: view in al. (2009, 2013)). A large number of usefull ADAS such as Fig. Fig. 1. G7-240 vineyard harvester right: rear view with tilt correction) in (Bouteldja et al. (2004) ) and for cars (Doumiati et al. Fig. profile, 1. The The Grégoire Grégoire G7-240 vineyard harvester (left: (left: view view (2009, 2013)). 2013)). A A large large number number of of usefull usefull ADAS ADAS such such as as profile, right: rear view with tilt correction) (2009, “Lane Departure Warning” are now available. The offprofile, right: rear view with tilt correction) (2009, 2013)). A large number ofnow usefull ADAS The suchoffas profile, right: rear view with tilt correction) “Lane Departure Warning” are available. “Lane Departure Warning” are now available. The and off- The rollover-risk in Humanoid Robotics is classicaly studroad context has been studied more recently in Peters “Lane Departure Warning” now available. The offThe rollover-risk in Humanoid Robotics is classicaly studroad context context has been been studiedare more recently in Peters Peters and The rollover-risk in Robotics is studroad has studied more recently in and using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover Theby rollover-risk in Humanoid Humanoid Robotics is classicaly classicaly studroad context has been studied more recently in Peters and ied ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover gives a criterion computable from onboard measurements. ADAS are not on the shelf and need some scientific and ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton etneed al. (2009). Anti-rollover gives a criterion computable from onboard measurements. ADAS are not on the shelf and some scientific and gives a criterion computable from onboard measurements. ADAS are not on the shelf and need some scientific and Its adaptation to a wheeled vehicle has been proposed technological studies in order to be adressed in European gives a criterion computable from onboard measurements. ADAS are notstudies on theinshelf need some scientific and Its adaptation to a wheeled vehicle has been proposed in in technological orderand to be be adressed in European European Its adaptation to vehicle has in technological studies in in order order to adressed in Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover Its adaptation to aa wheeled wheeled vehicle has been been proposed proposed in technological studies to be adressed in European Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover in the ideal case of a flat support surface and later in the ADAS could therefore be industrialized and marketed. It Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover in the ideal case of a flat support surface and later in the ADAS could could therefore therefore be be industrialized industrialized and and marketed. marketed. It It in the ideal case aa flat support and later in the ADAS context. The tyre/ground contacts no was the subject of the ActisurTT project the period in the ideal case of of flat support surface surface andwill later inmore the ADAS therefore industrialized andin It off-road off-road context. The tyre/ground contacts will no more was the thecould subject of the thebeActisurTT ActisurTT project inmarketed. the period off-road context. The tyre/ground contacts will no more was subject of project in the period be considered as planar and punctual but will be enclosed [2011, 2014] (see footnote) and the work presented here is off-road context. The tyre/ground contacts will no more was the subject of the ActisurTT project in the here period considered as planar and punctual but will be enclosed [2011, 2014] (see footnote) footnote) and the the work work presented is be be considered as and but enclosed [2011, 2014] (see and presented here is is inside boxes (3D intervals) the computation of which is aa small part of this project. The ActisurTT demonstrator be considered as planar planar and punctual punctual but will will be be enclosed [2011, 2014] (see footnote) and the work presented here inside boxes (3D intervals) the computation of which is small part of this project. The ActisurTT demonstrator inside boxes (3D intervals) the computation of which is a small part of this project. The ActisurTT demonstrator detailed. It makes it possible to define an interval version is a G7-240 vineyard harvester from Grégoire company. inside boxes (3D intervals) the computation of which is ais small part of this project. The ActisurTT demonstrator detailed. It makes it possible to define an interval version a G7-240 vineyard harvester from Grégoire company. detailed. It makes it possible to define an interval version is G7-240 vineyard harvester from Grégoire company. of the previous ZMP based risk An application in This vehicle is archetypal for this study: it is designed to detailed. It makes it possible to criteria. define an interval version is aa G7-240 vineyard harvester from Grégoire company. of the previous ZMP based risk criteria. An application in This vehicle is archetypal for this study: it is designed to the previous based risk An in This vehicle is archetypal archetypal forwhere this study: study: itgrow. is designed designed to of the context of aaZMP real size experiment is finally presented. operate in the steep slopes grapes The fact of the previous ZMP based risk criteria. criteria. An application application in This vehicle is this is to the context of real size experiment is finally presented. operate in the the steep slopes slopesforwhere where grapesitgrow. grow. The fact fact the context of a real size experiment is finally presented. operate in steep grapes The that the center of mass of this type of vehicle is high the context of a real size experiment is finally presented. operate in the steep slopes where grapes grow. The fact that the the center center of of mass mass of of this this type type of of vehicle vehicle is is high high that (∼ 2m) themass risk of of rollover as mentioned mentioned in Walz 2. THE ZMP-BASED CRITERION IN IDEAL that theincreases center of of rollover this type of vehicle is Walz high (∼ 2m) increases the risk as in 2. THE ZMP-BASED CRITERION IN IDEAL (∼ 2m) increases increases the the risk risk of of rollover rollover as as mentioned mentioned in in Walz Walz (2005). 2. THE ZMP-BASED CRITERION IN IDEAL CONDITIONS (∼ 2m) 2. THE ZMP-BASED CRITERION IN IDEAL (2005). CONDITIONS (2005). CONDITIONS (2005). CONDITIONS 2.1 ⋆ This work has been done in the PhD of Xu Han (2014) within the 2.1 Principles Principles of of the the ZMP ZMP analysis analysis applied applied to to a a wheeled wheeled 2.1 Principles of the ZMP analysis applied to a ⋆ This work has been done in the PhD of Xu Han (2014) within the vehicle on a planar surface. ⋆ 2.1 Principles of the ZMP analysis applied to a wheeled wheeled vehicle on a planar surface. work done in the PhD of Xu Han (2014) within the framework ofhas the been ActiSurTT research program funded by the French ⋆ This vehicle on a planar surface. This work has been done in the PhD of Xu Han (2014) within the framework of the ActiSurTT research program funded by the French vehicle on a planar surface. National Agency for Research (ANR) under the reference ANR-10framework of the ActiSurTT research program funded by the French framework of the ActiSurTT funded by the French National Agency for Researchresearch (ANR)program under the reference ANR-10Han et al. (2012) report aa large amount of anti-rollover VPTT-008. Authors Grégoire Poclain Hydraulics for National Agency for Research (ANR) under the ANR-10Han et al. (2012) report amount of anti-rollover National Agency for thank Research (ANR) and under the reference reference ANR-10VPTT-008. Authors thank Grégoire and Poclain Hydraulics for Han et al. (2012) report aa large large amount of anti-rollover stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA VPTT-008. Authors thank Grégoire and Poclain Hydraulics for Han et al. (2012) report large amount of anti-rollover VPTT-008. Authors thank Grégoire and Poclain Hydraulics for stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA stability criteria among which the ZMP criterion (Vukobratović and Borovac (2005); Bidaud et al. (2010)) is and EFFIDENCE for the experiment operations. the experimental harvester modifications and Grégoire, IRSTEA stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA bratović and and Borovac Borovac (2005); (2005); Bidaud Bidaud et et al. al. (2010)) (2010)) is and EFFIDENCE EFFIDENCE for for the the experiment experiment operations. operations. bratović and bratović and Borovac (2005); Bidaud et al. (2010)) is is and EFFIDENCE for the experiment operations. Copyright © 2015, 2015 IFAC 277Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015 IFAC 277 Copyright 2015 IFAC 277 Peer review© of International Federation of Automatic Copyright ©under 2015 responsibility IFAC 277Control. 10.1016/j.ifacol.2015.09.540
SAFEPROCESS 2015 278 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
The world reference frame R0 is Galilean and tied to the → → → y0− z 0 ). For ground. Its associated basis is B0 = (− x0 − convenience’sake, its origin is near the zone of operation → and − z 0 is assumed to be aligned with the gravity. In order to describe the tyre/ground contact, it is convenient to use another Galilean frame Rs whose basis is Bs = → → → → → ys− z s ) where (− y s ) is tangent to the ground. (− xs − xs − → Note that the gravity is aligned with − z 0 and not with → − z s. Fig. 2. Left : the admissible motions. Right: the forbidden rotations.
Assume that the ground/wheel contacts {C1 , C2 , C3 , C4 } → are punctual and coplanar, − n denotes the unit normal to this plane that can be considered as an estimation of the normal to the support plane. All coordinates being computed in the mobile reference frame RI , the ZMP OZ (3 coordinates) is defined by 1 scalar relation stating that it belongs to the support plane and 2 relations that express the fact that the moment (2) of the inertia and gravity → forces computed in OZ is parallel to − n (1).
G
C1 C2
x
C4
The mobile reference frame RI has its origin in a point G of the chassis in the vicinity of the center of gravity of the → → → yI − z I ) is defined so vehicle (Fig. 3). The basis BI = (− xI − → → that − x I (resp. − z I ) is directed toward the front (resp. the top) of the vehicle.
Oz C3
→ − → − → n = 0 τˆp |Bs ∧ − d −−−−→ − → τˆp |Bs = [Ts←I ]([IG,Σ ][ ΩRI /R0 ] dt −−−−→ −−−−→ + ΩRI /R0 ∧ [IG,Σ ]ΩRI /R0 −−−→ −−−−−−→ → + OZ G ∧ (mΓG∈Σ/R0 − m− g ))
Fig. 3. A simplified model of a vehicle, Zero Moment Point (ZMP) OZ , 4 contact points and the support polygon famous for humanoids. We propose to adapt it for the case of a reconfigurable all-terrain wheeled vehicle. The principle of the analysis by using the ZMP stems from the fact that, if the tyre/ground contact points stay on a flat surface, the possible motions of the vehicle are restricted to the longitudinal and lateral translations and to only one rotation around the normal to the plane of the vehicle (Fig. 2 left). The stability of this planar motion with respect to rollover (forbidden motions) introduces the possible rollover axes that are the boundaries of the support polygon (Fig. 3). The ZMP is a point OZ that belongs to the support plane and such that the moment, computed in OZ , of the inertia and gravity forces is orthogonal to the support plane. The ZMP non-rollover stability criterion is simply expressed by the fact that OZ lies inside the support polygon (Fig. 3): if a disturbance initiates a rollover with respect to one axle (Ci , Cj ), the fact that OZ belongs to the support polygon implies that the moment of the inertia and gravity forces counteracts the rollover. In the static case where only the weight and no inertial forces is considered, OZ is the intersection between the support plane and the → vertical line (G, − g ) passing through the center of mass G. More generally, the stability margin is quantified as the minimum distance between OZ and the possible rollover axes {(C1 , C2 ), (C2 , C3 ), (C3 , C4 ), (C4 , C1 )}. In the sequel, we then present how to compute i) the ZMP and ii) the support polygon from onboard measurements. 2.2 ZMP computation A first step consists to define the reference frames. 278
(1) (2)
Note that → − −−−−−−→ (1) the needed measurements Ω RI /R0 (resp. ΓG∈Σ/R0 ) is available from an onboard gyrometer (resp. acd −−−−→ celerometer) embedded in an IMU, dt ΩRI /R0 is given by a “derivative filter”, (2) the angles of the rotation matrix [Ts←I ] (from BI to Bs ) are also given by the IMU, (3) the inertia matrix [IG,Σ ] and the mass m are known from the CAD of the vehicle. → (4) a static approximation of − τˆp |Bs is sometimes used by −−−→ → g ). summarizing it by its static part OZ G ∧ (−m.− 2.3 Support polygon computation Although the method is general, it is illustrated it by application to the particular studied vehicle (Fig. 10). Computing the position of contact points is done in two steps: i) compute the coordinates of the wheels centers in the mobile frame BI , ii) compute wheel/ground contact points. Wheel centers positionning The position of the centers of the wheels in the mobile frame is determined by using a Denavit-Hartenberg specification of the machine parametrized by its CAD data together with the measurements of the steering wheel angle (Fig. 4, right) and the adjustable lengths (L1 , L2 , L3 , L4 ) of the rods of the hydraulic jacks. Note that these variables, denoted as “internal configuration” in the sequel, are used to adjust the attitude of the vehicle w.r.t. the terrain. They may be controlled either independantly or by the patented
SAFEPROCESS 2015 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
279
ground, the contact point (Ci ) is the intersection of this sphere with a semi-line d whose origin is the center of the → wheel Ki and directed with −− n (Fig. 5).
n
K
K n C
x C
Y
Fig. 5. Wheel/ground contact when assuming a non deformable wheel model. Fig. 4. Kinematics of the tractor with hydraulic jacks for to control height, attitude and steering. Left : rear arms. Right : front legs TM
AUTOlevel system operated by the driver by a pushbutton action. Let us consider the rear wheels (left of Fig. 4). Changing the lengths (L3 , L4 ) of the hydraulic jacks change the position of the wheel centers with respect to the chassis. For the front wheels, the conclusion is the same but the design of the vehicle (Fig. 4, right) is more complex. The → − rotation axis N used for the steering is directed to the front and outside of the vehicle. There is also an offset between the steering axis and the center of the front wheels (right →∗ − → − of Fig. 4). Finally, the new position U of a vector U after → − a rotation angle β around the vector N can be written as: →− − → →∗ − (3) U = RβN U → − →− − →T → − with 1 RβN = cos(β)I3×3 +(1−cos(β)) N N +sin(β)sk( N ). The position of the center of the front wheel, in RI is obtained by applying the transformation (3). Summing up the results, the computation of each wheel center is done by a program that instantiates a differentiable function of the curvature χ deduced from the steering angle and of the vector Λ = [L1 , L2 , L3 , L4 ] of the lengths of the extensions of the hydraulic jacks used for reconfiguration. Ki denoting the coordinates of the ith wheel center in the mobile frame, it is a function defined as: Ki = Fi (χ, Λ) with i ∈ {1, 2, 3, 4}.
(4)
The 4 wheel centers Ki ; i ∈ {1, 2, 3, 4} are supposed to be coplanar in this section. The radii of the 4 wheels being → equal, the normal − n to the ground is the normal to the plane containing the 4 wheel centers, all those items being computed in the mobile frame RI . Wheel/ground contact points Let us continue to consider an ideally simple world and an ideal undeformable wheel on a slope. The wheel/ground contact points are on the surface of a sphere centered on the center Ki of the wheel → with a static radius R. − n being the upward normal to the 1
→ − →→ → − − → sk( N ) is an anti-symmetric matrix such as sk( N )− υ = N ∧− υ.
279
2.4 Relative distances of the ZMP to polygon The non-rollover condition of a vehicle is that the ZMP lies inside the support polygon (C1 , C2 , C3 , C4 ). This condition is numerically computed by the 4 algebraic distances between the ZMP and the support lines of each side of polygon. They should all be positive for the ZMP to lie inside the support polygon. The induced computations are as follows. • Let {C1 , C2 , C3 , C4 } denote the 4 vertices of the polygon, ordered clockwise from the front left corner. → • Let − u i ; i ∈ {1, 2, 3} be the unit inner normal vector → to the edge (Ci , Ci+1 ) and − u 4 unit vector normal to → − the edge (C4 , C1 ). n denoting the upward unit vector normal to the plane containing (C1 , C2 , C3 , C4 ), the inner normals are defined as: −−−−→ −−−−−→ → → n ∧ Ci Ci+1 / Ci , Ci+1 ; i ∈ {1, 2, 3} and ∗ − ui = − −−−→ −−−−→ → → ∗ − u4 =− n ∧ C4 C1 / C4 , C1 . • The algebraic distance of point OZ to the edge i is (5) by definition. Thus, the algebraic distance of point OZ to the polygon (C1 , C2 , C3 , C4 ) is (6). −−−→ → (5) u i , i ∈ {1, 2, 3, 4} , Di = Ci OZ , − D = min {Di } . i=1,2,3,4
(6)
Note that the computation of the criterion D (6) and of each distance Di (5) involves the positions of the wheelcenters (4). Each quantity Di is then a differentiable function of (χ, Λ) and, as D is a minimum, it is a nondifferentiable 2 function of (χ, Λ). When the criterion D falls lower than a threshold (to be defined), the side that gives the smallest distance indicates the side where the rollover risk is maximum. The determination of this threshold is now a Human Machine System study where the ADAS must be experimented in situ or in simulation with a panel of human operators. Note that it is difficult (or not realistic) to model the human decision process by binary tests and that a fuzzy decision can be considered as more adapted. The interval version of the criterion exposed in the sequel will facilitate the definition of this fuzzy decision. 2 It is differentiable almost everywhere but not everywhere: this is important in optimizing the function.
SAFEPROCESS 2015 280 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
This section has detailed the computation of a ZMP based non-rollover criterion in the ideal case of a flat support polygon. This is certainly not the case because the tyres are deformable, the ground is compactable and the position of each wheel is determined by its own hydraulic jack (Fig. 4). In the sequel, the wheel/ground contact points are no more considered as ponctual but distributed in sets included in computable 3D-intervals (boxes). 3. A MORE REALISTIC ZMP-BASED CRITERION Reconsider now (K1 , K2 , K3 , K4 ) the polygon of the wheel centers that is probably non-planar since the position of each wheel centered is tuned by an independant hydraulic jack. Possible limit equilibria are done when the vehicle is supported by only 3 wheels, which is equivalent to say that the support polygons are the 4 triangles shown on Fig. 6. Each triangle has an easily computable upward unit → normal − n i and we assume that, for any equilibrium, the normal to the ground lies inside the convex hull built from the 4 normals of the limit cases. Moreover, gravity tends to deform the tyre in the direction of the slope. This influence is taken into account by including the gravity into the convex hull (7) that defines the possible normals to the ground (Fig. 7). − → n =
4
→ λi .− ni
n
ni ; i i=1 :4
bounding box
(7)
− → → → n 0 = −− g / �− g�
and
∀i = 0...4, λi ≥ 0;
4
λi = 1.
m zoo
Fig. 8. The bounding box including the possible contact area
i=0
K1
K1
K2 n
n
area in a 3D interval, e.g. a box (Fig. 8) with axes aligned with those of BI . For each wheel, the box is computed as the one enclosing the punctual intersections of the vertices of the convex hull of the normals with the spheres of respective radii R and R − δR with the same center as the wheel. Note that the fact that the tyre/ground contact points are situated in a box is coherent with the possible deformations of both tyre and ground and with the fact that the tread of the tyre contains sculptures and is not a pure cylinder.
K2
3 n
1
K4
n
K4
4
2
K3
possible contact areas
Fig. 7. Possible contact area (the vector of gravity is outside of the cone built by the 4 normals)
i=0
with
g outside of the convex hull of the 4 normals ni; i=1...4
K3
Fig. 6. The 4 triangular limit support polygons 3.1 Deformable wheel and ground Consider the deformation of the tyre and the possible sinking of the sculptures of the tyre in the ground. A simple but significant model for the deformation of both tyre and ground is an interval formulation. It consists to state that the contact point Ci lies between to spheres centered in the center Ki of the wheel with respective radii R and R − δR. In the case of the vineyard harvester, an expert advice 3 has given δR = 15 cm. Still assuming that the −−−→ vector Ki Ci is aligned with a possible normal, the set of possible contact point is the intersection of the convex hull (7) with the set between the two spheres centered in Ki with respective radii R and R − δR. For the sake of simplicity, we suggest to enclose the possible contact 3
Chanet (2013) citing Godbole et al. (1993)
280
possible contact areas
Fig. 9. 4 bounding box for all the contact areas
SAFEPROCESS 2015 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
281
3.2 Interval version of the criterion In the previous sections, the computation of the criterion (6) has been detailed from i) an expression (1) that involves onboard measurements and ii) the 4 contact points (Fig. 9). We can thus take into account that we only know that each contact point is included in a box (Fig. 8) by using the tools of interval computation (Jaulin et al. (2001)). We thus define interval version of the criterion D (6) from the interval-computations of each distance Di (5) the arguments of which are the measurement based torque estimation (1) and the 3D intervals (boxes) containing the possible ground/tyre contact points. This interval version of the criterion computes an interval Di , Di in place of the unique distance Di (5). Similarly, the criterion D (6) is replaced by an interval D, D . 3.3 Fuzzy interpretation of the criterion
Fig. 10. The GREGOIRE vine harvester with instrumentation.
We imagine the results of a Human-Machine-System study as follows. Let T be the maximum threshold below which all operators say that there is a risk and T be the minimum threshold above which all operators say that there is no risk. • When D > T , there is no risk. • When T > D, the risk is maximum and the rollover should have occured. • Between these extreme values, the warning must be increased in a continuous way as, for instance, the level of a sound or the blinking frequency of a light.
334.4 334.3 70 60
334.2
50 334.1 40 Y (m)
30
start 334 332 330 −10
334 333.9
20 333.8 10
0
10
20
30 X (m)
0
40
50
333.7
altitude
Fig. 11. Path following during test
4. EXPERIMENTS Steering wheel angle
20
0 −10 −20 −30 −40 −50 −60
angle [deg]
The relevance of those definitions has been shown by computing the criterion from the data collected during a full size experiment organized in April, 2014 in Montoldre (France) during the “Ecotechs” meeting. For this occasion, the vineyard harvester Grégoire G7TM (Fig. 10) was equipped with the instrumentation listed below.
10
−70
• MTi (Xsens). Inertial Measurement Unit featuring a 3D accelerometer, a 3D gyrometer and 3 magnetometers. • LX-PA-40 Position sensor (TME). 4 cable potentiometer used to measure the length of the hydraulic jacks. • Steering wheel angle. Available on CAN bus. The monitoring of the experiment has been simplified by using the following non-necessary items: • GPS ProFlex 500 (Ashtech). Mobile RTK. • Radar speed sensor Delta DRS1000. The localization system is composed of a fixed reference antenna installed on the experimental field together with a mobile antenna fixed on the top of the cabin (Fig. 10). The speed sensor looks at the ground at an offset angle of 45[o ] with the transversal plane of the harvester. 4.1 Experiment The path of the experiment is shown on Fig. 11 and the evolution steering angle on Fig. 12. During this test, the 281
−80 0
time [s] 20
40
60
80
100
120
140
160
180
Fig. 12. Steering angle evolution: the U-turn occurs between 110 and 130 s. vehicle starts with the higher part of the field on its left side. It then goes straight while climbing the slope, makes a U-turn and then returns. During the return path, the inclination of the terrain w.r.t. the vehicle is obviously inverted. The inclination of the vehicle w.r.t. the ground is adapted by action of the AUTOlevelTM system during the U-turn. A punctual evaluation of the criterion has been achieved following the lines of section 2 under the assumption that the ideal support plane was generated by 2 rear wheels and an equivalent front wheel defined at the middle of the real front wheels. Results are shown on Fig. 13. The most dangerous side for the vehicle being determined by the lowest value of the criterion, it comes that the right side is the most dangerous one in the beginning (t ∈ [0; 45]s), then the left side during the straight way (t ∈ [55; 80]s). A manual manoeuver acting on the length of the hydraulic
SAFEPROCESS 2015 282 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
jacks in order to compensate the slope was performed at time t ≈ 80s). Its benefits on the level of risk is clearly seen on Fig. 13-bottom. This manoeuver is followed by an evolution on a almost flat ground (t ∈ [100; 120]s). After the U-turn, the return of the vehicle shows a symmetric result where the right side has become the most dangerous one. On Fig. 13-top, it is clear that the rollover-risk with respect to the front and the rear axles is very low.
distance [m]
minimum distance to support polygon 1.6
REFERENCES
Front Rear risk zone F
1.4 1.2 1 20
40
60
80
100
120
140
160
180
160
180
time [s]
distance [m]
1.2
Right Left risk zone R risk zone L
1.1 1 0.9 0.8 20
40
60
80
100
120
140
Fig. 13. Evolution of the components of a punctual ZMP based risk criterion The result of an interval based version of the criterion for the right (resp. left) side is shown on Fig. 14 (resp. Fig. 15). The punctual estimations of the Fig. (13-bottom) are included between the upper and lower bounds. d2 right 1.3
maximum minimum
distance [m]
1.2 1.1 1 0.9 0.8 20
40
60
80
100
120
140
160
180
time [s]
Fig. 14. Upper and lower bounds of the distance from the ZMP and the right side of the polygon. The distance interval crosses the 0.80 m threshold at time 130.2 s but the upper bound is always larger than 0.84 m. This situation could generate a mild alarm. 1.35
distance [m]
interval version of the ZMP-based criterion has been developped. The level of risk is given, in a fuzzy-like manner, by the position of a threshold in a scalar interval. In addition, the criterion gives the main origin of the danger. The punctual and interval versions of the criterion have been presented from data acquired in a real experiment and show the relevance of the methodology. As a perspective to this work, the possibility to restrict the operator control inside a "safe" domain in which the interval ZMP-based rolloverrisk criterion should not be degraded.
d4 left
1.3
1.25 1.2
1.15 1.1 1.05 1 0.95
maximum minimum
0.9
time [s]
0.85 20
40
60
80
100
120
140
160
180
Fig. 15. Upper and lower bounds of the distance from the ZMP and the left side of the polygon. The distance nevers falls under the 0.80 m threshold. 5. CONCLUSION This paper has shown an application of the rollover ZMP criterion in the domain of off-road vehicles. It has been applied first by considering an ideal situation and a theoretical context. Taking into account more realistic assumptions about the tyre/ground contact has been done by computing boxes (3D intervals) that contains the possible tyre/ground contact area. Based on those intervals, an 282
Bidaud, P., Han, X., and Pasqui, V. (2010). A rotational perturbation plateform based on a non-overconstrained 3-dof spherical parallel mechanism. In Proceedings of CLAWAR’2010: 13th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines, 936–945. Nagoya, Japan. Bouteldja, M., M’Sirdi, N.K., Glaser, S., and Dolcemascolo, V. (2004). Stability analysis and rollover scenario prediction for tractor semi-trailer. In Int. Conf. in Advances in Vehicle control and Safety AVCS. Bouton, N., Lenain, R., Thuilot, B., and Martinet, P. (2009). An active anti-rollover device based on predictive functional control: Application to an all-terrain vehicle. In IEEE International Conference on Robotics and Automation. Kobe, Japan. CCMSA (2010). Les chiffres utiles de la MSA. Technical report, La Caisse Centrale de la Mutualité Sociale Agricole. Chanet, M. (2013). Modèles de déflection. Technical report, IRSTEA. Doumiati, M., Charara, A., Victorino, A., Lechner, D., and Dubuisson, B. (2013). Vehicle Dynamics Estimation using Kalman Filtering. John Wiley & Sons. Doumiati, M., Victorino, A., Charara, A., and Lechner, D. (2009). Lateral load transfer and normal forces estimation for vehicle safety: experimental test. Vehicle System Dynamics, 47(2), 1511–1533. Godbole, R., Alcock, R., and Hettiaratchi, D. (1993). The prediction of tractive performance on soil surfaces. Journal of Terramechanics, 30(6), 443–459. Han, X. (2014). Sécurité des véhicules à roues en milieu tout-terrain. Thèse en image, signal et automatique, Université de Limoges. Han, X., Mourioux, G., Stéphant, J., and Meizel, D. (2012). About the prediction of all-terrain vehicles rollover. In Proc. of Mecatronics-REM’2012. Jaulin, L., Kieffer, M., Didrit, O., and Walter, E. (2001). Applied Interval Analysis. Springer-Verlag, London. Lapapong, S. and Brennan, S.N. (2010). Terrain-aware rollover prediction for ground vehicles using the zeromoment point method. In Proc. ACC, 1501–1507. Peters, S.C. and Iagnemma, K. (2006). An analysis of rollover stability measurement for high-speed mobile robots. In Proc. IEEE Int. Conf. Robotics and Automation ICRA, 3711–3716. Vukobratović, M. and Borovac, B. (2005). Zero-moment point - thirty five years of its life. International Journal of Humanoid Robotics, 1(1), 157–173. Walz, M.C. (2005). Trends in the static stability factor of passenger cars, light trucks, and vans. Technical report, The National Highway Traffic Safety Administration.
ScienceDirect
A A A A Xu Xu Xu Xu
IFAC-PapersOnLine 48-21 (2015) 277–282
ZMP based interval criterion ZMP based interval criterion ZMP based interval criterion ZMP based interval criterion rollover-risk diagnosis rollover-risk diagnosis rollover-risk diagnosis rollover-risk diagnosis
for for for for
HAN HAN HAN HAN
Joanny STEPHANT Gilles MOURIOUX Joanny STEPHANT Gilles MOURIOUX Joanny STEPHANT Gilles MOURIOUX Dominique MEIZEL Joanny STEPHANT Gilles MOURIOUX Dominique MEIZEL Dominique MEIZEL Dominique MEIZEL XLIM, XLIM, UMR UMR CNRS/ CNRS/ Limoges Limoges University University #7252, #7252, 16 16 rue rue Atlantis, Atlantis, XLIM, UMR CNRS/ Limoges University #7252, 16 rue 87068 Limoges, France (e-mail: [email protected]). XLIM, UMR CNRS/France Limoges University #7252, 16 rue Atlantis, Atlantis, 87068 Limoges, (e-mail: [email protected]). 87068 Limoges, France (e-mail: [email protected]). 87068 Limoges, France (e-mail: [email protected]). Abstract: This paper presents an interval criterion used to anticipate rollover accidents of Abstract: This This paper paper presents presents an an interval interval criterion criterion used used to to anticipate anticipate rollover rollover accidents accidents of of Abstract: all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid Abstract: This paper presents an interval criterion used to anticipate rollover accidents of all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid all-terrain wheeled vehicles. The Zero Moment Point concept, initially designed for humanoid robots, is presented the case of 4 wheels vehicle, first in an ideal case of aa perfectly ground all-terrain wheeled in vehicles. Moment Point initially forflat humanoid robots, is is presented presented in the case caseThe of aa aZero 4 wheels wheels vehicle, firstconcept, in an an ideal ideal case of ofdesigned perfectly flat ground robots, in the of 4 vehicle, first in case a perfectly flat ground and finally in the off-road context. This adaptation is done by assuming that the tyre/ground robots, is presented in the case of a 4 wheels vehicle, first in an ideal case of a perfectly flat ground and finally finally in in the the off-road off-road context. context. This This adaptation adaptation is is done done by by assuming assuming that that the the tyre/ground tyre/ground and contact is no more punctual but distributed in set enclosed in computable interval). and finally the off-road This adaptation is done by thatbox the(3D tyre/ground contact is no noinmore more punctualcontext. but distributed distributed in aaa set set enclosed enclosed in aaaassuming computable box (3D interval). contact is punctual but in in computable box (3D interval). It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is contact is no more punctual but distributed in a set enclosed in a computable box (3D interval). It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is experimented by using data collected during a real experiment done with a vineyard harvester It is then possible to derive an interval version of the ideal ZMP-based criterion. The method is experimented by using data collected during a real experiment done with a vineyard harvester experimented by using data collected during experiment done with harvester that is typical wheeled reconfigurable vehicle. The interval gives flexibility experimented by case usingof collected during aa real real experiment done criterion with aa vineyard vineyard harvester that is is aaa typical typical case ofdata wheeled reconfigurable vehicle. The interval interval criterion gives aaa flexibility flexibility that case of wheeled reconfigurable vehicle. The criterion gives for its interpretation that is more adapted to the complex and varying all-terrain context. that is interpretation a typical case that of wheeled reconfigurable vehicle. Theand interval criterion givescontext. a flexibility for its is more adapted to the complex varying all-terrain for its that is to complex and all-terrain context. for its interpretation interpretation that Federation is more more adapted adapted to the theControl) complex and varying varying all-terrain context. © 2015, IFAC (International of Automatic Hosting by Elsevier Ltd. All rights reserved. Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Assistance Keywords: Vehicle control, Rollover Estimator, Zero Moment Point, Off-Road Vehicle, Driver Assistance Assistance Assistance 1. INTRODUCTION 1. INTRODUCTION INTRODUCTION 1. 1. INTRODUCTION The ultimate motivation of the study is societal: it aims to The ultimate ultimate motivation motivation of of the the study study is is societal: societal: it it aims aims to to The reduce the number of fatal accidents caused by the reversal The ultimate motivation of the study is societal: it aims to reduce the number of fatal accidents caused by the reversal reduce the number of fatal fatal accidents accidents caused by the theas reversal of heavy vehicles operating in an off-road context it was was reduce the number of caused by reversal of heavy heavy vehicles operating in an an off-road off-road context as it of vehicles operating in context as it was reported in CCMSA (2010) for agriculture. There is a need of heavy vehicles operating off-road context was reported in CCMSA CCMSA (2010) in foran agriculture. There as is aaitneed need reported in (2010) for agriculture. There is for an Advanced Driver Assistant System (ADAS) that reported in CCMSA (2010) for agriculture. There is a need for an Advanced Driver Assistant System (ADAS) that for andiagnose Advanced Driver Assistant System (ADAS) that could the rollover risk and warn the driver to take for an Advanced System that could diagnose theDriver rolloverAssistant risk and and warn warn the (ADAS) driver to to take take could diagnose the rollover risk the driver a corrective action or even alters its command when the could diagnose the rollover risk and warn the driver to take a corrective corrective action action or or even even alters alters its its command command when when the the a proximity of the danger is too close. In the road context, aproximity corrective or even alters its In command the of action the danger danger is too too close. the road roadwhen context, proximity of the is close. In the context, this problematic been for trucks, for instance proximity of the has danger is studied too close. the road context, this problematic problematic has been studied forIntrucks, trucks, for instance instance this has been studied for for in (Bouteldja et al. (2004) ) and for cars (Doumiati et al. al. Fig. 1. The Grégoire G7-240 vineyard harvester (left: view this problematic has been studied for trucks, for instance in (Bouteldja (Bouteldja et et al. al. (2004) (2004) )) and and for for cars cars (Doumiati (Doumiati et et 1. The Grégoire G7-240 vineyard harvester (left: view in al. (2009, 2013)). A large number of usefull ADAS such as Fig. Fig. 1. G7-240 vineyard harvester right: rear view with tilt correction) in (Bouteldja et al. (2004) ) and for cars (Doumiati et al. Fig. profile, 1. The The Grégoire Grégoire G7-240 vineyard harvester (left: (left: view view (2009, 2013)). 2013)). A A large large number number of of usefull usefull ADAS ADAS such such as as profile, right: rear view with tilt correction) (2009, “Lane Departure Warning” are now available. The offprofile, right: rear view with tilt correction) (2009, 2013)). A large number ofnow usefull ADAS The suchoffas profile, right: rear view with tilt correction) “Lane Departure Warning” are available. “Lane Departure Warning” are now available. The and off- The rollover-risk in Humanoid Robotics is classicaly studroad context has been studied more recently in Peters “Lane Departure Warning” now available. The offThe rollover-risk in Humanoid Robotics is classicaly studroad context context has been been studiedare more recently in Peters Peters and The rollover-risk in Robotics is studroad has studied more recently in and using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover Theby rollover-risk in Humanoid Humanoid Robotics is classicaly classicaly studroad context has been studied more recently in Peters and ied ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton et al. (2009). Anti-rollover gives a criterion computable from onboard measurements. ADAS are not on the shelf and need some scientific and ied by using the Zero Moment Point (ZMP) concept that Iagnemma (2006) and Bouton etneed al. (2009). Anti-rollover gives a criterion computable from onboard measurements. ADAS are not on the shelf and some scientific and gives a criterion computable from onboard measurements. ADAS are not on the shelf and need some scientific and Its adaptation to a wheeled vehicle has been proposed technological studies in order to be adressed in European gives a criterion computable from onboard measurements. ADAS are notstudies on theinshelf need some scientific and Its adaptation to a wheeled vehicle has been proposed in in technological orderand to be be adressed in European European Its adaptation to vehicle has in technological studies in in order order to adressed in Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover Its adaptation to aa wheeled wheeled vehicle has been been proposed proposed in technological studies to be adressed in European Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover in the ideal case of a flat support surface and later in the ADAS could therefore be industrialized and marketed. It Lapapong and Brennan (2010). It is reconsidered here, first standards organizations. Reliable certificated anti-rollover in the ideal case of a flat support surface and later in the ADAS could could therefore therefore be be industrialized industrialized and and marketed. marketed. It It in the ideal case aa flat support and later in the ADAS context. The tyre/ground contacts no was the subject of the ActisurTT project the period in the ideal case of of flat support surface surface andwill later inmore the ADAS therefore industrialized andin It off-road off-road context. The tyre/ground contacts will no more was the thecould subject of the thebeActisurTT ActisurTT project inmarketed. the period off-road context. The tyre/ground contacts will no more was subject of project in the period be considered as planar and punctual but will be enclosed [2011, 2014] (see footnote) and the work presented here is off-road context. The tyre/ground contacts will no more was the subject of the ActisurTT project in the here period considered as planar and punctual but will be enclosed [2011, 2014] (see footnote) footnote) and the the work work presented is be be considered as and but enclosed [2011, 2014] (see and presented here is is inside boxes (3D intervals) the computation of which is aa small part of this project. The ActisurTT demonstrator be considered as planar planar and punctual punctual but will will be be enclosed [2011, 2014] (see footnote) and the work presented here inside boxes (3D intervals) the computation of which is small part of this project. The ActisurTT demonstrator inside boxes (3D intervals) the computation of which is a small part of this project. The ActisurTT demonstrator detailed. It makes it possible to define an interval version is a G7-240 vineyard harvester from Grégoire company. inside boxes (3D intervals) the computation of which is ais small part of this project. The ActisurTT demonstrator detailed. It makes it possible to define an interval version a G7-240 vineyard harvester from Grégoire company. detailed. It makes it possible to define an interval version is G7-240 vineyard harvester from Grégoire company. of the previous ZMP based risk An application in This vehicle is archetypal for this study: it is designed to detailed. It makes it possible to criteria. define an interval version is aa G7-240 vineyard harvester from Grégoire company. of the previous ZMP based risk criteria. An application in This vehicle is archetypal for this study: it is designed to the previous based risk An in This vehicle is archetypal archetypal forwhere this study: study: itgrow. is designed designed to of the context of aaZMP real size experiment is finally presented. operate in the steep slopes grapes The fact of the previous ZMP based risk criteria. criteria. An application application in This vehicle is this is to the context of real size experiment is finally presented. operate in the the steep slopes slopesforwhere where grapesitgrow. grow. The fact fact the context of a real size experiment is finally presented. operate in steep grapes The that the center of mass of this type of vehicle is high the context of a real size experiment is finally presented. operate in the steep slopes where grapes grow. The fact that the the center center of of mass mass of of this this type type of of vehicle vehicle is is high high that (∼ 2m) themass risk of of rollover as mentioned mentioned in Walz 2. THE ZMP-BASED CRITERION IN IDEAL that theincreases center of of rollover this type of vehicle is Walz high (∼ 2m) increases the risk as in 2. THE ZMP-BASED CRITERION IN IDEAL (∼ 2m) increases increases the the risk risk of of rollover rollover as as mentioned mentioned in in Walz Walz (2005). 2. THE ZMP-BASED CRITERION IN IDEAL CONDITIONS (∼ 2m) 2. THE ZMP-BASED CRITERION IN IDEAL (2005). CONDITIONS (2005). CONDITIONS (2005). CONDITIONS 2.1 ⋆ This work has been done in the PhD of Xu Han (2014) within the 2.1 Principles Principles of of the the ZMP ZMP analysis analysis applied applied to to a a wheeled wheeled 2.1 Principles of the ZMP analysis applied to a ⋆ This work has been done in the PhD of Xu Han (2014) within the vehicle on a planar surface. ⋆ 2.1 Principles of the ZMP analysis applied to a wheeled wheeled vehicle on a planar surface. work done in the PhD of Xu Han (2014) within the framework ofhas the been ActiSurTT research program funded by the French ⋆ This vehicle on a planar surface. This work has been done in the PhD of Xu Han (2014) within the framework of the ActiSurTT research program funded by the French vehicle on a planar surface. National Agency for Research (ANR) under the reference ANR-10framework of the ActiSurTT research program funded by the French framework of the ActiSurTT funded by the French National Agency for Researchresearch (ANR)program under the reference ANR-10Han et al. (2012) report aa large amount of anti-rollover VPTT-008. Authors Grégoire Poclain Hydraulics for National Agency for Research (ANR) under the ANR-10Han et al. (2012) report amount of anti-rollover National Agency for thank Research (ANR) and under the reference reference ANR-10VPTT-008. Authors thank Grégoire and Poclain Hydraulics for Han et al. (2012) report aa large large amount of anti-rollover stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA VPTT-008. Authors thank Grégoire and Poclain Hydraulics for Han et al. (2012) report large amount of anti-rollover VPTT-008. Authors thank Grégoire and Poclain Hydraulics for stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA stability criteria among which the ZMP criterion (Vukobratović and Borovac (2005); Bidaud et al. (2010)) is and EFFIDENCE for the experiment operations. the experimental harvester modifications and Grégoire, IRSTEA stability criteria among which the ZMP criterion (Vukothe experimental harvester modifications and Grégoire, IRSTEA bratović and and Borovac Borovac (2005); (2005); Bidaud Bidaud et et al. al. (2010)) (2010)) is and EFFIDENCE EFFIDENCE for for the the experiment experiment operations. operations. bratović and bratović and Borovac (2005); Bidaud et al. (2010)) is is and EFFIDENCE for the experiment operations. Copyright © 2015, 2015 IFAC 277Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015 IFAC 277 Copyright 2015 IFAC 277 Peer review© of International Federation of Automatic Copyright ©under 2015 responsibility IFAC 277Control. 10.1016/j.ifacol.2015.09.540
SAFEPROCESS 2015 278 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
The world reference frame R0 is Galilean and tied to the → → → y0− z 0 ). For ground. Its associated basis is B0 = (− x0 − convenience’sake, its origin is near the zone of operation → and − z 0 is assumed to be aligned with the gravity. In order to describe the tyre/ground contact, it is convenient to use another Galilean frame Rs whose basis is Bs = → → → → → ys− z s ) where (− y s ) is tangent to the ground. (− xs − xs − → Note that the gravity is aligned with − z 0 and not with → − z s. Fig. 2. Left : the admissible motions. Right: the forbidden rotations.
Assume that the ground/wheel contacts {C1 , C2 , C3 , C4 } → are punctual and coplanar, − n denotes the unit normal to this plane that can be considered as an estimation of the normal to the support plane. All coordinates being computed in the mobile reference frame RI , the ZMP OZ (3 coordinates) is defined by 1 scalar relation stating that it belongs to the support plane and 2 relations that express the fact that the moment (2) of the inertia and gravity → forces computed in OZ is parallel to − n (1).
G
C1 C2
x
C4
The mobile reference frame RI has its origin in a point G of the chassis in the vicinity of the center of gravity of the → → → yI − z I ) is defined so vehicle (Fig. 3). The basis BI = (− xI − → → that − x I (resp. − z I ) is directed toward the front (resp. the top) of the vehicle.
Oz C3
→ − → − → n = 0 τˆp |Bs ∧ − d −−−−→ − → τˆp |Bs = [Ts←I ]([IG,Σ ][ ΩRI /R0 ] dt −−−−→ −−−−→ + ΩRI /R0 ∧ [IG,Σ ]ΩRI /R0 −−−→ −−−−−−→ → + OZ G ∧ (mΓG∈Σ/R0 − m− g ))
Fig. 3. A simplified model of a vehicle, Zero Moment Point (ZMP) OZ , 4 contact points and the support polygon famous for humanoids. We propose to adapt it for the case of a reconfigurable all-terrain wheeled vehicle. The principle of the analysis by using the ZMP stems from the fact that, if the tyre/ground contact points stay on a flat surface, the possible motions of the vehicle are restricted to the longitudinal and lateral translations and to only one rotation around the normal to the plane of the vehicle (Fig. 2 left). The stability of this planar motion with respect to rollover (forbidden motions) introduces the possible rollover axes that are the boundaries of the support polygon (Fig. 3). The ZMP is a point OZ that belongs to the support plane and such that the moment, computed in OZ , of the inertia and gravity forces is orthogonal to the support plane. The ZMP non-rollover stability criterion is simply expressed by the fact that OZ lies inside the support polygon (Fig. 3): if a disturbance initiates a rollover with respect to one axle (Ci , Cj ), the fact that OZ belongs to the support polygon implies that the moment of the inertia and gravity forces counteracts the rollover. In the static case where only the weight and no inertial forces is considered, OZ is the intersection between the support plane and the → vertical line (G, − g ) passing through the center of mass G. More generally, the stability margin is quantified as the minimum distance between OZ and the possible rollover axes {(C1 , C2 ), (C2 , C3 ), (C3 , C4 ), (C4 , C1 )}. In the sequel, we then present how to compute i) the ZMP and ii) the support polygon from onboard measurements. 2.2 ZMP computation A first step consists to define the reference frames. 278
(1) (2)
Note that → − −−−−−−→ (1) the needed measurements Ω RI /R0 (resp. ΓG∈Σ/R0 ) is available from an onboard gyrometer (resp. acd −−−−→ celerometer) embedded in an IMU, dt ΩRI /R0 is given by a “derivative filter”, (2) the angles of the rotation matrix [Ts←I ] (from BI to Bs ) are also given by the IMU, (3) the inertia matrix [IG,Σ ] and the mass m are known from the CAD of the vehicle. → (4) a static approximation of − τˆp |Bs is sometimes used by −−−→ → g ). summarizing it by its static part OZ G ∧ (−m.− 2.3 Support polygon computation Although the method is general, it is illustrated it by application to the particular studied vehicle (Fig. 10). Computing the position of contact points is done in two steps: i) compute the coordinates of the wheels centers in the mobile frame BI , ii) compute wheel/ground contact points. Wheel centers positionning The position of the centers of the wheels in the mobile frame is determined by using a Denavit-Hartenberg specification of the machine parametrized by its CAD data together with the measurements of the steering wheel angle (Fig. 4, right) and the adjustable lengths (L1 , L2 , L3 , L4 ) of the rods of the hydraulic jacks. Note that these variables, denoted as “internal configuration” in the sequel, are used to adjust the attitude of the vehicle w.r.t. the terrain. They may be controlled either independantly or by the patented
SAFEPROCESS 2015 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
279
ground, the contact point (Ci ) is the intersection of this sphere with a semi-line d whose origin is the center of the → wheel Ki and directed with −− n (Fig. 5).
n
K
K n C
x C
Y
Fig. 5. Wheel/ground contact when assuming a non deformable wheel model. Fig. 4. Kinematics of the tractor with hydraulic jacks for to control height, attitude and steering. Left : rear arms. Right : front legs TM
AUTOlevel system operated by the driver by a pushbutton action. Let us consider the rear wheels (left of Fig. 4). Changing the lengths (L3 , L4 ) of the hydraulic jacks change the position of the wheel centers with respect to the chassis. For the front wheels, the conclusion is the same but the design of the vehicle (Fig. 4, right) is more complex. The → − rotation axis N used for the steering is directed to the front and outside of the vehicle. There is also an offset between the steering axis and the center of the front wheels (right →∗ − → − of Fig. 4). Finally, the new position U of a vector U after → − a rotation angle β around the vector N can be written as: →− − → →∗ − (3) U = RβN U → − →− − →T → − with 1 RβN = cos(β)I3×3 +(1−cos(β)) N N +sin(β)sk( N ). The position of the center of the front wheel, in RI is obtained by applying the transformation (3). Summing up the results, the computation of each wheel center is done by a program that instantiates a differentiable function of the curvature χ deduced from the steering angle and of the vector Λ = [L1 , L2 , L3 , L4 ] of the lengths of the extensions of the hydraulic jacks used for reconfiguration. Ki denoting the coordinates of the ith wheel center in the mobile frame, it is a function defined as: Ki = Fi (χ, Λ) with i ∈ {1, 2, 3, 4}.
(4)
The 4 wheel centers Ki ; i ∈ {1, 2, 3, 4} are supposed to be coplanar in this section. The radii of the 4 wheels being → equal, the normal − n to the ground is the normal to the plane containing the 4 wheel centers, all those items being computed in the mobile frame RI . Wheel/ground contact points Let us continue to consider an ideally simple world and an ideal undeformable wheel on a slope. The wheel/ground contact points are on the surface of a sphere centered on the center Ki of the wheel → with a static radius R. − n being the upward normal to the 1
→ − →→ → − − → sk( N ) is an anti-symmetric matrix such as sk( N )− υ = N ∧− υ.
279
2.4 Relative distances of the ZMP to polygon The non-rollover condition of a vehicle is that the ZMP lies inside the support polygon (C1 , C2 , C3 , C4 ). This condition is numerically computed by the 4 algebraic distances between the ZMP and the support lines of each side of polygon. They should all be positive for the ZMP to lie inside the support polygon. The induced computations are as follows. • Let {C1 , C2 , C3 , C4 } denote the 4 vertices of the polygon, ordered clockwise from the front left corner. → • Let − u i ; i ∈ {1, 2, 3} be the unit inner normal vector → to the edge (Ci , Ci+1 ) and − u 4 unit vector normal to → − the edge (C4 , C1 ). n denoting the upward unit vector normal to the plane containing (C1 , C2 , C3 , C4 ), the inner normals are defined as: −−−−→ −−−−−→ → → n ∧ Ci Ci+1 / Ci , Ci+1 ; i ∈ {1, 2, 3} and ∗ − ui = − −−−→ −−−−→ → → ∗ − u4 =− n ∧ C4 C1 / C4 , C1 . • The algebraic distance of point OZ to the edge i is (5) by definition. Thus, the algebraic distance of point OZ to the polygon (C1 , C2 , C3 , C4 ) is (6). −−−→ → (5) u i , i ∈ {1, 2, 3, 4} , Di = Ci OZ , − D = min {Di } . i=1,2,3,4
(6)
Note that the computation of the criterion D (6) and of each distance Di (5) involves the positions of the wheelcenters (4). Each quantity Di is then a differentiable function of (χ, Λ) and, as D is a minimum, it is a nondifferentiable 2 function of (χ, Λ). When the criterion D falls lower than a threshold (to be defined), the side that gives the smallest distance indicates the side where the rollover risk is maximum. The determination of this threshold is now a Human Machine System study where the ADAS must be experimented in situ or in simulation with a panel of human operators. Note that it is difficult (or not realistic) to model the human decision process by binary tests and that a fuzzy decision can be considered as more adapted. The interval version of the criterion exposed in the sequel will facilitate the definition of this fuzzy decision. 2 It is differentiable almost everywhere but not everywhere: this is important in optimizing the function.
SAFEPROCESS 2015 280 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
This section has detailed the computation of a ZMP based non-rollover criterion in the ideal case of a flat support polygon. This is certainly not the case because the tyres are deformable, the ground is compactable and the position of each wheel is determined by its own hydraulic jack (Fig. 4). In the sequel, the wheel/ground contact points are no more considered as ponctual but distributed in sets included in computable 3D-intervals (boxes). 3. A MORE REALISTIC ZMP-BASED CRITERION Reconsider now (K1 , K2 , K3 , K4 ) the polygon of the wheel centers that is probably non-planar since the position of each wheel centered is tuned by an independant hydraulic jack. Possible limit equilibria are done when the vehicle is supported by only 3 wheels, which is equivalent to say that the support polygons are the 4 triangles shown on Fig. 6. Each triangle has an easily computable upward unit → normal − n i and we assume that, for any equilibrium, the normal to the ground lies inside the convex hull built from the 4 normals of the limit cases. Moreover, gravity tends to deform the tyre in the direction of the slope. This influence is taken into account by including the gravity into the convex hull (7) that defines the possible normals to the ground (Fig. 7). − → n =
4
→ λi .− ni
n
ni ; i i=1 :4
bounding box
(7)
− → → → n 0 = −− g / �− g�
and
∀i = 0...4, λi ≥ 0;
4
λi = 1.
m zoo
Fig. 8. The bounding box including the possible contact area
i=0
K1
K1
K2 n
n
area in a 3D interval, e.g. a box (Fig. 8) with axes aligned with those of BI . For each wheel, the box is computed as the one enclosing the punctual intersections of the vertices of the convex hull of the normals with the spheres of respective radii R and R − δR with the same center as the wheel. Note that the fact that the tyre/ground contact points are situated in a box is coherent with the possible deformations of both tyre and ground and with the fact that the tread of the tyre contains sculptures and is not a pure cylinder.
K2
3 n
1
K4
n
K4
4
2
K3
possible contact areas
Fig. 7. Possible contact area (the vector of gravity is outside of the cone built by the 4 normals)
i=0
with
g outside of the convex hull of the 4 normals ni; i=1...4
K3
Fig. 6. The 4 triangular limit support polygons 3.1 Deformable wheel and ground Consider the deformation of the tyre and the possible sinking of the sculptures of the tyre in the ground. A simple but significant model for the deformation of both tyre and ground is an interval formulation. It consists to state that the contact point Ci lies between to spheres centered in the center Ki of the wheel with respective radii R and R − δR. In the case of the vineyard harvester, an expert advice 3 has given δR = 15 cm. Still assuming that the −−−→ vector Ki Ci is aligned with a possible normal, the set of possible contact point is the intersection of the convex hull (7) with the set between the two spheres centered in Ki with respective radii R and R − δR. For the sake of simplicity, we suggest to enclose the possible contact 3
Chanet (2013) citing Godbole et al. (1993)
280
possible contact areas
Fig. 9. 4 bounding box for all the contact areas
SAFEPROCESS 2015 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
281
3.2 Interval version of the criterion In the previous sections, the computation of the criterion (6) has been detailed from i) an expression (1) that involves onboard measurements and ii) the 4 contact points (Fig. 9). We can thus take into account that we only know that each contact point is included in a box (Fig. 8) by using the tools of interval computation (Jaulin et al. (2001)). We thus define interval version of the criterion D (6) from the interval-computations of each distance Di (5) the arguments of which are the measurement based torque estimation (1) and the 3D intervals (boxes) containing the possible ground/tyre contact points. This interval version of the criterion computes an interval Di , Di in place of the unique distance Di (5). Similarly, the criterion D (6) is replaced by an interval D, D . 3.3 Fuzzy interpretation of the criterion
Fig. 10. The GREGOIRE vine harvester with instrumentation.
We imagine the results of a Human-Machine-System study as follows. Let T be the maximum threshold below which all operators say that there is a risk and T be the minimum threshold above which all operators say that there is no risk. • When D > T , there is no risk. • When T > D, the risk is maximum and the rollover should have occured. • Between these extreme values, the warning must be increased in a continuous way as, for instance, the level of a sound or the blinking frequency of a light.
334.4 334.3 70 60
334.2
50 334.1 40 Y (m)
30
start 334 332 330 −10
334 333.9
20 333.8 10
0
10
20
30 X (m)
0
40
50
333.7
altitude
Fig. 11. Path following during test
4. EXPERIMENTS Steering wheel angle
20
0 −10 −20 −30 −40 −50 −60
angle [deg]
The relevance of those definitions has been shown by computing the criterion from the data collected during a full size experiment organized in April, 2014 in Montoldre (France) during the “Ecotechs” meeting. For this occasion, the vineyard harvester Grégoire G7TM (Fig. 10) was equipped with the instrumentation listed below.
10
−70
• MTi (Xsens). Inertial Measurement Unit featuring a 3D accelerometer, a 3D gyrometer and 3 magnetometers. • LX-PA-40 Position sensor (TME). 4 cable potentiometer used to measure the length of the hydraulic jacks. • Steering wheel angle. Available on CAN bus. The monitoring of the experiment has been simplified by using the following non-necessary items: • GPS ProFlex 500 (Ashtech). Mobile RTK. • Radar speed sensor Delta DRS1000. The localization system is composed of a fixed reference antenna installed on the experimental field together with a mobile antenna fixed on the top of the cabin (Fig. 10). The speed sensor looks at the ground at an offset angle of 45[o ] with the transversal plane of the harvester. 4.1 Experiment The path of the experiment is shown on Fig. 11 and the evolution steering angle on Fig. 12. During this test, the 281
−80 0
time [s] 20
40
60
80
100
120
140
160
180
Fig. 12. Steering angle evolution: the U-turn occurs between 110 and 130 s. vehicle starts with the higher part of the field on its left side. It then goes straight while climbing the slope, makes a U-turn and then returns. During the return path, the inclination of the terrain w.r.t. the vehicle is obviously inverted. The inclination of the vehicle w.r.t. the ground is adapted by action of the AUTOlevelTM system during the U-turn. A punctual evaluation of the criterion has been achieved following the lines of section 2 under the assumption that the ideal support plane was generated by 2 rear wheels and an equivalent front wheel defined at the middle of the real front wheels. Results are shown on Fig. 13. The most dangerous side for the vehicle being determined by the lowest value of the criterion, it comes that the right side is the most dangerous one in the beginning (t ∈ [0; 45]s), then the left side during the straight way (t ∈ [55; 80]s). A manual manoeuver acting on the length of the hydraulic
SAFEPROCESS 2015 282 September 2-4, 2015. Paris, France
Xu HAN et al. / IFAC-PapersOnLine 48-21 (2015) 277–282
jacks in order to compensate the slope was performed at time t ≈ 80s). Its benefits on the level of risk is clearly seen on Fig. 13-bottom. This manoeuver is followed by an evolution on a almost flat ground (t ∈ [100; 120]s). After the U-turn, the return of the vehicle shows a symmetric result where the right side has become the most dangerous one. On Fig. 13-top, it is clear that the rollover-risk with respect to the front and the rear axles is very low.
distance [m]
minimum distance to support polygon 1.6
REFERENCES
Front Rear risk zone F
1.4 1.2 1 20
40
60
80
100
120
140
160
180
160
180
time [s]
distance [m]
1.2
Right Left risk zone R risk zone L
1.1 1 0.9 0.8 20
40
60
80
100
120
140
Fig. 13. Evolution of the components of a punctual ZMP based risk criterion The result of an interval based version of the criterion for the right (resp. left) side is shown on Fig. 14 (resp. Fig. 15). The punctual estimations of the Fig. (13-bottom) are included between the upper and lower bounds. d2 right 1.3
maximum minimum
distance [m]
1.2 1.1 1 0.9 0.8 20
40
60
80
100
120
140
160
180
time [s]
Fig. 14. Upper and lower bounds of the distance from the ZMP and the right side of the polygon. The distance interval crosses the 0.80 m threshold at time 130.2 s but the upper bound is always larger than 0.84 m. This situation could generate a mild alarm. 1.35
distance [m]
interval version of the ZMP-based criterion has been developped. The level of risk is given, in a fuzzy-like manner, by the position of a threshold in a scalar interval. In addition, the criterion gives the main origin of the danger. The punctual and interval versions of the criterion have been presented from data acquired in a real experiment and show the relevance of the methodology. As a perspective to this work, the possibility to restrict the operator control inside a "safe" domain in which the interval ZMP-based rolloverrisk criterion should not be degraded.
d4 left
1.3
1.25 1.2
1.15 1.1 1.05 1 0.95
maximum minimum
0.9
time [s]
0.85 20
40
60
80
100
120
140
160
180
Fig. 15. Upper and lower bounds of the distance from the ZMP and the left side of the polygon. The distance nevers falls under the 0.80 m threshold. 5. CONCLUSION This paper has shown an application of the rollover ZMP criterion in the domain of off-road vehicles. It has been applied first by considering an ideal situation and a theoretical context. Taking into account more realistic assumptions about the tyre/ground contact has been done by computing boxes (3D intervals) that contains the possible tyre/ground contact area. Based on those intervals, an 282
Bidaud, P., Han, X., and Pasqui, V. (2010). A rotational perturbation plateform based on a non-overconstrained 3-dof spherical parallel mechanism. In Proceedings of CLAWAR’2010: 13th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines, 936–945. Nagoya, Japan. Bouteldja, M., M’Sirdi, N.K., Glaser, S., and Dolcemascolo, V. (2004). Stability analysis and rollover scenario prediction for tractor semi-trailer. In Int. Conf. in Advances in Vehicle control and Safety AVCS. Bouton, N., Lenain, R., Thuilot, B., and Martinet, P. (2009). An active anti-rollover device based on predictive functional control: Application to an all-terrain vehicle. In IEEE International Conference on Robotics and Automation. Kobe, Japan. CCMSA (2010). Les chiffres utiles de la MSA. Technical report, La Caisse Centrale de la Mutualité Sociale Agricole. Chanet, M. (2013). Modèles de déflection. Technical report, IRSTEA. Doumiati, M., Charara, A., Victorino, A., Lechner, D., and Dubuisson, B. (2013). Vehicle Dynamics Estimation using Kalman Filtering. John Wiley & Sons. Doumiati, M., Victorino, A., Charara, A., and Lechner, D. (2009). Lateral load transfer and normal forces estimation for vehicle safety: experimental test. Vehicle System Dynamics, 47(2), 1511–1533. Godbole, R., Alcock, R., and Hettiaratchi, D. (1993). The prediction of tractive performance on soil surfaces. Journal of Terramechanics, 30(6), 443–459. Han, X. (2014). Sécurité des véhicules à roues en milieu tout-terrain. Thèse en image, signal et automatique, Université de Limoges. Han, X., Mourioux, G., Stéphant, J., and Meizel, D. (2012). About the prediction of all-terrain vehicles rollover. In Proc. of Mecatronics-REM’2012. Jaulin, L., Kieffer, M., Didrit, O., and Walter, E. (2001). Applied Interval Analysis. Springer-Verlag, London. Lapapong, S. and Brennan, S.N. (2010). Terrain-aware rollover prediction for ground vehicles using the zeromoment point method. In Proc. ACC, 1501–1507. Peters, S.C. and Iagnemma, K. (2006). An analysis of rollover stability measurement for high-speed mobile robots. In Proc. IEEE Int. Conf. Robotics and Automation ICRA, 3711–3716. Vukobratović, M. and Borovac, B. (2005). Zero-moment point - thirty five years of its life. International Journal of Humanoid Robotics, 1(1), 157–173. Walz, M.C. (2005). Trends in the static stability factor of passenger cars, light trucks, and vans. Technical report, The National Highway Traffic Safety Administration.