ModelSim / Simulink Cosimulation and FPGA Realization of a Multiaxis Motion Controller

This paper is to implement a multiaxis servo controller and a motion trajectory planning within one chip. At first, SoPC (system on a programmable chip) technology which is composed of an Altera FPGA (field programmable gate arrays) chip and an embedded soft-core Nios II processor is taken as the development of a multiaxis motion control IC. The multiaxis motion control IC has two modules. The first module is Nios II processor which realizes the motion trajectory planning by software. It includes the step, circular, window, star, and helical motion trajectory.The secondmodule presents a function of themultiaxis position/speed/current controller IP (intellectual property) by hardware. And VHDL (VHSIC Hardware Description Language) is applied to describe the multiaxis servo controller behavior. Before the FPGA realization, a cosimulation work by ModelSim/Simulink is applied to test the VHDL code. Then, this IP combined by Nios II processor will be downloaded to FPGA.Therefore, a fully digital multiaxis motion controller can be realized by a single FPGA chip. Finally, to verify the effectiveness and correctness of the proposedmultiaxismotion control IP, a three-axis motion platform (XYZ table) is constructed and some experimental results are presented.


Introduction
The integration of a multiaxis servo and motion controller into one chip and then application to XYZ table, CNC (Computed Numerically Controlled) machine or robotic system, and so forth, for reducing the system volume and increasing the system performance and motion stability, have become a popular research topic in literatures [1][2][3][4][5].Further, in typical applications, adopting PMSM as an actuator in automatic multiaxis equipment is a major technique trend due to its excellent high power density and better motion control characteristics, fast response and accuracy positioning [6][7][8].In traditional machine, it usually utilizes one central controller for planning the motion trajectory and multiple microprocessors for multiaxis servo motor drive system to establish a multiaxis motion controller.In this architecture, the coordination and communication between center controller and multiple microprocessors are a challenge task in the automatic multiaxis equipment.However, some drawbacks appear, such as large volume, being easily affected by the noise, expensive cost, and inflexible.In addition, data communication and handshaking protocol between the central controller and multiple microprocessors certainly slow down the system executing speed.To solve this problem, FPGA provides a best solution among those commercial high-end microprocessors due to its fast computational power, parallel processing ability, field programmable characteristics, and SoPC environment which can perform hardware-software codesign task [9][10][11][12].For example, in the design of PMSM servo drive using FPGA, it only needs less than 2 s in realizing the SVPWM algorithm, less than 3 s in implementing the current vector controller, and less than 3 s in executing the fuzzy controller algorithm in speed loop.The fast computational and parallel processing characteristics make it possible to integrate multiaxis servo controllers and the function of motion trajectory planning into one FPGA.
Recently, a cosimulation work by Electronic Design Automation (EDA) Simulator Link has been gradually applied to verify the effectiveness of the HDL (Hardware Description Language) code in the motor drive system [13][14][15].The EDA Simulator Link [16] provides a cosimulation interface between Simulink and ModelSim [17] where you can use Simulink models as a test bench that generates stimulus for ModelSim and analyzes the simulation's response [16].Therefore, before the FPGA realization, a cosimulation work by EDA Simulator Link will be applied to the multiaxis motion control system in this paper.The multi-PMSMs, multi-inverters, a motion trajectory planning, and motion position response are performed in Simulink and the position/speed/current controller IP (Intellectual Property) which is described by VHDL code is executed in ModelSim.After successful verification in simulation, the position/speed/current controller IP can be directly used in the FPGA.
The realization in this paper is the SoPC technology in which FPGA embedding a Nios II processor is established to develop a multiaxis motion control IC, and it is shown in Figure 1.It includes multiple servo controllers IP and one Nios II processor in a FPGA.Each servo controller IP performs the function of position/speed/current controller for each PMSM drive and Nios II processor executes the function of motion trajectory planning.In addition, the detailed inside architecture of the proposed multiaxis motion control IC in FPGA is presented in Figure 2. In servo controller IP, the vector control is adopted in the current loop, P controller is used in the speed loop, and fuzzy controller is applied in the position loop for each PMSM drive.However, the multiaxis motion control IC shown in Figure 1 can be applied multiaxis automatic equipment, such as XYZ table, SCARA robot arm, vertical-type robot arm, and CNC machine.

