2.1 Mathematical model

The mathematical model of the bipedal walking robot can be considered as a hybrid system [Reference Westervelt, Grizzle, Chevallereau, Choi and Morris27]. This mathematical model, which is frequently used in studies on biped walking control, is also applied as a robot model in this study. Accordingly, the mathematical model consists of two parts named as single support phase and double support phase. In the single support phase section, the swing and stance phases of the legs are defined as a continuous-time ordinary differential equation. In the double support phase section, the collision event is defined as an impact model.

2.1.1. Single support phase model

As shown in Fig. 1, the robot has a point foot and therefore there is no ankle actuator. Joint angles are defined as $q\;:\!=\;[q_1,q_2,q_3,q_4,q_5]^T.$ In addition to the joint angles, the angle between the hip point and the ground contact point is called $\theta_{sync}$ . (See Section 4.1 for the role of the $\theta_{sync}$ angle in the gait controller.)

Figure 1. Joint angles of the five-link robot.

It is assumed that the support leg of the robot does not slip during the single support phase. In this case, the robot can be considered as a serial link manipulator. The center of the gravity of each link can be found via with forward kinematics. Dynamic equations of the biped robot in this phase can be derived in the form of Eq. (1) by Lagrange method [Reference Westervelt, Grizzle, Chevallereau, Choi and Morris27].

(1) \begin{equation} D(q) \ddot q + C(q,\dot q) \dot q + G(q) =Bu \end{equation}

where $D(q)\in R^{5\times5} $ is the positive defined symmetrical inertial matrix, $C(q,\dot q) \in R^{5\times5} $ centrifugal and Coriolis components matrix, $ G(q) \in R^5$ gravity component vector, and $B\;:\!=\;[I_{4},0_{4\times1}]^T$ is the constant vector indicating whether there is an actuator. It is seen from vector B that there is no actuator on the torso angle $q_5$ . $u \;:\!=\; [u1, u2, u3, u4]^T$ is the torque to be applied to the actuators between the torso-femur and femur-tibia. System dynamics are defined as state space form in Eq. (2) where $x \;:\!=\; [q^T ,\dot q^T]^T $ .

(2) \begin{equation} \dot x = \begin{bmatrix} \dot q\\ D^{-1}(q) [-C(q,\dot q)\dot q -G(q)+Bu] \end{bmatrix}\end{equation}

According to the control input, the state space representation of the system dynamics is rewritten as the sum of the nonlinear functions:

(3) \begin{equation} \dot x = f(x)+g(x)u\end{equation}

where $ f(x)= \begin{bmatrix} \dot q\\ D^{-1}(q) [-C(q,\dot q)\dot q -G(q)]\end{bmatrix} $ and $ g(x)= \begin{bmatrix}0_{5\times1}\\D^{-1}(q)Bu\end{bmatrix} $ .

2.1.2 Double support phase model

The single support phase ends when the swing foot touches on the ground. The collision occurred at the time of contact causes a discontinuity in the state variables. The post-collision values of the state variables are calculated according to the conservation of the angular momentum [Reference Hurmuzlu, Génot and Brogliato28]. To determine the force acting on the second leg during the collision, the generalized coordinate dynamic equation is derived by adding the endpoint coordinates of the second leg to the state variables.

(4) \begin{equation} D_e(q_e) \ddot q_e + C_e(q_e,\dot q_e) \dot q_e + G_e(q_e) =B_e u+\delta F_{ext} \end{equation}

The force acting on the second leg is assumed to be impulsive. When Eq. (4) is integrated during the instantaneous collision time, the external force $F_{ext}$ can be found as in Eq. (5).

(5) \begin{equation} D_e(q_e^+) \dot q_e^+ - D_e(q_e^-) \dot q_e^- = F_{ext} \end{equation}

The post-collision speed vector $\dot q_e^+$ is calculated based on the pre-collision speed vector $\dot q_e^-$ . Although the robot position does not change during the collision time, the swing and the support legs physically switch. According to this alterations, the new position vector can be written as $q_s^+\;:\!=\;[q_2^- q_1^- q_4^- q_3^- q_5^-]^T$ . By derivating the collision model according to the state space state variables, the collision transformation map can be obtained as in Eq. (6)

(6) \begin{equation} x^+=\Delta(x^-)\;:\!=\; \begin{bmatrix} \Delta _q q^-\\ \Delta _{\dot q}(q^-) \dot q^- \end{bmatrix}\end{equation}

