ploughing calculations

8/17/2019 ploughing calculations

1/178

Autonomous Agricultural Robottowards robust autonomy

Group members of IAS10-1032b

Martin Holm Pedersen

Jens Lund Jensen

Fall 2006–Spring 2007

AALBORG U NIVERSITYDEPARTMENT OF E LECTRONIC SYSTEMS

AUTOMATION AND CONTROL


2/178


3/178

The Faculty of Engineering, Science and Medicine

Intelligent Autonomous Systems

THEME:Final Thesis

TITLE:Autonomous Agricultural Robot:towards robust autonomy

PROJECT PERIOD:IAS9–IAS10Sep. 2006–Jun. 2007

GROUP:IAS10–1032b

GROUP MEMBERS:Martin Holm Pedersen

Jens Lund Jensen

SUPERVISORS:Roozbeh Izadi-Zamanabadi

Jesper A. Larsen

NUMBER OF DUPLICATES: 6

NUMBER OF PAGES IN REPORT: 128

NUMBER OF PAGES INAPPENDIX: 50

TOTAL NUMBER OF PAGES: 178

Abstract: This master thesis doc-uments the work of group 1032band concerns the development of amodel based (Fault Detection and Iso-lation)FD I scheme to detect and iso-late faults in an four wheeled agricul-tural robot called an A PI (AutonomousPlatcare Instrumentation system). Thethesis describes a number of differentmethods for deploying different FDIstrategies as well as preliminary test-ing on a preexisting non-linear modelof the robot.Furthermore the thesis documents theefforts to make the A PI a more reliableand robust platform with the designat implementation of new sensors aswell as steps to make is possible for therobot to diagnose itself.

The different F DI methods were tested

successfully on the non-linear model

and were able to detect and isolatesome of the selected faults. A new

inclinometer was designed and imple-

mented on the robot to replace the old.

Two new proximity sensors were de-

signed and implemented.


4/178


5/178

Preface

This report is written by two students as their master thesis at Aalborg University - de-partment of Electronic Systems. The thesis is the final part of the specialisation in Intel-ligent Autonomous Systems. This report is the documentation of the work conductedin the period from September 2006 to June 2007 and is focused on fault detection andisolation and the A PI project.

This project is based on the Autonomous Plant-care Instrumentation system (API)which is a joint venture between Aalborg University, Danish Institute of Agricultural

Science, The Royal Veterinary and Agricultural University and 4 industrial companiesand is a pilot project to determine the feasibility of using autonomous platforms for fieldmonitoring and plant care.

The goal of this report is to design and implement fault detection and isolation of steering and propulsion faults. A secondary goal is to design and implement additionalhardware in order to make the API a more robust and reliable platform for future researchgroups.

The report is divided in five parts: Part one is a description of the project and thespecification of requirements. The second part is modeling. The third part deals withdesigning and testing various methods of fault detection and isolation. The fourth partis the overall conclusion of the project. The last part is a collection of appendices to

supplement the report.

Citations throughout the report are indicated by a number and optional page or chap-ter number, such as [7, Chapter 2].

The enclosed C D contains the report in P DF and P S formats and the MATLAB and AnsiC source code.

Martin Holm Pedersen Jens Lund Jensen

5


6/178


7/178

Nomenclature and Abbreviations

General Nomenclature N Fixed global reference frameM Global reference frame rotated with the inclination of the

robotB Robot reference frame which is fixed with the robotRX→Y The rotation matrix from the

X to the

Y -frame

ψx, ψy Angles describing the rotation between N and M [rad]

Nomenclature for Mechanical Modelling

θM The angle of the robot with respect to the x-axis of the M-frame

[rad]

y The absolute position in the y-direction [m]x The absolute position in the x-direction [m]χ The absolute position vector of the robot consisting of

[x y θ]T

χ̇ The velocity vector of the robot consisting of [ẋ ẏ θ̇]T

χ̈ The acceleration vector of the robot consisting of [ẍ ÿ θ̈]T β The angle of the wheels with respect to the body of the ve-

hicle[rad]

rw The radius of the wheels [m]

φ̇ The angular velocities of the wheels [rad/s]ywi The y-position of the i’th wheel in the B-frame [m]xwi The x-position of the i’th wheel in the B-frame [m]yIC R The y-position of the ICR in the B-frame [m]xIC R The x-position of the ICR in the B-frame [m]F x The propulsion forces provided by the wheels:

[F x1 F x2 F x3 F x4][N]

τ r The propulsion torque provided by the actuators [Nm]C f The cornering stiffness of the tires [N /rad]α The slip angles of the wheels [rad]V The velocity of the robot [m/s]

7


8/178

v The speed of the robot [m/s]κ Lengths describing the positions of the wheels(wrt. GC) [m]γ Angles describing the positions of the wheels (wrt. GC) [rad]β max The maximum angle of the wheels with respect to the body

of the vehicle. 2β max defines the area between the two stop-pers.

[rad]

β̇ max The maximum speed of the turning actuators [rad/sec]F Mg The gravity force affecting the robot in the M-frame [N]C f 1 The cornering stiffness stiction constant [N /rad]C f 2 The cornering stiffness dynamic constant [N /rad]Ra Armature resistance of the actuators [Ω]

K m Motor constant which is equal to the torque constant K tand the electrical constant K e

[ NmA ]

Abbreviations

ADI Active Fault IsolationAPI Autonomous Plant-care Instrumentation systemCM Centre of MassBFDF Beard Fault Detection FilterCOM COMmuniation systemDGPS Differential Global Positioning SystemFDI Fault Detection and IsolationFSB Front Sensor BoardFTC Fault Tolerant ControlGC Geometrical Centre of robotGPS Global Positioning SystemICR Instantaneous Centre of Rotation

OBC OnBoard ComputerPDF Probability Density FunctionPF Particle FilterPF-FDI Particle Filter - FDIPWM Pulse Width ModulationSA Structural AnalysisSF Sensor FusionSO Severity Occurrence indexSPI Serial Peripheral Interface Bus

8 Aalborg University 2007


9/178

Contents

I Introduction 13

1 Background 15

1.1 The API Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

1.2 Project Focus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2 System description 19

2.1 External Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.2 Internal Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.3 API Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3 Additional Hardware and Software 25

3.1 Forward Proximity Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.2 Inclinometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.3 Front Sensor Board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.4 Remote Shutdown of Wheels . . . . . . . . . . . . . . . . . . . . . . . . . . 29

II Modeling 31

4 Model 33

4.1 API Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.2 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.3 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.4 Hybrid Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.5 Model Verification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

III Fault Detection and Isolation 55

5 Introduction 57

6 Fault Analysis 61

6.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

9


10/178

CONTENTS

7 Isolability analysis 71

8 Linear Model based FDI of Steering and Propulsion System 75

8.1 Hybrid State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

8.2 Continuous State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

8.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

9 Nonlinear Particle Filter Based FDI of Steering and Propulsion System 91

9.1 Requirements for Particle Filter-FDI . . . . . . . . . . . . . . . . . . . . . . 91

9.2 Particle Filter Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

9.3 FDI using Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95

9.4 Performance Parameters with regards to Particle Filter-FDI . . . . . . . . . 96

9.5 Preliminary Test of Particle Filter-FDI Method . . . . . . . . . . . . . . . . 97

9.6 Preliminary Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97

10 Active Fault Isolation Supervisor 101

10.1 Active Isolation of Steering Faults. . . . . . . . . . . . . . . . . . . . . . . . 101

10.2 Active Isolation of Propulsion Faults. . . . . . . . . . . . . . . . . . . . . . . 102

10.3 Partial Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

IV Conclusion 107

11 Acceptest of Linear FDI 109

11.1 Hybrid State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

11.2 Continuos State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

12 Accept Test of Particle Filter-FDI Method 115

12.1 Conclusion on Particle Filter-FDI . . . . . . . . . . . . . . . . . . . . . . . . 115

13 Accept test of Active Fault Isolation Supervisor 119

13.1 Test of Steering Fault Isolation. . . . . . . . . . . . . . . . . . . . . . . . . . 119

13.2 Test of Propulsion Fault Isolation. . . . . . . . . . . . . . . . . . . . . . . . . 122

13.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122

14 Conclusion 123

V Appendix 129

A Additional Hardware 131

B Hardware Test 135

B.1 Inclinometer Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135



11/178

CONTENTS

B.2 Proximity Sensor Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136

C Implemented Software 139

C.1 Simulink blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139

C.2 Standalone Programs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142

C.3 Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143

D Linear FDI 145

D.1 Linear Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

D.2 Matlab Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

E Fault Analysis 147

