Detecting external forces on an autonomous lawn mowing robot with inertial, wheel speed and wheel motor current measurements

Transcription

1 Master of Science Thesis in reglerteknik Department of Electrical Engineering, Linköping University, 2017 Detecting external forces on an autonomous lawn mowing robot with inertial, wheel speed and wheel motor current measurements Gustav Norin

2 Master of Science Thesis in reglerteknik Detecting external forces on an autonomous lawn mowing robot with inertial, wheel speed and wheel motor current measurements Gustav Norin LiTH-ISY-EX 17/5034 SE Supervisor: Examiner: Oskar Ljungqvist isy, Linköpings universitet Mikaela Åhlén Husqvarna AB Fredrik Gustafsson isy Linköpings universitet Division of Automatic Control Department of Electrical Engineering Linköping University SE Linköping, Sweden Copyright 2017 Gustav Norin

3 Abstract An autonomous lawn mowing robot moves around randomly within an area enclosed by a magnetic wire and makes decision based on sensor information. To ensure human and animal safety it is essential that the robotic lawn mower can detect and stop if, for instance, it is being lifted by a human. This thesis takes a look at how on-board sensors could be used to detect a few critical events, here called fault cases. Data such as acceleration, angular velocity and motor currents are recorded and then used to develop three methods for detection briefly described below. The Odometry method uses constraints on valid movement of the robotic lawn mower and a fault case is detected if estimated velocity in global coordinates violates these constraints. The pitch angle relationship estimates the relation between electrical currents needed to drive the robotic lawn mower at a certain speed in certain pitch angle. When the electrical currents corresponding to a certain pitch angle according to the relation deviates from measured currents a fault case would be detected. The frequency method is based on the idea that disturbances on signals caused by uneven ground should decrease when the robotic lawn mower is lifted or held. The method would then detect this damping of disturbances by examining frequency content. The best method is the pitch angle relationship while the other two proposed methods have potential but would need higher sampling frequencies and additional signals to fully perform satisfactorily. With additional information such as position of the robotic lawn mower the estimation of the global velocities could be significantly improved which in turn would improve the odometry method and serve as a complement to the current pitch angle relation. The frequency methods would also be valid if the sampling frequencies were much higher, something that might not be as cost efficient as needed to make the method profitable. iii

4

5 Acknowledgments The thesis suggestion was provided by Husqvarna AB and I would like to give special thanks to the following persons for helping me throughout the work. Jonas Rangsjö, Husqvarna AB Oskar Ljungqvist, supervisor, Linköping University Mikaela Åhlén, supervisor, Husqvarna AB Fredrik Gustafsson, Examiner, Linköping University Linköping, August 2016 Gustav Norin v

6

7 Contents Notation ix 1 Introduction Background Purpose Conditions and limitations Objectives Method Method limitation Related work Conditions Equipment and software Coordinate systems Kinematic constraints Signals Definition of external forces Standing still Lift front Lift back Lift whole Lift side Collision Wheel slip Driving Lift front Lift back Lift whole Lift side Collision Wheel slip Fault case summary vii

8 viii Contents 4 Detection methods Odometry Integrate accelerometer data Pitch current relationship Pitch and current in static relationship Pitch and current dynamic relationship Frequencies Evaluation & results Method evaluation Odometry Pitch current relationship Frequencies Standing still Driving Conclusions Summary Future work Bibliography 49

9 ix

10 x Notation Notation Notations that will be used in the report Notation T s Description Sampling interval θ pitch-angle φ roll-angle ψ yaw-angle θ pitch angular velocity or pitch-rate φ roll angular velocity or roll-rate ψ yaw angular velocity or yaw-rate X, Y, Z Global coordinate system x, y, z Local coordinate system v Linear velocity of the robotic lawn mower (along x, same as V x ) V a Velocity along coordinate-axis a {X, Y, Z, x, y, z} A a Acceleration along coordinate-axis a {X, Y, Z, x, y, z} ω j Angular speed of wheel j, j = 1, 2 i j Current on motor j = 1, 2 d = Distance between wheels in meters r = Wheel radius in meters () k Variables or parameters with subscript k denote discrete time ()(t) Variables or parameters followed by (t) denote continuous time g = 9.82 Gravitational acceleration in m/s 2 ˆx The estimated state P State covariance J Motor inertia in kgm 2 K Torque in N m T L j Load torque in N m on motor j = 1, 2 B Friction/damping-constant SP position of imu T O Turn origo, i.e the center of a coordinated turn

11 1 Introduction 1.1 Background Husqvarna Automower consist of a chassis and a cover. The wheels and the blades are attached to the chassis which also carries all electronics such as sensors and motors. The cover is on top of the chassis. The cover is connected to the chassis by magnets which in turn that are equipped with hall effect sensors. If the cover is under an external force the connecting magnets will come loose, an event registered by the hall effect sensor. This is how external forces are detected by Husqvarna Automower today. If the cover could be removed this would reduce production materials and assembling steps ultimately leading to a lower production costs for the company. This master thesis examines alternative ways of detecting external forces to remove the need of a cover. The thesis is performed at Linköping University in collaboration with Husqvarna AB. 1.2 Purpose The purpose of this thesis is to explore and analyze alternative approaches for detecting external forces acting on an robotic lawn mower other than the approach described in Section

