NEW GENERATION ROBOTIC CONTROLLER SOFTWARE FOR SPACE MANIPULATORS

Size: px

Start display at page:

Download "NEW GENERATION ROBOTIC CONTROLLER SOFTWARE FOR SPACE MANIPULATORS"

Oliver Chapman
5 years ago
Views:

In Proceedings of the 8th ESA Worksho on Advanced Sace echnologies for Robotics and Automation 'ASRA 2004' ESEC, Noordwijk, he Netherlands, November 2-4, 2004 NEW GENERAION ROBOIC CONROLLER SOFWARE

1 In Proceedings of the 8th ESA Worksho on Advanced Sace echnologies for Robotics and Automation 'ASRA 2004' ESEC, Noordwijk, he Netherlands, November 2-4, 2004 NEW GENERAION ROBOIC CONROLLER SOFWARE FOR SPACE MANIPULAORS N. Cikic (2), E. De Marchi (2), M. Filiini (2), F. Fusco (3),. Grasso (2), A.. Olivieri (1), A. Rusconi (3) (1) ASI Contrada erlecchia, 75100, Matera, IALY (2) ecnomare SA S. Marco 3584, Venezia, IALY (3) Galileo Avionica via Montefeltro 8, Milano, IALY Introduction EUROPA (External Use of Robotics for Payload Automation) is a robotic exeriment to be flown on the International Sace Station (ISS) and is intended to erform a realistic end-to-end robotic technology demonstration in exosed environment. It is mainly comosed by: a taskboard, where significant robotic exeriments are housed a robotic arm, based on the existing ASI 7 dof SPIDER arm, used to erform the robotic exeriments a robot controller, to control arm motion and to erform safety checks data handling and ower unit (DHPU), taking care of interfacing with ISS and roviding ower to all the equiments hold down mechanism, to latch arm during launch and re -entry hases 1

2 he assigned volume for all the equiment is a workcell whose dimensions are about 1m x 1m x 1m (see figure above). his means that, considering arm dimensions, the arm shall be rovided with secific caabilities for an effective motion without volume rotrusions while erforming oerations in both free and contact modes. he EUROPA robot controller rovides the necessary comuting ower to include sohisticated control algorithms and reach a high degree of autonomy for the on-board oerations. It is comosed by a VME crate where the following boards are housed: Main CPU board, based on MPC750 rocessor where control software runs Four Driver boards, roviding ower to the arm motors and motor shaft resolvers acquisition two microcontroller boards, based on 8086 rocessor, used for both outut shaft resolvers acquisition and indeendent safety checks, as from Comuter Based Control System (CBCS) aroach ([6]) one I/O board, used for digital I/O and Force/orque Sensor acquisition. Digital I/O are mainly connected with the hold down resence sensors one Power suly board he Controller is at an advanced imlementation stage and has assed the Safety Review 0/1 with NASA. he roosed aer will focus on the innovative features of the SW Robotic Controller. he art related to Robot Control is structured according a NASREM -like architecture, and imlements the following main items: Execution of Robotic Programs written in Euroa Programming Language (EPL) Resolution of arm redundancy based on arm configuration angle, seen as the 7 th cartesian degree of freedom Position/Force control at the end effector, allowing recise interaction with the environment Possibility to move the arm in teleoeration mode, in joint or cartesian sace Possibility to customize software behaviour, by means of some configuration and database files. hey allow oerator to modify control gains, check thresholds, command arameter limits, timing and so on. he art of the software not related to Robot Control is mainly devoted to communication with ISS through DHPU, and to safety checks, according to the CBCS aroach. Arm redundancy resolution EUROPA arm has seven degrees of freedom and is kinematically redundant, since six joints are sufficient for generic endeffector osition and orientation. In recent years many techniques to solve the roblem have been roosed; for the most art, they focus on local otimization to achieve EE (end effector) trajectory while otimizing additional criteria like obstacle avoidance, singularity avoidance, motor torque minimization, joint limits avoidance. See, for examle, [1], [2], [3]. he use, for examle, of the seudoinverse (or other local minimization) control results, in general, in the loss of cyclicity of maniulator configuration as the end effector cycles through a eriodic motion. his means that as the EE is constrained to move along a well secified ath, the arm may exerience unredictable self-motion wanderings, and this is unaccetable in EUROPA alication, where all the tasks that the maniulator has to accomlish will be lanned and tested in the ground reference model and then executed in orbit, execting about the same behaviour tested on ground. he aroach used to solve the roblem is resented in the following figure 1, that shows the EUROPA arm self motion; note that the elbow oint E make exactly a circle around the line connecting the shoulder oint S and the wrist oint W that are fixed. Analyzing the arm self motion, a natural way to solve redundancy is to add, as additional task the arm has to accomlish, the control of the elbow osition E along the circle traveled during self motion. his is obtained introducing a new kinematic equation to directly control the arm angle (see [4]), defined as the angle from a reference lane containing a reference unit vector (with origin in S) and the shoulder-wrist line SW to the shoulder-elbowwrist lane SEW in the right hand sense about the vector w = W-S. Selecting as reference vector the joint 1 axis, the arm angle ψ is defined by the following equation tanψ = wˆ Vˆ, Vˆ (1) where Vˆ is the reference unit vector, w w = w ˆ, [ ( ) ] ˆ =, = ( I wˆ wˆ ) e he arm angle jacobian can be also analytically comuted, and is given by: J ψ = 3 and e = E-S. ( ) wˆ ˆ Vˆ w wˆ e E ( wˆ lˆ ) ( wˆ ˆ ) W l w, (2) 2