F Theory of UIO 157

F.1 UIO Design Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159

G FDI Method Test 163

G.1 Test of UIO method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163

G.2 Test of Beard Fault Detection Filter method . . . . . . . . . . . . . . . . . . 166

H Active FI Supervisor 169

H.1 Active Isolation of Steering Faults. . . . . . . . . . . . . . . . . . . . . . . . 169

H.2 Active Isolation of Propulsion Faults. . . . . . . . . . . . . . . . . . . . . . . 170

I Kalman Filter 173

J Operational Requirements 175

K Pitch and Roll Compensation of GPS, Gyro and Compass 177

K.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177

K.2 Gyro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177

K.3 Compass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178

Group 1032b 11


12/178


13/178

Part I

Introduction

13


14/178


15/178

Chapter 1

Background

As farms grow in size, together with the size of the equipment used on them, there is aneed for ways to automate processes, previously performed by the farmer himself, suchas controlling the fields for pests. These tasks are perfectly suited for autonomous robots,

as they often require numerous repetitions over a long period of time and over a largearea.

The use of robots is a rather new development as most of the existing solutions for au-tomatic supervision, is designed for standard farm equipment, such as tractors, combinesand pesticide sprayers. One such solution is FIELDSTAR from AGCRO[19].

In most cases a small agricultural robot would be ineffective in performing farming jobs, as these often require a large quantity of materials, either to put into the ground,such as seeds or fertilisers, or to take from the field during harvest. But when dealingwith monitoring and mapping of fields or precision spraying of pesticides, a smallerrobot is ideal, as it is more gentle on the crops but also to the ground. This is due to thelower weight compared to a tractor, causing much lesser soil compaction (see Fig. 1.1).

The degree of soil compaction is important to consider, especially when dealing withmonitoring and mapping as this is often performed multiple times throughout the year,as soil compaction can cause a number of problems, such as reduced crop growth anddenitrification[15].

Figure 1.1: Effect of wheel load and tire size on soil compactment depth[15].

15


16/178

Background

1.1 The API Project

In the year 2000 a co-operation between Aalborg University, The Danish Institute of Agri-cultural Science, The Royal Veterinary and Agricultural University and 4 industrial com-panies was formed[7]. The goal was to create a Autonomous Plant-care Instrumentationsystem(API) and through the development gain expertise and knowledge in autonomous

field operations. The main use for the API is crop and weed monitoring and precisionspraying of pesticides.

The first version of the API was a somewhat simple design using small low-weightspoke-wheels mounted on a simple frame as seen in Fig. 1.2.

Figure 1.2: The first version of the A PI.

I 2002 a more rugged AP I was designed using larger knobbed wheels and a more ro-

bust frame, which also provides compartments for the batteries and the instrumentation(see Fig. 1.3 on the facing page). The mechanical implementation was performed by TheDanish Institute of Agricultural Science research-center in Bygholm.

A number of research projects has been performed with regards to both instrumenta-tion, control and fault detection and isolation. The projects are:

Autonom Robot til Markanalyse: This project deals with the specification, design andimplementation of the first prototype of the A PI robot[3].

Navigation af Autonom Markrobot: This project[11] focuses on the development of anavigation system for the A PI. The electrical system of the A PI is also designed andimplemented in this project.

Modeling and Fault-Tolerant Control af an Autonomous Wheeled Robot: The main fo-cus of this project[4] was the modeling and control of the API. A FDI method respon-sible for detecting faults in the sensors and in minor detail the actuators, is designedand implemented. A F TC scheme is also designed and implemented.

The project status as of September 2006 is that the mechanical part of the project, afully actuated four-wheeled robot, is completed. A number of sensors, such as GPS ,



17/178

1.2 Project Focus

gyro and compass, is implemented. A hybrid controller, designed for path following, isimplemented and tested. A F DI and F TC scheme is partly implemented.

Figure 1.3: The A PI robot in its current form and configuration

1.2 Project Focus

The focus of this project is to make the API a robust platform for use in future research

projects focusing on the use of autonomous robots in both agricultural settings but alsoin research areas involved in the use of larger robots in general outdoor environments.

This involves the design and implementation of a F DI scheme fully capable of detect-ing and isolating faults occurring in the wheel actuators and sensors, as well as the designand implementation of additional hardware and software needed for reliable operation.Completing these improvements will improve the robustness and reliability of the cur-rent system and allow future research groups to use the AP I without a control or elec-tronic background, allowing them to focus on their research.

Based on the focus of the project a number of objectives can be established.

1.2.1 Project Objectives

The primary objective is to make the API a robust platform. This is primarily obtained byimplementing a full FDI scheme focusing on the faults occurring on the wheel actuatorsand sensors. This is archived though the following sub-objectives:

1. Perform a fault analysis and severity assessment of all wheel, proximity sensor andinclinometer faults..

2. Design and implement a linear model based F DI method.

Group 1032b 17


18/178

Background

3. Verify the detection of critical faults.

To complement the linear methods two additional methods for F DI is to be examined,resulting in the following objectives:

1. Design and implement a non-linear F DI method.

2. Design and implement an active F DI method.

The practical objectives involving the additional hardware and software are:

1. Design and implementation of proximity sensors on the A PI.

2. Design and implementation of a separate inclinometer in order to provide pitchand roll measurements.

3. Design and implementation of relays for disconnecting individual wheels.

1.2.2 Operational RequirementsA number of requirements for the operational performance of the API, has been set byprevious groups. The relevant requirements for use in this project are:

1. When a fault occurs, the robot is allowed to deviate from its course, for instancedriving on top of the rows, for a maximum of 30 seconds.

2. The robot must be able to continue operation with at least one sensor/actuator fault.

3. Operating under both normal and faulty operation the robot must not be potentiallydangerous to its environment.

The complete list of requirements can be seen in App. J on page 175.



19/178

Chapter 2

System description

The following chapter is an overview of the different components that the AP I is equippedwith. The different components are placed on the outside of the robot as well as insidethe two compartments placed in front and in the back of the robot.

2.1 External Components

The components on the outside can be seen in Fig. 2.1 on the following page, with thefront and rear compartments shown in Fig. 2.2 on page 21 and Fig. 2.3 on page 22.

For communication the A PI is equipped with three different antennas:

• A GP S antenna used for communication between the GPS receiver and the availableGPS satellites.

• An omni-directional radio modem antenna for communication between the GPS

receiver and the G PS base station to facilitate the use of DGPS.

• Combined WLAN device and antenna with USB interface for communication be-tween the O BC and the A PI base station.

Sensors placed on the outside are:

• Compass: Measures the heading of the A PI relative to magnetic north.

• Doppler radar: Measures the forward speed of the A PI.

• Inclinometer: Measures the pitch and roll of the A PI.

• Proximity sensors: Measures the distance to the nearest obstacle using the time of flight of emitted sound waves.

2.2 Internal Components

The front and rear compartments contain a number of common components:

19


20/178

System description

Compass

GPS antenna

WLAN

Radio modem antenna

Inclinometer

Proximity sensors

Doppler Radar

Figure 2.1: Components attached to outer structure of the robot.



21/178

2.3 API Subsystems

• LH28 : Embedded micro controller sampling all propulsion and steering sensors aswell as controlling the steering and propulsion actuators. Communicates with theOBC via a C AN bus.

• H-bridge: Driver step responsible for outputting P WM signals to the steering actua-tors.

• Interface Electronics: Interfaces the sensors to the LH28 controller as well as provid-ing the interface from L H28 to the steering and propulsion actuators.

In addition to the common components the front compartment contains a Gyro, GPSreceiver and Radio Modem. The front compartment also houses the Inclinometer andProximity Sensor unit. The DC/DC converter, providing the voltage for the 5Vcc and12Vcc power bus, is placed below the Inclinometer and Proximity Sensor unit.

The back compartment contains the O BC which provides the overall control of the APIrobot and the battery charger.

Interface electronics

LH28H-bridges Inclinometer and prox. sensor unit

GyroShutdown interface

GPS receiverRadio modem

Figure 2.2: Front compartment of the A PI robot.

2.3 API Subsystems

Based on the components listed in the previous chapter a number of subsystems can be defined with regards to functionality. The subsystems are: Wheel, Gyro, DopplerRadar, OBC, CO M, Compass, GPS , Proximity Sensors and Inclinometer. Each subsystemis divided into smaller modules as seen in Fig. 2.3 on page 23, where the boxes withrounded corners symbolizes hardware modules. The square boxes are software modules.

Group 1032b 21


22/178

System description

Interface electronics

LH28

