## 4 Pre-Filter Acceleration Feedback Enhanced PID Controller
### Controller  
The proportional-integral-derivative (PID) algorithm is widely used in various industrial robots because of its simplicity, efficiency and reliability. However, because of the constant parameters of the classical PID algorithm, it is difficult to effectively maintain the high tracking ability of the reconfigurable SCARA robot. Therefore, the pre-filter AFE control strategy is integrated into the series PID controller to effectively suppress the disturbance and improve the tracking performance. The structure of the controller is shown in Figure 6.  
Figure 6: Pre-filter acceleration feedback enhanced PID controller.  
The tracking error of position is defined as \(e_{x}=x_{d}-x\). Where \(x_{d}\) is the desired position, then the position PID of the controller can be expressed as Equation (10),  
\[v_{d}=K_{Px}e_{x}+K_{Dx}\dot{e}_{x}+K_{Ix}\int e_{x}dt \tag{10}\]  
where \(v_{d}\) is the desired speed of the system, \(K_{\text{Px}}\), \(K_{\text{Dw}}\), \(K_{\text{Ix}}\) represents the proportional parameter, derivative parameter and integral parameter of position PID, respectively. Through the trial-and-error method, \(K_{\text{Px}}\) = 1.2, \(K_{\text{Dx}}\) = 1.5, \(K_{\text{Ix}}\) = 0.4. Furthermore, the sample frequency of the position PID is 1000 Hz. The digital form of the position PID can be expressed as Equation (11) based on incremental PID.  
\[\left\{\begin{array}{c}\Delta u_{x(k)}=u_{x(k)}-u_{x(k-1)}=(K_{Px}+K_{Ix}+K_{ Dx})e_{x(k)}-(K_{Px}+2K_{Dx})e_{x(k-1)}+K_{Dx}e_{x(k-2)}\\ e_{x(k)}=x_{d(k)}-x_{k}\end{array}\right. \tag{11}\]  
where \(x_{d(k)}\) is the desired position of the system, \(x_{k}\) is the actual position of the \(k\)-th sample, and \(u_{x(k)}\) is the output of the position PID.  
The tracking error of speed is defined as \(e_{v}=v_{d}-v\). Where \(v\) is the desired speed, then the speed PID of the controller can be expressed as Equation (12),  
\[\dot{v}_{d}=K_{Pv}e_{v}+K_{Dv}\dot{e}_{v}+K_{Iv}\int e_{v}dt \tag{12}\]  
where \(\dot{v}_{d}\) is the desired acceleration, \(K_{\text{Pv}}\), \(K_{\text{Dv}}\), \(K_{\text{Iv}}\) represents the proportional parameter, derivative parameter and integral parameter of speed PID respectively. Through the trial-and-error method, \(K_{\text{Pv}}\) = 0.16, \(K_{\text{Dv}}\) = 12, \(K_{\text{Iv}}\) = 0. Furthermore, the sample frequency of the speed PID is 4000 Hz. The digital form of the speed PID can be expressed as Equation (13) based on incremental PID.  
\[\left\{\begin{array}{c}\Delta u_{v(k)}=u_{v(k)}-u_{v(k-1)}=(K_{Pv}+K_{Iv}+K_ {Dv})e_{v(k)}-(K_{Pv}+2K_{Dv})e_{v(k-1)}+K_{Dv}e_{v(k-2)}\\ e_{v(k)}=v_{d(k)}-v_{k}\end{array}\right. \tag{13}\]  
where \(v_{d(k)}\) is the desired speed of the system, \(v_{k}\) is the actual speed of the \(k\)-th sample, and \(u_{v(k)}\) is the output of the speed PID.  
The dynamic equation of a single joint can be expressed as Equation (14),  
\[\frac{\dot{I}}{R}\ddot{x}=\frac{J}{R}\dot{v}_{d}+G(q)+\frac{J}{R}\dot{v}_{f}+ \triangle_{f} \tag{14}\]  
where \(\ddot{x}\) represents the actual acceleration of the system, \(\dot{v}_{f}\) is the output of pre-filter AFE, \(\triangle_{f}\) is the disturbance of the system and \(R\) is the distance from the weight center of robot to the rotation center of the joint. According to the pre-filter AFE control strategy, \(\dot{v}_{f}\) and \(\triangle_{f}\) should be expressed as Equation (15),  
\[\frac{\frac{\dot{I}}{R}\dot{v}_{f}+\triangle_{f}}{\triangle_{f}}=L(s)=\frac{s }{s+c} \tag{15}\]  
According to Equations (14) and (15), the output \(\dot{v}_{f}\) of the system is expressed as Equation (16).  
\[v_{f}=C\frac{R}{\int\iint\left(\frac{I}{R}\dot{v}_{d}+G(q)-\frac{J}{R}\ddot{x} \right)dt.dt \tag{16}\]  
The proposed parameters are taken into consideration for the closed loop system in MATLAB Simulink, as shown in Figure 6(a). The Bode diagram shows that the closed loop system with such proposed parameters is stable (Figure 6(b)).