Integration of force feedback into a nuclear decommissioning robot

Integration of force feedback into nuclear decommissioning robots
Author: Saravanan Yogeswaran
MSc. Smart Systems
Supervisor: Dr. C. J. Taylor
Date: 12 September 2014

i
Declaration
Lancaster University
Faculty of Science and Technology
Engineering Department
Signed Declaration on the submission of a dissertation
I declare that this dissertation is my own work and has not been submitted in substantially the same form towards the award of a degree or other qualification. Acknowledgement is made in the text of assistance received and all major sources of information have been appropriately referenced. I confirm that I have read and understood the publication Guidance on Writing Technical Reports published by the Department.
Saravanan Yogeswaran
12 September 2014

ii
Abstract
The rise in the number of nuclear reactors reaching the end of their expected life is a growing concern for governments and organisations across the world. The process of decommissioning these reactors is costly, time consuming and hazardous to human life. There is, therefore, a growing call for robots to replace humans in this environment. This study investigated the use of force feedback, as part of various control architectures, to allow general purpose robots to perform useful tasks during the decommissioning of these nuclear reactors. This was done by implementing pure force, hybrid force/position and impedance controller on a general purpose BROKK 40 robot that has been equipped with the HydroLek HLK-7W robotic arms. A range of experiments was then used to evaluate the performance of these controllers whilst the robot conducted tasks that would be useful in the nuclear decommissioning environment. The results of these experiments showed that the general purpose robot was successfully able to perform tasks such as the coordinated lifting of an object, drilling into an object and object pushing/moving. The experiments also showed how force feedback could be used to compensate for external disturbances. The hybrid force/position controller demonstrated its ability to perform tasks which can be decomposed into pure force and position components (such as drilling). It was noted, however, that one of the main flaws of this controller was that it could not be suitable tuned for the contact dynamics between the robot and the object or environment. The controller was also sensitive to environmental disturbance, such as obstacles. On the other hand, the impedance controller proved to be extremely versatile in that the desired dynamic response of the robot to the environment and objects of interest could be tuned by the designer. The controller could therefore be used for a range of tasks (such as the pushing of objects), except those that required pure force or position control. Based on these conclusions, the author of this study recommended that further research take place on the hybrid impedance controller, as this may provide the most versatile and robust control architecture for general purpose robots. Recommendations about ways to improve the state of the hardware in order to aid future research were also made during this study.

iii
Table of Contents
Declaration .................................................................................................................................. i
Abstract ...................................................................................................................................... ii
List of figures ............................................................................................................................. v
List of figures ........................................................................................................................... vii
1. Introduction ........................................................................................................................ 1
2. Objectives ........................................................................................................................... 4
3. Literature review ................................................................................................................. 5
3.1 Control architecture ..................................................................................................... 5
4. Hardware description ........................................................................................................ 11
4.1 Introduction ............................................................................................................... 11
4.2 Hardware description ................................................................................................ 11
4.3 Hardware limitations ................................................................................................. 13
4.4 Conclusion ................................................................................................................. 19
5. Consolidation of previous research .................................................................................. 20
5.1 Overview ................................................................................................................... 20
5.2 System architecture ................................................................................................... 20
5.3 Low level control ...................................................................................................... 21
5.4 Simulation program ................................................................................................... 23
5.5 Conclusion ................................................................................................................. 24
6. Low level control .............................................................................................................. 25
6.1 System model ............................................................................................................ 25
6.2 PID algorithm ............................................................................................................ 26
6.3 Methodology ............................................................................................................. 26
6.4 Results and discussion ............................................................................................... 28
6.5 Conclusion ................................................................................................................. 37
7. Force control ..................................................................................................................... 38

iv
7.1 Background ............................................................................................................... 38
7.2 Hardware description ................................................................................................ 38
7.3 Force control architecture.......................................................................................... 39
7.4 Implementation.......................................................................................................... 41
7.5 Loop tuning ............................................................................................................... 42
7.6 Conclusion ................................................................................................................. 46
8. Hybrid force/position control ........................................................................................... 47
8.1 Background ............................................................................................................... 47
8.2 Hybrid force/position control architecture ................................................................ 47
8.3 Implementation.......................................................................................................... 48
8.4 Loop tuning ............................................................................................................... 50
8.5 Conclusion ................................................................................................................. 53
9. Impedance control ............................................................................................................ 55
9.1 Background ............................................................................................................... 55
9.2 Impedance control architecture ................................................................................. 56
9.3 Implementation.......................................................................................................... 57
9.4 General methodology ................................................................................................ 58
9.5 Results ....................................................................................................................... 58
9.6 Conclusion ................................................................................................................. 59
10. Experiments .................................................................................................................. 61
10.1 Single manipulator force control ............................................................................... 61
10.2 Force control for a cooperative task .......................................................................... 65
10.3 Hybrid force/position control for a cooperative task ................................................ 68
10.4 Impedance control for a cooperative task ................................................................. 71
11. Conclusions and recommendations............................................................................... 74
References ................................................................................................................................ 77
Appendix A .............................................................................................................................. 79

v
Appendix B .............................................................................................................................. 81
List of figures
Figure 1: Problems relating to pure position control ................................................................. 5
Figure 2: Example showing the time varying components of position, compliance and force [11] ............................................................................................................................................. 7
Figure 3: Force and position plots using the hybrid controller [11] .......................................... 8
Figure 4: Brokk 40 demolition robot [17]................................................................................ 11
Figure 5: HydroLek dual manipulators and spool valves [17] ................................................ 12
Figure 6: cFP and power supply [17] ....................................................................................... 13
Figure 7: Damage to hydraulic hose ........................................................................................ 14
Figure 8: Effect of stiction on a control valve ......................................................................... 15
Figure 9: Stiction in joint 5 (right) ........................................................................................... 16
Figure 10: Stiction in joint 4 (right) ......................................................................................... 16
Figure 11: Valve drift in joint 7 ............................................................................................... 17
Figure 12: Asymmetric response for joint 2 of the left manipulator ....................................... 18
Figure 13: Control system architecture .................................................................................... 21
Figure 14: Normal step response (a) vs. implemented step response (b) ................................ 22
Figure 15: Simulation program for the MARS-ND ................................................................. 23
Figure 16: Modified simulation program for the MARS-ND .................................................. 24
Figure 17: Generic model of each joint of the HydroLek manipulator ................................... 25
Figure 18: Response of a generic joint to different step inputs ............................................... 25
Figure 19: Block diagram of generic feedback loop ................................................................ 26
Figure 20: Proportional controller for a joint of the HydroLek manipulator ........................... 26
Figure 21: Closed loop response for the proportional controller ............................................. 27
Figure 22: Example of a trajectory used for loop tuning ......................................................... 28
Figure 23: Closed loop response of joint 1 for different gain values ....................................... 29
Figure 25: Magnified view of closed loop performance for joint 2 ......................................... 30
Figure 26: Closed loop response of joint 3 to different gain values ........................................ 31

vi
Figure 30: Closed loop response of joint 4 with set point tolerances implemented ................ 34
Figure 33: Comparison of closed loop response before and after the PID optimisation ......... 37
Figure 34: Phidgets USB interface (left) and the Flexiforce pressure sensor (right) ............... 39
Figure 35: Block diagram of the implemented force controller .............................................. 39
Figure 36: Illustration of an object being gripped ................................................................... 40
Figure 37: Contact force vs. gripper position when gripping an object................................... 40
Figure 38: Illustration demonstrating the implementation of a split range controller ............. 40
Figure 39: Physical packaging of the FlexiForce sensor ......................................................... 41
Figure 40: Illustration of the effect of having a larger contact layer 1 surface ........................ 41
Figure 41: Trajectory used for the tuning of the force control loop ........................................ 43
Figure 42: Closed loop response for experiment 1 .................................................................. 44
Figure 45: Block diagram of the implemented hybrid force/position controller implemented on joint 1 .................................................................................................................................. 48
Figure 46: Decomposition of workspace into position control and force control segments ... 48
Figure 47: Hardware setup for the hybrid force/position controller ........................................ 49
Figure 48: Trajectory used for the tuning of the hybrid force/position control loop ............... 50
Figure 49: Trajectory maintaining constant (right) gripper orientation ................................... 51
Figure 50: Graphs of the closed loop response for different Proportional gain values ........... 52
Figure 51: Illustration of the implemented hybrid force/position controller ........................... 53
Figure 52: Block diagram of the impedance controller [22].................................................... 57
Figure 53: Closed loop response for different inertia values ................................................... 59
Figure 54: Closed loop response for different stiffness values ................................................ 59
Figure 55: Hardware setup for the rolling disturbance experiment ......................................... 62
Figure 56: Hardware setup for the oscillatory excitation experiment ...................................... 62
Figure 57: Closed loop response to a rolling disturbance ........................................................ 63
Figure 58: Graph showing the integral of the OP during the rotation of joint 6...................... 64
Figure 59: Closed loop response to an oscillatory disturbance................................................ 64
Figure 60: Hardware setup for the coordinated lifting experiment .......................................... 66

