1. Introduction
This paper serves as a high level overview of previous work I led involving the control law synthesis for a nonlinear, coupled, hydraulically actuated robot manipulator.
A literature survey has been performed to understand currently used techniques for hydraulic control of robot manipulators, the results of which will be outlined in section 2. In section 3 a controls strategy will be presented that has been designed in light of the literature survey, and section 7 highlights simulation results of the designed control law.
2. Literature Survey
The most common approach encountered in the literature for stationary manipulators is feedback linearization via inverse dynamics, coupled with an outer PD position control loop, and an inner torque loop.
The work regarding the HyQ robot is interesting because the authors take the time to derive relations that other papers do not.[1] In their paper, the authors used feedback linearization via inverse dynamics, with an outer position loop and an inner torque loop. High performance servo-valves with a bandwidth of ~250hz are used. The position loop is closed with 80,000 PPR encoders, while the torque loop is closed with a load-cell measuring the cylinder force. Both control loops run at 1 kHz. Joint torques are computed by multiplying the cylinder force by the actual lever arm.
Practically speaking, the torque loop was able to provide a bandwidth of 40 hz. The electrohydraulic system is modeled by employing the continuity equation at the valve + actuator sub-system level (1).
where is the fluid density, t is time, and u is the flow velocity vector field.
By applying force and flow balance, and neglecting hydraulic viscous friction, the dynamics of the hydraulic force are described as:
The valve command u is governed by a control law, v, which is chosen to be a PID control law. The torque control law uses seven states: the cylinder position, xp, cylinder velocity, , the cylinder A side chamber pressure, Pa, cylinder B side chamber pressure, Pb, the supply pressure, Ps, the tank pressure Pt, and torque, . The position control law uses only position feedback. The inverse dynamics term is governed by a reference angular acceleration (reference trajectory shaping), and feedback angular position and angular velocity. The largest control effort is exerted by the inverse dynamics term.
Feng used model predictive control with inverse dynamics and inverse kinematic high level control, with joint level torque controllers with parameters specified by Boston Dynamics. [2]
Overviews of the HyQ design used a patched real time linux kernel (xenomai), in conjunciton with Simulation Laboratory developed by the computation learning motor control lab at USC. They ultimately conclude PID control is insufficient for their control goals. [3] SL, the simulation and real-time control software package is detailed in [4].
Sirouspour et al. use the generalized actuator dynamics equation (2) listed above to model their 3-way valve dynamics. They discuss the use of an augmented error inverse dynamics back-stepping controller, as well as an adaptive variant with lyapunov based adaptation law. The controller is implemented using the matlab real time workshop toolbox with custom parallel I/O comms and a 512 hz controller frequency. Passivity and sliding observers are used to avoid acceleration measurement. The derivation of and for the actuator dynamics are included in an appendix, where the A and B pressures, and valve spool position are used as states. [5]
Cunha et al. show that a gain scheduled PID controller is insufficient for the hydraulically actuated HyQ robot leg. Actuator dynamics modelling is included which is based on Bernoulli’s equation (orifice flow rate). Proportional valves with overlap and a bandwidth of 30-40 hz were used. Proportional flow metering in the spool valves is used with a 2 kHz PWM signal. A 1 kHz control loop frequency is used. [6]
Habibi et al. used a computed torque sliding mode controller with a sampling frequency of 500 hz. Again, the largest control effort is exerted by the inverse dynamics term, as was shown with the HyQ leg. The authors conclude very good control was achieved with their approach. Pressure feedback is used and a thorough derivation of the actuator models are included. [7]
In the Matilla et al. survey, all nonlinear controllers outperformed the best linear controllers; a performance indicator, , was used to compare controls studies. The authors indicate all surveyed papers reported variable displacement pumps having a response time between 40 and 130ms, whereas cutting-edge servovalves had a response time of ~1.8 ms. The authors indicate actuator dynamics must be included in hydraulic systems as they are highly nonlinear. They list use of Lyuaponov methods, L2 and L-infinity stability analysis, sliding mode control, adaptive inverse dynamics, adaptive inverse dynamics plus PI control, model reference adaptive control with velocity measurement, observer based robust adaptive control, virtual decomposition control, inverse dynamics, etc. Using Lagrangian dynamics they show the computational burden of calculating the robot dynamics increases as . [8]
Boaventura et al. used a feedback linearization controller (inverse dynamics), with the use of a slightly modified version of the actuator dynamics equation (2) shown above, as was required for compliance control. Feedback linearization is used in conjunction with a PD position controller. The authors tuned the PD loop for a phase margin of 60 degrees for a fast, non-oscillatory response. They conclude that higher valve bandwidths are able to increase the force controller stability margins and/or the closed-loop force controller bandwidth. Both the outer impedance loop and inner torque loop were sampled at 500hz. [9]
Zhihua et al. used feedback linearization in conjunction with a sliding mode controller; they also include actuator dynamics modelling. [10]
Fochi et al. compared PID, LQR and feedback linearization with servovalves with a bandwidth of 30-40 hz. They chose a state vector composed of joint angular positions and velocities, position and velocity of both cylinder rods, solenoid currents of both valves and hydraulic forces produced by the cylinders. System ID was used to estimate flow gain, valve deadband asymmetry, etc. The valve flow gain was estimated using a sinusoidal position reference and obtaining flow as a function of cylinder velocity as measured with a linear potentiometer. Valve deadband asymmetry was assessed by measuring piston displacement in both directions and valve input voltage. Valve spool dynamics were neglected since they are faster than the leg dynamics; valve overlap was compensated for algebraically with the corresponding inverse nonlinearity. The time rate of change of the hydraulic force was related to the valve voltage, Uv, through the pressure dynamics and the valve equation listed above. Regarding this equation, they indicate that f(x) contains the effects of leakage, friction and varying capacity of the cylinder chambers, while g(x) characterizes the effects of the flow-pressure nonlinearity. The authors concluded the feedback linearization controller had by far the fastest response and was the best performing controller. [11]
Bech et al. employed the same continuity equation as others for the pressure dynamics in conjunction with the orifice equation (based on Bernoulli’s equation). The authors compared four nonlinear controllers to five linear controllers and concluded all nonlinear controllers performed much better than the best linear controller. The nonlinear controllers included a sliding mode controller, adaptive inverse dynamics controller (indirect adaptive), adaptive inverse dynamics with PI control (indirect adaptive) and model reference adaptive controller with velocity feedback (direct adaptive); the adaptive inverse dynamics and MRAC controllers performed the best. Adaptive controller parameters were determined using the nonlinear constrained optimization fmincon in the MATLAB optimization toolbox. I’m unsure of the global stability claim the authors make about the adaptive inverse dynamics controller as the parameter update law appears to be a simple gradient descent. [12]
Honegger et al. utilized a high level indirect adaptive inverse dynamics control law, with a PD position control loop and a low level force PID controller with a low pass filter. The authors discuss actuator modelling; parameter estimates are adapted via gradient descent. Again, I am unsure what stability guarantees they get as a result of using gradient descent. They discuss the potential of using a feed forward term in the PID force controller to improve tracking. The control strategy is implemented with proportional control valves (Moog DDV 633), that feature an internal valve spool position controller and have a bandwidth of approximately 25hz. The sensors are sampled at 1kHz, the dynamic model is calculated at 100hz, and parameter adaptation is occurs at 20 hz. [13]
Bonchis et al. implemented a system consisting of proportional directional control valves with a bandwidth of 6hz and a 50hz controller. The authors compared tracking and robustness of 10 different control strategies, MRAC, acceleration feedback using an experimentally identified friction model (FRID), and sliding mode controllers performed the best. Inverse dynamics apparently was not considered; no other implementation details of their controllers are given. [14]
Becker et al. model actuator dynamics and implement a sliding mode controller with a continuous approximation of the sliding surface generated by a saturation function. The authors conclude the control law is strictly passive and therefore BIBO stable. They approximate the dynamic behavior of the servo-valve with a linear first-order differential equation.
The hydraulic model is based on the commercial simulation software “hyvos 6.0” from Bosch-Rexroth. Robustness was evaluated by deviating control parameters by 20 percent. They showed that given uncertainties the output error remained within the prescribed switching surface boundary. They do not discuss implementation or sampling rates, etc. [15]
In a second paper, Fochi et al. utilize a torque controller running at 1 kHz, while an outer impedance controller runs at 200hz. Torque control is achieved via a PI actuator controller, with an outer PD position control loop and inverse dynamics torque. The servovalves used have a bandwidth of 250 hz, position and velocity feedback is achieved with 80,000 PPR incremental encoders. [16]
Peng et al. discuss hydraulic actuator mathematical modelling and PID control. [17]
In a second paper Habibi et al. discuss hydraulic actuator system modelling and use in computed torque type controllers. [18]
Koivumaki discusses hydraulic actuator modelling and virtual decomposition control. [19]
Robotic system modelling is largely referenced from [20].
Hydraulic valve coefficients and other hydraulic control esoterica can be found in [21].
Destro and De Negri discuss the use of a flow coefficient, , which is used to replace the flow gain, which is a function of orifice geometry and spool position, as the orifice geometry is not easily determined from a spec sheet. [22] This coefficient is used in the actuator derivations for the HyQ robot. [1] [3] [6]
Sohl and Bobrow designed a Lyapunov based backstepping controller for the force control of a hydraulic actuator. [23] They showed that fluid bulk modulus and valve flow parameters are important for successful control and give a method for the off-line identification of those parameters. The authors also show that friction can have a large effect on tracking accuracy. The closed loop system bandwidth in their experiments was roughly 10 Hz, which allowed them to assume that the control, u, applied to the spool valve is directly proportional to the spool position, i.e. the dynamics of the valve motor / flapper are fast enough to be neglected. They indicate that in their literature review sensors used to measure the spool valve position had only a minimal performance improvement for position tracking. Their system states of cylinder position, cylinder velocity, A chamber pressure and B chamber pressure were sampled at 500 Hz. They performed system ID with various sine sweeps and used least squares to estimate the fluid bulk modulus and hydraulic valve coefficients. The authors modeled friction effects with a simple piece wise continuous linear function dependent on piston velocity. High frequency dynamics were present in the force tracking analysis due to hydraulic line dynamics at approximately 195 Hz.
3. Proposed Control Architecture
The initial proposed control system architecture is comprised of a high level inverse dynamics and PD type position control loop which then sends torque commands to a low level inverse dynamics + PID type joint torque controller, as shown in Figure 1.
In Figure 1, the manipulator level controls will be implemented on an embedded Linux computer with a loop rate of ~60 Hz, while the joint level controllers will be implemented on a real time controller with a loop rate of 500 Hz.
A reduced form of this cascaded control structure can be seen below in Figure 2 which helps to show the high level motion controller and the low level joint controller.
Given the design requirement of doing a full manipulation cycle every <N> seconds or faster, we will over-spec the system to operate with high performance servo-valves with a bandwidth of in excess of 200 Hz. After initial experimentation the loop rates and servo-valves may be degraded to save computation and monetary expense.
System states to be measured or estimated will be: generalized joint position, q, generalized joint velocity, , joint torque, , hydraulic cylinder position, , hydraulic cylinder velocity, , spool position, z, cylinder A chamber pressure, , cylinder B chamber pressure, , supply line pressure, , and tank pressure, .
4. Manipulator Dynamics Modeling
The dynamic model for an n-joint manipulator can readily be derived using Lagrangian mechanics and is show to be [20]:
where B is the n x n inertial matrix, C contains the effects of Coriolis and centripetal forces, F contains the effects of joint friction, is the resultant joint torque required for the motion, and is the vector of forces exerted by the environment through the manipulator Jacobian. The inertial matrix can be calculated from rotation matrices and the manipulator Jacobian. The Coriolis matrix can be calculated from the Christoffel symbols of the first type as related to the inertial matrix. Initial modelling will neglect interactions with the environment, reducing equation 4 to:
where
By setting our control input, u, to the required torque, , and introducing a virtual control input, y, we obtain:
and
This relation is the simple feedback linearization via inverse dynamics required to reduce the system to a double integrator system, which we then aim to stabilize.
By selecting a PD type controller, we obtain:
and using the relation above leads to:
As this is a simple second order system, and can be chosen as diagonal matrices of the form:
and
According to [20], given any desired trajectory, tracking of the trajectory for the output q(t) is ensured by choosing:
Finally, this yields the error dynamics:
This formulation varies slightly from that in [1] due to the fact that they technically used a computed torque method, which is additive, as opposed to inverse dynamics, which is multiplicative.
One item to note, is that upon grasping an object with the end effector, the effective mass and moment of inertia of the last link, in this case the end effector, will change substantially. This may have to take this into account when grasping an object.
5. Actuator Dynamics Modelling
Hydraulic actuator dynamics will be modeled as shown in [1].
For our application, we neglect the effects of hydraulic line and pump dynamics as most papers have done, allowing us to arrive at equation 16, governing the rate of change of generated actuator force.
where is the hydraulic cylinder position, and P is the vector of hydraulic pressures. The input u is effectively the spool position, or more specifically, the ratio of spool position to nominal spool position as indicated in [6].
By taking into account the continuity equations for the A and B side chambers of the hydraulic cylinder we obtain [1]:
and
Where is the supply pressure, is the A side chamber pressure, is the B side chamber pressure, is the tank (return) pressure, is the is the hydraulic cylinder position, is the A chamber volume as a function of cylinder position, is the B chamber volume as a function of cylinder position, is the effective bulk modulus of the hydraulic fluid, and is the valve gain defined by:
Where is the valve flow rate at the nominal input signal ( @ ), and measured at a specified pressure drop, . [22]
Note that the derivation beginning with the equations presented in [6] varies slightly from that presented in [1] due to not using the ratio of A and B side cylinder surface areas and using the quantities directly.
In light of equation 16, a feedback linearization technique can be applied to improve the force, and consequently the position tracking.
By choosing a valve command u of the form:
a control law, v, can be designed to satisfy the force dynamics system requirements. The control law v was chosen as a PID type controller in [6].
By rewriting the force dynamics equation in a simplified form:
and choosing the control law, u, as we have above
our actuator force dynamics reduce to:
It is worth noting that [23], [11], [1], [3], [6] all appear to assume that the control input voltage applied to the spool valve is directly proportional to the spool position. [23] and [11] make this explicit, whereas [6] leaves the control input as a ratio of spool position to nominal spool position.
The time rate of change of hydraulic force can be related to joint torque by:
and
where r is the lever arm attached to the linear hydraulic actuator used to create a moment. [11]
In addition to these fluid power terms, there will necessarily be a term, as alluded to above, that will take into consideration the nonlinearly changing lever arm that the piston acts through.
6. Potential Avenues of Control Improvement
- Perform system identification on dynamic model parameters to increase model fidelity.
- Robustify the outer control loop via Lyapunov Redesign. I prefer this approach to adaptive control methods as it will likely be more robust to sudden system changes.
- Make the outer control loop directly or indirectly adaptive.
- If the change in the mass and moment of inertia of the end effector is causing an issue when picking up an object, a gain scheduled type of approach may need to be used wherein an approximate mass and MOI are added to the end effector link upon picking up the object.
- In the 2010 paper regarding control of the HyQ robot leg, the authors indicated that since the term in the joint level inverse dynamics controller is strongly time varying it produced oscillations due to a non-ideal cancellation of the hydraulic actuator nonlinearities. The authors circumvented this by implementing a modified control law that generated a feedforward action that was applied additively to the control action v. [11] In other words, instead of using
they used:
This implementation of the control law was shown to achieve very good position tracking. In 2010 they use valve voltage as u, in the derivation of the valve pressure dynamics they use, z/zn as the control input.
7. Simulation
Simulation was performed in matlab with appropriately modeled hydraulic actuators using simulink and simscape on a 4 DoF arm.
In order to compare the usefulness of an advanced control law design, first a standard PD controller was designed without regard of the system dynamics. The simulated linear controller step response is shown below in Figure 3.
As you can easily see in Figure 3 above, the dynamics of the system are coupled and a linear controller performs extremely poorly. If we instead synthesize a simple computed torque controller, we arrive at a step response as shown in Figure 4.
Figure 4 illustrates the dramatic difference that can be achieved with a simple computed torque control law. If we model the full system and synthesize a high level inverse dynamics + PD control law that is then sent to a low level PID type controller to control each individual joint angle, we arrive at Figure 5, below.
From figures 5 and 6 we can easily see that the inverse dynamics controller performs quite well.
To evaluate robustness, system parameters were varied by 20% and simulations were re-run. The inverse dynamics controller still performed decently, but it appeared that parameter disturbances justified using a robust control law. For this, Lyapunov redesign was used to robustify the controllers, which then allowed them to handle parameter uncertainty up to 20% quite well.
8. Experimental Results
Unfortunately, due to time and budgetary constraints, this project was cancelled during the implementation and experimentation phase. A physical 4 degree of freedom arm was constructed, equipped with hydraulic actuators and encoders, with a System 76 laptop running Ubuntu 18.04 acting as the high level controller, which was then connected to a real time controller via an ethernet connection sending UDP messages. High and low level control software was written in C++. The real time controller was partially managed by a contract engineering firm, which proved to be too black box for our use case, and resulted in the dragging out of the project beyond initial time estimates, and ultimately, its cancellation.
9. Conclusions
A nonlinear inverse dynamics controller was synthesized for a custom 4 degree of freedom robotic manipulator and simulated. Simulations indicated that linear controllers for our highly coupled dynamics performed very poorly, whereas computed torque, inverse dynamics + PD, and inverse dynamics + PD with Lyapunov redesign performed very well.
References
[1] T. Boaventura, C. Semini, J. Buchli, M. Frigerio, M. Focchi, and D. G. Caldwell. Dynamic torque control of a hydraulic quadruped robot. In 2012 IEEE International Conference on Robotics and Automation, pages 1889–1894, May 2012.
[2] S. Feng, X. Xinjilefu, C. G. Atkeson, and J. Kim. Optimization based controller design and implementation for the atlas robot in the darpa robotics challenge finals. In 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), pages 1028–1035, Nov 2015.
[3] C. Semini. HyQ – Design and Development of a Hydraulically Actuated Quadruped Robot. PhD thesis, Italian Institute of Technology, 2010.
[4] S. Schaal. The SL Simulation and Real-Time Control Software Package. Computational Learning and Motor Control Laboratory, University of Southern California, 2006.
[5] M. R. Sirouspour and S. E. Salcudean. Nonlinear control of hydraulic robots. IEEE Transactions on Robotics and Automation, 17(2):173–182, April 2001.
[6] Thiago Cunha, Boaventura@emc Br, Claudio Semini, Darwin Caldwell, and Darwin It. Gain scheduling control for the hydraulic actuation of the hyq robot leg. 01 2009.
[7] R. J. Richards S.R. Habibi. Computed-torque and variable-structure multi-variable control of a hydraulic industrial robot. IMechE, 1991.
[8] J. Mattila, J. Koivumki, D. G. Caldwell, and C. Semini. A survey on control of hydraulic robotic manipulators with projection to future trends. IEEE/ASME Transactions on Mechatronics, 22(2):669–680, April 2017.
[9] T. Boaventura, J. Buchli, C. Semini, and D. G. Caldwell. Model-based hydraulic impedance control for dynamic robots. IEEE Transactions on Robotics, 31(6):1324–1336, Dec 2015.
[10] C. Zhihua, W. Shoukun, X. Kang, W. Junzheng, Z. Jiangbo, and N. Shanshuai. Research on high precision control of joint position servo system for hydraulic quadruped robot. In 2019 Chinese Control Conference (CCC), pages 755–760, July 2019.
[11] M. Focchi, E. Guglielmino, C. Semini, T. Boaventura, Y. Yang, and D. G. Caldwell. Control of a hydraulically-actuated quadruped robot leg. In 2010 IEEE International Conference on Robotics and Automation, pages 4182–4188, May 2010.
[12] M. M. Bech, T. O. Andersen, H. C. Pedersen, and L. Schmidt. Experimental evaluation of control strategies for hydraulic servo robot. In 2013 IEEE International Conference on Mechatronics and Automation, pages 342–347, Aug 2013.
[13] M. Honegger and P. Corke. Model-based control of hydraulically actuated manipulators. In Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164), volume 3, pages 2553–2559 vol.3, May 2001.
[14] A. Bonchis, P. I. Corke, and D. C. Rye. Experimental evaluation of position control methods for hydraulic systems. IEEE Transactions on Control Systems Technology, 10(6):876–882, Nov 2002.
[15] O. Becker, I. Pietsch, and J. Hesselbach. Robust task-space control of hydraulic robots. In 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422), volume 3, pages 4360–4365 vol.3, Sep. 2003.
[16] M. Focchi, T. Boaventura, C. Semini, M. Frigerio, J. Buchli, and D. G. Caldwell. Torque control based compliant actuation of a quadruped robot. In 2012 12th IEEE International Workshop on Advanced Motion Control (AMC), pages 1–6, March 2012.
[17] S. Peng, D. Branson, E. Guglielmino, and D. G. Caldwell. Simulated performance assessment of different digital hydraulic configurations for use on the hyq leg. In 2012 IEEE International Conference on Robotics and Biomimetics (ROBIO), pages 36–41, Dec 2012.
[18] S. R. Habibi, R. J. Richards, and A. A. Goldenberg. Hydraulic actuator analysis for industrial robot multivariable control. In Proceedings of 1994 American Control Conference – ACC ’94, volume 1, pages 1003–1007 vol.1, June 1994.
[19] J. Koivumaki. Virtual decomposition control of a hydraulic manipulator. Master’s thesis, Tampere University of Technology, 2012.
[20] Robotics: Modelling, Planning and Control. Springer, 2009.
[21] Noah D. Manring. Hydraulic Control Systems. Wiley, 2005.
[22] Mario C. Destro and Victor J. De Negri. Method of combining valves with symmetric and asymmetric cylinders for hydraulic systems. International Journal of Fluid Power, 19(3):126–139, 2018.
[23] G. A. Sohl and J. E. Bobrow. Experiments and simulations on the nonlinear control of a hydraulic servosystem. IEEE Transactions on Control Systems Technology, 7(2):238–247, March 1999.