12 2 1 Introduction 1.3 Conditions and limitations There is a specific setup of sensors and means of recording on the robotic lawn mower that is used in this thesis. To limit the scope of the work this setup will not be altered with. Methods to be explored and analyzed are thus restricted in the sense that these will be developed from the specific setup and not from a general perspective. Sensors and signals in the current setup are presented and described in Chapter 2. Further the thesis is limited to cases when the robotic lawn mower is driving straight or standing still. 1.4 Objectives The objectives of the thesis are: Develop methods or models that could detect the external forces defined in Chapter 3 with the signals presented in Chapter 2. Document the limitations of developed methods. Examine and discuss how current setup might limit the detection possibilities. 1.5 Method The thesis starts by searching for related previous work that could either inspire or reject ideas on how to solve the problem at hand. Data is then recorded while external forces are both present and not present and while the robotic lawn mower is both standing still and driving straight. The signals are compared to gain knowledge of signal characteristics which can then be used to develop models that accurately relate signals under valid movements but output contradictions while external forces are present, this is called fault diagnosis. Only one of the external forces in Chapter 3 will be present at one time. The recordings are performed in Robot Operating System (ros) with help from employees at Husqvarna AB while analyzes of data and model implementations are conducted in Matlab. Lastly limitations of developed models are discussed. All models assume an online implementation since external forces are supposed to be detected as soon as they occur. 1.6 Method limitation There are infinite many ways on how to apply external forces and hence there is a variety on how these forces effect available signals. However the variety is as-

13 1.7 Related work 3 sumed to have scaling affects meaning that the overall characteristics of a force s influence do not change but the magnitude of said characteristics will vary. In fault diagnosis the magnitudes of various events are nonetheless of importance and as a consequence this could become limitations for some models. The subject itself is within a broad field of signal processing, modeling, estimation and diagnosis, i.e. the subject is not a deeper evaluation or development of existing methods or within a narrow technical field. Thus ideas, developed models and relevant theories are inevitably based on the authors previous knowledge which would mean that there might be other more suitable solutions. Another limitation is time which in this case is approximately 20 weeks. 1.7 Related work Ivanic [6] was the closest match to this work even though the degree might be to low for using as a reference. [6] focuses on using the accelerometer to detect external forces where the main strategy was to compute a moving average of accelerometer readings to detect collision. However the method seemed to be working if the surface was flat and even and not otherwise. Most robotic lawn mowers can track its rotation and thereby detect when the rotation exceeds preset limits to avoid driving in steep terrain that could result in the robotic lawn mower falling over. Ståhl [6] suggests that the robotic lawn mower s rotational limit could used as a detection algorithm for when the robotic lawn mower was lifted. However all cases where a lift does not result in a rotation exceeding the limit would not be detectable. [12] is a master thesis that look into collision detection of a robotic lawn mower at Husqvarna which suggests moving average of electrical currents applied by the motors driving each wheel to detect collision. The main difficulty seemed to be to account for slopes which could lead to harsh collisions before detection. In Sabatini and Genovese [11] the authors discuss how to track vertical motion but compared to this thesis there is an additional measurement of pressure from a barometric altimeter, something that is not available in this case. Other work that could relate to the topic of this thesis is mainly with the purpose to control the robotic lawn mower in a two-dimensional environment and thus not directly related to detecting external forces.

14

15 2 Conditions This section will describe the general characteristics of the robotic lawn mower. These characteristics are: coordinate systems, relevant equipment, kinematic constraints and measurable quantities (i.e. available signals). 2.1 Equipment and software The robotic lawn mower is equipped with four wheels where the two at the back are fixed on the robotic lawn mower and are connected to two separate electrical motors. The back wheel are illustrated as filled black circles or squares when drawn in figures, see Figure 2.1. The two front wheels are orientable wheels or so called Caster-wheels and are illustrated as circles or squares when drawn in figures, see Figure 2.1. As mentioned in Section 1.1 there is a cover on top of the robotic lawn mower connected with magnets to its chassis. When the cover moves in relation to the body the magnets come loose and thus an external force is detected. There is an Inertial Measurement Unit (imu) placed in the middle between the two back-wheels as illustrated in Figure 2.1. What is included in the imu is listed in Table 2.4. Besides the physical aspects described above there is also software of interest. The computer programs running on the robotic lawn mower are written in C++ and implemented with ros. There is also a Proportional, Integral & Differential regulator (pid) implemented. The angular velocity of the wheels are measured from which the linear velocity of 5

16 6 2 Conditions Figure 2.1: Placement of imu each wheel is computed. This result is then compared with the constant desired velocity of the vehicle of 0.3m/s. The input in this feedback system is then the current i. 2.2 Coordinate systems There are two coordinate systems, one is fixed on the robotic lawn mower and will be referred to as local coordinates or lowercase and with origin at the imu s placement, see Section 2.1. Since acceleration and angular velocity are measured by the imu this is also the point on the robotic lawn mower in which the measured acceleration and angular velocity occurs, therefore this will be defined as the origin of the local coordinate system. The local coordinate system is shown in Figure 2.2. The other coordinate system is fixed to the ground and will be referred to as global coordinates or with capital letters. The rotational relation between the two systems are presented in Figure 2.3 where the blue lines represent the local coordinate system and the black lines the global system. y x z x z (a) Side view y (b) Top view Figure 2.2: Illustration of the local coordinate system 2.3 Kinematic constraints To distinguish an external force in the signals a general understanding of the robotic lawn mowers valid movement is needed. When the robotic lawn mower is driving it can only move straight along the local coordinate-axis x and turn by applying different currents to each wheel and thus produce a non-zero yaw-rate

