Autonomous Agricultural Robot

Autonomous Agricultural Robot towards robust autonomy

Group members of IAS10-1032b

Martin Holm Pedersen Jens Lund Jensen

Fall 2006–Spring 2007 A ALBORG U NIVERSITY D EPARTMENT OF E LECTRONIC S YSTEMS A UTOMATION AND C ONTROL

The Faculty of Engineering, Science and Medicine Intelligent Autonomous Systems THEME: Final Thesis TITLE: Autonomous Agricultural Robot: towards robust autonomy PROJECT PERIOD: IAS9–IAS10 Sep. 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 IN APPENDIX: 50 TOTAL NUMBER OF PAGES: 178

Abstract: This master thesis documents the work of group 1032b and concerns the development of a model based (Fault Detection and Isolation)FDI scheme to detect and isolate faults in an four wheeled agricultural robot called an API (Autonomous Platcare Instrumentation system). The thesis describes a number of different methods for deploying different FDI strategies as well as preliminary testing on a preexisting non-linear model of the robot. Furthermore the thesis documents the efforts to make the API a more reliable and robust platform with the design at implementation of new sensors as well as steps to make is possible for the robot to diagnose itself. The different FDI methods were tested successfully on the non-linear model and were able to detect and isolate some of the selected faults. A new inclinometer was designed and implemented on the robot to replace the old. Two new proximity sensors were designed and implemented.

Preface This report is written by two students as their master thesis at Aalborg University - department of Electronic Systems. The thesis is the final part of the specialisation in Intelligent Autonomous Systems. This report is the documentation of the work conducted in the period from September 2006 to June 2007 and is focused on fault detection and isolation and the API 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 companies and is a pilot project to determine the feasibility of using autonomous platforms for field monitoring 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 additional hardware in order to make the API a more robust and reliable platform for future research groups. The report is divided in five parts: Part one is a description of the project and the specification of requirements. The second part is modeling. The third part deals with designing and testing various methods of fault detection and isolation. The fourth part is 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 chapter number, such as [7, Chapter 2]. The enclosed CD contains the report in PDF and PS formats and the MATLAB and Ansi C source code.

Martin Holm Pedersen

Jens Lund Jensen

5

Nomenclature and Abbreviations General Nomenclature N M B RX →Y ψx , ψy

Fixed global reference frame Global reference frame rotated with the inclination of the robot Robot reference frame which is fixed with the robot The rotation matrix from the X to the Y-frame Angles describing the rotation between N and M

[rad]

Nomenclature for Mechanical Modelling θM y x χ χ˙ χ ¨ β rw φ˙ ywi xwi yICR xICR Fx τr Cf α V

The angle of the robot with respect to the x-axis of the M- [rad] frame The absolute position in the y-direction [m] The absolute position in the x-direction [m] The absolute position vector of the robot consisting of [x y θ]T ˙T The velocity vector of the robot consisting of [x˙ y˙ θ] ¨T The acceleration vector of the robot consisting of [¨ x y¨ θ] The angle of the wheels with respect to the body of the vehicle The radius of the wheels The angular velocities of the wheels The y-position of the i’th wheel in the B-frame The x-position of the i’th wheel in the B-frame The y-position of the ICR in the B-frame The x-position of the ICR in the B-frame The propulsion forces provided by the wheels: [Fx1 Fx2 Fx3 Fx4 ] The propulsion torque provided by the actuators The cornering stiffness of the tires The slip angles of the wheels The velocity of the robot

[rad] [m] [rad/s] [m] [m] [m] [m] [N] [Nm] [N/rad] [rad] [m/s]

7

v κ γ βmax β˙max FgM Cf 1 Cf 2 Ra Km

The speed of the robot Lengths describing the positions of the wheels(wrt. GC) Angles describing the positions of the wheels (wrt. GC) The maximum angle of the wheels with respect to the body of the vehicle. 2βmax defines the area between the two stoppers. The maximum speed of the turning actuators The gravity force affecting the robot in the M-frame The cornering stiffness stiction constant The cornering stiffness dynamic constant Armature resistance of the actuators Motor constant which is equal to the torque constant Kt and the electrical constant Ke

[m/s] [m] [rad] [rad]

[rad/sec] [N] [N/rad] [N/rad] [Ω] [ Nm A ]

Abbreviations ADI API CM BFDF COM DGPS FDI FSB FTC GC GPS ICR OBC PDF PF PF-FDI PWM SA SF SO SPI

8

Active Fault Isolation Autonomous Plant-care Instrumentation system Centre of Mass Beard Fault Detection Filter COMmuniation system Differential Global Positioning System Fault Detection and Isolation Front Sensor Board Fault Tolerant Control Geometrical Centre of robot Global Positioning System Instantaneous Centre of Rotation OnBoard Computer Probability Density Function Particle Filter Particle Filter - FDI Pulse Width Modulation Structural Analysis Sensor Fusion Severity Occurrence index Serial Peripheral Interface Bus

Aalborg University 2007

Contents I Introduction

13

1 Background 1.1 The API Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Project Focus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

15 16 17

2 System description 2.1 External Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Internal Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 API Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

19 19 19 21

3 Additional Hardware and Software 3.1 Forward Proximity Sensors . . 3.2 Inclinometer . . . . . . . . . . . 3.3 Front Sensor Board . . . . . . . 3.4 Remote Shutdown of Wheels .

25 25 26 27 29

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

II Modeling

31

4 Model 4.1 API Geometry . . . 4.2 Kinematic Model . 4.3 Dynamic Model . . 4.4 Hybrid Modeling . 4.5 Model Verification .

33 33 35 39 46 50

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

III Fault Detection and Isolation

55

5 Introduction

57

6 Fault Analysis 6.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

61 67 9

CONTENTS

7 Isolability analysis

71

8 Linear Model based FDI of Steering and Propulsion System 8.1 Hybrid State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 Continuous State Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

75 75 81 88

9 Nonlinear Particle Filter Based FDI of Steering and Propulsion System 9.1 Requirements for Particle Filter-FDI . . . . . . . . . . . . . . . . . . 9.2 Particle Filter Design . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3 FDI using Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . 9.4 Performance Parameters with regards to Particle Filter-FDI . . . . . 9.5 Preliminary Test of Particle Filter-FDI Method . . . . . . . . . . . . 9.6 Preliminary Conclusion . . . . . . . . . . . . . . . . . . . . . . . . .

91 91 94 95 96 97 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 13.1 Test of Steering Fault Isolation. . . . . . . . . . . . . . . . . . . . . . . . . . 13.2 Test of Propulsion Fault Isolation. . . . . . . . . . . . . . . . . . . . . . . . . 13.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

119 119 122 122

14 Conclusion

123

V Appendix

129

A Additional Hardware

131

B Hardware Test 135 B.1 Inclinometer Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 10


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

Part I

Introduction

13

Chapter 1

Background As farms grow in size, together with the size of the equipment used on them, there is a need for ways to automate processes, previously performed by the farmer himself, such as 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 large area. The use of robots is a rather new development as most of the existing solutions for automatic supervision, is designed for standard farm equipment, such as tractors, combines and 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 dealing with monitoring and mapping of fields or precision spraying of pesticides, a smaller robot is ideal, as it is more gentle on the crops but also to the ground. This is due to the lower 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 with monitoring 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 and denitrification[15].

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

15

Background

