3.1.1. Task demonstration and data collection

In the task demonstration, the yarn storage paths of the two mechanisms at different starting points $(X_{\textit{start}},Y_{\textit{start}},Z_{\textit{start}})$ and ending points $(X_{end},Y_{end},Z_{end})$ are taught. Each path is composed of a sequence of path points $(x_{i},y_{i},z_{i})$ . Collect teaching data for storing yarn process in yarn storage mechanism 2 and yarn winding mechanism 5. First, B-spline interpolation is used to convert the sparse path points into rich training data due to the sparsity of teaching path points. Second, split the coordinate values according to the coordinate direction and normalize them to obtain the coordinate sequences $t_{x}, t_{y}$ , and $t_{z}$ for the directions $x, y$ , and $z$ , respectively. Finally, the training samples $D_{x}=\{X,t_{x}\}, D_{y}=\{Y,t_{y}\}$ and $D_{z}=\{Z,t_{z}\}$ for each coordinate are composed, where $X, Y$ , and $Z$ are the sets of starting and ending points in the x, y, and z directions, respectively.

3.1.2. Network model training by ELM

ELM has high learning accuracy and less network training time, and its output is that

(1) \begin{equation} t_{zj}=o=\sum _{i=1}^{L}\beta _{i}f_{i}\!\left(Z_{j}\right)=\sum _{i=1}^{L}\beta _{i}g\!\left(W_{i}^{T}Z_{j}+b_{i}\right),j=1,\ldots,N \end{equation}

In Eq. (1), L represents the number of hidden nodes, N represents the number of training samples, and $g(x)$ represents the activation function. $W_{i}=[w_{i,1},w_{i,2}]$ is the weight vector connecting the ith hidden node and the input node, $\beta _{i}=[\beta _{i,1},\beta _{i,2},\ldots,\beta _{i,m}]$ is the weight vector connecting the ith hidden node and the output node, and $b_{i}$ is the deviation of hidden layer. The values of $W_{i}$ and $b_{i}$ are randomly given during the training process, and the minimum square method is used to minimize the error of the output result

(2) \begin{equation} \min \!\left(H\beta ^{T}-T\right) \end{equation}

(3) \begin{equation} H\!\left(W,Z,b\right)=\left[\begin{array}{c@{\quad}c@{\quad}c} g\!\left(w_{1}^{T}Z_{1}+b_{1}\right) & \ldots & g\!\left(w_{L}^{T}Z_{1}+b_{L}\right)\\ \ldots & \ldots & \ldots \\ g\!\left(w_{1}^{T}Z_{N}+b_{1}\right) & \ldots & g\!\left(w_{L}^{T}Z_{N}+b_{L}\right) \end{array}\right]\in R^{N\times L} \end{equation}

In Eq. (2), $H$ is the hidden layer output matrix, $\beta$ is obtained from $\hat{\beta }^{T}=H^{\dagger }T$ where $H^{\dagger }$ is the Moore–Penrose generalized inverse matrix of matrix $H$ .

The ELM network model is trained by training samples $D_{x}=\{X,t_{x}\}, D_{y}=\{Y,t_{y}\}$ , and $D_{z}=\{Z,t_{z}\}$ to obtain the mapping relationships of $f_{x}\colon X\rightarrow t_{x}, f_{y}\colon Y\rightarrow t_{y}$ , and $f_{z}\colon Z\rightarrow t_{z}$ . The training framework of ELM is shown in Fig. 5. $X$ remains unchanged since there is no guide rail in the x direction, $f_{x}=t_{x}$ ; the independent variable of $f_{y}$ is $Y_{end}$ since the starting point of the system path is located at the vertical centerline of yarn fixing unit 1; and the independent variables of $f_{z}$ are $Z_{\textit{start}}$ and $Z_{end}$ .

Figure 5. Training framework of ELM.

3.1.3. The output of imitation path