17 2.3 Kinematic constraints 7 y Y ψ, yaw x X z Z (a) Rotation around z results in a yaw-angle also denoted ψ x X θ, pitch z Z y Z φ, roll x Y y Y (b) Rotation around y results in a pitch-angle also denoted θ z X (c) Rotation around x results in a roll-angle also denoted φ Figure 2.3: Rotaion of the local coordinate system relative the global coordinate system ψ. Hence the robotic lawn mower is restricted on the angular velocities φ and θ as well as on the linear velocities along y and z. Consequently the robotic lawn mower should have two degrees of freedom and four nonholomonic constraints [5], [1]. To compute the constraints the kinematics of the robotic lawn mower must be sorted out. To simplify the kinematics computation we will first look at the twodimensional case as seen in Figure 2.4. As previously stated the robotic lawn mower can control its rotation ψ and its translation along x. The reader is referred to Figure 2.4 for notations used in the following calculations. For calculation of the yaw rate ψ lets assume that the robotic lawn mower traveled from time t 0 until time t 1 and during the travel turned ψ radians around T O, which also is the yaw-angle ψ, and thus traveled along the arc s, see Figure

18 8 2 Conditions 2.4. The calculation of ψ thus become and the calculation of v becomes Using (2.2) in (2.1c) yields v = v 1 + v 2 2 = r(w 1 + w 2 ) 2 ds dt = dψ dt R v = ψr v R = ψ (2.1a) (2.1b) (2.1c) / v1 = w 1 2π 2rπ = w / 1r v 2 = w 2 2π 2rπ = w = (2.2a) 2r ψ = w 1r + w 2 r 1 2 R (2.2b) (2.3) Finally R needs to be expressed in known quantities, thus the ratio between v 1 and v 2 is computed. v 1 (2.1b) = v 2 ψr 1 Figure 2.4 = ψr 2 R + d 2 R d 2 v 1 (R d 2 ) = v 2(R + d 2 ) (2.4a) R(v 1 v 2 ) = d 2 (v 1 + v 2 ) 1 R = 2 v 1 v 2 = 2 r(w 1 w 2 ) d v 1 + v 2 d r(w 1 + w 2 ) = 2 w 1 w 2 d w 1 + w 2 (2.4b) (2.4c) Inserting (2.4c) in (2.3) yields ψ = r(w 1 + w 2 ) 2 w 1 w 2 = r(w 1 w 2 ) 2 d w 1 + w 2 d (2.5) In [3] the gravitational force which only has a magnitude in global coordinate Z can be rotated to the local coordinate system by multiplying the rotational matrix Q in (2.6) with the Z-component. cos θ cos ψ cos θ sin ψ sin θ Q = sin φ sin θ cos ψ cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ sin φ cos ψ cos φ cos θ (2.6) Instead of rotating from global coordinates to local coordinates one can rotate the other way around by instead multiply Q 1 with a component of the local system to get it in global coordinates. [3] further states that Q 1 = Q T. and the velocity in the local coordinate system is thus multiplied by Q T. Since the velocity in the local coordinate system is strictly constrained to x the second and third column

19 2.4 Signals 9 of the rotational matrix will be multiplied by 0 and thus the kinematics of the robotic lawn mower can be written as in (2.7). V X V Y V Z cos θ cos(ψ) = cos θ sin(ψ) sin θ ψ = r(w 1 w 2 ) d r(w 1 + w 2 ) 2 Multiplying (2.6) with a vector of global velocities [V X, V Y, V Z ] T yields (2.7a) (2.7b) V x = cos θ cos ψv X + cos θ sin ψv Y sin θv Z V y = (sin φ sin θ cos ψ cos φ sin ψ)v X + (sin φ sin θ sin ψ+ cos φ cos ψ)v Y + sin φ cos θv Z V z = (cos φ sin θ cos ψ + sin φ sin ψ)v X + (cos φ sin θ sin ψ sin φ cos ψ)v Y + cos φ cos θv Z (2.8) However V y and V z should be 0 and thus the two constraints have been computed, these are seen in (2.9). (sin φ sin θ cos ψ cos φ sin ψ)v X + (sin φ sin θ sin ψ+ cos φ cos ψ)v Y + sin φ cos θv Z = 0 (cos φ sin θ cos ψ + sin φ sin ψ)v X + (cos φ sin θ sin ψ sin φ cos ψ)v Y + cos φ cos θv Z = 0 (2.9) The solution to (2.9) is further given in (2.7a). 2.4 Signals The measurable quantities or signals that can be used to perform fault diagnosis on the system is listed in Table 2.1 below

20 10 2 Conditions Table 2.1: Available measured signals on the auto mower. Sensor Accelerometer Gyroscope Quaternions Motor currents Wheel angular velocities Description Placed inside the imu and measures acceleration in x, y and z with a sampling frequency f s of 180Hz. Placed inside the imu and measures angular velocities ψ, φ and θ with a sampling frequency f s of 180Hz. The imu sensor have a quaternions estimation implemented by the manufacturers and is also delivered with a sampling frequency f s of 180Hz. The motor current can be measured with a sampling frequency f s of 18Hz. The angular velocities by the two backwheels are measured with a respective Hall effect sensor, each with a sampling frequency f s of 18Hz.

