Detecting External Forces on an Autonomous Lawnmowing Robot with Inertial, Wheel Speed and Wheel Motor Current Measurements

(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)

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: Oskar Ljungqvist

isy_{, Linköpings universitet}

Mikaela Åhlén

Husqvarna AB

Examiner: Fredrik Gustafsson

isy_{Linköpings universitet}

Division of Automatic Control Department of Electrical Engineering

(3)

Abstract

An autonomous lawn mowing robot moves around randomly within an area en-closed 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 de-scribed 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 vio-lates 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 addi-tional signals to fully perform satisfactorily. With addiaddi-tional 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, some-thing that might not be as cost efficient as needed to make the method profitable.

(4)

(5)

Acknowledgments

The thesis suggestion was provided by Husqvarna AB and I would like to give spe-cial 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

(6)

(7)

3 Definition of external forces 13 3.1 Standing still . . . 13 3.1.1 Lift front . . . 14 3.1.2 Lift back . . . 14 3.1.3 Lift whole . . . 15 3.1.4 Lift side . . . 15 3.1.5 Collision . . . 16 3.1.6 Wheel slip . . . 16 3.2 Driving . . . 16 3.2.1 Lift front . . . 16 3.2.2 Lift back . . . 17 3.2.3 Lift whole . . . 18 3.2.4 Lift side . . . 18 3.2.5 Collision . . . 19 3.2.6 Wheel slip . . . 19

3.3 Fault case summary . . . 20

(8)

4 Detection methods 23

4.1 Odometry . . . 24

4.1.1 Integrate accelerometer data . . . 25

4.2 Pitch current relationship . . . 26

4.2.1 Pitch and current in static relationship . . . 26

4.2.2 Pitch and current dynamic relationship . . . 30

4.3 Frequencies . . . 32

5 Evaluation & results 33 5.1 Method evaluation . . . 33

5.1.1 Odometry . . . 33

5.1.2 Pitch current relationship . . . 36

5.1.3 Frequencies . . . 40 5.2 Standing still . . . 43 5.3 Driving . . . 45 6 Conclusions 47 6.1 Summary . . . 47 6.2 Future work . . . 48 Bibliography 49

(9)

(10)

Notation

Notations that will be used in the report Notation Description Ts 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 Vx)

Va Velocity along coordinate-axis a ∈ {X, Y , Z, x, y, z}

Aa Acceleration along coordinate-axis a ∈ {X, Y , Z, x, y, z}

ωj Angular speed of wheel j, j = 1, 2

ij Current on motor j = 1, 2

d = 0.04645 Distance between wheels in meters

r = 0.1225 Wheel radius in meters

()k Variables or parameters with subscript k denote

dis-crete time

()(t) Variables or parameters followed by (t) denote contin-uous time

g = 9.82 Gravitational acceleration in m/s2 ˆ

x The estimated state

P State covariance

J Motor inertia in kgm2

K Torque in N m

T Lj Load torque in N m on motor j = 1, 2

B Friction/damping-constant

SP position of imu