vii
Figure 61: Closed loop response of the force controller during a coordinated movement task.................................................................................................................................................. 67
Figure 62: Hardware setup for the coordinated drilling experiment ........................................ 69
Figure 63: Closed loop response of the hybrid force/impedance controller during the simulation of a drilling task ..................................................................................................... 70
Figure 64: Closed loop response of the impedance controller during an object 'pushing' task73
List of figures
Table 1: Summarised list of gain values that were evaluated for the joint 1 of the right manipulator .............................................................................................................................. 29
Table 6: Selected proportional gain values for the low level control loops of the left manipulator .............................................................................................................................. 36
Table 7: Summarised list of gain values that were evaluated for the force controller ............. 44
Table 8: Summarised list of gain values that were evaluated for the hybrid force/position controller .................................................................................................................................. 51

1
1. Introduction
Nuclear power plants have long been touted as the long term solution to meeting the ever rising energy demands of the modern age. The world’s first nuclear facility to power a city was successfully commissioned in 1954 in the Soviet Union [1]. This milestone was soon followed by the successful commissioning of a number of power generation facilities across the globe. Despite a range of technical challenges, as well as both political and environmental opposition, the number of nuclear power facilities has steadily risen to over 435 active reactors with a further 71 under construction [2].
As one would expect, the increase in the number of active reactors has led to a rise in the number of reactors that have required decommissioning. Nuclear reactors have a life expectancy of 40 years [3]. According to the International Atomic Energy Agency (IAEA) there are currently 44 active reactors worldwide over the age of 40, with a further 148 reaching that age within the next 10 years [4]. With existing plans to decommission up to 80 nuclear power stations during this time-frame [5], there is considerable demand for a robotic solution to perform high risk decommissioning activities.
One of the major challenges associated with nuclear power generation is how to safely remove waste safely, as well as how to decommission the reactor at the end of its life span. This concern is born from the radioactive hazards that are inherent in the nuclear environment. As one would imagine, the process of decommissioning such a facility is generally complex and time consuming. Workers are required to wear protective Hazmat suits and often have to work in extreme environments where they experience high temperatures and humidity. This often reduces their mobility and inhibits their ability to perform complex operations. Regulations do not allow the workers to be exposed to more than 50 mSv per annum [6]. This restriction means that companies have to employ more workers per task, and that the maintenance or decommissioning process takes substantially longer than it would normally in a non-nuclear environment, leading to a substantial increase in costs. It is these factors that have led to calls for robots to play a more significant role in maintenance and decommissioning operations in hazardous environments.
The use of robots in the nuclear environment is not a new concept. Robots are being increasingly used to perform specialized tasks in hazardous and inaccessible areas of the plant; the primary use of robots to date has been to perform tests and inspections in these

2
areas. The Pegasys robot, for instance, was commissioned in 2003 to perform leak inspections of steam generator tubes using the eddy current technique [6]. While traditional human inspection methods require that the steam boilers be completely overhauled, the Pegasys robot can be used while the boiler is simply put offline, which significantly reduces downtime [6]. Similarly, the Snake-Arm Feeder Inspection Robotic Equipment (SAFIRE) was commissioned in 2010 to perform inspections on inaccessible pipework, resulting in substantial cost savings [6].
Although the majority of robots in the nuclear environment are used for inspections, a number of robots have been developed for specific applications within the decommissioning process. The BROKK B90 robot, for instance, was used to demolish the concrete reinforcement around 2 tanks during the decommissioning of the Dounreay reactors in Scotland. With a range of compatible end effectors, this robot can be used for a number of specialised tasks such as drilling, cutting and grinding [6]. The system is remotely operated and therefore eliminates the exposure of workers to radiation. The Tethered Robot Operating Device (TROD) is another general purpose robot that is used to pick up debris and decontaminate walls and floors. The system is mounted on a conveyor base and has a 6 Degree of Freedom (DOF) manipulator that is again remotely operated [7]. In a 7 month period, this robot was credited with reducing the overall exposure of workers to radiation by 1180-1960 mSV, which is the annual maximum allowable radiation dose for 24 adults [7].
Despite the advances in robotic technology and the use of robots in a number of particular nuclear applications, there are challenges that prevent robots from fully replacing humans in hazardous areas. There is major demand for a general purpose robot that is capable of performing multiple functions. The dismantling of nuclear reactors is a complex process that often requires a combination of actions such as manoeuvring, lifting, grinding and cutting. While there are robots that can perform each of these tasks individually, it is far more difficult for a robot to perform a combination of these tasks, which usually requires precise coordination of 2 or more robotic arms (e.g. holding a pipe in one hand, whilst cutting it with the other). Although such robots do exist, they often have limited manoeuvrability and lack the accuracy of specialised robots. These robots also tend to be fully dependent on the operators’ input and as the operator does not get any direct feedback from the system, even simple tasks such as unscrewing a bolt can prove difficult [7].

3
It is believed that the implementation of force feedback may help to mitigate some of these issues. Apart from the obvious application of moderating the contact force exerted on an object being gripped, force feedback can be used in a range of control architectures to perform useful tasks, while making the system less sensitive to inaccuracies in the other control loops (such as the position control loops for the joints of a robotic manipulator). This research will therefore investigate and evaluate the use of force feedback, as part of a control architecture, to perform useful tasks using the BROKK 40 decommissioning robot platform.

4
2. Objectives
The aim of this project was to develop a control strategy for the existing robotic platform such that it was capable of performing tasks that would be useful in the nuclear decommissioning environment. To achieve this aim, various objectives were set for this study. These objectives were categorised into 3 sections:
1. Consolidation of previous research
2. Main research objectives
3. Additional research objectives
The first phase of this study was to consolidation of the research conducted by P. Besset in 2013. While this involved testing the previously implemented control architecture and making the necessary modifications, the main objective during this phase of the project was to build a low-level control architecture for the left manipulator (which had not been done in the previous study), as well as to optimise the low-level control loops for both the right and left HydroLek manipulators.
The main objective of this study which was to implement a control architecture using force feedback, which could be used on a general purpose robot to perform useful tasks. The author of this study decomposed this objective into the design and implementation of a simple low- level force controller, followed by the high level hybrid force/position controller. This was done to modularise the work done in this study, which would make it easier for future studies to reuse sections of this project. The controllers were required to be implemented on a general purpose robot and be used to perform tasks that would be useful in the nuclear decommissioning environment.
The final phase of the project was to design and implement a high-level impedance controller on the general purpose robot platform. This objective was outside the primary scope of the project and was only attempted once all the other objectives had already been accomplished. Once again, the controller was required to be implemented on a general purpose robot and be used to perform tasks that would be useful in the nuclear decommissioning environment. As not a lot of time was allocated to this objective, this controller was considered as a foundation on which to build for future research.

5
3. Literature review
3.1 Control architecture
The control of robotic arms presents an interesting challenge. While intuition may lead a designer to build a position control architecture, as has been done in many other robot applications, this would result in a system that is only useful for a specific application within a controlled environment. The problem with this method can easily be visualised if one considers the challenge of holding an egg, as seen in Figure 2. Cases one and two demonstrate that the position of the manipulators can easily result in an object, in this case the egg, being either crushed or dropped. Only in the third case are the manipulators perfectly positioned, providing sufficient contact force to prevent the egg from falling.
While this may be an exaggerated example, it clearly displays the flaws of pure position control, which is the method currently implemented on the robot platform. Position control will only work in controlled environments using idealised control laws and where the exact geometry of the object is known. In most other cases, pure position control would result in extremely large contact forces being generated, which may either break or deform the object being manipulated [9]. It is apparent that some level of force control needs to be implemented.
The following sections will therefore evaluate the various methods of control that have been identified using the available literature on the topic.
3.1.1 Master/Slave control
The master/slave controller was as one of the earliest solutions to the problem of controlling cooperating robotic manipulators. In this instance the designer assigns an independent control strategy to each of the manipulators, with one selected as the master controller, and the other as the slave [10]. The master is generally based on position control, while the slave is based on force control and there is no feedback between the two. In theory, the master moves to the
Figure 1: Problems relating to pure position control

