Research on foot trajectory tracking of parallel wheel-legged robot based on dynamic model predictive control

: In this paper, the foot trajectory tracking control for parallel structure of the six wheel-legged robot is investigated. The accuracy of trajectory tracking and dynamic response with heavy load are the main challenges of parallel mechanism. To guarantee the tracking performance and improve dynamic response frequency to posture input, a method based on dynamic model predictive control is proposed under the establishment of dynamic model of single leg. Newton-Eulerian equation is derived and converted into a discrete state space expression for velocity loop control, appropriate parameters including prediction time domain, control time domain and proportional gain are determined by co-simulation. Desired sinusoidal trajectories with different frequencies are tracked with satisfactory performance in terms of accuracy and response frequency. Finally, comparative experimental results using BIT-NAZA robot derived from the proposed control strategy indicate that the delay error and amplitude error are better than PI controller under the same conditions. This research can provide theoretical and engineering guidance for accurate planning of intelligent robot, and facilitate the control performance of wheel-legged robot in practical system.


introduction
Parallel robots owns many advantages compared to serial robots, such as higher movement efficiency, higher load capacity, satisfactory precision and can reach a higher speed and acceleration [1]. Many researchers have been studying on those robot devices since the original Gough-Stewart parallel mechanism was developed by Gough and Stewart in 1954 and 1965, respectively [2][3][4][5]. The parallel structure of six wheel-legged robot developed in this paper named BIT-NAZA is shown in Fig.1, whose leg is composed of the Stewart platform inverted and mobile tires added to the bottom movable platform, which can realize wheel, foot and wheel-foot compound movements, and the payload can reach 400Kg. Fig.1 Parallel structure of six wheel-legged robot Trajectory tracking is the main challenge in practical applications for parallel platform because of its complicated dynamic and kinematic modeling. Two strategies named joint space control and task space control have been proposed to control a Stewart platform. In [6], the desired lengths of actuators of platform has been measured based on solving inverse kinetic problem of the desired trajectory in task space, which developed the joint space control. Task space control can be performed based on: (1) Position and rotation of the movable platform using position, orientation and vision sensors [7], (2) Solving forward kinematic problem (FKP) by the values of sensors [8]. The first approach is expensive and the latter method needs lots of calculations. In [9], according to a motion-tracking system, a vision-based control has been proposed for a 6-DoF parallel robot by measuring posture of movable platform. In [10], based on Homotopy Continuation approach, the nonlinear equations in FKP of a parallel robot were solved. Their method has been experienced in solving problem with high precision.
Researchers have developed different control strategies in control of a 6-DOF Stewart Platform such as Linear Quadratic Gaussian (LQG) controller, Sliding-Mode controller, Robust approaches, PID controller, et al. In [11], by combining a Linear Quadratic Estimator (LQE) with a Li-near Quadratic Regulator (LQR), the LQG controller using reference position tracking has been proposed for motion control of a pneumatically-actuate 6-DoF platform. Meanwhile, the authors evaluated the robustness of the controller under different external disturbances in experimental tests. In [12], the trajectory tracking control of a 6-DoF pneumatically Stewart platform has been achieved by PID controller with feedback linearization. In the foregoing study, the controller's parameters were tuned by heuristic algorithm which was designed to solve non-convex problems. The work pressure has been controlled in inner control loop and the outer loop has been combined with displacement of the load under different external forces. Their analysis results were achieved in the Simulink of Matlab. An adaptive controller has been designed for the position tracking of a 6-DoF platform based on a linearized dynamic equations model. Two control loops have been employed and the PD controller and adaptive compensation have been combined by authors in [13]. In [14], the authors proposed a hybrid method based on Sliding Mode and Neural Networks, and a robust adaptive controller for a 6-DOF robot has been implemented without requiring inverse dynamic model. In [15], a robust adaptive controller has been designed according to task space dynamic equations. At last, a Stewart Platform was built in Simulink environment of MatLab software. The performance of controller for sinusoidal trajectory tracking was also evaluated. Sliding Mode control method has many advantages for control of robotic systems with large uncertain disturbances [16]. At present, vision-based methods have been widely researched for intelligent robots. In [17], a visual-based kinetic equations model of parallel platform has been designed by observing its legs measured by geometry of lines. The identification and control of platform were both simplified by proposed method. In [18], the vision servo system of parallel robots has been proposed based on hidden robot model by observing the leg directions.
In previous works, an observer-based robust controller [19] equipped with a fast friction estimator for a 6-DOF parallel electrical manipulator (PEM) in the joint-task space has been proposed. The cooperative control framework [20] is implemented in a host CPU and four slave CPUs for the four wheel-legged robot. In addition, an event triggered active disturbance rejection control (ADRC) of DC torque motors in position tracking problem has been propose in [21,22]. In this paper, a trajectory tracking control method for the foot of parallel wheel-legged robot based on dynamic model predictive control is proposed to improve the tracking accuracy and response rate of the foot to input posture. The main contributions of the paper are summarized as follows: 1) In order to improve the tracking accuracy and response frequency of the foot to the expected pose, consider the dynamic model of the platform established by the Newton-Euler equation under the loading condition and transform it into a discrete state space equation. Based on this, the prediction equation of the MPC controller is established and the optimal objective function is set to obtain the optimal control law. 2) Associated simulation to adjust the controllers of velocity and position loop to determine the appropriate control time domain, predicted time domain and other parameters. Comparative experimental results proved that the proposed method has more accurate performance in trajectory tracking, and the dynamic response effect is also better, which can provide theoretical and engineering guidance for the intelligent robot. The remainder of the paper is organized as follow. Section 2 presents problem formulation and establishes the dynamic model of the Stewart platform. In section 3, MPC combined with the dynamic model establishes the platform's predictive equation, optimal objective function and solved optimal control amount. The simulation analysis and verification is discussed in section 4. The comparative experimental results are carried out in section 5 before a conclusion is drawn in section 6.