(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 sen-sors 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 de-tected 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 de-tecting external forces acting on an robotic lawn mower other than the approach described in Section 1.1.

(12)

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 possibili-ties.

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 mod-els that accurately relate signals under valid movements but output contradic-tions 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 as-sume 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

(13)

as-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 sub-ject 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 de-tect 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 pre-set 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 de-tection of a robotic lawn mower at Husqvarna which suggests moving average of electrical currents applied by the motors driving each wheel to detect colli-sion. 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 ad-ditional 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 con-straints 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 calledCaster-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 inC++

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

(16)

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 aslocal 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 asglobal 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.

x

z y

(a)Side view

x

y

⊗z

(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 X Y Z x y z ψ, yaw

(a)Rotation around z results in a yaw-angle also denoted ψ Z X Y z x y θ, pitch

(b)Rotation around y results in a pitch-angle also denoted θ Y Z X x y z φ, roll

(c) Rotation around x results in a roll-angle also denoted φ

Figure 2.3:Rotaion of the local coordinate system relative the global coordi-nate 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 two-dimensional 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 calcula-tions. For calculation of the yaw rate ˙ψ lets assume that the robotic lawn mower

traveled from time t0until time t1and during the travel turned ψ radians around

(18)

2.4. The calculation of ˙ψ thus become ds dt = dψ dt R ⇔ (2.1a) v = ˙ψR ⇔ (2.1b) v R = ˙ψ (2.1c)

and the calculation of v becomes

v = v1+ v2 2 ,v1 =_2πw12rπ = w1r v2 =_2πw22rπ = w2r , = (2.2a) = r(w1+ w2) 2 (2.2b) Using (2.2) in (2.1c) yields ˙ ψ = w1r + w2r 2 1 R (2.3)

Finally R needs to be expressed in known quantities, thus the ratio between v1

and v2is computed. v1 v2 (2.1b) = ψR˙_˙ 1 ψR2 Figure2.4 = R + d 2 R − d₂ ⇔_v₁_{(R −}d 2) = v2(R + d 2) ⇔ (2.4a) R(v1−v2) = d 2(v1+ v2) ⇔ (2.4b) 1 R = 2 d v1−v2 v1+ v2 = 2 d r(w1−w2) r(w1+ w2) = 2 d w1−w2 w1+ w2 (2.4c) Inserting (2.4c) in (2.3) yields ˙ ψ = r(w1+ w2) 2 2 d w1−w2 w1+ w2 = r(w1−w2) 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.

Q =        

cos θ cos ψ cos θ sin ψ −_{sin θ} 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 = QT. and the velocity in the local coordinate system is thus multiplied by QT. 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).

        VX VY VZ         =         cos θ cos(ψ) cos θ sin(ψ) −_{sin θ}         r(w1+ w2) 2 (2.7a) ˙ ψ = r(w1−w2) d (2.7b)

Multiplying (2.6) with a vector of global velocities [VX, VY, VZ]T yields

Vx= cos θ cos ψVX+ cos θ sin ψVY −sin θVZ

Vy= (sin φ sin θ cos ψ − cos φ sin ψ)VX+ (sin φ sin θ sin ψ+

cos φ cos ψ)VY + sin φ cos θVZ

Vz= (cos φ sin θ cos ψ + sin φ sin ψ)VX+ (cos φ sin θ sin ψ−

sin φ cos ψ)VY+ cos φ cos θVZ

(2.8)

However Vyand Vzshould be 0 and thus the two constraints have been computed,

these are seen in (2.9).

(sin φ sin θ cos ψ − cos φ sin ψ)VX+ (sin φ sin θ sin ψ+

cos φ cos ψ)VY+ sin φ cos θVZ = 0

(cos φ sin θ cos ψ + sin φ sin ψ)VX+ (cos φ sin θ sin ψ−

sin φ cos ψ)VY+ cos φ cos θVZ = 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)

Table 2.1:Available measured signals on the auto mower. Sensor Description

Accelerometer Placed inside the imu and measures ac-celeration in x, y and z with a sampling frequency fsof 180Hz.

Gyroscope Placed inside the imu and measures an-gular velocities ˙ψ, ˙φ and ˙θ with a

sam-pling frequency fsof 180Hz.

Quaternions The imu sensor have a quaternions esti-mation implemented by the manufactur-ers and is also delivered with a sampling frequency fsof 180Hz.

Motor currents The motor current can be measured with a sampling frequency fsof 18Hz.

Wheel angular velocities The angular velocities by the two back-wheels are measured with a respective Hall effect sensor, each with a sampling frequency fsof 18Hz.

(21)

2.4 Signals 11

t

0

s

1

s

2

w

1

v

1

w

2

v

₂ SP

v

t

1

w

1

v

1

w

2

v

2 SP

v

T O d R R1 R2

ψ