21 2.4 Signals 11 T O ψ R 2 R R 1 t 1 w 2 v 2 v t 0 s 2 ψ SP v 1 w 1 w 2 v2 s d SP v s 1 w 1 v 1 Figure 2.4: Pitch current relationship figure

22

23 3 Definition of external forces This chapter will describe and specify the external forces that are of interest to detect. Since we are not near any singularities, Euler angles will be used. Moreover they represent the same thing and one need to simply use the definition, see [3] to translate an angle to quaternions or vice versa. Quaternions or Euler angles are needed for calculations of the kinematics and rotations but there would be no difference whether the orientation is due to an external force or ground shape and thus the these signals could not directly be used to detect an external force. The same goes for the angular velocities of the back-wheels though as a result of the feedback controller that keeps certain velocities on each wheel. When discussing movement of the robotic lawn mower it is actually the movement of the imu that is discussed. There should also be noted that each case is simplified to a movement in only one axis or a rotation around only one axis. 3.1 Standing still When no control command has been sent telling the robotic lawn mower to drive there should be no movement at all. Meaning that all available signals should be constant. 13

24 14 3 Definition of external forces Lift front The lift is illustrated in Figure 3.1. When lifted at front there would be a non- X y x y x Z (a) Global coordinate system z (b) Not lifted z (c) Lifted at front Figure 3.1: Illustration of a front lift and a similar though valid case when the robotic lawn mower is standing still. zero angular velocity and the gravitational acceleration would decrease in z and increase in y, thus in conclusion the lift should be possible to detect simply by setting a threshold indicating that one of the signals has changed Lift back Lifting back while standing still would be similar as when lifted at front, there would be a non-zero angular velocity and the gravitational acceleration would decrease in z and increase in y, thus in conclusion the lift should be possible to detect simply by setting a threshold indicating that one of the signals has changed. The lift is illustrated in Figure 3.2. X y x y x Z (a) Global coordinate system z (b) Not lifted z (c) Lifted at back Figure 3.2: Illustration of a back lift and a similar though valid case when the robotic lawn mower is standing still.

25 3.1 Standing still Lift whole Lifting the whole robotic lawn mower would differ from a back or front lift since there would be no angular velocity and thus the gravitational acceleration would not shift either. However the magnitude of the gravitational acceleration would change when lifted, thus in conclusion the lift should be possible to detect simply by setting a threshold indicating that the acceleration has changed. The lift is illustrated in Figure 3.3. X y x y x Z (a) Global coordinate system z (b) Not lifted z (c) Lifted of whole Figure 3.3: Illustration of a whole lift and a similar though valid case when the robotic lawn mower is standing still Lift side Lifting on side while standing still would be similar as to when lifted at front or back. There would be a non-zero angular velocity (though in another axis compared to front and back lift) and the gravitational acceleration would decrease in z and increase in x, thus in conclusion the lift should be possible to detect simply by setting a threshold indicating that one of the signals has changed. The lift is illustrated in Figure 3.4. X y x y x Z (a) Global coordinate system z (b) Not lifted z (c) Lift at side Figure 3.4: Illustration of a side lift and a similar though valid case when the robotic lawn mower is standing still.

26 16 3 Definition of external forces Collision A collision while standing still would be that something has hit the automover, depending on the impact acceleration and gyroscope data would be affected. Thus in conclusion the collision should be possible to detect simply by setting a threshold indicating that one of the signals has changed Wheel slip A wheel slip while standing still would mean that the robotic lawn mower has started to slide in some direction. The lift should be possible to detect simply by setting a threshold indicating that one of the signals has changed. 3.2 Driving In contrast to when the robotic lawn mower is standing still the signals will be changing while driving Lift front The lift is illustrated in Figure 3.5. When the robotic lawn mower start to climb a hill the local coordinate system is shifted with the pitch angle and as a result of the kinematics in (2.7) the local velocity in x would then have components in both Z and X, i.e the robotic lawn mower should move along Z and X. When the robotic lawn mower is lifted in front however there will be no movement along Z and thus there is a contrast which might be detectable if a velocity is produced with (2.7) and compared with how the movement looks according to the accelerometer. Concerning the motor currents there should be a higher demand when the robotic lawn mower climbs a hill to obtain constant speed. Therefore is seems likely that there is a relation between the motor currents and the angle θ. If a sufficient relation between the currents and θ would be determined deviation could be detected and assumed to be caused by an external force. However the relation could also be subjected to ground surface, e.g. a hard dry surface without grass would demand less current than soft wet moss. Another impeding aspect could be that the lift hinders the robotic lawn mower to move forward and thus results in a higher demand of current similar to a valid case.

27 3.2 Driving 17 E F D A V XB B C (a) Driving up over bump V Z θ V X V x (b) Driving when lifted at front Figure 3.5: Illustration of a front lift and a similar though valid case when the robotic lawn mower is driving forward Lift back The lift is illustrated in Figure 3.6. Similar to the case a climb down a hill would with the kinematics in (2.7) result in the local velocity to have components in both Z and X, i.e the robotic lawn mower should move along Z and X. When the robotic lawn mower is lifted at back however there will a positive velocity along Z, which should be negative since the nose of the robotic lawn mower is pointing down. Additionally since the two back wheels are lifted of the ground the robotic lawn mower will stop even though the wheels are still turning. Thus there are two contrast which might be detectable if a velocity is produced with (2.7) and compared with how the movement looks according to the accelerometer. Concerning the motor currents there should be a lower demand when the robotic lawn mower drives down a hill to obtain constant speed. Again indicating a relation between the motor currents and the angle θ. If a sufficient relation between the currents and θ would be determined deviation could be detected and assumed to be caused by an external force. However for this case when both wheels are off the ground and the demanded current would go down similar to when when the robotic lawn mower is driving down a hill which could mean that the contrast between the valid case and the lift is not sufficiently significant.