H-bridgesShutdown interface Charger

OBC

Figure 2.3: Back compartment of the A PI robot.



23/178

2.3 API Subsystems

API robot

Wheels Gyro Doppler

radar OBC COM Compass GPS Proximity sensors

Inclinometer

Temp. Sens

4 Prop. act. Temp. sens. PC-104 W LAN Inclinometer DGPS

4 Steer. act. Shared mem. CAN bus

4 Prop. sens. Controller

4 Steer. sens. Sensor Fusion

LH28

Propul. cont.

Steer cont.

Figure 2.4: API subsystems.

The functionality of the different subsystem are listed below:

Wheels The four wheels gives the AP I the possibility of moving in any direction dueto the individual steering on all four wheels. The wheel system is based on foursteering actuators and four propulsion actuators. The wheels are controlled by aLH28 microcomputer. The available sensors are steering angle and wheel speed.

Gyro The gyro subsystem provides a measurement of the vertical angular velocity of the A PI. As the gyro measurement is dependent on the temperature, a temperaturesensor is available to correct the measurements.

Doppler radar The Doppler radar measures the A PIs speed using microwaves. The mea-surement is calculated using the doppler shift of the transmitted microwaves. TheDoppler radar measurements are collected by the L H28 responsible for wheel 1.

OBC The on-board computer is the the overall control system. The OBC is also respon-sible for collecting measurements from the gyro, compass, GPS, proximity sensorsand inclinometer. The O BC is a P C104 stack with a 133MHZ CPU. The O S on the O BC

is Linux. To facilitated manual control of the API a monitor, keyboard and joystickis also implemented.

COM The communication subsystem provides the communication between the OBC andthe four LH28s as well as the communication between the A PI and the base station.The communication between the OBC and the wheel nodes are based on the CA N bus and consist of the steering and propulsion references and the sensor measure-ments provided by the wheels. The link connecting the API to the base station

Group 1032b 23


24/178

System description

is standard WLAN. The communication between base station and API is the way-points, describing a path in addition to function as a monitoring system, when theAPI is operating autonomously.

Compass The compass provides the heading of the AP I with regards to the magneticnorth. To compensate for tilt offset on the heading a internal inclinometer is present.

This compensation is due to a noisy and overly sensitive inclinometer, disabled inthe Compass.

GPS The GPS system provides the position of the API . The standard GPS precision isincreased by using DGPS. The DGPS precision is obtained using a base station witha fixed position. The base station can then calculate the error on the GPS measure-ments and transmit the results to the A PI using the radio modem.

Proximity Sensors The proximity sensors provide a distance measurement of objects infront of the API. This distance can be used for obstacle avoidance and as an emer-gency system, in case of a possible collision.

Inclinometer The inclinometer is placed close to the geometric center of the API andmeasures the pitch and roll of the robot.



25/178

Chapter 3

Additional Hardware and Software

When the A PI robot was handed over from the last group it was clear that some modifi-cations were necessary on the physical system in order to make it more robust and moreusable for the current and future projects. The main problems were:

• The implemented WLAN solution was not very robust or very usable as it didn’thave the desired range and was prone to failure when used.

• The interface electronics boxes which are a very important link between the LH28sand the different sensors and actuators related to each wheel were unreliable andimpractical in use.

• The only inclinometer on the API robot housed in the compass is fluid based andsensitive to vibrations to the point where the measurements are nearly unusable.

• The A PI robot did not have any proximity sensors to detect obstacles in its path.

To try and solve these problems several initiatives are taken.

• A new USB WLAN device is implemented and the old subsystem removed from therobot.

• New interface electronics boxes are designed and implemented on the robot.

• A new accelerometer based inclinometer which is less sensitive to vibrations is de-signed and implemented on the robot.

• Two ultrasonic range finders are designed and implemented on the robot.

This chapter describes the design and implementation of the additional hardware andsoftware needed to improve the existing base on the A PI robot.

3.1 Forward Proximity Sensors

The AP I is in its current state essentially blind. There are no sensors dealing with theenvironment. This means that when encountering an obstacle, the robot just continuesits operation. This can result in serious damage to both the API and the obstacle as the AP Iweighs approximately 225kg and has been measured to reach speeds of at least 2.2 m/s.

25


26/178


The requirements for the current application are:

• Wide detection volume (>15◦ cone).

• Range of at least 2 m.

The range demand is based on a measured speed of the robot of 2.2 m/s which gives astopping distance measured to be about Ddyn=1.2 m. The total distance travelled fromdetection of an obstacle until the A PI robot comes to a complete stop is:

Dstop = Dsample + D process + Ddyn (3.1)

The API robot proximity sensors are sampled once every 60ms yielding a sample distanceat 2.2 m/s of:

Dsample = 2.2 m/s · 0.060s = 0.132m (3.2)

D process is assumed to be negligible so the estimated total stopping distance is:

Dstop = 1.20m + 0.132m = 1.33m (3.3)

There are several alternatives in range finding sensors. Most of them use sound or laserto sense distances but each have very different detection area and volume. While lasersensors generally have long range and accuracy, none provide the wide sensing volumeof the ultrasonic sensor. In light of this and based on the assumption that the AP I willdrive in a straight line most of the time, it is decided to outfit the AP I with two forwardfacing proximity sensors. This allows the AP I to sense any object directly in front of it andtake appropriate countermeasures.

The chosen sensors are two SensComp 6500 Ranging Modules[17] and two SensComp7000 sonar transducers[18]. The sensors are rated at a maximum detection distance of

10 meters. The ranging modules are connected to a micro controller, which performs theactual range sampling. The result is transmitted to the O BC via a serial R S232 connection.To save components and implementation, the additional computation power and unusedports on the micro controller is used to sample the new inclinometer described in Sec. 3.2.The ultrasonic transducers are placed on the robot as shown in Fig. 3.1 on the next page.

3.1.1 Conclusion

The two ultrasonic proximity sensors have been mounted on the robot and interfaced tothe OBC. The sensors where tested in Sec. B.2 on page 136 and found to live up to thedemands previously stated in this section as it could detect obstacles in its path whendriving straight and at the desired range.

3.2 Inclinometer

The API is in the current state, assumed, to always be level. This is not a viable solution,when driving in fields, as the pitch and roll of the A PI affect both GPS and compass read-ings as well as introducing disturbances to the estimation of χM. The reason the robotis assumed level, is due to noise in the inclinometer built into the compass. Previous



27/178

3.3 Front Sensor Board

Figure 3.1: The placement of the two Proximity Sensors

attempts to remove the noise has been unsuccessful and a new inclinometer is proposed.The selected inclinometer is the SCA100T[20]. The sensor uses MEMS technology instead of the fluid inclinometer in the compass, which makes it more resistant to the external noiseoriginating from driving in rugged terrain, the API is expected to operate in. The SCA100Tis mounted under the GPS antenna and is interfaced using a S PI connection to the microcontroller on the forward proximity sensors. The inclination measurements can then beused to calculate the correct G PS position and the correct compass heading as well as the

correct gyro measurements. App. K on page 177 shows the tilt correction. It is assumedthat the A PI in its current status as a research project, will only move on surfaces, that can be considered level. As a consequence of that, the tilt correction Will not be implemented,only described.

3.2.1 Conclusion

The inclinometer has been designed, implemented and tested successfully in App. B.1 onpage 135. A function and vibration test was conducted and the inclinometer works asintended and the vibration sensitivity is much less than the original inclinometer.

3.3 Front Sensor BoardThe basis of this hardware board is a Microchip PIC16F877[14], responsible for samplingthe sensors. The board and the connected sensors will be named as the Front SensorBoard(FSB). Even though only two proximity sensors are implemented, the FSB has spacefor and can use up to eight as seen in Fig. 3.2 on the next page. This allows future groupsto easily expand the area covered by the proximity sensors. The eight sensors could bearranged with two on each side to enable the A PI to move around obstacles.

Group 1032b 27


28/178


G N D

GND

CON1CON2CON3

CON3CON2CON1

TTL-RXTTL-TX

WACT1WACT2

SDA

SDO

ECHO-J3ECHO-J2

BINH-J3BINH-J2

INIT-J3INIT-J2

INIT-J5INIT-J4

INIT-J7INIT-J6

INIT-J9INIT-J8

ECHO-J5ECHO-J4

ECHO-J7ECHO-J6

ECHO-J9ECHO-J8

BINH-J4BINH-J5BINH-J6

BINH-J8BINH-J9

BINH-J7

SCK

VCC

VCC

VCC

VCC

VCC

74LS138-J274LS138-J2

A1

