Industrial Robot Arm Controller Based on Programmable System-on-Chip Device

Field-programmable gate arrays (FPGAs) and, recently, System on Chip (SoC) devices have been applied in a wide area of applications due to their flexibility for real-time implementations, increasing the processing capability on hardware as well as the speed of processing information in real-time. The most important applications based on FPGA/SoC devices are focused on signal/image processing, Internet of Things (IoT) technology, artificial intelligence (AI) algorithms, energy systems applications, automatic control and industrial applications. This paper develops a robot arm controller based on a programmable System-OnChip (SoC) device that combines the highperformance and flexibility of a CPU and the processing power of an FPGA. The CPU consists of a dualcore ARM processor that handles algorithm calculations, motion planning and manages communication and data manipulation. FPGA is mainly used to generate signals to control servo and read the feedback signals from encoders. Data from the ARM processor is transferred to the programmable logic side via the AXI protocol. This combination delivers superior parallelprocessing and computing power, real-time performance and versatile connectivity. Additionally, having the complete controller on a single chip allows the hardware design to be simpler, more reliable, and less expensive.


INTRODUCTION
Because of the ability to perform dangerous, dirty and /or repetitive tasks with consistent precision and accuracy, the industrial robot arm is increasingly used in a variety of industries and applications such as handling, palletizing, cutting, finishing, sealing and gluing, spraying, welding… [1][2][3][4][5][6].
Controller is the core of an industrial robot arm system [7]. The robot controller plays an important role in ensuring accuracy and directly affects the speed of the robot's movement. In recent years, many studies have focused on improving robot controllers both in hardware and software. The software includes algorithms that perform kinematics calculations, trajectory planning, communication… and the hardware platform refers to hardware circuit [8]. A controller must have the computational ability to perform trigonometric functions in the inverse kinematics problem, trajectory interpolation, synchronous motion control. Additionally, the control system must be robust and scalable while allowing for future system improvements and expansion. Nowadays, with the development of embedded processors, complex control algorithms can be implemented by ARM processors instead of basing on industrial computers or PCI cards. Moreover, field programmable gate array (FPGA) technology is growing and being used more and more in motion controllers. In [9], the FPGA is used to control servo motors with the control signal is sent from the ARM processor. In this paper, the whole controller includes a core board and an interface board. The core board contains two main chips: STM32F207 ARM processor and Itera cyclone II EP2C8Q208 FPGA chip. The communication between these two chips is done by the FSMC (Flexible Static Memory Controller) technology which is a unique technology in STM32. A multifunctional robot arm control system based on Linux and FPGA is presented in [8]. This paper adopts IPC as the controller and FPGA as the core processor of the motion control board. Ligong-Sun in [10] designs a kind of open robot controller combining powerful functions and remarkable advantages, which realizes a programmable controller on chip of industrial robot using ALTERA FPGA and an embedded soft-core processor of NIOS II. In [11], a new motion control IC is developed andimplemented on an industry standard FPGA provided by Xilinx. This research develops functions of closed current loop control, closed position/velocity loop control, incremental encoder logic, PWM modulation, fault/brake logic, velocity estimator, host communication module, UART module and delta-sigma Analog to Digital converter. The hardware system executes quickly in dedicated parallel hardware, so the update rates of the current control loop and position/velocity control loop can reach 120 kHz and 20 kHz, respectively.
The rapid development of modern electronic technology, especially the presence of System-on-Chip devices provides a new method to implement the robot controller. The system-on-chip (SoC) devices combine high-performance CPUs and the processing power of programmable logic which delivers superior parallelprocessing, computing power, real-time performance and versatile connectivity [13]. Barrios-dV et. al. in [12] design and implement a real-time controller system for robot navigation using a Xilinx Zynq® System on Chip. The system consists of a decentralized neural inverse optimal controller, an inverse kinematic model, and a path-planning algorithm. The motor control is obtained based on a discrete-time recurrent high order neural network trained with an extended Kalman filter, and an inverse optimal controller designed without solving the Hamilton Jacobi Bellman equation. In [14], a compact CNN accelerator for the IoT endpoint System-on-Chip (SoC) is proposed to meet the needs of CNN computations. The results show that the compact accelerator proposed in this paper makes the CNN computational power of the SoC based on the Cortex-M3 kernel two times higher than the quad-core Cortex-A7 SoC and 67% of the computational power of eightcore Cortex-A53 SoC.
This paper develops a robot controller using a Zynq-7000XC7Z020SoC device. The XC7Z020 devices are equipped with a dual-core ARM Cortex-A9 processor integrated with 28 nm Artix-7 based programmable logic for excellent performance-per-watt and maximum design flexibility. With the dual-core processor, the controller can implement two parallel tasks. The main core process algorithm calculations include kinematics algorithm and trajectory planning. The algorithms are executed when receiving commands from the teach pendant. Another core handles the communication and processing data from teach pendant which includes decoding robot language and G-code. After implementing the calculation, the controller needs to send signals to drivers to control servo motors via a motion control board. Because the motion control board synchronously controls six servo motors which requirea maximum frequency of up to 4Mpps, we utilize parallel computing to shorten the control cycle based on FPGA.
The remainder of this paper is structured as follows: Section 2 introduces the trajectory planning and motion control algorithms to control the robot arm. Section 3 describes hardware structure. Section 4 describes software design on ARM core and FPGA. Section 5 shows the experimental results. Finally, Section 6 is conclusion.