where $x^+ \;:\!=\; [{q^+}^T ,\dot {q^+}^T]^T $ .

According to the single and double support phases, the general model of biped walking is defined in Eq. (7) as a hybrid system.

(7) \begin{equation} \sum:\Bigg\{ \begin{matrix} \dot{x}& =&f(x)+g(x)u &x^-\notin S\\ x^+& =&\Delta(x^-) &x^-\in S \end{matrix}\end{equation}

where S is the impact surface.

2.2. Physical model

The test platform is designed to exhibit both humanoid walking properties and the mathematical model of the five-link robot. In addition, the carrier platform is used to reduce the three-dimensional motion space to a two-dimensional sagittal plane. The carrier platform consists of two parallel links that can rotate $360^\circ$ on the tower. At the tip of the carrier platform, there is a connecting rod having a single degree of freedom. The connecting rod always remains horizontal via the parallel link mechanism. Therefore, the workspace of the hip point of the robot is limited to approximately a cylindrical surface (quasi-planar). The designed test robot is shown on the carrier platform as in Fig. 2.

Figure 2. The actual view of the carrier platform and test robot.

The reference coordinate system of the biped robot is oriented by the torso in accordance with the mathematical model. However, the global angle values of the joints should be known to perform trajectory tracking of the leg endpoints. A BNO055 [Reference Sensortec29] IMU sensor is used for coordinate transformation where the global position information will be received.

To provide the configuration in Fig. 1, the right and left legs are produced symmetrically. Each leg consists of a series of two links called the femur and tibia whose lengths are 32 cm, and the torso length is 34 cm. The total weight of the robot is approximately 6 kg. The detailed physical parameters of the robot are shown in Table I.

Table I. Physical model parameters.

The robot has four actuators in total, two of them are on the hips and the others are on the knees. To increase the trajectory tracking success of the links, the position of the joints must be precisely controlled. In this way, the joints are designed to apply both high torque and also precise position control. However, conventional reducers are insufficient for precise position control, because a backlash is caused by the gap between the gears.

Cycloid reducers are suitable for precise position control applications due to their low backlash value, compact size, and high conversion rates. According to electro-mechanical properties of the robot, the reducer demand torque is at least 20 Nm. To meet this requirement, the reducer speed reduction rate is determined to be 1:44. The integrated joint is designed taking into consideration the requirements of the biped robot. The motor is positioned inside the gearbox and a direct connection of the input shaft to the eccentric disk is provided. The output disk and cycloid teeth pins are extended to cover the motor. Thus, the input and output connections of the joint are positioned symmetrically on both sides. The stable force transfer is provided between the links by a symmetrical input and output connections. A CAD model of the designed joint is shown in Fig. 3.

Figure 3. Cad model of the designed joint.

Reaction forces occurring in applications that interact with the environment can both damage the joint and disrupt trajectory tracking. Therefore, it is advantageous to use series elastic actuators (SEA) in biped applications. SEA offers advantages of low mechanical output impedance, impact forces tolerance, and passive mechanical energy storage [Reference Hurst and Rizzi30]. In this design, the torque output of the cycloid reducer is provided with flexible material to the link. The reverse force acting on the output of the reducer by the load creates physical slip in the flexible material. The slip is calculated by measuring the input and output angles of the joint separately, so that the force acting on the joint is estimated depending on the slip. Input and output angles are measured by an As5047 magnetic encoder. A brushless DC (BLDC) motor is preferred as the motion source of the joint due to high speed and high torque advantages as well as precise position control. Those features make the BLDC motor one of the ideal options for robotic applications [Reference Mefoued and Belkhiat31]. In addition, field-oriented control (FOC) is used for joint position control [Reference Fayazi, Pariz, Karimpour and Hosseinnia32]. The driver board is designed and manufactured to provide current up to 30A. According to the experimental measurements, the maximum speed of the joint driven by the BLDC motor is 140 rad/s and the holding torque 20 Nm is observed, within the safe limits. In robotic applications, besides the mechanical structure, the electronic and communication infrastructure affects the system success as well. NI-Myrio is preferred as the central control unit, as it offers a user-friendly interface with LabVIEW. In addition, the coexistence of FPGA and microprocessor increases the processing capability. The central controller communicates with four local controllers and sensor units at high speed. Four magnetic encoders and two motor controllers are connected to one SPI line for one leg. Thanks to the parallel processing capability of the FPGA, the communication period can be completed in less than 150 us. The global controller is completed within 400 us on the ARM processor. Since the processor and FPGA can work independently, the control and communication periods do not affect each other. Also, each local controller (BLDC motor driver) operates at 170 us sampling rate.

3.1. Fixed-time optimization

In any collocation method, collocation points are used to reduce the decision variables to an array with N values instead of infinite values $x(t) \rightarrow [x_0,x_1,...x_N], u(t) \rightarrow [u_0,u_1,...u_N]$ . Transcription from continuous dynamics to discrete dynamics is achieved by applying the Simpson quadrature method between each consecutive pair of collocation points. Transcription requires the transformation of both system dynamics and integral form. Simpson’s rule for integration is given in Eq. (9) and system dynamics transformation is given in Eq. (12).

(9) \begin{eqnarray} \int_{t_0}^{t_f} w(\tau) d\tau &\rightarrow& \sum_{k=0}^{N-1} \frac {\Delta t}{6}(w_k+4w_{k+1/2}+w_{k+1})\end{eqnarray}

(10) \begin{eqnarray} \dot x&=&\textbf{F}\end{eqnarray}

(11) \begin{eqnarray} \int_{t_k}^{t_{k+1}} \dot x dt&=&\int_{t_k}^{t_{k+1}}\textbf{F}dt\end{eqnarray}

(12) \begin{eqnarray} x_{k+1}-x_k&=&\frac {(\textbf{F}_{k+1}+4\textbf{F}_{k+1/2}+\textbf{F}_k)}{6}\Delta t\end{eqnarray}

The Hermite–Simpson method requires a midpoint for each segment. To calculate the $ \textbf{F}_{k+1/2} $ which is derived from the known system dynamic functions $ f(x_{k+1/2}) $ and $ g(x_{k+1/2}) $ , it is necessary to find the $ x_{k+1/2} $ . Therefore, a second collocation equation is required. In Eq. (13), midpoint system dynamics are calculated according to Hermite interpolation. Detailed formulation of the Hermite–Simpson direct collocation method can be found in Betts’s book [Reference Betts34].

(13) \begin{equation} x_{k+1/2}=\frac{1}{2}(x_k+x_{k+1})+\frac{\Delta t}{8}(\textbf{F}_k-\textbf{F}_{k+1})\end{equation}

Continuous-time optimization problem written in Eq. (8) is discretized by direct collocation method then rewritten in Eq. (14) as a NLP problem [Reference Kelly33].

(14) \begin{equation} \begin{aligned} \min_{\substack{u_0,u_1,...u_N\\x_0,x_1,...x_N}} \quad & \sum_{k=0}^{N-1} \frac {\Delta t}{6}(u_k^2+4u_{k+1/2}^2+u_{k+1}^2)\\ \textrm{s.t.} \quad & \frac {(\textbf{F}_{k+1}+4\textbf{F}_{k+1/2}+\textbf{F}_k)}{6}\Delta t=x_{k+1}-x_k\\ &x_{k+1/2}=\frac{1}{2}(x_k+x_{k+1})+\frac{\Delta t}{8}(\textbf{F}_k-\textbf{F}_{k+1})\\ &x_{min} \leq x_k \leq x_{max} \\ &u_{min} \leq u_k \leq u_{max} \\ \end{aligned}\end{equation}

Solutions of NLP for N points are obtained with fmincon function in Matlab toolbox. To clarify the biped walking robot trajectory design problem, eight collocation points in Fig. 4a are drawn as an example. Graphics are shown in terms of normalized phase variable. Normalization details are given in Eq. (18).

Figure 4. Optimized trajectories ( $N=8$ ).

As a result of optimization, state ( $\hat{x}(t_k)$ ), derivative of state ( $\dot{\hat{x}}(t_k)$ ), and control ( $ \hat{u}(t_k) $ ) values that provide system dynamics for N points are obtained. Let $\hat{x}(t)$ , $\dot{\hat{x}}(t) $ , and $ \hat{u}(t) $ be the interpolated state, derivative of state, and control values that are calculated according to the optimization result at an arbitrary time t. By evaluating $ \hat{x}(t) $ and $ \hat{u}(t) $ with actual system dynamics, $\textbf{F}(\hat{x},t,\hat{u})$ can be calculated. The difference between $ \dot{\hat{x}}(t) $ and $\textbf{F}(\hat{x},t,\hat{u})$ gives transcription error $\xi(t)$ . Transcription error indicates the optimization solution accuracy. Evaluating the optimization for the collocation points ensures that highly accurate optimization solution for $\xi(t_k)$ . On the other hand, high error values for other points can be considered as solution failure.

(15) \begin{equation} \xi(t)=\left| \dot{\hat{x}}(t)-\textbf{F}(\hat{x},t,\hat{u}) \right|\end{equation}

As can be seen in the optimization error graph in Fig. 4b, the error value is higher than the average at the endpoints. The reason for the high error values is that these points are the starting or ending points of the contact between the robot and the ground. This indicates that the system dynamics are being moved away at the moment of step. The divergence of targeted trajectories from system dynamics decreases walking performance.

The simplest way to overcome the problems caused by a poor optimization accuracy is to increase the sample point number. However, increasing the number of samples increases the NLP solution time exponentially. According to the fixed-time trajectory optimization method, sampling number, maximum error value, and optimization solution time comparisons are given in Table II. As a solution, variable time interval optimization methods are suggested and tested for various applications [Reference Yang, Pan, Long, Song and Zhang35].

Table II. Fixed-time trajectory optimization error values and solution times.

3.2. Variable time optimization

In this study, a method is suggested to increase optimization accuracy by preserving NLP solution time. In the proposed method, collocation points are based on variable time intervals rather than fixed time intervals. While few collocation points are sufficient for low error rate regions, new collocation points should be added in regions with high error rates. According to the applied collocation method, determining the location of the new point increases the accuracy. Increasing the accuracy by adding a minimum number of collocation points is provided by metaheuristic search-based optimization methods.

In this study, finding the critical points to increase the accuracy is acquired by the tree-based breadth-first search optimization method. The tree structure is a graph that branches off from a single point and does not create a loop (). Each node in the tree structure shows a time interval change in trajectory optimization. However, since the optimum sampling time cannot be the same all over the trajectory, there are different levels of solution points on the tree. A new sampling time is determined for each solution point on the tree and new collocation points are found according to the applied collocation method. The tree-based breadth-first search optimization method is illustrated in Fig. 5. The iteration level is denoted by i and the parent number by j. Possible solution points are shown in green since the solution points are not child nodes, there may be unused j numbers at some levels (e.g., $i = 3$ ).

Figure 5. The tree-based breadth-first search optimization illustration.

In this study, Hermite–Simpson method is used as the collocation method. There is a midpoint between the two endpoints in the Hermite–Simpson method. In other words, the sampling time at the starting node of the tree structure is determined as half of the period $\Delta t=({t_f}-{t_0})/2$ . The branching rule has been developed to descend from a node to sub-nodes. According to the branching rule:

• The first collocation point used on the old node will be used as the first collocation point for the new node.

• The midpoint of the old node which is determined according to Hermite–Simpson will be the endpoint of the new node.

• A new midpoint will be calculated according to Hermite–Simpson for the new node.

The time resolution is increased by $3/2$ by this branching rule. Each new node is created according to the branching rule, and the error value is calculated and evaluated whether it is acceptable or not. The branching rule is repeated iteratively until it reaches the acceptable error value. Through iterative breadth-first search, solution points can be found on the tree structure. Figure 6 illustrates how to add a new point. In here, black, blue lines, red, and green dots indicate the system dynamics, optimized trajectories, collocation points, and newly added points, respectively.

Figure 6. New collocation point illustration.

As can be seen in Fig. 4 (q4 – swing leg knee joint), the relative joint angle value range found as a result of optimization is less than $ 1.2 $ radians. It is aimed that the optimization error does not exceed 0.1% within the specified range; therefore, the maximum acceptable error limit is limited to $1.2~\times~10^{-3}$ . According to the optimization results by the proposed method, the number of collocation points to achieve this goal is $N=21$ . (In the Hermite–Simpson method, the number of segments is seen to be twofold since there is always a midpoint.) The trajectory optimization within the same number of collocation points is tested to compare the performance of the proposed method. In optimization with the same number of collocation points without changing the time resolution, it is seen that the segment errors extremely exceed the limit error value. The error rates of the proposed variable time trajectory optimization and the fixed-time trajectory optimization method consisting of the same number of segments are compared in Fig. 7. Fixed-time interval trajectory optimization results are shown by blue dashed lines and the proposed method by red lines.

Figure 7. Comparison of the proposed method with fixed-time interval trajectory optimization.

As seen in Fig. 7, the optimization accuracy increases in the moments when the foot touches on the ground and leaves from the ground. It is clear that the proposed variable time interval trajectory optimization method can reduce the error below the threshold level in each segment. In addition, new points added to minimize the error and increase the processing time by a limited amount. The error value which is below the limit value can be reduced at 36 sampling points with fixed-time interval trajectory optimization methods, and in this case the optimization solution time is 143 s. However, with the proposed variable time interval trajectory optimization method, the same limit error value can be reached with 21 sampling points at 51.62 s. Although the same number (N=21) of fixed-time trajectory optimization methods can solve it in a relatively short time (45.44 s), it exceeds the error limit considerably (nearly four times more). Measurements performed with Matlab 2020a on Intel i7700HQ CPU. The performance comparison of the fixed-time trajectory optimization method and the proposed variable time trajectory optimization method is given in Fig. 8.

Figure 8. Performance comparison of the proposed method with fixed-time interval trajectory optimization.

In the performance comparison, it can be clearly seen that the variable time trajectory optimization takes less time than the fixed-time trajectory optimization for the same error limit. Reducing the trajectory optimization time from $ 143.04$ to $51.62$ s provides a valuable improvement. Unfortunately, both classical and proposed trajectory optimization methods cannot perform trajectory optimization within a step period. In addition, since periodic walking is provided, it is not necessary to re-optimize for each step. Trajectory optimization needs to be recalculated only when walking parameters change.

4.1 Global controller

In the first stage of walking control, virtual constraints method is applied. By the virtual constraints method, the motion space of joints is limited to desired trajectories. The constraint trajectories are defined by the synchronization angle ( $\theta_{sync}$ ) which is written based on joint angles. The synchronization angle that determines the rhythmic movement of the robot is defined as the angle between the hip and the support foot. In this way, all the joints can perform synchronous movement within the target trajectories.

$\theta_{sync}$ is shown with the red line in Fig. 1. The synchronization angle is calculated by summing the joint angles multiplied by the weights. The weight vector $ c=[-1, 0, -1/2, 0, -1] \in R^{1\times5}$ is obtained from the robot geometry. Synchronization angle is given in Eq. (16). The range of the synchronization angle can be given by the Bézier polynomial as an input by normalizing it between zero and one. Normalization function is given in Eq. (17). The range of the synchronization angle is set around $\pi \pm 10^\circ$ in radians ( $\theta_{max}=3.31$ , $\theta_{min}=2.96$ ). Complex walking trajectories found as a result of the optimization are applied with the Bézier polynomial. With the proposed method, optimization is solved using 21 points without exceeding the error limit. Each solution point is used as the Bézier coefficient. By connecting solution points with the Bézier polynomial that given at Eq. (18), continuous-time solution of desired trajectories is derived. M is the Bézier polynomial degree. $ a^i_k $ is the k numbered polynomial coefficient of the joint i. $ \theta_{norm} $ is the normalized synchronization angle.

(16) \begin{eqnarray} \theta_{sync}&=&cq \end{eqnarray}

(17) \begin{eqnarray} \theta_{norm}&=&\frac{ \theta_{sync} - \theta_{max} }{ \theta_{min} - \theta_{max} } \end{eqnarray}

(18) \begin{eqnarray} b_i(\theta_{norm})&=&\sum_{k=0}^{N} a_k^i\frac{M!}{k!(M-k)!}\theta_{norm}^k(1-\theta_{norm})^{M-k}\end{eqnarray}

In the second part, these trajectories are traced by the virtual constraint method. By this method, the motion space of joints is limited to predesigned trajectories [Reference Westervelt, Grizzle, Chevallereau, Choi and Morris27]. The constraint trajectories are defined by the synchronization angle which is written based on joint angles. The synchronization angle determines the rhythmic movement of the robot. In this way, all joints can perform synchronous movement within the target trajectories.

The robot model is given in Eq. (3) for a single support phase. System outputs are $y \in R^{4x1}$ and desired trajectories are $h_d=[b_1,b_2,b_3,b_4]^T $ . The $h_d$ vector is the combination of the Bézier functions that define the desired trajectory of each joint. The system output targeted to be set to zero is written as in Eq. (20).

(19) \begin{eqnarray} y&=& h \end{eqnarray}

(20) \begin{eqnarray} y&=& \begin{bmatrix} q_1& q_2& q_3& q_4 \end{bmatrix}^T-h_d(\theta_{norm})\end{eqnarray}

The feedback that will take the system outputs to zero can be easily acquired by input–output linearization. Relative degree is $r=2$ due to the structure of mechanical models and since $L_gh = 0$ . Output function derivatives are written in Eqs. (21)–(23).

(21) \begin{eqnarray} \frac{dy}{dt}&=& \frac{\partial h}{\partial x}\dot x \end{eqnarray}

(22) \begin{eqnarray} \frac{dy}{dt}&=& L_fh(x) \end{eqnarray}

(23) \begin{eqnarray} \frac{d^2{y}}{dt^2}&=& {L^2_fh}(x)+{L_gL_fh}(x){u}\end{eqnarray}

The control signal that can asymptotically take the system output to zero is written as in Eq. (24) according to Eq. (23). The parameter v is the auxiliary control signal and works as a PD controller. $ K_d $ and $ K_p $ are positive controller gains and $\epsilon$ is a tuning parameter which is $0<\epsilon<1$ . During each step, an impact occurs at the norm of $\dot y$ . To reduce this effect, derivative gain should be chosen smaller relative to the proportional gain. Therefore, derivative component of the controller is multiplied by $ 1/\epsilon $ and proportional component by $ 1/\epsilon^2$ .

(24) \begin{eqnarray} {u}&=& ({L_gL_fh})^{-1}({v}-{L^2_fh}) \end{eqnarray}

(25) \begin{eqnarray} {v}&=& -\frac{1}{\epsilon^2}K_p{y}-\frac{1}{\epsilon}K_d\dot {{y}}\end{eqnarray}

4.2. Local controller

The local controller provides the position control of the joint according to the angle and angular velocity references that are received from the global controls. The local controller function diagram is shown in Fig. 10. The diagram is drawn for joint number i and joint number can vary between one to four. For each local controller, joint angle, joint angular velocity, and BLDC rotor position are denoted by $ q_i $ , $ \dot {q_i} $ , and $ \theta_{BLDC} $ , respectively.

Figure 10. Local controller.

With the FOC method, motor currents are divided into a torque component $I_q$ and magnetic flux component $I_d$ . Since the BLDC motor supplies the magnetic flux from the permanent magnets, the $I_d$ current is desired to be zero. Reference $I_q$ current is adjusted according to the demanded torque. To control both joint angle and angular velocity within the certain limits, the cascade controller has been used. The position is controlled at the first stage of the cascade controller as defined in Eq. (28).

(26) \begin{eqnarray} e_{q_i}&=&q_{i_{ref}}-q_i \end{eqnarray}

(27) \begin{eqnarray} \dot{e}_{q_i} &=& \dot{q_{i}}_{ref}-\dot{q_i} \end{eqnarray}

(28) \begin{eqnarray} {\tilde{\dot{q_i}}}_{ref}&=&\bigg( K_p \cdot e_{q_i}+K_I\int_{0}^{t}{e_{q_i}(\tau)d\tau} + K_d \cdot \dot {e}_{q_i} \bigg)+K_{ff} \cdot \dot {q}_{i_{ref}}\end{eqnarray}

At the second stage of the cascade controller, the torque current is controlled according to the speed. The torque controller stage is tuned to respond approximately ten times faster than the position controller. Although the torque control stage is tuned to be aggressively fast, it is not sufficient to regulate the cogging torque caused by the mechanical structure of the motor [Reference Islam, Mir and Sebastian37]. According to the Fourier analysis, the sixth harmonic is observed as the dominant frequency. Since cogging torque cannot sufficiently suppress with the PID, adaptive feedforward cancellation (AFC) is applied. AFC has a significant effect on suppressing periodic disturbances based on the position information [Reference Qamar and Hatziadoniu38]. The torque controller stage is written in Eq. (30). Local controller PID and AFC gains are shown in Table III.

(29) \begin{align} e_{\dot{q_i}}&={\dot{q_i}}_{ref}-\dot{q_i} \end{align}

(30) \begin{align} I_{q_{ref}}&= \begin{aligned}[t] &\bigg( K_p e_{\dot{q_i}}+K_I\int_{0}^{t}{e_{\dot{q_i}}(\tau)d\tau} + K_d \dot e_{\dot{q_i}} \bigg) \\ &+ cos\big(6 \theta_{BLDC}(t)\big)\int_{0}^{t}{K_{afc} cos\big(6 \theta_{BLDC}(\tau)+\phi\big)d\tau}\\ &+ sin\big(6 \theta_{BLDC}(t)\big)\int_{0}^{t}{K_{afc} sin\big(6 \theta_{BLDC}(\tau)+\phi\big)d\tau} \end{aligned}\end{align}

Table III. Local controller gains.