B2

C3

Y0 15

Y1 14

Y2 13

Y3 12

Y4 11

Y5 10

Y6 9

Y7 7

G16

G2A4

G2B5

74LS138-J174LS138-J1

A1

B2

C3

Y0 15

Y1 14

Y2 13

Y3 12

Y4 11

Y5 10

Y6 9

Y7 7

G16

G2A4

G2B5

U4

74LS151

U4

74LS151

D0 4D1 3D2 2D3 1D4 15D5 14D6 13D7 12

A 11B 10C 9

G 7

Y6

Y5

R1

4.7k

R1

4.7k

Y1Y1

LEDLED

R2

470

R2

470LEDLED

ButtonButton

R3

470

R3

470

PIC16F877PIC16F877

OSC1/CLKIN13

OSC2/CLKOUT14

MCLR/Vpp/THV1

RA0/AN02

RA1/AN13

RA2/AN2/VREF-4

RA3/AN3/VREF+5

RA4/T0CKI6

RA5/AN4/SS7

RE0/AN5/RD8

RE1/AN6/WR9

RE2/AN7/CS10

RB0/INT 33

RB1 34

RB2 35

RB3/PGM 36

RB4 37

RB5 38

RB6/PGC 39

RB7/PGD 40

RC0/T10S0/T1CKI 15

RC1/T10S1/CCP2 16

RC2/CCP1 17

RC3/SCK/SCL 18

RC4/SDI/SDA 23

RC5/SDO 24

RC5/TX/CK 25

RC7/RX/DT 26

RD0/PSP0 19

RD1/PSP1 20

RD2/PSP2 21

RD3/PSP3 22

RD4/PSP4 27

RD5/PSP5 28

RD6/PSP6 29

RD7/PSP7 30

VDD11

VSS12

VDD32

VSS31

LEDLED

C222pFC222pF

R4

470

R4

470

C322pFC322pF

Figure 3.2: The diagram of the Front Sensor Board.

Figure 3.3: The implemented version of the Inclinometer and Proximity Sensor board



29/178

3.4 Remote Shutdown of Wheels

Figure 3.4: Schematic of the Emergency Stop circuit.

3.4 Remote Shutdown of Wheels

As a additional safety measure and as a method for F DI and FTC, a shutdown box capableof disabling the power to the individual wheels, is implemented on the AP I . This willenable the control system to power off one or more wheels in case of emergency or as amethod of fully isolating steering and propulsion faults. The current version of the AP Iis not capable of turning off actuators, which in situations with faults occurring on the

wheels can result in limited control of the A PI and in some cases can cause actual damageto the robot. The requirements for the shutdown boxes are:

1. It must not interfere with or disable the emergency stop buttons, mounted on therobot.

2. It shall be able to provide power to the wheels, even when the OBC is shutdown.

3. It shall be able to turn the power to the wheels on or off.

4. It must be able to turn the wheels on, using the voltage provided by the parallelport on the O BC .

The emergency stop system on the API is implemented by a previous group as 4 emer-gency stop switches and 4 relays in series, so if one of the emergency stops are triggered,all relays switches off. A schematic can be seen of Fig. 3.4.

This means that relays for each wheel are already implemented and that the remoteshutdown can be implemented between the emergency switches and the relays for thewheels. The proposed method is to use a transistor placed before the relay, to enablethe power to the relay. The transistor is pulled high by the OBC , triggering the relay and

Group 1032b 29


30/178


OBC_1

OBC_2

5V

24V

24V

Wheel Relays

Wheel Relay 1Wheel Relay 1

COM

A

B

NC

NO

On/OBCOn/OBC

2.7k2.7k

2.7k2.7k

2.7k2.7k1k1k

BC337BC337

2.7k2.7k

BC337BC337

DIODEDIODE

2.7k2.7k

2.7k2.7k

BYV28-200BYV28-200

1

2

1k1k

1.6k1.6k

LEDLED

BYV28-200BYV28-200

1

2

Wheel Relay 2Wheel Relay 2

COM

A

B

NC

NO

DIODEDIODE

1.6k1.6k

LEDLED

Figure 3.5: Schematic of the remote shutdown relays

Figure 3.6: Picture of the remote shutdown boxes

providing power to the wheel actuators. As this requires the OBC to actively turn onthe wheel motors, a switch is added, allowing the wheels to be either permanently on orcontrolled by the O BC. As the relays are placed in both the front and back compartments,two shutdown boxes are implemented, each capable of controlling the two wheels in thecompartment. The schematic of one of the shutdown boxes can be seen in Fig. 3.5.

The final shutdown box can be seen in Fig. 3.6

3.4.1 Implementation Status

The remote shutdown boxes has been implemented and is capable of turning the wheelson or off. Furthermore the functionality of the emergency switches is not bypassed. Thismeans that the functionality described above has been successfully implemented.



31/178

Part II

Modeling

31


32/178


33/178

Chapter 4

Model

This chapter contains the kinematic and dynamic model of the A PI robot. The kinematicmodel is a mathematical model, which maps the orientation and angular velocity of thewheels to the movement of the robot. The dynamic model maps the propulsion torquesand wheel orientation to the acceleration of the robot. The two models is a result of thework of many previous groups working on the model and is represented here in its latestform with few modifications. The third section describes a hybrid model, based on thedynamic model.

4.1 API Geometry

As mentioned in the System Description, the API is a four wheeled robot with an steeringactuator and propulsion actuator on each wheel. The dimensions of the A PI is shown inFig. 4.1

76cm

150cm 30cm

100cm35cm 15cm

1 0 0 c m

6 8 c m

50cm 34cm

22cm 27cm

107cm

100cm

r =23cmw

60cm

Figure 4.1: Dimensions of the A PI robot[4].

In order to describe the position and orientation of the A PI a posture vector is definedas χ = [x,y,θ]T , where x and y is the position of the API oriented with angle θ in theinertial reference frame N . To simplify some of the relations between the API and thesurface is is placed on as well as the relations between the different parts of the API, anumber of frames is defined:

N Frame This frame is the inertial reference frame and is defined as the terrain, the robotmoves on as seen from above. The frame is similar to the layout of a map.

33


34/178

Model

M Frame This frame is a non-inertial reference frame and describes the same terrain asframe 0 but with the additional information of the pitch and roll of the robot ψx andψy . This is the frame, that describes the actual movement on the field.

B Frame This frame describes the orientation of the robot in M. The frame is fixed in thegeometric center (GC) of the robot and is aligned with the forward direction of the

API placed in the x-axis of the frame. This means that this frame is M rotated withthe orientation of the robot θM.

The sensors available on the API is measured in different frames. In order to use asensor measurement from another frame, the measurement must be rotated by the oneor more of the rotation matrices shown below:

R N→M =

cos(ψx) 0 sin(ψx)

− sin(ψx)cos(ψx)sin(ψy)

|z|

cos(ψy)

|z|

cos(ψx)2 sin(ψy)

|z|− sin(ψx) cos(ψy)

|z|− cos(ψx) sin(ψy)

|z|cos(ψx) cos(ψy)

|z|

(4.1)

where

|z| =

cos(ψx)2 + cos(ψy)2 − cos(ψx)2 cos(ψy)2

R B→M =

cos(θM) sin(θM) 0−sin(θM) cos(θM) 0

0 0 1

(4.2)

Figure 4.2 on the facing page show the definition of the M

and B

frames together withthe definition of the wheel angles β i. The wheels are fixed in the B frame with (xw,i, yw,i)as the position of the ith wheel. To the right of Fig. 4.2 the position of the wheel withregards to the GC is shown as the intersection of the lines: κi with angle γ i. The definitionof the posture vector is also shown.

The values for κi and γ i can be seen in Table 4.1.

Parameter κ1 κ2 κ3 κ4Value 0.707 m 0.707 m 0.707 m 0.707 m

Parameter γ 1 γ 2 γ 3,g γ 4Value 45◦ 135◦ 225◦ 315◦

Parameter m IValue 211.5 kg 83.5 kg·m2

Table 4.1: The parameters of the robot.

The geometric center is selected as the basis for all simulation and modeling. Anotherpossibility is to use the center of mass(CM), but as the behaviour of the robot when usingthe G C is more intuitive, this GC is chosen.



35/178

4.2 Kinematic Model

M

B

β 1

GC

M

M M x ,y

θ

β 4

β 2

β 3

N B

γ

γ

1

4

2γ

γ 3

κ 3

1κ

κ 2

κ 4

w,3

w, 4

w, 1

w,2 w,2( x ,y )

w,3( x , y )

w, 4

