Mass Estimation in Manipulation Tasks of Domestic Service Robots using Fault Reconstruction Techniques

The project we propose consists in applying model-based fault reconstruction techniques [3] to estimate the weight of the object being carried by the robot. If we consider the manipulator without load as the nominal system, and the weight of the object as an external perturbation causing a faulty behavior, we can apply fault reconstruction techniques to estimate such perturbation, i.e., to estimate object’s weight. There are several approaches for model-based fault reconstruction. Residual generation is a common technique where an observer is designed to be sensitive to fault signals. On the other hand, Sliding Mode Observers (SMO) are dynamic estimators designed to be robust against fault signals [9]. SMOs also provide the ability to reconstruct the fault signal by filtering the so called output error injection term [1]. With the estimation of the manipulated object mass, we expect to improve the manipulation performance in domestic service robots.

This technical report is organized as follows: In section 2 we briefly introduce the concept of Sliding Mode Observers (SMO) and we describe the robot Justina’s manipulator, the platform we chose to test our proposal. In section 3 we explain the theoretical part of our proposal while section 4 is dedicated to describe the implementation using a model-based design approach in MATLAB and Simulink. Section 5 describe our results and also, the cases where the proposal can fail. In section 6 we show the reusability of our proposal by replicating some results using a simulated Katana manipulator. Finally in section 7 we state our conclusions and sketch the future work.

Sliding mode observers are disticontinuos observers that have the properties of finite time convergence and accurate tracking of the measured states once the sliding surface is reached. These type of observers can be used to reconstruct faults or disturbance signals through an appropiate filtering of the so-called injection output error. Interested readers can find further information about Sliding Mode Observers in [9, 2, 4].

Fault detection and isolation, from the control theory approach, is commonly achieved by the implementation of residual generators. A common way to design such residual generators is through observers that are sensitive to the fault signals. Nevertheless, SMOs inherit the properties of Sliding Modes theory, i.e., SMOs are designed to be robust against disturbances and fault signals and to accurately track the system states. As previously explained, the discontinuos term of the SMO will contain information about the fault signal wich can be reconstructed by obtaining the equivalent output error injection.

Justina is a domestic service robot built at the Biorobotics Laboratory of the National Autonomous University of Mexico and developed under the ViRBot architecture [8]. This robot and its predecessors have been participating in the Robocup@Home league [11] since 2006 performing several tasks such as cleaning a table, serving drinks, and several other tasks that humans ask for.

Among other actuators, Justina has two 7-DOF manipulators built with Dynamixel servomotors. Such servomotors allow the user to control them in several forms: by setting a goal position, setting a goal movement speed or setting a desired current which will produce a torque. This latter option is used in this project. Also, these motors provide position sensing with a resolution of less than a tenth of degree. Figure 1(a) shows robot Justina and its sensors and actuators.

To describe the kinematic chain and some physical properties of the manipulator, we use an URDF file. We already had an URDF describing the whole robot kinematic chain, nevertheless, for this project it was necessary only one of the mainpulators. We chose the left arm and extracted the corresponding tags to a new file. This file was used as a starting point for modeling and simulation purposes. Figure 1(b) shows the left arm of robot Justina as represented in the URDF.

From the Langrangian of the manipulator, a dynamic model of the following form can be obtained:

where $q\in\mathbb{R}^{7}$ are the joint angles, $M(q)\in\mathbb{R}^{7\times 7}$ is the inertia matrix, $C(q,\dot{q})\in\mathbb{R}^{7\times 7}$ is the Matrix of Coriollis forces, $B\dot{q}\in\mathbb{R}^{7}$ is the vector of friction forces, $G(q)\in\mathbb{R}^{7}$ is the vector of gravitational forces, $u$ is the input torque, considered as control signal, and $\Delta(q,\dot{q},u)$ is a vector containing all errors due to uncertanties, disturbances and fault signals.

To design a SMO it is necessary to write the model in variable states form. Let $x_{1}=[q_{1}\;q_{2}\;q_{3}\;q_{4}\;q_{5}\;q_{6}\;q_{7}]^{T}$ and $x_{2}=[\dot{q}_{1}\;\dot{q}_{2}\;\dot{q}_{3}\;\dot{q}_{4}\;\dot{q}_{5}\;\dot{q}_{6}\;\dot{q}_{7}]^{T}$ be the state variables. Then (1) can be written as:

Equation (3) can also be written in the form:

where $f(x_{1},x_{2},u)=-M^{-1}(q)\left(C(q,\dot{q})\dot{q}+B\dot{q}+G(q)-u\right)\in\mathbb{R}^{7}$ is the nominal part and $\phi(x_{1},x_{2},u)\in\mathbb{R}^{7}$ contains all terms related to uncertainties, disturbances and fault signals. If the system is correctly identified, and assuming we have no other disturbances than the object being carried, then $\phi(x_{1},x_{2},u)$ corresponds only to the fault signals, which, in this work, will be caused by the weight of the object being manipulated.

Thus, if we reconstruct the signal $\phi$ we will be able to estimate the mass of the manipulated object.

If a SMO is used to estimate the joint speeds, the unknown term $\phi(x_{1},x_{2},u)$ in (2)-(3) can be reconstructed by an appropriate filtering of the output error injection term. We used the observer proposed by [9]:

where $z_{1}$ and $z_{2}$ are the output error injection terms calculated as

In this observer, the sliding surface is given by $\sigma=x_{2}-\hat{x}_{2}$. When the sliding mode is reached, it holds that:

Since, $x_{2}=\hat{x}_{2}$, then

where $z_{2_{eq}}$ is the equivalent output error injection which can be obtained by an appropriate low-pass filtering of $z_{2}$.

Noisy and low sampling frequency rates can cause chattering effects on the estimated states. To mitigate this effect on mass estimation, the output error injection will be low-pass filtered. However, if estimated states are used for a closed-loop control, then actuator signals will have high frequency components that will result in hardware damage. Thus, as it will be later discussed, the SMO is used to estimate the fault signal but, to implement a closed-loop control, an Extended Kalman Filter is used instead.

The term $\phi(x_{1},x_{2},u)$ in equation (3) is a function of input torque, joint positions and joint speeds. The mass of the object could be calculated in any point of the state space, nevertheless, calculations are much simpler if such estimation is made only when the manipulator is in a constant position. If $\dot{q}=\ddot{q}=0$, then $\phi(x_{1},x_{2},u)=\phi(q)$ depends only on the gravitational torques caused by the weight of the object being carried. From (3) we can see that disturbance $\phi$ is a signal of acceleration, not a torque signal. Let $\tau_{o}=[\tau_{o}1,\dots,\tau_{o}7]^{T}$ be the torque exerted by the manipulated object with mass $m_{o}$ and weight $w_{o}=m_{o}g$ on each joint. Vector $\tau_{o}$ can be obtained by multiplying signal $\phi$ by the inertia matrix $M(q)$. Thus, from equation (6):

Figure 2(a) shows torques $\tau_{o}$ on the different joints. Torques are zero where the weight force is applied along the axis of rotation. In figure 2(a) this is the case for joints $O_{3}$, $O_{5}$ and $O_{7}$.

Torques $\tau_{o}$ are caused by the weight of the object being carried and thus they can be obtained in a form similar to term $G(q)$ in model (1). The mass $m_{o}$ can be estimated from any component of $\tau_{o}$, nevertheless, due to the kinematic configuration, it is much easier if we use torque on joint $O_{6}$.

Consider the figure 2(b). The frame $O_{6}$, attached to joint 6, is the frame $O_{0}$ translated and rotated. To represent the rotation we used the Roll-Pitch-Yaw angles $(\rho,\theta,\psi)$. As it can be seen, the weight $w_{o}=m_{o}g$ causes the torque $\tau_{6}=-m_{o}gl_{4}\sin\theta$ and the mass of the manipulated object can thus be calculated as:

where $\theta$ is the pitch angle of frame $O_{6}$ w.r.t. $O_{0}$, $g$ is the gravity acceleration, $\tau_{6}$ is the disturbance torque calculated according to (7) and $l_{4}$ is the distance from joint $O_{6}$ to the center of mass of the manipulated object. As it will be latter explained, errors in the estimation of $l_{4}$ will cause proportional errors in the estimation of $m_{o}$.