28 18 3 Definition of external forces B v A C F E D (a) Driving up over bump V Z θ V X V x (b) Driving when lifted at front Figure 3.6: Illustration of a front lift and a similar though valid case when the robotic lawn mower is driving forward Lift whole When lifting the whole robotic lawn mower straight in Z all angles and angular velocities would remain the same. The accelerometer would show a peak in z corresponding to the lift and the wheel current would drop. The contrasts here would be that the according to the kinematics the robotic lawn mower is still driving straight forward but the accelerometer readings would indicate a stop similar to The relation between motor current and pitch angle would say that the current should remain the same since there is no pitch angle but the wheel currents still drop since they do not have traction to the ground Lift side Lifting at a side would mean that one wheel loses traction to the ground and the corresponding motor s current would drop and the other motor s current would increase. A side lift would also mean that the rotation is around the opposite wheel compared to the side the lift occurs. Therefore this should show as a movement in y which would not be permitted under normal conditions.

29 3.2 Driving Collision There should be an acceleration in the opposite direction of the force, if it is a front collision stopping the automover there should be a negative acceleration in x. The wheel current would go up since the feedback controller is trying to make up the external force hindering the robotic lawn mower Wheel slip Similar as for the case of collision there should be an acceleration in the opposite direction of the current heading. The electrical current would go down since the grip has come lose and it does not take as much current to drive the wheels at a certain velocity.

30 20 3 Definition of external forces 3.3 Fault case summary The fault cases are summarized with respective characteristics in table 3.1. Proposed methods are also included to indicate what methods could detect what fault case. For further explanation of the methods the reader is referred to Chapter 4.

31 3.3 Fault case summary 21 Table 3.1 Driving Force Characteristic(s) Detection method Odometry Load Frequency Change in signals No Lift front Angle change X X Lift back Angle change X X Lift whole Angle change X X Lift side Angle change X X Collision Acceleration change X Wheel slip Acceleration change X Yes Lift front Pitch angle non-zero but no V Z. Pitch change not corresponing to change in demand of electrical current X X Lift back Pitch angle non-zero but no V Z. Stop in V X. Pitch change not corresponing to change in demand of electrical current X X X Lift whole Stop in V X. Change in demand of electrical current but no change in pitch. X X X Lift side Demand of electrical current different of each wheel. X X X Collision Stop in V X. Change in demand of electrical current but no change in pitch. X X Wheel slip Stop in V X. Change in demand of electrical current but no change in pitch. X X

32

33 4 Detection methods Given the definition of the external forces and their signal characteristics in Chapter 3 and the constraints and knowledge of the robotic lawn mowers proper movement this chapter investigate three ideas an external force might be detected. All methods would ultimately create residuals based on probabilistic diagnosis since there are measurement noise present, see [2]. The distribution of these will be assumed Gaussian, with expected value (µ) and variance (σ) determined by their definitions taken from [7] and seen in (4.1b) and (4.1c) respectively. The expected value and variance will be calculated from data not influenced by external forces. r i N(µ i, σ i ) µ i = E(r i ) = 1 N (4.1a) N r i,n i = 1, 2 (4.1b) n=1 σ i = E((r i µ i ) 2 ) = E(r 2 i ) 2µ i + µ 2 i = 1 N N ri,n 2 2µ i + µ 2 i i = 1, 2 (4.1c) The residuals created should have the expected value of zero and thus a Hypothesis test can be constructed according to (4.2). Moreover the variance, σ, will be assumed the same whether an external force is acting on the robotic lawn mower or not meaning that the expected values is what is going to be altered by an external force. n=1 H 0 :r N(0, σ) No external force (4.2a) H 1 :r N(µ 0, σ) External force (4.2b) 23

34 24 4 Detection methods The next step is to select a threshold so that the probability of both false and missed detection is low. This is a trade off since a lower threshold would result in more detections and thereby more false alarms whereas a higher threshold would give less detection and thus more missed detections. For the robotic lawn mower it is crucial to detect actual lifts meaning that false alarm is preferred over missed detection. Let α denote the probability of false detection and T denote the threshold, the relation between these two is then according to (4.3). The equation states that the probability of the residual being larger than the threshold provided that no actual external force is present, i.e. a false detection, equals a desired value of α. Since false detection is preferred over missed detection a relative high alpha is to be selected. [2] 1 α 2 α = P ( r > T µ = 0) α = P (r < T µ = 0) + P (r > T µ = 0) α = 2P (r > T µ = 0) α = 2(1 P (r < T µ = 0)) (4.3a) (4.3b) (4.3c) (4.3d) = P (r < T µ = 0) (4.3e) (4.3) is then standardized to µ = 0 and σ = 1 by subtracting µ and dividing by the standard deviation, σ, from both sides of the inequality. The result is seen in (4.4). 1 α ( r 2 = P < T ) µ = 0 (4.4) σ σ When α is selected the threshold T can be computed by using a normal distribution table which relates probabilities with thresholds for a stochastic variable with standard normal distribution. 4.1 Odometry One idea would be to estimate the velocity of the imu along the global vertical axis X, Y and Z and then insert the estimated signals in the constraints given in (2.9). With current signals there are two ways to compute or estimate V X,V Y and V Z. The accelerometer values could be rotated to global coordinates and then integrated, however the constraint in (2.9) are in fact a rotation of global to local coordinates and thus the constraint could be checked directly by integrating the accelerometer data to get the local velocities, where V y and V z should be zero. The second way would be to compute the velocities with the kinematics equation, (2.7), but once again the constraints are simply rotating these back to local velocities meaning that this would result in a rotation to global velocities and then back to local, a procedure where no deviations would be seen. It seems plausible to use a filter to estimate the global velocities such as a Kalman filter since there

