Optimal Robot Motion Planning of Redundant Robots in Machining and Additive Manufacturing Applications

Abstract

The paper deals with the generation of optimal trajectories for industrial robots in machining and additive manufacturing applications. The proposed method uses an Ant Colony algorithm to solve a kinodynamic motion planning problem. It exploits the kinematic redundancy that is often present in these applications to optimize the execution of trajectory. At the same time, the robot kinematics and dynamics constraints are respected and robot collisions are avoided. To reduce the computational burden, the task workspace is discretized enabling the use of efficient network solver based on Ant Colony theory. The proposed method is validated in robotic milling and additive manufacturing real-world scenarios.

1. Introduction

The growing demand for advanced industrial robotic applications such as machining and additive manufacturing highlights the importance of motion planning algorithms in this field [1,2]. As a consequence of this trend, a strong effort has been put into the improvement of the algorithm efficiency, especially in terms of reduction of the computing time. Moreover, researchers have investigated path and/or trajectory planners that are able to avoid collisions also taking into account the robot dynamics constraints [3,4,5,6].

The easiest motion planning problem consists in finding a collision-free point-to-point trajectory from a starting configuration to an end configuration without any constraints on the intermediate points. This problem is often addressed by means of path planning algorithms, which compute a suitable path between the points without taking into account constraints on velocity, accelerations, and torques [7,8,9,10]. The trajectory is then obtained by using a suitable time parametrization of the path [11,12]. Other approaches also consider constraints on velocity, acceleration and motor torque during the planning phase [13,14]. This class of problems is known as kinodynamic motion planning [15].

The lack of constraints on the intermediate points is an important drawback in many applications. For example, one might want to keep the robot tool oriented toward the floor during the whole trajectory for safety reasons. Recent works addresssed problem of this kind when the closed form of the constraints is not known a priori [16,17]. In this paper, we consider a more specific but still important class of trajectories, in which the number of degrees of freedom of the task is smaller than the one of the robot. This is a common scenario, as technological tasks often require to constrain only some of the Cartesian coordinates and velocities. Thus, the kinematic redundancy of the robot with respect to the task can be exploited to optimize a desired criterion [18,19,20,21]. It is worth stressing that, in this kind of tasks, path constraints are known a priori. This is a fundamental difference with respect to [16], because the dimension of the problem can be reduced by only exploring the subspace given by the kinematic redundancy.

The robotic machining and the additive manufacturing tasks are two significant technological examples where the use of industrial robots has become important and the motion planning strategies cover a crucial role [20,21]. However, the use of dedicated motion planning algorithms for these kind of applications is not common in the literature and in industry.

Thank to the improvements of robots kinematics and dynamics performance, the use of industrial robots in the field of machining applications has profitably increased during the last years [22]. However, industrial robots still have low kinematics and dynamics performance if compared to CNC machines, hence a kinodynamic motion planning can be a suitable solution to tackle such limitations. In machining tasks, kinematics constraints strictly depend on the process and hence by the adopted tool. In general, all the machining processes using a rotating tool have at least one redundant degree of freedom (DoF). The redundancy permits to choose an optimized robot configuration among an infinite set of solutions, as also clarified in Figure 1. Depending on the technological requirements of the specific machining task the redundant DoF/DoFs can be optimized to improve the process results [23].

Another emerging field for industrial robots is represented by the additive manufacturing and laser cladding, that is, metal material deposition with techniques such as laser metal deposition or electron beam melting [24,25,26]. The kinematic constraints with laser metal deposition technique consists in keeping the translation speed constant and identifying the best tilt of the laser head with respect to the underlying surface. [27,28,29,30] highlight that the process is feasible, although not optimal, for a large range of relative orientations between the deposition axis and the line perpendicular to the surface. Thus, it is possible to optimize the trajectory in order to keep the tilt angle as small as possible and respecting also kinodynamic constraints at the same time.

In particular, the main axis of the operating tool and the axis perpendicular to the surface to be machined/coated define an angle that should be always kept within certain limits depending on the specific process. As consequence, there is always (i) an operative redundancy (the rotation around the main axis of the operating tool) and (ii) a range of permitted angles around the axis perpendicular to the surface. The operative redundancy, namely the allowed cone around the axis perpendicular to the surface, can be exploited to find an optimal orientation able to satisfy both the technological constraints and the robot kinematics/dynamics limits. Figure 2 describes the general idea of these motion planning issues in additive manufacturing. Pipe welding [20] and gluing [31] applications can also be described with a similar optimization problem.