6
position set point while the slave’s response maintains the requested force set point such that the object being lifted is not dropped. However, this is an idealised principle that requires the slave to move instantaneously with the master. In practise, it has been found that the master will move first, which results in the slave’s force controller over-reacting. The slave controller would require low impedance to follow the master, which adversely makes precise force control more difficult to achieve [11]. The experimental results of this architecture show that any acceleration of the master leads to a large force error, while any change in the direction of the master may lead to a significant drop in the applied force [10], [12]. The result of these extremes in the contact force is that the object being carried may be crushed or dropped.
These issues were also similarly encountered by Clark and Stark (1986) in their attempt to use a master/slave control strategy for a two-armed simulated robot which was required to move a load across an angular plane [12]. It was found that the slave controller was simply to0 slow to react to the output of the master. The performance of the slave was improved by detuning the master, however it was determined that the entire system was now too slow to perform any useful actions. The study then identified the lack of feedback to the slave arm regarding position error as a key issue. By implementing a cross filter that passed position information to the slave, the overall performance of the system was significantly improved [12].
It is apparent from the above literature evaluation that a master/slave control architecture has a number of limitations. The large variations in the force error of the slave result in extreme contact forces being generated. The system must also be carefully set up for the specific dynamics of the object being manipulated, inherently resulting in a system unable to adjust easily to changes in the environment/object. The method would therefore only be suitable for applications where the object being carried is compliant enough to dampen the instability of the system (e.g. a sponge). Although Clark and Stark (1986) were able to achieve reasonable system performance, this was largely due to the use of a filtered feedback loop that was added to the slave. From this evaluation, it is hypothesised that optimal system performance can be achieved using a control architecture that supplies both of the manipulators with force and position feedback.

7
3.1.2 Hybrid Force/position control
Robotic tasks can usually be described using time varying components of position, force and compliance, as can be seen in Figure 3 [11]. This has led a number of researchers to believe that a hybrid control architecture that combines these components can be used for the successful coordination of the robotic manipulators. Hybrid control schemes have traditionally been implemented by deconstructing the robot workspace of each arm into purely force controlled directions and motion controlled directions [13]. However, this can only be found in an ideal system. Many hybrid architectures now describe force and motion control in the same direction [13]. Uchiyama et al. (1987) successfully implemented such a system, where the architecture was based on the simultaneous control of the force and position of the robot manipulators. The aim of the study was to develop an architecture that allows the low compliance needed for position control, as well as the high compliance required for force control. The study defined a set of unique and common workspace coordinates for each manipulator [11]. This method of creating unique and shared workspaces meant that the robotic arms were equally well suited to operating independently (e.g. moving aside pieces of rubble), as they were functioning cooperatively (e.g. hold and moving a large object). The forward kinematic equations for the robot are used to derive the Jacobian matrix of the system. Although the literature discusses a method of using the principle of ‘virtual work’ to derive the function describing the generalised force vectors, an actual force sensor was used in this experiment.
Figure 2: Example showing the time varying components of position, compliance and force [11]
This control architecture was implemented on a 4 DOF dual arm robot and was evaluated based on the robots ability to perform 3 basic actions: grasping, pulling and pushing. The controller was found to successfully maintain simultaneous position and force control, as can be seen on the respective graphs in Figure 4 [11]. In general, experimental results have shown the hybrid controller to have lower error, better set point tracking and faster response than its Master/Slave counterpart [10].

8
Figure 3: Force and position plots using the hybrid controller [11]
There are a number of minor variations of the hybrid force/position control scheme. Kopf and
Yabuta (1988) developed a similar architecture, except that the position error was defined
based on the relative position of both the robot manipulators [10]. This was done to ensure
that the centre of the object being manipulated was always placed on the set point. A further
benefit of this method is that there is full cooperation between the manipulators’ movements.
To evaluate this controller, the system was excited with a 5 Hz sine wave, as well as a step
input. While the results of this controller show significant performance improvements
compared with a Master/Slave controller, questions must be asked about the suitability of this
method to real world applications. As both the arms share the position error, their movements
will tend to be coordinated, making it difficult for the robotic arms to accomplish certain
independent tasks simultaneously. Manoeuvring within a crowded environment may also
prove to be a challenge for the operator of the robot.
The chief advantage of this architecture is its ability to control position and force in the same
workspace direction. The results of experimental comparisons also show that the hybrid
control architecture offers faster responses than the Master/Slave strategy, which is
unsurprising given the use of both force and position-error to control each of the
manipulators. This architecture also ensures that the system is robust to environmental
disturbances and to the type of object being manipulated. The method of using unique and
shared workspaces for each manipulator, as explored by Uchiyama et al. (1987), also allows
the robotic arms to be used to perform independent tasks, which increases the flexibility of
the system. The main challenge with regards to the use of this method is the complexity of
the architecture. Three different workspace coordinates, as well as the algorithms required for
the combined force/position control need to be defined.
3.1.3 Impedance control
Most control strategies that are developed for the coordinated control of dual manipulators
are based on the hybrid force/position control architecture. This can be attributed to the fact
that force and position are the two most intuitive parameters used to describe a robotic task.

9
However, these methods do not take into account the interactions of the object/manipulators with the environment, resulting in unwanted forces along the position control direction, and unwanted movement along the force control direction.
Impedance control allows designers to regulate the ratio of force to position control of the manipulators [13], [14]. Thus if there is a deviation in the position control of a manipulator, the impedance of the system can be used to limit the contact force [15]. In this way, the behaviour of the system to objects can be controlled such that the system has high compliance except in the direction where high compliance is required (e.g. in the direction that an object is being pushed).
Jung et al. (2004) developed an impedance control algorithm for force tracking that was designed for simplicity, as well as robustness to uncertainties in both the robot and environmental models. The model worked using an adaptive gain to minimise force errors in an unknown environment [14]. Gains for the inertia, damping and stiffness of the systems are selected to achieve the desired dynamic response. Independent algorithms were used for when the manipulators where in free space and in contact with the object. The impedance controller was successfully implemented on a single-arm robot which was required to do wall tracking on a flat, sinusoidal and irregular shaped wall. Good force tracking was achieved even when the stiffness of the wall was suddenly changed [14]. The adaptability of this system to variations in the environment makes this method particularly attractive for nuclear decommissioning where the environment is often unknown. The one concern about this method, however, is that it has not yet been tested on a dual arm robot.
Anderson and Spong (1987) developed a method of incorporating impedance control into a traditional hybrid force/position control by considering the environment as either capacitive or inertial. Using the principle of duality, the impedance control element was modelled as the inverse of the environment [16]. Although this method was successfully implemented, there are a number of issues that make it unsuitable to this study. Firstly, this method requires that the environment be modelled correctly before the controller is built. While this is reasonable for a robot in a fixed environment, this does not seem logical for a general purpose, nuclear decommissioning robot that will regularly be in unknown environments. The method used in this approach also seems unnecessarily complex; it is more logical to have fixed and transitional impedance behaviour depending on the task the robot is performing, rather that modelling the environment - which may contain both inertial and capacitive elements.

10
3.1.4 Conclusion
From the review of the above literature, it is apparent that the current control architecture of the robot platform can be improved through the implementation of force control. Three different methods of force control have been investigated in this literature. Although simple to implement, the literature indicates that the Master/Slave controller should not be implemented. This is predominantly due to the lack of position control of the slave-arm leading to extremes in the generated contact forces. Both the Hybrid force/position and the impedance controllers seem to be suitable for use in a general nuclear decommissioning robot. While the impedance controller is more versatile and has better disturbance rejection, which is important in an unknown environment, it is far more complex than hybrid controller. As such, the primary focus of this study was to implement and evaluate a hybrid force/position controller. The remaining time was then used to implement a basic impedance controller, which was then evaluated.

11
4. Hardware description
4.1 Introduction
Before any new research can be conducted, it is important to gain a full understanding of the existing hardware platform. In this section, the author will therefore provide a detailed description of the current condition of the MARS-ND with a specific focus on any limitations that may adversely influence the stated goals of this project.
4.2 Hardware description
The MARS-ND is composed of 2 sub-systems: the Brokk 40 base and the HydroLek-HLK- 7W robotic arms.
4.2.1 Brokk 40
The Brokk 40 (Figure 4) is the smallest model in the Brokk range, and was originally designed to carry out demolition work in small, inaccessible locations [8]. The system is powered using a 4 kW electric motor and has an hydraulically actuated 5 DOF arm with a reach of 2.4 metres and a turning radius of 245 degrees [17]. Although normally equipped with a hammer tool, this has been replaced with the HydroLek sub-system.
Figure 4: Brokk 40 demolition robot [17]
4.2.2 HydroLek-HLK-7W robotic arms
The HydroLek subsystem is made up of dual 7 DOF manipulators, as seen in Figure 5. This hydraulically actuated system was selected due to its flexibility and versatility at performing a number of different tasks and operations, which is important in the nuclear decommissioning environment. Each arm consists of six rotational joints, as well as a gripper [18]. The first five rotation joints are each fitted with a potentiometer to provide position