35 4.1 Odometry 25 are two independent ways to compute global velocities. However the kinematics equation will not provide valid results if there is an external force present. Moreover it is the contrast between ways to obtain global velocities that can be used to detect external forces and in a Kalman filter the information of both would be used resulting in a vanished contrast between valid and invalid movement. Therefore no Kalman filter will be used. Before the accelerometer data can be used the gravitational influence needs to be removed. The gravitational components in the local coordinate system can be calculated by multiplying g with Q in (2.6) and thus produce the local accelerations x x 0 y = y Q 0 (4.5) z z g There is also biases on the signals which should be taken into account. The bias of the signal will be considered as the mean of the data of each signal collected from when the robotic lawn mower is standing still. Since the data is sampled a discretization of the integration is needed. Using zero-order-hold as in [9] the integration becomes as shown in (4.6). V k = V k 1 + A k T s (4.6) Integrate accelerometer data As previously claimed the velocities V X,V Y and V Z could be calculated by first rotate the accelerometer data (A x,a y and A z ) with Q T to obtain A X,A Y and A Z which would then be integrated to get the global velocities. On the other hand the constraint is simply a rotation back to local coordinates and the test would only need to integrate accelerometer data to see if there is velocities in y or z are not zero.

36 26 4 Detection methods 4.2 Pitch current relationship Another idea is to try and find the relation between motor currents and the pitch angle θ. It should be noted that a feedback controller is implemented to keep the robotic lawn mower at desired local velocity which will complicate or perhaps impede a sufficient estimation Pitch and current in static relationship The idea with this model is that when the wheels are not affected by an external force the only thing that would demand more or less current to keep the same speed is the pitch angle, provided that no turning is performed. Another thing that could influence needed current is the ground surface. For instance wet moss would require higher currents than dry wood to keep the same speed. However the influence of a turn is neglected and the data is naturally collected when no turn is performed. Therefore a simple relation between the motor currents and the pitch angle based on force equilibrium seen in [10] is presented below. Moreover the model is only assumed to be true if the robotic lawn mower is moving forward. k = 1,..., N Ki k = mg sin(θ k ) + F 0 + e k (4.7) e k N(0, σ e,k ) K is a constant that multiplied by i k result in the force driving the robotic lawn mower forward. The first term on the right hand side of (4.7) is the influence of the gravitation, F 0 is the fundamental inertia of the wheels and e k is assumed to be white Gaussian noise. If θ k is assumed fixed the model becomes linear and (4.7) can be rewritten as. where i k = aa k + F + e k (4.8) a = 1 K A k = mg sin(θ k ) F = F 0 K Measurement of i k and A k for k = 1,..., N yields i 1 A 1 1 ( ) e 1 a. =. + F. A N 1 i N e N (4.9a) (4.9b) (4.9c) (4.10)

37 4.2 Pitch current relationship 27 and then using vector and matrix notations i 1 Y =. i N A 1 1 H =. A N 1 ( ) a Θ j = F e 1 e j =. e N (4.11a) (4.11b) (4.11c) (4.11d) the model given in (4.8) can be written as Y = HΘ + e (4.12) where the noise e is uncorrelated and the noise vector e thus have the standard deviation R shown in (4.13). e,1 R =... σ σ e,n (4.13) To estimate Θ the Least Square (LS) method will be used as defined in [3]. The cost function V is defined as and the estimation of Θ becomes Solving V (Θ) for Θ as shown below. V (Θ) = (Y HΘ) T (Y HΘ) (4.14) ˆΘ = arg minv (Θ) (4.15) Θ V (Θ) = (Y HΘ) T (Y HΘ) = Y T Y Y T HΘ Θ T H T Y + Θ T H T HΘ = Y T Y 2Θ T H T Y + Θ T H T HΘ (4.16a) (4.16b) (4.16c) (4.16c) is then derived with respect to Θ and set equal to 0 to find the minimum