Modelling, Control, and Motion Trajectory Design
Figure 2 includes multiple servo controllers IPs and one Nios II processor in a FPGA.Each servo controller IP performs the function of position/speed/current controller for one PMSM drive and Nios II processor executes the function of motion trajectory planning.Detailed design methodology is described as follows.

Mathematical Model of Single-Axis Table.
The typical mathematical model of a PMSM is described, in two-axis d-q synchronous rotating reference frame, as follows: where V  , V  are the and -axis voltages;   ,   , are the and -axis currents;   is the phase winding resistance;   ,   are the and -axis inductance;   is the rotating speed of magnet flux;   is the permanent magnet flux linkage.The current loop control of PMSM drive in Figure 2 is based on a vector control approach.That is, if the   is controlled to 0, the PMSM will be decoupled and controlling a PMSM will be similar to controlling a DC motor.Therefore, after decoupling, the torque of PMSM can be written as the following equation: with is the motor torque and  is pole pairs.Thus, considering the mechanical load, the overall dynamic equation of linear table system is obtained by is force constant;   is the inertial value;   is damping ratio;   is the external torque;   represents the displacement in -axis table;   is the lead of the ball screw.

Fuzzy Controller (FC) Design.
The FC in this study uses singleton fuzzifier, triangular membership function, productinference rule, and central average defuzzifier method.In Figure 2, the tracking error  and the error change  are defined by where   represents the output of the FC;  *  (),   () represent th-axis position command and th-axis position feedback, respectively.The design procedure of the FC is described as follows.
(a) Take ,  as the input variable of FC, and define their linguist variables as  and dE.
(c) Select the initial FC rules by referring to the dynamic response characteristics, such as  where   and   are fuzzy number and  , is real number.(d) Establish the fuzzy system   (, ) by using the singleton fuzzifier, product-inference rule, and central average defuzzifier method.Although there are total 49 fuzzy rules that will be inferred, actually only 4 fuzzy rules can be effectively excited to generate a nonzero output.Therefore the output of the fuzzy inference can be obtained by the following expression: where  , ≜    () *    () and ∑ +1 = ∑ +1 =  , = 1.

Motion Trajectory
Planning.The circular, window, star, and helical circular motion trajectories are typically used [18] as the performance evaluation of the motion controller for XYZ table.
(c) Star motion trajectory: it is shown in Figure 3(b).The formulation is derived as follows: a-trajectory: b-trajectory: c-trajectory: d-trajectory: e-trajectory: The motion speed of the table is determined by .

Digital Circuit Design for Position/Speed/Current Controller IP
The proposed FPGA-based three-axis motion control IC for XYZ table is shown in Figure 4.In position/speed/current controller IP, the vector control is adopted in the current loop, P controller is used in the speed loop, and fuzzy controller is applied in the position loop.The FPGA herein uses Cyclone IV-EP4CE115, which is the product of Altera cooperation.There are 114,480 LEs (Logic Elements), 3,981,312 RAM bits, and 150 embedded multipliers in the Cyclone IV.In addition, a Nios II processor is embedded into FPGA to establish an SoC environment.The motion control IC comprises a Nios II embedded processor and multiple position/speed/current controller IP for multiaxis machine.The Nios II processor is utilized to perform the motion trajectory computation.
The code in Nios processor is developed with C language.Main program establishes the initial parameters setting, the motion command setting, and so forth and the ISR (interrupt service routine) executes the motion trajectory computation, position command generation, and sending out to three controller IPs.Each internal digital circuit of position/speed/current controller IP is also shown in Figure 4 and it includes one circuit of position FC and speed P controller, one circuit of current controller and coordinate transformation (CCCT), one SVPWM, one ADC interface, and one QEP interface circuit.All circuits in Figure 4