3 where l l = l ˆ ( ), l w V w Fig 1: EUROPA arm self-motion. = ˆ, and E and W are the Jacobians that relate joint rates to elbow E and wrist W linear velocit ies. he use of the arm angle as extra kinematic equation induces a control algorithm that have the roerty of cyclicity; furthermore it allows a recise elbow oint control (the widely moving oint during self motion), that has turned out to be a fundamental feature to avoid workcell rotrusions during arm oerations. EUROPA robotic controller SW Robotic controller software has been develoed adoting a hierarchical architecture insired to NASREM. Four hierarchic levels have been imlemented: ask, EMove, Primitive and Servo; their main tasks are described in the following. ask Level Controller his is the highest level controller, and its main function is to imlement a virtual machine able to execute byte code roduced by EPL language comiler and linker. When a call to a function that requires arm or grier motion is rocessed, the elementary moves necessaries to execute the motion are generated and assed (one by one) to the underlying EMove level controller. Synchronization with EMove level is achieved through feedback that are received each time an elementary move has been executed. his level is not synchronized: the next byte code will be executed, if resent, once the current is finished. EMove Level Controller he main function of EMove level controller is to rocess elementary moves coming from ask level. When an elementary move is received, a set of motion rimitives is generated for both arm and grier subsystem and sent (all together) to the underlying Primitive level controller for execution. Primitives are mainly comosed of five order olynomials describing the desired behaviour of the arm and grier. Usually three rimitives are generated for each subsystem: one for the acceleration hase, one for constant seed hase and one for deceleration hase. Primitives are generated in such a way to assure a coordinated motion with continuity of trajectories, and of their first and second derivatives. Synchronization with Primitive level is obtained through feedbacks: the execution of an elementary move is finished when all generated rimitives have been executed from Primitive level controller. 3

