Contact and Force Detection using Hybrid Estimation

1. Contact and Force Detection using Hybrid Estimation Lars Blackmore Brett Kennedy and Eric Baumgartner

2. Overview • Part A: Introduction and Approach • Contact and Force Detection: Problem Statement • Existing approaches • Different approach: Hybrid Estimation • Modeling LEMUR II-B manipulator • Part B: Experimental Results • Contact, Force Detection with Shoulder Torque Sensor • Contact, Force Detection with no Force/Torque Sensing • Conclusion

3. Problem Statement • With limited sensing capabilities: • Detect contact of manipulator endpoint with environment • Estimate force at endpoint • “What is the minimum set of sensors required?” LEMUR II-B LEMUR II-A Images courtesy of Caltech/JPL

4. Existing Approach • Solution used for MER instrument deployment device: • Hardware contact switches detect contact • Additional mass, volume • Must be mechanically robust to unexpected contact, dust, debris • Force estimation uses: • Knowledge of contact from switch • Accurate compliance model, to determine overdriving state, and hence tip force • What if contact switch not available? Contact Sensor Rock Abrasion Tool Image courtesy of Caltech/JPL

5. Alternative Approach • Available observations: • Encoder readings • Control voltage applied to motor • Limited force detection • Hidden state: • Contact • Tip forces • Bend in manipulator links • Alternative Approach: • Instead of using dedicated sensor… Use all available information to inferhiddenstate based on a model of system • Contact detection • “do observations fit model of contact, or of free motion?”

6. Hybrid Estimation Discrete-valued Continuous-valued • Would like to estimate hidden state • Kalman filters typically used for state estimation with continuous system dynamics • In manipulation case, hidden state is a hybrid of both discrete and continuous components • Contact state  { contact, no-contact} • Tip force • Link bend angle • Estimation with hybrid systems  Hybrid Estimation

7. Classical Estimation • Purely continuous case: use Kalman Filters to estimate state from noisy observations • Kalman filtering is a probabilistic approach • Handles noisy sensors • Represents model uncertainty explicitly • Robust to anomalous observations Continuous system model Noisy observations Estimated most likely state

8. Classical Estimation • Kalman Filtering • Calculate beliefstate about hidden variables • Approximate as Gaussian • Predict/update cycle: • Start with belief state at t-1 • Predict belief state at t using system model • Use measurement at t to adjust the belief state

9. Hybrid Estimation 0.001 0.999 1 failed nominal 0 • Hybrid case: probabilistic hybrid model of system • Stochastic transitions between discrete modes • Different continuous dynamics for each mode

10. Hybrid Estimation • Hybrid case: estimate hybrid state from noisy observations Continuous state Discrete mode

11. Hybrid Estimation • Examples of estimates that can be obtained: • Most likely mode (“is there contact?”) • Probability of being in given mode, e.g. contact • Mean, covariance of hidden state, e.g. tip force • Conditional distribution of hidden state, e.g. tip force given that we have contact • Probabilistic inference, similar to Kalman Filter • “Any-time, any-space” algorithm • Relies on hybrid system model…

12. Overview • Part A: Introduction and Approach • Contact and Force Detection: Problem Statement • Existing approaches • Different approach: Hybrid Estimation • Modeling LEMUR II-B manipulator • Part B: Experimental Results • Contact, Force Detection with Shoulder Torque Sensor • Contact, Force Detection with no Force/Torque Sensing • Conclusion

13. Model Learning • How do you obtain hybrid system models? • Model learning approach • Combine engineering knowledge and on-line learning • Automatic hybrid model learning is an opportunity for future research • For this work, employed an intermediate approach • Discrete modes identified manually • Linear least squares parameter estimation used to learn continuous dynamics within each mode

14. LII-B Manipulator Model Torque Voltage HD motor driver Current Manipulator Compliance Bending torques Link deflections • Need to model: • Compliance of manipulator links • Motor torque response to voltage commands

15. Compliance Model Links modeled as rigid beams τ2 Δq2 F Δq1 Torsional springs at each joint τ1 • Assume linear elastic response for small deflections • During contact, assume no slip at endpoint

16. Compliance Model • Now learn compliance parameters using experimental data • Contact experiments carried out using LEMUR II-B

17. Compliance Model • Iterative linear least squares parameter estimation • Good model fit

18. Motor Model • Very complex behavior to model using traditional methods • example contact: Hysteresis in relationship

19. Motor Model No contact Contact • Very complex behavior to model using traditional methods • But can identify different operationalmodes: • Free • Driving • Holding • Backdriven • behavior within each mode can be modeled

20. Motor Model • Free • After stiction transient, joint velocity approximately proportional to commanded voltage

21. Motor Model • Driving • motor does work against manipulator stiffness • bend angle Δq increases • monotonic relationship between V and torque

22. Motor Model • Holding • Motor can react large torques with small V • Δq is constant • V gives no information about torque

23. Motor Model • Backdriven • Voltage is small or zero • Motor is driven backwards under applied load • Δq reduces towards zero