38 28 4 Detection methods of V (Θ), these calculations are performed below. dv dθ = 2HT Y + 2H T HΘ = 0 H T HΘ = H T Y ˆΘ = (H T H) 1 H T Y (4.17a) (4.17b) (4.17c) The second derivative of (4.16c) is d 2 V dθ 2 = 2HT H > 0 (4.18) which is always larger than zero since it is a square product and thus (4.14) will have a minimum for the estimated Θ. If the data have been generated by the model for true parameter values Θ o then the estimation in (4.17c) becomes and thus ˆΘ = (H T H) 1 H T Y = (4.19a) = (H T H) 1 H T (HΘ o + e) = (4.19b) = Θ o + (H T H) 1 H T e (4.19c) E( ˆΘ j ) = Θ o j (4.20a) Cov( ˆΘ j ) = (H T H) 1 (H T RH)(H T H) 1 (4.20b) Three sets of data are concatenated and will be used to estimate Θ. To smooth out the disturbances on each current and make the estimation less influenced by outliers the two currents sent to the left and right wheel respectively will be added up and then divided by two to get the mean current for each time instance. This approach should be valid since the thesis is restricted to driving straight and thus the model parameters in (4.7) should be the same for each motor. The concatenated data of current and gravitational influence (A k in (4.9b)) is seen in Figure 4.1a respectively 4.1b. From the estimated Θ a current is simulated with data from another set other than the data used for estimation and a residual is calculated as defined in (4.21). The result is seen in Figure 4.2. A residual can then be computed as r = Y H ˆΘ (4.21)

39 4.2 Pitch current relationship 29 (a) Mean current of left and right motor used in estimation of Θ in (4.19a) (Y) (b) Gravitational effect used in estimation of Θ in (4.19a) Figure 4.1: Data used for estimation of ˆΘ

40 30 4 Detection methods Figure 4.2: Validation of the estimation of ˆΘ Pitch and current dynamic relationship There is a feedback controller setting the current to keep desired speed which would mean that the current is depending on previous values and thus the static model in (4.7) might not be sufficient. To see if there is a dynamic relation between the current and pitch an Auto-Regressive with an extra input (ARX) -model, see [4], will be modeled. The general ARX-model using the same force equilibrium that was used when setting up (4.7) is presented in (4.22) i k + + a n i k n = b 1 (mg sin(θ k ) c) + + b m (mg sin(θ k m ) c) + F 0 + e(t) (4.22) where e(t) is white noise, c is a time delay and n and m are not yet decided model orders. The delay c is here placed outside sin because mgsin(θ) will be seen as a fixed value to keep the model linear. Using the notations ϕ k = (i k 1... i k n mg sin(θ k ) c... mg sin(θ k m ) c 1) T (4.23a) Θ = (a 1... a n b 1... b m F 0 ) (4.23b) a prediction îk of i k can be formulated as end thus the prediction error is defined as ˆ i k = ϕ T k Θ (4.24) i k ϕ T k Θ (4.25)

41 4.2 Pitch current relationship 31 The estimated model would have two inputs, mg sin(θ) and F 0, which will be referred to as u 1 and u 2 respectively. The estimation is performed by varying the model orders n, m and the delay for u 1 where the combination that results in the lowest prediction error are selected. In a general sense there could also be a model order above one and a delay on u 2, however this is a constants, not depending on previous values, representing a fundamental ampère need to drive thus have an immediate effect on the current.

42 32 4 Detection methods 4.3 Frequencies The last idea is to look at frequencies of the data where there might be some damping on the signals when simply holding the robotic lawn mower. In order to detect a shift in frequency contents an analysis needs to be performed on intervals of data, i.e. compute spectrograms, see [8]. A spectrogram shows the energy for different frequencies at different times. Since the data is sampled the spectrogram is computed by dividing the signal in equally long segments and then compute the Discrete Fourier Transform (dft) on each segment, for computational advantages the implementation uses Fast Fourier Transform (fft) instead or dft. The energy is then calculated by squaring each frequency component in each segment. A shorter segment length would give good time resolution but poor frequency resolution and vice versa for long segments, [4]. It should be noted that the most significant frequency content change was seen in the currents which is why the analysis and graphs depict the currents and no other signals.

43 5 Evaluation & results This chapter will summarize the findings and results of the proposed detection algorithms described in Chapter Method evaluation Odometry Integration of the accelerometer data for four different data sets are shown in Figures 5.1, 5.2, 5.3 and 5.4. The significant drifting seen in Figures 5.1, 5.2, 5.3 and 5.4 results in poor method results meaning that this can not directly be used to detect external forces. 33

44 34 5 Evaluation & results Figure 5.1: Integrated accelerometer data, no lift Figure 5.2: Integrated accelerometer data, lift back and then front

45 5.1 Method evaluation 35 Figure 5.3: Integrated accelerometer data, lift front and then back Figure 5.4: Integrated accelerometer data, lift right side then left side and then lift of whole

46 36 5 Evaluation & results Pitch current relationship Estimation according to (4.17c) with the concatenated data in Figure 4.1 results in ( ) 13.1 ˆΘ (5.1) Using the definition of Θ in (4.11c) and further the definition of a and F in (4.9c) and (4.9a) the original parameters are presented in (5.2). K 0.08 F (5.2) A mean current is simulated with (4.12) and from pitch data where an external force have been applied and the results are shown in Figure 5.5. The comparison Figure 5.5: True mean current and simulated current when lifted for static model between true and simulated current are done with three concatenated data sets. The first data set, to the left of the left green line in Figure 5.5 have two lifts, first it is lifted at back around sample 250 and then it is lifted at front around sample 600. The second data set, in between the two green lines have a font lift around sample 1200 in Figure 5.5 and is again lifted at back around sample The third data set is first lifted at one side around sample 2100 in Figure 5.5 and then at the other side around sample 2300 and lastly the whole robotic lawn mower was lifted around sample 2600.