1.1 The API Project In the year 2000 a co-operation between Aalborg University, The Danish Institute of Agricultural Science, The Royal Veterinary and Agricultural University and 4 industrial companies was formed[7]. The goal was to create a Autonomous Plant-care Instrumentation system (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 precision spraying of pesticides. The first version of the API was a somewhat simple design using small low-weight spoke-wheels mounted on a simple frame as seen in Fig. 1.2.

Figure 1.2: The first version of the API. I 2002 a more rugged API was designed using larger knobbed wheels and a more robust 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 The Danish Institute of Agricultural Science research-center in Bygholm. A number of research projects has been performed with regards to both instrumentation, control and fault detection and isolation. The projects are: Autonom Robot til Markanalyse: This project deals with the specification, design and implementation of the first prototype of the API robot[3]. Navigation af Autonom Markrobot: This project[11] focuses on the development of a navigation system for the API. The electrical system of the API is also designed and implemented in this project. Modeling and Fault-Tolerant Control af an Autonomous Wheeled Robot: The main focus of this project[4] was the modeling and control of the API. A FDI method responsible for detecting faults in the sensors and in minor detail the actuators, is designed and implemented. A FTC scheme is also designed and implemented. The project status as of September 2006 is that the mechanical part of the project, a fully actuated four-wheeled robot, is completed. A number of sensors, such as GPS , 16


1.2 Project Focus

gyro and compass, is implemented. A hybrid controller, designed for path following, is implemented and tested. A FDI and FTC scheme is partly implemented.

Figure 1.3: The API 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 also in research areas involved in the use of larger robots in general outdoor environments. This involves the design and implementation of a FDI scheme fully capable of detecting and isolating faults occurring in the wheel actuators and sensors, as well as the design and implementation of additional hardware and software needed for reliable operation. Completing these improvements will improve the robustness and reliability of the current system and allow future research groups to use the API without a control or electronic 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 by implementing a full FDI scheme focusing on the faults occurring on the wheel actuators and sensors. This is archived though the following sub-objectives: 1. Perform a fault analysis and severity assessment of all wheel, proximity sensor and inclinometer faults.. 2. Design and implement a linear model based FDI method. Group 1032b

17

Background

3. Verify the detection of critical faults. To complement the linear methods two additional methods for FDI is to be examined, resulting in the following objectives: 1. Design and implement a non-linear FDI method. 2. Design and implement an active FDI method. The practical objectives involving the additional hardware and software are: 1. Design and implementation of proximity sensors on the API. 2. Design and implementation of a separate inclinometer in order to provide pitch and roll measurements. 3. Design and implementation of relays for disconnecting individual wheels.

1.2.2 Operational Requirements A number of requirements for the operational performance of the API, has been set by previous 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 instance driving 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 potentially dangerous to its environment. The complete list of requirements can be seen in App. J on page 175.

18


Chapter 2

System description The following chapter is an overview of the different components that the API is equipped with. The different components are placed on the outside of the robot as well as inside the 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 the front and rear compartments shown in Fig. 2.2 on page 21 and Fig. 2.3 on page 22. For communication the API is equipped with three different antennas: • A GPS antenna used for communication between the GPS receiver and the available GPS satellites. • An omni-directional radio modem antenna for communication between the receiver and the GPS base station to facilitate the use of DGPS. • Combined WLAN device and antenna with tween the OBC and the API base station.

USB

GPS

interface for communication be-

Sensors placed on the outside are: • Compass: Measures the heading of the API relative to magnetic north. • Doppler radar: Measures the forward speed of the API. • Inclinometer: Measures the pitch and roll of the API. • 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

System description

WLAN

Compass

Radio modem antenna GPS antenna Inclinometer

Proximity sensors

Doppler Radar

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

20


2.3 API Subsystems

•

LH 28 : Embedded micro controller sampling all propulsion and steering sensors as well as controlling the steering and propulsion actuators. Communicates with the OBC via a CAN bus.

• H-bridge: Driver step responsible for outputting PWM signals to the steering actuators. • Interface Electronics: Interfaces the sensors to the LH28 controller as well as providing the interface from LH28 to the steering and propulsion actuators. In addition to the common components the front compartment contains a Gyro, GPS receiver and Radio Modem. The front compartment also houses the Inclinometer and Proximity Sensor unit. The DC/DC converter, providing the voltage for the 5Vcc and 12Vcc power bus, is placed below the Inclinometer and Proximity Sensor unit. The back compartment contains the OBC which provides the overall control of the API robot and the battery charger.

H-bridges

LH28

GPS receiver

Radio modem Shutdown interface

Inclinometer and prox. sensor unit

Gyro

Interface electronics

Figure 2.2: Front compartment of the API 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, Doppler Radar, OBC, COM, Compass, GPS , Proximity Sensors and Inclinometer. Each subsystem is divided into smaller modules as seen in Fig. 2.3 on page 23, where the boxes with rounded corners symbolizes hardware modules. The square boxes are software modules. Group 1032b

21

System description

LH28

Shutdown interface

H-bridges

OBC

Charger

Interface electronics

Figure 2.3: Back compartment of the API robot.

22


2.3 API Subsystems

API robot

Wheels

Gyro

4 Prop. act.

Temp. sens.

Doppler radar

OBC

COM

Compass

GPS

PC-104

WLAN

Inclinometer

DGPS

4 Steer. act.

Shared mem.

CAN bus

4 Prop. sens.

Controller

4 Steer. sens.

Sensor Fusion

Proximity sensors

Inclinometer

Temp. Sens

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 API the possibility of moving in any direction due to the individual steering on all four wheels. The wheel system is based on four steering actuators and four propulsion actuators. The wheels are controlled by a LH 28 microcomputer. The available sensors are steering angle and wheel speed. Gyro The gyro subsystem provides a measurement of the vertical angular velocity of the API. As the gyro measurement is dependent on the temperature, a temperature sensor is available to correct the measurements. Doppler radar The Doppler radar measures the APIs speed using microwaves. The measurement is calculated using the doppler shift of the transmitted microwaves. The Doppler radar measurements are collected by the LH28 responsible for wheel 1. OBC The on-board computer is the the overall control system. The OBC is also responsible for collecting measurements from the gyro, compass, GPS, proximity sensors and inclinometer. The OBC is a PC104 stack with a 133MHZ CPU. The OS on the OBC is Linux. To facilitated manual control of the API a monitor, keyboard and joystick is also implemented. COM The communication subsystem provides the communication between the OBC and the four LH28s as well as the communication between the API and the base station. The communication between the OBC and the wheel nodes are based on the CAN bus and consist of the steering and propulsion references and the sensor measurements provided by the wheels. The link connecting the API to the base station Group 1032b

23

System description

is standard WLAN. The communication between base station and API is the waypoints, describing a path in addition to function as a monitoring system, when the API is operating autonomously. Compass The compass provides the heading of the API with regards to the magnetic north. 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 in the Compass. GPS The GPS system provides the position of the API . The standard GPS precision is increased by using DGPS. The DGPS precision is obtained using a base station with a fixed position. The base station can then calculate the error on the GPS measurements and transmit the results to the API using the radio modem. Proximity Sensors The proximity sensors provide a distance measurement of objects in front of the API. This distance can be used for obstacle avoidance and as an emergency system, in case of a possible collision. Inclinometer The inclinometer is placed close to the geometric center of the measures the pitch and roll of the robot.

24

API

and


Chapter 3

Additional Hardware and Software When the API robot was handed over from the last group it was clear that some modifications were necessary on the physical system in order to make it more robust and more usable for the current and future projects. The main problems were: • The implemented WLAN solution was not very robust or very usable as it didn’t have the desired range and was prone to failure when used. • The interface electronics boxes which are a very important link between the LH28s and the different sensors and actuators related to each wheel were unreliable and impractical in use. • The only inclinometer on the API robot housed in the compass is fluid based and sensitive to vibrations to the point where the measurements are nearly unusable. • The API 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 robot.

WLAN

device is implemented and the old subsystem removed from the

• New interface electronics boxes are designed and implemented on the robot. • A new accelerometer based inclinometer which is less sensitive to vibrations is designed 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 and software needed to improve the existing base on the API robot.

3.1 Forward Proximity Sensors The API is in its current state essentially blind. There are no sensors dealing with the environment. This means that when encountering an obstacle, the robot just continues its operation. This can result in serious damage to both the API and the obstacle as the API weighs approximately 225kg and has been measured to reach speeds of at least 2.2 m/s. 25

Additional Hardware and Software

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 a stopping distance measured to be about Ddyn =1.2 m. The total distance travelled from detection of an obstacle until the API robot comes to a complete stop is: Dstop = Dsample + Dprocess + Ddyn

(3.1)

The API robot proximity sensors are sampled once every 60ms yielding a sample distance at 2.2 m/s of: Dsample = 2.2 m/s · 0.060s = 0.132m

(3.2)

Dprocess 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 laser to sense distances but each have very different detection area and volume. While laser sensors generally have long range and accuracy, none provide the wide sensing volume of the ultrasonic sensor. In light of this and based on the assumption that the API will drive in a straight line most of the time, it is decided to outfit the API with two forward facing proximity sensors. This allows the API to sense any object directly in front of it and take appropriate countermeasures. The chosen sensors are two SensComp 6500 Ranging Modules[17] and two SensComp 7000 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 the actual range sampling. The result is transmitted to the OBC via a serial RS232 connection. To save components and implementation, the additional computation power and unused ports 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 to the OBC. The sensors where tested in Sec. B.2 on page 136 and found to live up to the demands previously stated in this section as it could detect obstacles in its path when driving 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 API affect both GPS and compass readings as well as introducing disturbances to the estimation of χM . The reason the robot is assumed level, is due to noise in the inclinometer built into the compass. Previous 26


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 noise originating from driving in rugged terrain, the API is expected to operate in. The SCA100T is mounted under the GPS antenna and is interfaced using a SPI connection to the micro controller on the forward proximity sensors. The inclination measurements can then be used to calculate the correct GPS position and the correct compass heading as well as the correct gyro measurements. App. K on page 177 shows the tilt correction. It is assumed that the API 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 on page 135. A function and vibration test was conducted and the inclinometer works as intended and the vibration sensitivity is much less than the original inclinometer.

3.3 Front Sensor Board The basis of this hardware board is a Microchip PIC16F877[14], responsible for sampling the sensors. The board and the connected sensors will be named as the Front Sensor Board(FSB). Even though only two proximity sensors are implemented, the FSB has space for and can use up to eight as seen in Fig. 3.2 on the next page. This allows future groups to easily expand the area covered by the proximity sensors. The eight sensors could be arranged with two on each side to enable the API to move around obstacles. Group 1032b

27


R2

R3

VCC 470

LED

R1 4.7k

U4

13 14 1

OSC1/CLKIN OSC2/CLKOUT MCLR/Vpp/THV

Y1 C2 22pF

470

LED

2 3 4 5 6 7

C3 22pF

8 9 10

VCC

Button

RB0/INT RB1 RB2 RB3/PGM RB4 RB5 RB6/PGC RB7/PGD

RA0/AN0 RA1/AN1 RA2/AN2/VREFRA3/AN3/VREF+ RA4/T0CKI RC0/T10S0/T1CKI RA5/AN4/SS RC1/T10S1/CCP2 RC2/CCP1 RE0/AN5/RD RC3/SCK/SCL RE1/AN6/WR RC4/SDI/SDA RE2/AN7/CS RC5/SDO RC5/TX/CK RC7/RX/DT

11 12

VDD VSS

32 31

VDD VSS

RD0/PSP0 RD1/PSP1 RD2/PSP2 RD3/PSP3 RD4/PSP4 RD5/PSP5 RD6/PSP6 RD7/PSP7

33 34 35 36 37 38 39 40 15 16 17 18 23 24 25 26 19 20 21 22 27 28 29 30

CON3 CON2 CON1

6 5 SCK SDA SDO TTL-TX TTL-RX WACT1 WACT2

Y Y

G

7

GND

C B A

9 10 11

CON1 CON2 CON3

D7 D6 D5 D4 D3 D2 D1 D0

12 13 14 15 1 2 3 4

ECHO-J9 ECHO-J8 ECHO-J7 ECHO-J6 ECHO-J5 ECHO-J4 ECHO-J3 ECHO-J2

74LS151

VCC

LED

GND

VCC

A B C

6 4 5

G1 G2A G2B

Y0 Y1 Y2 Y3 Y4 Y5 Y6 Y7

15 14 13 12 11 10 9 7

INIT-J2 INIT-J3 INIT-J4 INIT-J5 INIT-J6 INIT-J7 INIT-J8 INIT-J9

15 14 13 12 11 10 9 7

BINH-J2 BINH-J3 BINH-J4 BINH-J5 BINH-J6 BINH-J7 BINH-J8 BINH-J9

74LS138-J2

PIC16F877 R4

1 2 3

VCC

470

1 2 3

A B C

6 4 5

G1 G2A G2B

Y0 Y1 Y2 Y3 Y4 Y5 Y6 Y7

74LS138-J1

Figure 3.2: The diagram of the Front Sensor Board.

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

28


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 FDI and FTC, a shutdown box capable of disabling the power to the individual wheels, is implemented on the API . This will enable the control system to power off one or more wheels in case of emergency or as a method of fully isolating steering and propulsion faults. The current version of the API is not capable of turning off actuators, which in situations with faults occurring on the wheels can result in limited control of the API and in some cases can cause actual damage to the robot. The requirements for the shutdown boxes are: 1. It must not interfere with or disable the emergency stop buttons, mounted on the robot. 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 parallel port on the OBC . The emergency stop system on the API is implemented by a previous group as 4 emergency 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 remote shutdown can be implemented between the emergency switches and the relays for the wheels. The proposed method is to use a transistor placed before the relay, to enable the power to the relay. The transistor is pulled high by the OBC, triggering the relay and Group 1032b

29


24V NC COM NO A

2

1.6k

1

BYV28-200 LED

B Wheel Relay 1

BC337

OBC_1 2.7k

1k 2.7k 2.7k DIODE

5V

On/OBC 24V NC COM NO 1.6k

A

2

DIODE

1

BYV28-200 2.7k

LED

B Wheel Relay 2

BC337

OBC_2

Wheel Relays 2.7k

1k 2.7k

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 on the wheel motors, a switch is added, allowing the wheels to be either permanently on or controlled by the OBC. 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 the compartment. 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 wheels on or off. Furthermore the functionality of the emergency switches is not bypassed. This means that the functionality described above has been successfully implemented.

30


Part II

Modeling

31

Chapter 4

Model This chapter contains the kinematic and dynamic model of the API robot. The kinematic model is a mathematical model, which maps the orientation and angular velocity of the wheels to the movement of the robot. The dynamic model maps the propulsion torques and wheel orientation to the acceleration of the robot. The two models is a result of the work of many previous groups working on the model and is represented here in its latest form with few modifications. The third section describes a hybrid model, based on the dynamic model.

4.1 API Geometry As mentioned in the System Description, the API is a four wheeled robot with an steering actuator and propulsion actuator on each wheel. The dimensions of the API is shown in Fig. 4.1 150cm

30cm

68cm

100cm

22cm

50cm

27cm

34cm 76cm 60cm 107cm

rw =23cm 35cm

100cm

15cm

100cm

Figure 4.1: Dimensions of the API robot[4]. In order to describe the position and orientation of the API a posture vector is defined as χ = [x, y, θ]T , where x and y is the position of the API oriented with angle θ in the inertial reference frame N . To simplify some of the relations between the API and the surface is is placed on as well as the relations between the different parts of the API, a number of frames is defined: N Frame This frame is the inertial reference frame and is defined as the terrain, the robot moves on as seen from above. The frame is similar to the layout of a map. 33

Model

M Frame This frame is a non-inertial reference frame and describes the same terrain as frame 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 the geometric 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 with the orientation of the robot θ M . The sensors available on the API is measured in different frames. In order to use a sensor measurement from another frame, the measurement must be rotated by the one or more of the rotation matrices shown below:

RN →M = 

cos(ψx )

0

 − sin(ψx ) cos(ψx ) sin(ψy )  |z0 | − sin(ψx ) cos(ψy ) |z0 |

sin(ψx )

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



cos(ψx )2 sin(ψy )   |z0 | cos(ψx ) cos(ψy ) |z0 |

(4.1)

where q 0 |z | = cos(ψx )2 + cos(ψy )2 − cos(ψx )2 cos(ψy )2 RB→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 with the 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 with regards to the GC is shown as the intersection of the lines: κi with angle γi . The definition of the posture vector is also shown. The values for κi and γi can be seen in Table 4.1. Parameter Value Parameter Value Parameter Value

κ1 0.707 m γ1 45◦

κ2 0.707 m γ2 135◦

m 211.5 kg

κ3 0.707 m γ3,g 225◦

κ4 0.707 m γ4 315◦

I 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. Another possibility is to use the center of mass(CM), but as the behaviour of the robot when using the GC is more intuitive, this GC is chosen. 34


4.2 Kinematic Model

β1

M

N

,1

(x w

(xw,2 ,y

w,2 )

β2

B

κ2

θM GC

β4

xM,yM

)

1

,y w,

B

κ1 γ1

γ2 γ3

κ3

γ4

κ4 ,yw,4 (x w,4

β3

)

(xw,3 ,yw,3 )

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 the angular velocity of the propulsion actuators (βi and φ˙ i ) to the velocities of the robot. The velocities are given by the vector χ˙ N . βi φ˙ i

χ˙ N

Kinematic model 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 Instantaneous Center of Rotation(ICR). Assumption 2 It is assumed that the movement of the API is pure rolling with no slip. The API robot is modeled using the B and M-frame. The posture vector is χM . When turning the API robot turns around the ICR in the M-frame. In order to calculate the ICR 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 ICR from wheel orientations βi . 2. Projecting the wheel velocities to an angular velocity around the ICR. 3. Calculate the translatory movement from the angular velocity and the vector to the ICR . Group 1032b

35

Model

ICR

(xw,1 , yw,1 ) N -frame

β1 +

(xw,2 , yw,2 )

β2 +

π 2

π 2

B θ (x, y)

β4 +

β3 +

π 2

π 2

(xw,4 , yw,4 )

(xw,3 , yw,3 )

Figure 4.4: Geometric definitions for API robot when turning around the ICR. The equations used for calculating the ICR can be derived by looking at Fig. 4.4 and Fig. 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 = ai xi + bi π yi = tan βi + x i + bi 2 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 substituted 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 ICR and the center of wheel i: yi = (xw,i − xi ) cot(βi ) + yw,i

(4.4)

The ICR can be described as the intersection between two of these lines. The result of this 36


4.2 Kinematic Model

ICR yi = ai xi + bi βi +

π 2

βi (xw,i , yw,i ) Figure 4.5: The line from the center of a wheel to the ICR. operation can be seen in Eq. (4.5) and Eq. (4.6). B y1 = yICR

xB ICR = B yICR =

∧ x1 = xB ICR ⇒

B B B xB w,i cot(βi ) − xw,j cot(βj ) + yw,i − yw,j cot(βi ) − cot(βj )

B − cot(β )y B + cot(β ) cot(β ) xB − xB cot(βi )yw,j j w,i i j w,j w,i cot(βi ) − cot(βj )

(4.5)

(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 straight line. The following two equations will be used to model the behavior of the API, when driving in a straight line: x˙ B = y˙ B =

4 1 X ˙ φi cos(βi ) 4

(4.7)

1 4

(4.8)

i=0 4 X i=0

φ˙ i sin(βi )

When the ICR has been determined, the angular velocity around it (θ˙M ) can be found from the velocity of a wheel (Vwi ) and the distance from a wheel to the ICR(ri ). This is illustrated in Fig. 4.6 on the following page and shown in Eq. (4.9). Vwi θ˙ M = ri This means that θ˙ Group 1032b

M

⇔

ri × Vwi M θ˙ = riT ri

(4.9)

can be written as in Eq. (4.10). 37

Model

ICR

θ˙M

ri Vw,i = φ˙ i · rw

Figure 4.6: The angular velocity around the ICR.    B xB cos(βi ) w,i − xICR B − yB ˙    ri =  yw,i ICR , Vwi = rw φi sin(βi ) 0 0 

⇓

M θ˙

  ˙  B xB rw φi cos(βi ) w,i − xICR B − yB   ˙  yw,i  ICR × rw φi sin(βi ) 0 0 ri × Vwi = = 2 2 T ri ri B B − yB xB − x + y w,i w,i ICR ICR   0   0  = B B B B  (xw,i −xICR ) sin(βi )−(yw,i −yICR ) cos(βi )  ˙ rw φi 2 2 B −y B (xBw,i −xBICR ) +(yw,i ICR ) 

(4.10)

This rotational velocity around the ICR will equal the rotational velocity around the GC. The translational velocity of the robot will be the tangential velocity, which can be calculated as shown in Eq. (4.11).   B     B  0 x˙ B xICR yICR y˙ B  = rICR × θ˙ M =  y B  ×  0  = −xB  θ˙ M ICR ICR 0 0 0 θ˙ M 

(4.11)

The derivative of the posture vector (χB ) can then be written as shown in Eq. (4.12) B where Eq. (4.10) is substituted into Eq. (4.11). The position of the ICR (xB ICR and yICR ) are derived in Eq. (4.5) and Eq. (4.6).   B B − yB B yICR sin(β ) − y xB − x i w,i w,i ICR cos(βi ) ICR  rw φ˙ i (4.12) χ˙ B = −xB 2 ICR 2 B B − yB xB − x + y 1 w,i w,i ICR ICR Now the velocities have been determined in the B-frame and in order to describe the motion in the N -frame, χ˙ B is first rotated into the M-frame by RB→M and is then rotated 38


4.3 Dynamic Model into N by using RM→N as in Eq. (4.13).

χ˙ N = RM→N RB→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 ICR, angular velocity, and translational velocity of 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   B B B B B yICR xw,i − xICR sin(βi ) − yw,i − yICR cos(βi )  ˙ = RM→N RB→M −xB 2 2 ICR rw φi B − yB B + y xB − x 1 w,i w,i ICR ICR

(4.14)

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

B B B xB w,i cot(βi ) − xw,j cot(βj ) + yw,i − yw,j = cot(βi ) − cot(βj )

B yICR =

B − cot(β )y B + cot(β ) cot(β ) xB − xB cot(βi )yw,j j w,i i j w,j w,i cot(βi ) − cot(βj )

4.3 Dynamic Model The dynamic model sought is a three degree of freedom model incorporating the two following properties: • Individual use of all four steering actuators. This would enable the model to function even if the wheels were not placed in the correct steering angles as required in the kinetic model. This requires the modeling of the friction caused by the wheel. • Individual propulsion torques. This enables the model to function with different propulsion forces from each wheel. The setup of the model is seen in Fig. 4.7 on the next page. The forces Fx is the propulsion forces, vi is the velocity of a wheel. The friction force of the wheel Fy is dependent on the slip angle of the wheel, α. In order to simplify the model the following assumption is made: The robot is fixed with regards to the suspension. This eliminates the roll and pith of the robot due to the effect of acceleration. The model has two types of inputs: • The steering angle of the four wheels βi . • The propulsion forces of the four wheels Fxi . and the output is the translational and rotational acceleration of the robot in the M frame:  M  x ¨ M  χ ¨ = y¨M  (4.15) θ¨M Group 1032b

39

Model V1

yM

α1 Fx 1 β1 θM

Fy 1

Fy 2 yB Fx 2

.θM

xB θM

xM

Fx 4

Fy 4

Fy 3

Fx 3

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 friction force and the slip angle: Fyi = −Cf · αi (4.16) where: • Fyi is the sideways friction force of the i’th wheel [N]. • Cf 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 expanded with viscous friction. The expanded equation is shown in Eq. (4.17). Fyi = −(Cf 1 + Cf 2 · V M ) · αi

(4.17)

where: • Cf 1 is the cornering stiffness constant, that accounts for the stiction in the system [N/rad]. • Cf 2 is the cornering stiffness constant, that accounts for the coulomb and viscous N·s ]. friction in the system [ rad·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 from Fxi to Vi in quadrant 1 and 4 and from Vi to Fxi in quadrant 2 and 3. This can be seen in Fig. 4.8 on the facing page. 40


4.3 Dynamic Model V1 Fx 1

Fx 1

β1 θM

V1 α1

V1

α1

Fx 1

β1 θM

β1 θM

Fy 1

Fy 1

α =0

o

o

o

α =−30

α =−90

β1

β1

Fx 1

θM

α1

V1

V1

V1

β 1 Fy 1,res

Fy 1,res

θM

α1 θM

Fy 1 Fy 1 o

α =−60

α1

Fx 1 Fy 1

Fx 1

o

o

α =60

α =30

Figure 4.8: Illustration of the slip angle at different steering angles. The green Fyi shown the definition of the friction force and the cyan Fyi,res shown where the definition and the resulting force differs[4]. In order to determine the slip angle it is necessary to find the true velocity of the wheel (Vi ) which consists of two components: One from the velocity of the GC (V M ) and a component from the rotation of the robot around the GC (Vti ) as seen on Fig. 4.9 on the next page. Vti is a function of the angular velocity and distance from GC of the centre of the wheel. Vti = θ˙ M · κi

(4.18)

Separating Vti into its components results in the following equations: Vtxi = θ˙ M · κi · sin(γi + θ M ) Vtyi = θ˙ M · κi · cos(γi + θ M )

(4.19) (4.20)

Using Fig. 4.10 on the following page is can be seen that x˙ M − Vtxi and y˙ M + Vtyi forms the opposite and adjacent sides of the triangle with V1 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 wheel can rotate more than 180◦ past the velocity direction resulting in the friction force pointing in the wrong direction in some cases as seen in Fig. 4.8. Case 2 and 5 should give the same friction force. To correct this behavior the slip angle is redefined as seen in Eq. (4.22) Group 1032b

41

Model

V1

.M

y

α1 Fx1 β1 θM .M x

Vty1

Vt1 Vtx1

yM

VM

κ1 γ1

xB

θM

. θM

xM

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

 for π/2 ≤ α∗i < 3π/2  π − α∗i ∗ αi = α − 2π for 3π/2 ≤ α∗i  i∗ αi for α∗i < π/2

(4.22)

Where α∗i is defined as the αi from Eq. (4.21).

4.3.2 Translational Motion To describe the translational motion of the robot, the acceleration in the M-frame: x ¨M M and y¨ must be derived. The acceleration is divided into four different contributions:

yM+Vty1

V1

.

. yM+Vty1

• Sideway friction forces: Fyi

Fx1

α

M

β+θ .

xM−Vtx1 Figure 4.10: The right-angled triangle formed by x˙ M − Vtxi and y˙ M + Vtyi [4]. 42


4.3 Dynamic Model

• Propulsion forces: Fxi • Centripetal force: Fc • Gravity force: Fg The Sideway friction force was defined in the previous section and the propulsion τ force is defined as Fxi = rr,i , where τr,i is the propulsion torque for the propulsion actuaw tor. These two forces are then projected onto the M-frame as seen in Fig. 4.11

V1 Fy1 β +θM 1

α1 Fx1 M

β1 +θ

Figure 4.11: The sideways friction and propulsion forces and their projection onto the M-frame[4]. This leaves the centripetal force and the gravity force. The contribution from the centripetal force can be determined using a scenario, where all wheels are perpendicular to the ICR 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 ⇔ ar = V M · θ˙M

(4.23) (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/s2 ]. V M is the velocity of the robot [m/s]. To separate the vector into its two components arx and arx , the relationship between the 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

Model

.. yM

ICR

ar

ary . yM

arx

. θM

VM . xM

.. xM Figure 4.12: The robot with centripetal acceleration shown[4]. The gravity force (F M g ) can be calculated as shown in Eq. (4.26).   M  M  Fgx agx 0  = RN →M  0   M  = m · aM FM gy g = Fgy M m·g aM Fgz gz

(4.26)

The complete equation for the translational motion is performed by summing all the forces from each of the four wheels, dividing by the mass of the API robot adding the centripetal and gravity acceleration as shown in Eq. (4.27).   M  1 P4 M · θ˙ M − aM M ) · F − sin(β + θ M ) · F cos(β + θ i xi i yi − y˙ gx x ¨ i=1 m  M   1 P4  M M M M y ¨ = χ ¨M = ˙ tran  m i=1 sin(βi + θ ) · Fxi + cos(βi + θ ) · Fyi + x˙ · θ − aM gy M ¨ θ 0 (4.27)

4.3.3 Rotational Motion The rotational motion of the robot is caused by two different forces: The friction of the wheels and the force of the propulsion actuators. These two forces results in a rotation around the GC and therefore must be projected into a line perpendicular to the line originating in the GC and crossing though the center of the wheel. This line is given by κi and γi . As seen of Fig. 4.13 on the facing page this perpendicular line is also the line on which the velocity Vti is pointing. The projection of the forces is performed using the angle βi − γi . The angular acceleration around GC can then be found by multiplying the projected forces with the arm (κi ), summing over the four wheels and dividing with the moment of inertia as shown in Eq. (4.28).   M   0 x ¨   y¨M  =  χ ¨M  0P  (4.28) rot = 4 1 θ¨M i=1 sin(βi − γi ) · κi · Fxi + cos(βi − γi ) · κi · Fyi I 44


   

4.3 Dynamic Model

Vt1

γ1

Fx 1

β1

β1 γ1

Fy 1

GC

κ1 γ1

xB

Figure 4.13: Wheel 1 and the GC of the robot with relevant vectors and angles for calculation 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. The main equations for the robot is shown here: Robot: χ ¨M = χ ¨M ¨M (4.29) rot + χ trans =   1 P4 M · θ˙ M − aM M ) · F − sin(β + θ M ) · F  M  − y ˙ cos(β + θ i xi i yi gx i=1 x ¨  mP   4 1 M M M M M ˙  y¨M  =   m i=1 sin(βi + θ ) · Fxi + cos(βi + θ ) · Fyi + x˙ · θ − agy    θ¨M 1 P4 sin(β − γ ) · κ · F + cos(β − γ ) · κ · F i i i xi i i i yi i=1 I

(4.30)

where: Fyi = −(Cf 1 + Cf 2 · V M ) · αi

and

Fxi =

 for π/2 ≤ α∗i < 3π/2  π − α∗i αi = α∗ − 2π for 3π/2 ≤ α∗i  ∗i αi for α∗i < π/2 α∗i

= tan

−1

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

τr,i rw

(4.31)

(4.32)

!

− βi − θ M

(4.33)

Actuators: τr,i

Km(d) 4.5 ∗ τrefgain · τref,i − = Ra(d) 100

βi = βref,i Group 1032b

2 Km(d)

Ra(d)

+ b(d)

!

φ˙ i

(4.34) (4.35) 45

Model

The transfer function between βi and βref,i is chosen as one, as the dynamics of the steering actuators is very fast compared to the rest of the API. The propulsion actuator model is dependent on the angular velocity of the wheel. The equation describing the relation between the velocity of the API with the individual angular velocity of the wheels is: Vwi φ˙ i = rw

(4.36)

where: rw is the radius of the wheels [m]. Vwi is the translational velocity of the wheel [m/s]: Vwi = cos(βi + θ M ) · (x˙ M − Vtxi ) + sin(βi + θ M ) · (y˙ M + Vtyi )

(4.37)

4.3.5 Summary The kinematic and dynamic models models each describe the posture vector (χ) in different ways. However the dynamic model has potential to be more accurate as it factors in the 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 on page 35. Therefore this model is chosen as the primary model used throughout the rest of the project. The kinematic model does however add some useful results as well. The dynamic model and the ICR -part of the kinematic model are used together when the API is moving on a circle with radius R as illustrated in Fig. 4.14 R

βi,ref

ICR

Steering act.

βi

Dyn. model

βi τi,ref

Propulsion act.

χ˙

τ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 FDI, a hybrid model of the non-linear system is developed. The concept is to make a complete hybrid approximation of a given path before the API robot begins to traverse the field. The hybrid model will contain enough states to make it possible for the API to travel on a predetermined path and have a linear model based FDI scheme detect and partially isolate a number of faults as defined in Cha. 6 on page 61. 46


4.4 Hybrid Modeling

4.4.1 Typical scenarios The API will spend most of its operation time traversing a field by following the crop rows 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 the robot will typically traverse the field in the same way: Driving in straight lines along the length of the field and turning in circles at the ends of the rows as shown in figure Fig. 4.16.

IC R

-IC R

M-frame

θM

Figure 4.16: Typical driving scenario.

Group 1032b

47

Model

4.4.2 Path approximation When a path has been selected for the API robot the next step is to approximate the nonlinear behavior of the model with a number of linear models. To facilitate this an algorithm has been developed in MATLAB which take the different ICR s and angles the API 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 on page 145. The linear model can be seen in Eq. (D.5). The result is a finite number of linear models which can approximate the entire planned path. The path on which the API will be travelling can be described with straight lines and circle segments. The straight lines can be described by a single working point, but as seen in Fig. 4.17 the inherent non-linearities of the circle segments makes it necessary to have several working points to describe the API robots behavior accurately.

xM

Velocity[m/s]

0.25 0.2 0.15 0.1 0.05 0 −0.05 −0.1 −0.15 −0.2 −0.25

M

Y

0

5

10

15

20

25

30

Position[rad]

2π

3 π 2

π

1 π 2

θM 0

0

5

10

15

20

25

30

Time [s] 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 states on a full revolution of the API robot. In this case 16 states are deemed minimum for describing motion, when driving on a circle. 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 different outputs and inputs as well as the states of the model in a standardized way as described in the literature[2]. H1 = {Q, X, U, Y, Init, f } , 48

(4.38) Aalborg University 2007

4.4 Hybrid Modeling

1.5

1.5

1

1

0.5

0.5

0

0

−0.5

−0.5

−1

−1

−1.5

−1.5 0

0.5

1

1.5

2

2.5

3

Figure 4.18: The hybrid system with 8 discrete states per turn compared with the nonlinear system.

0

0.5

1

1.5

2

2.5

3

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

where Q1 = {q1 , q2 . . . qn } n o ˙ θ X1 = x, ˙ y, ˙ θ,

(4.39) (4.40)

U1 = {UD1 ∪ UC1 } = {β1 , β2 , β3 , β4 , τ1 , τ2 , τ3 , τ4 } n o ˙ θ Y1 = {YD1 ∪ YC1 } = x, ˙ y, ˙ θ, o n Init = q1 , x˙ = x˙ 0 , y˙ = y˙0 , θ˙ = θ˙0 , θ = θ0  ˙ θ), (β1 , β2 , β3 , β4 , τ1 , τ2 , τ3 , τ4 )) f1 (q1 , (x, ˙ y, ˙ θ,     f2 (q2 , (x, ˙ ˙ y, ˙ θ, θ), (β1 , β2 , β3 , β4 , τ1 , τ2 , τ3 , τ4 )) f= . ..     ˙ θ), (β1 , β2 , β3 , β4 , τ1 , τ2 , τ3 , τ4 )) fn (qn , (x, ˙ y, ˙ θ,

4.4.4 Test case

(4.41) (4.42) (4.43)         

Choosing three different ICRs a test path for the hybrid model is formulated. The aim is to prove the hybrid concept for the API robot model by selecting enough states to be able to 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 API robot’s behavior within a predefined working area. A working algorithm has been developed to facilitate this. The actual test is performed when the observer has been designed later in the project. Group 1032b

49

Model

12

10

ylabel

8

6

4

2

0

Non−linear Model −4

−2

0

2

4

6

8

10

xlabel

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

4.5 Model Verification This section deals with the with the verification of the dynamic model, described in Sec. 4.3 on page 39. The model is verified using the path, shown in Fig. 4.21 on the facing page with the control signals shown in Fig. 4.22 on the next page. The test is performed without a path controller and on asphalt assumed to be level. As some modifications has been made to the API since the last parameter estimation was performed, a number of parameters have changed. These parameters are the mass m, moment of inertia I, Propulsion actuator gain τrefgain as well as the two variable describing the cornering stiffness Cf 1 and Cf 2 . These variables is used in the parameter estimation, meaning that the variables 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: τrefgain = 0.74. • Cornering stiffness: Cf 1 = 40N/rad and Cf 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 seen the dynamics of the simulated velocities match the measured velocities. The only difference is an offset on the x˙ M and y˙ M velocities, which has proven difficult to remove with parameter estimation using the existing model. This can be caused by the assumptions made in the model as well as physical influences of the API which is not included in the model. This includes the suspension on the front wheels of the robot which is not modelled. The surface on which the API runs is also significant an the absence of an accurate 50


4.5 Model Verification

1 Verification path 0.5

0

−0.5

y M [m]

−1

−1.5

−2

−2.5

−3

−3.5

−4 −6

−5

−4

−3

−2

−1

xM [m]

0

1

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

Steering Reference[deg.]

50

βref,1 β

ref,2

βref,3 β

ref,4

0

−50

Propulsion Torque Reference[%]

0

5

10

5

10

15

20

25

30

15

20

25

30

50 τref,1 τ

40

ref,2

τ

ref,3

30

τref,4

20 10 0

0

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

Group 1032b

51

Model

x˙ M

0.5 0 Measured Simulated

−0.5 0

5

10

15

20

25

Measured Simulated

0.5

y˙ M

30

0

−0.5 0

5

10

15

20

25

Measured Simulated

0.5

θ˙ M

30

0

−0.5 0

5

10

15

20

25

30

Time [s] 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 impact of 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 verification performed by a previous group at a higher speed showed a higher match between the measured and simulated response of the API. Furthermore the steering actuators have different dynamics as well as a small time offset in relation to one another. An example is shown in Fig. 4.24 on the next page. The differences are not intentional and is caused by incorrectly tuned steering controllers or a steering 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 system and 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 deemed sufficient to account for the deviations between model and reality. In this thesis the focus is on model based FDI. This means that an accurate model is 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 are not verified on the model it is a good assumption that if the model works in the fault free case then it will also work in the faulty case if the fault can be represented in the model.

52


4.5 Model Verification

50

β1

40

β3

β

2

β

4

Steering Position[deg.]

30 20 10 0 −10 −20 −30 −40 −50 10

10.2

10.4

10.6

10.8

11

11.2

11.4

11.6

11.8

12

Time [s] Figure 4.24: The measured position of the steering actuators. The steering reference on all four wheel is changed at the same time.

Group 1032b

53

Part III

Fault Detection and Isolation

55

Chapter 5

Introduction The ability to detect, isolate and if possible accommodate faults or failures is especially important in an autonomous system because it is meant to run unsupervised for long periods of time. So to ensure safe operation and to maximize the time the system can continue operation an FDI-scheme must be designed and implemented in the API robot. This task has already been undertaken once by another group which laid the foundation for the continued work in the current project. As seen in Fig. 5.1 the API robot has a number of different sensors, actuators and communication interfaces. They all have the potential Compass

GPS

Inclinometer PC-104

Gyro

Prox. sensors

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 API robot. to fail at some point. The purpose of the

FDI

scheme described in this part is mainly to 57

Introduction

provide a way to detect and isolate faults that occur in the steering and propulsion subsystem of the API robot using model based methods. The remaining subsystems apart from the inclinometer and proximity sensors are already covered in a previous project [4] and will thus not be covered in the current FDI scheme. The newly implemented inclinometer and proximity sensors are not included in the model 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 FDI part of this master thesis all concerning the Fault Detection and Isolation of the steering and propulsion system of the API robot: Fault Analysis and Severity Assessment: The steering and propulsion system of the API robot is analysed for faults and simulations are carried out which together with empirical knowledge of the system will help determine their severity as well as how the faults propagate in the system. Each fault is then ranked according to their severity and occurrence(SO-index). The SO-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 yields the SO-index used. Isolation Analysis: This analysis is carried out in order to determine how different faults affect the system. Using the linearization algorithm described in Sec. D.2 on page 145 the model is linearized in two different working points in order to determine isolablilty of input faults in the API . Linear model based FDI : This chapter holds the analysis and test a linear FDI scheme for the API robot. It is a hybrid scheme which uses a hybrid state observer and linear FDI. During the chapter, three different hybrid state observers are tested as well as two different continous linear FDI methods. Non-linear Particle based FDI : This chapter holds a proof of concept of how a nonlinear 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 fault isolation supervisor able to complete the isolation of a given fault in the steering and propulsion system of the robot by actuating on the API robot.

FDI scheme The above methods can be used alone or as a part of an FDI scheme to use the best from each method. One method could be only applying the linear FDI scheme. Its relatively light weight but cannot isolate faults completely. Another possibility is to use the particle filter method alone, but it is very computationally demanding and needs an exact model of each fault to isolate. In order to use the lightweight characteristica of the linear FDI and still have the ability to isolate faults the following FDI scheme is proposed: 58


When a path is planned for the API robot a hybrid representation of that path is made using the hybrid model. The path is then run with linear FDI activated. When a fault is detected the API is stopped and the next step is initiated. This step includes the application of either a active FI routine. This should result in an isolated fault and if possible remedial actions can be initiated. The scheme will only be run 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 FDI.

Check for fault event.

Descision

No Fault

Fault Stop API robot.

Run active fault isolation

Fault Isolated

Initiate possible remedial action.

Figure 5.2: Overall FDI scheme.

Group 1032b

59

Chapter 6

Fault Analysis The following contains a fault analysis of the steering and propulsion system on the API robot as well as the newly implemented inclinometer and proximity sensors. This includes the sensors shown in Fig. 6.1. Faults on all these subsystems are considered except • Four propulsion actuators

• Four steering actuators

• Four propulsion sensors

• Four steering sensors

• Two proximity sensors

• One Inclinometer

• Eight wheel stoppers Figure 6.1: Sensors and actuators fault analyzed. for the eight wheel stoppers. These are safety features and assumed to be fault free. All steering and propulsion faults are simulated and possible causes and effects and are discussed. The faults are simulated in open loop although the robot will be running using a speed and path controller in the real world and thus behave a bit differently. However the wheel controllers are taken into consideration in the fault modelling so the simulations should give a good indication of the severity of the different faults. The faults and their effect of the API 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. Furthermore all faults are abrupt faults. That is they don’t grow over time as faults which are a result of wear and tear do. The faults are rated for severity and occurrence using 61

Fault Analysis

the SO-index for each fault. This will help to determine which faults are more important to detect.

Propulsion actuators The propulsion actuators consist of four Heinzman DC-motors which are incorporated into 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 No actuation

Max. positive actuation Max. negative actuation Actuator offset

Description/Local effect The wheel isn’t applied any torque no matter what speed reference is sent to the controller. The wheel can still rotate(free run). The wheel spins at maximum positive speed no matter what speed reference is set. The wheel spins at maximum negative speed no matter what speed reference is sent to the controller. The wheel gain of the propulsion actuator changes to be more or less than expected.

Table 6.1: Effects of propulsion actuator faults. No actuation has impact on the state of the API robot as seen in Fig. E.1- E.2 on page 148 and Fig. E.3- E.4 in App. E. The effect is not severe and it will more than likely be possible to compensate with the rest of the propulsion actuators if correctly detected and isolated. Max. positive actuation and Max. negative actuation are more severe although probably less likely. These also have potential to blow the main fuses of the API robot which is a worst case scenario since all subsystems will fail completely due to lack of power. It is important to detect and isolate such a fault fast. The fault Actuator offset is probably the least severe of the propulsion actuator faults since it can be removed by a controller as long as the offset is not larger than to allow for the compensation. 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 situated in each wheel hub and output the current speed of a given wheel to the LH28 micro controllers dedicated to each wheel. The Henizman has a built-in safety feature which means that if the sensor output does not look into a specific impedance the motor goes into a free run states and thus no longer provide torque to the wheel. This will to some extent guard 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 an estimate 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 no impact on the API robots performance. Therefore the faults are not simulated but only 62


Fault No actuation


Possible cause Faulty relay, broken wires in either interface or the DCmotor itself. Mechanical causes include a broken shaft or bearing. It could also be caused by a bug in LH28 software or software on the OBC. Impedance has changed on sensor output. Possible causes are a bug in software on the LH28 or software on the OBC. Possible causes are a bug in software on the LH28 or software on the OBC. A change in the actuator gain due to wear and tear or a damaged bearing or shaft. A bug in the software on the LH 28 or software on the OBC could also cause this fault.

Table 6.2: Causes of propulsion actuator faults.

described. The faults in the propulsion actuators considered for detection and isolation are shown in Table 6.3. Fault No output

Description The signal from the sensor is zero no matter what speed reference is sent to the propulsion controller. The signal from the sensor is offset to what the real value is. The signal from the sensor is outside the possible range of the sensor.

Sensor offset Non sense output

Table 6.3: Considered propulsion sensor faults.

Fault No output

Sensor offset

Non sense output

Possible cause Possible causes include broken wires or a sensor circuit malfunction. It could also be caused by a bug in LH28 software or software on the OBC. A change in the sensor gain due to a mechanical failure where one of the magnetic elements are not detected leading to a average lower speed than the gain would suggest A bug in the software on the LH28 or software on the OBC could also cause this. Sensor circuit malfunction. Loose or partially broken wires. Bug in LH28 software or software on the OBC. Table 6.4: Causes of propulsion sensor faults.

Group 1032b

63

Fault Analysis

Steering actuators The steering actuators are four Maxon DC-motors with each a worm reducing gear. The wheel is PWM controlled by the LH28 micro controller through an H-bridge which amplify the current, setting the speed and torque at which a wheel is turned. Faults in the steering actuators considered for detection and isolation are show in Table 6.5. The following faults in the steering actuators are considered for detection and isolation. Fault No actuation Max. positive actuation Max. negative actuation Actuator offset

Description/Local effect The wheel does not rotate no matter what reference position is sent to the steering controller. The wheel rotates to full positive position no matter what reference position is sent to the steering controller. The wheel rotates to full negative position no matter what reference position is sent to the steering controller. The wheel position is offset in relation to the reference position.

Table 6.5: Considered steering actuator faults. Fig. E.11- E.12 in App. E shows how the No actuation fault affect the API robot. The No actuation fault is modelled as if a wheel is fixed in 0◦ so the fault does not show up when the robot is moving in a straight line. It only turns up when the wheels are turned to change the direction of the robot. It is however a severe fault and has the potential to blow the main fuse. The Max. positive actuation and Max. negative actuation faults are show in Fig. E.13- E.16 in App. E They are equally severe and have potential to blow the main fuse. The last fault is the Actuator offset fault which is shown in Fig. E.17- E.18 in App. E. The severity is proportional to the size of the offset but is generally considered severe as the robot cannot continue normal operation if it occurs. Fault No actuation


Possible cause Possible causes include faulty relay, broken wires or faulty H-bridge. Mechanical causes include a broken shaft or bearing. It could also be caused by a bug in LH28 software or software on the OBC. A fault in steering in the control loop can cause this fault. Possible causes are a bug in software on the LH28 or software on the OBC. A fault in steering in the control loop can cause this fault. Possible causes are a bug in software on the LH28 or software on the OBC. An offset on the sensor output in the control loop or a bug in the software on the LH28.

Table 6.6: Causes of steering actuator faults.

64


Steering sensors The steering sensors are optic tachometers which measure the number of holes passing on a rotating disk and enable the LH28 microcontrollers to determine the distance travelled. It does not measure a absolute position but rather a relative one. This makes it important to carry out calibration before operation of the API robot starts and to keep exact track of where the wheel moves during operation. Faults in the steering sensors considered for detection and isolation are shown in Table 6.7. Fault No output Sensor offset Non sense output

Description The signal from the sensor is zero no matter what reference signal is sent to the controller. The signal from the sensor is at an offset to what the real value is. The sensor has outputs a non-sense signal instead of the real signal. The signal is characterized by being outside the known operating area in terms of slew rate and value. Table 6.7: Considered steering sensor faults.

Fault No output Sensor offset Non sense output

Local effect The controller will turn the wheel to a maximum position. The controller will turn the wheel according to the sensor offset position. The controller will try to follow to noise signal causing the wheel to rotate erratically. Table 6.8: Effects of steering sensor faults.

Simulation of global effects of the No output fault can be found in Fig. E.13- E.14 in App. E and Fig. E.15- E.16 in App. E. The fault can either cause Max. Negative Actuation or Max. Positive Actuation depending on the value of the position reference. The effects are very severe depending on the speed of the API robot when the fault occurs. At low speeds the robot will either come to a almost complete stop as seen in the simulations or move in a small circle. At higher speeds the No output fault will blow the main fuse and possibly damage the robot severely. The fault is however not very likely because the position is the consequence of an integration of the holes in a tacho disc. So the only cause is a software bug. Sensor offset is shown in Fig. E.17E.18 in App. E and shows a moderate offset which is less severe than the No output fault but the Sensor offset fault could be equally severe. No output is more likely as it has more causes as seen in Table 6.9 on the next page. See Table 6.10 on the following page to get an overview of how the steering sensor faults propagate.

Proximity Sensors The proximity sensors are mounted in the front end of the API robot as described in Sec. 3.1 on page 25 and can observe if an obstacle is present in the path of the robot. They Group 1032b

65

Fault Analysis

Fault No output

Possible cause Disconnected cable or broken wires or causing power loss or sensor circuit malfunction. It could also be caused by a bug in LH28 software or software on the OBC. Temporary failure to detect holes in the tacho disc. A loose tacho disc. Temporary power loss. Uncalibrated wheels. Sensor circuit malfunction. Loose or partially broken wires. Bug in LH28 software or software on the OBC.


Table 6.9: Causes of steering sensor faults. Steering system

Non sense output

x

Sensor offset

Max. positive output

x x

Max. negative output

No output

Sensor

x x x

Local Effect No actuation Max. positive actuation Max. negative actuation Actuator offset Erratic wheel movement

Table 6.10: Cause and effects on steering system with wheel controller. work by the principle of active sonar, where a sound is emitted and the time from an emission until an echo is received from an obstacle is measured. This time is proportional to the distance to the obstacle. The considered faults in the proximity sensors are shown in Table 6.11. Fault No output Fixed Output Non sense output

Description The signal from the sensor is zero even though there is an obstacle in range of a given sensor. The signal from the sensor is fixed at a certain output. The sensor has outputs a non-sense signal instead of the real signal. The signal is characterized by being outside the known operating area in terms of slew rate and value. Table 6.11: Considered proximity sensor faults.

As the proximity sensors are an essentially safety features a fault could be very severe. 66


6.1 Conclusion

If gone undetected a faulty proximity sensors could cause the API robot to crash into obstacles in its path and damage itself and/or the obstacle. Worse yet is the possibility for humans or animals to get hit by the robot. Another consequence of a faulty proximity sensor is that they can stop the operation of the API robot completely if the an obstacle is erroneously detected in front of the API. Fault No output

Possible cause Disconnected cable or broken wires, power loss or sensor circuit malfunction. It could also be caused by a bug in LH28 software or software on the OBC. The sensor has become detached from its housing and is sensing the inside of the housing. It could also be caused by a bug in LH28 software or software on the OBC. Sensor circuit malfunction. Loose or partially broken wires.

Fixed output

Non sense output

Table 6.12: Causes of proximity sensor faults.

Inclinometer The inclinometer is of the type SCA100T- D02[20] mounted in the GC of the API robot and serves to measure the pitch and roll of the vehicle. Its purpose is to compensate for the inclination of the robot in the API model and of the GPS and compass measurements. Generally the inclinometer measurements are important for an accurate estimate of the Fault No output Sensor offset Non sense output

Description The signal from the sensor is zero. The signal is offset from the true value. The sensor has outputs a non-sense signal instead of the real signal. The signal is characterized by being outside the known operating area in terms of slew rate and value.

Table 6.13: Considered inclinometer sensor faults. χM -vector when the robot is on an incline. They become increasingly important if the GPS signal is lost. Faulty measurements are severe if they go undetected since it would mean that the χM -vector estimate would become unreliable.

6.1 Conclusion In conclusion the different subsystems will be rated for severity and occurrence using the SO-index as seen in Table 6.15 on page 69. The proximity sensor faults rank highest on the SO-index and with good reason. All three faults should get priority in detection and isolation. Fortunately they are all easy to detect with a sanity check when running. This will not be investigated further in the current project since the focus is on model based FDI . Next in line are faults on the steering sensors and actuators which are also a high Group 1032b

67

Fault Analysis

Fault No output


Possible cause Disconnected cable or broken wires, power loss or sensor circuit malfunction. It could also be caused by a bug in sensor board software or software on the OBC. Fault in temperature sensor would cause a small offset. The mechanical mounting could get shifted or turned. Sensor circuit malfunction. Loose or partially broken wires.

Table 6.14: Causes of inclinometer sensor faults. priority. As described the behaviour of these two subsystems is highly correlated which give them equally high SO-index. This leaves propulsion actuators and sensors as well as the inclinometer. The propulsion sensors are the ones with the lowest priority because its not really used in the current control scheme. But it would be worthwhile to find faults in the propulsion actuators and inclinometer as it has a significant impact on the API robots performance. In the current FDI scheme all faults will be investigated except faults on the propulsion sensor and the non-sense outputs.

68


6.1 Conclusion

Sensor/Actuator Propulsion actuator

Propulsion sensor

Steering actuator

Steering sensor

Proximity sensor

Inclinometer

Fault No actuation Max. positive actuation Max. negative actuation Actuator offset No output Sensor offset Non sense output No actuation Max. positive actuation Max. negative actuation Actuator offset No output Sensor offset Non sense output No output Fixed output Non sense output No output Fixed output Non sense output

Severity 4 8

Occurrence 4 3

SO-index

8

3

24

2 2 2 2

4 4 4 3

8 8 8 6

8 8

4 5

32 40

8

5

40

4–8 8–9 4–9 4–8

4 3 4 4

16–36 24–27 16–36 16–32

7 7 7

4 6 5

28 42 35

4 4 5

4 4 4

16 16 20

16 24

Table 6.15: Severity assessment

Group 1032b

69

Chapter 7

Isolability analysis The purpose of the isolability analysis is to determine how different inputs to the system affect the state of the system. Specifically to identify inputs which affect the system in different ways and if this changes with the state of the system. This will identify if input faults can be isolated. In this chapter this will be done by using linearized versions of the dynamic model of the API robot. By looking at the input matrices of each model it is possible to assess isolability in different working-points and compare them. There are basically two different types of states: Straight driving and turning. It will be explored how these states differ with respect to isolability. Later this knowledge can be exploited in the design of the FDI scheme.

Straight driving states The first case is when the robot is driving in a straight line along the on the xM -axis with 10% torque. To do this the linear state space system is introduced as described in App. D.1 on page 145. In this case it is a linearization of the non-linear API model in the working point: βref,1 : 0 βref,2 : 0 βref,3 : 0 βref,4 : 0 τref,1 : 10 τref,2 : 10 τref,3 : 10 τref,4 : 10 θwp : 0 x˙ wp : 0.2664 y˙wp : 0 θ˙wp : 0

(7.1)

Using these working points the system matrices have the following input matrix: 

0.0134  0.0000 B=  -0.0160 0

0.0134 0.0000 -0.0160 0

0.0134 0.0000 0.0160 0

0.0134 0.0000 0.0160 0

0.0000 4.8805 5.8180 0

0.0000 4.8805 -5.8180 0

0.0000 4.8805 -5.8180 0

 0.0000 4.8805   (7.2) 5.8180  0

Looking at Eq. (7.2) it can be seen that some columns of the input matrix B have the 71

Isolability analysis

same direction as specified below. bτref,1 = bτref,2 bτref,3 = bτref,4 bβref,1 = bβref,4 bβref,2 = bβref,3

(7.3)

This means that when the API is moving in the designated working point it is only possible to isolate a propulsion actuator fault down to either wheel 1 or 2 or wheel 3 or 4. The same thing holds true in the steering. Here faults register in such a way that the FDI can only isolate steering actuator faults down to either wheel 1 or 4 or wheel 2 or 3. The wheel pairs are shown in Fig. 7.1. This limitation has nothing to do with the modeling itself but is rather a limitation in the physical system which cannot be overcome using passive FDI. Additionally it can be concluded that this limitation will exist for any θ as coupling between the wheels are independent of the current heading of the API robot. Propulsion Pair 1-2

Steering Pair 1-3

Steering Pair 2-4

Propulsion Pair 3-4

Figure 7.1: The wheel pairs and the possible isolation of faults.

Turning states After looking at the straight driving scenario next in line is the turning scenario. The linearization of the non-linear API model turning in a circle around an ICR of 0.6m yielded the following working points: βref,1 : 1.3734 βref,2 : −1.3734 βref,3 : −0.4266 βref,4 : 0.4266 τref,1 : 10 τref,2 : 10 τref,3 : 10 τref,4 : 10 θwp : 6.0868 x˙ wp : 0.1563 y˙ wp : −0.0314 θ˙wp : 0.2665 72

(7.4)


Using the working points the input matrix has the following values:  0.0051 0.0000 0.0109 0.0130 -2.8184 3.0559 1.7249  0.0124 -0.0134 -0.0078 0.0031 1.1819 0.0159 2.4087 B=  0.0125 0.0125 0.0211 0.0211 4.2966 -4.2986 -1.7608 0 0 0 0 0 0 0

 -0.6767 2.8844   1.7530  0

It can be seen that none of the columns have exactly the same direction and that it is therefore theoretically possible to isolate a faulty input by looking at the output when the robot is in the given working point. Also it can safely be assumed that the same would be the case when turning in other circles.

Partial conclusion It is concluded that using passive FDI in the very best case it is only possible to isolate input faults in straight driving as shown in Table 7.1.

x

x

x

Wheel 4

Wheel 3

x

Wheel 2

x

Steering act. Wheel 1

x

Wheel 4

Wheel 2

x

Wheel 3

Wheel 1

Propulsion act.

x

Fault Group Fault group 1 Fault group 2 Fault group 3 Fault group 4

Table 7.1: Isolability in straight driving. Fortunately we can also concluded that it is theoretically possible to isolate faults to a specific input when the robot is turning. This does however have other problems because when the robot is running it will quickly leave it again as the robot turns. Noting the limitations in straight driving these it is still worthwhile to minimize the amount of active FDI needed for the final isolation after a fault is detected and partially isolated.

Group 1032b

73

Chapter 8

Linear Model based FDI of Steering and Propulsion System This chapter deals with the design and implementation of a linear model based FDI scheme. As described in Sec. 4.4 on page 46 the behavior of the API robot is highly nonlinear. This means that in order to take advantage of the many methods for linear FDI a hybrid approach is proposed in order to make a piecewise linear model. The hybrid scheme consists of a hybrid observer which observes the current hybrid state of the nonlinear API system. This hybrid state is then used in a continues observer which estimates the continues state of the system. Since the hybrid system is an approximation of the ˆ M -vector will be the true state plus a behavior of the non-linear model the observed χ linearization error. When the continuous state is estimated, the residue of the real and estimated state is evaluated and the faults detected and if possible isolated. The scheme is summed up on Fig. 8.1 on the following page. Several methods of designing hybrid and continuous state observers will be investigated followed by an evaluation and selection of the most effective approach.

8.1 Hybrid State Observer The hybrid system is described using using the ICR of each turn as well as the position of the robot on the turn circle. The ICR is determined by the angles of the wheels. Since one of the goals of the FDI is to detect steering encoder faults, the measurements of the steering angles is not suitable for use in the hybrid state observer. Instead the steering references βref,i as well as the propulsion references τref,i is used, when relevant. Three different methods for state observation is designed, implemented and evaluated on the nonlinear model, resulting in the best suited being considered for implementation on the API robot. The three methods are: Modified Multiple Hypothesis Testing (MHT ) , which state observation is based on the statistical properties of both the linearized models of each discrete state and the properties provides by the linear Kalman filter. Directional State Observer (DSO) , which uses the directional vector spanned by the el75

Linear Model based FDI of Steering and Propulsion System

βi τi

API (q , χM )

χM

Hybrid FDI Hybrid State Observer qˆ Continuous Observer χ ˆM Linear FDI Faults Figure 8.1: The FDI structure of the propulsion and steering system. ements in the continuous state vector and its correlation with the vector spanned by the sensor readings. ICR State Observer , which based on the velocity of the robot and the rotational speed around the ICR, can calculate the radius of the circle, the API is traveling on. The requirements set for the state observers is that they have to accurately detect all states described in Sec. 4.4 on page 46, meaning multiple parallel driving states as well as multiple turning states In order to test the state observers a number of discrete states has been selected: a parallel driving state and a turning state. The turning state is based on an ICR of 1 meter. Only the first three states are selected, forming a arc from 0 to 60◦

8.1.1 MHT State Observer The basic idea of Multiple Hypothesis Testing[13] is that each model is a candidate for the true model, or in the case of an hybrid system, that each discrete state is a candidate for current discrete state location of the system. MHT is mostly used for multi model estimation and sensor fusion. The method requires implementation of a Kalman filter for each model and that those filters are run in parallel. The Kalman Filter used in this method is described in App. I on page 173. The structure of the state observer can be seen in Fig. 8.2 on the next page 76


8.1 Hybrid State Observer

Sensors & Inputs

State 1 Kalman Filter

Discrete State Estimate State 2 Kalman Filter

qˆ

State Observer

State n Kalman Filter

Figure 8.2: Structure of Modified Multiple Hypothesis Test State Observer In order to perform MHT two assumptions are made: 1. The Kalman filter based on the current discrete state is among the proposed candidates. 2. The system has been in the same discrete state since t = 0. The second assumption presents some limitations, when dealing with systems, which discrete state changes. These limitations will be approached later in this chapter. A number of hypotheses is then formed, one for each state: Hi = State qi is correct. The null hypothesis is due to assumption one impossible and is omitted. The probability that state qi is correct at time tk , conditioned on the measurements Zk is µi (k) , p(qi |Zk )

(8.1)

The probability can then be defined by use of recursion[1] using Bayes rule and the previous probability values: λi (k)µi (k − 1) , µi (k) = Pn j=1 λj (k)µj (k − 1)

where λi (k) = Si (k|k − 1) =

−1 1 T 1 e− 2 ri (k)Si (k|k−1)ri (k) −1 1/2 (2π) det(Si (k|k − 1)) Ci (k)Pi (k)CiT (k) + Ri (k) m/2

ri (k) , z(k) − Ci x î (k|k − 1)

(8.2)

(8.3) (8.4) (8.5)

The elements of Eq. (8.2) are: • The innovation covariance Si (k|k − 1) is calculated using a linear Kalman Filter • The estimated dynamic state x î (k) is calculated by the linear Kalman Filter, one for each of the discrete states. • The innovation vector ri (k) is the measurement residual. • m is the dimension of the innovation vector. Group 1032b

77


• The likelihood of the innovation vector: λi (k) is the probability the observation z(k) would be made, given that the discrete state qi is the correct state. • The output matrix ci . The Kalman Filter, that has the highest probability ui (k), is then assumed to use the discrete state and dynamic model, that best describes the real system: ui (k) = max(uj (k)) 7→ state(k) = i j

i ∈ {1, 2 . . . n}

(8.6)

where the 7→ operator describes the relation between the statement on the left and the statement on the right. The use of an recursive probability function makes the state observer more robust to sudden changes in measurements or if two states have the same probability in the same sample. This has the consequence that the probability over time will converge to a single state as is mentioned in the second assumption: That the system has been in the same state since k = 0. As the hybrid model describing the API robot is expected to change frequently due to the behaviour described in the discrete state, a way to force the MHT observer to react to state changes, without sacrificing the robustness of the current method, has to be found. The chosen solution is to reset the Kalman filters and the MHT every 2 seconds. This interval can be increased to provide a more stable discrete state estimate or decreased to allow faster state changes. As the full state vector is available as sensor measurements, the Kalman filters are reset by setting x î (k − 1) where i ∈ {1, 2 . . . n} to the current sensor reading of the state vector subtracted the working points for each linear model and resetting the estimate covariance Pi (k − 1) to Pi (0), where i ∈ {1, 2 . . . n}. The MHT is reset by setting µi (k) = n1 , where i ∈ {1, 2 . . . n}. The results of the turning scenario can be seen in Fig. 8.3 on the facing page. The parallel driving state is also correctly detected but is not shown.

8.1.2 Directional State Observer The Directional State Observer is based on the vector correlation of the directional vector, spanned by the continuous output vector χ ˆM i of the linear model, where i represents the th i discrete state in the hybrid system, and the vector spanned by the sensor measurements, corresponding to the χM vector. The vector correlation is defined by: corri (t) =

ˆ˙M (t) |χ˙ M (t)|T · χ i ˆ˙M (t)| |χ˙ M (t)| · |χ i

i ∈ {1, 2 . . . n}

(8.7)

ˆ˙M vector is calculated using the linear model derived for each discrete The estimated χ i state in the hybrid system. The predictor used in this approach is shown in Eq. (8.8). x˙ = Ai x + Bi u ˆ˙M χ i

= Ci x

(8.8)

ˆ˙M vector’s correlation is calculated. The Each predictor is run concurrently and each χ i model, which shows the largest correlation with the measurements is deemed the “true” 78


8.1 Hybrid State Observer

0.3 0.2 0.1 M 0 −0.1 −0.2 −0.3

x˙

Nonlinear Model Hybrid System 0

1

2

3

4

5

6

7

0.3 0.2

y˙ M 0.1

Nonlinear Model Hybrid System

0 −0.1

0

1

2

3

4

5

6

7

0.3

θ˙ M

0.2 Nonlinear Model Hybrid System

0.1 0

0

1

2

3

4

5

6

7

3

State

2

True State Est. State

1 0

1

2

3

4

5

6

7

Time [s] Figure 8.3: Simulation results of the MHT State Observer when driving around an 1 m.

ICR

@

state vector and the corresponding discrete state is then the “true” state of the hybrid system. This is shown mathematically in Eq. (8.9) corri (t) = max(corrj (t)) 7→ state(t) = i j

i ∈ {1, 2 . . . n}

(8.9)

The simulation results of the turning scenario is shown in Fig. 8.4 on the next page. The parallel driving state is also correctly detected but is not shown.

8.1.3 ICR State Observer The third method is to calculate the radius of the circle, the API robot is moving on, us√ x˙ 2 +y˙ 2 ing rICR = and comparing the radius with the circles described in the hybrid θ˙ model. Using the information about the models describing the current trajectories, the corresponding discrete state can be found by comparing the position on the circle (θ) to the working points of the linear model. This is then used in the continuous observers. The process is performed in the following order: 1. The current ICR is calculated using Eq. (8.10) 2. In order to find closest match to the ICRs defined in the hybrid model, the difference between ICRs is calculated and the smallest is chosen as the “true” candidate for an ICR . Se Eq. (8.11) 3. Using Eq. (8.12) all discrete states, which is defined using the “true” ICR, is compared with regards to θ M and the θwp , defined in the discrete state. Selecting the discrete state closest to the current θ M as the “true” state completes the process. Group 1032b

79


x˙

0.3 0.2 0.1 M 0 −0.1 −0.2 −0.3


1

2

3

4

5

6

7

0.3 0.2

y˙ M 0.1


0 −0.1

0

1

2

3

4

5

6

7

0.3

θ˙ M


0.1 0

0

1

2

3

4

5

6

7

3

State

2


1 0

1

2

3

4

5

6

7

Time [s] Figure 8.4: Simulation results of the Directional State Observer when driving around an ICR @ 1 m. (x˙ M )2 + (y˙ M )2 θ˙ M = rICRcurrent − min(rICRcurrent − rICRj )

rICRcurrent = rICRwp

p

(8.10) (8.11)

j

θwpi = min(|θ M − θwpj |) 7→ state(t) = i j

i ∈ j ∀{rICRwp = rICRj }

(8.12)

where: • rICRcurrent is the current radius of the circle, the API is moving on. • rICRwp is the closest match in the current ICR.

ICR s

defined in the hybrid model (rICRj ) to the

• θwpi is the closest match of the position of the the hybrid model.

API ,

in regards to the θwp defined in

The simulation results is shown in Fig. 8.5 on the facing page. The parallel driving state is also correctly detected but is not shown.

8.1.4 Partial Conclusion The simulation of the state observers shows that all three observers successfully detects the three turning states although at different times. The observers also correctly detects the parallel driving state. The MHT is slower to detect a state change due to the inherent property of the hypothesis testing algorithm namely assumption two: “The system has been in the same discrete state since t = 0.” 80


8.2 Continuous State Observer

0.3 0.2 0.1 M 0 −0.1 −0.2 −0.3

x˙


1

2

3

4

5

6

7

0.3 0.2

y˙ M 0.1


0 −0.1

0

1

2

3

4

5

6

7

0.3

θ˙ M


0.1 0

0

1

2

3

4

5

6

7

3

State

2


1 0

1

2

3

4

5

6

7

Time [s] Figure 8.5: Simulation results of the ICR State Observer when driving around an ICR @ 1 m. Further simulations, show that they are equally successful, when turning around other ICR s.

Figure 8.6 on the next page shows the comparison between the reference discrete state and the state estimates from each of the three state observers. The reference state is based on the information concerning the working area for each state, which for state 1,2 and 3 is equal to {0 ≤ θ M < 18 π}, { 81 π ≤ θ M < 14 π} and { 41 π ≤ θ M < 38 π} resulting in state 3 5 1 π, 16 π and 16 π. changes at 16 The comparison shows that the ICR observer follows the hybrid system the closest although the difference compared to the directional observer is minimal, and it is therefor decided to use the ICR state observer in the Hybrid FDI. Furthermore, the ICR observer is the least complex in terms of computing power and method complexity and is therefor ideal for implementing on a less powerful computer, such as the OBC.

8.2 Continuous State Observer The purpose of the continuous state observer is to estimate the continuous state using a model. Because of the non-linearities of the system a number of linear models are chosen to cover different areas of the state space. The models are defined in Sec. 4.4 on page 46. The current discrete state and the corresponding model is chosen based on the state provided by the hybrid state observer, designed in the previous section. Two different methods for continuous state observation is designed, implemented and evaluated on the non-linear model, resulting in the best suited being considered for implementation on the API robot. The two methods are: Unknown Input Observers, which is an approach where it is possible to perform linear Group 1032b

81


True Discrete State State 3 2 1 0

1

2

0

1

2

0

1

2

3

4

5

6

7

3

4

5

6

7

3

4

5

6

7

5

6

7

MHT Observer

State 3 2 1

Directional Observer

State 3 2 1

ICR Observer State 3 2 1 0

1

2

3

4

Time [s]

Figure 8.6: Comparison of the discrete state of the three methods, when driving around an ICR @ 1 m. disturbance decoupling which can also be used for isolation. Beard Fault Detection Filter, which is a standard state observer, with detection and isolation based on the "fault event direction". As the observers is to be used for FDI , the area of interest is not the continuous state itself, but rather the residual, from which it is possible to detect and isolate a given fault. All methods take starting point in the linear model of the following form: x(t) ˙ = Ax(t) + Bu(t)

(8.13)

y(t) = Cx(t)

(8.14)

where u(t) = [τref,1 (t) τref,2 (t) τref,3 (t) τref,4 (t) βref,1 (t) βref,2 (t) βref,3 (t) βref,4 (t)]T h iT ˙ θ(t) x(t) = x(t) ˙ y(t) ˙ θ(t)

(8.15) (8.16)

There are a number of proven linear methods for generating residuals with FDI in mind. In the next sections two different approaches are presented and evaluated to determine which is better in the current case.

8.2.1 Unknown Input Observers Unknown Input Observers(UIOs) is a method, with which it is possible to decouple the observed state from known disturbances and thereby also from the residual. This property improves the robustness of the observer and can be used to help isolate faults in a system. The theory of the UIO can be found in App. F on page 157. 82



UIOs are designed for the API robot system to detect and isolate faults in the propulsion and steering system using a linearized version of the non-linear API robot model. See Sec. D.2 on page 145 for details. The linear model has the following structure:

x(t) ˙ = Ax(t) + Bu(y) + Ed(t)

(8.17)

y(t) = Cx(t)

(8.18)

where E is the fault distribution matrix. To design the UIOs eight different models are needed. Each one decoupling a different part of the model inputs. In this case each of the wheel angles, βi , are decoupled along with each of the propulsion torques of the wheels, τi . The decoupled models are presented below: x˙ 1 =A1 x + B1 [τref,2 τref,3 τref,4 βref,1 βref,2 βref,3 βref,4 ]T + E1 τref,1 x˙ 2 =A2 x + B2 [τref,1 τref,3 τref,4 βref,1 βref,2 βref,3 βref,4 ]T + E2 τref,2 x˙ 3 =A3 x + B3 [τref,1 τref,2 τref,4 βref,1 βref,2 βref,3 βref,4 ]T + E3 τref,3 x˙ 4 =A4 x + B4 [τref,1 τref,2 τref,3 βref,1 βref,2 βref,3 βref,4 ]T + E4 τref,4 x˙ 5 =A5 x + B5 [τref,1 τref,2 τref,3 τref,4 βref,2 βref,3 βref,4 ]T + E5 β1 x˙ 6 =A6 x + B6 [τref,1 τref,2 τref,3 τref,4 βref,1 βref,3 βref,4 ]T + E6 β2 x˙ 7 =A7 x + B7 [τref,1 τref,2 τref,3 τref,4 βref,1 βref,2 βref,4 ]T + E7 β3 x˙ 8 =A8 x + B8 [τref,1 τref,2 τref,3 τref,4 βref,1 βref,2 βref,3 ]T + E6 β4

(8.19)

The decoupling results in each of the corresponding UIO residuals to become insensitive to a fault in the decoupled input. Each fault presents with a different set of residuals. Ideally a fault should have no effect on the corresponding decoupled residual. However, due to the inherent non-linearities it is necessary to evaluate the residual in the following way: i = min(rj (t)) (8.20) j

where i is the index of the residual least affected by a given fault. The residual ri is given the value 0 and the rest are given the logic value 1. In Eq. (8.21) the different faults are presented with their corresponding logical residual patterns. F ault in r1 r2 r3 r4 r5 r6 r7 r8 β1 0 1 1 1 1 1 1 1 β2 1 0 1 1 1 1 1 1 β3 1 1 0 1 1 1 1 1 β4 1 1 1 0 1 1 1 1 (8.21) τ1 1 1 1 1 0 1 1 1 τ2 1 1 1 1 1 0 1 1 τ3 1 1 1 1 1 1 0 1 τ4 1 1 1 1 1 1 1 0 These decoupled systems are then used to design 8 different design is explained by an example in App. F.1 on page 159. Group 1032b

UIOs

. The process of

UIO

83


Detection is performed by simple threshold logic: |r(t)| < T hreshold f or |r(t)| ≥ T hreshold f or

f ault − f ree case

f aulty case

(8.22) (8.23)

8.2.2 Test of UIO method This test will verify the method of detecting and isolating faults using the Unknown Input Observer method. It is a simulation test where the non-linear model is used at a predefined working point. The working point is given by: β1 β2 β3 β4

=0 =0 =0 =0

τ1 τ2 τ3 τ4

= 10 x˙ = 0.2664 = 10 y˙ = 0 = 10 θ˙ = 0 = 10 θ=0

(8.24)

That is, the API robot is driving straight ahead with a 10% actuation on all wheels. The test is performed without a path controller and all faults are introduced at 3 seconds. It must be noted that, as it is only possible to see actuator faults directly, only the effect of the different actuator faults are tested. For example a No actuation fault could have several different causes as described in Cha. 6 on page 61. In this case a fault is isolated by looking at what residue has lowest value. This follows from the assumption that the residue, where a given input is decoupled in the UIO , will be the least affected by a fault in this input. It is observed on Fig. 8.7 on the next page that within 10 ms of the introduction of a fault, the values of UIO−τ1 and UIO-τ2 (B1 and B2 respectively) are affected less than the rest of the residues. This can also be observed in Fig. 8.8 where the fault isolation indicates a fault in the inputs τ1 or τ2 . Furthermore it can be seen that the indication remains constant for at least 1.5 seconds. As previously established it is not possible to isolate the fault more using passive model based FDI because of the inherent dependencies between the two inputs. Figures from Fig. G.1 to G.7 on pages 163–165 show the isolation of the rest of the fault effects as indicated by each figure.

8.2.3 Beard Fault Detection Filter method The Beard Fault Detection Filter method(BFDF) calls for the use of an full order state observer[9] to generate directional residuals. This residual can then be used to isolate a given fault based on its direction. The method is based on a method presented in [5, page 87-98] and can be extended to use UIO theory to help decouple disturbances. In the current case nothing is known about the disturbances of the system, so instead a standard full order observer is applied. The current linear system can be described as: x(t) ˙ = Ax(t) + Bu(t) + bi fai (t) y(t) = Cx(t) + Ij fsj

(8.25)

where: 84



−3

UIO-τ1 :

2.5 1.25 0

UIO-τ2 :

2.5 1.25 0

UIO-τ3 :

UIO-τ4 :

UIO-β1 :

UIO-β2 :

2.5 1.25 0 2.5 1.25 0 2.5 1.25 0 2.5 1.25 0

UIO-β3 :

2.5 1.25 0

UIO-β4 :

2.5 1.25 0

x 10

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0 0.5 −3 x 10

1

1.5

2

2.5

3

3.5

4

4.5

5

0

1

1.5

2

2.5

3

3.5

4

4.5

5

0.5

Time [s] Figure 8.7: wheel 1.

UIO

residues following the propulsion actuator fault: No actuation on

bi Is the ith column of the input matrix B. fai (t) Denotes a fault in the ith actuator. Ij Is the j th column of a unit matrix I. fsj (t) Denotes a fault in the j th sensor. The residual of a full order observer is described by: x ˆ˙ (t) = Aˆ x(t) + Bu(t) + K(y(t) − C x ˆ(t)) r(t) = y(t) − C x ˆ(t)

(8.26)

In the current system all sensor faults will present as actuator faults because of feedback in steering and propulsion sensors. As a consequence the model can be reduced to: x(t) ˙ = Ax(t) + Bu(t) + bi fai (t) r(t) = Cx(t)

(8.27)

It is known that if a fault occurs in the ith actuator it will have a signature direction denoted by bi also called the fault event direction. When the full order observer is designed, there exists some design freedom in the form of the feedback gain K. The feedback gain must stabilize the error system associated with Group 1032b

85


β4

Faulty input

β3 β2 β1 τ4 τ3 τ2 τ1 no fault

0

1

2

3

4

5

Time [s] Figure 8.8: UIO decision function following the propulsion actuator fault: actuation on wheel 1.

No

the observer: e(t) ˙ = (A − KC)e(t) + bi fai (t)

r(t) = Ce(t)

(8.28)

Since there is not a single solution to this the remaining design freedom can be used to ensure that the residual, r(t) maintains a fixed direction. This can be done as described in [5, page 89] by ensuring that: rank[bi (A − KC)bi · · · (A − KC)n−1 bi ] = 1,

(8.29)

i.e. that the controllability matrix of the pair (A, bi ) has rank one. After the observer is designed a way of evaluating the residual vector has to be found. One way is to observe how the current residual correlates with the different fault event directions. In this case there are eight fault event directions so there are eight correlations which can be described by: CORRi (t) =

|(Cbi )T r(t)| |Cbi |2 |r(t)|2

(8.30)

If CORRj > CORRk then the fault is more likely to be in actuator j than actuator k. Detection is performed by simple threshold logic: |r(t)| < T hreshold f or |r(t)| ≥ T hreshold f or

f ault − f reecase

f aultycase

(8.31) (8.32)

8.2.4 Test of Beard Fault Detection Filter method This test will verify the method of detecting and isolating faults using the BFDF method. It is a simulation test where the non-linear model is used at a predefined working point. 86



The working point is given by: β1 β2 β3 β4

=0 =0 =0 =0

τ1 τ2 τ3 τ4

= 10 = 10 = 10 = 10

x˙ y˙ θ˙ θ

= 0.2664 =0 =0 =0

(8.33)

That is, the API robot is driving straight ahead with a 10% actuation on all wheels. The test is performed without a path controller and all faults are introduced at 3 seconds. It must be noted that, as it is only possible to see actuator faults directly, only the effect of the different actuator faults are tested. For example a No actuation fault could indicate several different causes as described in Cha. 6 on page 61. 1

BFDF-τ1 :

0.5 0 0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

1

BFDF-τ2 :

0.5 0 1

BFDF-τ3 :

0.5 0 1

BFDF-τ4 :

0.5 0 1

BFDF-β1 :

0.5 0 1

BFDF-β2 :

0.5 0 1

BFDF-β3 :

0.5 0 1

BFDF-β4 :

0.5 0

Time [s] Figure 8.9: wheel 1.

BFDF

correlation following the propulsion actuator fault: No actuation on

In this case a fault is isolated by looking at what residue has the highest correlation with b1 in this case. It is observed on Fig. 8.9, that within 10 ms of the introduction of a fault at the 3 second mark the correlation with τ1 and τ2 (b1 and b2 respectively) increase to values higher than the rest. This can also be observed by looking at Fig. 8.10 on the following page where the fault isolation indicates a fault in BFDF-τ1 or BFDF-τ2 . Furthermore it can be seen that the indication remains constant for at least 2 seconds. This is not a factor in the isolability but it shows something about the robustness of the method but also that the fault might is not severe enough to make the API leave its working point. As previously established it is not possible to isolate the fault more using passive model based FDI because of the inherent dependencies between the two inputs. Group 1032b

87

Faulty input


β4 β3 β2 β1 τ4 τ3 τ2 τ1

no fault

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure 8.10: BFDF decision function following the propulsion actuator fault: actuation on wheel 1.

No

Figures from Fig. G.8 to G.14 on pages 166–168 show the isolation of the rest of the fault effects as indicated by each figure.

8.2.5 Partial Conclusion Both methods are tested with the straight driving linear model and work as expected. The full state observer seems like a good choice since only one observer is needed as opposed to the UIO method. When inspecting the time that a given fault remains isolated it is apparent that the BFDF method is more robust against non-linearities when a fault occurs. It outperforms the UIO method in every fault situation as can be seen in App. G.2 on page 166 and App. G.1 on page 163. The tests used the non-linear model and give a good indication of how it will work on the real system. There are however uncertainties with regards to noise-levels and model accuracy which will have an impact on performance of the fault isolation particularly. Comparing the two methods it can be concluded that the UIO method is more computationally demanding because it needs to run eight linear models simultanously while the BFDF method only needs eight correlation calculations. Since the UIO method does not provide any other significant advantages to it has been chosen to continue with the BFDF method in the hybrid FDI proposed for the API robot.

8.3 Conclusion The purpose of the linear FDI part of the thesis was to design a method to enable the use of linear FDI on the API robot. The proposed method called for two parts: First a hybrid model with a hybrid state observer to ensure that the current linear model was the best fitting of the available models. Second a continous linear FDI observer to detect and if possible isolated faults that occur in the API system. In Sec. 8.1 on page 75 a hybrid observer was succesfully designed and tested as after a three different methods were explored se described in Sec. 8.1.4 on page 80. This also tested the hybrid model defined in Sec. 4.4 on page 46. The conclusion is that the stateobserver maintains a good fit to the non-linear models state trajectory and functions as it was designed to. 88


8.3 Conclusion

Sec. 8.2 on page 81 described the design and test of two different linear FDI methods the BFDF method and the UIO method. Both were tested on the non-linear model and found suitable for implementation in the linear FDI scheme and the BFDF method was selected for implementation as described in Sec. 8.2.5 on the preceding page. The last part was to combine and test the ICR observer and BFDF methods to make up the hybrid FDI method described in Cha. 8 on page 75. The test was carried out in Sec. 11 on page 109 and showed that the had some problems detecting and isolating the some of the faults described in Sec. 6 on page 61 but could detect the No actuation faults on wheel 1 and 3.

Group 1032b

89

Chapter 9

Nonlinear Particle Filter Based FDI of Steering and Propulsion System The chapter describes the design and implementation of a particle based approach for FDI . The focus of the project until now has been the linear approach, due to its relative maturity compared to a non-linear approach as the particle filter FDI method is. The nonlinear approach is chosen as a Proof-of-Concept, due to computational limitations, which will be explained in detail later. The FDI method described in this chapter is a particle filtering based likelihood ratio approach[12], which combines the Log Likelihood Ratio (LLR) with multi model particle filters. The method will in this chapter be abbreviated as PF -FDI (Particle Filter-FDI ). The structure of the PF - FDI method can be seen in Fig. 9.1

Sensors & Inputs

Particle filter: No Faults

Fault Particle filter: Fault 1

Fault Diagnosis

Particle filter: Fault N

Figure 9.1: The structure of the PF - FDI method

9.1 Requirements for Particle Filter-FDI As mentioned in the introduction the particle filter is based on multiple non-linear models. Normally the PF is used for state estimation, requiring only the nominal model of the system. But as this method is for use with FDI, a additional number of models are required, more specifically a model describing each of the fault scenarios, subject to detection and isolation. This entails the requirement that all faults must be described by a non-linear stochastic state space model. This excludes the fault scenario: Erratic 91

Nonlinear Particle Filter Based FDI of Steering and Propulsion System

wheel movement, since a random change of the parameters of the model can not be described adequately enough due to the inherent difference of the modelled random parameter and the actual parameter. A special case is the fault scenarios: Actuator offset and Sensor offset. These scenarios is possible to model accurately but as the size of the offset is not a constant, a large number of models is required to cover the range of possible offsets. This has the consequence that all Offset faults is excluded from use in the PF - FDI method. In the case of the API: 6 fault scenarios each applicable to any one the wheels, resulting in a total of 25 models required for full FDI of all singular faults. The 6 fault scenarios can be seen in Cha. 6 on page 61. The PF - FDI can be extended to include multiple faults occurring or present at the same time. This however requires models for all possible combinations of two concurrent faults. The scenarios and therefore models selected for implementation in this case is: No Faults: The nominal model. Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1. Fault 2: Propulsion Actuator Fault: No Actuation on wheel 3. Fault 3: Steering Actuator Fault: Max. Negative Actuation on wheel 2. The level of isolability described in Cha. 7 on page 71, is also valid for PF - FDI method as the couplings are due to the physical design of the API and not limitations due to modeling. The models are chosen as the dynamic model described in Sec. 4.3 on page 39 and is displayed below for reference:   1 P4 M M M M M ˙  M  cos(βi + θ ) · Fxi − sin(βi + θ ) · Fyi − y˙ · θ − agx x ¨   m Pi=1  4 M 1 M · θ˙ M − aM  M ) · F + cos(β + θ M ) · F  y¨  =  + x ˙ sin(β + θ i xi i yi gy    mP i=1 θ¨M 4 1 sin(β − γ ) · κ · F + cos(β − γ ) · κ · F i i i xi i i i yi i=1 I (9.1) where: Fyi = −(Cf 1 + Cf 2 · V M ) · αi (9.2) τr,i (9.3) Fxi = rw ! 2 Km(d) Km(d) τr,i = (9.4) · V(d),i − + b(d) φ˙ i Ra(d) Ra(d) 2 Km(d) The faults are introduced by setting τr,i = − Ra(d) + b(d) φ˙ i , where i is 1 for fault 1 and

3 for fault 2 The model for fault 3 is obtained by setting β2 = −π/2.2 Choosing the dynamic model has one shortcoming as the model is dependent on θ M , which causes the different models, to diverge more quickly due to the strong effect of the orientation of the robot. This shortcoming is solved, by using the sensor measurements of the API orientation as θ M in all models, making all models point in the same direction, while still allowing the induced faults to effect the χM vector. 92


9.1 Requirements for Particle Filter-FDI

The models is described by the following set of nonlinear state space models indexed by m = 0, 1, . . . , M (m) (m) (m) (9.5) xk = f (m) xk−1 , wk−1 (m) (m) (m) (9.6) yk = h(m) xk−1 , vk−1

where

• x is the state vector. • w is a zero mean white noise with known . • f (x, w) is the non-linear model. • y is the output measurements vector. • v is a zero mean white measurement noise with known PDF. • h(x, v) is the non-linear measurement model. The process noise w is for the steering actuator considered to be insignificant, due to the quality of the steering encoder used for feedback in the steering angle controller on the LH 28s. The remaining actuator is the propulsion actuator. As no actuator noise determination has occurred, the process noise is selected based on the propulsion sensor. This is made possible, due to the way the sensor noise is estimated[4]. The sensor noise is estimated without correction in regards to the actual speed of the wheel. This means that both sensor and actuator noise is present in the noise measurements. Since the amount of actuator noise versus sensor noise is unknown, the worst case scenario is chosen and the noise estimated is selected as 100% actuator noise. The propulsion actuator noise can then be written as: 2 wpropulsion = N (0, σpropulsion ) 2 −4 ˙ σ = 5 · 10 · φ propulsion

(9.7) (9.8)

The new actuator model is: τr,i

Km(d) · V(d),i − = Ra(d)

2 Km(d)

Ra(d)

+ b(d)

!

2 (φ˙ i + N (0, σpropulsion ))

(9.9)

The measurement noise v is selected as the noise on the three sensors used in the output M ˙M T measurement vector[4]: y = [x˙ M GP S y˙ GP S θGyro ] . The noise is: vx˙ M = N (0, σx2˙GP S ) vy˙ M = N (0, σy2˙GP S )

(9.11)

N (0, σθ2˙ ) gyro

(9.12)

σx2˙GP S = 1 · 10−4 [m]

(9.13)

vθ˙M =

σy2˙GP S σθ2˙ Gyro Group 1032b

(9.10)

−4

= 1 · 10

[m]

−5

= 1.1 · 10

◦

[ /s]

(9.14) (9.15) 93


The basis idea of this multi-model approach is to calculate the probabilities of each model (m) being the correct model. This probability is called the conditional probability p(xk |Zk ), where Zk is the set of measurements up to time k. Comparing the probabilities to each other and selecting the model with the highest probability would then provide the correct model thereby detecting and isolating the fault or confirm nominal operations. Using linear models, these probabilities could be obtained using Kalman filters, but when dealing with non-linear models, this is not possible and a more advanced solution is required, such as particle filters.

9.2 Particle Filter Design (m)

The PF generates a approximation of the p(xk |Zk ) using a swarm of particles, each propagated though the non-linear models. The particle filter uses the following algorithm: (m)

(m)

0. Initial conditions: N particles {x0 (i) : i = 1, 2, . . . , N } sampled from the PDF p(x0 |Z0 ) (m)

1. Prediction: Sample from the noise vector w, N samples {wk−1 (i) : i = 1, 2, . . . , N }. These sample can then be propagated together with the particles from the previous (m) time step {xk−1 (i) : i = 1, 2, . . . , N } through the non-linear model: (m) (m) (m) (m) (9.16) xk|k−1 (i) = fk−1 xk−1 (i), wk−1 (i) (m)

2. Update: Each particle is assigned a weight, which results in the posterior PDF p(xk |Zk ) being represented in terms of weighted particles. The weights are given for each particle i = 1, 2, . . . , N , by: p(yk |xk|k−1 (i)) wk (i) = PN j=1 p(yk |xk|k−1 (j))

(9.17)

The probability p(yk |xk|k−1 ) is called the likelihood and is defined by: T 1 −1 −3 p(yk |xk|k−1 ) = (2π) /2 |Σv | /2 exp − yk − hk (xk|k−1 ) Σ−1 y − h (x ) k k k|k−1 v 2 (9.18) where yk is the sensor measurement for the current time step and where the covariance matrix Σv is defined by:  2  σx˙ GP S 0 0  σy2˙GP S 0  Σv =  0 (9.19)  0 0 σθ2˙ Gyro

3. Resample: In order to ensure that the particles accurately describes the PDF of the different models, the particles that have drifted so far away, that their contribution to the PDF is negligible, is excluded from further propagation. The chosen method is Systematic Resampling[6, 8]. The remaining particles is then used as basis for the next time step of the algorithm. 4. Step 1,2 and 3 form a single iteration and is performed for each time step k. 94


9.3 FDI using Particle Filters

9.3 FDI using Particle Filters The FDI part of the PF - FDI combines the log likelihood ratio with multiple hypothesis testing and particle filters. The method described is an adaptation of the Generalized Likelihood Ratio[22] to function with PFs instead of the linear Kalman filters used in the original method. The design function for detecting a single fault can be written as: gk =

H1 ≷ λ H0

max Sjk j

(9.20)

where: • Sjk is the joint log likelihood ratio of the PDF of the nominal and faulty model, from time j to k. • H0 and H1 is the no change and change hypothesis. • λ is the threshold between the two hypothesis. The PF - FDI approach extends the decision function with particle based LLRs and multiple hypothesis testing. The extended joint LLR for model (m) is defined as: Sjk (m)

=

k X

ln

r=j

p(yr |Hm , Zr−1 ) p(yr |H0 , Zr−1 )

(9.21)

The extended joint LLR is also known as the cumulative sum. The likelihood p(yr |Hm , Zr−1 ) is defined using the likelihood of each particle, calculated in the Update step of the PF algorithm in Sec. 9.2 on the preceding page: N 1 X (m) p yr |xr|r−1 (i) p(yr |Hm , Zr−1 ) = N

(9.22)

i=1

The decision function for multi-model cases can then be written as: gk = max

1≤j≤k

H1 max Sjk (m) ≷ λ 1≤m≤M H0

(9.23)

The λ fault threshold is selected as 200 in order to avoid small spikes in the joint LLR, furthermore all LLR values exceeding 5000 is ignored, as a larger value is considered outside the expected range of the LLR under both nominal and faulty operation Fault detection is obtained when gk > 0, with fault isolation being archived by determining which fault index m is responsible for the change of gk . As the decision function requires a linear growing number of calculations as Sjk must be calculated for each possible fault time from 1 to k, a sliding window (W) is applied to the function resulting in the final decision function: gk = Group 1032b

max

max Sjk (m)

k−W +1≤j≤k 1≤m≤M

(9.24) 95


The window must be wide enough to provide detection and isolation of the selected faults. This is obtained using trial and error, which in this implementation has resulted in a window size of 25 samples equal to a period of 0.5 seconds. The design of the PF - FDI method is now complete.

9.4 Performance Parameters with regards to Particle Filter-FDI The PF - FDI algorithm is computational heavy with regards to CPU usage due to its recursive and multi model nature as well as the methods underlying PFs . The implementation uses the following parameters: No. of models: M = 4 One nominal model and 3 fault models. No. of Particles: N=7 The number of particles needed for estimating the PDF of the different models. The number of particles is decided using a commonly used rule of thumb: 2 times the number of states plus one. The non-linear model used consist of three states. Sample Rate: Ts = 50Hz = 0.02 The sample rate used throughout the and estimation software.

API

control,

FDI

Numerical Integration method: Runge-Kutta The choice of integration method requires 4 pass-throughs of the nonlinear models in order to estimated the result of the integration. Intruns = 4. Complexity of the Nonlinear Model The dynamic model used is a complex model due to the use of trigonometric functions and multiplications This adds up to the following equation, which illustrates the number of times the nonlinear model is calculated. nodynamic model runs = M · N · intruns 1 runs · = 112 sample T s runs = 5600 second

(9.25) (9.26) (9.27)

The full implementation of the PF - FDI multiply this number by a factor of 6 due to the 25 fault scenarios. The FDI part of the algorithm is also dependent on the constants listed above, but is less demanding due to the absence of numerical integrations and trigonometric functions. As mentioned in the introduction, the PF - FDI is only a Proof-of-Concept, as the current OBC is not powerful enough to compute the required number of passthroughs of the dynamic model, due to its 150MHZ CPU. Therefore the test of the PF - FDI method is performed offline using the non-linear model as the real system. 96


9.5 Preliminary Test of Particle Filter-FDI Method

9.5 Preliminary Test of Particle Filter-FDI Method This section functions as a preliminary test of the PF - FDI. The scenario from the hybrid model chapter is the basis of the simulation of the PF - FDI method. The path can be seen in Fig. 4.20 on page 50. The fault tested in this section is Propulsion Actuator Fault: No Actuation on wheel 1(Fault 1) and is introduced at time = 25. The result can be seen in Fig. 9.2 on the next page The figure shows the behaviour of the different fault models and how the measurements changes from following the no fault particles to following the fault 1 particles. The joint LLR can be seen in Fig. 9.3 on the following page. The output of the decision function can be seen in Fig. 9.4. The results show that the fault detected was not the fault introduced although the fault was of the same type as the real fault. Previous simulations has also shown false alarms occurring before any faults was introduced. This is caused by the behaviour of the different nonlinear models. When going from driving straight to turning or when going from nominal operation to faulty operation, the velocities cross each other causing the joint LLR to indicate a high likelihood of one or more fault models. The largest of the joint LLR is detected by the decision function resulting in a false alarm or wrong fault type. These crossings are of short duration and therefore an additional requirement is added to the decision function. The requirement is that for fault detection, a fault must be detected by the original decision function continually over a period of 2 seconds. This concludes the design of the PF - FDI method.

9.6 Preliminary Conclusion In this chapter the PF - FDI method was designed. The fault scenarioes and the nominal scenario was implemented as Particle Filters using the non-linear dynamic model. The FDI part of the method was also implemented and can be concluded to function as expected, although with the added requrement that a faults is to be present in at least 2 seconds before detection. The implementation of the method was designed to allow easy addition of the remaining faults.

Group 1032b

97


1 Measurements No Faults Fault 1 Fault 2 Fault 3

0.5

x˙ M

0

−0.5

−1

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0.6 0.4 0.2

y˙ M

0

−0.2 −0.4

1 0.8 0.6

θ˙ M

0.4 0.2 0 −0.2 −0.4

Time [s]

Figure 9.2: Simulation results of the Particle Filters with Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1 @ t=25

250 Fault 1 Fault 2 Fault 3

Real Fault Time

200

Log Likelihood Ratio

150

100

50

0

−50

−100

−150

−200

−250

0

5

10

15

20

25

Time [s]

30

35

40

45

50

Figure 9.3: Joint Log Likelihood of the Particle Filters with Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1 @ t=25

98


9.6 Preliminary Conclusion

Fault Detected

3

Detected Fault Time

Real Fault Time

Fault

2

1

0 0

5

10

15

20

25

Time [s]

30

35

40

45

50

Figure 9.4: Decision function of the Particle Filters with Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1 @ t=25. The detected fault is not the fault occurring in the simulation.

Group 1032b

99

Chapter 10

Active Fault Isolation Supervisor As described in Cha. 7 on page 71, the chosen FDI scheme is only able to isolate faults down to which wheel pair is faulty as seen in Fig. 7.1 on page 72, when driving straight whereas only fault detection is possible when turning. In order to provide complete fault isolation down to which sensor or actuator is responsible for the deviation of the API, a Active Fault Isolation scheme(AFI) is to be implemented. The purpose of the AFI is to take control of the API in case of a detected fault. The AFI will then perform a series of tests in order to isolate the faults. The method is divided into two parts, one responsible for isolating steering faults and the second responsible for the propulsion faults. The placement of the AFI supervisor is between the FD supervisor and the FTC supervisor. A overview of the method can be seen in Fig. 10.1 on the next page. The figures describing the AFI method use double ringed circles as start states, thick bordered circles as stop state and thin circles as states. This is due to the discrete event nature of the method. A added value of the active testing of the different actuators and sensors, is that false alarms can be detected and consequently ignored. This is an improvement over the passive FDI, which due to only observing the system, is vulnerable to detecting and isolating a non existing faults i.e.. a false alarm. The passive FDI then propagates the false alarm to the FTC supervisor resulting in poor performance of the control system, due to the disabling of working actuators or sensors.

10.1 Active Isolation of Steering Faults. Isolation of steering faults, both sensor and actuator, is based on the electro-mechanical stoppers implemented on the API. The stoppers are designed as a security measure in order to prevent turning the wheels outside the safe area as well as avoiding breaking the cables inside the joint due to twists. The procedure of the isolation of steering faults can be seen in Fig. 10.2 on page 103 and is described in App. H.1 on page 169.

101

Active Fault Isolation Supervisor

Active FI Supervisor Actived

Steering Fault Detected

Steering Fault Isolation

Propulsion Fault Detected

No Fault Isolated

Fault Isolated

No Fault Isolated

Active FI Supervisor Disabled

Propulsion Fault Isolation

Fault Isolated

Figure 10.1: Flowchart of the AFI process

10.2 Active Isolation of Propulsion Faults. The isolation of propulsion faults is based on the velocity of the model: q V B = (x˙ B )2 + (y˙ B )2 q B B 2 2 Vref = (x˙ B ref ) + (y˙ ref ) x˙ B ref

API

and the kinematic

4 1 X ˙ = φi cos(βi ) 4

B = y˙ ref

1 4

i=0 4 X i=0

φ˙ i sin(βi )

The propulsion faults, that are considered for detection are: Non-model Based The following faults can be isolated by setting the wheels to zero actuation and comparing the V M and wheel sensor measurements. • Max. positive actuation • Max. negative actuation • Max. positive output • Max. negative output A special case is the No actuation fault, which requres that the wheel is given a non zero reference, as the isolation method is to verify that the API is moving, when subjected to actuation. 102


10.2 Active Isolation of Propulsion Faults.


Move to Nearest Stopper

No Stopper Activated

Max Stopper Activated at Start

Max Positive Output W.o. Stopper Actived

No Change in Output

Wheels remaining Continue to next Wheel

No Fault Isolated

Max Min Negative Output Stopper W.o. Activated at Stopper Start Actived

Stopper Actived with Offset on Sensor

No Fault Isolated

No Output Max Positive Actuation

Max. Positive Output

Max. Negative Output

No Actuation

Max Negative Actuation Sensor Offset

Steering Fault Isolated

Fault Isolated


Figure 10.2: Flowchart of AFI, when isolating steering faults.

Group 1032b

103

Active Fault Isolation Supervisor

Model Based The remaining faults require a model based approach, which involves driving back and forth and comparing the measured velocity V M with the model B . based velocity Vref • No output • Sensor offset Since the propulsion sensor is not used in the control of the propulsion torque, only the isolation of actuator faults will be implemented. The procedure of the isolation of propulsion faults can be seen in Fig. 10.3 on the facing page and is described in App. H.2 on page 170. The speed of the robot V M can be measured two different ways. One is to use the velocity of the API as reported by the GPS. The other is to use the equipped doppler radar, which measures the speed of the API directly. During the implementation, it became clear that the doppler radar is not able to distinguish which direction the API is moving as the measured speed is always postive. If used in the isolation of propulsion faults the two Max. positive actuation and Max. negative actuation scenarioes will be deteced as Max. positive actuation. Therefore the speed measurment will be calculated using the GPS velocities.

10.3 Partial Conclusion The AFI for isolation of steering faults has been implemented fully as for the propulsion part only the propulsion actuator faults has been implemented. The isolation of steering sensor faults Max. positive output and Max. negative output have been implemented but can not be tested on the API. Instead the algorithm for detecting these two faults have been verified by setting the variables to values, that mimic the effect of the two faults. This simulation have shown that the algorithm work and is able to isolate the faults. During the implementation of the steering faults a design fault was detected in the LH28 software. The error is in the way the status of the electromechanical stoppers are made available to the OBC. The stoppers are a part of an FDI scheme on the LH28 and is only transmitted, when the controller is active and when the wheel is actuated. This effects the detection and isolating of the Max. positive actuation and Max. negative actuation on the steering actuators although only when the fault is simulated by fixing the steering control signal to the max. or min. value. The two faults are detected as No actuation, which means that even though the fault type is not correct, a fault is detected. It is expected that the real fault will enable the status of the stoppers to be sent, thereby enabling full fault isolation. The remaining faults are not effected and is correctly isolated.

104


10.3 Partial Conclusion

i VM Vref Vi Vi,ref

= Index i is the wheel, being testet = The current velocity of the API = The calculated velocity of the API = The velocity of wheel i = The calculated velocity of wheel i

Wheels remaining

Propulsion Fault Isolation

Continue to No Fault Wheel i+1 Isolated

No Wheels remaining

Model Based FI

VM = 0 & M V = 0 M i V V =0 Smaller & Then Vi = min Thredshold

VM VM = 0 Greater & than Vi = max Threshold

VM = Vref & Vi = 0

No Actuation

Max Positive Actuation

Max. Positive Output

Max. Negative Output

Continue to Wheel i+1 VM = Vref & Vi = Vi,ref VM = Vref & Vi Vi,ref

No Output

Sensor Offset

Max Negative Actuation

No Fault Isolated Propulsion Fault Isolated on Wheel i

Fault Isolated Propulsion Fault Isolation

Figure 10.3: Flowchart of AFI, when isolating propulsion faults.

Group 1032b

105

Part IV

Conclusion

107

Chapter 11

Acceptest of Linear FDI The two methods that make up the linear FDI scheme will be tested in this chapter and the results analysed. The two methods are integrated into a SIMULINK block as described in App. C.1.3 on page 141 and tested using the scenario described in Sec. 4.4 on page 46 As it was decided to only detect faults when the robot was driving straight. A simple way of detecting this was implemented in the matlab code. A simple threshold on θ˙ was used to detect weather the robot was turning or driving in a straight line. A second threshold was implemented on the Euclidean norm of the observer residue in order to detect when the BFDF isolation should be activated. It proved to be a challenge to tune the different thresholds in the the BFDF to be able to detect different faults correctly. It was however possible to get the FDI scheme to detect and isolate a few faults reliably. Figure Fig. 11.1 on the following page and No Actuation show the results of faults being introduced after 3 seconds on wheel 1 and 3. It can be seen that the two scenarios detect the given fault correctly. As was discovered in Sec. 7 on page 71 it is not possible to isolate input faults more than indicated in the results. Fig. 11.1 on the following page should indicate a fault in τ1 or τ2 at 3 seconds which it does. Fig. 11.1 on the next page should indicate a fault in τ3 or τ4 and while less apparent it also indicates the correct faults initially. It was discovered during the test that it was difficult to separate faults from state changes and this interfered with the fault isolation. However a 20 sample delay on the state observer was enough to give the BFDF algorithm time to detect a given fault. Still it was difficult to get the algorithm to detect the different faults using identical thresholds.

Linear FDI Conclusion It can be concluded that while the hybrid FDI scheme implementation has some problems which need to be dealt with but detects the No Actuation fault on propulsion correctly on all wheels.

109

Acceptest of Linear FDI

0.2

x˙ M

0 −0.2 −0.4

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30

40

50

60

70

80

90

100

110

No Fault 0

10

20

30

40

50

60

70

80

90

100

110

0.2

y˙ M

0 −0.2

0.2

θ˙ M

0 −0.2

: BFDF-β3 : BFDF-β4

: BFDF-β1 : BFDF-τ4 : BFDF-τ3 : BFDF-τ2 : BFDF-τ1 :

BFDF-β2

t

Figure 11.1: Showing the results of the No Actuation fault on wheel 1

11.1 Hybrid State Observer This section will determine if the hybrid state observer designed in Sec. 8.1 on page 75 is able to follow the test path set forward in Sec. 4.4 on page 46. In addition to testing the state observer, the hybrid model is also tested. The results of the test can be seen in Fig. 11.3 on page 112 and Fig. 11.4 on page 113

Partial Conclusion The hybrid model and thereby the hybrid state observer is shown to follow the nonlinear model. As the hybrid model is linearized in a finite number of working points, a complete match between the hybrid model and the non-linear model is impossible. If the deviation is deemed to large it is trivial to devide the state space into smaller areas. The current 110


11.2 Continuos State Observer

0.2

x˙ M

0 −0.2 −0.4

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30

40

50

60

70

80

90

100

110

No Fault 0

10

20

30

40

50

60

70

80

90

100

110

0.2

y˙ M

0 −0.2

0.2

θ˙ M

0 −0.2

: BFDF-β3 : BFDF-β4

: BFDF-β1 : BFDF-τ4 : BFDF-τ3 : BFDF-τ2 : BFDF-τ1 :

BFDF-β2

t

Figure 11.2: Showing the No Actuation fault on propulsion on wheel 3 number of states for a turning scenario is 16, which for the case of FDI is deemed enough. The final conclusion can only be made, after the FDI method has been tested.

11.2 Continuos State Observer This chapter will describe the accept test of the linear fdi method BFDF.

Group 1032b

111

Acceptest of Linear FDI

12

10

8

yM

6

4

2

Non−linear Model Hybrid Model

0 −2

0

2

xM

−4

4

6

8

10

Figure 11.3: The xy plot of the Non-linear model compared to the Hybrid model.

112


11.2 Continuos State Observer

0.5

x˙ M

0 −0.5

Non−linear Model Hybrid Model 0

10

20

30

40

50

60

70

80

90

100

0

10

20

30

40

50

60

70

80

90

100

0

10

20

30

40

50

60

70

80

90

100

0.5

y˙ M

0 −0.5 0.5

θ˙ M

0 −0.5 100 State

State

50 0

0

10

20

30

40

50

60

70

80

90

100

Time [s] Figure 11.4: The velocities of the Non-linear model compared to the Hybrid model. At the bottom the discrete state is shown.

Group 1032b

113

Chapter 12

Accept Test of Particle Filter-FDI Method This chapter presents the accept test of the Particle Filter-FDI Method designed and implemented in Cha. 9 on page 91 The scenario from the hybrid model chapter is the basis of the simulation of the PF FDI method. The path can be seen in Fig. 4.20 on page 50. The faults are introduced 25 seconds into the simulation. The faults tested in this section is the three fault scenarios listed below: Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1. Fault 2: Propulsion Actuator Fault: No Actuation on wheel 3. Fault 3: Steering Actuator Fault: Max. Negative Actuation on wheel 2. The results of Fault 1 detection can be seen in Fig. 12.1 on the following page. The simulations shows that the fault was detected at approximately 28 seconds, 3 second after fault induction. Furthermore the fault was correctly isolated as fault 1. The simulation results for the remaining faults can be seen in Fig. 12.2 to 12.3 on pages 116–117

12.1 Conclusion on Particle Filter-FDI The Fig. 12.1 to 12.3 on pages 116–117 shows that the PF - FDI is able to detect and identify all implemented faults in under 4 seconds or 200 samples after fault induction. This is according to the specifications, listed in Sec. 1.2 on page 17, more specifically requirement 1, well below the maximum time allowed for the API from deviating from its path. Consequently this means that the PF - FDI is a valid method for detecting and isolating faults on the API. Therefore the only remaining problem is the processing requirements mentioned in Sec. 9.4 on page 96, which with the current specifications of the API OBC is unavailable. If the OBC is replaced, the designed PF - FDI method is easily extended to include all fault scenarios as well as all combinations of faults if so desired.

115

Accept Test of Particle Filter-FDI Method


0.5

x˙ M

0 −0.5 −1

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0.6 0.4

y˙ M

0.2 0 −0.2 −0.4

1

θ˙M

0.5

0

−0.5

3

Fault

Detected Fault Time

Real Fault Time

2

Fault Detected

1

0 0

5

10

15

20

25

Time [s]

30

35

40

45

50

Figure 12.1: The simulation results of the Particle Filters FDI method with Fault 1: Propulsion Actuator Fault: No Actuation on wheel 1 @ t=25. 1 Measurements No Faults Fault 1 Fault 2 Fault 3

0.5

x˙ M

0 −0.5 −1

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0.6 0.4

y˙ M

0.2 0 −0.2

1

θ˙M

0.5

0

−0.5

3

Real Fault Time

2

Fault

Detected Fault Time

Fault Detected

1

0 0

5

10

15

20

25

Time [s]

30

35

40

45

50

Figure 12.2: The simulation results of the Particle Filter FDI method with Fault 2: Propulsion Actuator Fault: No Actuation on wheel 3 @ t=25.

116


12.1 Conclusion on Particle Filter-FDI


0.5

x˙ M

0 −0.5 −1

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

0

5

10

15

20

25

30

35

40

45

50

1 0.5

y˙ M

0 −0.5 −1

1

θ˙ M

0.5

0

−0.5

3

Fault

Detected Fault Time

Real Fault Time

2

Fault Detected

1

0 0

5

10

15

20

25

Time [s]

30

35

40

45

50

Figure 12.3: The simulation results of the Particle Filter FDI method with Fault 3: Steering Actuator Fault: Max. Negative Actuation on wheel 2 @ t=25.

Group 1032b

117

Chapter 13

Accept test of Active Fault Isolation Supervisor This chapter will describe the accept test of the Active FI supervisor designed and implemented in Cha. 10 on page 101. As the AFI method is divided into two parts, the steering and propulsion faults will be tested in two separate tests.

13.1 Test of Steering Fault Isolation. The test is performed by introducing faults to the wheels by means of the fault simulation code, designed and implemented by a previous group[4]. The first scenario is performed with no faults active, in order to establish a baseline for the test. The faults introduced to the wheels are: • No output • No actuation • Sensor offset The results of the test is presented as the steering angle as measured by the steering sensor. The results of the no faults test can be seen in Fig. 13.1 on the following page The results show that the wheels is moved from side to side in order to test each fault possibility. The electro-mechanical stoppers was activated as expected. The detected fault was no faults i.e. a false alarm. The No output test in Fig. 13.2 on the next page shows that the steering angle measurement is not changing although the stoppers are activated as expected. This behaviour is correctly detected and isolated as the No output fault. The No actuation test in Fig. 13.3 on page 121 shows that the steering angles are constant and that the stoppers is not activated. This behaviour is correctly detected and isolated as the No actuation fault. The Sensor offset test in Fig. 13.4 on page 121 shows that the steering angles is offset by approximately 40◦ and that the stoppers is activated. This behaviour is correctly detected and isolated as the Sensor offset fault. 119

200 100 0 −100 −200 0

5

10

15

20

25

0

5

10

15

20

25

100 0 −100

Elec. Stoppers

Steering PWM

Steering Angler

Accept test of Active Fault Isolation Supervisor

1

Stopper Max Stopper Min

0 0

5

10

15

20

25

Time [s]

200 100 0 −100 −200 0

5

10

15

20

25

0

5

10

15

20

25

100 0 −100

Elec. Stoppers

Steering PWM

Steering Angler

Figure 13.1: The input and output of the no faults test.

1


0 0

5

10

15

20

25

Time [s] Figure 13.2: The input and output of the No output test.

120


200 100 0 −100 −200 0

5

10

15

20

25

0

5

10

15

20

25

100 0 −100

Elec. Stoppers

Steering PWM

Steering Angler

13.1 Test of Steering Fault Isolation.

1


0 0

5

10

15

20

25

Time [s]

200 100 0 −100 −200 0

5

10

15

20

25

0

5

10

15

20

25

100 0 −100

Elec. Stoppers

Steering PWM

Steering Angler

Figure 13.3: The input and output of the No actuation test.

1


0 0

5

10

15

20

25

Time [s] Figure 13.4: The input and output of the Sensor offset test.

Group 1032b

121

Accept test of Active Fault Isolation Supervisor

13.2 Test of Propulsion Fault Isolation. The test is performed by introducing faults to the wheels by means of the fault simulation code, designed and implemented by a previous group[4]. The three propulsion actuator was successfully detected and isolated. All wheels except the wheel subjected to the isolation, were shutdown. In the case of the No actuation fault, an attempt to move the API with the tested wheel was performed.

13.3 Conclusion The steering part of the AFI scheme was able to detect and isolate all introduced faults as well as detecting a false alarm, when the baseline test was performed. The propulsion actuator faults were likewise detected and isolated.

122


Chapter 14

Conclusion In the final conclusion of the report the objectives described in the introduction Cha. 1 on page 15 is evaluated and concluded on.

Objective 1: Fault Analysis Objective 1 was to perform a fault analysis and severity assessment of all wheel, proximity sensor and inclinometer faults. The fault analysis was performed on all selected faults. The severity assessment showed that the faults with the highest severity index was the proximity sensor faults. The wheel faults with the highest severity index was the steering sensor and actuation faults, due to the strong correlation between the sensor and actuator faults. The remaining actuator faults and the inclinometer faults was conclude to have a lower severity index, but was included due to their effect on the APIS performance. The propulsion sensors was excluded from further investigation as was all non-sense output faults.

Objective 2: Linear FDI Objective 2 was to design and implement a linear model based FDI method. The objective consists of two parts: A hybrid state observer and a continuous state observer. Three hybrid state observers were designed and implemented on the non-linear model. The best suited was selected as the state observer used to provide the continuous state observer with an linear model. The state observer was successfully able to follow a path selected as an representative of an actual path.

Objective 3: Critical Faults Objective 3 was to verify the detection of critical fault in the API . This has not been performed because the FDI scheme was not fully implemented on the API robot. Critical faults were however simulated and found as intended.

Objective 4: Non-linear FDI Objective 4 was to design and implement a non-linear FDI method. The chosen method was the a particle filter based approach, which was implemented for a subset of faults. 123

Conclusion

The approach was successfully tested with all implemented faults detected and isolated. In order for the particle filter method to be implemented on the API , a more powerful OBC must be present.

Objective 5: Active FDI Objective 5 was to design and implement an active FDI method. The method was fully implemented and tested for all steering actuator and sensor faults. The propulsion actuator faults were also implemented and tested. The active FDI method was able to detect and isolate all the desired faults faults.

Objective 6: Proximity Sensors Objective 6 was to design and implement proximity sensors on the API. The proximity sensors has been implemented and is capable of detecting obstacles in front of the API at such a distance, that the API is able to brake and stop before hitting the object.

Objective 7: Inclinometer Objective 7 was design and implement separate inclinometer in order to provide pitch and roll measurements. The inclinometer was implemented as is able to provide pitch and roll measurements at much greater accuracy then the built-in inclinometer on the compass.

Objective 8: Remote Shutdown Objective 8 was to design and implement relays for disconnecting individual wheels. The necessary hardware and software has been implemented. The Shutdown boxes is able to turn off or on each wheel individually.

124


Bibliography [1] T. Bak. Lecture notes - estimation and sensor information fusion. Available online on http://www.control.aau.dk/~tb/Teaching/Courses/ Estimation/sensfusion.pdf, November 2000. [2] Thomas Bak and Rooxbeh Izadi-Zamanabadi. Lecure notes - hybrid systems. Available online on http://www.control.auc.dk/~tb/hybrid/hs.pdf, 2004. [3] Jens Biltoft, Johnny Nielsen, and Peter Thomsen. Autonom robot til markanalyse. Master’s thesis, Aalborg University, 2001. [4] Morten Bisgaard, Dennis Vinter, and Kasper Zinck Østergaard. Modelling and fault-tolerant control of an autonomous wheeled robot. Master’s thesis, Aalborg University, 2004. [5] Jie Chen and R.J. Patton. Robust Model-based Fault Diagnosis for Dynamic Systems. Kluwer Academic Publishers, 1999. [6] Z Chen. Bayesian filtering: From kalman filters to particle filters, and beyond. Technical report, The Natural Sciences and Engineering Research Council of Canada, 2006. Available online on http://soma.crl.mcmaster.ca/~zhechen/download/ ieeebayesian.ps. [7] Svend Christensen. Api project. Aalborg University, 2000. Available online on http://www.cs.auc.dk/api/. [8] Randal Douc and Olivier Cappe. Comparison of resampling schemes for particle filtering. Image and Signal Processing and Analysis, 2005. ISPA 2005. Proceedings of the 4th International Symposium on, pages 64–69, 2005. 125

BIBLIOGRAPHY

[9] Gene F. Franklin, J. David Powell, and Micheal Workman. Digital Control of Dynamic Systems. Addison-Wesley, 1998. [10] Digital Compass Module HMR3000. Honeywell, 2004. Available online on http://www.ssec.honeywell.com/magnetic/ datasheets/hmr3000.pdf. [11] Anders Joergensen, Michael Kristensen, Peter Egtoft Nielsen, and Rene Soerrensen. Navigation af autonom markrobot. Master’s thesis, Aalborg University, 2003. [12] Ping Li and Visakan Kadirkamanathan. Particle filtering based likelihood ratio approach to fault diagnosis in nonlinear stochastic systems. IEEE transaction on systems,man and cybernetics, 31(3):337–343, August 2001. [13] D. D. Magill. Optimal adaptive estimation of sampled stochastic process. IEEE transaction of automatic control, 10(4), 1965. [14] PIC16F87X Data-sheet. Microchip Technology Inc., 2001. Available online on http://www.senscomp.com/specs/7000% 20electrostatic%20spec.pdf. [15] B. H. Nolte and N. R. Fausey. Soil compaction and drainage. Available online on http://ohioline.osu.edu/b301/index.html, 2001. [16] Eric J. Rossetter and J. Christian Gerdes. A Study of Lateral Vehicle Control Under a ’Virtual’ Force Framework. Stanford University, 2002. [17] 6500 Series Ranging Modules. SensComp, Inc., Sep 2004. Available online on http://www.senscomp.com/specs/6500%20module% 20spec.pdf. [18] Series 7000 Transducer. SensComp, Inc., Sep 2004. Available online on http://www.senscomp.com/specs/7000% 20electrostatic%20spec.pdf. [19] AGCO Global Technologies. Fieldstar. Available online on FieldStarUK/, 2007. 126

http://www.fieldstar.dk/Agco/FieldStar/


BIBLIOGRAPHY

[20] The SCA100T dual axis inclinometer series. VTI technologies, rev. a edition, May 2007. Available online on http://www.vti.fi/midcom-serveattachmentguid-e69426306a371172 SCA100T_inclinometer_datasheet_8261800A.pdf. [21] Greg Welch and Gary Bishop. An introduction to the kalman filter, July 2006. [22] Alan S. Willsky and Harold L. Jones. A generalized likelihood ratio approach to the detection and estimation of jumps in linear systems. Automatic Control, IEEE Transactions on, 21(1):108–112, February 1976.

Group 1032b

127

Part V

Appendix

129

Appendix A

Additional Hardware The API is equipped with a number of wheel actuators and sensors, each of which is controlled or sampled by one of the four LH28 computers. The interface between the LH28 and the wheels consist of four interface electronics boxes with additional components enabling the communication between different types of signals. The boxes is responsible for: • Connecting the steering PWM signal from the LH28 with the H-bridge. • Connecting the propulsion PWM signal from the LH28 with the Heinz motors. • Providing a safety measurement capable of disabling the actuation of steering actuator in one direction if the corresponding electromechanical stopper has been activated. • Connecting the steering encoders to the LH28. • Connecting the electromechanical stoppers to the LH28. • Removing non-linearities in the propulsion control signal. Previously the boxes has been in the form of a prototype and has not been completely documented in its current configuration. The prototype can be seen in Fig. A.1 on the next page The prototypes has previously been the source of problems. The use of fragile wires throughout the prototype together with the use of 24Vcc direct from the batteries increases the likelihood of serious short circuits. The 24Vcc used in the boxes are also used for the actuation of the wheels and are therefor protected with a 60 Amp fuse. This causes the majority of short circuits between the 24Vcc to either destroy the components in the affected circuit or burn a wire or path away on the prototype board. To avoid such situations from happening the prototype boards has been reverse engineered and implemented with a printed circuit board. The diagram of one the boxes can be seen in Fig. A.2 on page 133 The in/output of the different connectors are listed below: 131

Additional Hardware

Figure A.1: The prototype of the interface electronics boxes

132


4.75k 6.8k

VCC 6.8k

4.75k

1 14 2 15 3 16 4 17 5 18 6 19 7 20 8 21 9 22 10 23 11 24 12 25 13

4

74LS08

VCCh

6

5 12

J2 74LS08

5 9 4 8 3 7 2 6 1

11

13

VCC

HEADER 10

10 9 8 7 6 5 4 3 2 1

10 9 8 7 6 5 4 3 2 1

4.75k 4.75k

VCCh 10k 10k VCCh

HEADER 10

LH Agro

2.2u LED 74LS14 2

1

24vdc 22.1k 10k

10k

BC547

8 15 7 14 6 13 5 12 4 11 3 10 2 9 1

J3

1 6 2 7 3 8 4 9 5

H-bridge

Stoppers

5 9 4 8 3 7 2 6 1 Encoder

1 2 3

VCC

VCC gnd 24vdc

Heinz BD139 1k

7

33.2k 3.92k

3

+

2.21k 6

-

TLC2201 4

2

Figure A.2: Diagram of the interface electronics boxes

LH28 connector 3 Electromechanical stopper left output 15 Electromechanical stopper right output 5 Steering PWM input 6 Steering PWM input 7 Steering Encoder output 8 Steering Encoder output 10 Propulsion PWM output 11 Wheel speed output 12 Propulsion direction output

Heinz connector 1 Propulsion direction input 3 Wheel speed input 5 Propulsion PWM input 10 Status output Group 1032b

133

Additional Hardware

Steering encoder connector 3 Steering PWM output 4 Steering PWM output

Electromechanical stoppers connector 1 Electromechanical stopper left output 5 Electromechanical stopper right output

H-bridge connector 3 Steering Encoder output 5 Steering Encoder output The final version of the interface electronics boxes can be seen in Fig. A.3

Figure A.3: The final version of the interface electronics Boxes The new boxes works identically to the old and has subsequently been used as the standard instrumentation for the API.

134


Appendix B

Hardware Test In order to verify that the two implemented sensors function correctly and to get statistical data, a number of tests are carried out on the proximity sensor as well as the implemented inclinometer.

B.1 Inclinometer Test The inclinometer test is divided into two parts: A function test and a vibration test. The purpose of the function test is to determine if the inclinometer function as intended. The vibration test is carried out in order to determine if the new MEMS based sensor can handle the vibrations of the API robot better than the existing fluid based inclinometer.

B.1.1 Function test Test setup The inclinometer is placed on a surface which can be tilted on one axis with its x-axis along the edge of the surface. The surface is then tilted to an approximated angle, which was found using a ruler and simple geometry. Then the measurements are taken. The surface is tilted 3 times for each axis and 20 samples recorded for each angle and the average taken. The test is not intended as an accuracy test as the equipment for this is not available but rather as an integrity test of the measurements from the SCA100T inclinometer[20]. Results Approx. angle. 0◦ 6◦ 14◦ 24◦

Inclinometer output -2.72 3.14 9.12 20.37

Table B.1: Y-axis inclinometer test.

Approx. angle. 0◦ 6◦ 14◦ 24◦

Inclinometer output 0 5.88 11.85 23.10

Table B.2: Y-axis test corrected for offset.

135

Hardware Test

Approx. angle. 0◦ 6◦ 14◦ 24◦

Inclinometer output -3.33 2.63 9.81 20.73

Approx. angle. 0◦ 6◦ 14◦ 24◦

Table B.3: X-axis inclinometer test.

Inclinometer output 0 5.96 13.15 24.06

Table B.4: X-axis test corrected for offset.

As can be seen in Table B.2 on the preceding page and Table B.4 the measurements fit the angles well when compensated for the initial offset and therefore the data integrity is assumed to be good.

B.1.2 Vibration test The previous group[4] experienced some problems with the built-in inclinometer of the HMR 3000 compass module implemented on the API robot [10]. The sensor was far to noisy to be used for compensating the compass when the robot was moving. A new module was proposed as described in Sec. 3.2 on page 26. The inclinometer needs to be tested after implementation to verify its noise sensitivity. It is expected that the MEMSbased SCA100T[20] will handle vibration noise better than the old sensor could but being accelerometer based will probably have an affect on accuracy of the SCA100T when the API robot accelerates. Test setup The API robot is placed on an asphalt surface, which is close to level. All wheels are turned to 0◦ and the robot is actuated with 20% torque on all wheels for 10 seconds as the measurements from each sensor are logged by the OBC. Results Sensor HMR 3000[10] SCA 100T [20]

Roll[◦ ] 4.51 126.41

Pitch[◦ ] 2.48 36.43

Table B.5: Comparison of the variance calculated for the two different sensor types. When comparing data from the old inclinometer, seen in Fig. B.1 on the next page, with the new shown in Fig. B.2 on page 138 it is easily observed that the new sensor is less noisy than the old sensor.

B.2 Proximity Sensor Test The proximity sensors are implemented on the API robot in order to make it safer for its surroundings as well as to avoid damaging the API if it encounter an obstacle. The purpose of this test is to verify that the API will detect obstacles which are placed in its path. This includes a test to see if it is possible for an stationary object to get hit by the 136


B.2 Proximity Sensor Test

20

Roll[◦ ]

10 0

−10 −20 0

1

2

3

4

5

6

7

8

9

10

0

1

2

3

4

5

6

7

8

9

10

0

1

2

3

4

5

6

7

8

9

10

20

Pitch[◦ ]

10 0

−10 −20

Speed[m/s

0.6 0.4 0.2 0

Time[s] Figure B.1: Data from the HRM3000 inclinometer when moving on asphalt. The speed of the API is shown in the bottom graph. robot when it travels in a straight line. Test setup The assumption is that if the proximity sensors can detect an object which is inside the lines formed by the edge of the left and right wheels at a distance of 2m, it will not be possible to hit this object when the robot runs in a straight line. Additionally an object will be moved to different places inside the area in front of the robot to try and find a blind spot. The obstacle chosen for this test is a a metal cylinder 1.1m and 0.03m in diameter. The setup can be seen in Fig. B.3 on the next page Results As described the metal cylinder was placed at a distance of 2m from the proximity sensors as seen in Fig. B.3 on the following page. Different locations across the beams were tested and no blind spots where found inside the edges of the left and right wheel. Additionally the rod was moved to one side until it was no longer in sight of the sensors to find the real detection angle at that distance. Using simple geometry the angle is found to approx. 13◦ .

Group 1032b

137

Hardware Test

20

Roll[◦ ]

10 0

−10 −20 0

1

2

3

4

5

6

7

8

9

10

0

1

2

3

4

5

6

7

8

9

10

0

1

2

3

4

5

6

7

8

9

10

20

Pitch[◦ ]

10 0

−10 −20

Speed[m/s

0.6 0.4 0.2 0

Time[s] Figure B.2: Data from the SCA100T inclinometer when moving on asphalt. The speed of the API is shown in the bottom graph.

Edge of left wheel.

θ = 13◦

2m

θ = 13◦

Edge of right wheel.

Figure B.3: Top view of the detection area sensitivity angles on the SensComp 7000 ultrasonic transducers. The measured detection angle is shown.

138


Appendix C

Implemented Software This chapter describes the software modified and added during the project. The full source code can be found on the CD attached. The software can be divided into three groups: Simulink blocks comprising of both control and FDI blocks, stand alone programs and sensor/actuator interfaces. A list of the added software is shown below:

Simulink Blocks: • Proximity Supervisor. • Active FDI Supervisor • Hybrid FDI and State Observer

Standalone Programs: • Proximity Supervisor. • Automatic Calibration of Steering Actuators

Interfaces: • Inclinometer Interface • Proximity Sensor Interface • Remote Shutdown Interface The following sections describes the design and implementation of the software.

C.1 Simulink blocks These Simulink blocks provide additional features and functionalities to the Simulink controllers, FDI and state estimators. They can not be run by them self. 139

Implemented Software

C.1.1 Proximity Supervisor The proximity supervisor is responsible for the safe operation of the API, when the robot is operation autonomously, in regards to objects in front of the robot. The supervisor continually monitors the distances reported by the proximity sensors, and in case of a object inside the safety zone of 2 meters, instructs the controller, to enter a halt state. This state disables the controller, sets the LH28 controllers into standby and sets the propulsion reference to 0%. This effectivily stops the API from moving until the object is moved or moves itself away. A hysteresis of 0.3 meters is added in order to avoid a situation, where the API and the object is moving in the same direction, resulting in multiple starts and stops as the API catches up to the object, and then stops. The proximity supervisor is placed before the controller and the FDI in the controller structure shown in Fig. C.1. Proximity Supervisor

Controller

FDI and FTC

Steering & Propulsion controller

Figure C.1: The placement of the Proximity Supervisor in the control structure of the API. The supervisor has been tested and stops the API, when the object is inside the safety zone and returns to normal operation when the object is moved.

C.1.2 Active FDI supervisor The Active FDI Supervisor described in Cha. 10 on page 101, has been implemented in Simulink. The supervisor is placed after the FDI methods described in the following section. The supervisor uses the wheel pairs identified by the FDI methods as potentially faulty and performs a series of tests in order to isolated the faults. The output of the supervisor is compatible with the FTC supervisor implemented by a previous group. As the test involved in isolating faults involves disabling of the wheels, thereby disable the braking of the wheels, a safety check is implemented, that requires the API to be both stationary and on a level surface, before the test can be performed. The block can be seen in Fig. C.2 on the next page. The inputs are: • The status of the electro mechanical stoppers. • The speed of the wheels • The steering angle of the wheels • A control signal enabling the supervisor 140


C.1 Simulink blocks

Stoppers

Fault Fault Detected

Dphi

Wheel Shutdown

Beta

Steering Open Loop Enable Beta Ref Steering Pairs Propulsion PWM Propulsion pairs

Fault 07gr1032 b Active FDI in Progress

Speed

Steer PWM

Inclination Active FDI Supervisor

Figure C.2: The Simulink block: Active FDI Supervisor • The wheel pair to be tested for steering faults • The wheel pair to be tested for propulsion faults • The speed of the API • The inclination of the API The outputs are: • The fault detected as used by the previous FTC • The current state of the API :

FAULT , NOFAULTS

or HALT.

• The wheels that are to be disabled by the Remote Shutdown block. • The status of the wheel controllers: Open loop or closed loop • The steering reference signal • The propulsion PWM signal • The fault detected as described in this project • A signal indication the activation of the supervisor • The steering PWM signal for use in open loop control

C.1.3 Hybrid FDI and State observer The hybrid state observer selected in Sec. 8.1 on page 75 and the linear FDI method selected in Sec. 8.2 on page 81 has been implemented as a Simulink block. The block can be seen in Fig. C.3 on the following page. The inputs are: • Inputs given to the API robot. Includes torques and wheel angles for each wheel. Group 1032b

141


residual u

corr fault state

dchi−vector+theta

hybrid state fault detected

time

Detect only linear FDI (BFDF)

Figure C.3: The Simulink block: Linear FDI(BFDF) • Current estimated state. • Time The outputs are: • The euclidean norm of the residue. • The correlation vector that indicates correlation between an fault an a given input. • The current isolated fault or faults. • The linear state of the current linear model. • The current hybrid state. • Fault detected port. TRUE if a fault is detected. • Indicate whether the detect only state is activated.

C.2 Standalone Programs These programs can be used without the controllers active.

C.2.1 Proximity Supervisor The standalone proximity supervisor is identical to the proximity supervisor described in Sec. C.1 on page 139 in regards to the functionality it provides. The standalone supervisor can be run concurrently with the manual control. If a object is detected, a signal is sent to the previously implemented CAN communication program: “canif.c”, which subsequently issues a standby command to the LH28s. This stops the API from moving until the object is moved. The source code for this program is located in “c-code\proximity_supervisor.c” for the main program and “c-code\canif.c” for the extension to the CAN communication program. 142


C.3 Interfaces

C.2.2 Automatic Calibration of Steering Actuators The current software on the LH28s loses all information of steering position, when it is terminated or the API is powered down. As the encoder on the steering actuator is measured relative to the previous position, the wheel is left at the angle it had when the software closed. When the API is restarted, the current and potentially wrong position is used as the origin. The results in offset on the steering. This will in most cases only result in offset of the position and speed of the API, which can be suppressed by the controller but it can also result in complete shutdown of the robot. This happens when the initial position is more than 45◦ from the real origin. This allows the wheel to move to a position, where it is perpendicular to the direction of the robot, which causes the wheels to draw to much current and the fuses blowing. To prevent this situation from happening, the wheels had to be individually and manually calibrated, with either the joystick or the keyboard. This is a time consuming progress. In order to avoid this a automatic calibration of the steering wheels is needed. The designed method uses the electro-mechanical switches installed at the outer limits of the wheel. These stoppers are activated, when a magnet on the wheel moves close to them. The stoppers then prevents the wheels from turning in that direction by disabling the PWM signal to the H-bridge, driving the steering. The status of the stoppers are available on the OBC. The algorithm for the automatic calibration is as follows: 1. Turn the wheel in one direction until a stopper is activated. 2. Reset the LH28s measurement of the steering angle to zero. 3. Using the now know position of the wheels and the distance from the stopper to the real origin to move the wheel in the opposite direction until the halfway point between the two opposite stoppers is reached. 4. Reset the LH28s steering angle to the now correct origin. 5. Repeat step 1–4 for each wheel The algorithm has been implemented and is capable of calibrating the wheel to ± a couple of degrees, when standing on the laboratory floor or on asphalt or hard gravel land. If the API is placed on grass, the steering actuators is not powerful enough to turn the wheels and the calibration fails to return the wheels to the correct position. The problem with the steering actuators is known and has been documented by a previous group[4]. The program is implemented as a standalone program, requiring the program to be run manually before the use of any controllers. The algorithm can easily be converted to a Simulink, block allowing the controller to initialize the calibration, when needed. The source code for this program is located in “c-code\calibrate_wheels.c”.

C.3 Interfaces This section describes the interfaces designed and implemented for the use of the new sensors and actuators. The interface is defined as both the interface to the actual sensor Group 1032b

143


measurements or actuator control signals as well as the interfaces to Simulink in the form of Simulink block. The new interfaces are: Inclinometer Interface This interface samples the inclinometer data received from the FSAC box. The Simulink block makes the pitch and roll measurements available to the controller and the state estimator. Proximity Sensor Interface This interface samples the proximity sensors and calculates the distance from the API to the nearest object. The Simulink interface makes the measurements available to the proximity supervisor. Remote Shutdown Interface This interface is responsible for turning the wheels on or off as requested by the Active FDI Supervisor or a FTC supervisor. The interface uses the parallel port to send a signal to the Remote Shutdown Boxes as described in Sec. 3.4 on page 29. Furthermore a interface for the status of electromechanical stoppers has been made, allowing the Active FDI an known position of the wheel. The Simulink blocks can be seen in Fig. C.4.

Distance : Front Left Disable Wheel Distance : Front Right Proximity Sensor

Remote Shutdown

Pitch Stoppers Roll Inclinometer

Stoppers

Figure C.4: The interface blocks used in Simulink The source code for the new interfaces is located in “c-code\fsac_collect.c” for the Inclinometer and Proximity sensor interface, “c-code\remote_shutdown.c” for the Remote Shutdown interface.

144


Appendix D

Linear FDI D.1 Linear Model To be able to use linear observers for FDI on the API robot, a linear model is needed. The current nonlinear model can be found in Cha. 4.3 on page 39 and has the form:    ˙ θ, β1 , β2 , β3 , β4 , τref,1 , τref,2 , τref,3 , τref,4 )  x ¨ f (x, ˙ y, ˙ θ, ˙ θ, β1 , β2 , β3 , β4 , τref,1 , τref,2 , τref,3 , τref,4 )   y¨  =  f (x, (D.1) ˙ y, ˙ θ, ¨ ˙ θ, β1 , β2 , β3 , β4 , τref,1 , τref,2 , τref,3 , τref,4 ) θ f (x, ˙ y, ˙ θ,

To linearize the model a first order Taylor approximation is applied to the nonlinear model. The Taylor approximation is expressed formally as: df (x) ·x ˜ (D.2) f (x) ≈ f (¯ x) + dx x=¯ x

where x ¯ is the constant value of the operating point, and x ˜ is the changing small signal value, or in other terms: x=x ¯+x ˜ (D.3) For functions of two variables the principle is the same: ∂f (x, y) ∂f (x, y) ·x ˜+ f (x, y) ≈ f (¯ x, y¯) + ∂x ∂y x=¯ x, y=¯ y

x=¯ x, y=¯ y

· y˜

(D.4)

After applying this approximation (also called the first degree Taylor approximation), the operating point values are subtracted from the equation, after which only the small signal terms remain.

D.2 Matlab Implementation Because the model is being linearized in many different working points and because the model is fairly complicated, the process of linearizing the model is simplified by implementing a MATLAB function, which takes a set of working points as input and generates a linear model from these working points. The function does the following: 145

Linear FDI 1. Calculates the operating points for x˙ wp , y˙ wp, θ˙wp based on choices of βwp,i , τwp,ref,i and θwp . ˙ βi , τwp,ref,i and θ. 2. Differentiating the equations with regards to x, ˙ y, ˙ θ, 3. Evaluating the differentiated equations with regards to x˙ wp , y˙ wp, θ˙wp , βwp,i , τwp,ref,i and θwp giving the gains for every input and state. 4. Placing the resulting gains in the linear matrices A, B, C and D. The result is a linear model for every working point with the following state-space structure. x˙ = Ax + Bu y = Cx + Du

(D.5)

where 

  A= 

x ¨x,g x ÿ,g x ¨θ,g x ¨θ,g ˙ ˙ ˙ y¨x,g y ¨ y ¨ y ¨θ,g ˙ ˙ y,g ˙ θ,g ¨ ¨ ¨ θx,g θy,g θθ,g θ¨θ,g ˙ ˙ ˙ 0 0 1 0

    

(D.6)

 x ¨τref,1 ,g x ¨τref,2 ,g x ¨τref,3 ,g x ¨τref,4 ,g x ¨β1 ,g x ¨β2 ,g x ¨β3 ,g x ¨β4 ,g  y¨τref,1 ,g y¨τref,2 ,g y¨τref,3 ,g y¨τref,4 ,g y¨β1,g y¨β2 ,g y¨β3 ,g y¨β4 ,g   B=  θ¨τ θ¨τref,2 ,g θ¨τref,3 ,g θ¨τref,4 ,g θ¨β1,g θ¨β2 ,g θ¨β3 ,g θ¨β4 ,g  ref,1 ,g 0 0 0 0 0 0 0 0   1 0 0 0  C= 0 1 0 0  0 0 1 0 

D=0

(D.7)

(D.8) (D.9)

and h

M

x = x˙

y˙

M

θ˙ M θ M

iT

(D.10)

u = [τref,1 τref,2 τref,3 τref,4 βref,1 βref,2 βref,3 βref,4 ]T iT h y = χ˙ M = x˙ M y˙ M θ˙ M

146

(D.11) (D.12)


Appendix E

Fault Analysis This appendix show the behaviour of the API under a fault scenario. The faults simulated is described in Cha. 6 on page 61. As mentioned before all faults manifest them self as actuator faults due to the controllers placed on the LH28s, only actuator faults are simulated in the appendix. Due to the different behaviour of the API when turning in regards to when the API is driving straight, two nominal scenarios is chosen as basis for the simulation. The chosen scenarios are straight driving and turning around an ICR @ 5 meters. The figures show both scenarios side by side, with the nominal behaviour marked with . All faults are valid for all four wheels, but due to the similar behaviour only the first fault scenario is simulated on all wheels. The remaining faults are only simulated for wheel one and three. The faults are introduced 3 seconds into the simulation and remain throughout the simulation. The conditions for the test is: Straight driving

ICR

= 0m.

• β1−4 = 0 • τ1−4 = 10% • Simulation time = 60 seconds. • Fault time = 3 seconds. • Controller = Wheel controller. Turning

ICR

= 5m. 

• β1−4

 0.1107  −0.1107   =  −0.0907  0.0907

• τ1−4 = 10%

• Simulation time = 75 seconds. • Fault time = 3 seconds. • Controller = Wheel controller.

147

Fault Analysis

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.1: Simulation of the Propulsion Actuator Fault: No Actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]


148


Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]


Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]


Group 1032b

149

Fault Analysis

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.5: Simulation of the Propulsion Actuator Fault: Max. Positive Actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.6: Simulation of the Propulsion Actuator Fault: Max. Positive Actuation on wheel 3, when driving straight and turning around an ICR @ 5m.

150


Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.7: Simulation of the Propulsion Actuator Fault: Max. Negative actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.8: Simulation of the Propulsion Actuator Fault: Max. Negative actuation on wheel 3, when driving straight and turning around an ICR @ 5m.

Group 1032b

151

Fault Analysis

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.9: Simulation of the Propulsion Actuator Fault: Actuator Offset on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.10: Simulation of the Propulsion Actuator Fault: Actuator Offset on wheel 3, when driving straight and turning around an ICR @ 5m.

152


Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.11: Simulation of the Steering Actuator Fault: No Actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.12: Simulation of the Steering Actuator Fault: No Actuation on wheel 3, when driving straight and turning around an ICR @ 5m.

Group 1032b

153

Fault Analysis

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.13: Simulation of the Steering Actuator Fault: Max. Positive Actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.14: Simulation of the Steering Actuator Fault: Max. Positive Actuation on wheel 3, when driving straight and turning around an ICR @ 5m.

154


Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.15: Simulation of the Steering Actuator Fault: Max. Negative actuation on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.16: Simulation of the Steering Actuator Fault: Max. Negative actuation on wheel 3, when driving straight and turning around an ICR @ 5m.

Group 1032b

155

Fault Analysis

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 10 xN [m]

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.17: Simulation of the Steering Actuator Fault: Actuator Offset on wheel 1, when driving straight and turning around an ICR @ 5m.

Straight Driving

10

10

5

5

0

0

−5

−5

−10

−10

−15 −10

−5

0

5 N

x [m]

10

Turning

15

y N [m]

y N [m]

15

15

20

−15 −10

Nominal Faulty −5

0

5

10

15

20

xN [m]

Figure E.18: Simulation of the Steering Actuator Fault: Actuator Offset on wheel 3, when driving straight and turning around an ICR @ 5m.

156


Appendix F

Theory of UIO A linear system with all possible faults and additive unknown disturbance can be described by a state space model as [5]: x(t) = Ax(t) + Bu(t) + Ed(t) + R1 f (t) (F.1) y(t) = C x(t) + R2 f (t) where x(t) is the state vector, y(t) is the output vector, u(t) is the input vector, d(t) is the unknown disturbance vector, f (t) is a fault vector. A, B, C are system matrices, E is a disturbance distribution matrix, R1 and R2 are fault distribution matrices which represent the effect of systems faults. The structure of a full-order observer, for this system Eq. (F.1), is described as [5, page 71]:

z(t) = F z(t) + T Bu(t) + K y(t) x(t) = z(t) + H y(t)

(F.2)

where x(t) is the estimated state vector and z(t) is the state of the full-order observer and F , T , K, H are unknown matrices that have to be designed for achieving unknown input de-coupling and some other design requirements. The full-order observer is illustrated in Fig. F on the following page. When the UIO-based residual generator Eq. (F.2) is applied to the system Eq. (F.1), the state estimation error will be: e(t) = x(t) − x(t) ˆ˙ e(t) = x(t) − x(t)

˙ = x(t) − z(t) − H y(t)

= x(t) − z(t) − H(C x(t) + R2 f (t))

= (1 − H C)x(t) − F z(t) − T Bu(t) − K y(t) − H R2 f (t)

(F.3)

where K is defined as: K = K1 + K2 K y(t) = K1 y(t) + K2 y(t) = K1 (C x(t) + R2 f (t)) + K2 y(t)

(F.4) 157

Theory of UIO

disturbance Ed(t) input

faults R1 f (t)

R2 (t)f (t) output

System

u(t)

y(t)

TB

H r(t)

K −

R

z(t)

x(t)

y(t)

C

F Full-order Unknown Input Observer

Figure F.1: The structure of a full-order unknown input observer. Then substituting Eq. (F.4) into Eq. (F.3) yields:

e(t) = (1 − H C)[Ax(t) + Bu(t) + Ed(t) + R1 f (t)] − F z(t)

−T Bu(t) − K1 C x(t) − K1 R2 f (t) − K2 y(t) − H R2 f (t)

= [(1 − H C)A − K1 C]x(t) + [(1 − H C)B − T B]u(t) +(1 − H C)Ed(t) − F z(t) − K2 y(t)

+[(I − H C)R1 − K1 R2 ]f (t) − H R2 f (t)

(F.5)

By adding Eq. (F.6) to Eq. (F.5)

0 = −[(I − H C)A − K1 C]x(t) + [(I − H C)A − K1 C]x(t)

= −[(I − H C)A − K1 C]x(t) + [(I − H C)A − K1 C](z(t) + H y(t))

(F.6)

yield:

e(t) = [(I − H C)A − K1 C]e(t) + [(I − H C)B − T B]u(t)

+(1 − H C)Ed(t) + ([(1 − H C)A − K1 C] − F )z(t)

+([(1 − H C)A − K1 C]H − K2 )y(t)

+[(1 − H C)R1 − K1 R2 ]f (t) − H R2 f (t)

158

(F.7) Aalborg University 2007

F.1 UIO Design Example

The residual can be generated as: r(t) = y(t) − y(t)

= C x(t) + R2 f (t) − C x(t) = C e(t) + R2 f (t)

(F.8)

From Eq. (F.7) it can be seen, that if the following relations hold true: (1 − H C)E = 0 T

F

= (1 − H C)

= (1 − H C)A − K1 C

K2 = F H

(F.9) (F.10) (F.11) (F.12)

then the residual will be independent of the disturbance Eq. (F.9), inputs Eq. (F.10), outputs Eq. (F.12) and the internal states of the observer Eq. (F.11). Therefore the design of the UIO is to solve Eqs. (F.9) - (F.12) and making all the eigenvalues, of the matrix F , stable so the estimation error e(t) will approach zero asymptotically. There are some necessary and sufficient conditions for Eq. (F.2) to be a UIO for the system defined by Eq. (F.1) and these are: (i) rank(C E) = rank(E) (ii) (C, A1 ) is detectable pair, where A1 = A − E[(C E)T C E]−1 (C E)T C A = A − HCA = TA

(F.13)

F.1 UIO Design Example Step 1: Check for rank condition Scenario 1: rank(E1 ) = 1

(F.14)

rank(C · E1 ) = 1

(F.15)

rank(E1 ) = rank(CE1 ) ⇒ an UIO exists. Step 2: Compute H,T and A1 Scenario 1: H = E1 [(CE)T Ce]−1 (CE)T

(F.16)

T = I − HC

(F.17)

A1 = T A Group 1032b

(F.18) 159

Theory of UIO

where 

0.0000 0.0000 0.0000 0.0000

-0.4956 0.0000 0.5660 0.0000



0.0000 1.0000 0.0000 0.0000

0.4956 0.0000 0.4340 0.0000



-9.9978 -71.701 -8.7543 0.0000

0.4340  0.0000 H =  -0.4956 0.0000 0.5660  0.0000 T =  0.4956 0.0000 -1.1879  0.0008 A1 =   -1.0401 0.0000

-22.980 -7.7937 -20.122 1.0000

 0.0000 0.0000   0.0000  0.0000  0.0000 0.0000   0.0000  1.0000  6.6772 47.746   5.8468  0.0000

Step 3: Check observability Scenario 1: Observability matrix: 

              obsA1 C =              

1.0000 0.0000 0.0000 0.0000 -1.1879 0.0008 -1.0401 0.0000 25.306 8.0484 22.159 -1.0401 -626.69 -799.42 -548.74 22.159

0.0000 1.0000 0.00000 0.00000 -9.9978 -71.701 -8.7543 0.0000 929.90 5209.3 814.25 -8.7543 -71956. -3.8027·105 -63007. 814.25

0.0000 0.0000 1.0000 0.00000 -22.980 -7.7937 -20.122 1.0000 574.32 763.37 502.89 -20.122 -20005 -59614 -17517 502.89

0.0000 0.0000 0.0000 1.0000 6.6772 47.746 5.8468 0.0000 -619.64 -3469.0 -542.58 5.8468 47926 2.5324·105 41965 -542.58

                            

(F.19)

rank(obsA1 C) = 4

(F.20)

length(A1 ) = 4

(F.21)

rank(obsA1 C) = length(A1 ) ⇒ an UIO exists. 160


F.1 UIO Design Example

Step 4: Compute K1 Scenario 1: The MATLAB command “place” is used in order to place poles of the observer K1 = place(A1 , C, [−10 − 10 − 10 − 10]);



8.8121  0.0008 K1 =   -1.0401 0.0000

-9.9978 -61.701 -8.7543 0.0000

-22.980 -7.7937 -10.122 1.0000

(F.22)

 6.6772 47.746   5.8468  10.000

Step 5: Compute F and K F = A1 − K1 C

(F.23)

K = K1 + K2 = K1 + F H 

-10.000  0.0000 K1 =   0.0000 0.0000 

4.4723  0.00080000 K=  3.9161 0.00000

Group 1032b

0.0000 -10.000 0.0000 0.0000 -9.9978 -61.701 -8.7543 0.00000

0.0000 0.0000 -10.000 0.0000 -18.024 -7.7937 -15.783 1.0000

(F.24)  0.0000 0.0000   0.0000  -10.000  6.6772 47.746   5.8468  10.000

161

Appendix G

FDI Method Test G.1 Test of UIO method Figures from Fig. G.1 to G.7 on pages 163–165 show the isolation of the fault effects as indicated by each figure.

Faulty input

β4 β3 β2 β1 τ4 τ3 τ2 τ1 no fault 0

1

2

3

4

5

Time [s] Figure G.1: Isolated fault following the propulsion actuator fault: No actuation on wheel 1.

Faulty input


1

2

3

4

5

Time [s] Figure G.2: Isolated fault following the propulsion actuator fault: Max. positive actuation on wheel 1. 163

FDI Method Test

Faulty input


1

2

3

4

5

Time [s] Figure G.3: Isolated fault following the propulsion actuator fault: Max. negative actuation on wheel 1.

Faulty input


1

2

3

4

5

Time [s] Figure G.4: Isolated fault following the propulsion actuator fault: Sensor offset on wheel 1.

164


G.1 Test of UIO method

Faulty input


1

2

3

4

5

Time [s] Figure G.5: Isolated fault following the steering actuator fault: Max. positive actuation on wheel 1.

Faulty input


1

2

3

4

5

Time [s] Figure G.6: Isolated fault following the steering actuator fault: Max. negative actuation on wheel 1.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.7: Isolated fault following the steering actuator fault: Actuator offset on wheel 1.

Group 1032b

165

FDI Method Test

G.2 Test of Beard Fault Detection Filter method Figures from Fig. G.8 to G.14 on pages 166–168 show the isolation of the rest of the fault effects as indicated by each figure.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.8: Isolated fault following the propulsion actuator fault: ‘No actuation’ on wheel 1.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.9: Isolated fault following the propulsion actuator fault: ‘Max. positive actuation’ on wheel 1.

166


G.2 Test of Beard Fault Detection Filter method

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.10: Isolated fault following the propulsion actuator fault: ‘Max. negative actuation’ on wheel 1.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.11: Isolated fault following the propulsion actuator fault: ‘Sensor offset’ on wheel 1.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.12: Isolated fault following the steering actuator fault: ‘Max. positive actuation’ on wheel 1.

Group 1032b

167

FDI Method Test

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.13: Isolated fault following the steering actuator fault: ‘Max. negative actuation’ on wheel 1.

Faulty input


0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time [s] Figure G.14: Isolated fault following the steering actuator fault: ‘Actuator offset’ on wheel 1.

168


Appendix H

Active FI Supervisor This Appendix describes the procedure involved in the active isolation of faults in the steering and propulsion actuators and sensors.

H.1 Active Isolation of Steering Faults. Isolation of steering faults, both sensor and actuator, is based on the electro-mechanical stoppers implemented on the API . The stoppers are designed as a security measure in order to prevent turning the wheels outside the safe area as well as avoiding breaking of the cables inside the joint due to twists. The procedure of the isolation of steering faults is as follows: 1. When a fault regarding steering is detected, the Active FDI Supervisor is activated. 2. All wheels are powered down via the hardware implemented in App. 3.4 on page 29. 3. The following procedure is used for those of the four wheels, that the tected a possible faulty situation in.

FDI

has de-

3.a The wheel is turned on and the steering controller is set to open loop. 3.b The wheel is turned until the nearest stopper is activated, indicating full actuation. 3.c If the stoppers is not activated after a time period of 5 s. the wheel is identified as having a faulty steering actuator, specifically the no actuation fault. 3.d If the stopper indicating Max. actuation is activated at the start time of the AFI and if the stopper is not disabled, when turning the wheel the opposite direction, the wheel is identified as having a faulty steering actuator, specifically the Max. positive actuation fault. 3.e If the stopper indicating Min. actuation is activated at the start time of the AFI and if the stopper is not disabled, when turning the wheel the opposite direction, the wheel is identified as having a faulty steering actuator, specifically the Max. negative actuation fault. 3.f If one of the stoppers are activated, but the steering sensor shows no change from the sensor measurement taken at the start of the AFI, the wheel is identified as having a faulty steering sensor, specifically the No Output fault. 169

Active FI Supervisor

3.g If the sensor measurement shows that the wheel is at its maximum positive position, without the stopper indicating Min. actuation being activated, the wheel is identified as having a faulty steering sensor, specifically the Max. positive output fault. 3.h If the sensor measurement shows that the wheel is at its maximum negative position, without the stopper indicating Min. actuation being activated, the wheel is identified as having a faulty steering sensor, specifically the Max. negative output fault. 3.i If one of the stoppers is activated, but the steering sensor shows an offset between the known position of the stoppers and the one measured by the sensors, the wheel is identified as having a faulty steering sensor, specifically the Sensor offset fault. 3.j The wheel is turned off. 4. The isolated faults is returned to the FDI supervisor.

H.2 Active Isolation of Propulsion Faults. The isolation of propulsion faults is based on the velocity of the API : V M = 1. when a fault regarding propulsion is detected, the Active vated.

FDI

p

(x˙ M )2 + (y˙ M )2

Supervisor is acti-

2. All wheels are returned to β = 0. 3. All wheels are powered down via the hardware implemented in App. 3.4 on page 29. 4. The following procedure is used for those of the four wheels, that the tected a possible faulty situation in.

FDI

has de-

4.a The wheel is turned on and the propulsion controller is set to zero torque reference:τref,i = 0. 4.b If the velocity V M is greater than a set threshold, the wheel is identified as having a faulty propulsion actuator, specifically the Max. positive actuation fault. 4.c If the velocity V M is smaller than a set threshold, the wheel is identified as having a faulty propulsion actuator, specifically the Max. negative actuation fault. 4.d If the velocity V M shows that the API is stationary and the propulsion sensor show the max. positive speed, the wheel is identified as having a faulty propulsion sensor, specifically the Max. positive output fault. 4.e If the velocity V M shows that the API is stationary and the propulsion sensor show the Min. positive speed, the wheel is identified as having a faulty propulsion sensor, specifically the Min. positive output fault. 4.f The propulsion controller is set to 25% torque reference:τref,i = 25. 4.g If the velocity V M shows that the API is stationary, the wheel is identified as having a faulty propulsion actuator, specifically the No actuation fault. 170


H.2 Active Isolation of Propulsion Faults.

4.h The wheel is turned off. 5. If no propulsion actuator faults is isolated, the remaining propulsion sensor faults can be isolated 5.a All wheels are turned on and the propulsion controller is set to τref,i = 25% for 2 seconds hereafter the reference is set to zero. (a) Using the kinematic model for straight driving as seen in Eq. (H.1) 4 1 X ˙ φi cos(βi ) x˙ = 4 B

y˙ B =

1 4

i=0 4 X i=0

φ˙ i sin(βi )

(H.1)

5.b If one of the propulsion sensors shows zero output and the velocity V M is equal to the calculated velocity, the wheel is identified as having a faulty propulsion sensor, specifically the No output fault. 5.c If the velocity V M is equal to the calculated velocity, and the propulsion sensor is not equal to the estimated wheel speed, which is calculated based on Eq. (4.36), the wheel is identified as having a faulty propulsion sensor, specifically the Sensor offset fault. 5.d The wheels are turned off. 6. The isolated faults is returned to the FDI supervisor.

Group 1032b

171

Appendix I

Kalman Filter The Kalman filter used in the MHT state observer designed in Sec. 8.1 on page 75 is the linear discrete Kalman Filter[21] The Kalman filter is an recursive continuous state observer using knowledge of the systems model, process noise and measurement noise. The model of the system can be written as a discrete state: xk = Axk−1 + Buk + wk

(I.1)

zk = Cxk + vk The A,B and C matrix can be found in App. D.1 on page 145. The process noise w is introduced as the change in τr,i resulting from the noise in the propulsion sensor[4]. The propulsion actuator noise is: 2 ) wpropulsion = N (0, σpropulsion = 5 · 10−4 · φ˙ σ2 propulsion

(I.2) (I.3)

2 The process noise covariance matrix is Q = I · σpropulsion The measurement noise v is selected as the noise on the four sensors used in the output M M ˙M T measurement vector[4]: y = [θCompass x˙ M GP S y˙ GP S θGyro ] . The noise is:

vθM = N (0, σθ2Compass )

(I.4)

vx˙ M = N (0, σx2˙GP S )

(I.5)

vθ˙M =

(I.7)

vy˙ M =

N (0, σy2˙GP S ) N (0, σθ2˙ ) Gyro

(I.6)

Where σθ2Compass = 0.016[◦ ]

(I.8)

σx2˙GP S = 1 · 10−4 [m] σy2˙GP S σθ2˙ Gyro

−4

= 1 · 10

[m]

−5

= 1.1 · 10

◦

[ /s]

(I.9) (I.10) (I.11) 173

Kalman Filter

The measurement noise covariance is: 

  R= 

σθ2Compass 0 0 0

0 σx2˙GP S 0 0

0 0

0 0 0

σy2˙GP S

σθ2˙

0

Gyro

The Kalman filter process is divided into two steps:

    

(I.12)

• The prediction step, which uses a previously estimated state and the linear model to predict the value of the next state as well as the state estimate covariance: x ˆk|k−1 = Axk|k−1 + Buk

(I.13)

T

Pk|k−1 = APk−1|k−1 A + Q • The update step, which uses the current measurement of the output together with the statistical properties of the model, to correct the state estimate. The values calculated is the innovation covariance, the Kalman gain resulting in the updated state estimate and state estimate covariance:

Sk = CPk|k−1 C T + R

(I.14)

Kk = Pk|k−1 C T Sk−1 x ˆk|k = Axk|k−1 + Kk (Zk − C x ˆk|k−1

Pk|k−1 = (I − Kk C)Pk|k−1

The two steps is repeated for every sample: k = 1, 2, . . . , K

174


Appendix J

Operational Requirements A number of requirements for the operational performance of the previous groups:

API ,

has been set by

1. The robot must be able to follow a straight line connecting two way points with a precision better than ±10 cm. 2. The robot must be able to arrive at a way point in a set with a orientation within ±10◦ of the designated orientation. 3. The robot must be able to determine its position and orientation with a precision better than ±5 cm and ±5◦ . 4. When the robot is operating nominally and driving on a field, planted with sensitive crops or with crops with sufficient width between the row, to fit the wheels of the robot, it must never drive on top of the crops. 5. When a fault occur, the robot is allowed to deviate from its course, for instance driving on top of the rows, for a maximum of 30 seconds. 6. The robot must be able to continue operation with at least one sensor/actuator fault. 7. The robot must be kept operational as long as possible under faulty conditions. 8. Operating under both normal and faulty operation the robot must not be potentially dangerous to its environment. As the API is designed for versatility, a single set of requirements is impossible. The requirement listed above is selected based on the considerations listed below: Requirement 1 is selected as a path traversing a field is expected to have a overlap, of approximately 10cm in order to ensure perfect coverage. Requirement 2 is selected based on the fact that the API can drive in all directions. In order to avoid the API arriving at a way-point in a undesirable orientation, the API must be inside ±10◦ of the chosen orientation. 175

Operational Requirements

Requirement 3 is chosen based on the precision of the sensors responsible for the measurements, the requirements are to be compared to. The sensors is question is the compass and the GPS , with the following specifications: • Compass: ±1.5◦ • GPS: ±3 cm

These values are under optimal conditions and therefore the selected values are set at a higher value. Requirement 4 and 5 is selected as a worst case scenario. This involves driving on a field with sensitive crops. It is decided that a controller capable of driving on that kind of field, is easily adapted to driving on less sensitive crops. Requirement 6 and 7 is a general requirement as an autonomous robot always is expected to function as long as possible. Furthermore if the robot fails in the middle of a field, there is the additional disturbances caused by locating and retrieving the robot, as is it unlikely that all but the most simple faults can be repaired in the field. Requirement 8 is based in the environment, the robot is designed to function in. The reason is that the robot can not be assumed to be the only entity on the field. Some of the other entities are tractors, animals, both domesticated and wild, and in the case of faults or service to the robot, also the farmer himself or a service technician.

176


Appendix K

Pitch and Roll Compensation of GPS, Gyro and Compass The three sensors GPS, Gyro and Compass all measures in the M or B frame. Since the inertial frame is the N frame, the sensor measurements must be compensated for the tilt of the robot. The inclinometer described in Sec. 3.2 on page 26 provides a pitch and roll measurement, which describes the tilt of the robot in regards to the B frame. In order to compensate for measurements in the M frame, the pitch and roll measurement must be rotated using the rotation matrices listen in Sec. 4.1 on page 33. The rotated inclinometer measurement is shown in the following equation: pitch ψx (B→M)T · =R (K.1) ψy roll The ψx and ψx angles is the tilt of the M

K.1 GPS The GPS system provides the position of the API. The GPS uses information from three or more satellites to determine the position. This position is centered in the GPS antenna placed in the GC : 107 cm from the ground. When driving on level terrain, it is correct to assume that the position reported by the GPS is the position of the GC in relation to the ground. This changes when the API is placed on a tilted surface. If the API is placed on a hill, the vector projected from the GPS antenna to the ground, does not cross the GC as when level. This means that the position reported by the GPS does not correspond to the actual position of the API. The offset of the position measurement can be calculated using the tilt of the surface and the hight of the GPS antenna as shown in equation K.2. ∆xGP S (t) sin(ψx )hGP S = (K.2) ∆yGP S (t) sin(ψy )hGP S

K.2 Gyro The gyro measures the angular velocity θ˙ of the API in the B frame. To convert the gyro measurements from the B frame to the N two rotations is needed. The tilt corrected 177

Pitch and Roll Compensation of GPS, Gyro and Compass

angular velocities can be seen in the equation below: T T θ˙ N = R(M→N ) · R(B→M) · θ˙ B

(K.3)

K.3 Compass The compass measures the orientation θ of the API with regards to the magnetic north. This means that the compass measurement is performed in the M frame. The compensated compass measurement can be seen in the equation below: T

θ N = R(M→N ) · θ M

178

(K.4)