12
feedback. Joint 6 (wrist yaw) and joint 7 (gripper) are currently used in open loop. Each arm
has a reach of 1.5 metres and supports a maximum hydraulic pressure of 210 bars [17].
Figure 5: HydroLek dual manipulators and spool valves [17]
The HydroLek manipulators have been mounted onto the Brokk 40’s hydraulic arm and are
actuated using a common hydraulic system. The actuation of each joint is controlled using
proportional solenoid operated spool valves, which regulate the flow of hydraulic fluid to the
actuator and therefore control the position of the joint [8]. These spool valves are mounted on
the rear of the Brokk, as seen in Figure 5. The flow rate Q delivered by the each valve is
given as follows:
푄 = 푓푣푎푙푣푒 ∗ (퐼푐표푖푙) 1 [8]
Where:
 푓푣푎푙푣푒 is a valve specific constant
 퐼푐표푖푙 is the input current
A proportional amplifier is used to derive the input current 퐼푐표푖푙 from the control input using
the following formula:
퐼푐표푖푙 = 푓푎푚푝 ∗ (푈푐퐹푃 ) 2 [8]
Where:
 푓푎푚푝 is an amplifier specific constant
 푈푐퐹푃 is the control input

13
4.2.3 Control interface
Control signals are communicated to the HydroLek manipulators using a National Instrument (NI) Compact FieldPoint (cFP) controller and its respective Analog Input (AI) and Analog Output (AO) modules, as seen in Figure 6. The cFP is an industrial standard Programmable Automation Controller (PAC), which offers Programmable Logic Controller (PLC) performance while still interfacing with a general purpose PC and the National Instruments LabVIEW software. This means that control algorithms with high functionality and performance can be implemented, while still having the flexibility to interface with 3rd party software, such as MATLAB. The cFP interfaces with the operator’s PC via Ethernet.
Figure 6: cFP and power supply [17]
4.3 Hardware limitations
Hardware limitations are one of the biggest risks in a practical based research project. Although a number of hardware issues had been previously identified and appropriately planned for, various new issues have since been identified and diagnosed by the author. This section will provide a detailed description of all known hardware issues and their effect on the objectives of this research.
4.3.1 Hydraulic fluid leaks
One of the longest standing issues with the MARS-ND is the leaking of hydraulic fluid. This is due to both the condition of the hydraulic hoses, as well as damage to hardware components. While the leaking of the hydraulic fluid does not seem to adversely affect the performance of the HydroLek manipulators, it does mean that the hydraulic fluid must be routinely replenished. Unfortunately, there is no expertise within the department to perform the required maintenance to eliminate these leaks. This problem will therefore need to be managed for the foreseeable future.

14
Figure 7: Damage to hydraulic hose
4.3.2 Insufficient hydraulic spool valves
As mentioned previously, each joint of the HydroLek manipulators is controlled using a proportional solenoid operated spool valve. The HydroLek manipulators have seven joints on each arm, and therefore require fourteen spools in total. Unfortunately, there are only thirteen spools available on the existing manifold which cannot be expanded. The result of this limitation is that the gripper on the left manipulator is currently not functioning.
This obviously represents a significant challenge for performing useful tasks using the coordinated control of both manipulators, which is one of the stated goals of this project. Experiments will therefore need to be designed such that the left gripper is not needed, or can be set to a fixed position. This means that complex coordinated actions will not be considered during this study.
4.3.3 Overload relay trip
The author has noted that the MARS-ND system regularly experiences safety trips during normal operation. Further investigation has revealed that these trips are caused by an overload breaker trip on the Brokk 40. The breaker eventually resets once it has cooled down, which can often take in excess of 40-60 minutes. Although the actual root cause of the overload breaker trips has yet to be determined, the author has identified that an incompatibility of the Brokk with the HydroLek manipulators as the most likely explaination. The Brokk 40 is certified to carry a maximum load of 60 kg, yet the mass of the HydroLek dual manipulators is 95 kg. While this may not cause any issues over short periods of time, it is believed that this may result in excessive heating/current draw during continuous running, leading to the safety trips.
Although this issue does not affect the performance of the MARS-ND, it does mean that all activities on the system need to be carefully planned in order to reduce continuous running. It

15
is also recommended that a long term solution be investigated, although that is outside the scope of this project.
4.3.4 Valve stiction
During the PID loop tuning, the author identified the presence of valve stiction in a number of joints of the HydroLek manipulator. Stiction is defined as a valve characteristic where the smooth response of the valve to a changing input is preceded by a sudden movement called a slipjump [19]. Stiction is caused by the rise in the static friction coefficient of the valve mechanism. The result is that the control input needs to rise to a point where there is sufficient force to overcome the static friction in the mechanism. Figure 8 shows the typical effect of stiction on the output of a valve. As can be seen, the value responds in sudden and abrupt steps as the control input builds up to the point when it overcomes the friction force. It should be noted that stiction normally only takes effect from a stationary valve position, or where there is a change in direction. Once the valve is in motion, stiction is usually no longer a factor.
Figure 8: Effect of stiction on a control valve
Figure 9 shows the effect of stiction on joint 5 of the right manipulator. As can be seen, the process variable (in this case the measured joint angle) is steady or moving in the opposite direction to that commanded by the control input. The control input then builds up to the point where it can overcome the static friction in the joint mechanism, resulting in a sudden and aggressive movement. This aggressive movement often leads to overshoot and, therefore, poor setpoint tracking. To compound this issue, there seems to be damage to the valve

16
mechanism of joint 5 that prevents it from shutting off completely. This means that the joint cannot be held in a specific position and will always drift towards its mechanical limit.
Figure 9: Stiction in joint 5 (right)
Figure 10: Stiction in joint 4 (right)
Stiction has also been found to affect joint 4 of the right manipulator. Figure 10 shows oscillations of the measured position of joint 4. The control output for of this joint has an inverse response as it attempt to counteract the oscillations. Despite experimenting with numerous PID gain configurations, the author was unable to eliminate this response. The likely cause of this oscillation has therefore been attributed to stiction in the joint, albeit to a much less severe extent than joint 5.

17
The effect of stiction poses a major risk with regards to achieving the stated goals of this project. As previously stated, setpoint tracking is essential for the coordinated control that is being attempted in this study. In the case of joint 5, the combined effect of stiction and the mechanical damage to the valve means that setpoint tracking is not possible. Experiments will therefore need to be designed such that setpoint tracking is not vital for this particular joint, or where the joint can be set to its mechanical limit. This will obviously limit the complexity of the experiments that can be attempted during this study.
In the case of joint 4, the oscillatory behaviour caused by valve stiction can be eliminated by shutting off the hydraulic flow to the actuator once a position within an acceptable tolerance has been reached. This will obviously mean a slight reduction in the accuracy of the joint, but should be more suitable for performing coordinated tasks.
4.3.5 Valve drift
During the optimisation of the force control loop, the author discovered the presence of PV drift in joint 7 (gripper) of the right manipulator. Figure 11 shows the results of a quick experiment done to confirm the drifting of the joint. During the experiment, the gripper was required to reduce the force exerted on an object until the force went below 10 N. The shut- off valve of the gripper was closed, which would in theory mean that the gripper would maintain a 10 N contact force. However, as can be seen from the graph, there is a continuous reduction in force as the gripper drifts into its open position.
Figure 11: Valve drift in joint 7

18
The presence of drift indicates that there is either damage to the joint/valve mechanism, or that maintenance action is required on the shut-off valve. Unfortunately, as the joint/valve could not be replaced or serviced during the time frame of this study, there was no means of eliminating the joint drift behaviour.
The presence of drift in joint 7 means that there will be some oscillation in the closed loop performance of the force controller. Although not ideal, this behaviour is acceptable for this study, which is primarily a proof of concept exercise. The author had also initially planned to run experiments to evaluate the disturbance rejection of the force controller by comparing it with the performance of the open-loop control of the gripper. This will, however, not be possible due to the presence of drift in the joint.
4.3.6 Asymmetric joint behaviour
During the optimisation of the low level control loops of the left manipulator, the author discovered that each of the joints had an asymmetric response (to varying levels) based on their direction of travel. This behaviour can be clearly seen in Figure 12 where set point tracking is achieved for decreasing set points, while a large steady state error exists for increasing set points.
Figure 12: Asymmetric response for joint 2 of the left manipulator
This behaviour is caused by different friction coefficients in the joint based on the direction of travel. Although differences in the friction coefficients are found in almost all joints of this type, the author believes that the differences are particularly severe in this case due to the general lack of use of the left manipulator. As joint 7 of the left manipulator cannot be used, most experiments are conducted solely using the right manipulator (as in the previous study).

