- Email: [email protected]
Critical Real-Time Software for an AUV Navigation System
Critical RealTime Software for an AUV Navigation System Giovani Amianti* Ettore A. de Barros**
*Escola Politécnica, São Paulo University, São Paulo, Brazil, (email: [email protected]) ** Escola Politécnica, São Paulo University, São Paulo, Brazil, (email: [email protected]) Abstract: This work describes the navigation software for the AUV Pirajuba. It is considered a critical realtime system and, therefore it should be developed according to a critical realtime methodology. The methodology proposed was based into five guidelines: development process; norms; operating system tools, application tools, and mathematical tools. The GESAM control architecture is presented for achieving generality, extensibility, safety, reliability, deliberation and reactivity. Inside GESAM, an Extended Kalman Filter was implemented for integrating information from Inertial Measurement Unit, GPS and Compass sensors. Performance tests showed that the system could fulfill requirements using 38.3% of processing consumption. Keywords: Architectures, Navigation Systems, Extended Kalman Filters, AUV, SafetyCritical, Real Time System, Methodology, Requirements Analysis, Model Driven Development, System Certification. 1. INTRODUCTION The Pirajuba Project has been executed at the Unmanned Vehicles Laboratory (LVNT), in the University of São Paulo. This project deals with the development of low cost Autonomous Underwater Vehicles (AUVs), emphasizing investigations on hydrodynamics and navigation. The Pirajuba (which means “yellow fish”, in the Tupi language) is an AUV which is 1.78m in length, and weighs 66kg in air (Fig. 1). The cruising speed and autonomy are 1m/s and 1h, respectively.
Fig. 1. Model of the AUV Pirajuba. The hybrid hardware architecture includes a central unit based on a PC104, and a number of distributed units based on microcontroller boards. The central unit performs the main functions during the mission such as sensor data acquisition, navigation, control, guidance, and mission management. The distributed units perform the control of actuators and the communication with the base station.
for AUVs. Overviews of the many approaches can be found in Hall and Adams (1992), Valavanis et al.(1997), and Kim and Yuh (2004). Recent proposals of control architectures are presented in Sotzing et al. (2007), El Jalaoui et al. (2005), Kim and Yuh (2004). Other works have claimed the realtime implementation of functions related to specific tasks such as navigation (Thakur and Conrad, 2007), path planning (Duan and Zhang, 2006; Kawano, 2006) and mapping (Liu and Schimidt, 2004). In all those works, only basic elements of a critical realtime system are presented, such as: hardware architecture, operating system, programming language, and software architecture. In order to consider the development of a critical realtime system, this work shows that a deeper approach should be taken into account. For that purpose a new methodology for the control architecture development is presented. In the next section, the methodology is presented. The software requirements that are analyzed specifically for the Pirajuba control architecture are presented in section 3. The software architecture is presented in section 4. The application for the AUV navigation module, which is based on the implementation of an Extended Kalman Filter, is presented in section 5. Laboratory results including sensor data acquisition and calculation in realtime are presented in section 6. Finally, section 7 includes the final comments on the proposed methodology and software architecture. 2. METHODOLOGY
This vehicle, as any other AUV, can be classified as a critical realtime system. Failures in performing the right instructions at the right time may jeopardize the mission tasks, or even endanger the vehicle and the surrounding environment. Consequently, the whole system, besides navigation software, should be developed according to a methodology of critical realtime systems that guarantees safety and reliability. A number of papers have presented control architectures
The software development has been oriented according to the following guidelines: development process, certification norms, operating system tool, application tool and mathematical tool. They are detailed in the next subsections. 2.1 Development Process A development process can be defined as a sequence of working phases. Douglas (2007) proposes the Harmony
development process, which was adopted in this project. It is divided in two main phases: systems engineering and software engineering. In the systems engineering phase, system requirements are modeled and then the system architecture (hardware and software) is modeled and analyzed. From the system modeling, the software requirements are defined. In the software engineering phase, functional and temporary requirements of software are modeled (“analysis stage”), the software architecture is designed (“design stage”), code is generated from the model (“implementation stage”), and finally, unitary and integration tests are made (“testing stage”). 2.2 Norms for certification The norms attest that, when correctly operated, a piece of equipment is able to accomplish its function safely. However, norms do not exist for unmanned underwater vehicles and, in this context, adaptations of aeronautical norms (DO178B, 1992) that are already applied in unmanned aerial vehicles (UAVs) will be adopted. The norm DO178B is applied to any software that runs in a CPU, including: board support packages, operating systems, drivers, application software and mathematical calculations. The norm basically defines many orientations for the software development and require many documents for prove norm compliance. 2.3 BSP, RTOS and Drivers Tools An operating system is responsible for executing applications, and for the management of hardware resources. For supporting these tasks, tools such as board support packages (BSP) and drivers are included. The QNX Neutrino was chosen as BSP, RTOS and Drivers Tool. It has been recommended in comparative analyses by Melanson (2003) and Dedicated System Experts (2006), among several RTOS. Moreover, QNX has for application development, the Integrated Development Environment (IDE) QNX Momentics that provides communication between the development and embedded computers, which remotely allows debugging, system profiling, memory analysis, application profile, and code coverage. 2.4 Application Tools To select the application tool, programming language and specification method should be defined previously. Programming Language According to HighRely (2007) the most efficient programming languages for DO178B certification are C, C++ and ADA. To assist requirements of object oriented paradigm, language diffusion and resources availability, C++ should be used. Specification Method The development of critical or Hard RealTime systems demands their specification. During the systems engineering phase, the SysML was adopted for specifying, analyzing, designing, and verifying complex systems that may include hardware, software, information, personnel, procedures, and facilities. During the software engineering phase the UML was
adopted. UML is an object oriented modeling language for general purposes. The support to realtime systems development is provided by the UMLRT derivation. Considering the adoption of the Agent paradigm in the next sections, the AUMLRT was adopted as a software specification method. Application Tool Among tools that implement SysML, AUMLRT, DO178B and integrate into QNX Neutrino and Momentics, Telelogic Rhapsody and Rational Rose/RT are very popular in the software developers community. Between those tools, according to Bichler et al. (2002), only Rhapsody fulfills all requirements. Rhapsody is a development tool driven by requirements and models whose focus is automation, because it creates executable systems from UML models. Automation is provided by the code generation from UML models through a realtime framework, called OXF. Rhapsody enables certification and produces DO178B certifiable code because the ability to capture requirements, design, implementation, test and documentation all within Rhapsody enables the user to follow a well defined, repeatable and requirements driven process within Rhapsody. A welldefined and repeatable process is the key to DO178B certification. 2.5 Mathematical Tools The AUV control architecture includes math code for implementing functions such as navigation, path planning, motion control, etc. A critical point in the embedded system development is the correctness of the math code. A proposal to solve this problem is the use of automatic code generation. Several numerical tools are commercially available. However, only Matlab together with Simulink and RealTime Workshop fulfills all requirements. 3. SOFTWARE REQUIREMENTS DEFINITION The definition of software requirements was composed in three stages. At first, the system requirements are defined, in other words, requirements that AUV (composed by platform, hardware and software) imposes to system (composed by hardware and software). At the next stage, the embedded system modeling is carried out. These first two stages correspond to systems engineering phase. At the third stage, the navigation software requirements are filtered and refined. 3.1 Embedded System Requirements Analysis For determining functional and temporal system requirements, the external elements that interact with the system should be considered (the “actors”, according to the UML nomenclature). For Pirajuba those actors are composed by: the pilot, the GPS and, the AUV platform. The major functional requirements considered are: the embedded system should estimate platform states (speed, position and attitude); at the sea surface, the platform states should be sent to the base station; at the sea surface, the pilot should be able to control
platform remotely; at the sea surface, the embedded system should capture GPS signal and receive Differential GPS (DGPS) correction data. The temporal requirements were defined according to the AUV platform closed loop dynamics. The estimated settling time lies between 1 and 5 seconds. Taking this interval into account, and considering the digital signal reconstruction, the sampling frequency at 10Hz was chosen. 3.2 System Analysis The system model is composed by block diagrams that represent relationships among elements that compose the system structure (Fig. 2), which was defined according to the SysML modeling language. Embedded 1
IMU
1
CPS
1
PC104
imu
rs232
1
rs232 1
Switch
net1 net2
cps
net3
GPS ant
rs232
gps
net
dataLink net
gsp 1 1
Servo
1
pwm
ActuatorModule
servo
DataLink ant
net
Fig. 2. Embedded Internal Block Diagram. Acquiring Sampling
tm(4) Idle
ttm(1) m(1)
Processing
Transfering tm(4.7)/ OUT_PORT(rs232)>GEN(msgIMU());
tm(7) DataProcessed
Transfering2Buffer tm(1)
RS232BufferNull
RS232BufferFull
Fig. 3. IMU Behavioral Model. System behavior is modeled by sequence diagrams and statecharts. The modeling of Inertial Measurement Unit (IMU), Compass (CPS) and GPS sensors as well as switches, datalinks and joystick, follow specifications of concurrence, time, and communication defined by manufacturers. For example, the behavioral model of IMU is shown in figure 3. The behavioral models of embedded computer and base station computer were also implemented, in order to predict how modules will interact. 3.3 Navigation Software Requirements Analysis The main functional requirements of the embedded navigation software are: the embedded software should read IMU, GPS and CPS sensors; the embedded software should provide sensor fusion to generate estimation of speed, position and attitude, the embedded software should send the state estimation to the base station when the AUV is at surface;
the embedded software should receive DGPS correction sign when the AUV is at surface. After the embedded system simulation, the operation period at 100ms was defined as the temporal requirement. 4. SOFTWARE ARCHITECTURE The software architecture establishes the relationship structure among several software modules. It is considered indispensable that software architecture should satisfy the following characteristics: deliberation and reactivity; safety and reliability (previsibility, fault tolerance, and error detection and recovery), generality and extensibility. In the Pirajuba project, a new architecture in development at LVNT is adopted, which is called GESAM (Generic and Extensible Software Architecture for Mobots). Due to its generality and extensibility, the architecture was preliminarily developed for UAVs, and nowadays it is extended to AUVs. In order to combine deliberation and reactivity in GESAM, the Agents paradigm was adopted. They are implemented by objects. The intrinsic characteristic in objects is encapsulation (attributes and methods) and the addition of behavior, interface and concurrence allow the objects to act as separate and autonomous machines, the so called Software Agents (Douglass, 1999). In order to achieve safety and reliability, three different design patterns were combined: the Homogeneous Redundancy Pattern (HRP), the Diverse Redundancy Pattern (DRP) and the Safety Executive Pattern (SEP) (Douglass, 1999). The HRP uses identical channels to increase reliability. A channel, in this context, means a set of devices including hardware (processors, sensors, actuators) and software that handle a related, cohesive set of data or control flows from incoming event to ultimate system response. In HRP, all redundant channels are running in parallel, and the outputs of channels are compared. If an odd number of channels is used, then a “majority wins” policy can be implemented that can detect and correct failures in minority channels. The advantage of this pattern is improved robustness in the presence of failures in the channels, without a large development overhead. Because all the channels are identical, they need only be “cloned” rather than developed. A failure is an event that happens at particular point in time. This is distinct from an error which is a systematic fault. The big disadvantage of HRP is that it detects only failures and not errors because all channels are identical and then if one channel contains an error, all other redundant channels will contain the same error. This implies that HRP protects only against hardware failures but not against software faults. The DRP mitigates de primary disadvantages of HRP by providing redundant channels that are implemented by different means. The channels are all approximately equal in terms of accuracy, throughput, and reliability. Each channel is implemented differently, using different software and hardware components, providing additional protections against errors, as well as failures. The SEP uses a centralized coordinator for safety monitoring and control of system recovery from faults. The SEP acts like a smart watchdog that
tracks and coordinates all safety monitoring. The basic characteristic of GESAM is the abstraction layers structure (Fig. 4). Higher level layers have smaller number of agents with larger abstraction. The system layer implementation creates a base for the whole system, so that the software is expanded from this layer. In figure 5, the system layer implementation can be observed.
behavior extends to all agents. «Layer»
management
CPS 1
«Agent»
management
CPSManager
Fail Safe Processing Channel
dev iceX
dev iceY
Watchdog
System
SEP 1..*
«Agent»
1
management
«Agent»
TCM2_50
management
TCM3
Type Category
DRP HRP
Subsystem
Device
Fig. 6. Safety and Reliability of GESAM.
5. EKF AGENT
Fig. 4. Layer representation of GESAM.
The Extended Kalman Filter (EKF) algorithm (Jang, 2006; Brown, 1997) was implemented in the EKF agent at the GESAM architecture. The model can be represented by dynamics (2) and measurement (3): process Manager (2) autopilot actuator observer communicator payload xɺ (t) = f (x, u, t) + w(t) (3) z = h(x,t) + v(t) Where f and h are known functions, x is state vector, u is 1 «Layer» management 1 «Layer» management Payload AutoPilot control vector, z is measurements vector, w and v are actuator observer Gaussian white noises with null mean. observer communicator communicator actuator The propagation cycle is composed by the state propagation equation (4) and the covariance propagation (5): «Layer» management 1 «Layer» management 1 «Layer» managem ent (4) xk+1 = xˆ k + f (x,u,t).T Obs erver Com m unicator Actuator payload T autopilot (5) payload autopilot Pk+1 = Ak .Pˆk Ak + Qk autopilot System
1
«Layer»
1
communicator
observer
payload
Fig. 5. System Layer of GESAM.
where: Ak = I + ∂f (xk ,u k ,t k ).Δt ∂x
Qk = E(wk , wk ) .
(7)
T
“Category”, “Type” and “Device” are flexible layers. However, they should respect a design pattern that abstracts the related composites of the immediately lower level layer. Basically, the current composite “X” is in a defined layer and has an agent “XManager”, which is responsible for abstracting the correspondent lower level layer composites that are inside composite “X”. As an example, it is possible to mention the component CPS (Fig. 6), which is in the Type layer. TCM250 and TCM3 agents belong to CPS in the lower level layer “Device” (they are inside CPS because CPS is more abstract than TCM250 and TCM3). The XManager CPSManager abstracts from TCM250 and TCM3 to CPS. This abstraction includes data structure (from specific data as TCM250 to more general data as CPS), error detection and treatment, etc. Figure 6 also shows a safety and reliability representation of the abstraction pattern proposed. The agent TCM250 can have n redundancy showing the HRP. The agent TCM250 working together with agent TCM3 is example of DRP. Finally, the agents TCM250, TCM3, and CPSManager form an example of SEP. FailSafe processing of SEP is executed by XManager agent standard behavior. This standard
(6)
The updating cycle is composed by the gain (8), the state mean correction (9) and the covariance correction (10). −1 T T (8) K k = Pk .H k H k .Pk .H k + Rk
[
[
]
]
xˆ k = xˆ k + K k ⋅ z k − H k ⋅ xˆ k Pˆ = P − K ⋅ H ⋅ P k
k
k
k
(9) (10)
k
where: H k = ∂h (xk ,t k ) ∂x T Rk = E(vk ,v k ) .
(11) (12)
The navigation model developed includes the states vector: (13). x = [λ φ g ba x
h VN VE VD
ba y
ba z
bg x
bg y
a b c d bg z ]
(13)
Where: [λ φ h] are latitude, longitude and altitude, [VN VE VD ] is NED platform speed, [a b c d ] are quaternion elements, g is NED gravity acceleration, [ba ba ba ] are accelerometers bias in platform system, x
y
z
gx
bg y
bg z
[b
] are gyroscope bias in platform system.
The model includes the control vector (14): u = [a x a y a z ω x ω y ω z ]
system functionality not its processing performance. In figure 7, the main agents (IMU, CPS, GPS and DataLink) and the (14) sequence of the correspondent events can be observed. The other agents are omitted in ENV. The IMU is operating at Where [a x a y az ] and [ω x ω y ω z ] are platform accelerations 85Hz, CPS, GPS and DataLink at 10Hz. System operation and rotation rate, respectively. correctness can be verified by observing all expected The model includes the process dynamic equation (15): messages represented in figure 7. f (x,u,t) =
ENV
VN (
Rλ + h )
VE (Rφ + h).cos(λ ) − VD 2 VE .sin (λ ) VN .VD − + − 2.V .Ω.sin (λ ) + aN − ba N ( Rφ + h ) .cos(λ ) ( Rλ + h) D VE .VN .sin (λ ) + VE .VD + 2.Ω.(V .sin (λ ) + V .cos(λ )) + a − b N D E aE (Rφ + h ) .cos (λ ) ( Rλ + h )
2 2
VN V − − 2.VE .Ω.cos(λ ) + g + aD − ba D − E ( Rφ + h ) ( Rλ + h )
− b − c − d ωx − bg x 1 a −d c . ω y − bg y a − b 2 d ωz − bg z a − c b 0 0 0 0 0 0 0
[
]
[
[
' (15)
]
]
ENV
Avi on i cs .Observer.S enso r.IMU. VG600AA
IMU
Avi on i cs .Observer. Senso r.CPS .TCM2_ 50
CPS
Avi on i cs . Observer. Sensor . GP S. Mi LLenn i um
GPS
Avi on i cs . Communi c at or. Bas eSt a t ion. Dat aLi nk. Xt end
DataLink
e vU pd ate Da ta () e vU pd ate Da ta ()
e vU pd ate Da ta () e vU pd ate Da ta () e vU pd ate Da ta () e vU pd ate Da ta ()
e vU pd at e Da ta () e vU pd ate Da ta () e vU pd ate Da ta () e vU pd at eDa ta () evU pd at e Es ti m a t i ve ()
e vU pd ate Da ta ()
Fig. 7. Main events sequence. The next results were obtained through code debugging, in which the instrumented kernel sends information from the operating system and application to the IDE QNX Momentics. The embedded computer has a clock speed of 1.2GHz and memory space of 128Mbytes. A total processing consumption of 53.9% is verified, including 38.3% for the application, 8.7% for the system (including kernel instrumentation) and 6.9% for interruptions.
Where Rλ and Rφ are earth radius in latitude and longitude
axis, Ω is earth rotation and [a N − ba ] , [a E − ba ] e [a D − ba ] are obtained from (16). N
a N − ba N a E − ba E = a D − ba D
(
a2 + b2 − c2 − d 2 2.(b.c + a.d ) 2.(b.d − a.c )
E
D
(16)
)
(a
2.(b.c − a.d ) 2
− b2 + c2 − d 2 2.(c.d + a.b )
)
2.(b.d + a.c )
a x − ba x .a y − ba y 2 2 2 2 a − b − c + d a z − ba z
(
2.(c.d − a.b )
)
Fig. 8. Distribution of processing consumption.
In figure 8, the distribution of processing consumption is (17) presented related to twelve threads. They constitute the most demanding treads from the application that includes the own BN (18) application as well as drivers and interruptions used by −1 hCPS = [C pn ] BE application. The most demanding threads are: 1Extended BD Kalman Filter Thread; 2 IMU Serial Driver; 3IMU Serial Where [BN BE BD ] are earth magnetic fields in NED and Read Thread. In figure 9, interruptions are represented, as well as the C pn is the coordinate transformation matrix from platform to inter process communication that occur during 100ms of the operation period. In the first line, the CPS serial interruption NED. is observed at 9600bps and 10Hz; in the second line, the GPS 6. EXPERIMENTAL RESULTS serial interruption at 57600bps and 10Hz; and in the third At the present development stage, tests of the EKF line, the IMU serial interruption at 38400bps and 85Hz. In implementation focused on the computing performance the fourth line, the network interruption is observed that is shared between application and kernel instrumentation, instead of the quality of the estimations. The first result was obtained from model debugging, in presenting more interruptions than expected. Fifth to seventh which the instrumented embedded software sends messages lines respectively represent CPS and GPS serial driver; to Rhapsody. This test procedure is efficient to analyze network driver; and IMU serial driver. Finally, the Finally, the measurement equations are given by: hGPS = I 3x3 | 0 3x14 .x
[
]
application processing is represented by the last line. In the sixth line, two markers are observed that indicate 103.457ms. These indicate that a message was sent to the base station. As this is the lowest priority thread of the system, the compliance with 100ms period requirement was verified.
Fig. 9. Time line in one cycle. 6. CONCLUSIONS A design methodology for a critical realtime system was proposed, and applied to the navigation software of an AUV. The methodology was created based on areas related to the software development: development process, software certification norms, operating system tools, application tools, and mathematical tools. The Pirajuba navigation module was designed by integrating inertial data with GPS and CPS data, through an extended Kalman filter. The software implementation of this module was tested for the processing performance. In laboratory tests, it was verified that the system can satisfy the critical realtime requirements using 38.3% of processing resources. REFERENCES Amianti, G. (2008). Arquitetura de Software Aviônico de um VANT com Requisitos de Homologaçãos, 220p. Dissertação da Escola Politécnica, Universidade de São Paulo, São Paulo. Bichler, L., A. Radermacher and A. Schürr (2002). Evaluating UML Extensions for Modeling RealTime Systems. Proceedings of the Seventh International Workshop on ObjectOriented RealTime Dependable Systems, 7, 271278. Brown, R.G. and P.Y.C. Hwang (1997). Introduction to Random Signals and Applied Kalman Filtering, 3ºed. John Wiley & Sons, New York. Dedicated Systems Experts (2006). RTOS Evaluation Project.. DO178B (1992), Software Considerations in Airborne Systems and Equipment Certification. Radio Technical Comission for Aeronautics. Douglass, B.P. (1999). Doing Hard Time: Developing Real Time Systems with UML, Objects, Frameworks and Patterns, 749p. Addison Wesley, Boston. Douglass, B.P. (2007). RealTime UML Workshop for Embedded Systems, 408p. Newes, Oxford.
Duan, Q. and M. Zhang (2006). Research on realtime path planning method for the underwater robot in unknown environment with random shape obstacle. Proceedings of the 2006 IEEE International Conference on Mechatronics and Automation, 757761. El Jalaoui, A., D. Andreu and B. Jouvencel (2005). A control architecture for contextual tasks management: application to the AUV Taipan. Proceedings of IEEE Oceans 2007, 2, 752757. Hall, W.D. and M. B. Adams (1992). Autonomous vehicle software taxonomy. Proceedings of the 1992 Symposium on Autonomous Underwater Vehicle Technology, 4964. Highrely (2007). DO178B & DO254 Questions & Answers.. Jang, S. J. and D. Liccardo (2006). Automation of small UAVs using a low cost MEMS sensor and embedded computong platform. Proceedings of the Twentyfifth Digital Avionics Systems Conference, 25, 19. Kawano, H. (2006). Realtime obstacle avoidance for underactuated autonomous underwater vehicles in unknown vortex sea flow by the MDP approach. Proceedings of International Conference on Intelligent Robots and Systems, 30243031. Kim, T.W. and J. Yuh (2004). Development of a realtime control architecture for a semiautonomous underwater vehicle for intervention missions. Control Engineering Practice, 12(12), 15211530. Liu, T., and H. Schimidt (2004). A framework of realtime seabottom target detection using acoustic sensors on AUVs. Proceedings of IEEE Oceans 2004, 4, 2019 2024. Melanson, P. (2003). A Selection Methodology for the RTOS Market. Proceedings of Data Systems in Aerospace Conference. Ogata, K. (1995). Discretetime control systems, 2ºed. 745p. Prentice Hall, Englewood Cliffs. Santos, A., P. Rives, B. Espiau and D. Simon (1995). Dealing in realtime with a priori unknown environment on autonomous underwater vehicles. Proceedings of IEEE International Conference on Robotics and Automation, 2, 15791584. Sotzing, C.C., J. Evans and D.M. Lane (2007). A multiagent architecture to increase coordination efficiency in multi AUV operations. Proceedings of IEEE Oceans 2007, 18 21. Thakur, S. and J.M. Conrad (2007). An embedded Linux based navigation system for an autonomous underwater vehicle, Proceedings of IEEE Southeast Conference, 237242. Valavanis, K.P., D. Gracanin, M. Matijasevic, R. Kolluru, G.A. Demetriou (1997). Control architectures for autonomous underwater vehicles. IEEE Control Systems Magazine, 17(6), 4864. Yuan, H., J. Yang, Z. Qu and J. Kaloust (2006). An optimal realtime motion planner for vehicles with minimum turning radius. Proceedings of the sixth World Congress on Intelligent Control and Automation, 1, 397402.
*Escola Politécnica, São Paulo University, São Paulo, Brazil, (email: [email protected]) ** Escola Politécnica, São Paulo University, São Paulo, Brazil, (email: [email protected]) Abstract: This work describes the navigation software for the AUV Pirajuba. It is considered a critical realtime system and, therefore it should be developed according to a critical realtime methodology. The methodology proposed was based into five guidelines: development process; norms; operating system tools, application tools, and mathematical tools. The GESAM control architecture is presented for achieving generality, extensibility, safety, reliability, deliberation and reactivity. Inside GESAM, an Extended Kalman Filter was implemented for integrating information from Inertial Measurement Unit, GPS and Compass sensors. Performance tests showed that the system could fulfill requirements using 38.3% of processing consumption. Keywords: Architectures, Navigation Systems, Extended Kalman Filters, AUV, SafetyCritical, Real Time System, Methodology, Requirements Analysis, Model Driven Development, System Certification. 1. INTRODUCTION The Pirajuba Project has been executed at the Unmanned Vehicles Laboratory (LVNT), in the University of São Paulo. This project deals with the development of low cost Autonomous Underwater Vehicles (AUVs), emphasizing investigations on hydrodynamics and navigation. The Pirajuba (which means “yellow fish”, in the Tupi language) is an AUV which is 1.78m in length, and weighs 66kg in air (Fig. 1). The cruising speed and autonomy are 1m/s and 1h, respectively.
Fig. 1. Model of the AUV Pirajuba. The hybrid hardware architecture includes a central unit based on a PC104, and a number of distributed units based on microcontroller boards. The central unit performs the main functions during the mission such as sensor data acquisition, navigation, control, guidance, and mission management. The distributed units perform the control of actuators and the communication with the base station.
for AUVs. Overviews of the many approaches can be found in Hall and Adams (1992), Valavanis et al.(1997), and Kim and Yuh (2004). Recent proposals of control architectures are presented in Sotzing et al. (2007), El Jalaoui et al. (2005), Kim and Yuh (2004). Other works have claimed the realtime implementation of functions related to specific tasks such as navigation (Thakur and Conrad, 2007), path planning (Duan and Zhang, 2006; Kawano, 2006) and mapping (Liu and Schimidt, 2004). In all those works, only basic elements of a critical realtime system are presented, such as: hardware architecture, operating system, programming language, and software architecture. In order to consider the development of a critical realtime system, this work shows that a deeper approach should be taken into account. For that purpose a new methodology for the control architecture development is presented. In the next section, the methodology is presented. The software requirements that are analyzed specifically for the Pirajuba control architecture are presented in section 3. The software architecture is presented in section 4. The application for the AUV navigation module, which is based on the implementation of an Extended Kalman Filter, is presented in section 5. Laboratory results including sensor data acquisition and calculation in realtime are presented in section 6. Finally, section 7 includes the final comments on the proposed methodology and software architecture. 2. METHODOLOGY
This vehicle, as any other AUV, can be classified as a critical realtime system. Failures in performing the right instructions at the right time may jeopardize the mission tasks, or even endanger the vehicle and the surrounding environment. Consequently, the whole system, besides navigation software, should be developed according to a methodology of critical realtime systems that guarantees safety and reliability. A number of papers have presented control architectures
The software development has been oriented according to the following guidelines: development process, certification norms, operating system tool, application tool and mathematical tool. They are detailed in the next subsections. 2.1 Development Process A development process can be defined as a sequence of working phases. Douglas (2007) proposes the Harmony
development process, which was adopted in this project. It is divided in two main phases: systems engineering and software engineering. In the systems engineering phase, system requirements are modeled and then the system architecture (hardware and software) is modeled and analyzed. From the system modeling, the software requirements are defined. In the software engineering phase, functional and temporary requirements of software are modeled (“analysis stage”), the software architecture is designed (“design stage”), code is generated from the model (“implementation stage”), and finally, unitary and integration tests are made (“testing stage”). 2.2 Norms for certification The norms attest that, when correctly operated, a piece of equipment is able to accomplish its function safely. However, norms do not exist for unmanned underwater vehicles and, in this context, adaptations of aeronautical norms (DO178B, 1992) that are already applied in unmanned aerial vehicles (UAVs) will be adopted. The norm DO178B is applied to any software that runs in a CPU, including: board support packages, operating systems, drivers, application software and mathematical calculations. The norm basically defines many orientations for the software development and require many documents for prove norm compliance. 2.3 BSP, RTOS and Drivers Tools An operating system is responsible for executing applications, and for the management of hardware resources. For supporting these tasks, tools such as board support packages (BSP) and drivers are included. The QNX Neutrino was chosen as BSP, RTOS and Drivers Tool. It has been recommended in comparative analyses by Melanson (2003) and Dedicated System Experts (2006), among several RTOS. Moreover, QNX has for application development, the Integrated Development Environment (IDE) QNX Momentics that provides communication between the development and embedded computers, which remotely allows debugging, system profiling, memory analysis, application profile, and code coverage. 2.4 Application Tools To select the application tool, programming language and specification method should be defined previously. Programming Language According to HighRely (2007) the most efficient programming languages for DO178B certification are C, C++ and ADA. To assist requirements of object oriented paradigm, language diffusion and resources availability, C++ should be used. Specification Method The development of critical or Hard RealTime systems demands their specification. During the systems engineering phase, the SysML was adopted for specifying, analyzing, designing, and verifying complex systems that may include hardware, software, information, personnel, procedures, and facilities. During the software engineering phase the UML was
adopted. UML is an object oriented modeling language for general purposes. The support to realtime systems development is provided by the UMLRT derivation. Considering the adoption of the Agent paradigm in the next sections, the AUMLRT was adopted as a software specification method. Application Tool Among tools that implement SysML, AUMLRT, DO178B and integrate into QNX Neutrino and Momentics, Telelogic Rhapsody and Rational Rose/RT are very popular in the software developers community. Between those tools, according to Bichler et al. (2002), only Rhapsody fulfills all requirements. Rhapsody is a development tool driven by requirements and models whose focus is automation, because it creates executable systems from UML models. Automation is provided by the code generation from UML models through a realtime framework, called OXF. Rhapsody enables certification and produces DO178B certifiable code because the ability to capture requirements, design, implementation, test and documentation all within Rhapsody enables the user to follow a well defined, repeatable and requirements driven process within Rhapsody. A welldefined and repeatable process is the key to DO178B certification. 2.5 Mathematical Tools The AUV control architecture includes math code for implementing functions such as navigation, path planning, motion control, etc. A critical point in the embedded system development is the correctness of the math code. A proposal to solve this problem is the use of automatic code generation. Several numerical tools are commercially available. However, only Matlab together with Simulink and RealTime Workshop fulfills all requirements. 3. SOFTWARE REQUIREMENTS DEFINITION The definition of software requirements was composed in three stages. At first, the system requirements are defined, in other words, requirements that AUV (composed by platform, hardware and software) imposes to system (composed by hardware and software). At the next stage, the embedded system modeling is carried out. These first two stages correspond to systems engineering phase. At the third stage, the navigation software requirements are filtered and refined. 3.1 Embedded System Requirements Analysis For determining functional and temporal system requirements, the external elements that interact with the system should be considered (the “actors”, according to the UML nomenclature). For Pirajuba those actors are composed by: the pilot, the GPS and, the AUV platform. The major functional requirements considered are: the embedded system should estimate platform states (speed, position and attitude); at the sea surface, the platform states should be sent to the base station; at the sea surface, the pilot should be able to control
platform remotely; at the sea surface, the embedded system should capture GPS signal and receive Differential GPS (DGPS) correction data. The temporal requirements were defined according to the AUV platform closed loop dynamics. The estimated settling time lies between 1 and 5 seconds. Taking this interval into account, and considering the digital signal reconstruction, the sampling frequency at 10Hz was chosen. 3.2 System Analysis The system model is composed by block diagrams that represent relationships among elements that compose the system structure (Fig. 2), which was defined according to the SysML modeling language. Embedded 1
IMU
1
CPS
1
PC104
imu
rs232
1
rs232 1
Switch
net1 net2
cps
net3
GPS ant
rs232
gps
net
dataLink net
gsp 1 1
Servo
1
pwm
ActuatorModule
servo
DataLink ant
net
Fig. 2. Embedded Internal Block Diagram. Acquiring Sampling
tm(4) Idle
ttm(1) m(1)
Processing
Transfering tm(4.7)/ OUT_PORT(rs232)>GEN(msgIMU());
tm(7) DataProcessed
Transfering2Buffer tm(1)
RS232BufferNull
RS232BufferFull
Fig. 3. IMU Behavioral Model. System behavior is modeled by sequence diagrams and statecharts. The modeling of Inertial Measurement Unit (IMU), Compass (CPS) and GPS sensors as well as switches, datalinks and joystick, follow specifications of concurrence, time, and communication defined by manufacturers. For example, the behavioral model of IMU is shown in figure 3. The behavioral models of embedded computer and base station computer were also implemented, in order to predict how modules will interact. 3.3 Navigation Software Requirements Analysis The main functional requirements of the embedded navigation software are: the embedded software should read IMU, GPS and CPS sensors; the embedded software should provide sensor fusion to generate estimation of speed, position and attitude, the embedded software should send the state estimation to the base station when the AUV is at surface;
the embedded software should receive DGPS correction sign when the AUV is at surface. After the embedded system simulation, the operation period at 100ms was defined as the temporal requirement. 4. SOFTWARE ARCHITECTURE The software architecture establishes the relationship structure among several software modules. It is considered indispensable that software architecture should satisfy the following characteristics: deliberation and reactivity; safety and reliability (previsibility, fault tolerance, and error detection and recovery), generality and extensibility. In the Pirajuba project, a new architecture in development at LVNT is adopted, which is called GESAM (Generic and Extensible Software Architecture for Mobots). Due to its generality and extensibility, the architecture was preliminarily developed for UAVs, and nowadays it is extended to AUVs. In order to combine deliberation and reactivity in GESAM, the Agents paradigm was adopted. They are implemented by objects. The intrinsic characteristic in objects is encapsulation (attributes and methods) and the addition of behavior, interface and concurrence allow the objects to act as separate and autonomous machines, the so called Software Agents (Douglass, 1999). In order to achieve safety and reliability, three different design patterns were combined: the Homogeneous Redundancy Pattern (HRP), the Diverse Redundancy Pattern (DRP) and the Safety Executive Pattern (SEP) (Douglass, 1999). The HRP uses identical channels to increase reliability. A channel, in this context, means a set of devices including hardware (processors, sensors, actuators) and software that handle a related, cohesive set of data or control flows from incoming event to ultimate system response. In HRP, all redundant channels are running in parallel, and the outputs of channels are compared. If an odd number of channels is used, then a “majority wins” policy can be implemented that can detect and correct failures in minority channels. The advantage of this pattern is improved robustness in the presence of failures in the channels, without a large development overhead. Because all the channels are identical, they need only be “cloned” rather than developed. A failure is an event that happens at particular point in time. This is distinct from an error which is a systematic fault. The big disadvantage of HRP is that it detects only failures and not errors because all channels are identical and then if one channel contains an error, all other redundant channels will contain the same error. This implies that HRP protects only against hardware failures but not against software faults. The DRP mitigates de primary disadvantages of HRP by providing redundant channels that are implemented by different means. The channels are all approximately equal in terms of accuracy, throughput, and reliability. Each channel is implemented differently, using different software and hardware components, providing additional protections against errors, as well as failures. The SEP uses a centralized coordinator for safety monitoring and control of system recovery from faults. The SEP acts like a smart watchdog that
tracks and coordinates all safety monitoring. The basic characteristic of GESAM is the abstraction layers structure (Fig. 4). Higher level layers have smaller number of agents with larger abstraction. The system layer implementation creates a base for the whole system, so that the software is expanded from this layer. In figure 5, the system layer implementation can be observed.
behavior extends to all agents. «Layer»
management
CPS 1
«Agent»
management
CPSManager
Fail Safe Processing Channel
dev iceX
dev iceY
Watchdog
System
SEP 1..*
«Agent»
1
management
«Agent»
TCM2_50
management
TCM3
Type Category
DRP HRP
Subsystem
Device
Fig. 6. Safety and Reliability of GESAM.
5. EKF AGENT
Fig. 4. Layer representation of GESAM.
The Extended Kalman Filter (EKF) algorithm (Jang, 2006; Brown, 1997) was implemented in the EKF agent at the GESAM architecture. The model can be represented by dynamics (2) and measurement (3): process Manager (2) autopilot actuator observer communicator payload xɺ (t) = f (x, u, t) + w(t) (3) z = h(x,t) + v(t) Where f and h are known functions, x is state vector, u is 1 «Layer» management 1 «Layer» management Payload AutoPilot control vector, z is measurements vector, w and v are actuator observer Gaussian white noises with null mean. observer communicator communicator actuator The propagation cycle is composed by the state propagation equation (4) and the covariance propagation (5): «Layer» management 1 «Layer» management 1 «Layer» managem ent (4) xk+1 = xˆ k + f (x,u,t).T Obs erver Com m unicator Actuator payload T autopilot (5) payload autopilot Pk+1 = Ak .Pˆk Ak + Qk autopilot System
1
«Layer»
1
communicator
observer
payload
Fig. 5. System Layer of GESAM.
where: Ak = I + ∂f (xk ,u k ,t k ).Δt ∂x
Qk = E(wk , wk ) .
(7)
T
“Category”, “Type” and “Device” are flexible layers. However, they should respect a design pattern that abstracts the related composites of the immediately lower level layer. Basically, the current composite “X” is in a defined layer and has an agent “XManager”, which is responsible for abstracting the correspondent lower level layer composites that are inside composite “X”. As an example, it is possible to mention the component CPS (Fig. 6), which is in the Type layer. TCM250 and TCM3 agents belong to CPS in the lower level layer “Device” (they are inside CPS because CPS is more abstract than TCM250 and TCM3). The XManager CPSManager abstracts from TCM250 and TCM3 to CPS. This abstraction includes data structure (from specific data as TCM250 to more general data as CPS), error detection and treatment, etc. Figure 6 also shows a safety and reliability representation of the abstraction pattern proposed. The agent TCM250 can have n redundancy showing the HRP. The agent TCM250 working together with agent TCM3 is example of DRP. Finally, the agents TCM250, TCM3, and CPSManager form an example of SEP. FailSafe processing of SEP is executed by XManager agent standard behavior. This standard
(6)
The updating cycle is composed by the gain (8), the state mean correction (9) and the covariance correction (10). −1 T T (8) K k = Pk .H k H k .Pk .H k + Rk
[
[
]
]
xˆ k = xˆ k + K k ⋅ z k − H k ⋅ xˆ k Pˆ = P − K ⋅ H ⋅ P k
k
k
k
(9) (10)
k
where: H k = ∂h (xk ,t k ) ∂x T Rk = E(vk ,v k ) .
(11) (12)
The navigation model developed includes the states vector: (13). x = [λ φ g ba x
h VN VE VD
ba y
ba z
bg x
bg y
a b c d bg z ]
(13)
Where: [λ φ h] are latitude, longitude and altitude, [VN VE VD ] is NED platform speed, [a b c d ] are quaternion elements, g is NED gravity acceleration, [ba ba ba ] are accelerometers bias in platform system, x
y
z
gx
bg y
bg z
[b
] are gyroscope bias in platform system.
The model includes the control vector (14): u = [a x a y a z ω x ω y ω z ]
system functionality not its processing performance. In figure 7, the main agents (IMU, CPS, GPS and DataLink) and the (14) sequence of the correspondent events can be observed. The other agents are omitted in ENV. The IMU is operating at Where [a x a y az ] and [ω x ω y ω z ] are platform accelerations 85Hz, CPS, GPS and DataLink at 10Hz. System operation and rotation rate, respectively. correctness can be verified by observing all expected The model includes the process dynamic equation (15): messages represented in figure 7. f (x,u,t) =
ENV
VN (
Rλ + h )
VE (Rφ + h).cos(λ ) − VD 2 VE .sin (λ ) VN .VD − + − 2.V .Ω.sin (λ ) + aN − ba N ( Rφ + h ) .cos(λ ) ( Rλ + h) D VE .VN .sin (λ ) + VE .VD + 2.Ω.(V .sin (λ ) + V .cos(λ )) + a − b N D E aE (Rφ + h ) .cos (λ ) ( Rλ + h )
2 2
VN V − − 2.VE .Ω.cos(λ ) + g + aD − ba D − E ( Rφ + h ) ( Rλ + h )
− b − c − d ωx − bg x 1 a −d c . ω y − bg y a − b 2 d ωz − bg z a − c b 0 0 0 0 0 0 0
[
]
[
[
' (15)
]
]
ENV
Avi on i cs .Observer.S enso r.IMU. VG600AA
IMU
Avi on i cs .Observer. Senso r.CPS .TCM2_ 50
CPS
Avi on i cs . Observer. Sensor . GP S. Mi LLenn i um
GPS
Avi on i cs . Communi c at or. Bas eSt a t ion. Dat aLi nk. Xt end
DataLink
e vU pd ate Da ta () e vU pd ate Da ta ()
e vU pd ate Da ta () e vU pd ate Da ta () e vU pd ate Da ta () e vU pd ate Da ta ()
e vU pd at e Da ta () e vU pd ate Da ta () e vU pd ate Da ta () e vU pd at eDa ta () evU pd at e Es ti m a t i ve ()
e vU pd ate Da ta ()
Fig. 7. Main events sequence. The next results were obtained through code debugging, in which the instrumented kernel sends information from the operating system and application to the IDE QNX Momentics. The embedded computer has a clock speed of 1.2GHz and memory space of 128Mbytes. A total processing consumption of 53.9% is verified, including 38.3% for the application, 8.7% for the system (including kernel instrumentation) and 6.9% for interruptions.
Where Rλ and Rφ are earth radius in latitude and longitude
axis, Ω is earth rotation and [a N − ba ] , [a E − ba ] e [a D − ba ] are obtained from (16). N
a N − ba N a E − ba E = a D − ba D
(
a2 + b2 − c2 − d 2 2.(b.c + a.d ) 2.(b.d − a.c )
E
D
(16)
)
(a
2.(b.c − a.d ) 2
− b2 + c2 − d 2 2.(c.d + a.b )
)
2.(b.d + a.c )
a x − ba x .a y − ba y 2 2 2 2 a − b − c + d a z − ba z
(
2.(c.d − a.b )
)
Fig. 8. Distribution of processing consumption.
In figure 8, the distribution of processing consumption is (17) presented related to twelve threads. They constitute the most demanding treads from the application that includes the own BN (18) application as well as drivers and interruptions used by −1 hCPS = [C pn ] BE application. The most demanding threads are: 1Extended BD Kalman Filter Thread; 2 IMU Serial Driver; 3IMU Serial Where [BN BE BD ] are earth magnetic fields in NED and Read Thread. In figure 9, interruptions are represented, as well as the C pn is the coordinate transformation matrix from platform to inter process communication that occur during 100ms of the operation period. In the first line, the CPS serial interruption NED. is observed at 9600bps and 10Hz; in the second line, the GPS 6. EXPERIMENTAL RESULTS serial interruption at 57600bps and 10Hz; and in the third At the present development stage, tests of the EKF line, the IMU serial interruption at 38400bps and 85Hz. In implementation focused on the computing performance the fourth line, the network interruption is observed that is shared between application and kernel instrumentation, instead of the quality of the estimations. The first result was obtained from model debugging, in presenting more interruptions than expected. Fifth to seventh which the instrumented embedded software sends messages lines respectively represent CPS and GPS serial driver; to Rhapsody. This test procedure is efficient to analyze network driver; and IMU serial driver. Finally, the Finally, the measurement equations are given by: hGPS = I 3x3 | 0 3x14 .x
[
]
application processing is represented by the last line. In the sixth line, two markers are observed that indicate 103.457ms. These indicate that a message was sent to the base station. As this is the lowest priority thread of the system, the compliance with 100ms period requirement was verified.
Fig. 9. Time line in one cycle. 6. CONCLUSIONS A design methodology for a critical realtime system was proposed, and applied to the navigation software of an AUV. The methodology was created based on areas related to the software development: development process, software certification norms, operating system tools, application tools, and mathematical tools. The Pirajuba navigation module was designed by integrating inertial data with GPS and CPS data, through an extended Kalman filter. The software implementation of this module was tested for the processing performance. In laboratory tests, it was verified that the system can satisfy the critical realtime requirements using 38.3% of processing resources. REFERENCES Amianti, G. (2008). Arquitetura de Software Aviônico de um VANT com Requisitos de Homologaçãos, 220p. Dissertação da Escola Politécnica, Universidade de São Paulo, São Paulo. Bichler, L., A. Radermacher and A. Schürr (2002). Evaluating UML Extensions for Modeling RealTime Systems. Proceedings of the Seventh International Workshop on ObjectOriented RealTime Dependable Systems, 7, 271278. Brown, R.G. and P.Y.C. Hwang (1997). Introduction to Random Signals and Applied Kalman Filtering, 3ºed. John Wiley & Sons, New York. Dedicated Systems Experts (2006). RTOS Evaluation Project.
Duan, Q. and M. Zhang (2006). Research on realtime path planning method for the underwater robot in unknown environment with random shape obstacle. Proceedings of the 2006 IEEE International Conference on Mechatronics and Automation, 757761. El Jalaoui, A., D. Andreu and B. Jouvencel (2005). A control architecture for contextual tasks management: application to the AUV Taipan. Proceedings of IEEE Oceans 2007, 2, 752757. Hall, W.D. and M. B. Adams (1992). Autonomous vehicle software taxonomy. Proceedings of the 1992 Symposium on Autonomous Underwater Vehicle Technology, 4964. Highrely (2007). DO178B & DO254 Questions & Answers.