( x , y

)

w, 1

( x

, y )

Figure 4.2: To the left: Definitions of the B and M frames with the steering angles of the wheels (purple) and the posture vector (red) shown in the M frames. To the right:Position of the geometric center (GC) given by κi and γ i[4].

4.2 Kinematic Model

The purpose of the kinematic model is to map the steering angles of the wheels and theangular velocity of the propulsion actuators (β i and φ̇i) to the velocities of the robot. Thevelocities are given by the vector χ̇ N .

Kinematic model

β iφ̇i χ̇ N

Figure 4.3: Inputs and outputs of the Kinematic model

The following assumptions are made in the kinematic model:

Assumption 1 All wheel orientations are nominal perpendicular to the InstantaneousCenter of Rotation(IC R).

Assumption 2 It is assumed that the movement of the A PI is pure rolling with no slip.

The API robot is modeled using the B and M-frame. The posture vector is χM. Whenturning the A PI robot turns around the I CR in the M-frame.

In order to calculate the I CR the individual wheel angles β i + π2 are used instead of β i,

as the intersecting lines have to be perpendicular to the wheel.

The derivation of the kinematic model is separated into three parts:

1. Calculating the I CR from wheel orientations β i.

2. Projecting the wheel velocities to an angular velocity around the I CR.

3. Calculate the translatory movement from the angular velocity and the vector to theIC R.

Group 1032b 35


36/178

Model

β 2 + π

2

β 1 + π

2

β 4 + π

2

β 3 + π

2

(xw,2, yw,2)

(xw,1, yw,1)

(xw,3, yw,3)

(xw,4, yw,4)

(x, y)

θ

B

ICR

N -frame

Figure 4.4: Geometric definitions for A PI robot when turning around the I CR.

The equations used for calculating the IC R can be derived by looking at Fig. 4.4 andFig. 4.5 on the next page. Equation (4.3) describes a straight line, with a slope of tan(β i+

π2 )

on which the point (xi, yi) is placed.

yi = aixi + bi

yi = tan

β i + π

2

xi + bi

yi = − cot(β i)xi + bi (4.3)

A known point on the line is the position of the wheels (xw,i, yw,i). This point is substi-tuted into Eq. (4.3) which is then solved to find bi.

yw,i = − cot(β i) · xw,i + bi ⇒bi = yw,i + xw,i cot(β i)

The calculated bi is then substituted into Eq. (4.3) to yield the final description of the line,that goes through the I CR and the center of wheel i:

yi = (xw,i − xi)cot(β i) + yw,i (4.4)

The I CR can be described as the intersection between two of these lines. The result of this



37/178

4.2 Kinematic Model

β i + π

2

β i

(xw,i, yw,i)

ICR

yi = aixi + bi

Figure 4.5: The line from the center of a wheel to the I CR.

operation can be seen in Eq. (4.5) and Eq. (4.6).

y1 = yBIC R ∧ x1 = xBIC R ⇒

xBIC R =xBw,i cot(β i) − xBw,j cot(β j ) + yBw,i − yBw,j

cot(β i) − cot(β j ) (4.5)

yBIC R =cot(β i)y

Bw,j − cot(β j )yBw,i + cot(β i)cot(β j )

xBw,j − xBw,i

cot(β i) − cot(β j ) (4.6)

Since the above equations only have a solution when the lines intersect, the model cannot be used when the robot is moving in a straight line. The model therefore effectively

becomes a hybrid model with two states. One for turning and one for driving in a straightline. The following two equations will be used to model the behavior of the AP I, whendriving in a straight line:

ẋB = 1

4

4i=0

φ̇i cos(β i)

(4.7)

ẏB = 1

4

4i=0

φ̇i sin(β i)

(4.8)

When the IC R has been determined, the angular velocity around it ( θ̇M) can be found

from the velocity of a wheel (V wi) and the distance from a wheel to the IC R(ri). This isillustrated in Fig. 4.6 on the following page and shown in Eq. (4.9).

θ̇M = V wi

ri⇔ θ̇M = ri × V wi

riT ri(4.9)

This means that θ̇M

can be written as in Eq. (4.10).

Group 1032b 37


38/178

Model

V w,i = φ̇i

·rw

ICR

ri

θ̇M

Figure 4.6: The angular velocity around the I CR.

ri =

xBw,i − xBIC RyBw,i − yBIC R

0

, V wi = rw φ̇i

cos(β i)sin(β i)

0

⇓

θ̇M

= ri × V wiriT ri

=

xBw,i − xBIC RyBw,i − yBIC R

0

×

rw φ̇i cos(β i)rw φ̇i sin(β i)

0

xBw,i − xBIC R

2+

yBw,i − yBIC R2

=

00

rw φ̇i(xBw,i−xBICR) sin(β i)−(yBw,i−yBICR) cos(β i)

(xBw,i−xBICR)2+(yBw,i−yBICR)

2

(4.10)

This rotational velocity around the IC R will equal the rotational velocity around the GC.The translational velocity of the robot will be the tangential velocity, which can be calcu-lated as shown in Eq. (4.11).

ẋBẏB

0

= rICR × θ̇M =

xBIC RyBIC R

0

×

00

θ̇M

=

yBIC R−xBIC R

0

θ̇M (4.11)

The derivative of the posture vector (χB) can then be written as shown in Eq. (4.12)where Eq. (4.10) is substituted into Eq. (4.11). The position of the I CR (xBIC R and y

BIC R) are

derived in Eq. (4.5) and Eq. (4.6).

χ̇B =

yBIC R−xBIC R

1

rw φ̇i

xBw,i − xBIC R

sin(β i) −

yBw,i − yBIC R

cos(β i)

xBw,i − xBIC R2

+

yBw,i − yBIC R2 (4.12)

Now the velocities have been determined in the B-frame and in order to describe themotion in the N -frame, χ̇B is first rotated into the M-frame by R B→M and is then rotated



39/178

4.3 Dynamic Model

into N by using R M→N as in Eq. (4.13).χ̇ N = R M→N R B→Mχ̇B (4.13)

With this the complete kinematic model can be calculated using Eqs. (4.12), (4.5), (4.6)and (4.13) to calculate respectively the IC R, angular velocity, and translational velocityof the robot. The result is shown in Eq. (4.14), which is the total kinematic model of the

robot in inertial coordinates.

χ̇ N = RM→N RB→Mχ̇B

= RM→N RB→M

yBIC R−xBIC R

1

rw φ̇i

xBw,i − xBIC R

sin(β i) −

yBw,i − yBIC R

cos(β i)

xBw,i − xBIC R2

+

yBw,i − yBIC R2

(4.14)

where Eq. (4.5) and Eq. (4.6) yields:

xBIC R =xBw,i cot(β i) − xBw,j cot(β j ) + yBw,i − yBw,j

cot(β i) − cot(β j)

yBIC R =cot(β i)yBw,j − cot(β j)yBw,i + cot(β i)cot(β j )xBw,j − xBw,i

cot(β i) − cot(β j )

4.3 Dynamic Model

The dynamic model sought is a three degree of freedom model incorporating the twofollowing properties:

• Individual use of all four steering actuators. This would enable the model to func-tion even if the wheels were not placed in the correct steering angles as required inthe kinetic model. This requires the modeling of the friction caused by the wheel.

• Individual propulsion torques. This enables the model to function with differentpropulsion forces from each wheel.

The setup of the model is seen in Fig. 4.7 on the next page. The forces F x is the propul-sion forces, vi is the velocity of a wheel. The friction force of the wheel F y is dependenton the slip angle of the wheel, α.

In order to simplify the model the following assumption is made: The robot is fixedwith regards to the suspension. This eliminates the roll and pith of the robot due to theeffect of acceleration.

The model has two types of inputs:

• The steering angle of the four wheels β i.

• The propulsion forces of the four wheels F xi.

and the output is the translational and rotational acceleration of the robot in the Mframe:

χ̈M =

ẍMÿM

θ̈M

(4.15)

Group 1032b 39


40/178

Model

Fx3

Fy2

Fx2

Fy3

Fx4F 4y

y

y

x

1yFx1F

θ

B B xθ

V

β1

α1

1

M

M M

θ.M

M

Figure 4.7: The general setup for the dynamic robot model[4].

4.3.1 Modeling of Sideways Friction in the Wheels

The sideways friction forces is defined in [16] as a linear relation between the frictionforce and the slip angle:

F yi = −C f · αi (4.16)where:

• F yi is the sideways friction force of the i’th wheel [N].

• C f is the cornering stiffness of the tire [N/rad].