TRAJECTORY PLANNING AND MOTION CONTROL
Trajectory planning creates reference signals for the robot controller so that the robot moves in the desired trajectory. Motion control uses interpolation functions and inverse kinematics to generate discrete data of joint angle values. The controller sends these values to the motion board to generate pulses for servo motor drivers.
The driver ensures that the motor turns properly at the reference value. Trajectory planning generates a time schedule for how to follow a path given constraints such as position, velocity, and acceleration. The trajectory planning can be conducted in the joint space or in the workspace. The trajectory can be defined by the initial point and final point of the path (point-to-point) or a finite sequence of points along the path. In this section, we just consider the point-to-point trajectory planning problem.

Joint Space Trajectories Planning
Planning trajectory in the joint space has many advantages such as less computation, easier to plan trajectories in real-time and no problem with singularities.The initial and final pose of the robot is usually given in the workspace, solving an inverse kinematic problem, we determine the initial and the final angles at each joint.The planning algorithm generates a function q(t) interpolating the given joint variables at each joint. In practice, a trapezoidal velocity profile is usually assigned (see Figure 1.a). The velocity graph consists of three phases namely constant acceleration, constant velocity and constant dece-leration. Assume that the angle from the initial position to the final position, q f , the maximum speed ω max and the constant acceleration/deceleration ω are given in advance. We need to determine the acceleration/ decelerationtime t c , the time for the constant velocity phase , the total time T, and the function of q(t). The velocity at the end of the acceleration phase is equal to the constant velocity, so: And the angle after the acceleration is: The area of trapezoid is equal to the total angle q f , so: Therefore, the time for the constant velocity phase is: , the velocity profile is a trapezoid, the total time is: The trajectory is formed by a linear segment connected by twoparabolic segments: ,, the velocity profile is a triangle that only consists of acceleration and deceleration phase (see Figure 1.b). The trajectory is formed by twoparabolic segments, we have: Therefore, the acceleration/deceleration time and the total time are: The maximum velocity in this case is: The function of angle in term of time t: We get six values of T i for six joints but all joints must be stopped at the same time, so the maximum value of T i is selected: Then, the velocity ω max at each joint is recalculated by solving equation (5): , the acceleration is recalculatedfrom equation (9) and the triangular profile is used:

Task Space Trajectories Planning
Joint space trajectories are usually used in pick and place applications that do not care about the end-effect or trajectory. But in many applications, such as welding, cutting, etc., it is requiredto control the robot motion to follow a specified path in the workspace. So, trajectory planning also needs to be executed directly in the workspace. In the task space trajectories, the position and orientation of the robot's end-effector are interpolated over time and transformed to the values of the joint angles bysolving the inverse kinematics. So, it requires a large amount of computation.
The trajectories are usually formed by linear and circular paths. Consider the linear trajectory between the initial position p i and final position p f . Perform thelinear interpolation: where s is a scalar, Usually, the trajectory starts and ends withzero speed, so the trapezoidal profile is used to defined s(t): is the maximum value of ( ) The values of t c and T are calculated from acceleration a m and velocity v m that are given in advance (similar as equation 1 and equation 5): where is the length of the line segment.
In the case of the circular trajectory with radius r, center C and orientation matrix R, the coordinate of a point on the circle is: where s(t) is the arc length. The value of n(t) = s(t)/ris also computed as in the linear interpolation.
When robot movement, the orientation of the endeffector is also changed.The orientation of the endeffector is represented by the rotation matrix. Denote the rotation matrix at the initial position is R(t=0) = R i and at the final position is R(t=T) = R f . To perform the orientation planning, define the matrix R f i that transforms the initial orientation to the final orientation. We have the relationship R f = R i R f i or: 11 Calculate the angle-axis parameter uθ from the rotation matrix R f i : 11 Choose the angle α(t) so that α(t=0) = 0, α(t=T) = (and use a trapezoidal profile) to calculate the matrix: with cα = cosα, sα = sinα, a = 1 -cα Then, calculate the orientation matrix of the endeffector:

Pulse train generation
Combining the trajectory planning and inverse kinematics, the joint angles at each joint can be generated over time. The motion control system needs to control the servo motors to reach these values. A servo motor is controlled by sending pulses to the motor driver. One pulse makes the motor rotate one small constant angle called resolution. The pulses are generated every sampling cycle T. Assuming q(t) is the joint angle at the time t and q(t+T) is the joint angle at the next period. The number of pulses sent to the controller is determined:

if q t T q t resolution n q t T q t if q t T q t resolution resolution
The number of pulses is sent to the motion controller and the motion controller ensures to generate pulses at the same time, so six servo motors will work synchronously. When pulses are generated independently on the motion controller, the pulse frequency can be up to several MHz so the motor can turn at high resolution and leads to many benefits such as: higher efficiency, less heat, increased stability, better speed control, higher torque to inertia ratio possibilities.

HARDWARE DESIGN
The integration of two ARM cores and FPGA on a single device helps to simplify the hardware structure. This research uses the MicroZed board as a central processor board. MicroZed contains two I/O headers that provide the connection to two I/O banks on the programmable logic (PL) side of the Zynq®-7000 All Programmable SoC device. An external board is designed to connect MicroZed board with servo driver through these I/O headers. In addition, the external board is also responsible for converting 2.5V signalson the MicroZed board to 5V signalson the servo driver and vice versa. The hardware design is shown in Figure 2.
Unlike traditional robot controllers, the main controller and motion controller are distinguished, our controller is designed on a single board. So, the size of the controller is very compact and the communication is also simple. Although integrated into a single board, the functions of each part are still the same as the previous controllers. The main core ARM is mainly responsible for algorithm calculations, trajectory planning, communication with teach pendant and other devices, controlling FPGA. FPGA is used to control the servo motor and read the feedback signals from the encoder. Moreover, FPGA provides reprogrammable IO and hardware reconfiguration capability, so we can expand IO pins to control the end-effector tools and create more hardware for communication standards. The main controller will manage this IO pin function and create software to control communications.