(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 move-ment 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.

(24)

3.1.1 Lift front

The lift is illustrated in Figure 3.1. When lifted at front there would be a

non-X

Z

(a)Global co-ordinate sys-tem x z y (b)Not lifted x z y (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.

3.1.2 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 crease in z and increase in y, thus in conclusion the lift should be possible to de-tect simply by setting a threshold indicating that one of the signals has changed. The lift is illustrated in Figure 3.2.

X

Z

(a)Global co-ordinate sys-tem x z y (b)Not lifted x z y (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 15

3.1.3 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

Z

(a)Global co-ordinate sys-tem x z y (b)Not lifted x z y (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.

3.1.4 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 com-pared 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

Z

(a)Global co-ordinate sys-tem x z y (b)Not lifted y z ⊗x (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)

3.1.5 Collision

A collision while standing still would be that something has hit the automover, de-pending on the impact acceleration and gyroscope data would be affected. Thus in conclusion the collision should be possible to detect simply by setting a thresh-old indicating that one of the signals has changed.

3.1.6 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.

3.2.1 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 de-tected 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 A _V_X B B C D E F

(a)Driving up over bump

VZ

VX

Vx

θ

(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.

3.2.2 Lift back

The lift is illustrated in Figure 3.6.

Similar to the case 3.2.1 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 be-tween 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 signifi-cant.

(28)

F E D

C

B _v A

(a)Driving up over bump

VZ

VX

Vx

θ

(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.

3.2.3 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 3.2.2. 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.

3.2.4 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 move-ment in y which would not be permitted under normal conditions.

(29)

3.2 Driving 19

3.2.5 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.

3.2.6 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)

3.3 Fault case summary

The fault cases are summarized with respective characteristics in table 3.1. Pro-posed methods are also included to indicate what methods could detect what fault case. For further explanation of the methods the reader is referred to Chap-ter 4.

(31)

3.3 Fault case summary 21 Table 3.1

Driv-ing

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 Colli-sion Acceleration change X Wheel slip Acceleration change X Yes Lift front

Pitch angle non-zero but no VZ. Pitch change

not corresponing to change in demand of electrical current X X Lift back

Pitch angle non-zero but no VZ. Stop in VX.

Pitch change not corresponing to change in demand of electrical current X X X Lift whole Stop in VX. 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 Colli-sion Stop in VX. Change in demand of electrical current but no change in pitch. X X Wheel slip Stop in VX. Change in demand of electrical current but no change in pitch.

(32)

(33)

4

Detection methods

Given the definition of the external forces and their signal characteristics in Chap-ter 3 and the constraints and knowledge of the robotic lawn mowers proper movement this chapter investigate three ideas an external force might be de-tected.

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 ex-pected value and variance will be calculated from data not influenced by external forces. ri ∼N (µi, σi) (4.1a) µi = E(ri) = 1 N N X n=1 ri,n i = 1, 2 (4.1b) σi = E((ri−µi)2) = E(r2i) − 2µi+ µ2i = 1 N N X n=1 r_i,n2 −_2µ_i_{+ µ}2 i i = 1, 2 (4.1c)

The residuals created should have the expected value of zero and thus a Hypoth-esis 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 exter-nal force.

H0:r ∼ N (0, σ ) No external force (4.2a)

H1:r ∼ N (µ , 0, σ ) External force (4.2b)

(34)

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]

in (4.4). 1 −α 2 = P r √ σ < T √ σ|µ = 0 ! (4.4) When α is selected the threshold T can be computed by using a normal distri-bution 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 VX,VY and VZ.

The accelerometer values could be rotated to global coordinates and then inte-grated, 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 Vy and Vz 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 ve-locities meaning that this would result in a rotation to global veve-locities 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. More-over 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 accelera-tions         x y z         =         x y z         −_{Q ∗}         0 0 g         (4.5) 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).

Vk = Vk−1+ Ak∗Ts (4.6)

4.1.1 Integrate accelerometer data

As previously claimed the velocities VX,VY and VZ could be calculated by first

rotate the accelerometer data (Ax,Ay and Az) with QT to obtain AX,AY and AZ

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)

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.

4.2.1 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. More-over the model is only assumed to be true if the robotic lawn mower is moving forward.

K ik = mg sin(θk) + F0+ ek _ek = 1, . . . , N

k ∼N (0, σe,k) (4.7)

K is a constant that multiplied by ik 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, F0is the fundamental inertia of the wheels and ek is assumed to

be white Gaussian noise.

If θkis assumed fixed the model becomes linear and (4.7) can be rewritten as.

ik = aAk+ F + ek (4.8) where a = 1 K (4.9a) Ak = mg sin(θk) (4.9b) F = F0 K (4.9c)

Measurement of ikand Ak for k = 1, ..., N yields

          i1 .. . iN           =           A1 1 .. . AN 1           a F ! +           e1 .. . eN           (4.10)

(37)

4.2 Pitch current relationship 27

and then using vector and matrix notations

Y=           i1 .. . iN           (4.11a) H=           A1 1 .. . AN 1           (4.11b) Θ_j= a_F ! (4.11c) ej=           e1 .. . eN           (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). R=           σe,1 . .. σe,N           (4.13)

To estimate Θ theLeast Square (LS) method will be used as defined in [3]. The

cost function V is defined as

V (Θ) = (Y − HΘ)T(Y − HΘ) (4.14) and the estimation of Θ becomes

ˆ

Θ= arg min

Θ

V (Θ) (4.15)

Solving V (Θ) for Θ as shown below.

V (Θ) = (Y − HΘ)T(Y − HΘ) = (4.16a)

YTY − YTHΘ − ΘTHTY+ ΘTHTHΘ= (4.16b) YTY −2ΘTHTY+ ΘTHTHΘ (4.16c)

(38)

of V (Θ), these calculations are performed below. dV dΘ = −2H T_Y_{+ 2H}T_HΘ_{= 0 ⇔} _(4.17a) HTHΘ= HTY ⇔ (4.17b) ˆ Θ= (HTH)−1HTY (4.17c)

The second derivative of (4.16c) is

d2V dΘ2 = 2H

T_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

ˆ Θ= (HTH)−1HTY= (4.19a) = (HTH)−1HT(HΘo+ e) = (4.19b) = Θo+ (HTH)−1HTe (4.19c) and thus E( ˆΘ_j) = Θo_j (4.20a) Cov( ˆΘ_j) = (HTH)−1(HTRH)(HTH)−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 (Akin (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

(39)

(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)

(40)

Figure 4.2:Validation of the estimation of ˆΘ

4.2.2 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 be-tween the current and pitch anAuto-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)

ik+ · · · + anik−n= b1(mg sin(θk) − c) + · · · + bm(mg sin(θk−m) − c) + F0+ 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 = (ik−1. . . ik−n mg sin(θk) − c . . . mg sin(θk−m) − c 1)T (4.23a)

Θ= (a1. . . an b1. . . bm F0) (4.23b)

a prediction ˆik of ikcan be formulated as

ˆ

ik = ϕTkΘ (4.24)

end thus the prediction error is defined as

(41)

The estimated model would have two inputs, mg sin(θ) and F0, which will be

referred to as u1 and u2 respectively. The estimation is performed by varying

the model orders n, m and the delay for u1 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 u2, 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)

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 seg-ments, [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 4.

5.1 Method evaluation

5.1.1 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.

(44)

Figure 5.1:Integrated accelerometer data, no lift

(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)

5.1.2 Pitch current relationship

Estimation according to (4.17c) with the concatenated data in Figure 4.1 results in ˆ

Θ ≈ 13.1 282.7

!

(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

F0≈21.6398 (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 1600. 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)

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 1.96. 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 detec-tions 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 detec-tion 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)

(a)Residual and diagnosis variable for data affected by external forces

(b) Residual and diagnosis variable for data not affected by external forces

(49)

(a)Residual and diagnosis variable for data affected by external forces

(b) Residual and diagnosis variable for data not affected by external forces

(50)

Figure 5.8:Prediction and its error from an ARX(6 [11 1] [5 0]) model

5.1.3 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 5.12. Now the sampling frequency was 18Hz for the currents meaning that the first lift around sample 250 would have the approximate time 250/18/60 ≈ 0.23 minutes, the left lift would similar have the time ≈ 0.48 and the last lift ≈ 0.75. 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)

(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)

(a)Left current

(b)Right current

(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)

(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 45

5.3 Driving

The method modeling a pitch current relationship that proved to have fairly good performance with current conditions. This method had the most significant devi-ations 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 de-tectable 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 detec-tion system. This method showed little drawbacks from available sensors mean-ing 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 effec-tive.

It is the author’s believe that an adequate method can be achieved with a

(58)

nation of an improved odometry method and the pitch angle relationship.

6.2 Future work

Improvement of the odometry method could be done with a Kalman filter. With for example, some external positioning system a Kalman filter could use accelerom-eter data as input and the position as measurement to produce more accurate estimates. With good estimates the constraints can be used to detect abnormal movement. Combining the improved odometry method above with the pitch an-gle relationship could result in a complete detection system.

The frequency method has its greatest potential in detecting back wheel lifts. Results would be improved if the sampling frequency was higher but the cost of better sensors might not make the method worth implementing.

(59)

Bibliography

[1] Anthony M Block, Jerrold E Marsden, and Zenkov Dmitry V. Nonholonomic dynamics. Norice of the AMS, 52(3):320–329, 2005. Cited on page 7. [2] Erik Frisk. Residual generation for fault diagnosis, 2001. Cited on pages 23

and 24.

[3] Fredrik Gustafsson. Statistical Sensor Fusion. 2010. ISBN 978-91-44-07732. Cited on pages 8, 13, and 27.

[4] Fredrik Gustafsson, Lennart Ljung, and Mille Millnert. Signal Processing. 2010. ISBN 978-91-44-05835. Cited on pages 30 and 32.

[5] Layton C Hale. Principles and techniques for desiging precision machines, 1999. Cited on page 7.

[6] Boris Ivanic. Användning av accelerometer för detektering av rörelse i husq-varna abs gräsklippare automower. Cited on page 3.

[7] Mikael Olofsson. Signal Theory. Studentlitteratur, 2011. ISBN 978-91-44-073538. Cited on page 23.

[8] Abhinav Parkhi and Mahesh Pawar. Analysis of deformities in lung using short time fourier transform spectrogram analysis on lung sound. IEEE, 2011. URL http://ieeexplore.ieee.org/stamp/stamp.jsp?tp= &arnumber=6112850. Cited on page 32.

[9] Ken C. Pohlmann. Principles of Digital Audio. McGraw-Hill/TAB Electron-ics, 2005. ISBN 0071441565. Cited on page 25.

[10] Marek Ryćko and Bugosław Jackowski. Determination of initial ca-ble forces in prestressed concrete caca-ble-stayed bridges for given design deck pro®les using the force equilibrium method. Com-puters and Structures, 74(1), 2000. URL http://ac.els-cdn. com/S0045794998003150/1-s2.0-S0045794998003150-main. pdf?_tid=276e11c6-654f-11e6-85e8-00000aab0f26&acdnat=

(60)

1471530312_8f642306e1d8e603b08016b49e5857b9. Cited on page 26.

[11] Angelo Maria Sabatini and Vincenzo Genovese. A sensor fusion method for tracking vertical velocity and height based on inertial and barometric altimeter measurements. Cited on page 3.

[12] Johan Ståhl. Kollisionsdetekteringssystem för autonom robot. LITH-ISY-EX-ET–15/0436–SE, Linköping University, Sweden, 2015. Cited on page 3.

Detecting External Forces on an Autonomous Lawnmowing Robot with Inertial, Wheel Speed and Wheel Motor Current Measurements

Master of Science Thesis in reglerteknik

Department of Electrical Engineering, Linköping University, 2017