AUV Inertial Guidance System
Table of Contents
The goal of the inertial guidance system (IGS) is to be able to calculate the current location of the AUV relative to some arbitrary initial starting position. The IGS consists mainly of a 2-axis accelerometer contained in single monolithic IC, a 3-axis compass, and a PIC16F876 microprocessor. The compass provides heading, pitch and roll information to the PIC via the AUV's main computer. With this information and sampled acceleration measurements the PIC calculates the AUV's current position. This is analogous to dead reckoning in a sailboat. The current position is sent to the main computer. Communication occurs over an I2C serial link.
Work on the IGS began in September 2002 and initial testing was completed in February 2003. The IGS was found to work in principle, but was very inaccurate. This is because the accelerations that need to be measured are of the same size or smaller then the noise produced by the accelerometer. Thus the accelerometer is not able to accurately measure the small acceleration that would be produced by a slow moving AUV.
The purpose of this paper is to document the work that went into the IGS for the AUV team members. Perhaps a working IGS can be produced in the future.
Appendix A contains a list of all documents referred to during the design of the IGS and all software used. It also lists the hardware components used.
The main steps in the design and testing of the IGS were as follows.
1) Established coordinate systems and theoretical relationships for calculating position.
2) Became familiar with the component specifications (PIC and accelerometer)
3) Created a pseudo code algorithm for calculating position.
4) Determined accelerometer settings.
5) Mounted the accelerometer on a prototyping board for testing and calibration.
6) Became familiar MPLAB IDE and C programming language.
7) Wrote C code for PIC.
8) Wrote C code for outputting values to an LCD.
9) Connected PIC, accelerometer and down loading hardware together on a breadboard.
10) Debugged and tested the IGS C code.
It was decided upon to use magnetic compass directions as an absolute coordinate system and to have a relative coordinate system located on the AUV. Please see Figure 1. The AUV's compass provides direction relative to magnetic north and this information can be passed to the IGS's PIC microcontroller. The relative coordinated system corresponds to the direction that the AUV is pointing and is level with the earth. The signs of the roll and pitch values were also decide upon at this time. Please see Figure 2. These signs correspond to the signed values obtained from the AUV's compass.
The calculation of the AUV's relative position is based on the following equation from physics.
The first part is for continuous acceleration and the second part is for discrete acceleration that is constant over the time period Dt. This can be assumed true if Dt is small enough. This equation is applied to both the AUV's x and y axis.
The accelerometer is an AXDL202 from Analog devices and the PIC microcontroller is a 16F876. The specifications of each of these were looked at and the relevant features of each were decided upon.
The accelerometer measures acceleration in two axes and has a range of ±2g. The output signals of the accelerometer are duty cycle modulated square waves, one for each axis. For 0g the nominal duty cycle is 50%. For 1g in either direction the duty cycle changes by a nominal ±12.5% (37.5% - 62.5% duty). These two signals are connected to the input capturing pins of the PIC16F876. The duty cycle of each signal can then be calculated by using interrupts and the PIC's internal 16-bit timer.
The PIC can be operated at a variety of frequencies with 20MHz being the fastest. This value of 20 MHz was chosen as this gives the best time resolution when measuring the duty cycles with the 16-bit timer.
Overview of IGS operation
The AUV's main processor will run in a control loop of approximately 0.1 seconds duration and each loop it will send an updated pitch, roll and magnetic direction to the IGS's PIC. Between updates the PIC will treat these values as constants.
The PIC runs in its own loop where it monitors the X and Y duty cycle signals from the accelerometer. The duty cycles are measured once each loop. The time between duty cycle measurements (acceleration samples) is fixed. The PIC then calculates a new position after each set of acceleration measurements.
The AUV's main processor will request the current position each control loop and the IGS's PIC will send it.
The communication between the AUV's main processor and the IGS's PIC consists of the following types.
1) sends updated magnetic direction (md), pitch (p) and roll (r)
2) requests current position (xm & ym)
3) requests a position reset (xm = ym = 0 )
other possible requests
4) requests a velocity reset (Vxm = Vym = 0)
5) request for current velocity (Vxm & Vym)
6) request for current acceleration (ax & ay)
- PIC uses the current values of pitch (p), roll (r) and magnetic direction (md).
- measure edge times for duty cycle signals, Txup, Txdown, Tyup and Tydown
- calculate duty cycles from Txup, Txdown, Tyup and Tydown
- calculate the acceleration in each direction on the chip, (axch) and (aych) (ch=chip).
- use pitch (p) and roll (r) values to correct for gravity when (r) and (p) are not
equal to zero. Gives (axchgc) and (aychgc) (gc=gravity corrected)
- use roll (r) and pitch (p) to convert accelerations to AUV's coordinate system
gives (ax) and (ay)
- multiply (ax) and (ay) by Dt (fixed) to get change in velocities (dvx and dvy) along x
and y axis of AUV.
- use magnetic direction (md) to break each change in velocity (dvx and dvy) into
magnetic components (dvxm1, dvxm2, dvym1 and dvym2)
- Add x and y components. dvxm = dvxm1 + dvxm2, dvym = dvym1 +dvym2
- Add change in velocities (dvxm & dvym) to velocities (vxm &vym)
Thus vxm=vxm+dvmx and vym=vym+dvym
- Calculate distances traveled (dxm and dym) in time Dt. dxm=vxm*Dt, dym=vym*Dt
- calculate new relative magnetic position for time, xm=xm+dxm, ym=ym+dym
end of main loop
There are two main setting for the accelerometer, the period length of the duty cycle and the signal bandwidth of one of the accelerometer's internal stages. The duty cycle influences how often an acceleration sample can be measured and is set with a single resistor, while the signal bandwidth influences the acceleration measurement resolution and is set with capacitors.
The noise floor for the accelerometer is proportional to the bandwidth, so the lowest possible bandwidth of 10 Hz was chosen. At this bandwidth the rms noise value is theoretically 0.8 mg (g = gravity = 9.81 m/s). This gives a peak-to-peak noise value of 3.2 mg (95% probability) or 4.8 mg (99.7% probability).
The range of possible periods for the duty cycle is form 0 .5 ms to 10 ms. A period of 10 ms was selected as this is fast enough and it was originally hoped that the position calculations could be done in the time between acceleration samples. It turns out that it actually takes several complete periods to calculate the position so it does not really matter that much.
The formulas for these calculations are given in the ADXL202's specification sheets. The resistor and capacitor values used are as indicated in the IGS schematic located in Appendix B.
The AXDL202 only comes in an 8-pin surface mount package. A copper clad board was cut up and made for mounting the device. This board was then attached to a small prototyping board with screws and spacers so that the accelerometer could be mounted level. Please see Figure 3. The prototyping board would hold the PIC and related components.
Using a breadboard to hold the required resistors and capacitors the accelerometer was tested in the lab. An oscilloscope was used to make duty cycle measurements for the x and y axes. The following results were obtained for Vcc=5.0 V and the set up shown in the schematic in Appendix B.
condition duty cycle
x axis level 51.75%
+x axis up 64.7%
+x axis down 38.8%
y axis level 50.3%
+y axis up 63.1%
+y axis down 37.5%
These measured values were used in the PIC software rather then the 37.5%, 50.0% and 62.5% nominal values.
The period of the duty cycle was found to be 9.08 ms instead of the nominal 10 ms, which gives a frequency of 110.31 Hz.
MPLAB is the free integrated development environment for the PIC processor. It has a source code editor, simulator and numerous other features. Only the simulator was used and the make command for compiling the C source code. The simulator does not work in real time and has limited ability to debug code that uses interrupts and so was not used very much.
The calculations, which are required to be performed on the PIC are too complicated to be done easily in assembly and so the software was written in C. A company called High-Tech C makes an ANSI C compiler for the PIC microprocessors and this is supported by the MPLAB IDE as a third party tool. Thus, with C all the calculations could be done in floating point representation.
High-Tech C has some extensions to the ANSI standard that allow it to work easily with the PIC. A couple of examples are as follows.
volatile unsigned int captured_CCP1_time @ 0x15;
volatile bank2 double T1x;
With @ 0x15 a specific byte at address 0x15 in the PIC's ram is assigned to the variable captured_CCP1_time. This specific byte is a special function register that holds the first byte of a captured timer value.
In the second example bank2 is a reserved word that tells the compiler to place the variable T1x in bank2 of the PIC's ram.
When using the make command in MPLAB, it compiles the source code by executing the DOS command PICC (High-Tech C's command line driver) followed by various arguments. The arguments used were as follows.
- FAKELOCAL -G -O -D24 -ASMLIST -16F876 -ICD -IC:\HT-PIC\INCLUDE
The -D24 argument is used to reduce the size of doubles from 4 bytes to 3 bytes. The 3 byte format is the modified IEEE 754 24 bit representation. This helps to reduce the memory requirements. The -O argument invokes the post-past optimizer. This greatly reduces the amount of RAM required for global variables. The C code written for the IGS would not compile without this argument! The -ICD argument reserves the top 255 words in RAM for a boot loader programme.
The C code is structured around the rising and falling of the Xout and Yout signals from the accelerometer. Interrupts are used to capture the times at which these events occur.
By knowing the times of each event relative to the initial Xout signal going high, it is possible to calculate the duty cycles for each output signals. The specific technique used,
is the one that is recommended in the document "Using the ADXL202 Duty Cycle Output", a Technical Note. Figure 4 is a block diagram that gives a simplified overview of the IGS software. Figure 5 gives shows the timing of the various software functions and how the whole process repeats every six periods. Note that the programme waits after the position calculations are performed for the fifth period of Xout to complete before going into the idle loop that waits for the first rising Xout edge that starts the whole process again. This was done so that the loop time would be fixed at six periods of Xout. This can be easily seen in the source code's main function. Figure 4 does not show this subtle point, which is important.
Of interest is the fact that the position calculations take about 2.6 periods. The PIC uses a 20 MHZ crystal and the instructions execute at a quarter of this frequency. The 2.6 periods thus equate to about 118000 microprocessor instructions!
The completed C source code ended up being 630 lines long. Note that the function that handles interrupts from the AUV's main processor via the IIC serial line has not been written.
In order to test the IGS software on the PIC in real time, an LCD was connected to it. This allowed any single number to be displayed. Initially the LCD code was written for the PIC as a stand-alone programme to ensure that it worked properly and then it was incorporated into the IGS programme as a function.
Most of the numbers that are used in the calculations are represented as floating point doubles. Thus it was necessary to write a programme/function that would take a double as its input and then break it apart in such a way that the individual ASCII code values could be obtained for each of its digits. The ASCII values are required when writing values to the LCD's data register. The process was simplified a little by scaling the received double and then making it an integer. For example the distance 14.6729989 meters would become 1467 centimetres. Which way the scaling went would depend on the size of the value that was to be displayed.
With this function written it was possible to stop (used an infinite loop) the PIC's programme anywhere and display the value of any variable that was desired. Also, the value of a continuously changing variable could be displayed on the LCD. In this way it was possible to debug the IGS software.
The LCD was a 1x16 Hitachi HD44780 that was borrowed from the lab technicians at school.
The circuit as shown in the schematic of Appendix B was constructed on a breadboard. In addition to this hardware, a level converter chip was also used. It converts the ±13V on a PC's serial cable to the 5V/0V used in TTL logic. Please see Figure 6 and 7. The pieces of Styrofoam were used so that the whole apparatus could be moved/slid around easily during testing.
To allow for easy downloading of the PIC's code without the continual need for a burner, a boot loader program was used. This programme is burned onto the PIC only once and then it is used to download any other code that is desired. It resides in the top 255 bytes of the PIC's RAM.
At reset the PIC starts executing the boot loader programme and it will monitor the USART's Tx and Rx pins for about 0.2 seconds. These pins are connected to a PC's serial port through the level converter chip. If the corresponding down loading software is running and has been directed to download a new programme, then the boot loader will download it and place it in the PIC's memory. The downloaded programme will then execute. If there is no programme to be downloaded then the boot loader programme will execute the previously downloaded programme.
The LCD and boot loader programme tuned out to be invaluable in debugging the IGS software. Initially it was thought (naively) that the MPLAB simulator would be sufficient.
The procedure for testing the software can be described generally as 1) make some modification to the source code, 2) recompile it with MPLAB to get the hex file, 3) download it to the PIC in about 30 seconds and 4) see what number gets displayed on the LCD. This little procedure was repeated dozens if not hundreds of times.
In debugging there were several logical mistakes and several type-o type mistakes found. There were also a few problems with variable types. In the code a large number of variable types were used and calculations were often done that involved variables of several different types. This was especially true in the binary to decimal conversion function for outputting values to the LCD.
However, the main mistake/bug made was in misunderstanding how the input-capture flags of the PIC got set. Initially it was thought that they did not get set if the input-capture interrupt was not enabled. In reality, they get set whenever the input-capture function is operating. This caused problems when the interrupt handler function polled various flags to determine the source of an interrupt. Once this was corrected the debugging advanced quickly.
One lesson learned was that, it would have been much easier to have had the LCD and boot loader set-up and working before writing the IGS source code. This way the code could have been tested incrementally as it was written instead of being tested all in one go. This would have saved some trouble.
For the condition of the accelerometer and hardware being level and not moving, the values of Dx and Dy (duty cycles) were displayed on the LCD and found to be as follows.
Dx = 51.80-52.00 % lab measurement --> 51.75%
Dy = 50.25-50.45 % lab measurement --> 50.30%
Both values had a range of about 0.2%. With 1g being represented by about a 12.5% change in duty cycle, the 0.2% range corresponds to a 16 mg range. The theoretical peak-to-peak noise was calculated to be 4.8 mg, which is about 3 times smaller then what was found!
The IGS software makes use of a dead-band, which is a range of duty cycle values centred around the 0 g value, for which the assumption is made that the acceleration is in fact 0 g. This allows the noise to be ignored when the IGS hardware is level and stationary. The result is that no change in calculated position results from the noise.
However, the problem is that when the dead-band is sufficiently large to do its job
(24 mg), it also causes the small and yet significant accelerations due to actual movement of the hardware to be ignored. If the dead-band is reduced then the noise causes fictitious positions to be calculated.
It turns out that the magnitude of the accelerations produced by slowing moving the IGS hardware around from rest to approximately walking speeds is the same size or smaller then the noise! The following diagram shows this visually.
The accelerometer measures static acceleration as well as dynamic, so gravity itself can be used to produce accelerations by tilting the hardware. Using this technique to simulate horizontal dynamic accelerations and watching the value of the position displayed on the LCD, it was found that the software does in fact work in principle. It is just not accurate for small acceleration values.
The IGS was found to work in principle, but was very inaccurate. This is because the accelerations that need to be measured are of the same size or smaller then the noise produced by the accelerometer. Thus the accelerometer is not able to accurately measure the small acceleration that would be produced by a slow moving AUV. If a working IGS is to be made then a more sensitive accelerometer most be used.
- list of reference documents
- ADXL202 specification sheets
- ADXL202 Duty Cycle Output, Technical Note
- PICmicro Mid Range MCU Family Reference Manual
- PIC16F87X Data Sheet
- MPE IDE, Simulator, Editor Users Guide
- MPE IDE Project Tutorials for Third Party Tools
- HI-TECH PIC C ANSI C compiler manual
- HD44780U LCD specification sheets
- binary to decimal conversion algorithm,
- boot loader information
- list of software used
- MPLAB IDE
- HI-TECH PIC C ANSI C compiler
- Target 3001 V10 (schematic capturing)
- PIC down-loader 1.08
- list of hardware components
- PIC 68F876
- ADXL202 dual axis accelerometer
- HD44780 LCD (for testing)
download the source code here