NASA Technical Memorandum 110335
Experimental Robot Position Sensor FaultTolerance Using Accelerometers and JointTorque Sensors
Hal A. Aldridge and Jer-Nan JuangLangley Research Center, Hampton, Virginia
March 1997
National Aeronautics andSpace AdministrationLangley Research CenterHampton, Virginia 23681-0001
Experimental Robot Position Sensor Fault Tolerance Using Accelerometers and Joint Torque Sensors
Hal A. Aldridge1 and Jer-Nan Juang2
NASA Langley Research CenterHampton, VA 23681-0001
1 Electrical Engineer, System Integration Branch2 Principal Scientist, Structural Dynamics Branch
Abstract
Robot systems in critical applications, such as those inspace and nuclear environments, must be able to operateduring component failure to complete important tasks.One failure mode that has received little attention is thefailure of joint position sensors. Current fault tolerantdesigns require the addition of directly redundantposition sensors which can affect joint design. Theproposed method uses joint torque sensors found inmost existing advanced robot designs along with easilylocatable, lightweight accelerometers to provide a jointposition sensor fault recovery mode. This mode usesthe torque sensors along with a virtual passive controllaw for stability and accelerometers for joint positioninformation. Two methods for conversion fromCartesian acceleration to joint position based on robotkinematics, not integration, are presented. The faulttolerant control method was tested on several joints of alaboratory robot. The controllers performed well withnoisy, biased data and a model with uncertainparameters.
I. Introduction
Fault tolerant control in robot systems has been an activeresearch area in the past few years[1,2,3,4]. The researchhas been driven by the need for robots to work reliably inspace and nuclear environments where human interventionis difficult. Joint position sensor fault tolerance is animportant characteristic of reliable robot systems that hasreceived little attention to date.
For a robot joint to be truly fault tolerant to positionsensor failure, it must be driven by a controller whichdoes not rely on position sensor feedback for stability.Control of robotic systems has been a difficult problemdue to the nonlinearity of the system equations withoutconsidering the complexity added by fault tolerancerequirements. Techniques to control this nonlinearsystem, such as feedback linearization[5], have beenpopular in the robotic control literature for some time.The main reason for the nonlinearity in the equations isthe need to calculate the dynamic effects on thestructure. This calculation requires position feedback inaddition to a complex system model. Controlling thenonlinear plant based on the full dynamic equations isnot the only method for controlling a robot
manipulator. If the joints have a torque sensor alongthe drive axis, the problem can be reduced tocontrolling the individual joint dynamics. Work inthis area has been published recently[6,7]. Thesemethods use known, mostly linear, electric motordriven joint models to control joint acceleration andvelocity. However, these methods still requiremeasurement of joint position and velocity to computethe control inputs. A new controller [8] was developedusing virtual passive design techniques, joint torquesensor feedback, and simple joint model which isLyaponov stable without joint position sensor feedback.
Although a controller that is stable without positionfeedback is available, position feedback is still required toservo to a position. The primary method used in reliablerobot design is the inclusion of redundant positionsensors. The redundant sensor’s output is compared withthe primary sensor’s output to detect failures and, duringfailure, is used instead of the primary sensor’s output.The addition of the redundant sensor usually affects jointdesign. Some other fault tolerant design techniques useend effector tracking or kinematic redundancy to continueoperation after a sensor failure. The success of thesetechniques is task and/or environment dependent. Amethod that is less task dependent and is integrable intoexisting robot designs would be desirable.
Utilizing double integration, perfect accelerometers wouldbe ideal for this purpose. Current accelerometer designsare small, require little power, and are easily incorporatedinto existing robot designs. Unfortunately, sensor effectssuch as noise and bias make the double integration basedtechnique impractical. A method to obtain joint positionfrom Cartesian accelerometers without integration [9] wasrecently developed to solve this problem. While effective,the number of accelerometers required by this method toprovide fault tolerance to all robot joints can beimpractical. A system wide method was developed in[10] to reduce the number for accelerometers required.
This paper will review both the torque sensor basedjoint controller and the joint specific and system wideaccelerometer based position determination techniques.The incorporation of these techniques into a faulttolerant control systems will be discussed. The newcontrol systems were implemented on a laboratorymanipulator. The results of experiments conducted to
show the performance of the resulting control systemswill be presented.
II. Virtual passive joint control
The virtual passive joint torque control technique isbased on a combination of the torque compensationmethod from Kosuge[5] and the virtual passive controldesign technique from Juang[11]. The controller designis detailed in Aldridge[8]. Figure 1 shows an electricmotor driven, direct drive joint for which the controllerwill be designed. Equation (1) describes the resultingsystem.
link i
Stator
τ i
τsi
Rotor
θi
link i-1
Figure 1: Diagram of proposed direct drive joint
τ θ τ θi i i si i i im v f= + + +˙ ˙ (1)
In Eq. (1), mi is the rotor inertia, vi is the viscousfriction, τi is the input torque, τsi is the sensed torque,and fi is the nonlinear dynamic term resulting frommoving the previous i-1 links. Note that all termsrelating to link and load dynamic and static torques aregrouped into the τsi term. As a result, these torques aresensed, not calculated. For reasonable robot motions,the fi term is small, is zero for the first link assuming afixed base, and decreases with system kinetic energy.As a result, the fi term can be treated as a disturbanceand ignored. Equation (2) defines a new quantity, τxi,ignoring the fi term.
τ θ θxi i i i i im v u= − − +˙ ˙ (2)
The controller design technique used is similar to thevirtual passive control technique presented in Juang.This technique is based on the concept that amechanical system can be represented by a second-ordersystem with inertia, damping, and stiffness relatedparameters. An active feedback controller can bedesigned with its dynamics equivalent to a mechanicalsystem. The resulting controller is,
H x H x H x g y uM c D c K c s˙ ˙ ( )+ + + = (3)
where ys is the measured system output, g is a userdefined function, xc is the controller state vector ofdimension nc, and HM, HD, and HK are the controller
mass, damping, and stiffness matrices respectively.These matrices are design parameters and can be chosento meet performance and stability requirements. Thefunction g is an arbitrary function of the measuredsystem outputs, ys. These outputs can be system statesor combinations of system states.
The torque controller using τxi will now be detailed.Note that if fi is treated as a disturbance, τxi=τsi. Let:
M diag m mrz p= ( ,..., )1 (4)V diag v vp= ( ,..., )1 (5)τ τ τx x xpdiag= ( ,..., )1 (6)ui i= τ (7)
Using the virtual passive controller design technique, acontroller that satisfies the Lyaponov stability criteria isgiven by,
˙
˙ ( ) ( ) ˙
x
x
I
R M K R M D
x
x
M M u
c
c rz c rz c
c
c
rz rz
x
=
− −
+−
′
− −
− −
0
0 0
1 1
1 1
τ ττ (8)
u K R V Dx
xc cc
c= +[ ]
τ ˙
(9)
where Dc, Rτ, and Kc are design matrices. Therestriction on these design matrices is that Dc and Rτmust be a symmetric and positive-definite. Theprevious joint torque command, u′, is known and usedto calculate the current torque command. Note that theterms x xc c and ˙ are not the joint position andvelocity. They are controller states used to satisfy thestability condition. As a result, this controller designcan dissipate joint kinetic energy without joint positionor velocity feedback.
The resulting controller has several important propertiesfor the fault tolerance application:• It does not depend on joint position or velocity
feedback for stability• It has tunable stiffness and damping terms• It can act as an active joint brake by dissipating
energy• Its stability is robust to modeling errors• It compensates for link and load dynamic effects
allowing for reduced position controller gains
III. Joint specific accelerometer based positionsensor fault tolerance
Although the virtual passive joint controller does notrequire position feedback for stability, position feedbackis required for position control. In this paper that
feedback will be provided by Cartesian accelerometers.The position determination method proposed, detailedin Aldridge[9], requires two triaxial Cartesianaccelerometers to be attached to the link driven by thejoint whose position sensor is to be made fault tolerant.The method also uses gravity or a known accelerationfield as a reference. The accelerometers must be of thetype that can sense constant acceleration for acceptablesteady state performance. Figure 2 shows a diagram ofthe proposed accelerometer configuration.
A1
A2r
p
l
link i
link i+1
joint i+1
link i+2
Figure 2: Proposed accelerometer configuration
The problem is to find a measurement which is directlyrelated to joint position. If the acceleration of a pointon link i coincident with joint i+1 is known fromkinematics and measurements of the first i joint states,in the special case where p is the zero vector the jointposition is given in Eq. 10.
θ α α
α αi y i z i x y x
y i z i y x x
a a A A a
a a A A a+
−= + −
+ +1
1 1 1
1 1
tan (( $ cos $ sin ) $ ,
( $ cos $ sin ) $ ) (10)
where,
a
a
a
a
A
A
A
Aii
x
y
z
x
y
z
+ =
=
1 1
1
1
1
ˆ
ˆ
ˆ
,
and αi is a known robot Devanit-Hartenberg parameter.
For any joint later in the kinematic chain to be madefault tolerant, the acceleration ai
i++
21 must be calculated.
The calculated position could be differentiated and usedalong with the robot kinematics to calculate theacceleration. Due to the noisy nature of accelerometerrelated signals, differentiation would be impractical.However, given the joint position and the angularvelocity and acceleration of the ith link (calculated fromthe first i measured joint states and kinematics), therelationship between Cartesian acceleration and jointvelocity and acceleration is given by Eq. (11).
˜
( )
A
r r r
r r r
r r
z x y x
z y x y
x x y y
=− − −− −
+
2
2
2 0 0
ΩΩ
Ω ΩΘ (11)
where,
ΩΩΩΩ
Θ= =
=
=
=
++
+
+
R r
r
r
r
A
A
A
Aii
ii
x
y
z
x
y
z
x
y
z
i
i
i
11
1
12
ωθθθ
˜
˜
˜
˜
˙
˙
˙
˜ ( ) ˙A A A R R r R rii
ii
ii
ii
ii
ii= − − × × + ×+ + +2 1 1 1 1ω ω ω
with Rii+1 the rotation matrix from the ith frame to the
(i+1)th frame and ω ii the angular velocity of the ith link
in the ith frame. The joint velocity and its square aretreated as independent variables to keep the equationslinear. Although the rank of the solution is notguaranteed, it can be solved by least squares. Asimpler solution method involves solving a specific setof equations derived from Eq. (11) depending on zerosin the first column of the matrix in Eq. (11). If rx=0 orry=0, the solution of the equations can be furthersimplified because the solution for θ i+1 is independent
from ˙θ i+1.
To solve Eq.(11), three cases must be considered. Let εbe some small number used to bypass small elements ofW that would be zero with perfect measurements.Assuming either rx or ry is non-zero, the cases are:
1. |Ωxrx+Ωyry| > ε ⇒ rank(W)=32. |Ω xrx+Ω yry| < ε and (|Ω zrx| >ε or |Ω zry| >ε )
⇒ rank(W)=2, equations are underdetermined3. |Ω xrx+Ω yry| < ε and |Ω zrx| <ε and |Ω zry| <ε
⇒ rank(W)=2, unique solution with no dependence on &θi +1
Solution techniques for the three cases in the generalsituation of rx and ry ≠0 can be derived. However, if thesystem is designed such that rx or ry is zero, the solutiontechniques become less complex. The followingtechniques can be used if rx=0, similar solutions can beshown for ry=0. For all cases of rx=0:
˙˜
θ ix
y
A
r+ =−
1 (12)
Now, calculating &θi +1 for each case:
Case 1 for rx=0
˙˜
θ iz
y y
A
r+ =1 2Ω(13)
Case 2 for rx=0
In this case, there exists a quadratic equation in &θi +1 .Solving with the standard quadratic solution:
˙( ˜ )
θ iz y y z y y
y
r r r A
r+ =± −
1
Ω Ω(14)
The proper root is selected by checking for consistencywith measured accelerations. In contrast to case 3, there issufficient information in the measured accelerations todetermine the proper root.
Case 3 for rx=0The solution for case 3 has no explicit dependence on&θi +1 . As a result, Eq. (15) must be used.
˙˜
θ iy
y
A
r+ =−
12 (15)
The magnitude of &θi +1 is calculated via a square root.Determination of the sign is problematic and will beaddressed later in this section.
Given the results of this calculation in either the generalor special case and known robot kinematics, thesolution for ai
i++
21 can be found without differentiation.
For a real robot system, the assumption that p is zero isnot practical. However, if p is non-zero, the positionsolution is no longer independent of the velocity andacceleration of the (i+1)th joint. As presented, the jointvelocity and acceleration calculation is alwaysdependent on the joint position. This problem can besolved by introducing a correction term and iteratingbetween the joint position and the joint velocity andacceleration solutions. First, the previous joint valuesare used to calculate the approximate angular velocityand acceleration of the (i+1)th link as shown in Eq. (16)and (17).
( ) ( ˙ ) ˆω ω θii
approx ii
ii
i approxR z++ +
+= +11 1
1 (16)
( ˙ ) ˙ ( ˙ ) ˆ ( ˙ ) ˆω ω ω θ θii
approx ii
ii
ii
ii
i approx i approxR R z z++ + +
+ += + × +11 1 1
1 1 (17)
These values are used in Eqs. (18) and (19) to producea new term, A1corr.
Acorr p pii
approx ii
approx ii
approx= × + × ×++
++
++( ˙ ) ( ) ( )ω ω ω1
111
11 (18)
A corr A Acorr1 1= − (19)
The A1corr term is used in the position solutioninstead of A1 to obtain (θi+1)approx. The (θi+1)approx termis then used in Ri
i+1 to obtain a better solution for thejoint acceleration and velocity from the ai
i++
21 solution.
The process is repeated by calculating a new Acorrusing the new approximation of the joint velocity andacceleration instead of the value at the previous time.
For a physical robot system, this iteration convergesrapidly at reasonable sample rates.
If the equations in (11) satisfy only case 3, then theequation for joint velocity is not solvable. The squareof the joint velocity is solvable but determining thedirection of the velocity is problematic. A method todetermine the sign of the velocity and enhance velocitytracking performance in the presence of accelerometernoise in all cases is to implement a velocity observer.The iterative, general position solution is modeled as alinear system expressed by:
x Ax Buy Cx
= += (20)
with state-space matrices:
x u y
AK
KB
K
KC
i
i
i
ii
v
k
v
k
=
=
=
=−−
=
= [ ]
+
+
+
++
˜
˜, ˙ , ˙θ
θ
θθ
θ1
1
1
11
1
0
0
10 1
The states are joint position θ i+1 and velocity ˜θ i+1, theinputs are calculated joint position θ i+1 and acceleration˙θ i+1 from the iterative solution, and the output is the
filtered joint velocity, ˜θ i+1. The Kk term is used tocompensate for possible calculated acceleration bias dueto accelerometer sensor error. Without the Kk term,the system is an unstable double integrator and a biasin the calculated acceleration can lead to unboundedjoint velocity. The Kk term can cause oscillations insteady state. The Kv term adds damping and can cancelout the oscillations.
IV. System wide accelerometer based joint positionsensor fault tolerance
The system wide solution method is designed tocomplement the joint specific method. Where the jointspecific method gives an almost closed form solutionfor joint position, the system wide method utilizes anoptimization algorithm to give an more flexiblesolution with fewer accelerometers.
The system wide solution method is designed for an nrevolute joint, rigid link robot system with m triaxialaccelerometers mounted at various locations on therobot. A robot of this kinematic configuration isgoverned by the following kinematic equations:
ω ω θii
ii
ii
iR z++ +
+= +11 1
1& $ (21)
& & & $ && $ω ω ω θ θii
ii
ii
ii
ii
i iR R z z++ + +
+= + × +11 1 1
1 (22)
a R l l aii
ii
ii
i ii
ii
i ii
++ += × + × × +1
1 1( & )ω ω ω (23)
Am pm pm aii
ii
i ii
ii
i ii
++
++
+ ++
++
+ ++= × + × × +1
111
1 11
11
1 11&ω ω ω (24)
whereω i
i angular velocity of ith link in ith frameai
i acceleration of ith link in ith frameRi
i+1 rotation matrix from ith frame to (i+1)th frame
θi position of ith joint Ami
i++11 acceleration of accelerometer Ami+1 in the
(i+1)th framepmi+1 vector offset from joint i+1 to
accelerometer Amii++11
li vector offset from joint i to joint i+1 in theframe i
z identity z vector [0 0 1]T
Each accelerometer contains information on theposition, velocity, and acceleration of joints precedingthe accelerometer in the kinematic chain. End-effectormounted accelerometers contain information about alljoints. However, this information is mixed together ina nonlinear fashion. Except for accelerometers attachedto the first joint, there is no single acceleration that is,in general, dependent on solely the position, velocity,or acceleration of the ith joint. Components of theacceleration due to certain joints can dominate due tokinematic configuration or trajectory. For certainconfigurations, the end-effector pose does not uniquelydetermine the joint trajectory of the robot. As a result,only utilizing end-effector mounted accelerometers isnot sufficient for determining joint trajectories.Distributing accelerometers along the arm in addition toend-effector mounted accelerometers can help alleviatethese problems by obtaining information at pointscloser to joints of interest.
The proposed solution method utilizes the distributedCartesian accelerometers in conjunction with anyworking joint position sensors to recover a lostposition sensor. This is done by calculating a knotpoint, a point in the trajectory having position,velocity, and acceleration, that will make the measuredaccelerations match the accelerations determined by Eq.(24). In Eq. (25), the data from the operational jointsensors is used to the extent possible, leaving onlyterms involving the failed joints,
Q A j bj j j( , & , && ) ,θ θ θ = ∈ (25)
where,b is the set of failed jointsA is the vector of accelerometer
measurements [ Am11 … Amm
m ]T
Q is a set of equations of type (24) corresponding to A
If r is the number of failed joints, then the resultingsystem has 3m nonlinear equations (one equation peraxis, three axes per accelerometer) in 3r (position,velocity, and acceleration of failed joints) unknowns. Ifthis system is solvable, then the knot points for all rjoints can be determined and the joint positionscalculated.
A minimum criteria for solution is that there be at leastas many equations as unknowns, i.e. m r≥ . If thiscondition is not satisfied, then the minimum errorsolution will be one using least squares. Due to thecomplexity of the equations, this condition isoptimistic. As r approaches m, the system becomesmore ill-conditioned. Choosing trajectories whichresult in a condition number of the gradient of Q lessthan some reasonable limit can help system solvability.
The system wide method, like the joint specificmethod, relies on the existence of a known accelerationfield, usually gravity. This field provides a known,constant excitation to the system accelerometers. Thesolution method can be applied to systems withoutsuch a field, but positions will be relative, not absolute,and drift due to small errors is likely. Theaccelerometers must be of the instrument type that candetect constant accelerations.
The system wide solution method requires acomputational technique for solving a system ofnonlinear equations. To be practical, the solution mustbe calculable in real-time. Real time in this senseimplies updates at a rate fast enough for stable jointcontrol if this method is used for fault recovery. Thisrequirement limits the available solution techniques.The solution technique must also be robust to sensornoise and bias. The convergence of the technique to agood solution must be predictable. Traditionalnonlinear solution techniques can be applied to thisproblem. In the experimental setup detailed in sectionVI, steepest descent [12] was used for its computationalefficiency. Most computational methods require agradient for rapid convergence. The gradient requiredfor this method can be computed numerically by finitedifferences [13] or, more efficiently, by the recursivealgorithm shown in Appendix A.
V. Fault tolerant control system
Incorporating an accelerometer based positiondetermination method and the virtual passive basedtorque controller results in a control system which iscapable of servoing to a position without joint positionsensor feedback. An example of such a control systemusing the joint specific method from section III can befound in Fig. 3. A similar version using the systemwide method from section IV is shown in Fig. 4.
The FDI, failure detection and identificationcomponent, uses existing FDI techniques to determineif a joint position sensor has failed and informs theposition solution which joint positions it mustcalculate. The position solution can be used onunfailed joints as part of the FDI procedure to verifyjoint position, velocity, and acceleration independentlyof the joint sensors. For the joint specific methodshown in Fig. 3, the output of accelerometer A1 andthe difference between A2 and A1, forming a relativeacceleration, are sent to lowpass filters to reduce noise.A bias filter, an extremely lowpass filter, attempts toremove steady state bias in the relative acceleration.The position output of the position solution and theobserved velocity are merged with valid joint positionand velocity measurements from working joints for theposition controller, in this case a simple PD controller.The position command torque and the sensed torque areboth filtered to remove noise and match phase shifts.The position command torque is used to offset thesensed torque to drive the robot in the desired direction.The offset sensed toque is processed by the virtualpassive torque controller and the commanded torque forthe robot joint is calculated.
The control diagram for the system wide method inFig. 4 is similar with the exception that all systemaccelerometers are used simultaneously without formingrelative accelerations. Although the solution method iscapable of producing joint position, velocity, and
acceleration, to reduce computational complexity thesolution was formulated to only produce position andacceleration. The velocity is the most difficult value toget accurate solutions for in the presence ofaccelerometer error. The control system used thevelocity observer described in section III to producecalculated velocity.
Using the virtual passive torque controller in thismanner makes the use of accelerometer based positionmeasurements practical for closed loop control. Sincethe torque controller compensates for the dynamic andstatic torques required to drive the links, the positioncontroller gains can be kept low. These low gains areimportant because, even filtered, the accelerometer basedpositions are noisier than optical encoder or resolverbased measurements. The passive controller gains canalso be adjusted to tolerate noisier inputs with thetradeoff of lower position tracking performance. Thecalculated positions may have a bias associated withthem due to sensor error. That position bias woulddegrade the effectiveness of gravity compensation. Byusing the passive torque controller, a gravitycompensation algorithm is unnecessary.
VI. Experimental setup
Experiments to examine the performance of theproposed method were conducted on a RoboticsResearch (RRC) 807i manipulator instrumented with
LowpassFilter
LowpassFilter
-
A1
A2
BiasFilter
Sensed JointPosition and Velocity from
Robot
IterativePositionSolution
FDI
PD PositionControl
TrajectoryGenerator
LowpassFilter
LowpassFilter
Sensed Torquefrom Robot
- -
- Virtual PassiveTorque Control
Torque Commandto Robot
VelocityObserver
Figure 3: Control system diagram using joint specific method
LowpassFilter
LowpassFilter
A1
Am
Sensed JointPosition and Velocity from
Robot
System WidePositionSolution
FDI
PD PositionControl
TrajectoryGenerator
LowpassFilter
LowpassFilter
Sensed Torquefrom Robot - Virtual Passive
Torque Control
Torque Commandto Robot
VelocityObserver
Figure 4: Control system diagram using system wide method
accelerometers. The RRC 807i is a 7 degree offreedom, 0.8m long manipulator with a 10kg payload.
Joint specific setup
The accelerometers for the joint specific method wereinstalled on two joints of the robot in a pattern similarto that in Fig. 2. Figure 5 shows the robot and theaccelerometer mounts attached to the shoulder pitch andelbow pitch joints. A closer view of the elbow pitchjoint mounted accelerometers is shown in Fig. 6. Theaccelerometers used were ICSensors model 3140piezoresistive accelerometers with internal amplificationand signal conditioning. The accelerometers had amaximum range of ±2g. These piezoresistiveaccelerometers were capable of sensing constantaccelerations, a necessary feature for the positionsolution method. Each accelerometer sensedacceleration along a single axis. Three accelerometerswere mounted orthogonally to form a single triaxialaccelerometer. The accelerometers were chosen for theirease to use and cost. The accelerometer mounts weredesigned to accommodate the accelerometers and useexisting mounting points on the robot. The resultingaccelerometer mounts are unsuitable for real world usedue to their size and location. However, smalleraccelerometers which could be incorporated directly intothe robot structure are readily available.
Figure 5: RRC robot with accelerometer mounts
The joint specific based controller was implemented ontwo 68040 based computers running on a VMEbackplane. The VME bus was connected to the RRCrobot controller via a bus to bus interface. The interfaceallowed the RRC robot to be controlled by reading andwriting to memory locations in the VME address space.The first 68040 based computer, referred to as thecontrol processor, ran the interface code for the arm, armsafety systems, and the virtual passive controller. Thecontrol processor generated new torque commands at200Hz. The second 68040, referred to as theaccelerometer processor, communicated with the armmounted accelerometers and ran the position solutioncode. The accelerometers were sampled at 100Hz and a
new position solution for two concurrent failures wasgenerated at 75Hz.
Figure 6: Elbow accelerometer mount
System wide setup
Figures 7 and 8 show the RRC manipulator and theaccelerometer mounts for the system wide solutionmethod. Single triaxial accelerometers were attached tothe shoulder pitch, elbow roll, and elbow pitch joints,joints 2,3, and 4 respectively. This configuration wasthe easiest to mount to the robot structure, not theoptimal accelerometer configuration. Two triaxialaccelerometers were mounted to the end-effector.
A third processor was added to the controllerimplemented for the joint specific method to handle theincreased computational complexity of the system widemethod. The third processor, a 33Mhz 68040 calledthe optimization processor, implemented the systemwide position determination algorithm. The finalsystem was capable of producing joint positions forthree simultaneous failures at 60Hz. While theoptimization processor was fully loaded at this rate, theother processors were not heavily loaded for the systemwide experiment.
Figure 7: RRC arm with accelerometers, front view
Figure 8: RRC arm with accelerometers, side view
To implement a real-time system for the system widemethod on the computational resources available,several compromises were required.
1. Although other solution techniques produce a morerobust system, steepest descent optimization wasused for its computational efficiency.
2. The gradient was not updated every control cycleto lower computational complexity. A fullgradient was available every four cycles. Themodularity of the recursive method in Appendix Amade distributing the computation over severalcycles simple.
3. A new singular value decomposition (SVD)required by the steepest descent method (due to thevariable rank of the gradient) was calculated thecycle after a new gradient was completed. Theresults of the last SVD were used with a currentfunction calculation and the measured accelerationto find the next position solution using the steepestdescent algorithm.
Setup notes common to both methods
In instrumenting the robot, electromagnetic noise fromthe robot and robot controller was a significantproblem. The final electrical configuration involvedusing differential accelerometer outputs and differentialA/D conversion. Each side of the differential signal wasfiltered using a 40Hz analog lowpass filter for noisereduction and anti-aliasing. The bus-to-bus interfacecontaminated the local ground interfering with the A/Dconversion. As a result, the analog lowpass filter wasaided by a 5Hz digital lowpass filter to help reduceresidual noise. With the existing system noise, theinternal accelerometer amplification to 1V/G wasessential. If signal quality was improved, lessrestrictive lowpass filters could be used improvingdynamic performance.
The RRC robot was commanded in torque mode. Inthis mode, the robot controller uses a basic torquecontroller to overcome joint effects. Its goal is to makethe harmonic gear driven joints perform like direct drivejoints. The resulting system does reduce geartraineffects but does not eliminate the effects. The virtualpassive controller must handle any remaining geartrainfriction, hysteresis, or backlash. Although the modelpresented in section II was direct drive, the controllercan be used on gear driven joints if the effects geartrainfriction and the gear ratio are considered and nonlineargeartrain effects, such as backlash, are minimized.
The motor parameters required by the controller werenot available from the robot manufacturer. Identifyingthe parameters of motors installed in the robot proveddifficult due to the parameters small size and theinability to bypass the low level torque controller. Asa result, qualitative data was used to estimate theparameters and the gains chosen to produce the requiredperformance. The resulting performance withsubstantial modeling errors shows the stabilityrobustness of the controller.
A simple automatic calibration system wasimplemented for the accelerometers using linearregression. This algorithm attempted to identify thelinear parameters to convert volts to m/s2. Theseparameters changed with temperature and other factors.Each accelerometer had a slightly different dynamicresponse. These responses resulted in a variable bias inthe sensed relative acceleration. For the joint specificmethod, this bias was corrected in steady state by thebias filter but did affect the calculated measurements.Although no relative accelerations were used, thedifference in accelerometer dynamic response degradedthe system wide method’s dynamic response.
VII. Experimental results
Before presenting the experimental results, it should benoted that this control method is proposed as a backupmethod. The position response using accelerometerfeedback cannot be expected to be as accurate as that of acontroller utilizing optical encoder or resolver positionfeedback. The goal of these experiments was to showstable performance with position tracking acceptable forcontinued operation during failure.
Joint specific technique
Figures 9 and 10 show the position tracking response ofthe system with both the shoulder pitch and elbowpitch joint position sensors failed. Although the RRCrobot also provides velocity feedback, this feedback wasalso considered failed. The joint position data wascollected for comparison. The robot was commandedalong a trajectory from [0 -π/2 -π -π/2 0 0 0], the Home
position, to [0.5 -1.27 -3.5 -1.27 0 0 0] in 1.5 seconds.The two failure case is considered to show how thecalculated results for the shoulder pitch joint affect theelbow pitch calculation.
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.55
−1.5
−1.45
−1.4
−1.35
−1.3
−1.25
−1.2
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Shoulder Pitch Joint
Figure 9: Position response of shoulder pitch joint totrajectory ending at [0.5 -1.27 -3.5 -1.27]
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.55
−1.5
−1.45
−1.4
−1.35
−1.3
−1.25
−1.2
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 10: Position response of elbow pitch joint totrajectory ending at [0.5 -1.27 -3.5 -1.27]
As discussed earlier, failures which occur before thejoint in question in the kinematic chain will affect theposition calculations for the joint. However, jointsfollowing that joint will not. This property explainswhy the wrist joints were not moved in thisexperiment. The shoulder roll and elbow roll jointswere not considered failed in this experiment and wereactuated to impart variable dynamic effects on the failedjoints making the experiment more interesting.
The shoulder pitch response was better than the elbowpitch response because it sees fewer dynamic effectsthan the elbow pitch joint. The bias in the steady stateresponse of the elbow pitch joint is due to low positiongains. It should be noted that although the actualposition did not track the commanded in steady state,the calculated position did track the actual. The
damping of the virtual passive controller contributes tothe smooth position tracking response.
Figures 11 and 12 show the response of moving fromHome to [1 -1 -4 -1] in 1.5 seconds.
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
−1
−0.9
−0.8
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Shoulder Pitch Joint
Figure 11: Position response of shoulder pitch joint totrajectory ending at [1 -1 -4 -1]
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
−1
−0.9
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 12: Position response of elbow pitch joint totrajectory ending at [1 -1 -4 -1]
This trajectory is faster than the previous trajectory. Asa result, the dynamic effects are more intense and it ismore difficult for the PD position controller to start andstop the trajectory as commanded. The faster trajectoryalso shows the effect of the delay between actual andcalculated position. This delay is caused by filter andinternal communication delays. This combination ofeffects results in a substantial overshoot in the shoulderpitch response. This overshoot can be reduced at thecost of poorer tracking of slower trajectories. This isfunction of PD controllers in general but is worsened bythe position calculation delay. The steady state errorbetween calculated and actual position is due tocalibration error. The automatic calibration algorithmwas optimized for positions significantly far away fromthe final configuration in this trajectory.
System wide technique
The most interesting failure considered using thesystem wide method involved three simultaneousposition sensor failures. Several sets of failed jointswere successfully controlled with the sameaccelerometer configuration. The representativeposition response is shown in Figs. 13-15. Thecommanded trajectory ran from [0 -π/2 -π -π/2 0 0 0] to[0.5 -1.27 -3.5 -1.27 0.3 -0.5 -0.5] in 1.5 seconds.The position sensors for the elbow pitch, wrist pitch,and toolplate roll joints were failed in this case.Different failure sets produced similar results.Lowering the number of simultaneous failures improvedresults.
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.55
−1.5
−1.45
−1.4
−1.35
−1.3
−1.25
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 13: Position response of elbow pitch joint
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Wrist Pitch Joint
Figure 14: Position response of wrist pitch joint
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
0.1
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Toolplate Roll Joint
Figure 15: Position response of toolplate roll joint
Similarly to the joint specific method, as the speed ofthe trajectories is increased, the dynamic effects are moreintense and it is more difficult for the PD positioncontroller to start and stop the trajectory ascommanded. Figure 16 shows the response of theelbow pitch joint along a trajectory faster than thatshown in Fig. 13.
Actual
Calculated
Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
−1
−0.9
−0.8
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 16: Position response of elbow pitch joint
General notes
The trajectories shown are faster than would reasonablybe expected for a robot with failed primary systems.They are shown to illustrate the stability and trackingperformance in extreme conditions. For real use, theposition gains would be tuned for better tracking at lowspeeds and the maximum velocity commands limited.As part of a system with a slow end-effector feedbackcapability to remove small position bias, such as ateleoperated system, the ability to stably continueoperation during failure would be more advantageousthan improved high-speed operation.
The main problem encountered in this experiment withthe accelerometer based position determination method
was the signal to noise ratio in certain robot and sensorconfigurations. In some configurations, theaccelerometer axes required to solve for position, notvelocity or acceleration, were nearly parallel to gravity.In these configurations, the signal to noise ratio is lowand the resulting position calculation suffers. Thisproblem could be solved by choosing trajectories toavoid these known configurations or by better sensorplacement.
Another advantage of the proposed control system isthat little reconfiguration is required to transition fromworking to failed states. The performance of the elbowpitch joint with encoder feedback along the sametrajectory as shown in Fig. 16 is shown in Fig. 17.This performance is similar to the RRC controller’sposition control mode. This performance is obtainedusing the low position gains for failed operation. Withthese gains, to change from failed to unfailed operation,the position feedback source for the position controlleris the only change necessary. The virtual passivetorque controller will damp out control error due to asmall discontinuity in position feedback duringswitching. If additional time is required to initializethe fault tolerant components, simply zero positioncontroller output and the virtual passive controller willattempt to slow the joint to a stop.
Actual Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
−1
−0.9
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 17: Position response of elbow pitch joint usingpassive controller with low gains
If the designer wishes to exploit the performanceenhancing capabilities of the control system, simplyraising the position gains leads to the response in Fig.18. With these gains the position controller canovercome the residual stiffness and damping on thevirtual passive controller which helps during failedconditions but reduces position tracking performance.The system still benefits from sensing static anddynamic loads. As a result, the system will maintainsimilar tracking in all loading conditions before actuatorsaturation. To revert to failed operation, the positiongains must be lowered. However, the virtual passive
torque controller will mask most of the discontinuity ofthe change. The virtual passive controller gains wereconstant for all experiments in this paper.
Actual Commanded
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
−1
−0.9
Time (seconds)
Pos
ition
(ra
dian
s)
Position Response of Elbow Pitch Joint
Figure 18: Position response of elbow pitch joint usingpassive controller with high gains
VIII. Comparison on position determinationmethods
Both position determination methods summarized herehave specific strengths and weaknesses. The jointspecific method only provides fault tolerance to thejoint which drives the link to which the accelerometersare attached. If only one or two joints in the robotsystems are required to be fault tolerant, then the jointspecific method is ideal. It will provide positionfeedback in a predictable, computationally simplemanner.
The cumulative errors inherent to both methods makecontinued operation with more than three simultaneousfailures impractical. The main advantage to using thesystem wide methods is not its theoretical ability tohandle more simultaneous failures with feweraccelerometers than the joint specific method. Themain advantage of the system wide method is that itcan provide fault tolerance to many sets of different jointfailures. The same set of accelerometers can providefault tolerance an elbow joint, a wrist joint, or both.This flexibility allows the system designer to providesystem wide fault tolerance with a reasonable number ofaccelerometers.
The flexibility of the system wide methods comes at asignificant cost. The optimization method required bythe system wide method is computationally intensive.In addition, the solvability of the system wide methoddepends on the joints that are failed, the trajectory, andthe placement of the accelerometers. The solvability ofthe failed system along a trajectory must be checkedbefore the robot command is given or the robot canbecome unstable. A method involving checking the
singular values of the determinant of the gradient Q isdescribed in [10].
A more complete discussion of the torque controller andboth position determination methods can be found in[14].
IX. Conclusions
This paper has presented a method for continuingoperation during the failure of a joint position sensor.The method can be retrofitted into current advancedrobot designs, is internal to the robot, and is suitablefor long term operation in unstructured environments.The proposed system includes a virtual passive torquecontroller which does not rely on position feedback forstability and one of two accelerometer based positiondetermination method which do not use integration toobtain position. Controller designs using bothposition determination methods were described. Theproposed controllers were implemented on a robot armand experimental results presented. The results showstable performance with reasonable tracking duringmultiple simultaneous joint failures even alongtrajectories which are more difficult than would beexpected of a robot with failed primary systems.
X. Bibliography
[1] Ting, Y., Tosunoglu, S., and Fernandez, B.,“Control Algorithms for Fault-Tolerant Robots”, 1994IEEE International Conference on Robotics andAutomation, vol. 1, pp. 910-915, 1994.[2] Visinsky, M.L., Walker, I.D., and Cavallaro, J.R.,“Layered Dynamic Fault Detection and Tolerance forRobots”, 1993 IEEE International Conference onRobotics and Automation, pp. 180-187, 1993.[3] Chladek, J.T., “Fault Tolerance for Space BasedManipulator Mechanisms and Control System”, FirstInternational Symposium On Measurement and Controlin Robotics, 1990.[4] Paredis, C.J.J., Au, W.K.F., and Khosla, P.K.,“Kinematic Design of Fault Tolerant Manipulators”,Computers and Electrical Engineering, vol. 20, no. 3,pp. 211-220, 1994.[5] Luh, J.Y., Walker, M., and Paul, R.P., “Resolvedacceleration control for mechanical manipulators”, IEEETransactions on Automatic Control, vol. 25, pp. 468-474, 1980.[6] Kosuge, K., Takeuchi, H., and Furita, K., “MotionControl of a Robot Arm Using Joint Torque Sensors”,IEEE Transactions on Robotics and Automation, vol.6, no. 2, pp. 258-263, 1990.[7] Imura, J., Yokohohji, Y., and Yoshikawa, T.,“Robust Control of Robot Manipulators Based onJoint Torque Sensor Information”, International Journalof Robotics Research, vol. 13, no. 5, pp. 434-442,1994.
[8] Aldridge, H.A. and Juang, J-N., “Virtual PassiveController for Robot Systems Using Joint TorqueSensors”, Fourth IASTED Conference on Robotics andManufacturing, pp. 73-76, 1996.[9] Aldridge, H.A. and Juang, J-N., “Joint PositionSensor Fault Tolerance in Robot Systems usingCartesian Accelerometers” AIAA Guidance, Navigation,and Control, paper AIAA 96-3898, 1996.[10] Aldridge, H.A., and Juang, J-N., “System widejoint position sensor fault tolerance in robotics usingCartesian accelerometers”, to be published in SPIEIntelligent Systems and Advanced ManufacturingConference, November 1996.[11] Juang, J-N., Wu, S., Phan, M., and Longman,R.W., “Passive Dynamic Controllers for NonlinearMechanical Systems”, AIAA Journal of Guidance,Control, and Dynamics, vol. 16, no. 5, pp. 845-851,1993.[12] Gerald, C.F. and Wheatly, P.O., Applied Numerical Analysis , 5th edition, Reading: Addison-Wesley, 1994.[13] Press, W.H., Flannery, B.P., Teukolsky, S.A.,and Vetterling, W.T., Numerical Recipes in C ,Cambridge: Cambridge University Press, 1988.[14] Aldridge, Hal A., Robot Position Sensor Fault Tolerance , Ph.D. dissertation, Carnegie-MellonUniversity, 1996.
Appendix A: Gradient calculation
Most nonlinear solution methods require a functiongradient for rapid convergence. The closed-formsolution for the gradient of an n link manipulatorbecomes more complex quickly as n grows. Thegradient could be computed numerically using finitedifferences[13]. Finite differences are not as accurate as aclosed-form solution. The approach taken here is tocalculate the gradient recursively. The systemequations are designed to be calculated recursively, sothe proper application of partial derivatives and thechain rule will lead to the desired outcome. Thedesired gradient has the form:
∂∂
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
∂∂θ
Q
Am Am Am
Am Am Am
Am Am Am
Am Am Am
n
m m
n
m
n n
m
n
m m
n
Ω=
1
1
1 1
1
1 1
1 1
1
1
1
L L
M M M
L L
L
M M M
L
˙
˙
˙ ˙ ˙
˙ ˙ ˙
(A1)
For simplicity, the measurement Ami is always in theith frame. Now,
∂∂θ
∂ω∂θ
∂ω∂θ
ω
ω ∂ω∂θ
∂∂θ
Ampm pm
pma
i
k
ii
ki
ii
kii
i
ii i
i
ki
ii
k
+ ++
+++
++
+
++ +
+
+++
= × + × ×
× × +
1 11
111
11
1
11 1
1
111
˙
+ (A2)
∂∂θ
∂ω∂θ
∂ω∂θ
ω
ω ∂ω∂θ
∂∂θ
Ampm pm
pma
i
k
ii
ki
ii
kii
i
ii i
i
ki
ii
k
+ ++
+++
++
+
++ +
+
+++
= × + × ×
× × +
1 11
111
11
1
11 1
1
111
˙˙˙ ˙
˙ ˙ +(A3)
∂∂θ
∂ω∂θ
∂∂θ
Ampm
ai
k
ii
ki
ii
k
+ ++
+++
= × +1 11
111
˙˙˙ ˙ (A4)
The partial derivative will be zero for all quantities ofjoint k where k>i+1. This a consequence of the laterjoints not affecting the previous links.
Equations (A2),(A3),and (A4) are not recursivethemselves, but the equations for the partials theydepend on are recursive. The equations require thatEqs. (17) through (19) be calculated for all n links todetermine the angular velocities, angular accelerations,and Cartesian accelerations. The partial derivatives canthen be calculated in the following manner:
∂ω∂θ
∂ω∂θωi
i
k
ii i
i
k
kk
kk
R
R++
+
+=
11
1
1
k < i +1
k = i +1
0 k > i +1
˙v
(A5)
∂ω∂θ
∂ω∂θ
ii
k
ii i
i
k
R
z++
+
=
11
1
˙
˙
ˆ
k < i +1
k = i +1
0 k > i +1
v
(A6)
∂ω∂θ
ii
k
++
= ∀11
0˙v k (A7)
∂ω∂θ
∂ω∂θ
∂ω∂θ
θ
ω ω θ˙
˙ ˙ ˆ
˙ ˙ ˙ ˙ ˆii
k
ii i
i
kii i
i
kk
ii
ii
ii
ii
k
R R z
R R z++
+ +
+ +=
×
×
11
1 1
1 1
+ k < i +1
+ k = i +1
0 k > i +1
v
(A8)
∂ω∂θ
∂ω∂θ
∂ω∂θ
θ
ω˙˙
˙˙ ˙
˙ ˆ
ˆii
k
ii i
i
kii i
i
kk
ii
ii
R R z
R z++
+ +
+=
×
×
11
1 1
1
+ k < i +1
k = i +1
0 k > i +1
v
(A9)
∂ω∂θ
∂ω∂θ˙
˙
˙˙
ˆii
k
ii i
i
k
R
z++
+
=
11
1 k < i +1
k = i +1
0 k > i +1
v
(A10)
∂∂θ
∂ω∂θ
∂ω∂θ
ω
ω ∂ω∂θ
∂∂θ
ω ω ω
a Rl l
la
R l l a
ii
k
ii
ii
ki
ii
kii
i
ii i
i
ki
ii
k
ii
ii
i ii
ii
i ii
++ +
+
=
× + × × +
× × +
× + × × +( )
11 1
1
˙
˙ ˙
k < i +1
k = i ++1
0 k > i +1v
(A11)
∂∂θ
∂ω∂θ
∂ω∂θ
ω
ω ∂ω∂θ
∂∂θ
a Rl l
lai
i
k
ii
ii
ki
ii
kii
i
ii i
i
ki
ii
k
++ +
=
× + × × +
× × +
≥
11 1
˙
˙˙ ˙
˙ ˙ k < i +1
0 k i +1v
(A12)
∂∂θ
∂ω∂θ
∂∂θ
a R la
ii
k
ii i
i
ki
ii
k++ +
= × +
≥
11 1
˙
˙˙ ˙ k < i +1
0 k i +1v
(A13)
where,
&sin cos cos cos sin
cos sin cos sin sinRii
i i i i i
i i i i i+
+ + +
+ + +=−− − −
11 1 1
1 1 1
0 0 0
θ θ α θ αθ θ α θ α (A14)
is the derivative of Rii+1 with respect to θi+1.
Using Eqs. (A1) through (A14), the elements of thegradient can be calculated. These equations have beenverified against numerically calculated gradients of Q.