Note that when $\theta=0$ it is not possible to estimate $m_{o}$ because $\theta=0$ means that the manipulated object is “hanging” from the joint and its weight is not exerting any torque on the joint. There are two possible ways to overcome this situation: use another disturbance $\tau_{i}$ or moving the manipulator to a useful position. The second option is more feasible since the relation between $w_{o}$ and $\tau_{i}$ becomes more complex in the rest of the joints.

Sliding Mode Observer have the great advantage of being robust against disturbances, nevertheless, due to the discontinuos ouput error injection, they can have high frequency components in the estimated states. SMOs are useful for fault reconstruction but, due to the noise and low frequency sampling rate, it is better to use an Extended Kalman Filter (EKF) as part of the closed-loop control strategy.

From (2)-(3), we have the state transition model for the noisy system:

where $\nu\in\mathbb{R}^{1}4$ is a vector of noise signals without temporal correlation, zero mean and covariance matrix $Q\in\mathbb{R}^{14\times 14}$. Remember that system state variables are the seven joint positions and seven joint speeds.

In this work, since we are measuring the joint positions, our observation model is:

where $\omega\in\mathbb{R}^{7}$ is Gaussian noise without temporal correlation, zero mean and covariance matrix $R\in\mathbb{R}^{7\times 7}$.

Chosing between the continuous or discrete version of the EKF depends on the frequency sampling. In this work we achieved a sampling of 250 Hz, which is much faster than the system dynamics, but too slow for a SMO. Also, since Simulink provide tools for continuos controls, we chose the continuos version of the EKF:

where $y=z-\hat{x}_{1}$ is the error between the estimated and measured outputs, $F$ is the Jacobian of the state transition model and $K$ is the Kalman Gain calculated as:

with $H\in\mathbb{R}^{7\times 14}$, the Jacobian of the observation model, which, in this case, is the constant matrix:

Note that in (13) we are using only the nominal part of the system and thus, if $\phi\neq 0$ (i.e., if an object is being manipulated), the estimated states will not converge to the real states. Since estimated states are used for position control, such estimation error will cause a steady state error in the controller. This is in principle undesirable, nevertheless, we can tolerate this error since the main goal of this work is the estimation of the mass, not the performance of the controller. Calculations to estimate $m_{o}$ are made under the assumption that the manipulator is in a constant position, thus, driving the manipulator to a constant $q_{f}$, although slightly different from the desired $q_{g}$, is a good enough performance for the control-observation loop.

As stated before, equations for estimating $m_{o}$ are derived under the assumption that the manipulator is in a constant configuration. We implemented a PD+Gravity controller using the EKF estimated states:

where $\hat{q}$ and $\hat{\dot{q}}$ are the EKF estimated positions and speeds, respectively; $G(\hat{q})$ is the vector of gravity torques of the nominal part of the model (no noise and no fault signals) and

As explained in the previous section, when the manipulator takes an object, the estimated states do not converge to the real ones and then the controller shows an steady state error. Nevertheless, this is tolerable since the goal of the proposed system is not the control performance but the observer performance for the mass estimation.

To improve control performance, a trajectory is planned using a 5th degree polynomial. This type of trajectory allows to set initial and final position, speed and acceleration, which is ideal for the manipulation tasks. It is important to note that since the trajectories are calculated in joint space, the cartesian movement of the end effector doesn not result in a straight line. Although algorithms to plan movement task for manipulators are out of the scope of this research, we used this approach to simplify the tuning of the control constants for experimentation.

Mass estimation was tested moving the manipulator to several positions commonly used for grasping objects. Figure 8 shows these predefined positions. It is worth to note that mass estimation will work in any configuration as long as wrist pitch $\theta$ (see equation (8)) is different from zero. In a real application, it is better to estimate the mass only when $|\theta|>\theta_{T}$ where $\theta_{T}$ is a threshold large enough to avoid large errors due to the division by a very small value of $\theta$. In this work, we used $\theta_{T}=0.2$.