19
It is believed that the settling of dirt and the drying of lubricating fluid during the months have disuse have led to the vast differences in dynamics of left manipulator. It is therefore believed that a service to the joints of the manipulator would reduce this disparity in performance. However, due to the limited timeframe of this study, the asymmetric performance of the joints cannot be eliminated.
The asymmetric behaviour of the joints of the left manipulator poses a major risk with regards to achieving the stated goals of this project. As previously stated, setpoint tracking is essential for the coordinated control that is being attempted in this study. Due to the above limitations, experiments will to be designed such that setpoint tracking is not vital for the affected joints. This will obviously limit the complexity of the experiments that can be attempted during this study.
4.4 Conclusion
This chapter has described the current state of the MARS-ND, including the various hardware limitations that have been discovered by the author. The effects of these limitations on the study have also been discussed, with a number of mitigating actions suggested. The chapter will be referred to at various points in this report to explain experimental results, or to assist the reader in following the decision making process of the author.
With the current state of the hardware understood, the next chapter will describe the elements of the previous research on the MARS-ND that will be re-used during this study.

20
5. Consolidation of previous research
This chapter will evaluate the study conducted on the MARS-ND by P. Besset (2013). Besset developed a position control system for the HydroLek manipulators using an inverse kinematic control algorithm. Various elements of Besset’s research will be used during this study. This section will therefore provide a brief overview of the work done by Besset. The author will then critically evaluate and discuss the limitations of the relevant sections of the previous study, before detailing the improvements or corrective action that have been carried out for this study. This is done to ensure that the software platform and control architecture are functioning correctly and to identify changes that must be made to in order to achieve the objectives of the current study.
5.1 Overview
The main objective of Besset’s research was to develop an inverse kinematic model for the position control of the HydroLek manipulators. To achieve this objective, the Besset developed a PID low level control system for the angular position control of each joint of the dual manipulators. After proposing a kinematic model, the forward kinematic equations were developed which allowed the orientation and position of the end effector to be calculated. The inverse kinematic model was then resolved using the iterative Jacobian transpose method. This allows users of the MARS-ND to derive the joint angles required to reach a specific end effector position and orientation.
5.2 System architecture
Due to the computationally expensive inverse kinematic algorithm, the Besset decided to split the system architecture as seen in Figure 13. A Matlab script file is used to convert the user specified end-effector trajectory into the relevant joint angles using the inverse kinematic algorithm. These joint angles are then used as input setpoints for the low level PID controller, which is run in the LabVIEW software environment. The LabVIEW software then sends the output of the PID algorithm to the cFP via Ethernet. The cFP is used to interface with the proportional solenoid-operated spool valves, which in turn controls the hydraulic actuators on each joint. The cFP is also used to feed back the potentiometer readings to the PID controllers.

21
Figure 13: Control system architecture
5.3 Low level control
The first task in Besset’s study was to develop the low level control system for the joints of the HydroLek manipulators. PID was selected as the closed-loop controller due to its simplicity and ease of implementation. The controller was implemented on the first five joints of each manipulator, as these are the only joints that are equipped with potentiometers for position feedback. Joints 6 and 7 on both arms were therefore driven in open loop.
5.3.1 Limitations
The author of the current study identified a number of issues with Besset’s implementation of the low level controllers. The biggest concern is with regards to the selection of gain values for the PID loops. As the low level control system was not a key project deliverable and due to a shortage of time, arbitrary gain values were selected. Although this meant that the performance of the low level control was poor, it still proved to be sufficient for achieving the stated goals of the research. However, the present study requires a high level of accuracy in the low level control to perform coordinated tasks using both manipulators. The PID gain values will therefore need to be optimised using either mathematical methods, or by trial and error.
There is a further issue with regards to the way that the PID algorithm has been implemented. Figure 14a shows a typical implementation of PID, where the Process Variable (PV) reaches the Set Point (SP) after a characteristic period of time. However, Figure 14b reveals the actual response from the implemented PID algorithm which shows that the PV settles within

22
a certain tolerance percentage of the SP. As the PV reaches the tolerance value, the hydraulic valve of that joint is immediately shut-off. Once all the joints have reached a percentage tolerance of the SP, a new iteration begins which allows the manipulator to move to the next point of the trajectory. This method explains how Besset was able to achieve satisfactory closed-loop performance, despite selecting arbitrary gain values.
Figure 14: Normal step response (a) vs. implemented step response (b)
There are a number of issues with this method. The most obvious problem is that there is no SP tracking and the tolerance errors may compound at each joint to cause a significant position error at the end-effector. Another issue is the mechanical shock to the system that is caused by suddenly shutting off a valve while the HydroLek arms are still moving at significant speed. It must be noted that the HydroLek manipulators have a mass of 95 kg and therefore a high inertia. The shaking and vibrations caused by suddenly shutting off a valve can be a large source of disturbance to the control system, and may also lead to hardware fatigue and failure in the long term.
5.3.2 Corrective action
The above issues will need to be addressed in order to achieve the objectives of the current study. Sufficient time has therefore been allocated to the low level PID tuning where the parameter gain values for each joint will be optimised. This process will be covered in greater detail at a later stage in this report.
The use of SP tolerances has also been removed from the low level PID algorithm. Each joint is tuned such that it achieves SP tracking, which should minimise the overall position error of the end-effector. After a fixed time interval, a new iteration begins which allows the manipulator to move to the next point of the trajectory.
SP
PV
PV
Tolerance
SP
(a)
(b)

23
5.4 Simulation program
One of the objectives of Besset’s study was to develop a simulation program on which to test both the kinematic and the inverse kinematic control algorithms. Figure 15 shows the interface for this program. The program can be run in two modes. In the manual mode, the user is able to select the desired configuration by setting the angle of each joint. In the second mode, the user is able to specify a desired end-effector position. The inverse kinematic model is then used to set each joint angle such that the desired end-effector position is achieved.
Figure 15: Simulation program for the MARS-ND
5.4.1 Limitations and improvements
The main limitation of the simulation program identified by the author is that it is designed for the control of one arm at a time, as can be seen from the single set of input parameters on the user interface. The software therefore needed to be modified to allow for the simultaneous control of both manipulators. This is an important feature for planning trajectories whilst performing tasks that require the coordinated control of both manipulators.
Figure 16 shows the modified interface for the MARS-ND simulation program. As can be seen, the user can now set trajectories for both manipulators simultaneously. This allows the user to test the planned trajectories for coordinated tasks in a simulated environment and therefore avoid potential collisions between the manipulators.

24
Figure 16: Modified simulation program for the MARS-ND
5.5 Conclusion
This chapter has provided a summary of the previous research on the MARS-ND conducted by P. Besset (2013), with a particular focus on the elements of the research that are relevant to the current study. The author has conducted a critical evaluation of relevant element, with a number of limitations being identified and discussed. The various improvements and corrective actions implemented by the author have also been detailed.
Having completed the evaluation of the existing hardware and software platforms, the following chapters will describe the methodology followed, and the experiments completed, to achieve the various objectives of this study.

25
6. Low level control
This chapter will describe the low level control system that is used in this study, with a specific focus on the control loop parameter optimisation. As mentioned in 5.3, P. Besset implemented a PID low-level control system during a previous study. However, the implemented system was not optimised as the control gain values were selected arbitrarily. This chapter will therefore provide a detailed description of the methodology used by the current author for the optimisation of each of the low level control loops. The author will then critically evaluate the results of the control loop optimisation, before concluding remarks are made.
6.1 System model
Figure 17 is a generic representation of the open loop control of a joint of the HydroLek manipulator. As can be seen, each joint can be modelled as an integrator. The response of the system to a step input will therefore be a ramp signal, as seen in Figure 18. This means that a step input to the joint of the manipulator results in either a positive or negative angular velocity of that joint. The gradient of the ramp signal (or angular velocity) changes in proportion to the gain applied to the step input. Higher gains lead to steeper gradients, and therefore more rapid motion of the joint. In order to achieve position control for the joint of the manipulator, a feedback controller will need to be implemented.
Figure 17: Generic model of each joint of the HydroLek manipulator
Figure 18: Response of a generic joint to different step inputs