Although a-priori knowledge of closed-form Cartesian constraints reduces the complexity of the problem, it is not trivial to find the solution of the optimization problem. Indeed, the problem is PSPACE-hard [32], highly nonlinear and the constraints make the feasible subset of the configuration space disconnected due to direct kinematics is surjective but not necessary injective. Moreover, to obtain an effective obstacles avoidance, collision checks are mandatory with a consequent increment of the computing time. Thus, solvers face local minima and computational burden issues. Many works proposed tailored solutions on the specific application or the number of redundant DoFs: welding [20,33], machining and 5-axes applications [34,35], additive manufacturing [21]. However, there is a lack of a general solution for the entire family of problems. Looking at the 5-axes CNC machines, some works deal with a simplified version (because of the lower number of DoFsand the particular structure of CNC machines) of the technologically constrained planning problem. In [36] a complete overview of motion planning for CNC machines is presented. In particular, the focus of the paper is on the definition of the tool orientation in order to: avoid collisions with gauges, preserve the position within joints limits, and get smooth speed profiles. Moreover, [37] investigated the optimization of the orientation of the cutting tools in 5-axes machines. Unfortunately, most of the assumptions valid for a 5-axes CNC machine do not hold for industrial robots, where high rotation speed leads to high dynamic effort with resulting motion inaccuracies and vibrations.

This work proposes a unified framework to deal with kinodynamic motion planning applied to machining, welding, gluing, and additive manufacturing tasks. This allows us to cope with different technological constraints and objectives, avoiding the need of tailored solutions.

The approach is based on the formulation of the redundant task as a net, whose vertices are the admissible joint configurations and the edges are the movements between two subsequent configurations. In this formulation, path constraints define the set of admissible joint configurations, that is, the vertices of the net. Then, kinodynamics constraints determine the admissible edges. The resulting net represents the constrained motion planning problem.

The paper shows examples of industrial processes that can be expressed in this framework. It is worth noticing that also complex tasks can be addressed, such as the minimization of elastic deformations during milling processes. Moreover, the discrete nature of the approach also permits to set a trade-off between the quality of the solution and the computational time. This is especially true considering that the granularity of the discretization directly affects the number of collision checks that the algorithm needs to perform, which represent the main bottleneck from the computational point of view.

To solve the discrete motion planning problem, we propose a modified Ant Colony Optimization algorithm. Ant colony optimization is a meta-heuristic technique to find optimal path through graphs [38]. It is inspired by the behavior of some species of ants to find the optimal route from the anthill to food resources. The idea behind the algorithm is that the ants release a pheromone track on the nodes, which attracts other ants. This track evaporates through iterations but is reinforced by other ants if the nodes belong to good paths. In the end, the high-pheromone tracks converge to the optimal route from the starting point to the goal. Ant colony optimization is particularly suitable for nonlinear high-dimensional discrete optimization problems, which makes it a good candidate for the problem at hand. Different variants of the ant colony algorithm have been proposed in the literature, as detailed in [39]. In this paper, we combine two strategies to develop an improved Ant Colony solver. First, we exploit the rank-based ant-colony strategy proposed in [40], in which only the best ants of each iteration are selected to update the pheromone trails. This approach is very promising for the problem considered in this paper, because it makes possible to dramatically reduce the computational burden by performing lazy collision checking (namely, to check collisions only for the most promising vertexes). Then, we use the modified version of the Ant Colony algorithm proposed in [41] to avoid premature convergence to local minima, which is a typical issue in motion planning problems. In the proposed approach, a rank-based ant-colony with pheromone saturation is used to solve the planing problem. Comparison with other ant-colony approaches shows its effectiveness to manage redundancy in technological trajectories.

The proposed approach is applied to a real-work milling and additive manufacturing scenarios, where the deflection of the robot tool is successfully minimized throughout the process. Moreover, the proposed algorithm shows faster convergence rate compared to other Ant Colony Optimization solvers.

The paper is organized as follows. Section 7 introduces the notation used in the paper. Definitions related to the technological processes and constraints are given in Section 2. Section 3 describes the proposed framework and the definition of the motion planning problem as a discrete optimization problem. Section 4 focuses on the application of the proposed framework to common technological tasks in industrial processes. Section 5 describes the modified Ant Colony Optimization solver used in this work. Then, case study on machining applications is discussed in Section 6, where the proposed solver is compared with other ant-colony strategies. Conclusions are drawn in Section 7.

2. Technological Trajectories And Constraints

Technological trajectories are typically generated by CAM software as an ordered set of couples containing tool center point coordinates and the tool z-axis orientations, and the corresponding time instant. Therefore, it is possible to define:

Definition 1.

A technological path is a set of N desired transformation matrices, each denoted as ${}^{w}T{}^{c^k}$, $k=1,\dots ,N$.

Definition 2.

A technological trajectory is a technological path where an execution time instant $t_k$ is assigned to each kth pose.

Definitions 1 and 2 define the user input to the optimization problem. Given a technological trajectory, the desired velocity and acceleration twist vectors ${}^{w}W{}^{c^k}$ and ${}^w\dot{W}{}^{c^k}$ can be derived through numerical differentiation.