4 Like ask level, also EMove level is not synchronized: the next elementary move will be executed, if resent, when the current is finished. Primitive Level Controller Primitive level controller is mainly comosed of two modules: one dedicated to execution of rimitives coming from EMove level, and one dedicate to execution of teleoeration inuts. hey are mutually exclusive: when a robotic rogram is running (default mode), no teleoeration inuts are acceted, and vice versa, when teleoeration mode is enabled, no robotic rograms can be executed. eleoeration mode: Arm can be teleoerated either in joint mode or cartesian mode. In joint mode joint seeds are acceted as teleoeration inut; in cartesian mode the oerator secify the tool linear and angular seeds, the arm angle seed, as well as the tool frame and the reference frame. Grier can be teleoerated only secifying its seed. Deending on teleoeration tye, suitable servo setoint are generated to assure the desired arm behaviour and sent to Servo level for execution. Robotic Mode: In default mode, Primitive level executes rimitives coming from EMove level samling trajectory secification olynomials at a fixed rate. Samles are used to comose servo setoint that are sent to Servo level for execution. When the execution of a rimitive finishes, the execution of the next one, if any, starts; if no more rimitives are in queue, t he last servo setoint is reeated. In both teleoeration and default mode Primitive level controller handles controlled sto requests stoing the arm and the grier in a controlled manner; this is done generating suitable rimitives that alies a smooth deceleration rofile while remaining on the same ath the arm is treading. Synchronization with servo level is assured by feedback received each time a servo has been executed. As Servo level sends feedback at a recise rate by the use of a real time clock, Primitive level controller results synchronized. Servo Level Controller he main servo level controller function is to generate, at a fixed rate, the current setoints for the motor driver boards controlling the eight motors of the arm and grier. Inuts for the control loo are obtained through linear interolation of servo setoints coming from Primitive level controller. Grier can be driven in two modes: osition mode and current mode. In current mode the current to be rovided to the grier motor is directly secified by the setoint; in osition mode a PID controller is used to comute current from osition setoint. Arm can be also driven in two modes: joint mode and cartesian mode. In joint mode, arm motor currents are comuted through seven PID controllers each one controlling one joint; in cartesian mode a cartesian hybrid osition/force control loo, deicted in Fig. 2 is used. his control scheme has been derived from [5]. As it can be seen in Fig. 2 the inuts of the control loo are the desired force Fd, the desired osition Pd and the desired arm angle aa d. he task secification matrices Ω and Ω ~, are built u starting from user secification of force and osition controlled direction in a suitable task frame. Position is, of course, not controlled along force controlled axis, as well as force is not controlled along osition controlled axis. L 0 (q) is the inertia matrix in the oerational sace and allows, with Coriolis, centrifugal and gravity term µ, the dynamic decouling, in such a way that the tool becomes equivalent to a single unit mass. he transose jacobian J 0 (q), built u from base jacobian, and arm angle jacobian, is used to convert generalized forces from oerational sace to joint sace. Note that through L 0 large variations of inertia to be controlled at the end effector, e.g. due to grabbing of large ayloads, can be comensated, imroving the dynamic erformances of the system. Finally, no exlicit inverse kinematics is foreseen in the loo. 4

5 F - F d Force PID F ff Ω P d P - Ṗ Position PID Ω Γ L 0 (q) J 0 (q) ARM F s q. q Force, Position, Seed comutation F P. P aa. aa µ(q,q). aa d aa - Arm Angle PID aȧ Fig. 2: Arm cartesian hybrid osition/force control scheme. Servo level controller also handles emergency sto requests stoing immediately the arm and the grier disabling their motor drivers and engaging brakes. Servo level is strictly synchronized by a real time clock running at a frequency of 1000 Hz. EUROPA rogramming language (EPL) Robotic rograms are, in EUROPA roject, the main way to move the arm and the grier (the other one is the teleoeration mode). hey are written using EPL language, a versatile novel robotic language that allows oerator to erform arm motion, controller status management and flexible data rocessing. EPL is a rogramming language, directly derived from PDL2, already used in revious rojects, with secific features for rogramming EUROPA robotic alications. A rogram is divided in three sections: rototye section, where the rogram rototye is secified defining its name, inut arguments and returned value declaration section, where variables, constants and functions used in the rogram shall be declared statement section, where the body of the rogram is written. Basic data tyes are REAL to reresent real values, INEGER for integer values, BOOLEAN for boolean data and CHAR for characters. Furthermore ARRAY (u to two dimensions) of the basic data tyes can be defined. All common logical (AND, OR, NO, XOR), arithmetic (, -, *, /, %), relational (<, <=, =, >, >=, <>), assignment (:=) and recedence (()) oerators are available. Program flow can be controlled using the IF HEN ELSE ENDIF statement to choose between two ossible flows of rogram deending on a boolean exression, or the WHILE DO ENDWHILE statement to execute a sequence of statements as long as a boolean exression is true. Previously declared functions can be called (also recursively) assing them arameters either by value or by reference. EPL language is rovided with a set of built-in functions that make it suitable for writing robotic alication. hey are divided in several grous: Mathematical, that allow the oerator to execute oerations such as ABS, SIN, COS, SQR, LN, ROUND, POW, etc. String Maniulation, that allow the oerator to deal with string; examles are: ENCODE_REAL, DECODE_INEGER, SRCA, SRCPY, SRCMP, etc. Motion Control, that enable the oerator to move the arm and the grier in several ways calling function like MOVE_RELAIVE, MOVE_JOINS, CHANGE_GRIPPER_WIDH, etc. For examle, to move the arm in contact, 5