Figure 2. Hardware design
Another advantage of the Zynq All Programmable SoC device is communication between ARM core and FPGA. Data is communicated via the AXI bus interconnect. Advanced Extensible Interface, or AXI, is part of ARM's AMBA specifications. The AXI is a point-topoint interconnect that is designed for high-performance, high-speed microcontroller systems. The AXI protocol is based on a point-to-point interconnect to avoid bus sharing and therefore allow higher bandwidth and lower latency. The Xilinx AXI Interconnect contains AXI-compliant master and slave interfacesand can be used to route transactions between one or more AXI masters and slaves. In our system, the ARM core is the master, slaves are modules designed on the FPGA side, which means one master interfaces with many slaves. Each slave device must have a unique address.
When the ARM chip writes data to FPGA, it sends address and control signals to slaves. The control signal can be 'read' or 'write'. After that, the master sendsthe channel address to choose which channel will be read or write. Each channel is a 32-bit register. If the control signal is 'read', the slave will send data to the master, otherwise if the control signal is 'write', the master will send data to the slave.

FPGA Design
To accelerate the creation of highly integrated and complex designs in programmable devices, we use intelligent IP integration delivered by the Vivado Design Suite software. An IP (intellectual property) core is a block of logic or data that is used in making anFPGA or application-specific integrated circuit (ASIC) for a product. Figure 3 shows the diagram designed using IP blocks, in which some blocks are pre-defined in the software, the rest are user-defined blocks. The user-defined blocks are programmed by VHDL language. The functions of the IP blocks are as follows: • ZYNQ7 Processing System IP: the software interface around the Zynq-7000 Processing System. • AXI Interconnect IP: connects one or more AXI memory-mapped masterdevices to one or more memory-mapped slave devices • AXI Timer IP: create a trigger signal to synchronize data processing and communication between ARM processor core and FPGA. The trigger signal is created after every 50us that is configured by the software.
• AXI UART16550 IP: provide the controller interface for asynchronous serial data transfer. An interrupt signal at the ip2intc_irpt pin is created after receiving enough 8 bytes of data. • Pulse_train_v1.1 IP (user define IP): includes six pulse generator modules that generate the pulse signals for six servo motors. All the pulse generator modules are triggered by the trigger signal from the AXI timer so that they can start to generate pulses at the same time. • switch_v1.0 and encoder_v1.1 IP (user define IP): reads signal from encoder. Encoder IP only reads one encoder once, so switch IP is designed to choose the encoder. The encoder IP consists of Debounce Circuit and Decoder Circuit as shown in Figure 4 and Figure 5. The debounce circuit is used to reduce noise when the signal level changes.FF1 and FF2 always store the last two logic levels of the encoder signal.If the signal'slevel changes, the values of FF1and FF2 are differentin a clock cycle, the output of the XOR logic gate is HIGH and reset the counter. If the signal's level is unchanging, the output of the XOR logic gate is LOW and the counter begins to count. If the signal's logic level is stable, the counter continues to increment until it reaches the debounce_time value. The FF3 is enabled and the stable value is clocked through to the output. The counter disables itself until there is another change on one of the inputs.
Based on the operation of quadrature encoders [16], Table 1 lists the possible a and b input transitions, along with the values of direction and position. As seen in the truth table, the direction is determined by: Direction = a_new XOR b_prev    Table   Previous Inputs  New Inputs  Results  a_prev b_prev a_new b_new Direction Position  0  0  1  0  1  increase  1  0  1  1  1  increase  1  1  0  1  1  increase  0  1  0  0  1  increase  0  0  0  1  0  decrease  0  1  1  1  0  decrease  1  1  1  0  0  decrease  1  0  0  0  0  decrease The value of the counter only changes (increase or decrease) when the debounce time is met (stable signal) and has a rising edge or a falling edge of the encoder pulse. The signal edge is detected by using an XOR logic gate.
The algorithm to generate the pulse signal for servo motors in the Pulse_train IP is illustrated in Figure 6. The value of pulse width is sent from the ARM processor through AXI interconnect. The direction of rotation depends on the sign of the pulse width and is outputted to servo_dir pins. The pulse signal is generated on servo_i pins. S_AXI_ACLK is a synchronous signal for AXI communication and it is used to change the value of the "count" variable. The frequency of S_AXI_ACLK is set to 50 MHz.