Platform modeling
The schematic diagram of the force on wheel-legged robot's single leg with load is shown in Figure 2. To simplify the analysis of the problem, the mass and rotational inertia of the single cylinder of each leg and the friction of the connecting part are ignored.
Assuming that the load mass is evenly distributed, C is the centroid of the load, and its coordinate in the inertial coordinate system N is c R . 16 ff are the thrust generated by the six legs, and 16 are the vectors of the thrust of the center of mass C to the connection points between each leg and the movable platform. (for simplicity, only 1 f and 1 are marked). The local coordinate system B uses the center of mass C as the origin and moves together with the load, and the inertial coordinate system N uses the center of the base platform as the origin. Then, Euler equation can be obtained by the torque analysis of the load in the local coordinate system B as follows: is the angular velocity vector of the load represented by the local coordinate system B , B i f is the thrust vector of the i -th actuator, and the diagonal matrix J represents the principal moment of inertia of the load which uses the axes of the local coordinate system B as the inertia principal axes.
In order to transform the vector coordinates in the local coordinate system into the coordinates in the inertial coordinate system, a rotation matrix R is introduced as follows: Where c and s represent the cosine and sine of the trigonometric function, , and are rotation angles of the load which rotates around the axes of the inertial coordinate system N. The rotation angular velocity of load, radial vector and coordinates of thrust vectors in the local coordinate system B can be represented as follows to the inertial coordinate system N.
Substituting equation (3) into equation (2), the Euler equation in the inertial coordinate system can be described as: Considering that the rotation angular velocity N and the radial thrust The cross product (×) operation in Eulerian equation (4) can be replaced by a matrix operation, then the Eulerian equation (4) can be described as: We can know that is the magnitude of thrust, then the both ends of equation (5) is multiplied by R at left, so the Euler equation and Newton 's equation of motion can be obtained as: Assuming that the sampling period of control system is Rn are all constant in a single sampling period, discrete equations can be obtained by discretizing equations (6) and (7) as follows: From equation (8) and equation (9), the state space equation can be obtained as: It is known that the variables The output equation can be written as

The principle of control to platform
The block diagram of control structure of the leg of parallel wheel-legged robot is shown in Fig.3.  (13) and (14) derived from the foregoing, the state observer equation (15) can be obtained as follows: After establishing the prediction model, the first prediction state can be obtained as follows: The prediction state of i -th step with update is as follows: The prediction output of i -th step can be obtained as follows:

The cost function and its constraints
To achieve the optimal control effect, setting the objective function and minimize it. The expression of the optimization objective function can be de-scribed as follows: Where | r y k i k is the target value in the pre-diction domain p starting from the current time k, which can be obtained by the target trajectory in the prediction time domain p, among this: k is the difference between the control vectors at two sequential moments. The control domain m satisfies m＜p, Q and R is weight matrices. The former item on the right side of equation (18) reflects the requirement for target performance tracking, and the latter item reflects the requirement for the change in control amount. In addition, by adding the relaxation factor, it can be guaranteed that the feasible solution can be obtained if the cost function violates the constraints, and the relaxation factors 0 and is the weight. In practical systems, constraints on the amount of actuating signal and the output of the actuators are often required, so the output signal of actuators can be described as follows:

Solving the objective optimization function
Equation (18) can be transformed as follows: Each time the control cycle is updated, the first element of this control sequence is added to 1 uk to obtain the optimal control amount at time k, so the control amount can be obtained as:

Simulation results
The simulation model block diagram in MATLAB / SIMULINK from the control structure diagram of Fig.3 is established, as shown in Figure 5. Equations (13) and (14) are included in the dynamic model, and the MPC controller completes the predicted output and optimal solution of the control variables. Create a model of the Stewart platform in the dynamic analysis software Adams and export it as an executable file in the Matlab environment, the control amount of the MPC controller is input to the model, at the same time, the measurements of length and velocity of each leg of the platform is fed back to the position loop controller and dynamic model. The step input is used to tune MPC in velocity loop and the sine-wave input is used to tune proportional controller in position loop.