Notice that a technological trajectory does not define a unique Cartesian trajectories, but it corresponds to a family of possible Cartesian trajectories that have to satisfy some hard constraints (for example, the position of the tool center point) and some soft constraints (for example, limitations of the maximum tilt of a laser with respect to the surface). Hard constraints are mathematically represented as a set of equalities, whereas soft constraints can be modeled as a set of inequalities. It is worth stressing that time parametrization is an input for the motion planner, as well as the number of trajectory steps N.

Definition 3.

The path hard constraints $H{C}_{pos}$ are a set of equality constraints that limit the pose of the tool with respect to the base such that, for all $k=1,\dots ,N$:

$$\begin{array}{c}H{C}_{pos}({}^{w}T{}^{c^k},{}^{w}T{}^{ee})=0.\hfill \end{array}$$

(1)

The path soft constraints $S{C}_{pos}$ are a set of inequality constraints that limit the pose of the tool with respect to the base such that, for all $k=1,\dots ,N$:

$$\begin{array}{c}S{C}_{pos}({}^{w}T{}^{c^k},{}^{w}T{}^{ee})\le 0.\hfill \end{array}$$

(2)

Definition 4.

Kinodynamics hard constraints are defined as a set of equality constraints that limit the tool velocity and acceleration with respect to the base such that, for all $k=1,\dots ,N$:

$$\left\{\begin{array}{c}H{C}_{vel}({}^{w}W{}^{c^k},{}^{w}W{}^{ee})=0\hfill \\ H{C}_{acc}({}^w\dot{W}{}^{c^k},{}^w\dot{W}{}^{ee})=0\hfill \end{array}\right.$$

(3)

Definition 5.

Kinodynamics soft constraints are defined as a set of inequality constraints that limit the tool velocity and acceleration with respect to the base such that, for all $k=1,\dots ,N$:

$$\left\{\begin{array}{c}S{C}_{vel}({}^{w}W{}^{c^k},{}^{w}W{}^{ee})\le 0\hfill \\ S{C}_{acc}({}^w\dot{W}{}^{c^k},{}^w\dot{W}{}^{ee})\le 0\hfill \end{array}\right.$$

(4)

3. Proposed Framework for Task Formalization

By considering constraints (1)–(2), the set ${}^w\mathcal{X}{}^{e{e}^k}$ of feasibile Cartesian paths can be defined as the set of all possible transformation matrices ${}^{w}T{}^{ee}$ that satisfy (1) and (2) at each step k, that is:

$${}^w\mathcal{X}{}^{e{e}^k}=\{{}^{w}T{}^{ee}:H{C}_{pos}({}^{w}T{}^{c^k},{}^{w}T{}^{ee})=0\wedge S{C}_{pos}({}^{w}T{}^{c^k},{}^{w}T{}^{ee})\le 0\}.$$

(5)

Technological tasks typically have a small number of redundant DoFs. For example, milling and deburring tasks require 5 DoFs and are typically performed by a 6-DoF manipulator. The exploration of the subset of Cartesian given by the redundancy is therefore practical. In particular, such space can be discretized and, for each sample, the closed-form inverse kinematics of the manipulator can be solved. We therefore refer to ${}^w\mathtt{X}{}^{e{e}^k}$ as the discretization of the continuous set ${}^w\mathcal{X}{}^{e{e}^k}$. Inverse kinematic problem is then solved for all the elements of ${}^w\mathtt{X}{}^{e{e}^k}$. Details about the discretization process for different technological tasks are described in Section 4.1 and Section 4.2.

Definition 6.

For each k, the feasible configuration set ${\mathtt{Q}}^k$ is the set of all joint configurations which correspond to a transformation matrix in ${}^w\mathtt{X}{}^{e{e}^k}$ and that respect the joint limits, that is:

$${\mathtt{Q}}^k=\left\{q\in I_p:\forall {}^{w}T{}^{ee}\in {}^w\mathtt{X}{}^{e{e}^k},f_{kin,p}\left(q\right)={}^{w}T{}^{ee}\right\}.$$

(6)

The elements of each set ${\mathtt{Q}}^k$ are the net vertex. The joint configuration $q_{i_k}$ is the element $i_k$ of set ${\mathtt{Q}}^k$, while ${}^{w}T{}^{ee}\left(i_k\right)=f_{kin,p}\left(q_{i_k}\right)$ is the corresponding transformation matrix. The transition between the element $i_k$ of set ${\mathtt{Q}}^k$ to the element $i_{k+1}$ of set ${\mathtt{Q}}^{k+1}$ is the net edge.

In order to check if constraints (3)–(4) are satisfied, it is necessary to know the edges that connect ${\mathtt{Q}}^1$ to ${\mathtt{Q}}^N$. We therefore define a network path as follows.

Definition 7.

A network path p is an ordered sequence $\{q_{i_1},\cdots ,q_{i_N}\}$ that connects the configuration sets ${\mathtt{Q}}^1,\dots ,{\mathtt{Q}}^N$.

This idea is also clarified in Figure 3. Given a network path p, it is possible to compute the finite-difference approximations of Cartesian velocities, joint velocities, joint accelerations, and joint efforts. Joint velocity and acceleration at step k are computed as:

$${\dot{q}}_k(i_k,i_{k-1})={\displaystyle \frac{q_{i_k}-q_{i_{k-1}}}{t_k-t_{k-1}}}$$

$${\ddot{q}}_k(i_{k+1},i_k,i_{k-1})={\displaystyle \frac{{\dot{q}}_k(i_{k+1},i_k)-{\dot{q}}_k(i_k,i_{k-1})}{t_{k+1}-t_{k-1}}}$$

Joint effort can be computed using inverse dynamics:

$${\tau }_k(i_{k+1},i_k,i_{k-1})=f_{dyn}\left(q_k,{\dot{q}}_k(i_k,i_{k-1}),{\ddot{q}}_k(i_{k+1},i_k,i_{k-1})\right)$$

Velocity and acceleration twists are computed as:

$${}^{w}W{}^{e{e}^k}(i_k,i_{k-1})=f_{kin,v}(q_k,{\dot{q}}_k(i_k,i_{k-1}))$$

$${}^w\dot{W}{}^{e{e}^k}(i_{k+1},i_k,i_{k-1})=f_{kin,a}(q_k,{\dot{q}}_k(i_k,i_{k-1}),{\ddot{q}}_k(i_{k+1},i_k,i_{k-1}))$$

A path is feasible if these values are inside the joint limits and Equations (3) and (4) hold.

In order to evaluate the goodness of the paths, it is necessary to define a cost function which depends on both the vertices and the edges. Vertex costs depend only on the path, while edge costs depend on kinodynamics quantities. The following definition of the trajectory planning problem therefore can be applied:

Definition 8.

The robot trajectory optimization is a minimization problem defined as follows:

$$\underset{p\in \mathcal{P}}{min}\Gamma \left(p\right)$$

(7)

where:

$$\Gamma \left(p\right)=\sum _{k=1}^N{f}_V\left(i_k\right)+\sum _{k=1}^{N-1}{f}_E(i_{k+1},i_k,i_{k-1})$$

(8)

$$\begin{array}{cc}\hfill \mathcal{P}=\{p:& H{C}_{vel}({}^{w}W{}^{c^k},{}^{w}W{}^{e{e}^k}(i_k,i_{k-1}))=0,\hfill \\ \hfill & H{C}_{acc}({}^w\dot{W}{}^{c^k},{}^w\dot{W}{}^{e{e}^k}(i_{k+1},i_k,i_{k-1}))=0,\hfill \\ \hfill & S{C}_{vel}({}^{w}W{}^{c^k},{}^{w}W{}^{e{e}^k}(i_k,i_{k-1}))=0,\hfill \\ \hfill & S{C}_{acc}({}^w\dot{W}{}^{c^k},{}^w\dot{W}{}^{e{e}^k}(i_{k+1},i_k,i_{k-1}))=0,\hfill \\ \hfill & q_{i_k}\in I_p,\hfill \\ \hfill & {\dot{q}}_k(i_k,i_{k-1})\in I_v,\hfill \\ \hfill & {\ddot{q}}_k(i_{k+1},i_k,i_{k-1})\in I_a,\hfill \\ \hfill & {\tau }_k(i_{k+1},i_k,i_{k-1})\in I_e,\hfill \\ \hfill & {}^{w}W{}^{e{e}^k}(i_k,i_{k-1})\in C_v,\hfill \\ \hfill & f_{col}\left(q_{i_k}\right)=0\}\hfill \end{array}$$

(9)

and where $f_V$ and $f_E$ are cost functions depending respectively on the vertices and the edges of the network path.

4. Examples of Common Technological Tasks

Based on the definitions given above, two typical problems of additive manufacturing, welding, and gluing applications are discussed in details: (i) a 5-DoF task, typical of machining applications, where the goal is to execute the trajectory by minimizing the deflections caused by the material removing forces; (ii) a 3-DoF task, where the goal is to reduced the angle between the end-effector and the surface normal to the tool z-axis.

4.1. 5-DoF Tasks

In 5-DoF tasks (such as deburring and milling), path hard constraints are related to the end-effector position and its z-axis orientation, while there are no soft path constraints. Path constraints are therefore given by:

$$H{C}_{pos}=\left\{\begin{array}{c}\left({}^{w}T{}^{c^k}-{}^{w}T{}^{ee}\right){\left[0,0,0,1\right]}^T=0\hfill \\ \left({}^{w}T{}^{c^k}-{}^{w}T{}^{ee}\right){\left[0,0,1,0\right]}^T=0\hfill \end{array}.\right.$$

(10)

Moreover, machining tasks require hard constraints on end-effector Cartesian linear velocity, that is, for all $k=1,\dots ,N$:

$$H{C}_{vel}=\begin{array}{c}\left({}^{w}W{}^{c^k}-{}^{w}W{}^{ee}\right){\left[0,0,0,1\right]}^T=0\hfill \end{array}.$$

(11)

The redundancy can be exploited to optimize the manipulator stiffness along the direction of the cutting forces of the milling process. A possible choice of $f_V\left(q^k\right)$ is therefore the squared norm of the so-called Cartesian Compliance Index, that is:

$$f_V\left(i_k\right)={∥{}^w\delta x^{ee}\left(i_k\right)∥}_M^2$$

(12)

where ${\parallel ·\parallel }_M$ is the ${\ell }_2$-norm weighted by vector M, and ${}^w\delta x^{ee}\in {\mathbb{R}}^6$ is a vector representing angular and linear deflections of the end-effector due to the milling force. The Cartesian Compliance Index is a scalar measure that represents the weighted norm of deflections introduced by robot elasticity. Minimizing this index improves quality of the technological task.

Deflection ${}^w\delta x^{ee}$ is computed as

$${}^w\delta x^{ee}\left(i_k\right)=J\left(q_{i_k}\right)K^{-1}J{\left(q_{i_k}\right)}^T{W}_e\left(k\right)$$

(13)

where $W_e\left(k\right)\in {\mathbb{R}}^6$ is the wrench vector given by the torque and the force generated by the milling process at the step k, J is the geometric Jacobian matrix, and K is the joint stiffness matrix. Milling force is computed by using model proposed in [42], force application point is the end-effector frame origin the milling torque is null.

The edge cost function $f_E$ is chosen as the sum of the squared norms of the joint velocity and acceleration vectors, that is:

$$f_E(i_{k+1},i_k,i_{k-1})={\lambda }_v{∥{\dot{q}}_k(i_k,i_k-1)∥}^2+{\lambda }_a{∥{\ddot{q}}_k(i_{k+1},i_k,i_{k-1})∥}^2.$$

(14)

where ${\lambda }_v>0$ and ${\lambda }_a>0$ are weighting constants.

Since there is only one redundant DoF, uniform gridding can be applied without curse of dimensionality. As mentioned in Section 3, the redundant degree of freedom is uniformly sampled between its limits obtaining a set of transformation matrices. Inverse kinematics is then computed to compute sets ${\mathtt{Q}}_1,\cdots ,{\mathtt{Q}}_N$. The discretization step can be chosen as a trade-off between the computational burden and the quality of the solution.

4.2. 3-DoF Tasks

In 3-DoF tasks (such as additive manufacturing, welding and gluing), path hard constraints are related to the end-effector position and its z-axis orientation. The following path constraints should therefore hold for all $k=1,\dots ,N$:

$$H{C}_{pos}=\left\{\begin{array}{c}\left({}^{w}T{}^{c^k}-{}^{w}T{}^{ee}\right){\left[0,0,0,1\right]}^T=0\hfill \\ \left({}^{w}T{}^{c^k}-{}^{w}T{}^{ee}\right){\left[0,0,1,0\right]}^T=0\hfill \end{array}.\right.$$

(15)

Soft path constraints consist in imposing the z-axis of ${}^{w}T{}^{ee}$ to be inside a cone with aperture equal to $2\theta$ such that, for all $k=1,\dots ,N$:

$$S{C}_{pos}=cos\theta -{\left({}^{w}T{}^{c^k}{\left[0,0,1,0\right]}^T\right)}^T\left({}^{w}T{}^{ee}{\left[0,0,1,0\right]}^T\right)\le 0.$$

(16)

These tasks also require hard constraints on end-effector Cartesian linear velocity, that is, for all $k=1,\dots ,N$:

$$H{C}_{vel}=\begin{array}{c}\left({}^{w}W{}^{c^k}-{}^{w}W{}^{ee}\right){\left[0,0,0,1\right]}^T=0\hfill \end{array}.$$

(17)

Vertex cost $f_V$ is the norm of the angle between the z-axis of the end effector and the z-axis of the desired pose, that is:

$$f_V\left(i_k\right)={∥arccos\left({\left({}^{w}T{}^{c^k}{\left[0,0,1,0\right]}^T\right)}^T\left({}^{w}T{}^{e{e}^k}\left(i_k\right){\left[0,0,1,0\right]}^T\right)\right)∥}^2$$

(18)

Edge cost term $f_E$ minimizes the sum of squared norms of the joint velocity and acceleration vectors as in the 5-DoF case (14).

Since redundancy subspace is three-dimensional, memory footprint can be a limit factor. In this case, uniform random sampling can be used. By using ZYX Euler angles coordinates, the y- and x-axis are rotated by a randomly sampled angle in $[-\theta ,\theta ]$, while the z-axis angle is randomly sampled in $[-\pi ,\pi ]$. Then, the resulting transformation matrix is stored if (16) is respected. In order to avoid oversampling in some regions of the redundant subspace, a new sample is discarded if the distance with the closest sample is smaller than a chosen threshold. The number of samples can be chosen as a trade-off between computational burden and the quality of the solution.

5. Ant Colony Optimization Algorithm

The robot trajectory optimization stated in Definition 8 is a discrete optimization problem that can be solve by means of an Ant Colony Optimization solver. The proposed solver uses a $Max-Min$ strategy [41], where a rank-based selection of the best m ants contribute to pheromone updates [40].

The quantity of pheromone added to the vertex depends on the quality of the ant network path (namely, it is inversely proportional to the related cost function). The pheromone evaporation is performed by applying an exponential decay after the passage of all ants. In order to avoid early stalling, the values of the pheromone is bounded between ${\nu }_{min}$ and ${\nu }_{max}$ as in [41]. The ants choose the next vertex based on a probability inversely proportional to the vertex cost $f_V$. Such probability is given by:

$$\pi \left(i_k\right|i_{k-1})=\frac{\nu \left(i_k\right)\eta (i_k,i_{k-1})}{{\sum }_{j_k=1}^{C_k}\nu \left(j_k\right)\eta (j_k,i_{k-1})},$$

(19)

where $\pi \left(i_k\right|i_{k-1})$ is the probability to go from vertex $i_{k-1}$ to $i_k$, $\nu \left(i_k\right)$ is the pheromone value of vertex $i_k$, $C_k$ is the cardinality of ${\mathtt{Q}}^k$,

$$\eta (i,j)=\left\{\begin{array}{cc}{\left(\xi +f_V\left(i\right)\right)}^{-1}\hfill & \mathrm{if}\mathrm{the}\mathrm{transition}\mathrm{is}\mathrm{feasible}\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right..$$

(20)

and $\xi >0$ has a small value to avoid division by zero.

Thus, the Ant Colony algorithm can be summarized as:

• Set ${}^w\mathcal{X}{}^{e{e}^k}$ is discretized into set ${}^w\mathtt{X}{}^{e{e}^k}$, sampling in redundancy subspace;
• Set ${\mathtt{Q}}^k$ is obtained by solving the inverse kinematic problem for all elements of ${}^w\mathtt{X}{}^{e{e}^k}$;
• The pheromone value of each vertex is initialized equal to ${\nu }_{max}$;
• Each ant randomly selects the next vertex based on the transition probability Equation (19);
• Ants are ranked based on the path cost;
• m best ants are selected, collision checking is performed on the vertexes which are selected the first time. If an ant path is in collision, this ant is replaced by the next ant in the ranking;
• The best solution found since the beginning of the whole optimization is added to the set of the best ants found at the current iteration;
• The best ants update the pheromone values of the vertexes of their path by adding a quantity that is inversely proportional to the cost function of its entire network path, that is:

  $$\nu \left(i_k\right)\leftarrow \nu \left(i_k\right)+\frac{1}{\Gamma \left(p\right)}.$$

  (21)
• The pheromone evaporation is performed for all the vertexes

  $$\nu \leftarrow min\left({\nu }_{max},max\left((1-\rho )\nu ,{\nu }_{min}\right)\right).$$

  (22)
• If termination conditions are not satisfied, go to step 4.

Termination conditions are the convergence of ants to the best path and the convergence of the pheromone level to a steady value, as suggested in [43]. It is worth highlighting that if in Step 6 there are less the m ants with a feasible path, all of them are selected. If no ant provides a feasible path, the optimization problem is aborted.

6. Case Studies

6.1. Milling Task

Algorithm effectiveness is analyzed in the milling task scenario shown in Figure 4. As stated in Section 4.1, the redundancy optimization criterion consists in minimizing the deflections caused by milling forces.

Weighting vector M in (12) is set equal to

$$M=\left[1000,1000,1000,\frac{180}{\pi },\frac{180}{\pi },0\right]$$

by imposing that a linear deflection equal to 1 [mm] has the same influence of an angular deflection equal to 1 [deg], and neglecting deflections around the redundant axis. Weighting factors are set equal to ${\lambda }_v={10}^{-5}$ and ${\lambda }_a={10}^{-7}$, respectively.

The considered robot model is a 6-DoF Comau NS16. The algorithm runs on a laptop equipped with a Intel Core i7-8565U, 16 GB RAM-LPDDR3 and a 512 GB SSD. The stiffness matrix was derived by following the identification procedure described in Chapter 4 of [44] and is equal to:

$$K=\mathrm{diag}\left(3.7·{10}^5,4.5·{10}^5,{10}^6,{10}^6,{10}^6\right)[\mathrm{Nm}/\mathrm{rad}]$$

The transformation matrix between the robot base frame and the workspace frame is

$${}^{base}T{}^w=\left[\begin{array}{cccc}\hfill 0.9968,& \hfill -0.0796,& \hfill 0.0001,& \hfill 0.9936\\ \hfill 0.0796,& \hfill 0.9968,& \hfill 0.0000,& \hfill -0.1699\\ \hfill -0.0001,& \hfill 0.0000,& \hfill 1.0000,& \hfill 0.6716\\ \hfill 0,& \hfill 0,& \hfill 0,& \hfill 1.0000\end{array}\right]$$

while the transformation matrix between the robot flange and the end-effector frame is

$${}^{flange}T{}^{ee}=\left[\begin{array}{cccc}\hfill 0.9991,& \hfill 0.0274,& \hfill 0.0334,& \hfill 0.1857\\ \hfill -0.0273,& \hfill 0.9996,& \hfill -0.0018,& \hfill 0.0002\\ \hfill -0.0335,& \hfill 0.0009,& \hfill 0.9994,& \hfill 0.1389\\ \hfill 0,& \hfill 0,& \hfill 0,& \hfill 1.0000\end{array}\right]$$

The milling trajectory is a 100 [mm] × 100 [mm] square with rounded corner (radius 5 [mm]), the center of the square is shifted by 50 [mm] from the the workspace origin along x and y directions, as shown in Figure 5, where also milling forces are represented. The milling parameters are in

Table 1. Technological parameters used to compute the milling forces described in [42]. Parameters are referred to aluminum milling.

b is the axial depth of cut	1.5 [mm]
$\mathsf{\Omega }$ is the spindle speed	13,000 [rpm]
$f_v$ is the feed velocity	1080 [mm/min] = 18 [mm/s]
${\varphi }_{ex}$ arc exit angle	3.14 [rad]
${\varphi }_{in}$ arc enter angle	1.0472 [rad]
$K_t$ is the tangential cutting coefficient	600 [N/mm${}^2$]
$K_r$ is the radial cutting coefficient	300 [N/mm${}^2$]
$K_{tee}$ is the tangential edge coefficient	10 [N/mm${}^2$]
$K_{ree}$ is the radial edge coefficient	5 [N/mm${}^2$]
$\alpha$ is the helix angle	0.6981 [rad]

.

The milling path is discretized in 157 points. In each of them, the redundancy angle is discretized by using 721 values equally spaced in the range [−$\pi$,$\pi$] (namely, sampling step equal to 0.5 [deg]). By computing the inverse kinematic for each value of the redundancy angle, the cardinality of sets ${\mathtt{Q}}_1,\cdots ,{\mathtt{Q}}_N$ varies between 1442 and 1868, depending on joint limits and possible collisions. Memory footprint of each vertex is 64 Byte (6 double variables for joint configuration, one double variable for the cost $f_V\left(q_{i_k}\right)$, one double variable for pheromone value), the footprint of the all net vertexes is 16 Mbyte.

We denote with RBMM (Rank-Based Max-Min) the solver proposed in Section 5. We compare it with:

• a standard Ant Colony Optimization solver (denoted as ACO) [38];
• a rank-based Ant Colony approach (denoted as RB) [40];
• a $Max-Min$ Ant Colony approach (denoted as MM) [41];
• a local solver (denoted as LS) where $q_{i_1}$ is selected as the vertex with minimum cost in ${\mathtt{Q}}_1$ and, for each step k, the next vertex $q_{i_{k+1}}$ is selected as the vertex with minimum cost in ${\mathtt{Q}}_{k+1}$ which can be reached from $q_{i_k}$ with a feasible transition.

In order to compare different algorithms performances, Problem (7) is solved 10 times for each algorithm. The chosen performance indices are the root mean square value of the Cartesian Compliance Index ($M{}^w\delta x^{ee}$) and the time taken by the algorithm to find a solution.

Table 2. Average and standard deviation of the Cartesian Compliance Index (root mean square value) and of the computational time.

Method	Cartesian Compliance Index [mm]	Computational Time [s]
RBMM	0.0682 ± 0.0019	2.21 ± 0.18
ACO [38]	0.0716 ± 0.0028	3.91 ± 1.16
RB [40]	0.0701 ± 0.0024	2.22 ± 0.22
MM [41]	0.0695 ± 0.0019	3.15 ± 0.93
LS	0.0829 ± 0.0000	0.51 ± 0.02

shows the average value and the standard deviation of the indices for the different methods. On the one hand, RBMM and MM converge to a better solution thanks to the pheromone saturation, which avoids stagnation in local minima. On the other hand, RBMM and RM show lower computational times, thank to the rank-based ant selection and the consequent reduction of collision checking. Notice that LS gives the worst solution, due to its deterministic and local nature.

The obtained deflection trends during the 10 runs of RBMM algorithm are shown in Figure 6, highlighting the algorithm repeatability.

6.2. Additive Manufacturing

An additive manufacturing task is considered. The technological path is a spiral defined as

$${}^w{T}^{w^k}=\left[\begin{array}{cccc}1& 0& 0& x\left(\theta \right)\\ 0& -1& 0& y\left(\theta \right)\\ 0& 0& -1& z\left(\theta \right)\\ 0& 0& 0& 1\end{array}\right]$$

(23)

where $\theta \in [0,4\pi ]$ is the curvilinear abscissa

$$\left\{\begin{array}{c}x\left(\theta \right)=0.8+\left(0.3-0.01\frac{\theta }{2\pi }\right)cos\left(\theta \right)\\ y\left(\theta \right)=0.5+\left(0.3-0.01\frac{\theta }{2\pi }\right)sin\left(\theta \right)\\ z\left(\theta \right)=0.5+\frac{0.05}{2\pi }\theta .\end{array}\right.$$

(24)

The curve is depicted in Figure 7.

The end-effort has to follow the curve during the time period $t\in [0,20]$ with the following timing law

$$\begin{array}{c}\hfill s\left(t\right)=\left\{\begin{array}{cc}\frac{1}{2}0.0569t^2\hfill & \mathrm{if}t<4\hfill \\ 0.4555+0.2278(t-4)\hfill & \mathrm{if}4\le t\le 16\hfill \\ 0.4555+0.2278(t-4)-\frac{1}{2}0.0569{(t-16)}^2\hfill & \mathrm{if}16<t\le 20.\hfill \end{array}\right.\end{array}$$

(25)

In this way, the modulus of Cartesian velocity is constant when $t\in [4,16]$, as shown in Figure 8.

The considered robot model is a 6-DoF Comau NS16, while the algorithm runs on a laptop equipped with a Intel Core i7-8565U, 16 GB RAM-LPDDR3 and a 512 GB SSD.

The additive path is discretized in 200 points. The cone apperture is set equal to $2\theta =25$ [deg]. In each of them, 2356 uniform-distributed transformation matrices respecting (16) are considered. By computing the inverse kinematic for each value of the redundancy angle, the cardinality of sets ${\mathtt{Q}}_1,\cdots ,{\mathtt{Q}}_N$ varies between 9424 and 18,848, depending on joint limits and possible collisions. As in Section 6.1, memory footprint of each vertex is 64 Byte, the footprint of the all net vertexes is 171 Mbyte.

The comparison is made by using the same algorithm of Section 6.1. In order to compare different algorithms performances, Problem (7) is solved 10 times for each algorithm. The chosen performance indices are the root mean square value of the angle (in degrees) w.r.t. the nominal path and the time taken by the algorithm to find a solution.

Table 3. Average and standard deviation of the angle between computed and nominal path. (root mean square value) and of the computational time.

Method	Angle [deg]	Computational Time [s]
RBMM	7.902 ± 0.251	5.66 ± 0.48
ACO [38]	8.061 ± 0.378	5.91 ± 1.96
RB [40]	7.924 ± 0.353	5.72 ± 0.55
MM [41]	7.952 ± 0.242	8.17 ± 2.93
LS	8.291 ± 0.000	0.91 ± 0.02

shows the average value and the standard deviation of the indices for the different methods. Considerations of Section 6.1 are confirmed also in this case study. RBMM and MM converge to a better solution avoiding stagnation in local minima. Moreover, RBMM and RM show lower computational times, thank to the rank-based ant selection and the consequent reduction of collision checking. Finally, LS gives the worst solution, due to its deterministic and local nature.

Also in this case, the algorithm repeatability is shown in Figure 9, where results of 10 runs are depicted.

7. Conclusions

This paper has proposed a unified motion planning framework to deal with under-defined Cartesian trajectories in technological applications, such as milling, additive manufacturing, welding, and gluing applications. In particular, we have used this framework to formulate two common technological problems by taking into account their particular technological requirements. The proposed formulation is based on the discretization of the configuration space: the allowed Cartesian space is sampled and a closed-form inverse kinematics is applied to obtain an admissible set of joint configurations for each point of the task. This leads to a discrete optimization problem, which is solved by a modified Ant Colony Optimization solver based on Rank-based and $Max-Min$ strategies. Notice that the formulation of the problem implicitly respects the constraints of the robot such as position, velocity, acceleration, and torque limits. The effectiveness of the method is demonstrated in real-word milling and additive manufacturing scenarios. Results show that the proposed approach is able to optimize the task by also taking into account complex elastic behaviours during the process, such as the deflection of the robot tool. A comparison with other Ant Colony solvers shows that the proposed algorithm has faster converge rate and it has a better repeatability then other Ant Colony Optimization implementations.