26
6.2 PID algorithm
The purpose of the low level control system is to actuate the joints of the HydroLek manipulators using inputs specified by the user. One of the most popular low level control systems is the Proportional Integral Derivative (PID) algorithm. Although this system dates back to 1910 [20], it is still widely used in industry due to its robustness to disturbances and changes in system dynamics.
Figure 19: Block diagram of generic feedback loop
Figure 19 shows the block diagram of a typical closed loop system where yd(t) is the system input, y(t) is the system output, D(t) are the disturbances that influence the system and e(t) is the control error. The PID algorithm minimises the control error (e), which is the difference between the output (y) and the input (yd) of the system, by implemented proportional, integral and derivative action.
6.3 Methodology
Although there many different methods for tuning PID controllers (such as Ziegler-Nichols and Cohen-Coon), the trial and error approach has been selected for this study. There are a number of reasons that led the author to this decision. Firstly, the model of the manipulator in 6.1 describes each joint as an integrator. This inherent integral action means that, contrary to most systems, setpoint tracking may be achieved by implementing pure proportional control, as seen in Figure 20 and Figure 21. Since pure proportional control may be sufficient for achieving set point tracking in most cases, it seems logical to choose the most simple and flexible tuning method, which is the trial and error approach.
Figure 20: Proportional controller for a joint of the HydroLek manipulator

27
Figure 21: Closed loop response for the proportional controller
6.3.1 Software setup
In order to optimise the PID gain values of all the low level control loops, a number of changes need to be made to the routines and sub-routines developed by P. Besset (2013). The changes, made by the author, to these files are described as follows:
ReadAll.vi
The ReadAll.vi subroutine is used to feedback all the sensor readings from the right manipulator, back into the PID control blocks. The sub-routine has now been replaced by the ReadAll_pcM.vi sub-routine, which retrieves the sensor readings simultaneously from both manipulators. This allows both manipulators to be controlled simultaneously, which is important for achieving the objectives of this study.
SubRightArm.vi
The SubRight.vi subroutine is used to implement the closed loop control for the joints of the right manipulator. This subroutine receives the user set points and sensor feedback values as inputs to the PID blocks. The calculated control actions are then sent to the joint actuators via the WriteAllM.vi subroutine. The subroutine has now been replaced by the SubBothArms.vi, which is used to implement closed loop control on the joints of both manipulators.
ParseFile.vi
The ParseFile.vi routine is the primary interface with the user. The user is able to input set points and PID gain values, while monitoring the closed loop performance of each loop. The routine has now been replaced by the ParseFileAll.vi routine, which will allow the user to set trajectories and gain values for both arms simultaneously.

28
6.3.2 Loop tuning
Having made the necessary changes to the relevant routines and sub-routine, the optimisation of the low level controllers was begun with each joint being tuned individually. The first step of the optimisation process involved selecting a suitable trajectory. Figure 22 is an example of such a trajectory, with each column corresponding to set points for joints 1 to 7. As can be seen from the figure, the joint of interest (joint1 in this case) receives step inputs of varying magnitude and in different directs. This allows the author to evaluate the performance of the loop across its operating range.
Once the trajectory has been specified, the loop tuning process was begun by selecting an arbitrary proportional gain value. The closed loop response was then evaluated, with the gain being increased if the response was too slow, or decreased if the response was oscillatory. As previously mentioned, all the joints of the manipulator can be modelled as integrators. As such, the use of Integral gain will only be considered where there is significant steady state error.
Figure 22: Example of a trajectory used for loop tuning
6.4 Results and discussion
The following section will review and discuss the results of the loop optimisation process for each joint of both the right and the left manipulators.
6.4.1 Right manipulator
Joint 1
Table 1 gives a summarised list of gain values that were evaluated during the optimisation of the joint 1 PID controller. While this list does not contain all gain values evaluated, it does allow the author to explain the process followed in selecting the optimum gain value. Figure 23 shows the closed loop response of joint 1 to these various proportional gain values. One can see that set point tracking has been achieved for all the responses, with the main differences between the responses being the settling time and the level of overshoot. It can be seen that the closed loop response for gain values of 1.2 and 1.5 are the two slowest responses, while the response for a gain value of 2.5 has the largest overshoot. The response

29
to a proportional gain of 2 seems to provide the best balance of fast settling time and small overshoot, and has therefore been selected for use.
Table 1: Summarised list of gain values that were evaluated for the joint 1 of the right manipulator
Experiment No.
Proportional Gain
Integral Gain
Derivative Gain
1
1.2
-
-
2
1.5
-
- 3 2 - -
4
2.5
-
-
Figure 23: Closed loop response of joint 1 for different gain values
Joint 2
Table 2 gives a summarised list of gain values that were evaluated during the optimisation of the joint 2 PID controller. As previously mentioned, this list does not contain all gain values evaluated, but rather a select set used to explain the final selection of the gain value. Figure 24 shows the closed loop response of joint 2 to the various proportional gain values. One can see that, once again set point tracking has been achieved for all the responses, with the main differences between the responses being the settling time and the level of overshoot. Apart from the response for a proportional gain of 5, the settling time for the remaining gain values are similar. Figure 25 shows a magnified view of the different responses. From this figure, one can see that a proportional gain of 12 leads to some overshoot, which may make it difficult to achieve the level of control required for coordinated tasks. As such, the author

30
selected a proportional gain value of 10 due to its balance between fast settling times and small overshoot.
Experiment No.
Proportional Gain
Integral Gain
Derivative Gain
1
5
-
-
2
8
-
- 3 10 - -
4
12
-
-
Figure 24: Closed loop response of joint 2 for different gain values
Figure 25: Magnified view of closed loop performance for joint 2

31
Joint 3
Table 3 is a summarised list of gain values that were evaluated during the optimisation of the joint 3 PID controller. Figure 26 shows the closed loop response of joint 3 to the various proportional gain values. One can see that, once again set point tracking has been achieved for all the responses. Apart from the response for a proportional gain of 12, the settling time for the remaining gain values are similar. Figure 27 is a magnified view of the different responses. From this figure, one can see that all three of the remaining curves have some level of overshoot, with the curves for gain values of 12 and 40 being the worst. As the two remaining responses have similar levels of overshoot, 30 was selected as the proportional gain value due to its marginally faster response.
Experiment No.
Proportional Gain
Integral Gain
Derivative Gain
1
12
-
-
2
20
-
- 3 30 - -
4
40
-
-
Figure 26: Closed loop response of joint 3 to different gain values

32
Joint 4
Table 4 is a summarised list of gain values that were evaluated during the optimisation of the joint 4 PID controller. Figure 28 shows the closed loop response of joint 4 to the various proportional gain values. Although set point tracking has been achieved by each response, it can be noted that there are oscillations present on all the curves. This can be seen more clearly in Figure 29, which shows a magnified view of the curves. The reason for these oscillations has been covered in 4.3.4. From a performance perspective, a proportion gain of 8 leads to the slowest settling time and is therefore discarded. Figure 29 shows that the proportional gain values of 30 and 20 both lead to overshoot, despite not significantly reducing the settling time. For these reasons, the proportional gain value of 12 was selected for use in this study due to its balance between fast settling time and small overshoot.
Experiment No.
Proportional Gain
Integral Gain
Derivative Gain
1
8
-
- 2 12 - -
3
20
-
-
4
30
-
-

33
To overcome the oscillations found in all the closed loop response curves, the author implemented a set point tolerance which is used to close the shut-off valve to the joint actuator. The author implemented this technique on the loop using a proportional gain of 12, with the resulting curve seen in Figure 30. Although this method means that there will always be some set point tracking error, the performance of the curve was deemed suitable for this study.

34
Figure 30: Closed loop response of joint 4 with set point tolerances implemented
Joint 5
Table 5 is a summarised list of gain values that were evaluated during the optimisation of the joint 5 PID controller. Figure 31 shows the closed loop response of joint 5 to the various proportional gain values. It can be noted that set point tracking has not been achieved for any of the curves due to stiction of the joint, as described in 4.3.4. Figure 32 also shows the presence of drift in the joint. As a result of these issues, the author changed the evaluation criteria for this joint to values that cause the least amount of drift. It is hoped that this will still enable the joint to be used for tasks requiring coordinated control. Based on this criterion, a proportional gain value of 3 has been selected for use.
Experiment No.
Proportional Gain
Integral Gain
Derivative Gain
1
2.5
-
-
2
2.8
-
- 3 3 - -
4
4
-
-