ModelSim/Simulink Cosimulation and Simulation Results
The Simulink/ModelSim cosimulation architecture for the three-axis motion control system is shown in Figure 5.Each axis system is driven by one PMSM.The SimPowerSystem blockset in the Simulink executes the three PMSMs      In this simulation cases, the following motion trajectory will be carried out and tested: (1) single-axis position step response; (2) circular motion trajectory tracking (twoaxis simultaneous motion); (3) window motion trajectory tracking (two-axis simultaneous motion); (4) star motion trajectory tracking (two-axis simultaneous motion); (5) helical circular motion trajectory tracking (three-axis simultaneous motion).Firstly, single-axis position step response is considered, and the step signal with 1.5 s and 5 mm amplitude is tested.The simulation results in position response and current response are presented in Figure 6.In this simulation, the Kp and Ki gains in position controller are, respectively, chosen by (15000, 0) and (25000, 95) in 16bit-Q15 format.In Figure 6(a), the position responses with 0.03 seconds rising time and zero overshoot and zero which show a good response performance.In Figure 6     the current in   axis approaches zero which presents the success of the vector control in PMSM drive.Further, in the two-axis simultaneous motion trajectory, the circular motion trajectory control with center (5, 5) mm and radius 10 mm is evaluated and the simulation results in motion trajectory tracking, tracking error, position response, and current response are shown in Figure 7.In this design, the Kp and Ki gains in position controller are, respectively, chosen by (18000, 0), (19000, 95) in 16bit-Q15 format.The tracking errors in Figure 7(b) show the maximum ±0.18 mm in -axis table, maximum ±0.18 mm in -axis table, and less than 0.1855 mm in overall circular motion tracking.It demonstrates a good tracking.Additionally, the current response of   -axis both in and -axis motors all can be controlled to zero which presents the success of the vector controller.In the window tracking motion, the trajectory is designed as Figure 3(a) and its simulation results in motion trajectory tracking, tracking error, position response, and current response are shown in Figure 8.In this design, the Kp and Ki gains in position controller are, respectively, chosen by (15000, 0), (25000, 95) in 16bit-Q15 format.The tracking errors in Figure 8 show the maximum ±0.185 mm in -axis table, maximum ±0.178 mm in -axis table, and less than 0.19 mm in overall motion tracking.
In the star tracking motion, the trajectory is designed as Figure 3(b) and its simulation results are shown in Figure 9.
In this experiment, the Kp and Ki gains in position controller are, respectively, chosen by (15000, 0), (25000, 95) in 16bit-Q15 format.The tracking errors in Figure 9(b) show the maximum ±0.14 mm in -axis table, maximum ±0.14 mm in -axis table, and less than 0.17 mm in overall star motion tracking.The results show a good tracking response in both window and star motion trajectory.Finally, the three-axis simultaneous motion trajectory which is helical circular trajectory is evaluated, and their simulation tracking results are shown in Figure 10, respectively.In this design, the Kp and Ki gains in position controller are, respectively, chosen by (18000, 0), (19000, 95) in 16bit-Q15 format.In Figure 10(b), the tracking errors show that it can keep within ±0.185 mm in and -axis table motion and ±0.19 mm in -axis table motion which reveals a good tracking response.Therefore, Figure 16 shows a good tracking result       in 3-axis motion tracking.Further, Figures 6∼10 present that the three-axis motion control system tested through ModelSim/Simulink cosimulation is effectiveness and correctness.After confirming the success of the proposed three-axis position/speed/current controller IP, it will directly apply to FPGA to be further evaluated.Cyclone IV-EP4CE115 which is the kernel of this system.There are 114,480 LEs (Logic Elements), 3,981,312 RAM bits, and 150 embedded multipliers in the Cyclone IV.Further, a Nios II processor can be embedded into FPGA to establish SoC environment.