• αi is the slip angle of the i’th wheel [rad].

This equation is found to introduce oscillations in the model and is therefore expandedwith viscous friction. The expanded equation is shown in Eq. (4.17).

F yi = −(C f 1 + C f 2 · V M) · αi (4.17)where:

• C f 1 is the cornering stiffness constant, that accounts for the stiction in the system[N/rad].

• C f 2 is the cornering stiffness constant, that accounts for the coulomb and viscousfriction in the system [ N·srad·m ].

• V M is the speed of the robot [m/s].

The slip angle αi is defined in the range [−π2 , π2 ] and is calculated as the angle fromF xi to V i in quadrant 1 and 4 and from V i to F xi in quadrant 2 and 3. This can be seen inFig. 4.8 on the facing page.



41/178

4.3 Dynamic Model

x1Fx1F

Fx1

1β

θ

α =0o

V1

β1

x1F

α1

β1

x1F

α1

α1

β1

x1F1yF

1yF

1yF

1yF

1yF

yF 1,res

yF 1,res

θ

α1

β1

o o=−30α α =−90

θ

α1

β1

VV1 1

θ

o=−60α

V1

M

M M

M

M

M θ

αo

V1

=60

θ

α =30o

V1

Figure 4.8: Illustration of the slip angle at different steering angles. The green F yi shownthe definition of the friction force and the cyan F yi,res shown where the definition and theresulting force differs[4].

In order to determine the slip angle it is necessary to find the true velocity of thewheel (V i) which consists of two components: One from the velocity of the G C (V

M) anda component from the rotation of the robot around the GC (V ti) as seen on Fig. 4.9 on thenext page.

V ti is a function of the angular velocity and distance from GC of the centre of the wheel.

V ti = θ̇M · κi (4.18)Separating V ti into its components results in the following equations:

V txi = θ̇M · κi · sin(γ i + θM) (4.19)

V tyi = θ̇M · κi · cos(γ i + θM) (4.20)

Using Fig. 4.10 on the following page is can be seen that ẋM−V txi and ẏM+V tyi formsthe opposite and adjacent sides of the triangle with V 1 as hypotenuse.

The slip angle can then be calculated as shown in Eq. (4.21).

αi = tan−1

ẏM + θ̇M · κi · cos(γ i + θM)ẋM − θ̇M · κi · sin(γ i + θM)

− β i − θM (4.21)

The definition of the sideways friction force does not take into account that the wheelcan rotate more than 180◦ past the velocity direction resulting in the friction force point-ing in the wrong direction in some cases as seen in Fig. 4.8. Case 2 and 5 should give thesame friction force. To correct this behavior the slip angle is redefined as seen in Eq. (4.22)

Group 1032b 41


42/178

Model

θ

.

θ

κ 1

y

x

xB

V 1Fx1α

β1

γ 1

V

Vt1

tx1V

V1

θ

M

M M

M

M

M

M M y

.x

.

ty1

Figure 4.9: Wheel 1 and centre of robot with relevant velocities and angles shown forfinding α1[4].

αi =

π − α∗i for π/2 ≤ α∗i


43/178

4.3 Dynamic Model

• Propulsion forces: F xi

• Centripetal force: F c

• Gravity force: F g

The Sideway friction force was defined in the previous section and the propulsionforce is defined as F xi = τ r,i

rw, where τ r,i is the propulsion torque for the propulsion actua-

tor.

These two forces are then projected onto the M-frame as seen in Fig. 4.11

F 1y β +θ1

β +θ1

1α

V1

F 1x

M

Figure 4.11: The sideways friction and propulsion forces and their projection onto theM-frame[4].

This leaves the centripetal force and the gravity force. The contribution from the cen-tripetal force can be determined using a scenario, where all wheels are perpendicular tothe IC R and where all propulsion actuators are powered down. The acceleration of the

robot is then only a result of the centripetal force:

ar = r · (θ̇M)2 ⇔ (4.23)ar = V

M · θ̇M (4.24)

where:r is the radius of the circle the robot is driving on [ m].ar is the centripetal acceleration of the robot [m/s

2].V M is the velocity of the robot [m/s].

To separate the vector into its two components arx and arx, the relationship betweenthe V M and ar vectors is derived. This is shown in Fig. 4.12 on the next page. The

resulting equations is shown in Eq. (4.25).

arx = ẏM · θ̇M

ary = ẋM · θ̇M (4.25)

The final contribution is the acceleration due to the gravity:

Group 1032b 43


44/178

Model

arxθ

x

ar a

V

.

.

.M

ry

yM .

.

.

.y

x

M

M

ICR

Figure 4.12: The robot with centripetal acceleration shown[4].

The gravity force (F Mg ) can be calculated as shown in Eq. (4.26).

F Mg =

F MgxF Mgy

F Mgz

= m ·

aMgxaMgy

aMgz

= R N→M

00

m · g

(4.26)

The complete equation for the translational motion is performed by summing all theforces from each of the four wheels, dividing by the mass of the AP I robot adding thecentripetal and gravity acceleration as shown in Eq. (4.27).

χ̈Mtran =

ẍM

ÿM

θ̈M

=

1m

4i=1

cos(β i + θ

M) · F xi − sin(β i + θM) · F yi

− ẏM · θ̇M − aMgx1

m

4i=1

sin(β i + θ

M) · F xi + cos(β i + θM) · F yi

+ ẋM · θ̇M − aMgy0

(4.27)

4.3.3 Rotational Motion

The rotational motion of the robot is caused by two different forces: The friction of thewheels and the force of the propulsion actuators. These two forces results in a rotationaround the G C and therefore must be projected into a line perpendicular to the line orig-inating in the GC and crossing though the center of the wheel. This line is given by κiand γ i. As seen of Fig. 4.13 on the facing page this perpendicular line is also the line onwhich the velocity V ti is pointing. The projection of the forces is performed using theangle β i

−γ i.

The angular acceleration around GC can then be found by multiplying the projectedforces with the arm (κi), summing over the four wheels and dividing with the momentof inertia as shown in Eq. (4.28).

χ̈Mrot =

ẍMÿM

θ̈M

=

001I

4i=1

sin(β i − γ i) · κi · F xi + cos(β i − γ i) · κi · F yi

(4.28)



45/178

4.3 Dynamic Model

x1F

1yF

1γ

κ 1

γ 1

β11β

B x

Vt1γ 1

GC

Figure 4.13: Wheel 1 and the GC of the robot with relevant vectors and angles for calcula-tion of the rotational motion[4].

4.3.4 Integration of the Dynamic Model

The complete dynamic model is integrated with the actuator and sensor models. Themain equations for the robot is shown here:

Robot:

χ̈M = χ̈Mrot + χ̈Mtrans = (4.29)

ẍM

ÿM

θ̈M

=

1m

4i=1

cos(β i + θ

M) · F xi − sin(β i + θM) · F yi

− ẏM · θ̇M − aMgx1

m

4i=1

sin(β i + θ

M) · F xi + cos(β i + θM) · F yi

+ ẋM · θ̇M − aMgy1

I 4

i=1

sin(β i − γ i) · κi · F xi + cos(β i − γ i) · κi · F yi

(4.30)

where:

F yi = −(C f 1 + C f 2 · V M) · αi and F xi = τ r,irw

(4.31)

αi =

π − α∗i for π/2 ≤ α∗i


46/178

Model

The transfer function between β i and β ref,i is chosen as one, as the dynamics of the steer-ing actuators is very fast compared to the rest of the A PI . The propulsion actuator modelis dependent on the angular velocity of the wheel. The equation describing the relation between the velocity of the A PI with the individual angular velocity of the wheels is:

φ̇i = V wirw

(4.36)

where:rw is the radius of the wheels [m].V wi is the translational velocity of the wheel [m/s]:

V wi = cos(β i + θM) · (ẋM − V txi) + sin(β i + θM) · (ẏM + V tyi) (4.37)

4.3.5 Summary

The kinematic and dynamic models models each describe the posture vector (χ) in differ-ent ways. However the dynamic model has potential to be more accurate as it factors inthe forces acting on the robot, such as the sideways friction forces and the gravity force.This can also be seen in the assumptions made for the kinematic model in Sec. 4.2 onpage 35. Therefore this model is chosen as the primary model used throughout the restof the project. The kinematic model does however add some useful results as well. Thedynamic model and the I CR -part of the kinematic model are used together when the A PIis moving on a circle with radius R as illustrated in Fig. 4.14

ICR Steering act.

Propulsion act.

Dyn. model

β i,ref R β i

τ i,ref τ i

χ̇β i

Figure 4.14: The dynamic model.