35
6.4.2 Left manipulator
The optimisation of the low level control loops for the joints of the left manipulator followed a similar process to that of the right manipulator. Once again, each joint’s control loop was optimised individually using the trial and error approach to determine suitable proportional gain values. In order to keep this report concise, the selected proportional gain values for each of the control loops have been summarised in Table 6. The closed loop performance of

36
each of the loops has also been commented on. The graphs showing the closed loop response for each joint can be found in appendix A.
Table 6: Selected proportional gain values for the low level control loops of the left manipulator
Joint number
Proportional gain
Comments
1
40
SP tracking not achieved. Asymmetric response
2
60
No SP tracking for increasing set points. Asymmetric response
3
70
SP tracking achieved
4
90
SP tracking achieved. Asymmetric response
5
20
SP tracking achieved. Overshoot present in response
One of the main observations during the loop tuning process is that, in general, the dynamics of the left manipulator are very different from that of the right. The gain values of the left arm are far larger than those of the right due to a high level of stiction in control valves of the joints, which prevents the joints from moving in response to small OP values. Although stiction is the primary reason why set point tracking cannot be achieved for joint 1 and joint 2, this problem is compounded by the asymmetric response of the joints. The difference in response means that for the same gain values, the response may overshoot for increasing set points, but not achieve set point tracking for decreasing set points.
Although the effects of the above mentioned issues could be mitigated by implementing split range control or other forms of advanced control, this was not considered due to the limited time available for this study. The performance of the controllers was taken into account when designing experiments at a later stage during in this study.
6.4.3 Comparison with previous study
In order to evaluate the performance of the low-level optimisation, the closed loop response before and after optimisation have been compared. This has only been done for the right manipulator, which was used by P. Besset (2013) during a previous study. Figure 38 shows the closed loop response for joints 1-4 using both the old gain values, as well as the optimised values. Joint 5 has not been shown, as the proportional gain value that was selected in a previous study was already the optimum value. Each of the graphs shows that the settling time of each loop has been significantly reduced. A further benefit of the optimised loops is that the time taken for each of the joints to reach its respective set point is now very similar, compared with the old gain values where there was a significant variation in settling time.

37
This fact is especially important when performing tasks requiring coordinated control of both manipulators, which is a stated goal of this project.
Figure 33: Comparison of closed loop response before and after the PID optimisation
6.5 Conclusion
This chapter has described the optimisation of the low-level PID control loops of both HydroLek manipulators. This included a description of the methodology followed during the tuning process, as well as an evaluation of the results that were achieved. The chapter also highlights the impact of some of the hardware issues on the low-level response of the joints. The optimised closed loop response of the joints of the right manipulator has also been compared with the gain values selected during a previous study, showing significant performance improvement. Having successfully optimised the low-level control loops, the following chapter will build on this control architecture through the implementation of pure force control.

38
7. Force control
This chapter will describe the implementation of pure force control on joint 7 (gripper) of the right HydroLek manipulator. This will include a brief background on force control as well as a description of the hardware selected by the author to measure the contact force between the gripper and the object. The implemented control architecture, as well as the methodology used by the author to tune the force control loop will then be described. Finally, a results and discussions section will be used to evaluate the performance of the loop.
7.1 Background
The purpose of implementing force control on joint 7 is to regulate the contact force between an object being gripped and the gripper itself. Currently, this joint is run in open loop which means that the contact forces are very large. This limits the applications that the MARS-ND can be used for, particularly in the nuclear decommissioning environment. Implementing force control would ensure that fragile or unstable objects can be handled and manoeuvred in a gentle and controlled manner.
7.2 Hardware description
As the MARS-ND is not equipped with a force sensor on its gripper, an external sensor needed to be purchased to complete the objectives of this study. The primary requirements of the sensor are that it is low cost, has a USB interface and that it has drivers to support its use in both the NI LabVIEW and Matlab software packages. A further requirement is that the interface is expandable to support the use of other sensors in the future. As this study is primarily a proof of concept, robustness, durability of high end accuracy were not considered to be requirements.
Although various industrial grade sensor and I/O packages (such as the I/O modules supplied by National Instruments) were considered, these were found to be too expensive for the purpose of this study. The following hardware was selected and purchased:
7.2.1 Phidgets USB interface
The Phidgets USB 8/8/8 interface module was selected as the I/O package for this study. As the name implies, this unit has a USB interface and supports the simultaneous use of 8 AI, 8 DI and 8 DO channels. Each AI channel has a 10 bit resolution and offers a sampling rate of 1000 samples/sec, which are both sufficient to achieve the goals of this project. The low cost of the board, together with the above specifications, make this I/O interface module suitable for this study.

39
7.2.2 Flexiforce pressure sensor
The Flexiforce pressure sensor has a piezoresistive element which alters its resistance based on the amount of force on its sensing area. The sensor is made up of a flexible printed circuit which is coated in a thin film. This makes it easier to mount it on irregular contact surfaces such as those found on the HydroLek manipulator. The sensor offers a range between 0-444.8 N and is linear within ±3%. The low cost of the sensor, together with the above considerations make it suitable for this study.
Figure 34: Phidgets USB interface (left) and the Flexiforce pressure sensor (right)
7.3 Force control architecture
Figure 35: Block diagram of the implemented force controller
Figure 35 reveals a typical block diagram implementation of a force control loop where Fd(t) is the force setpoint, y(t) is the exerted force, D(t) are the disturbances that influence the system, e(t) is the control error and 푥̇(푡) is the angular velocity setpoint sent to the joint actuator. Due to the limited time available for this study, the PID algorithm has been selected as the controller due to its simplicity and ease of implementation.
Although this may seem like a simple control system to implement, Figure 36 illustrates the challenge of implementing force control on the gripper. Initially, the object is not in contact with the gripper. The gripper is therefore required to close to the point that it comes into contact with the object. After this point, however, even a small closing movement of the gripper results in large contact forces (assuming that the object is incompressible), as can be seen in Figure 37. This varying behaviour makes it extremely difficult to select suitable PID gain values to achieve satisfactory closed loop responses for both states.

40
Figure 36: Illustration of an object being gripped
Figure 37: Contact force vs. gripper position when gripping an object
To overcome this challenge, it was hypothesised that a split range PID controller would be able to achieve the desired closed loop response. Figure 38 illustrates the principle of a slit range controller. Range 1 occurs for all force values under the maximum expected force values when an object is not being gripped (this should take into account noise and disturbances). In this range, the high gain values that are needed for position control are used to rapidly actuate the joint. Once the gripper has come into contact with the object, and the measured force exceeds the range 1 boundary, the low gains required for force control are used to regulate the contact forces. It is important to note that this architecture still represents pure force control, as only the force set points and feedback are used to set the control output.
Figure 38: Illustration demonstrating the implementation of a split range controller

41
7.4 Implementation
This section will describe the process followed to implement and tune the force controller for joint 7 of the right HydroLek manipulator.
7.4.1 Hardware setup
The main objective with regards to the hardware setup for the force controller is to make sure that all the force exerted by the gripper is transferred onto the FlexiForce sensor without causing any damage to the sensor surface. Figure 39 shows an exploded view of the physical packaging of the FlexiForce sensor that was used to achieve this objective. Contact layer 1 is a smooth piece of metal that receives contact from the top section of the gripper. The size of this piece of metal must be large enough to completely cover the surface of the puck, while not so large that a force applied by the gripper on the edge of its surface causes it to come into contact with contact layer 2, as seen in Figure 40.
Figure 39: Physical packaging of the FlexiForce sensor
Figure 40: Illustration of the effect of having a larger contact layer 1 surface
The puck is a small cylindrical shaped piece of rubber that is stuck over the sensor surface. The purpose of the puck is to protect the sensor surface, while also ensuring the force exerted on contact layer 1 is concentrated on the sensor surface. Below the sensor is contact layer 2, which is a longer piece of metal than contact layer 1. The purpose of this layer is to provide a smooth flat surface for the sensor to be attached onto.
All the layers are firmly taped together to reduce sliding as much as possible. The sensor package is then attached onto the object being gripped such there is little or no sliding between the package and the object. This is then attached to one of the gripper claws such

42
that all contact during the gripping of the object is centred on contact layer one. This process ensures that the measurement of the contact force during the gripping process is kept in as controlled environment as possible.
7.4.2 Software setup
As previously mentioned, joint 7 of the right manipulator has been run in open loop during past studies. To introduce the control architecture described in 7.3, this open loop controller is replaced by a closed loop PID controller. The LabView routine ParseFile.vi (now renamed ParseFile_Force.vi) and its various sub-routines have been modified to implement the updated architecture. These modifications are summarised as follows:
ReadAll_pcM.vi
The ReadAll_pcM.vi subroutine is used to feedback all the sensor readings into the PID control blocks. As a new force sensor is now being used, the relevant data capture block has been added to the subroutine.
SubRightArm_Force.vi
The SubRightArm_Force.vi subroutine is used to implement the control blocks. This subroutine receives the user set points and sensor feedback values as inputs to the PID blocks. The calculated control actions are then sent to the joint actuators via the WriteAllM.vi subroutine. The subroutine has been modified to include an additional PID block for force control in place of the open loop controller that was previously used.
ParseFile_Force.vi
The ParseFile_Force.vi routine is the primary interface with the user. The user is able to input set points and PID gain values, while monitoring the closed loop performance of each loop. The key change to this routine is the additional PID gain interface for the force control of the gripper. An extra chart has also been added to monitor the closed loop performance of this loop. Finally, this file has also been updated to initialise a channel on the Phidgets Interface Module, which allows for the input capture of the force sensor readings.
7.5 Loop tuning
As with any PID control loop, it is important to select suitable gain values to achieve acceptable closed loop performance. The gain values for the force control loop were selected experimentally using the trial and error method. This method was used due to the limited time available in this study. Considering the fact that this study is merely a proof of concept, it was

43
believed that the performance achieved using the trial and error approach would be satisfactory.
7.5.1 Methodology
In order to tune the force controller, a suitable trajectory needs to be used as an input to the ParseFile_Force.vi. Figure 41 shows the trajectory used for tuning of this loop with the first 6 columns correspond to the set points for the first 6 joints or the right manipulator. The 7th column specifies the set points for the force control loop. As can be seen, the set points for the first 6 joints are kept constant during the tuning process, while the force setpoint is varied. This is done to reduce the effect of disturbances during the tuning process.
Figure 41: Trajectory used for the tuning of the force control loop
With a suitable trajectory developed, the next step is to select suitable gain values. The gain value selected for Range 1 of the split range controller is arbitrarily set to 10. As this gain is merely used to close the gripper, and not to implement any form of force control, it is not necessary to optimise this value. The process of tuning the Range 2 PID controller is begun by selecting an arbitrary Proportional gain value. The response to this gain was then evaluated, with the gain being increased if the response is too slow, or decreased if the response is oscillatory. As previously mentioned, all the joints of the manipulator can be modelled as integrators. As such, the use of Integral gain will only be considered where there is significant steady state error.
7.5.2 Results and discussion
Table 7 is a summarised list of PID gain values that were evaluated during the tuning of the force control loop. The table is not a complete list of all gain values evaluated, but rather selected values that illustrate the logical process following to optimise the control loop. From the table, it can be noted that the use of Integral or Derivative gain values was not evaluated. Integral gain was not considered as there was no large steady state error when using pure proportional control action, while there are too many sources of disturbances for the derivative action to be effective.

44
Table 7: Summarised list of gain values that were evaluated for the force controller
Proportional Gain
Integral Gain
Derivative Gain
Experiment No.
Closing gripper
Opening gripper
1
0.05
0.05
-
-
2
0.05
0.005
-
-
3
0.05
0.001
-
-
Figure 42: Closed loop response for experiment 1

45
Figure 42 shows the closed loop response of the first experiment from Table 7. It is observed that the closed loop response has severe oscillations around the set point. While oscillations are expected due to the leaking of the shut-off valve as described in chapter 4.3.5, the severity of the oscillations is an indication that the gain values are too large. Another observation from the graph is that the oscillations are asymmetrical and not centred around the set point. This is most likely due to the asymmetrical nature of the hydraulic joint which exhibit different characteristics based on the direction of motion. In this case, the closed loop response when opening the gripper (reducing the contact force) is more aggressive that the response for closing the gripper (increasing the contact force). In an attempt to achieve more symmetrical behaviour, the author implemented different gain values for opening and closing the gripper.
Figure 43 shows the closed loop response for the proportional gain values of 0.05 (closing gripper) and 0.005 (opening gripper). One can immediately see an improvement in the performance of the response compared with the two previous graphs. Firstly, the peak-to- peak amplitude of the oscillations is significantly smaller due to the smaller gain values. Secondly, the oscillations are also more symmetrical in behaviour, which indicates that the use of 2 separate PID controllers has been successful. Despite the improvement, however, the steady state oscillations are still found to have a peak-to-peak amplitude of 0.5 N and the response exhibits some asymmetrical behaviour. Both of these issues seem to indicate that the gain used to open the gripper may still be too large.
Figure 44 shows the closed loop response for the proportional gain values of 0.05 (closing gripper) and 0.001 (opening gripper). The author decreased the gain require to open the gripper in an attempt to improve the symmetry of the oscillations, as well as to reduce its peak-to-peak amplitude. The object being gripped was also re-taped to prevent any

46
movement during the gripping process. From the graph, one can see a noticeable improvement in both the peak-to-peak amplitude, as well as the symmetry of the oscillations (on average 0.08N above and 0.25N below the set point). Although the response is still oscillatory (due to the leaking of the shut-off valve), the performance of this look was considered sufficient to proceed with this proof of concept study.
7.6 Conclusion
This chapter has described the successful implementation of pure force control loop for joint 7 (gripper) of the right manipulator. This includes a description of the methodology followed, the selection of a suitable force sensor, as well as the design and implementation of a split range controller to overcome the asymmetric characteristics of joint 7. Having successfully implemented pure force control, the following chapters will build on this control architecture to implement both a hybrid force/position control loop and an impedance control loop.

47
8. Hybrid force/position control
This chapter will describe the author’s implementation of hybrid force/position control on joint 1 (shoulder) of the right HydroLek manipulator. After providing a brief background on the hybrid force/position controller, the implemented control architecture will be described. The methodology used to tune the control loop will then be detailed, followed by a results and discussion section where the performance of the loop is evaluated.
8.1 Background
While pure force or position control is required to perform specialised tasks, the sole use of either is often insufficient for performing some of the more complex tasks performed by humans. The purpose of implementing hybrid force/position control is to perform coordinated tasks where both the force and the position need to be regulated (e.g. Picking up an egg and putting it down in a specified location). This is done be decomposing the workspace into position control and force control directions. The ability to perform such tasks is especially crucial in the nuclear decommissioning environment where hazardous material would need to be handled with care while being moved from one location to another.
8.2 Hybrid force/position control architecture
As mentioned in the literature review, hybrid force/position controllers have traditionally been designed by separating the workspace into pure force and position directions, which is a scenario that is rarely found in real world conditions. As such, it was decided to develop a controller based on the work done by Uchiyama et al. where the workspace is decomposed into a set of unique and common coordinates for both force and position control elements.
However, one of the problems with implementing this architecture is that the experiments performed during the study were aimed at completing tasks where motion was unconstrained (e.g. pushing and pulling a block). While these are useful tasks, there is also a need to perform tasks where the motion is constrained and accurate force control is required (e.g. regulating the contact force while drilling through an object. As such, it was decided to modify the control architecture developed by Uchiyama et al. to achieve this objective.

48
Figure 45: Block diagram of the implemented hybrid force/position controller implemented on joint 1
Figure 45 is a simplified block diagram of the implemented hybrid force/position controller where Fd(t) is the force setpoint, Xd(t) is the angular position set point, F(t) is the exerted force, x(t) is the measured angular position, D(t) are the disturbances that influence the system, e(t) is the control error and 푥̇(푡) is the angular velocity setpoint sent to the joint actuator. As can be seen, the controller accepts both force and position set points as an input and, likewise, force and position are measure at the output of the plant. However, the workspace is decomposed such that there is only pure force or position control in one direction at any time. Figure 46 shows the decomposition of the workspace using the measured force as a discriminating criterion. For force values below a specified threshold, pure position control (large gain values) is used to actuate joint one. However, once the force builds up past the threshold values, such as when the object to be drilled is encountered, pure force control (small gain values) is used to regulate the contact forces.
Figure 46: Decomposition of workspace into position control and force control segments
This architecture allows the designer to achieve both force and position control in the same direction (although force is given priority) for tasks where the motion is constrained.
8.3 Implementation
This section will describe the process followed to implement and tune the hybrid force/position controller for joint 1 of the right HydroLek manipulator.

Integration of force feedback into a nuclear decommissioning robot

Integration of force feedback into a nuclear decommissioning robot

Recommended

Recommended

More Related Content

Similar to Integration of force feedback into a nuclear decommissioning robot

Similar to Integration of force feedback into a nuclear decommissioning robot (20)

Integration of force feedback into a nuclear decommissioning robot