Experimental Results.
In experiment, the one-dimensional motion trajectory is firstly tested.The step signal with 1/8 Hz square wave and 5 mm amplitude is sent to the axis tables as the position command to cause tables motion.The experimental results in position response and current response using FC are shown in Figure 12.In Figure 12(a), the position response with 0.2 second rising time and 2% overshoot and near zero steady state zero which shows a good response performance.In Figure 12(b), the current in   axis approaches zero which reveals the success of the vector control in PMSM drive.To evaluate the tracking performance Mathematical Problems in Engineering   in multiaxis motion table, the indices are firstly defined as follows: where (),  *  (),   (), , , and , respectively, represent instantaneous error, th-axis position command, th-axis position feedback, number of motion table, total acquisition data, and mean tracking error.Further, in two-dimensional motion trajectory, the circular motion trajectory control with center (50, 50) mm and radius 25 mm is evaluated and the experimental results in motion trajectory tracking, tracking error, position response, and current response using same FC are shown in Figure 13.The tracking errors in Figure 13(b) show the maximum ±0.46 mm in -axis table, maximum   ±0.35 mm in -axis table, and 0.1759 mm mean tracking error in overall circular motion.It presents a good contour tracking result.Additional, the current response of   -axis both in and -axis motors all can be controlled to zero which presents the success of the vector controller.In the window tracking motion, the trajectory is designed as Figure 3(a) and its experimental results in motion trajectory tracking, tracking error, position response, and current response are shown in Figure 14.The tracking errors in Figure 14 show the maximum ±0.2 mm in -axis table, maximum ±0.2 mm in -axis table, and 0.054 mm mean tracking error in overall motion.In the star tracking motion, the trajectory is designed as Figure 3(b) and its experimental results are shown in Figure 15.The tracking errors in Figure 15 show the maximum ±0.4 mm in -axis table, maximum ±0.55 mm in axis table, and 0.0698 mm mean tracking error in overall star motion.However, the larger tracking error in both window and star motion trajectory occurred at the position of command suddenly changed; otherwise, the tracking error is very small.The results show a good contour tracking response in both window and star motion trajectory.Finally, the three-dimensional motion trajectory which is helical circular trajectory and "STUST" character trajectory are evaluated, and their experimental tracking results are shown in Figures 16 and 17, respectively.In Figure 16(b), the tracking errors show that it can keep within ±0.2 mm in and -axis table motion, ±0.01 mm in -axis table motion, and 0.1162 mm mean tracking error in overall helical circular motion which reveals a good tracking response.In performing the motion of "STUST" character, the -axis table just runs the up and down motion, the "STUST" character is drawn by the simultaneous motion of and -axis table.The tracking error in Figure 17 shows the maximum ±0.3 mm in -axis, -axis, and axis table and less than 0.3 mm in overall motion tracking.Therefore, Figures 16∼17 show a good tracking result in 3D motion contour tracking.Therefore, Figures 12∼17 present that the proposed FPGA-based multiaxis motion control IC is effectiveness and correctness.

Conclusions
The FPGA-based multiaxis motion controller has been successfully demonstrated from ModelSim/Simulink cosimulation to FPGA realization experiment in this paper.For a multiaxis motion controller system, DSP and FPGA all can provide a good solution in implementation.However, due to one DSP chip only can support one CPU inside and, 12 channels PWM output and 2 QEP output in its peripheral circuit, it is suitable for a two-axis motion controller system.If we want to realize a multiaxis motion controller system, it needs several DSP chips.In contrast with DSP, FPGA has the properties of fast computing power, parallel processing, and hardware-software codesign which cause the multiaxis motion controller system to be implemented by one FPGA chip.Finally, the work herein is summarized as follows.
(a) The functionalities required to build a multiaxis servo controller and motion trajectory planning can be integrated in one FPGA chip.
(b) The computation of key algorithms, such as SVPWM, CCCT, and FC in PMSM drive can be completed within 2 s by FPGA implementation.
(c) The VHDL code of the proposed multiaxis servo controller IP has been effectively verified from Mod-elSim/Simulink cosimulation results.
(d) Some experimental results by position step response, circular, window, star, and helical circular motion trajectory tracking, the specific character ("STUST") motion trajectory tracking, and so forth have revealed that the proposed FPGA-based multiaxis motion controller is effectiveness and correctness.

Figure 1 :
Figure 1: FPGA-based multiaxis motion control IC and its applications.

Figure 2 :
Figure 2: The inside architecture of FPGA-based multiaxis position/speed/current control IC.

Figure 4 :Figure 5 .
Figure 4: Internal circuit design of the proposed FPGA-based multiaxis motion control IC.

1 = 2 = 3 =Figure 5 :
Figure 5. Matlab m-code is used to develop the program of the motion trajectory.In this simulation cases, the following motion trajectory will be carried out and tested: (1) single-axis position step response; (2) circular motion trajectory tracking (twoaxis simultaneous motion); (3) window motion trajectory tracking (two-axis simultaneous motion); (4) star motion trajectory tracking (two-axis simultaneous motion); (5) helical circular motion trajectory tracking (three-axis simultaneous motion).Firstly, single-axis position step response is considered, and the step signal with 1.5 s and 5 mm amplitude is tested.The simulation results in position response and current response are presented in Figure6.In this simulation, the Kp and Ki gains in position controller are, respectively, chosen by (15000, 0) and (25000, 95) in 16bit-Q15 format.In Figure6(a), the position responses with 0.03 seconds rising time and zero overshoot and zero which show a good response performance.In Figure6(b),
s 0 s 1 s 2 s 3 s 4 s 5 s 6 s 7 s 8 s 9 s 10 s 11 s 12 s 13 s 14 s 15 s 16 s 17 s 18 s 19 s 20 s 21 s 22 s 23