6 the oerator can use the MOVE_IN_CONAC built -in function secifying the tool frame (wrt a reference frame), the cartesian seed, the desired forces, the directions to control in force and in osition, the relevant tool, reference frames for the contact oerations, and a termination condition for the force action (time, a linear/angular dislacement, a force/torque threshold or an external event). Data Acquisition, to allow the oerator to acquire (u to 1000 Hz frequency) and store a selectable set of robotic data (like joints osition, seeds, cartesian data, etc.), or data coming from external devices such as distance sensors or vision systems. Controller status, to retrieve the current status of the arm and use it in the rogram; examles are GE_ARM_JOIN_POSIION, GE_ARM_JOIN_SPEED, GE_GRIPPER_CURREN, GE_FORCE, etc. Database Management built-in functions, to retrieve and udate data from internal databases; examles are GE_SANDBY_CONFIG, SE_SAFEY_CONAC_SAE, etc. I/O Management: to write/read to/from digital I/O channels: GE_D_IO, SE_D_IO Other function like rinting functions, rogram control functions (DELAY, BREAK). EPL rograms are comiled and linked to roduce a byte-code that can be sent to task level controller virtual machine for interretation. his can be done using the EPLc and EPLlink rograms: a comiler and a linker exressly develoed for the EPL language. ests he EUROPA arm controller software architecture and coding have been develoed using Rational Rose Realime CASE tool and WindRiver ornado II running on WIN32 host latform. he target board is an AIECH (M) S210 board rovided with a 233MHz rocessor, 64Mb RAM, 64Mb File flash, 8 Mb User flash, 2 Mb boot flash, 2 RS422 serial lines, 1 ethernet controller. arget oerating system is VxWorks (M) 5.4. Some reliminary tests to verify target board comutational load have been carried out with the aid of a dynamic simulator of the arm. his simulator runs on a win32 system, and is connected with the target via ethernet CP/IP rotocol. able 1 shows the results of a test erformed while a robotic rogram executing arm cartesian motion is running. Due to heavy comutational load of dynamic simulator of the arm the test has been carried out in simulated time (1 simulated s 2 s); actual load column reresent the real target CPU load detected through vxworks sy functions, while the third and forth columns show the exected CPU load when the servo loo is triggered resectively with a 500 Hz and 1000 Hz real time clock. able 1: CPU exected loads Modules Actual Load Exected Exected Robotic control modules 8.8% 19.4% 38.8% Safety modules 2.3% 2.9% 4.0% Communications modules 0.6% 0.6% 0.6% Other modules 1.1% 1.1% 1.1% ot 12.9% 24.0% 44.5% As it can be seen, the exected medium CPU load is lower than 50% with servo loo running at 1000 Hz. Of course great art of the CPU ower is used from robotic control modules to close cartesian servo loo. Configuration databases EUROPA arm controller is fully configurable through a set of ASCII databases that can be udated from the oerator. hey are mainly divided in three categories: robotic controller databases, safety databases, environment databases. Robotic Controller Databases: they contains all the arameters related to robotic system controller; examles of contained arameters are: Denhavit-Hartenberg kinematic arameters of the arm, arm joint and grier offsets, arm joint and grier limits, resolver looku tables, transformation matrices (world-base, ee-grier, etc.), PIDs gains, dynamic arameters of the arm and motors (masses, inertias, etc.), servo and rimitive samle eriods, etc. Safety Databases: they contains all the arameters related to safety module; examle of such arameters are: seed thresholds, current thresholds, resolvers cross check thresholds, distance thresholds between arm and environment, encumbrance of the arm links (in term of cylinders), encumbrance of the environment (in terms of cylinders, lanes, 6

7 quadrilaterals and sheres), collision airs (airs of object which distance is comuted to check if a collision is imending), etc. Environment databases: they contains the descrition of the environment the arm has to interact with, in terms of objects osition, actions that can be executed on each object, and motion arameters that shall be used to execute each allowed action. Such motion arameters can be retrieved from robotic rograms using suitable built-in functions. Safety module In the EUROPA mission, the Comuter Based Control System (CBCS, as from [6]) has been adoted to control critical hazards such as enveloe rotrusion, collision with internal objects, hold down wrong ascertainment. In order to control these hazards,. he following tyes of checks have been imlemented in three indeendent CPUs, allowing for indeendent aths of intervention: Worksace: check that arm arts do not rotrude the assigned enveloe Imact energy: check that the seed and current of each joint do not overcross given thresholds Sensor reliability: check that the resolver readings for each joint (eg motor, coarse outut shaft and fine outut shaft resolvers) are coherent CPU cross monitoring: check that the other boards are correctly running hreshold cross check: check that the safety thresholds, stored in 3 different locations, are not corruted by SEU - SBHU For every communication with the safety module (i.e. customizations of check thresholds, reading of hold down status), a suitable Safe Protocol has been develoed. his rotocol (between EUROPA Control Station the only initiator- and destination comuter) allows safe and reliable communications against ossible software or timing errors that might cause inadvertent safety module modification, leading to hazards. Safe Protocol is based on an encoding function and a safety key, owned only by EUROPA Control Station and the destination comuter. Each time a message is sent by EUROPA Control Station, suitable data encoding is done and the destination is able to unders tand if the initiator is Ground itself or an intermediate comuter (due to errors). Conclusion he innovative feature of the resented SW controller for sace robotic arms derives from its being secifically develoed for telerobotic alications in sace. As such, it addresses all the secific issues deriving from this scenario: rogrammability from remote, with different levels of interaction with the human oerator, including teleoeration full re -configurability, including udating of kinematic arameters after arm recalibration rovision for a general set of maniulation motion/force rimitives, with exlicit force control management of arm redundancy management of safety according to NASA requirements. he controller is going to be integrated with the Ground Reference Model of the Euroa robotic arm. References [1] A. Liégeois, Automatic suervisory control of the configuration and behaviour of multibody mechanisms, in IEEE rans. Syst., Man., Cyber., vol. SMC-7, no. 12, , 1977 [2] J. Baillieul, Kinematic rogramming alternatives for redundant maniulators, in Proc. IEEE Int. Conf. Robotics Automat., St. Louis, Mar. 1985, [3] Y. Nakamura and H. Hanafusa, ask riority based redundancy control of robot maniulators, in Proc. 2 nd Int. Sym. on Robotics Research, Kyoto, Jaan, Aug [4] H. Seraji, Configuration Control of Redundant Maniulators: heory and Imlementation, in IEEE rans. on Robotics and Automat., vol. 5, no. 4, August 1989, [5] O. Khatib, A Unified Aroach for Motion and Force Control of Robot Maniulators: he Oerational Sace Formulation, in IEEE Journal of Robotics and Automation, Vol. 3, no. 1, February 1987, [6] NASA JSC letter MA , Fail Safe Aroach for Comuter-Based Control System, Setember 19,

ADVANCED MOTION-FORCE CONTROLLER FOR SPACE ARMS: EXPERIMENTAL RESULTS WITH THE GROUND REFERENCE MODEL OF EUROPA MISSION

ADVANCED MOTION-FORCE CONTROLLER FOR SPACE ARMS: EXPERIMENTAL RESULTS WITH THE GROUND REFERENCE MODEL OF EUROPA MISSION INTRODUCTION A. Terribile ( 1 ), M. Favaretto ( 1 ), M. Filippini ( 1 ), T. Grasso