According to the position of the mechanism on the vertical sliding guide rail 6 or horizontal sliding guide rail 7, input the starting point $(x_{\textit{start}},y_{\textit{start}},z_{\textit{start}})$ and ending point $(x_{end},y_{end},z_{end})$ into the mapping relationship to obtain the imitation path $(t'_{x},t'_{y},t'_{z})$ that resembles the yarn storage actions in the teaching path.

3.2.1. Collision detection

In order to accurately describe the robot and obstacle models, three envelope types of capsule, sphere, and cuboid are used to envelop the basic weaving unit. The algebraic method in literature [Reference Kong and Yu38] can be used to quickly realize the collision detection between capsule and capsule, capsule and sphere, and sphere and sphere. However, the collision detection between cuboid and capsule and between cuboid and sphere is more complicated. In such cases, the capsule and sphere need to be converted into convex multilateral bodies, and then GJK algorithm [Reference v. d. Bergen39] can be used to detect the collision. The capsule body can be regarded as a combination of cylinder and hemisphere, as shown in Fig. 6. Similarly, the sphere is a combination of two hemispheres, and the outer tangential support point of the cylinder and hemisphere is the vertex of the convex multilateral body.

Figure 6. Convex multilateral body converted from cylinder and sphere transformation.

A circumscribed prism with n edges is created on a cylinder with a radius of $r_{c}$ . The coordinates of the support points of the cylinder are expressed as

(4) \begin{equation} {}^{c}{C}{_{ij}^{}}=A_{i}+\frac{r_{c}}{\cos \theta }\!\left[\cos \!\left(2\!\left(j-1\right)\theta \right),\sin \!\left(2\!\left(j-1\right)\theta \right),0\right],\,i=1,2;\ j=1,2,\ldots,n \end{equation}

In Eq. (4), $\theta =\pi /\mathrm{n}$ . For a sphere with radius $r_{s}$ , the vertices of outer tangent polygon for m parallel circles are taken as the support points of the sphere, which are expressed as

(5) \begin{equation} {}^{s}{S}{_{ij}^{}}=O_{i}+\frac{r_{i}}{\cos \theta }\!\left[\cos \!\left(2\!\left(j-1\right)\theta \right),\sin \!\left(2\!\left(j-1\right)\theta \right),0\right],\,i=1,2,\ldots,m;\ j=1,2,\ldots,n \end{equation}

In Eq. (5), $O_{i}=\left[0,0,h_{i}\right], h_{i}=\frac{i}{m+1}r_{s}$ , and $r_{i}=\sqrt{r_{s}^{2}-h_{i}^{2}}$ . In order to detect the collision, a conversion process is carried out on the coordinates from $\{O_{c}\}$ to $\{O_{b}\}$ and $\{O_{s}\}$ to $\{O_{b}\}$ , obtaining the support points ${}^{b}{C}{_{ij}^{}}$ and ${}^{b}{S}{_{ij}^{}}$ . The GJK algorithm is then utilized for the collision detection of cuboid and sphere.

A detection queue is formed by the envelope of all obstacles. The method for collision detection is chosen based on the type of envelope to detect collision with each linkage of the robot individually. If there is any collision between the envelope of obstacle and the envelope of robot linkage, it will be flagged as a collision. Fig. 7 illustrates the basic weaving unit enveloped by capsule, sphere, and cube envelope.

Figure 7. The basic weaving unit enveloped by capsule, sphere, and cube envelope.

3.2.2. IRRT algorithm

The RRT algorithm is a path planning method based on random sampling. The classic RRT algorithm first constructs a tree with the root node as the starting point in the joint space. Then a random point $q_{rand}$ is generated in the joint space, and the node $q_{near}$ closest to $q_{rand}$ is found through traversing the tree. As the parent node, $q_{near}$ expand a step along the $q_{rand}$ direction to generate a new node $q_{new}$ . Finally, the each link position of the robot in $q_{new}$ is calculated to detect the collision. If the collision occurs, the point is discarded, otherwise, $q_{new}$ is added to the tree. If the distance between $q_{new}$ and $q_{goal}$ is less than the set threshold, the path expansion is successful. The above process is repeated until the expansion is successful or the number of iterations is exceeded.

The expansion process of the basic RRT algorithm is time consuming to converge to the target point in high-dimensional space. In addition, the step length of the joint space movement requires repeated trial and error processes. For this reason, as shown in Fig. 8, an improved RRT algorithm with adaptive direction and step size, namely the IRRT algorithm, is proposed to improve path quality.

Figure 8. Schematic diagram of IRRT.

(1) Adaptive direction.

The adaptive direction aims to synthesize the expansion direction according to the target gravitational vector and the random gravitational vector. The directions of the target gravitational vector and the random gravitational vector are determined by the target point and the random sampling point, respectively. The gravitational force of $q_{goal}$ is defined as $F_{goal}$ , and the gravitational force of $q_{rand}$ is defined as $F_{rand}$ . The expansion direction is defined as

(6) \begin{equation} \boldsymbol{n}=F_{rand}\!\left(\frac{q_{rand}-q_{near}}{\left\| q_{rand}-q_{near}\right\| }\right)+F_{goal}\!\left(\frac{q_{\textit{orient}}-q_{near}}{\left\| q_{\textit{orient}}-q_{near}\right\| }\right) \end{equation}

Taking the expansion failures number $N_{\textit{failed}}$ of its parent node as the independent variable, the new functions of $F_{rand}$ and $F_{goal}$ are defined as

(7) \begin{equation} F_{rand}=e^{\frac{N_{\textit{failed}}}{2}}-1,\,F_{goal}=e^{{-^{\frac{N_{\textit{failed}}}{2}}}} \end{equation}

The initial value of $N_{\textit{failed}}$ is zero, and $N_{\textit{failed}}$ is accumulated once when a collision is detected during expansion or the joint value exceeds the joint range. It can be seen from Eq. (7) that $\boldsymbol{n}$ is toward the target point and the new point directly is expanded toward the target point when $N_{\textit{failed}}=0$ . The greater the number of $N_{\textit{failed}}$ , the greater the value of $F_{rand}$ , and the smaller the value of $F_{goal}$ . Then the overall expansion direction tends to be more random. The new point after expansion is

(8) \begin{equation} q_{new}=q_{near}+\textit{stepsize}\frac{\boldsymbol{n}}{\left\| \boldsymbol{n}\right\| } \end{equation}

In Eq. (8), stepsize represents the step size of expansion, and its value is calculated by the adaptive step size method.

(2) Adaptive step size.

According to the current expansion direction and joint values, the adaptive step size of the joint space is obtained to ensure that the maximum movement distance of each joint is less than the minimum obstacle width. The Jacobian matrix reflects the influence of small changes in joint angles on the speed of the end effector in the form of

(9) \begin{equation}\mathit{v} = [{\mathit{w}_{\mathit{x}}}\mathit{,}{\mathit{w}_{\mathit{y}}}{\mathit{,}\mathit{w}_{\mathit{z}}}\mathit{,}{\mathit{v}_{\mathit{x}}}\mathit{,}{\mathit{v}_{\mathit{y}}}\mathit{,}{\mathit{v}_{\mathit{z}}}]^{\mathit{T}} = \mathit{J}(\mathit{q})\dot{\mathit{q}} \end{equation}

In Eq. (9), $J(q)$ represents the geometric Jacobian matrix, and $\mathit{v}$ represents the spatial velocity composed of angular velocity $w$ and linear velocity $v$ . Set $J(q)=[J_{w}(q),J_{v}(q)]^{T}$ , then the linear velocity of the $i-\mathrm{th}$ joint can be expressed as

(10) \begin{equation}\dot{\mathit{p}}_{\mathit{i}} = [{\mathit{v}_{\mathit{xi}}}\mathit{,}{\mathit{v}_{\mathit{yi}}}\mathit{,}{\mathit{v}_{\mathit{zi}}}]^{\mathit{T}} = \mathit{J}_{\mathit{vi}}\!\left(\dot{\mathit{q}}_{\mathit{i}}\right),\mathit{i}=1,2,\ldots,6 \end{equation}

In Eq. (10), $\dot{\mathit{p}}_{\mathit{i}}$ represents the linear velocity of the $i$ th joint, $\dot{\mathit{q}}_{\mathit{i}}=[{\dot{\mathit{\theta }}_{1}},\ldots,{\dot{\mathit{\theta }}_{\mathit{i}}}]^{\mathit{T}}\in \mathit{R}^{\mathit{i}\times 1}$ , and $J_{vi}(q)\in R^{3\times i}$ represents the linear velocity Jacobian matrix that regards ith joint as terminal joint, which is related to the rotation angle of the first i joint.

After obtaining the $\boldsymbol{n}$ by Eq. (6), the maximum step length in this direction is found: set $n_{j} = \max (| n_{1},\ldots,n_{i}| )$ where $j$ is the maximum subscript. Assume that each joint reaches a new point at the same time, that is, $\dot{\mathit{\theta }}_{1}\colon \dot{\mathit{\theta }}_{2}\colon \ldots \colon \dot{\mathit{\theta }}_{\mathit{i}} = \mathit{n}_{1}\colon \mathit{n}_{2}\colon \ldots \colon \mathit{n}_{\mathit{i}}$ , then $\dot{\mathit{q}}_{\mathit{i}} = [\frac{\mathit{n}_{1}}{\mathit{n}_{\mathit{j}}},\frac{\mathit{n}_{2}}{\mathit{n}_{\mathit{j}}},\ldots,\frac{\mathit{n}_{\mathit{i}}}{\mathit{n}_{\mathit{j}}}]^{\mathit{T}}\dot{\mathit{\theta }}_{\mathit{j}}$ is substituted into Eq. (10). Multiply both sides by $\Delta t$ to get that

(11) \begin{equation} \Delta p_{i}=J_{vi}\!\left(q_{i}\right)\left[\frac{n_{1}}{n_{j}},\frac{n_{2}}{n_{j}},\ldots,\frac{n_{i}}{n_{j}}\right]^{T}\Delta \theta _{j} \end{equation}

In order to simplify the expression, let $\Delta p_{i}=A_{i}\Delta \theta _{j}$ . It can be seen that $A_{i}$ is related to the expansion direction $\boldsymbol{n}$ and the rotation angle of the first $i$ joint. Use the compatibility of the norm $\| Ax\| \leq \| A\| \, \| x\|$ to get that

(12) \begin{equation} \left\| \Delta p_{i}\right\| \leq \left\| A_{i}\right\| \left\| \Delta \theta _{j}\right\| \end{equation}

Since $\| \Delta p_{i}\| _{2}=\sqrt{\Delta p_{ix}^{2}+\Delta p_{iy}^{2}+\Delta p_{iz}^{2}}$ represents the distance that ith linkage moves in Cartesian space, the 2 norm is selected for the Eq. (12). Suppose the width of the smallest obstacle in the working space is $\nabla$ , that is, the maximum value on the right side of Eq. (12) is $\nabla$ . We can get that

(13) \begin{equation} \left\| \Delta p_{i,\max }\right\| _{2}\leq \max \!\left\| A_{i}\right\| _{2}\left\| \Delta \theta _{j}\right\| _{2}=\nabla \end{equation}

(14) \begin{equation} \Delta \theta _{j}=\left\| \Delta \theta _{j}\right\| _{2}=\frac{\nabla }{\max \!\left\| A_{i}\right\| _{2}} \end{equation}

From Eq. (14), the maximum expansion step in the $\boldsymbol{n}$ direction is calculated as

(15) \begin{equation} \textit{Stepsize}=\frac{\Delta \theta _{j}}{n_{j}} \end{equation}

The distance of movement of the linkage of the robot is less than the width of the minimum obstacle under the step size.

3.2.3. Expansion of path points

The imitation path obtained in section 3.1 cannot guarantee that each joint of robot does not collide with the surrounding mechanism in the narrow space. Therefore, the path point expansion method is designed based on the advantage of fast expansion without collision in IRRT. Regarding the spatial guidance points of imitation path as theguidance for IRRT, the path shape of the yarn storage is obtained by expending along the guiding path when there is no collision. The expansion angle is adjusted continuously to ensure that each linkage of the robot does not collide with surrounding mechanisms when there is collision. The principle of path points expansion is shown in Fig. 9.

Figure 9. Schematic diagram of path point expansion.

(1) After inserting $m$ path points uniformly between the imitation path points, the initial pose and end pose are interpolated and the inverse kinematics of the robot is solved. The obtained joint data $\textit{Orient}\{i\}=[q_{1},q_{2},\ldots,q_{6}]$ are used as the spatial guidance points for the expansion point where $i=1,2,\ldots,m$ . The spatial guidance point is the instantaneous target point of IRRT, which leads IRRT to expand along the imitation path.

(2) Random sampling is conducted within the angle range of the robot to obtain sampling point $q_{rand}$ and a random number is generated between 0 and 1. If the random number is less than 0.9, the index $i_{\textit{closest}}$ of the guidance point closest to the end of the expansion tree is found and $q_{\textit{orient}}=\textit{Orient}\{i_{\textit{closest}}+1\}$ is regarded as the target point $q_{goal}$ for this expansion. The node $q_{near}$ closest to $q_{goal}$ is defined as the parent node; otherwise, the node $q_{near}$ closest to $q_{rand}$ is defined as the parent node. The index $i_{\textit{closest}}$ of the guidance point closest to the $q_{near}$ is calculated, and $q_{\textit{orient}}=\textit{Orient}\{i_{\textit{closest}}+1\}$ is used as the target point $q_{goal}$ of this expansion.

(3) IRRT algorithm is used to calculate the expansion direction and step size, and the new node $q_{new}$ is calculated.

(4) Collision detection is carried out at the angle of $q_{new}$ . If there is a collision or joint is out of range, $N_{\textit{failed}}=N_{\textit{failed}}+1$ , and return step (2). Otherwise, $q_{new}$ is expanded on the parent node $q_{near}$ (to speed up the convergence speed, this paper defines that when the $N_{\textit{failed}}$ >50 for this parent node, it will not generate a new node).

(5) The distance between $q_{new}$ and $q_{end}$ is calculated. If it is less than the threshold, the path expansion is successful. Otherwise, steps (2)–(5) will be repeated until the maximum number of iterations is reached.

Sharp path points reduce the stability of the robot movement and a smooth path is required. The principle of smooth path is shown in Fig. 10. Suppose that $p_{i+1}$ is a sharp point when $\angle p_{i+1}p_{i}p_{i+2}\gt 30^{\circ}$ and $p_{i+1}$ is deleted if there are no obstacles between $p_{i}$ and $p_{i+2}$ . $p_{1}p_{2}p_{3}p_{4}p_{5}p_{6}$ is an unsmoothed path. $\angle p_{2}p_{1}p_{3}\gt 30^{\circ}, p_{2}$ is a sharp point. Since there is no obstacle between $p_{1}$ and $p_{3}, p_{2}$ is deleted; $\angle p_{5}p_{4}p_{6}\gt 30^{\circ}$ and $p_{5}$ is a sharp point. Since there is no obstacle between $p_{4}$ and $p_{6}, p_{5}$ is reserved. The smoothed path is $p_{1}p_{3}p_{4}p_{5}p_{6}$ . Based on this, a smooth path method is designed.

Figure 10. Schematic diagram of smooth path.

(1) The distance between point $p_{i}$ and point $p_{i+1}$ is record as $d_{i,i+1}$ . The initial value of $i$ is 1, and $d_{i,i+1},d_{i,i+2},d_{i+1,i+2}$ is calculated.

(2) Determine whether $p_{i+1}$ is a sharp point. If $d_{i,i+2}+\delta \lt d_{i+1,i+2}+d_{i,i+1}, p_{i+1}$ is a sharp point, and m points are inserted uniformly between $p_{i}$ and $p_{i+2}$ for collision detection, and then enter step (3). Otherwise, $p_{i+1}$ is not a sharp point, and let $i=i+1$ , steps (1) and (2) are repeated. Among them, $\delta =2(\nabla -\nabla \cos 30^{\circ})$ , where $\nabla$ is the width of the minimum obstacle, and $m=\text{ceil}\!\left(\frac{d_{i,\,i+2}}{\nabla }\right)$ where ceil is a round-up function.

(3) If there is no collision, delete the point $i+1$ . Otherwise, let $i=i+1$ and repeat the process of (1) (2) (3).