Frequency sampling was set to 250 Hz. This value is the frequency achieved with the real manipulator. Dynamixel servomotors were configured to 1 Mbps of baudrate and we have nine motors in the manipulator: seven motors for the seven degrees of freedom, and two more for the manipulator. Considering the total number of bytes to be sent and received, the required time is in the order of 1 ms, nevertheless, smallest configurable USB latency in Ubuntu is 1 ms, which needs to be added to the time for sending and receiving RS485 data. 4 ms was chosen as approximately twice the time required to send torques and read positions from all motors. This frequency was set both for tests with real and simulated manipulator.

Experiment for mass estimation was made simulating an object with mass $m=0.25$ located at the center of the gripper. We used the constants shown in table 1, where $I_{14}$ and $I_{7}$ represent identity matrices of orders 14 and 7 respectively. To filter the output error injection term of the SMO we used a 4th degree Butterworth Low-Pass filter with cutoff frequency of 1 Hz.

Figure 9 shows the results for the estimation and filtering of the angular position while moving the arm to one of the predefined positions. As it can be observed, the measured position is noisy and does not converge to the desired position, nevertheless, this is an expected behavior. As explained before, the objective of this work is the mass estimation of the manipulated object, not the performance of the controller. In the presence of the fault signal (an object in the end effector), the controller shows steady state error. Comparing the estimated positions between the SMO and the EKF, we can see that the SMO-estimated positions perfectly track the measured positions, since in general, SMOs are designed to be robust against faults and disturbances, nevertheless, SMO-estimated positions show the problem of chattering. Instead, EKF-estimated positions show an steady-state error but do not show chattering nor noise, making them more suitable for implementing the PD+ control.

Figure 10 shows the results for the estimation of the angular speeds. Although in the real manipulator we don’t have a measurement of the joint speed, we included the simulated speed for testing purposes. As it can be seen, current speed does not converge to the desired one. Same as the angular position, this error is due to the simplicity of the controller. Similar to the positions, the SMO-estimated speeds are nearer to the current speeds but with the problem of big chattering. Instead, the EKF-estimated speeds show a big error but without noise, which makes them better to be used in the controller.

Finally, figure 11 shows the resulting estimated mass. As explained in section 3.3, in this work we make the mass estimation only when the manipulator is in a constant position. Ideally, estimations should be made only when $\dot{q}=0$, nevertheless, this is a hard condition. Instead, we set estimated mass to zero if $\|\dot{q}\|>0.5$, that is why in figure 11 the estimation is equal to zero for the first few seconds. As soon as the arm stops, the estimated value converges to 0.25 kg, the actual mass of the object.

There are two possible causes of errors in the estimation: uncertanties in the physical parameters (link masses and inertia, joint frictions, etc) and the position of the object along the gripper. In this work it is assumed that the system is correctly identified, nevertheless, sometimes this assumption could not be held, specially in complex systems such as the 7-DOF manipulator. In order to analyze the effect of parameter uncertainties in the estimation, we ran simulations with wrong parameter values. That is, the term $f(x_{1},\hat{x}_{2},u)$ in (16)-(17) is calculated with the wrong parameters but measurements $q$ come from the system simulated with the correct values. Figure 12(a) shows the estimation results using link masses and inertia with values 20% smaller than the nominal ones. As it can be seen, the system overestimates the mass value. The torque exerted on each joint is caused by the link mass and the manipulated object. If such link mass is smaller, then the exerted torque will be attributed to the manipulated object. The contrary effect happens when the physical parameters are overestimated, as shown in figure 12(b).

According to equation (8), we need to know the distance $l_{4}$ for a correct mass estimation. This distance depends on the object position along the gripper. We always assume that the object is gripped in the center of the end effector, nevertheless, there could be a small error in this assumption. The error in the estimated mass will be proportional to the error in the estimation of $l_{4}$. Figure 13 shows the estimated values for three different values of $l_{4}$. As it can be seen, the greater the distance $l_{4}$, the greater is the torque exerted on the joint with the same object mass and a greater value of mass is estimated. At this stage of the project, we don’t have a way to determine the position of the object along the gripper, nevertheless, for the goal application of this project, the errors in estimation are tolerable.