4.4 Hybrid Modeling

As the dynamic model described in the previous section is nonlinear, a linear model based approach to FDI is not directly applicable. To facilitate the use of linear model based F DI, a hybrid model of the non-linear system is developed. The concept is to makea complete hybrid approximation of a given path before the A PI robot begins to traversethe field.

The hybrid model will contain enough states to make it possible for the API to travelon a predetermined path and have a linear model based F DI scheme detect and partiallyisolate a number of faults as defined in Cha. 6 on page 61.



47/178

4.4 Hybrid Modeling

4.4.1 Typical scenarios

The AP I will spend most of its operation time traversing a field by following the croprows running the length of a field as seen in Fig. 4.15. These crop rows will have different

Figure 4.15: Typical field row placement.

distances between them and the field will be oriented differently geographically but therobot will typically traverse the field in the same way: Driving in straight lines alongthe length of the field and turning in circles at the ends of the rows as shown in figure

Fig. 4.16.

- I C R

I C R

θM

M-frame

Figure 4.16: Typical driving scenario.

Group 1032b 47


48/178

Model

4.4.2 Path approximation

When a path has been selected for the A PI robot the next step is to approximate the non-linear behavior of the model with a number of linear models.

To facilitate this an algorithm has been developed in MATLAB which take the differentIC Rs and angles the A PI will be travelling on, finds the desired number of working points

and then linearize the dynamic model in the working points as shown in App. D.1 onpage 145. The linear model can be seen in Eq. (D.5). The result is a finite number of linearmodels which can approximate the entire planned path.

The path on which the API will be travelling can be described with straight lines andcircle segments. The straight lines can be described by a single working point, but as seenin Fig. 4.17 the inherent non-linearities of the circle segments makes it necessary to haveseveral working points to describe the A PI robots behavior accurately.

0 5 10 15 20 25 30−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

xM

YM

0 5 10 15 20 25 300

θM

Time [s]

V e l o c i t y [ m

/ s ]

P o s i t i o n [ r a d ]

π

12

π

32

π

2π

Figure 4.17: Sinusoidal behaviors in Turning mode scenario.

Figures 4.18 to 4.19 on the facing page show a comparison of different number of stateson a full revolution of the A PI robot.

In this case 16 states are deemed minimum for describing motion, when driving on acircle. One state for each π/8 section of the circle.

4.4.3 Defining the Hybrid Model

To define the hybrid model, a hybrid tuple is used. The hybrid tuple define the differentoutputs and inputs as well as the states of the model in a standardized way as describedin the literature[2].

H 1 = {Q,X,U,Y, Init, f } , (4.38)



49/178

4.4 Hybrid Modeling

0 0.5 1 1.5 2 2.5 3

−1.5

−1

−0.5

0

0.5

1

1.5

Figure 4.18: The hybrid system with 8 dis-crete states per turn compared with thenonlinear system.

0 0.5 1 1.5 2 2.5 3

−1.5

−1

−0.5

0

0.5

1

1.5

Figure 4.19: The hybrid system with 16discrete states per turn compared with thenonlinear system.

where

Q1 = {q 1, q 2 . . . q n} (4.39)X 1 =

ẋ, ẏ, θ̇, θ

(4.40)

U 1 = {U D1 ∪ U C 1} = {β 1, β 2, β 3, β 4, τ 1, τ 2, τ 3, τ 4} (4.41)Y 1 = {Y D1 ∪ Y C 1} =

ẋ, ẏ, θ̇, θ

(4.42)

Init =

q 1, ẋ = ẋ0, ẏ = ẏ0, θ̇ = θ̇0, θ = θ0

(4.43)

f =

f 1

(q 1

, (ẋ, ẏ, θ̇, θ), (β 1

, β 2

, β 3

, β 4

, τ 1

, τ 2

, τ 3

, τ 4

))

f 2(q 2, (ẋ, ẏ, θ̇, θ), (β 1, β 2, β 3, β 4, τ 1, τ 2, τ 3, τ 4))...

f n(q n, (ẋ, ẏ, θ̇, θ), (β 1, β 2, β 3, β 4, τ 1, τ 2, τ 3, τ 4))

4.4.4 Test case

Choosing three different I CRs a test path for the hybrid model is formulated. The aim isto prove the hybrid concept for the A PI robot model by selecting enough states to be ableto test and verify the method. The path is shown in Fig. 4.20 on the next page.

In order to test the hybrid concept a hybrid observer is needed. This will be designed

later. The hybrid model tested in with a hybrid observer in Sec. 11.1 on page 110.

4.4.5 Partial conclusion

A hybrid model has been defined for the API robot. It is designed to estimate the AP Irobot’s behavior within a predefined working area. A working algorithm has been devel-oped to facilitate this. The actual test is performed when the observer has been designedlater in the project.

Group 1032b 49


50/178

Model

−4 −2 0 2 4 6 8 10

0

2

4

6

8

10

12

y l a b e l

xlabel

Non−linear Model

Figure 4.20: The test path for the A PI robot used to verify the hybrid model.

4.5 Model Verification

This section deals with the with the verification of the dynamic model, described inSec. 4.3 on page 39. The model is verified using the path, shown in Fig. 4.21 on the facingpage with the control signals shown in Fig. 4.22 on the next page. The test is performedwithout a path controller and on asphalt assumed to be level. As some modificationshas been made to the API since the last parameter estimation was performed, a numberof parameters have changed. These parameters are the mass m, moment of inertia I ,Propulsion actuator gain τ ref gain as well as the two variable describing the cornering stiff-ness C f 1 and C f 2. These variables is used in the parameter estimation, meaning that thevariables is compromised of both the actual physical value but also of model variations.The new values for the parameters are:

• Mass: m = 231.5kg.

• Moment of inertia: I = 83.5kg · m2.• Propulsion actuator gain: τ ref gain = 0.74.

• Cornering stiffness: C f 1 = 40N/rad and C f 2 = 4000N·s/rad·m.

The results of the verification run can be seen in Fig. 4.23 on page 52 As can be easily seenthe dynamics of the simulated velocities match the measured velocities. The only differ-ence is an offset on the ẋMand ẏMvelocities, which has proven difficult to remove withparameter estimation using the existing model. This can be caused by the assumptionsmade in the model as well as physical influences of the API which is not included in themodel. This includes the suspension on the front wheels of the robot which is not mod-elled. The surface on which the A PI runs is also significant an the absence of an accurate



51/178


−6 −5 −4 −3 −2 −1 0 1−4

−3.5

−3

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

Verification path

xM[m]

y M [ m ]

Figure 4.21: The path chosen to verify the dynamic model.

0 5 10 15 20 25 30

−50

0

50

βref,1

βref,2

βref,3

βref,4

0 5 10 15 20 25 300

10

20

30

40

50

τref,1

τref,2

τref,3

τref,4

Time [s]

S t e e r i n g R e f e r e n

c e [ d e g . ]

P r o p u l s i o n T o r q u e R e f e r e n c e [ % ]

Figure 4.22: The reference signals used for the path chosen to verify the dynamic model.

Group 1032b 51


52/178

Model

0 5 10 15 20 25 30

−0.5

0

0.5

Measured

Simulated

0 5 10 15 20 25 30

−0.5

0

0.5

Measured

Simulated

0 5 10 15 20 25 30

−0.5

0

0.5

Measured

Simulated

Time [s]

˙ x M

˙ y M

˙ θ M

Figure 4.23: Comparison of the measured and simulated velocities.

model of the friction and stiction between each tire and a given surface has an impactof model performance. The noise on the sensor measurements is also a significant factor,when driving at a slow speed like this test was performed at as the. A model verificationperformed by a previous group at a higher speed showed a higher match between themeasured and simulated response of the A PI.

Furthermore the steering actuators have different dynamics as well as a small timeoffset in relation to one another. An example is shown in Fig. 4.24 on the next page. Thedifferences are not intentional and is caused by incorrectly tuned steering controllers or asteering controller tuned to a different surface, than what this test is performed on. This being said the dynamics of the turning wheels are much faster than the rest of the systemand is not deemed necessary to model more precisely.

4.5.1 Conclusion

It can be concluded that the model fits reality well and could possibly be made to fit even better as indicated in a previous project[4]. The uncertainties discussed were are deemedsufficient to account for the deviations between model and reality.

In this thesis the focus is on model based FDI. This means that an accurate modelis important. The level of accuracy needed depends on what kind of faults are to be

detected and how well the model is able to replicate them. Even though the faults arenot verified on the model it is a good assumption that if the model works in the fault freecase then it will also work in the faulty case if the fault can be represented in the model.