ARM Software Design
The software will be divided into several smaller tasks. The tasks are performed depending on the command received from a teach pendant. After the ARM processor core starts, it conducts hardware initialization such as UART, Timer, GPIO and so on. Then, it will wait for the teach pendant commands to perform the tasks. Tasks can be online or offline. The online task means that after receiving a teach pendant command, the controller will immediately execute this command. Then, it waits for the next command. The online tasks include: set coordinate system, change speed, jogging commands, interpolation commands, … In the offline mode, the robot will receive a program that is written in the robot language, save the program in the memory. Then, the controller will encode and execute each command in the program. While implementing the program, except for an emergency stop command, all commands from the teach pendant will not be executed. Because we use the UART interrupt, the controller always receives data from the teach pendant, but when a program is running, only the emergency stop command is executed.

EXPERIMENT RESULTS
Our controller is used to control a Six-DOF robot arm which is shown in Figure 7. The controller receives commands from a teach pendant that is designed on a Samsung tablet (Figure 8). The robot canbe controlled to perform tasks such as pick and place, laser cutting, point welding, linear welding…  Figure 9 shows the results when applying the trapezoidal velocity profiles to create the angled command at each joint. All six joints are started and finished at the same time with a total time of 0.87s. The command angles of joint 4 and joint 5 aremuch smaller than the other joints, so they move with the triangle profiles with the maximum velocity of 40 deg/s.It should be noted that the motor is connected to the gearbox 50:1 ratio. The acceleration of joint 6 is set slightly larger than other joints. Figure 9shows the results of using the robot to draw some basic shapes.It can be seen that the robot can implement interpolation in the workspace and move smoothly.

CONCLUSION
In this paper, the 6 DOF industrial robot arm controller is designed based on the Zynq All Programmable SoC device. The main controller and motion controller are designed on a single board. So, the size of the controller is very compact, simple, low-cost and low power consumption.
The developed controller incorporates ARM's computational power and parallel processing capabilities of the FPGA, easily communicating with teach pendant and other peripherals. The ARM microprocessor was incorporated to take advantage of the high-level programming for algorithm calculations include kinematics algorithm and trajectory planning, handling the communication and processing data from teach pendant which inc-ludes decoding robot language and G-code. The FPGA-based motor motion control has given synchronously control of six servo motors which require a maximum frequency of up to 4Mpps so the motor can turn at high resolution and resulting in higher efficiency, less heat, increased stability, better speed control, higher torque to inertia ratio possibilities. Moreover, FPGA provides reprogrammable IO and hardware reconfiguration capability, so we can expand IO pins to control the end-effector tools and create more hardware for communication standards.
The experimental results show that the controller can control the robot to implement interpolation in both joint space and workspace leading to smooth movements. The robot can be used to perform most of the common tasks in industrial production.
In future work, AI algorithms will be integrated into our robot controller. AI algorithms are often used for robot motion planning in high-level control and artificial neural networks can be implemented with an FPGA in a System on Chip (SoC) device. The goal of an artificial neural network in the context of robot arm control is to train a deep policy neural network to create the optimal sequence of motion commands. The output of this policy network is torques or velocity commands for each actuator. Figure 11. Robot coordinates Transformation between two joints in a generic form is given by [15]: Robot coordinates shown in Figure 11are used to establish DH parameters ( Table 2). The DH parameters are substituted into equation (24) to find the transformation matrices, from 0 1 T to 5 6 T. The total transformation matrix between the base of the robot and link 6 is:

V U T T T T T T T V U T T T T T T T
where r ij is the orientation elements and (q x ,q y ,q z ) is the position elements. These elements are shown in the following equations: with s i = sinθ, c i = cosθ i c ij = cos(θ i +θ j ), s ij = sin(θ i +θ j ) The above equations are called forward kinematic equations that describe the relationship between theindividual joints of the robot manipulatorand the position and orientation of the end-effector. Contrarily, the inverse kinematic problemtransformsthepositionand orientationof theend-effector in the Cartesian space to the joint space.
To find the inverse kinematics solution, denote: The multiplications are carried out and yield the results as follows: