The joint angular position is calculated with both encoder and accelerometer sensors. Afterwards, the obtained information is fused through a Kalman filter. In the case of the encoders, the angular joint position θ_Ei can be calculated using Equation (1), where E_i is the accumulated count given by encoder i; SF is a scale factor relating the minimum angular displacement required to obtain a count in the encoder, the units are rad/counts: (1) θ E i = SF ( E i )

Concerning the estimation of the angular joint position using accelerometers θ_Ai, the corresponding equations are summarized in Table 1. In the case of the first angular position, the joint always moves perpendicularly to gravity force. Therefore, an accelerometer cannot detect the position changes in this joint. For this reason, only the encoder information is using for the angular position estimation in joint 1.

Such equations assume that the accelerometers provide a noise-free signal, which is unrealistic; thus, the signal requires a filtering stage before being used.

Forward kinematics provides the position and orientation (roll, pitch and yaw) of the robot through the angular position of each joint. In this case, the forward kinematics is calculated through the standard Denavit-Hartenberg notation [4]. The notation is a transformation matrix ⁰T_i relating the reference coordinate frame (Z₀,Y₀,X₀) with the coordinate frame of the joint i (X_i,Y_i,Z_i). The notation requires obtaining the link parameters of the robot. Those parameters are the link length (a_i), the link twist (ϕ_i), the joint distance (d_i) and the joint angle (θ_i). Based on Figure 1(b), forward kinematics can be calculated through the link parameters presented in Table 2.

Those parameters are used to estimate the transformation matrix ⁰T_i. The general form of the transformation matrix is presented in Equation (2): (2) T 0 i = [ M i D i 0 1 ]where M_i [Equation (3)] contains the rotation information and D_i is a vector containing the position of the link i [Equation (4)]: (3) M i = [ m i , 1 , 1 m i , 1 , 2 m i , 3 , 1 m i , 2 , 1 m i , 2 , 2 m i , 3 , 2 m i , 3 , 1 m i , 2 , 3 m i , 3 , 3 ] (4) D i = [ X i Y i Z i ]

Therefore, forward kinematics can be calculated using Equation (3) and Equation (4). Position is directly estimated through Equation (4). Orientation can be estimated using Equations (5–7). Where α_i,β_i,γ_i are the rotations along Z₀,Y₀,X₀ axis, respectively: (5)   α i = tan − 1 ( 1 − m i , 3 , 1 2 − m i , 1 , 1 2 / m i , 1 , 1 ) (6) β i = tan − 1 ( − m i , 3 , 1 / 1 − m i , 3 , 1 ) (7) γ i = tan − 1 ( 1 − m i , 3 , 1 2 − m i , 3 , 3 2 / m i , 3 , 3 )

Encoders can provide a high-resolution measurement, but they are not capable of detecting some nonlinearities. Conversely, accelerometers, mounted on the robot, can detect some joint flexibilities and they provide an absolute measuring of orientation as well; however, the measurements are noisy and need filtering before being used. The fusion information of encoder and accelerometer takes the best of both sensors incrementing the accuracy of the monitored angular positions. A flow diagram that depicts the required sequential computing for sensor fusion and forward kinematics estimation is presented in Figure 2. The required operations are the acquisition of six encoder signals, conversion from encoder counts to radians, acquisition of twelve accelerometer signals, estimation of each-joint angular position, Fusion of encoder and accelerometer signals and the estimation of the forward kinematics for each axis.

Because of the amount of tasks that must be executed to obtain the forward kinematics estimation, conventional sequential processors are not suitable for the implementation since conventional industrial controllers require a measurement at a sampling period of 1 ms for conventional controllers, and 100 μs for high-speed controllers [27]. Moreover, evaluating the forward kinematics for each joint is not an easy task since the model complexity increases proportionally to the number of joints to be calculated [16].

The fused smart sensor processes the information from encoder and accelerometer in order to obtain the angular position of each joint. An accelerometer, when mounted onto the joint of a flexible robot, contains information about the orientation with respect to gravity, plus vibrations and noise [5,29]. Only the orientation information from the accelerometer is required to estimate the angular position of each joint. Such information is located at low frequencies of the accelerometer bandwidth [21] requiring a filtering method and an additional processing unit to compute it. Filtering is a key factor to be considered since delays obtained with conventional filters are not permissible for online estimation. A method that has been proved to solve this issue is the Kalman filter [6,7,16,17,30]. Moreover, sensor fusion is also possible with this technique. Therefore, this algorithm is selected to filter and to combine encoder and accelerometer signals. The basic fused smart sensor structure is shown in Figure 4. Encoder E_i and accelerometer A_i−1 are oversampled, which helps to reduce the signal-to-quantization-noise ratio (SQNR) [21] and improves the filter response. The filtering stage (KF1) is performed by a Kalman filter, having the accelerometer signals as inputs (A_i−1X,A_i−1Y,A_i−1Z). Next, filtered accelerometer signals ( A i − 1   X * ,   A i − 1   Y * ,   A i − 1   Z *) are sent to a processing unit TF where the angular position of the joint θ_Ai is calculated. The encoder signal is processed in concordance with (1) in order to obtain the angular position θ_Ai. After that, the joint angular position, estimated with encoder and accelerometer, are sent to a Kalman filter (KF2) where sensor fusion is executed. Finally, an average decimation filter (ADF) [21] is applied to the fused angular position θ i * to match the working sample frequency of the robot.

The equations for the Kalman filter are based on [31] and are described next. A Kalman filter works similarly to a feedback controller; the filter estimates the next state of the signal (predict) and then it obtains feedback in the form of noisy measurements to modify the predicted state (correct). General equations for the “predict” stage are presented in Equations (8) and (9): (8) X k * = S   X k − 1 + B u k − 1 (9) P k * = S P k − 1 S T + Q

Matrix S relates the previous state (X_k−1) and the estimated actual state ( X k *), B relates an optional control input u with X k *, Q is the process covariance and P k * is the a priori estimated error covariance.

In the case of the “correct” stage, the required equations are summarized in Equations (10–12), where R is the measurement noise covariance, H relates the measurements (Z_k) with the current state X_k, K_k is a gain factor that minimizes the a posteriori estimated error covariance (P_k): (10) K k = P k * H T ( H   P k *   H T + R ) − 1 (11) X k = X k * + K k ( Z k − H   X k * ) (12) P k = ( I − K k H ) P k *

Concerning the filtering stage KF1, matrix S is an identity matrix; B = 0; X = [ A i − 1 X * , A i − 1 Y * , A i − 1 Z * ] T, Z = [A_i−1X,A_i−1Y,A_i−1Z]^T; Q is a diagonal matrix containing the covariance of each signal; likewise, R is a diagonal matrix with the noise covariance of each signal and H is an identity matrix. The processing unit TF differs for each joint of the robot. Required equations are summarized in Table 1.

Stage KF2 is a Kalman filter designed for the sensor fusion of two signals; in this case the parameters of the general Equations (8–12):S = 1; B = 0; X = θ i *, Z = [θ_Ei, θ_Ai]^T, Q, is the covariance of the angular position; R is a diagonal matrix with the noise covariance of each input signal and H = [1,1]^T.

The averaging decimation filter is described in Equation (13), where N is a decimation factor relating the sampling rate of the sensors acquisition and the working sample frequency of the robot controller: (13) θ i ( k ) = 1 N ∑ j = 0 N − 1 θ i * ( N k − j )

An important block of the smart processor is the block in charge of the forward kinematics (Figure 5). The input parameters are the link dimensions (a_i, d_i) and the angular position of each link (θ_i), those parameters are used by two sub-processors to estimate position (X_i, Y_i, Z_i) and orientation (α_i,β_i,γ_i) of each link in concordance with Equations (4–7). The position estimator uses a multiplier-accumulator unit (MAC) and a coordinate rotation digital computer (CORDIC) configured to execute sine and cosine operations [32]; both are coordinated by a control unit that manages the operations performed for each unit. The estimated position is sent to a register bank. At the same time, the orientation estimator calculates the orientation of each joint. This processor has two embedded CORDIC units to estimate sine, cosine and arctangent functions. Also, a square root unit and a MAC unit are embedded. Alike the position estimator, a control unit coordinates the operations executed for each block and, when the orientation estimation is ready, the data is sent to the register bank.

The smart processor is implemented in a proprietary Spartan 3E XC3S1600E FPGA platform running at 48 MHz. Table 3 summarizes the resource usage of the FPGA after compilation.

The performance of the forward kinematics smart processor is compared with a personal computer (Sony Vaio VGN-CS170 featuring a two-core processor running at 2.26 GHz and 4 GB RAM). For each sample the smart processor requires 40 μs to calculate the complete forward kinematics, this processing time being suitable for conventional as well as high-speed servomotor controllers. On the other hand, the personal computer requires 21.22 ms to execute the same task. Therefore, the FPGA-based smart processor has the capability of online calculating the forward kinematics of a 6-DOF robot, while the PC is unable to perform the task online.