47 5.1 Method evaluation 37 A residual is then computed with (4.21) and its distribution is calculated with (4.1b) and (4.1c) from the validation data seen in 4.2. The resulting distribution of r is presented in (5.3) r N(0, 62) (5.3) Using the estimated distribution with (4.4) a diagnosis variable is computed which is zero for all samples where the inequality in (4.4) holds and else non-zero. The last parameter to be decided is α. As previously stated it is preferred to have false detections over missed detection meaning that probability for false detection, α, can be selected relatively high. A common probability for false detection is 5% which corresponds to a threshold of This threshold is then implemented in (4.4) for two datasets, one of which is presented in Figure 5.5 where external forces are present and the other is presented in Figure 4.2 where no external forces are present, the resulting residual and diagnosis variable are presented in Figure 5.6 As seen in 5.6 all external forces are detected. However there are a false detections at approximately every 500 samples at best which corresponds to an alarm about every 3 second. To compensate for this the threshold is increased meaning that the probability for false detection is decreased and the probability for missed detection is increased. The first lift at the back around sample 250 is missed for even small increases in threshold indicating that a lift back perhaps could not be detected with current algorithm. On the other hand the algorithm still detects all lifts at front, of the whole robotic lawn mower, one of lifts at back and one side lift even as α approaches 0 indicating that the algorithm has robust detection for front lifts and lifts of whole, see Figure 5.7. The fact that one lift back is not detected is plausibly due to the lift mirroring the effects a slope with similar inclination would have, the same goes for the one side lift that is missed. Using the same data as in Figure 5.5 but with the dynamic relation is presented in 5.8. The resulting prediction is almost the same as the simulated data from the static case. One difference is the residual at the back lift around sample 250, on the other hand the back lift at around sample 1600 is not more visible than for the static case and there are a few more peaks where there should not be. In conclusion the most important information could apparently be described by the simpler static model.

48 38 5 Evaluation & results (a) Residual and diagnosis variable for data affected by external forces (b) Residual and diagnosis variable for data not affected by external forces Figure 5.6: True mean current and simulated current when lifted

49 5.1 Method evaluation 39 (a) Residual and diagnosis variable for data affected by external forces (b) Residual and diagnosis variable for data not affected by external forces Figure 5.7: True mean current and simulated current when lifted

50 40 5 Evaluation & results Figure 5.8: Prediction and its error from an ARX(6 [11 1] [5 0]) model Frequencies Uneven and rough surface materials give the motor currents a volatile behavior since there is a constant change in demand of current to keep a desired speed. When a wheel is lifted the ground surface no longer influence the demand and thus the current should be somewhat constant. The data in Figure 5.9 have a lift on the right hand side around sample 250, on the left hand side around sample 500 and the whole robotic lawn mower is lifted around sample 800. The lifts can be seen as a flat part of the signal for that wheel which has been off the ground. Consequently higher frequencies should have been damped and only low frequencies should dominate. A detection could then either be a lack of high frequency components or increasing intensity for lower frequencies. The trade off between frequency and time resolution then plays an important role. From the detection perspective a force should be detected as soon as possible and therefore three different segment sizes have been tested and are shown in Figure 5.10 to Now the sampling frequency was 18Hz for the currents meaning that the first lift around sample 250 would have the approximate time 250/18/ minutes, the left lift would similar have the time 0.48 and the last lift One can visually make out these areas in the spectrogram graphs but the lifts seems to damp all frequencies and not only higher frequencies. The damping is however not significant enough thus in order to make this method viable for detection higher sampling rate would be needed.

51 5.1 Method evaluation 41 (a) Left current (b) Right current Figure 5.9: Currents when the robotic lawn mower is lifted on the right side and left side and whole in that order

52 42 5 Evaluation & results (a) Left current (b) Right current Figure 5.10: Spectrogram with window of 10 samples

53 5.2 Standing still 43 (a) Left current (b) Right current Figure 5.11: Spectrogram with window of 20 samples 5.2 Standing still While standing still all signals should remain constant and a lift would simply be detected by setting a threshold on the gyroscope data for all cases except for a lift

54 44 5 Evaluation & results (a) Left current (b) Right current Figure 5.12: Spectrogram with window of 30 samples of the whole robotic lawn mower in which case the accelerometer data should be viewed.

55 5.3 Driving Driving The method modeling a pitch current relationship that proved to have fairly good performance with current conditions. This method had the most significant deviations in its residuals when a front lift and a lift of the whole robotic lawn mower was conducted but not as reliable for back and side lifts which means that the method would need a complimentary detection method. The estimation of global velocities could be significantly improved with additional measurements and the frequency method would need a higher sampling frequency.

56

57 6 Conclusions 6.1 Summary Available sensors and proposed methods could not detect all fault cases although a future complete solution seems plausible. When the robotic lawn mower is standing still all external forces should be detectable by simply using thresholds where a change in any signal would indicate that an external force is present. For the odometry method estimates were too poor to be used in constraints on valid movement meaning that no fault cases could be detected with the method. The primary problem was that estimates are strongly affected by disturbances. With additional ways of measuring either velocity of position the method could be significantly improved since more reliable estimation methods such as a Kalman filter could be used. The pitch angle relationship proved to be the best with robust detection for some cases but would need to be complemented for constructing an adequate detection system. This method showed little drawbacks from available sensors meaning that the method cannot be improved as much as for instance the odometry model. The frequency method showed too little difference between valid cases and cases where external forces where present to provide useful results. Better sensors with higher sampling frequency might not however make the methods cost effective. It is the author s believe that an adequate method can be achieved with a combi- 47