53/178


10 10.2 10.4 10.6 10.8 11 11.2 11.4 11.6 11.8 12

−50

−40

−30

−20

−10

0

10

20

30

40

50

β1

β2

β3

β4

Time [s]

S t e e r i n g P o s i t i o n [ d e g . ]

Figure 4.24: The measured position of the steering actuators. The steering reference onall four wheel is changed at the same time.

Group 1032b 53


54/178


55/178

Part III

Fault Detection and Isolation

55


56/178


57/178

Chapter 5

Introduction

The ability to detect, isolate and if possible accommodate faults or failures is especiallyimportant in an autonomous system because it is meant to run unsupervised for longperiods of time. So to ensure safe operation and to maximize the time the system can con-tinue operation an F DI-scheme must be designed and implemented in the API robot. Thistask has already been undertaken once by another group which laid the foundation forthe continued work in the current project. As seen in Fig. 5.1 the A PI robot has a numberof different sensors, actuators and communication interfaces. They all have the potential

Compass

GPS Inclinometer

Gyro Prox. sensors

PC-104

CAN bus

Doppler radar LH28

Propulsion cont. Steering cont.

Propulsion act. Steering act.

Propulsion sens. Steering sens.

Figure 5.1: The different subsystems of the A PI robot.

to fail at some point. The purpose of the FDI scheme described in this part is mainly to

57


58/178

Introduction

provide a way to detect and isolate faults that occur in the steering and propulsion sub-system of the API robot using model based methods. The remaining subsystems apartfrom the inclinometer and proximity sensors are already covered in a previous project [4]and will thus not be covered in the current F DI scheme.

The newly implemented inclinometer and proximity sensors are not included in themodel based FDI but the inclinometer has diagnosing capabilities. The proximity sensors

will receive a simple sanity checking in the implementation.

The following will be included in the F DI part of this master thesis all concerning theFault Detection and Isolation of the steering and propulsion system of the A PI robot:

Fault Analysis and Severity Assessment: The steering and propulsion system of theAP I robot is analysed for faults and simulations are carried out which together with em-pirical knowledge of the system will help determine their severity as well as how thefaults propagate in the system.

Each fault is then ranked according to their severity and occurrence(SO-index). TheSO-index is found by giving each fault a value from 1-10 according to the severity and

a value according to the predicted occurrence frequency. These values multiplied yieldsthe S O-index used.

Isolation Analysis: This analysis is carried out in order to determine how differentfaults affect the system. Using the linearization algorithm described in Sec. D.2 on page 145the model is linearized in two different working points in order to determine isolabliltyof input faults in the A PI .

Linear model based FDI : This chapter holds the analysis and test a linear FDI schemefor the API robot. It is a hybrid scheme which uses a hybrid state observer and linearFDI. During the chapter, three different hybrid state observers are tested as well as twodifferent continous linear F DI methods.

Non-linear Particle based FDI : This chapter holds a proof of concept of how a non-linear particle filter based could be used to detect and isolate faults occuring on the robot.

Active Fault Isolation Supervisor: This chapter describes the design of a active faultisolation supervisor able to complete the isolation of a given fault in the steering andpropulsion system of the robot by actuating on the A PI robot.

FDI schemeThe above methods can be used alone or as a part of an F DI scheme to use the best fromeach method. One method could be only applying the linear FDI scheme. Its relativelylight weight but cannot isolate faults completely. Another possibility is to use the particlefilter method alone, but it is very computationally demanding and needs an exact modelof each fault to isolate. In order to use the lightweight characteristica of the linear FDIand still have the ability to isolate faults the following F DI scheme is proposed:



59/178

When a path is planned for the A PI robot a hybrid representation of that path is madeusing the hybrid model. The path is then run with linear FDI activated. When a fault isdetected the A PI is stopped and the next step is initiated.

This step includes the application of either a active FI routine. This should result in anisolated fault and if possible remedial actions can be initiated. The scheme will only berun after the usual sanity checking of rate and value of the sensors stay fault free. The

overall scheme can be seen in Fig. 5.2.

Path planned

Create hybrid states for planned path.

Run path with linear F DI.

Check for fault event.

Descision

Stop A PI robot.

Run active fault isolation

Fault Isolated

Initiate possible remedial action.

No Fault

Fault

Figure 5.2: Overall FDI scheme.

Group 1032b 59


60/178


61/178

Chapter 6

Fault Analysis

The following contains a fault analysis of the steering and propulsion system on the A PIrobot as well as the newly implemented inclinometer and proximity sensors. This in-cludes the sensors shown in Fig. 6.1. Faults on all these subsystems are considered except

• Four propulsion actuators

• Four propulsion sensors

• Two proximity sensors

• Eight wheel stoppers

• Four steering actuators

• Four steering sensors

• One Inclinometer

Figure 6.1: Sensors and actuators fault analyzed.

for the eight wheel stoppers. These are safety features and assumed to be fault free. Allsteering and propulsion faults are simulated and possible causes and effects and are dis-cussed. The faults are simulated in open loop although the robot will be running usinga speed and path controller in the real world and thus behave a bit differently. Howeverthe wheel controllers are taken into consideration in the fault modelling so the simula-tions should give a good indication of the severity of the different faults. The faults andtheir effect of the A PI are based on the assumption that all wheel controllers are active.

The conditions for the simulations are:

Straight driving ICR = 0m.

• Simulation time = 60 seconds.

• Fault time = 3 seconds.

Turning ICR = 5m.

• Simulation time = 75 seconds.

• Fault time = 3 seconds.

All faults are single faults as two simultaneously occurring faults is very unlikely. Fur-thermore all faults are abrupt faults. That is they don’t grow over time as faults whichare a result of wear and tear do. The faults are rated for severity and occurrence using

61


62/178

Fault Analysis

the S O-index for each fault. This will help to determine which faults are more importantto detect.

Propulsion actuators

The propulsion actuators consist of four Heinzman DC

-motors which are incorporatedinto the hub of each wheel. They are integrated with built in speed sensors and run by a signal from one of the LH28 micro controllers dedicated to each wheel. The faults,considered in the propulsion actuators, are described in Table 6.

Fault Description/Local effect

No actuation The wheel isn’t applied any torque no matter what speedreference is sent to the controller. The wheel can still ro-tate(free run).

Max. positive

actuation

The wheel spins at maximum positive speed no matterwhat speed reference is set.

Max. negativeactuation The wheel spins at maximum negative speed no matterwhat speed reference is sent to the controller.

Actuator offset The wheel gain of the propulsion actuator changes to bemore or less than expected.

Table 6.1: Effects of propulsion actuator faults.

No actuation has impact on the state of the AP I robot as seen in Fig. E.1- E.2 onpage 148 and Fig. E.3- E.4 in App. E. The effect is not severe and it will more than likely bepossible to compensate with the rest of the propulsion actuators if correctly detected andisolated. Max. positive actuation and Max. negative actuation are moresevere although probably less likely. These also have potential to blow the main fusesof the API robot which is a worst case scenario since all subsystems will fail completelydue to lack of power. It is important to detect and isolate such a fault fast. The faultActuator offset is probably the least severe of the propulsion actuator faults sinceit can be removed by a controller as long as the offset is not larger than to allow for thecompensation. The effects can be seen Fig. E.9- E.10 in App. E.

Propulsion sensors

The propulsion sensors are hall effect sensors integrated into the Heinzman units situatedin each wheel hub and output the current speed of a given wheel to the LH28 micro con-

trollers dedicated to each wheel. The Henizman has a built-in safety feature which meansthat if the sensor output does not look into a specific impedance the motor goes into afree run states and thus no longer provide torque to the wheel. This will to some extentguard against electrical malfunctions. The propulsion sensors are not used for control of the wheels in the current configuration of the robot as its speed is controlled through anestimate of the current speed. This estimate depends mostly on the GPS measurements.That means that when the GPS is in action a fault in a wheel sensor will have almost noimpact on the API robots performance. Therefore the faults are not simulated but only



63/178

Fault Possible cause

No actuation Faulty relay, broken wires in either interface or the DC-motor itself. Mechanical causes include a broken shaft or bearing. It could also be caused by a bug in LH28 softwareor software on the OBC . Impedance has changed on sensoroutput.

Max. positive

actuation

Possible causes are a bug in software on the LH28 or soft-ware on the OBC.

Max. negative

actuation

Possible causes are a bug in software on the LH28 or soft-ware on the OBC.

Actuator offset A change in the ac

ploughing calculations

Documents

Transcript of ploughing calculations