Encoder and accelerometer signals are monitored during the execution of the three paths in the 6-DOF PUMA robot. In Figure 8, the monitored encoder [Figure 8(a)] and accelerometer [Figure 8(b)] signals from the sensor network are presented for the case of the circle path. It can be observed that the encoder signals of each joint are noise free, due to their digital nature. Conversely, the accelerometers provide noisy measurements.

The sensor network is composed of encoders and accelerometers that are processed by the FPGA-based forward kinematics smart processor, taking advantage of each sensor. Figure 9 shows the accelerometer signals after the filtering stage KF1, for the case of the circle path. The use of a Kalman filter for this purpose allows obtaining the filtered signals without delays.

Figure 10 shows the angular position θ_i obtained after the fusion and decimation of the angular position estimated with accelerometers θ_Ai and the estimated with encoders θ_Ei. Fused angular position takes the best of each sensor, the high resolution of encoders and the absolute measuring provided by accelerometers.

With the aim of depicting the controller tracking monitored with the fused smart sensor network, Figure 11 shows the controller errors in each joint of the robot ɛθ_i, when circle [Figure 11(a)], square [Figure 11(b)] and zigzag [Figure 11(c)] paths are executed.

As summarized in Table 4, the joint angular position errors ɛθ_i are monitored with the fused smart sensor network. Both absolute and relative errors are shown for the case of circle, square, and zigzag paths. As it can be seen, the major problem is found in the square path movement, where joint 5 has the highest absolute and relative error. Such problem can occur because of the controller tuning or some mechanical problems in the robot.

In Figure 12, the obtained angular position θ_i and the dimensions (a_i,d_i), are used by the forward kinematics processor in order to obtain the position [Figure 12(a)] and orientation [Figure 12(b)] for each joint. The obtained data corresponds to the circle path.

In order to evaluate the performance of the robot controller, the analytical end effector paths are compared with those measured with the fused smart sensor network. Figure 13 shows both the analytical and estimated path for the case of the circle [Figure 13(a)], square [Figure 13(b)] and zigzag paths [Figure 13(c)]. Errors between analytical and estimated paths for each axis (ɛX₆,ɛY₆,ɛZ₆) are shown in Figure 13(d–e) for the case of the welding paths and Figure 13(f) in the case of painting path. Due to the characteristics of the fused smart processor network the proprietary controller errors are estimated and evaluated. Such errors fluctuate around 10 mm in the case of the circle path, 20 mm for the square path and 10 mm for the zigzag path.

As well as position, the orientation end effector accuracy is evaluated using the fused smart sensor network. A representation of the monitored orientation is shown in Figure 14(a) for the case of circle path, Figure 14(b) for the case of square path and Figure 14(c) for the zigzag path. Figure 14(d–e) show the orientation errors (ɛα₆,ɛβ₆,ɛγ₆) for the circle, square and zigzag path respectively. Controller error oscillates around 0.08 rad for the circle path, around 0.1 rad in the case of the square path and 0.06 rad for the zigzag path.

The end effector position errors (ɛX₆,ɛY₆,ɛZ₆) and orientation errors (ɛα₆,ɛβ₆,ɛγ₆) are found with the fused smart sensor network are summarized in Table 5. The major problems are found in the square path, where the highest absolute and relative errors are measured. Such errors could be utilized by the controller and compensate them in order to increase the robot accuracy.

Three real-operation-oriented paths are analyzed in the experimentation showing the capability of the fused smart sensor network to perform an online estimation of the angular position and the forward kinematics of each joint of a 6-DOF PUMA Robot. A complete forward kinematics estimation would be advantageous to improve the robot performance, since the proposed fused smart sensor can evaluate the angular position of each joint θ_i online, at the same time than position (X_i,Y_i,Z_i) and orientation (α_i,β_i,γ_i) are estimated.

The forward kinematics estimation obtained by using only encoders is compared with the forward kinematics estimation obtained by using the smart sensor network. To guarantee the repeatability of the measurements, the experiment is repeated 40 times. Table 6 shows the obtained relative errors. It is observed that the fusion of encoder and accelerometer provides a more accurate measurement when compared with the conventional encoder sensing.

Additionally, the proposed work is compared with other proposals through Table 7, where a comparative of the number of estimated parameters and the implementation features are shown. It can be observed that most of the work proposes a fusing method to improve the estimations. However, all the works limit their proposals to one or three DOF due to the complexity of evaluating forward kinematics in multi-axis robots [16] and the orientation information is not provided.