24. Motor Model • Discrete modes: • Free, driving, holding, backdriven • Behavior within each mode learnt using parameter estimation • Learnt parameters still have significant uncertainty • Some effects still unmodeled • Will the model be accurate enough for estimation? • Hybrid discrete/continuous model useful tool for modeling complex system behavior

25. Motor Model: Discrete Transitions 1.0 Free 0.1 0.9 Backdriven Driving 0.1 0.9 0.9 Holding 0.1 • Now we have discrete modes and dynamics • Need to specify transitions between modes • Transition model gives estimator more information • ‘Biases’ mode estimates NB: Not all transitions shown, for clarity

26. Overview • Part A: Introduction and Approach • Contact and Force Detection: Problem Statement • Existing approaches • Different approach: Hybrid Estimation • Modeling LEMUR II-B manipulator • Part B: Experimental Results • Contact, Force Detection with Shoulder Torque Sensor • Contact, Force Detection with no Force/Torque Sensing • Conclusion

27. Force Estimation with LII-B • LEMUR II-B has accurate torque sensor at shoulder • Detect contact and estimate tip forces using • Shoulder torque sensor • Encoder data • Motor control voltages Image courtesy of Caltech/JPL

28. Estimation with Torque Sensor No information about this component This component fully known • What does LII-B shoulder torque sensor tell us about tip forces? F T

29. Estimation with Torque Sensor • How well can Hybrid Estimation estimate tip forces using • Compliance model • Motor model • Shoulder torque sensor? • Torque sensor gives accurate information about perpendicular component • Compliance and motor model fills in the gaps

30. Estimation with Torque Sensor F F Moment arm Moment arm F Moment arm • Contact scenarios with different moment arms • As moment arm decreases, torque sensor yields less and less information • Estimation relies more heavily on model • 10 mode sequences tracked T

31. Estimation with Torque Sensor Systematic error? • Results: moment arm at 0.15m • Average error = 7% Estimate smoother than measured force

32. Estimation with Torque Sensor • Force estimate accurate except for very small moment arm

33. Estimation with Torque Sensor • Conclusion • Hybrid Estimation able to fill in ‘missing’ information using compliance and motor model • Force estimates accurate to within 10% except for very small moment arm • Model-based approach means changing sensor type or location is simple

34. Estimation without Torque Sensor • Detect contact, forces at LEMUR II-B endpoint • without any force/torque sensing Image courtesy of Caltech/JPL

35. Ad-hoc Contact Detection • How would you make a contact detector without force sensing? • Doesn’t achieve desired velocity if have contact? Free motion Contact (tip stationary)

36. Ad-hoc Contact Detection • Need to look at lower level system dynamics • How does commanded voltage relate to observed encoder motion in different contact states? • Main point: information is there – how do we detect contact? Free motion Contact

37. Ad-hoc Contact Detection Contact here • How would you build a detection scheme now? • Threshold the voltage? • What about commanding different velocities? • What about transients? (noise, stiction…)

38. Contact Detection with Hybrid Estimation • Models of system behavior for each possible mode (contact, no contact) • Estimator looks at observations and determines evidence for each of models being true Initially both free and contact look likely Evidence against free builds up as V continues to increase

39. No Torque Sensor: Results • Detection not possible without torque sensor unless computational resource allocation increased • Increased allocation to 50 tracked sequences • Typical results:

40. No Torque Sensor: Results • Contact detected in all cases for force > 4N • Becomes unreliable below this threshold • Average detection delay: 0.37s • Average duration error: 21% • Consistently estimates shorter duration, perhaps backdriving model could be improved • Reliable contact detection is possible using only motor voltages and encoder counts • Are the computational resources available?

41. No Torque Sensor: Results • Tip force estimates • Typical result: • On average, force estimate accurate to 28%

42. No Torque Sensor: Summary • Probabilistic approach gives reliable contact detection using only motor voltages and encoder data • Evidence for contact builds up over several time steps • Robust to noise in sensors and modeling error • Relatively accurate tip force estimation also possible • Detailed validation not yet carried out • Significantly greater computational resources required than for detection with torque sensor

43. Experimental Lessons Learnt • Performance is highly sensitive to endpoint slip • Motion caused by slip attributed to increase in Δq, forces greatly overestimated • Performance depends on control law used • Problems occur when using joint space controller • Best performance when using cartesian trajectory control • Performance is sensitive to noise parameters in model • Difficult to model using engineering knowledge • Learning approach likely to be very useful

44. Computational Issues • Estimator not implemented on-line due to time restrictions • Off-line implementation not optimized for speed, memory • Algorithm is “any-time, any-space” • Tradeoff between sensor capabilities and computational resources

45. Future Research Opportunities • Further testing and validation of this approach • What sensors are necessary to achieve requirements? • Automated learning of hybrid models • Active estimation • Gain more information by actively probing a system • Design safe control inputs that distinguish optimally between uncertain modes • (Manipulator path planning with obstacles)

46. Conclusion • Using very limited sensor information, Hybrid Estimation can detect contact and estimate tip forces by reasoning about hybrid system models

47. Questions?