parameter tuning of velocity loop
The step signal with amplitude 0.15m/s is input to the MPC controller, and the constraint range of the actuating signal is ±700 N. In figure 6, the predication domain m=2 and the control domain p is variable. And in figure 7, the predication domain p=5 and the control domain m is variable.
Step response for m=2 and variable p Step response for p=5 and variable m Fig.6 Step response for variable p and m It can be seen from Fig. 6 that the change in the predicted time domain p has little effect on the step response, while the change in the control time domain m has a significant effect on the step response. The analysis of Table 2 shows that as m increases, the step response rising time decreases, and the steady-state relative error decreases. Considering that if m increases, the amount of calculation will also increase, which is not conducive to the real-time nature of system control, therefore, choosing p=5 and m=2 as the MPC controller parameters can ensure the control performance while avoiding the increase of the control calculation time caused by the excessive value of m.

Position loop simulation and tuning of proportional controller
Next, tune the gain of the position loop proportional controller. The sine signal with frequency 2Hz and amplitude 0.15rad is used as the input signal to the position loop, and m=2, p=5 of the MPC controller are known. By gradually increasing the gain of the proportional controller, at last a proper gain 0.12 is obtained without oscillation. The experimental results shown in Fig.7. (a) is the sinusoidal response of the pitch axis attitude; (b) is the actuating signal of 6 actuators.
(a) Sinusoidal response (b) Thrust force of actuator Fig.7 Sine wave response for m=2, p=5 From the curve shown in Fig.7(a) we can know that the amplitude error of the sinusoidal response is 0.0004rad and the delay error is 0.0005s. It can be seen from Fig.7(b) that under the conditions of the parameters in Table 1, the thrust forces of actuators of the Stewart platform are all in the range of -15N ~ 40N, and the thrust forces change smoothly and continuously throughout the entire motion cycle of the platform, which can provide theoretical and engineering guidance for the selection of actuators of the Stewart platform.
Both simulation results can meet the need of accuracy. Therefore, the parameters of the MPC controller and the position loop controller gain 0.12 are used as the controller parameter for the next experiment.

Experimental results
The framework of robot distributed system is shown in Fig.8. The communication network includes TCP/IP based network, CAN bus network, RS232 communication network etc. The robot status information is sent to a monitor computer by wireless UDP transmission technology when the robot is working. There are five nodes on the CAN bus. Host CPU is the master node; slave CPUs are the slave nodes.
The actuator of the Stewart platform used in the experiment is an electric cylinder, which is driven by a servo motor. The main control computer at the bottom is equipped with a multi-core processor and has a PC/104 interface and runs in the Simulink/Real-Time environment. The upper-layer development computer writes the control algorithm and compiles it, and then sends it to the bottom-level master computer. The target computer uses the D/A converter card to convert the control digital to analog input to the Stewart platform actuator servo driver, and reads the speed information of the electric cylinder from the servo driver through the encoder interface.
The walking trajectory of the wheel-legged hexapod robot is shown in Fig.9. The accurate tracking to expected posture of the foot ensures that it meets the foot drop planning, decreases the cumulative error during the all walking process to ensure the adaptability of the wheel-legged robot to complex terrain. The experiment was carried out in the order of the attitude and displacement tracking test, and the comparative experiment was performed with the PI controller. Fig.8 Framework of robot distributed system Fig.9 Trajectory tracking of the single leg First, an attitude tracking experiment is performed, and a sine wave signal is selected as the attitude input of the pitch axis. The frequencies are 1Hz, 2Hz, and 4Hz, and the amplitude is 0.0873rad. The PI controller with parameters P=25 and I=0.1 is applied in the comparative experiment. The experimental curve is shown in Fig.9, and the tracking performance comparison of MPC controller and PI controller is shown in Table 3. The next step is a trajectory tracking experiment, as shown in Fig.9, the arch width (AC) of the cycloid is 320mm, the arch height (the distance from B to AC) is 80mm, and the arc ABC movement time is 3s. The results in Fig.10 show that the cycloidal trajectory tracking accuracy of the MPC controller is significantly better than that of the PI controller.  3 and table 4 that MPC controller is superior to the PI controller in terms of tracking accuracy and dynamic characteristics. Because the MPC controller takes into account the prediction model of the Stewart platform and adds optimized control, it can use the historical information of the controlled object to predict the future control amount changes, so it can guarantee more accurate tracking performance and adaptation.

Conclusions
This paper addressed the trajectory tracking control of a 6-DOF platform used as leg of the developed six parallel wheel-legged robot. At present, the research method of Stewart platform considered dynamic model combined with MPC controller is relatively rare, this paper investigates the dynamic model predictive control for foot trajectory tracking of the six wheel-legged robot. A control-close loop was designed based on two control loops including outer position loop used proportional controller and inner velocity loop used MPC controller. Furthermore, trajectory tracking control of the robot's foot-end was performed for different desired trajectories which resulted in an appropriate performance in tracking. From the experimental results, it can be inferred that the proposed method can be employed for trajectory tracking control of the 6-DOF parallel robot, which can provide theoretical and engineering guidance for practical robot application. As ongoing works, to further improve the control accuracy and stability to external disturbances such as the internal-robot friction and external-robot and environment interaction forces, et al.