A posture measurement approach for an articulated manipulator by RGB-D cameras

Abstract

The objective of this article aims at the safety problems where robots and operators are highly coupled in a working space. A method to model an articulated robot manipulator by cylindrical geometries based on partial cloud points is proposed in this article. Firstly, images with point cloud data containing the posture of a robot with five resolution links are captured by a pair of RGB-D cameras. Secondly, the process of point cloud clustering and Gaussian noise filtering is applied to the images to separate the point cloud data of three links from the combined images. Thirdly, an ideal cylindrical model fits the processed point cloud data are segmented by the random sample consensus method such that three joint angles corresponding to three major links are computed. The original method for calculating the normal vector of point cloud data is the cylindrical model segmentation method, but the accuracy of posture measurement is low when the point cloud data is incomplete. To solve this problem, a principal axis compensation method is proposed, which is not affected by number of point cloud cluster data. The original method and the proposed method are used to estimate the three joint angular of the manipulator system in experiments. Experimental results show that the average error is reduced by 27.97%, and the sample standard deviation of the error is reduced by 54.21% compared with the original method for posture measurement. The proposed method is 0.971 piece/s slower than the original method in terms of the image processing velocity. But the proposed method is still feasible, and the purpose of posture measurement is achieved.

Introduction

With the development of science and technology, the types and applications of robots are becoming more and more diversified which are competent for complex tasks. Although robot manipulators bring a lot of conveniences, most of the robots are not equipped with the function of online collision detection and detour. Robot arms barely sense hazardous conditions around and respond to, for example, when there are people or obstacles approaching. This may not only affect the use of robot arms but also cause safety problems when people work. ^{1 –3} Therefore, this article proposes a method of posture measurement by RGB-D cameras as sensory devices for articulated manipulators. The method combines the captured images from two opposite RGB-D cameras, filters, and segments the point cloud data (PCD) of each cylindrical link. A cylindrical model is used to estimate the shape of the individual link by computing the eigenvectors of each point cloud as normal vectors so as that the cylindrical parameters of cylindrical posture measurement can be obtained.

The posture measurement for the manipulator is a method to obtain the joint angle from the perspective of three-dimensional (3-D). The posture measurement for the manipulator is not only applied to obstacle avoidance for the manipulator in dynamic environment ⁴ but also necessary for visual servo control, ⁵ especially position-based visual servo ⁶ control. Some methods for posture measurement of manipulator in dynamic environments have been proposed, such as extended Kalman filter ⁷ and immune evolutionary algorithm. ⁸ Posture measurement is applied not only to the manipulator but also to the snake-like robots, ⁹ steerable catheter, ¹⁰ and even the driver’s head posture. ¹¹

Vision-based posture measurement is the most commonly used method for manipulators, but the vision-based method usually relies on the robust image processing algorithm. Vision-based posture measurement for manipulators has attracted the attention of many scholars. Janabisharifi and Marey ⁷ introduced a method of position and attitude estimation of manipulator based on the Kalman filter method. This method can effectively filter out the noise in the image, but it cannot solve the dilemma of incomplete target caused by the occlusion of the manipulator joint. Suqi and Lim ¹² proposed a posture measurement method based on the contour recognition method. First, the PCD is obtained by sensors, then the contour of the target is measured and the posture of the robot is estimated.

However, there are many noises in the PCD, so the noise filtering needs to be adopted in the preprocessing of the PCD. The segmentation and projection of geometric model is a new method for posture measurement. Narayanan ¹¹ proposed a posture measurement for driver’s head based on geometric model. The posture of driver’s head is estimated by the segmentation and projection of geometric model, but the method based on geometric model depends on the complete target image. If the image of the target is incomplete, the precision of posture measurement will be lost. The cylindrical model segmentation method for manipulator is affected by the number of PCD. However, in practical applications, due to occlusion and other reasons, the PCD of the manipulator is often incomplete, so a method that will not be affected by the number of PCD is needed.

In the vision-based posture measurement method for manipulators, the use of PCD for posture measurement is a hot topic. In this article, a method of posture measurement for manipulators using PCD is studied, and a new method for posture measurement of manipulators is proposed, which is not affected by the number of PCD.

There are two contributions in this article. Firstly, aiming at the safety problems where robots and operators are highly coupled in a working space, this article proposes a posture measurement approach for an articulated manipulator. Secondly, the original method of normal vector calculation for PCD is the cylindrical model segmentation method. However, due to occlusion and other reasons, some joints of the manipulator cannot be captured by sensors, resulting in incomplete PCD. Therefore, the principal vector of PCD cannot be accurately calculated, and the parameter of the cylindrical model segmentation method is fixed, so the accuracy is not high. To overcome the shortcomings of the cylindrical model segmentation method, a principal axis compensation method is proposed in this article. This method cannot be affected by the number of point cloud cluster data. The experimental results show that this method improves the accuracy of posture measurement compared with the original method.

Plane model segmentation ¹³ is a common method to find the planar model by PCD ¹⁴ within random sample consensus. ^15,16 This method uses iterations to find the fitting model from a set of original data. $(x,y,z)$ represents a point in 3-D space. The four coefficients, $a,b,c$ , and d, in the parametric formula of a plane as expressed in equation (1) fit PCD perfectly to achieve the desired segmentation effect.

$ax+by+cz+d=0$

1

The first step to achieve this purpose is to randomly choose three PCD points which are not collinear. Assuming that the three points are $P_1,P_2,\hspace{0.17em}\text{and}\hspace{0.17em}{P}_3$ .Then, the normal vector ${\overrightarrow{N}}_{\text{plane}}$ of the plane $M_{\text{plane}}$ is computed with the cross product of two vectors $\overrightarrow{P_1{P}_2}$ and $\overrightarrow{P_1{P}_3}$ shown as equation (2) to get the three coefficients $a,b$ , and c in equation (1). The next step is to substitute $P_1$ into equation (1) to get the fourth coefficient d. Finally, the number of the inliers which fit the planar model is calculated by the planar equation

${\overrightarrow{N}}_{\text{plane}}=\overrightarrow{P_1{P}_2}\times \overrightarrow{P_1{P}_3}=(a,b,c)$

2

Noise of PCD filtering

Spatial coordinate filtering

Images captured by a Kinect sensor do not require many zeroes and other points. Therefore, the first step is to filter the PCD which is redundant after loading an image to avoid noise interference as well as to reduce the point number of the PCD. ¹⁷ The second step is to register the manipulator workspace of the PCD with the spatial coordinates of the XYZ values and delete other PCDs.

Assuming that $(x,y,z)$ is the PCD point in the manipulator working space, and the maximum and minimum values respective on the number lines of the X-axis, the Y-axis, and the Z-axis in the manipulator working space are $x_{\text{UR5max}},x_{\text{UR5min}},y_{\text{UR5max}},y_{\text{UR5min}},z_{\text{UR5max}},z_{\text{UR5min}};$ then, the PCD range of the manipulator working space can be represented as in equation (3)

$\{\begin{array}{l}\hspace{0.17em}{x}_{\text{UR5min}}\le x\le x_{\text{UR5max}}\\ \hspace{0.17em}{y}_{\text{UR5min}}\le y\le y_{\text{UR5max}}\hspace{1em}\hspace{1em},(x,y,z)\ne (0,0,0)\\ \hspace{0.17em}{z}_{\text{UR5min}}\le z\le z_{\text{UR5max}}\\ \hspace{0.17em}x\cap y\cap z\mathrm{.}\end{array}$

3

The principal axis of link 1 is parallel to the Y-axis of the world coordinate because the X-axis of the first RGB-D camera is parallel to the angle 0° of manipulator joint 1, and the Y-axis of the first RGB-D camera is parallel to the angle −90° of manipulator joint 2, and there is only the rotation rather than the translation within link 1. Consequently, it is allowed to let the principal axis vector of link 1 be $v_1=(0,1,0)$ and the PCD of link 1 can be removed. The PCD range of link 1 is represented as in equation (4), where the PCD of link 1 is $(x_1,y_1,z_1)$ , and the maximum and minimum values respective on the number lines of the X-axis, the Y-axis, and the Z-axis in the manipulator link 1 and working space are $x_{\text{1max}},x_{1\text{min}},y_{1\text{min}},y_{1\text{max}},z_{\text{1min}},z_{1\text{max}}$

$\{\begin{array}{l}\hspace{0.17em}{x}_{\text{1min}}\le x_1\le x_{1\text{max}}\\ \hspace{0.17em}{y}_{\text{1min}}\le y_1\le y_{\text{1max}}\hspace{1em},(x_1,y_1,z_1)\ne (0,0,0)\\ \hspace{0.17em}{z}_{\text{1min}}\le z_1\le z_{\text{1max}}\\ \hspace{0.17em}{x}_1\cap y_1\cap z_1\end{array}$

4

The filtered PCD is that of the manipulator without link 1. Next, and the PCD of links 2, 3, and 5 is divided into the color range of RGB value and separated from the original PCD. Although the translation of link 2 only moves on the plane which is parallel to XZ plane, the range of the working space can be divided through equation (5), where the PCD point of link 2 is $(x_2,y_2,z_2)$ , and the maximum and minimum values respective on the number lines of the X-axis, the Y-axis, and the Z-axis in the manipulator link 2 working space are $x_{\text{2max}},x_{\text{2min}},y_{2\text{min}},y_{2\text{max}},z_{2\text{min}},z_{2\text{max}}$

$\{\begin{array}{l}\hspace{0.17em}{x}_{\text{2min}}\le x_2\le x_{\text{2max}}\\ \hspace{0.17em}{y}_{\text{2min}}\le y_2\le y_{\text{1max}}\hspace{1em},(x_2,y_2,z_2)\ne (0,0,0)\\ \hspace{0.17em}{z}_{\text{2min}}\le z_2\le z_{\text{1max}}\\ \hspace{0.17em}{x}_2\cap y_2\cap z_2\end{array}$

5

Nevertheless, other links are probably in the working space of link 2, while the posture of joint 2 is closing to the floor from the initial state; that is, the angular range of joint 2 is from −90° to −180° or from −90° to 0°. These situations may affect the segmentation results of cylindrical models with link 2. Therefore, the PCD of link 2 cannot be divided by spatial coordinate filtering.

Gaussian noise filtering

After separating from other links through equation (6), $(r_2,g_2,b_2$ ) is the RGB values of PCD of link 2

$\frac{r_2+g_2}{2}-b_2\le -20$

6

Similarly, the PCD of links 3 and 5 is able to be independent clusters, which are, respectively, defined clusters 3 and 5 in this article, after separating from other links through equations (7) and (8), where $(r_3,g_3,b_3$ ) is the RGB values of cluster 3 in PCD, and $(r_5,g_5,b_5$ ) is the RGB values of cluster 5 in PCD

$\frac{b_3+g_3}{2}-r_3\le -20$

7

$\frac{b_5+r_5}{2}-g_5\le -20$

8

Assuming that $n_G$ denotes the number of points to input PCD points, and $P_{iG}(x_{iG},y_{iG},z_{iG})$ is the element to input PCD points, $i_G$ is the element number, $i_G\in N,i_G\cap n_G$ , and $k_G$ is the point number of neighbor points; $P_{i{G}_1}(x_{i{G}_1},y_{i{G}_1},z_{i{G}_1})$ to $P_{i{G}_{k_G}}(x_{i{G}_{k_G}},y_{i{G}_{k_G}},z_{i{G}_{k_G}})$ are the nearest $k_G$ points from $P_{iG}$ . Afterward, the Euclidean distances of $D_{i_{G_1}}$ to $D_{i_{G_{k_G}}}$ between $P_{iG}$ and respective $P_{i{G}_1}$ to $P_{i_{G_{k_G}}}$ can be represented as equation (9), where $j\in N$ and $j\cap k_G$ . $D_{i_{G_j}}$ is Euclidean distance of the jth neighbor point to $P_{iG}$

$D_{i_{G_j}}=\sqrt{{(x_{i_{G_j}}-x_{i_G})}^2+{(y_{i_{G_j}}-y_{i_G})}^2,(z_{i_{G_j}}-z_{i_G}{)}^2}$

9

After that, the average distance $\overline{D_{iG}}$ of $D_{i_{G_1}}$ to $D_{i_{G_{k_G}}}$ is shown as in equation (10)

$\overline{D_{iG}}=\frac{1}{k_G}{\displaystyle \sum _{j=1}^{k_G}}{D}_{i{G}_j}$

10

Similarly, the average value $\overline{\overline{D_G}}$ and the standard deviation ${\sigma }_G$ of all the input PCD average distances $\overline{D_{i_G}}$ are shown in equations (11) and (12), respectively

$\overline{\overline{D_G}}=\frac{1}{n_G}{\displaystyle \sum _{i_G=1}^{n_G}}\overline{D_{i_G}}$

11

${\sigma }_G=\sqrt{\frac{1}{n_G-1}{\displaystyle \sum _{i_G=1}^{n_G}}{(\overline{D_{i_G}}-\overline{\overline{D_G}})}^2}$

12

Eventually, the PCD point $P_{i_G}$ is classified according to the distance threshold $D_{GTh}$ with the average value $\overline{\overline{D_G}}$ and the standard deviation ${\sigma }_G$ . If the average distance is larger than the distance threshold, the PCD point will be considered outlier and filtered. Otherwise, the PCD point will be considered inlier and remained. The distance threshold $D_{GTh}$ is shown in equation (13), where ${\alpha }_G$ is a real parameter according to the noise distribution in the external environment. The classification of the PCD point $P_{i_G}$ is shown in equation (14)

$D_{GTh}=\overline{\overline{D_G}}+{\alpha }_G\cdot {\sigma }_G$

13

$P_{i_G}:\{\begin{array}{l}\text{inlier,}\hspace{0.17em}\hspace{0.17em}\text{if} \overline{D_G}\le D_{GTh}\\ \text{outlier,}\hspace{0.17em}\hspace{0.17em}\text{else}\mathrm{.}\end{array}$

14

Link modeling from PCD

The successive step after the noise filtering of PCD images is to process the PCD images.

Normal vectors of point cloud surface

Before the segmentation of the cylinder model, the two points of PCD need to be chosen by their normal vectors. The normal vectors of PCD points are determined by their adjacent points because they are calculated from the covariance matrix of their adjacent PCD points. ¹⁸ Assuming that $n_{\text{EV}}$ is the point number of the input PCD points, $P_{i_{\text{EV}}}$ is the element of the input PCD points, $i_{\text{EV}}$ is the element number, $i_{\text{EV}}\in N,i_{\text{EV}}\le n_{\text{EV}}$ , and $k_{\text{EV}}$ is the point number of the adjacent points; $P_{i_{{\text{EV}}_1}}$ to $P_{i_{{\text{EV}}_{k_{\text{EV}}}}}$ are the nearest $k_{\text{EV}}$ points from $P_{i_{\text{EV}}}$ . Afterward, the center point $\overline{P_{i_{\text{EV}}}}$ of $P_{i_{{\text{EV}}_1}}$ to $P_{i_{{\text{EV}}_{k_{\text{EV}}}}}$ is shown in equation (15). The covariance matrix ${\text{CM}}_{i_{\text{EV}}}$ is then computed with $\overline{P_{i_{\text{EV}}}}$ shown in equation (16). Next, the three eigenvalues ${\lambda }_{i\text{EV}}$ and the corresponding eigenvectors ${\overrightarrow{v}}_{i\text{EV}}$ are computed with ${\text{CM}}_{i_{\text{EV}}}$ shown in equation (17), and the normal vector ${\overrightarrow{N}}_{i\text{EV}}$ of the PCD point $P_{i\text{EV}}$ is the smallest eigenvalue ${\lambda }_{i\text{EV}}$ of its eigenvector ${\overrightarrow{v}}_{i\text{EV}}$ as shown in equation (18). Among them, ${\lambda }_{i{\text{EV}}_1},{\lambda }_{i{\text{EV}}_2},\hspace{0.17em}\text{and}\hspace{0.17em}{\lambda }_{i{\text{EV}}_3}$ represent three eigenvalues, respectively. Finally, the direction of the normal vector ${\overrightarrow{N}}_{i\text{EV}}$ is determined with the observation point $V_p$ of the input PCD shown in equation (19) ¹⁸

$\overline{P_{i_{\text{EV}}}}=\frac{1}{k_{\text{EV}}}{\displaystyle \sum _{j=1}^{k_{\text{EV}}}}{P}_{i_{{\text{EV}}_j}}$

15

${\text{CM}}_{i_{\text{EV}}}=\frac{1}{k_{\text{EV}}}{\displaystyle \sum _{j=1}^{k_{\text{EV}}}}(P_{i_{{\text{EV}}_j}}-{\overline{P}}_{i_{\text{EV}}})(P_{i_{{\text{EV}}_j}}-{\overline{P}}_{i_{\text{EV}}}{)}^T$

16

${\text{CM}}_{i_{\text{EV}}}\cdot {\overrightarrow{N}}_{i{\text{EV}}_j}={\lambda }_{i{\text{EV}}_j}\cdot {\overrightarrow{N}}_{i{\text{EV}}_j},j\in \{0,1,2\}$

17

${\overrightarrow{N}}_{i\text{EV}}=\frac{{\overrightarrow{N}}_{i{\text{EV}}_1}}{|{\overrightarrow{N}}_{i{\text{EV}}_1}|},{\lambda }_{i{\text{EV}}_1}\le {\lambda }_{i{\text{EV}}_2}\le {\lambda }_{i{\text{EV}}_3}$

18

${\overrightarrow{N}}_{i\text{EV}}=\{\begin{array}{l}-{\overrightarrow{N}}_{i\text{EV}},\text{if}\hspace{0.17em}{\overrightarrow{N}}_{i\text{EV}}\cdot (V_p-P_{i\text{EV}})\le 0\\ {\overrightarrow{N}}_{i\text{EV}},\hspace{0.17em}\hspace{0.17em}\text{else}\end{array}$

19

The accuracy of the normal vectors ${\overrightarrow{N}}_{i\text{EV}}$ is influenced by the adjacent point number $k_{\text{EV}}$ of the PCD. The default parameter value $k_{\text{EV}}$ ¹⁹ within the cylinder model segmentation gets better results of the PCD with complete cylindrical models. Nonetheless, the default parameter value $k_{\text{EV}}$ will probably cause large errors if the cylindrical models of the PCD are incomplete. Therefore, an improved method for the incomplete cylindrical models of the PCD is proposed in this article. The method improves the accuracy of the cylindrical models through adjusting the proper parameter value $k_{\text{EV}}$ . The main reasons for incomplete PCD are the postures of the robot arm and the relative position between RGB-D cameras and robot arm. The incompleteness of the PCD will increase if some links of robot arm block out other links so that RGB-D cameras are unable to capture the PCD of links. In addition, the parameter value $k_{\text{EV}}$ is also relative to the cylindrical radius R of the links of robot arm. The larger cylindrical radius R is, the less change the adjacent normal vectors within the PCD has; otherwise, the more change of the adjacent normal vectors within the PCD is. The computed normal vectors are probably far from the actual normal vectors because of asymmetric PCD under the conditions of incomplete PCD, small cylindrical radius, and large $k_{\text{EV}}$ value. As a result, the cylindrical radius R is relative to the parameter value $k_{\text{EV}}$ . In summary, the parameter value $k_{\text{EV}}$ is designed as an equation which is relative to the cylindrical radius R and the density of the PCD in this article. The equation is shown in equation (20), where the density of the PCD can be converted to the PCD point number of unit surface area, that is, $\frac{n_{\text{EV}}}{2\pi RL}$ in equation (20). In addition, $n_{\text{EV}}$ is the PCD point number, $\pi$ is circumference ratio, R is the cylindrical radius, and L is the cylindrical length in equation (20)

$k_{\text{EV}}=\left[\frac{n_{\text{EV}}}{2\pi RL}\times R\right]$

20

The density of the PCD computed by equation (20) is smaller than the actual value due to the incomplete cylinder of the PCD captured by RGB-D cameras; that is to say, the parameter value $k_{\text{EV}}$ is smaller than the actual value. Therefore, there multiplies a constant value $c_{\text{Com}}$ for the compensation with equation (20) in this article. Through our constant attempts, the average and the standard deviation of the angular absolute errors become small when the constant value $c_{\text{Com}}$ is about 7.96875. Due to the fact that $\frac{1}{2\pi }$ in equation (20) and 7.96875 are constants, this article combines the two constants to a constant value $c_{\text{EV}}$ , and the fixed equation is shown as below

$k_{\text{EV}}=\left[\frac{7.96875n_{\text{EV}}}{2\pi RL}\times R\right]=\left[c_{\text{EV}}\cdot \frac{n_{EV}}{2\pi RL}\right]=\frac{7.96875}{2\pi }$

21

Directions of link principal axis

The principal axis vector of the cylinders can be represented by the vector of sign symbol, which is one of the cylindrical parameters in the segmentation of cylinder model. This article describes how to get principal axis directions of the manipulator links. This section introduces the validation of principal axis direction of link 2 in the case of capture link 2. The center point $C_2$ of cluster 2 which can be computed as equation (22) is because there is only the PCD of link 2 within cluster 2 and the point number of cluster 2 $n_2$ is known. $(x_{2_i},y_{2_i},z_{2_i})$ is the ith PCD point of cluster 2 in the following equation

$C_2=\frac{{\displaystyle {\sum }_{i=1}^{n_2}(x_{2_i},y_{2_i},z_{2_i})}}{n_2}$

22

The center point $C_2$ which is not on the principal axis of link 2 is, shown in Figure 1(a), due to the incomplete cylinder of the PCD of cluster 2. However, the projection $C_{2_{\text{proj}}}$ of the center point $C_2$ on the principal axis can be found after the parameters of the link principal axis ${\overrightarrow{v}}_2$ and the link principal axis point $Q_2$ within cylinder model segmentation, as shown in equation (23) and Figure 1(b)

$C_{2_{\text{proj}}}=Q_2+\frac{\overrightarrow{Q_2{C}_2}\cdot {\overrightarrow{v}}_2}{{|{\overrightarrow{v}}_2|}^2}\times {\overrightarrow{v}}_2$

23

Figure 1.

The principal axis vector of link 2: (a) principal axis of link 2; (b) projection diagram of center point; (c) the principal axis of links 1 and 2.

The principal axis direction of link 2 is the vector $\overrightarrow{O{C}_{2\text{proj}}}$ , as shown in Figure 1(c), where the intersection O of links 1 and 2 is known. The direction of the link principal axis ${\overrightarrow{v}}_2$ is correct if the inner product of $\overrightarrow{O{C}_{2\text{proj}}}$ and ${\overrightarrow{v}}_2$ is no less than zero; otherwise, ${\overrightarrow{v}}_2$ will multiply by (−1) because of the opposite direction. The verified principal axis vector of link 2 is shown in equation (24)

${\overrightarrow{v}}_2=\{\begin{array}{l}{\overrightarrow{v}}_2,\text{if }\overrightarrow{O{C}_{2\text{proj}}}\cdot {\overrightarrow{v}}_2>0\\ -{\overrightarrow{v}}_2,\text{if }\overrightarrow{O{C}_{2\text{proj}}}\cdot {\overrightarrow{v}}_2\le 0\end{array}$

24

The method to verify the principal axis of link 3 is almost the same as link 2. The first step is to find the center point $C_3$ of cluster 3, as shown in equation (25) and Figure 2(a), where $n_3$ is the point number of cluster 3, and $(x_{3_i},y_{3_i},z_{3_i})$ is the ith PCD point of cluster 3. Next, the projection $C_{\text{3proj}}$ of the center point $C_3$ on the principal axis can be found with equation (26), where $v_3$ and $Q_3$ are, respectively, the principal axis vector and the principal axis point within cylinder model segmentation, as shown in Figure 2(b). $I_{23}$ is the intersection of the principal axes of links 2 and 3. The principal axis direction of link 3 is the vector $\overrightarrow{I_{23}{C}_{\text{3proj}}}$ , as shown in Figure 2(c). The direction of is correct if the inner product of $\overrightarrow{I_{23}{C}_{\text{3proj}}}$ and $v_3$ is no less than zero; otherwise, $v_3$ will multiply by (−1) because of the opposite direction. The verified principal axis vector of link 3 is shown in equation (27)

$C_3=\frac{{\displaystyle {\sum }_{i=1}^{n_3}(x_{3_i},y_{3_i},z_{3_i})}}{n_3}$

25

$C_{3_{\text{proj}}}=Q_3+\frac{\overrightarrow{Q_3{C}_3}\cdot {\overrightarrow{v}}_3}{{|{\overrightarrow{v}}_3|}^2}\times {\overrightarrow{v}}_3$

26

${\overrightarrow{v}}_3=\{\begin{array}{l}{\overrightarrow{v}}_3,\text{if }\overrightarrow{I_{23}{C}_{2\text{proj}}}\cdot {\overrightarrow{v}}_3>0\\ -{\overrightarrow{v}}_3,\text{if }\overrightarrow{I_{23}{C}_{2\text{proj}}}\cdot {\overrightarrow{v}}_3\le 0\end{array}$

27

Figure 2.

The principal axis vector of link 3: (a) principal axis of link 2; (b) projection diagram of center point; (3) the principal axis of links 2 and 3.

Similarly, the first step in terms of link 5 is to find the center point $C_5$ of cluster 5, as shown in equation (28) and Figure 3(a), where $n_5$ is the point number of cluster 5, and $(x_{5_i},y_{5_i},z_{5_i})$ is the ith PCD point of cluster 5. Next, the projection $C_{5\text{proj}}$ of the center point $C_5$ on the principal axis can be found with equation (29), where ${\overrightarrow{v}}_5$ and $Q_5$ are, respectively, the principal axis vector and the principal axis point within cylinder model segmentation, as shown in Figure 3(b)

$C_5=\frac{{\displaystyle {\sum }_{i=1}^{n_5}(x_{5_i},y_{5_i},z_{5_i})}}{n_5}$

28

$C_{5_{\text{proj}}}=Q_5+\frac{\overrightarrow{Q_5{C}_5}\cdot {\overrightarrow{v}}_5}{{|{\overrightarrow{v}}_5|}^2}\times {\overrightarrow{v}}_5$

29

Figure 3.

The principal axis vector of link 5: (a) principal axis of link 5; (b) projection diagram of center point; (3) The principal axis of links 3 and 5.

The next step which is different from the verifications of links 2 and 3 in this article is to find the end point $T_3$ of the link 3 principal axis shown in equation (30) through the intersection $I_{23}$ of the links 2 and 3 principal axes, the length of link 3 $L_3$ , and the principal axis vector ${\overrightarrow{v}}_3$ of link 3. There is an acute angle between the line segment $\overrightarrow{T_3{C}_{5\text{proj}}}$ and the link 5 principal axis, which is shown in Figure 3(c). At last, the direction of ${\overrightarrow{v}}_5$ is correct if the inner product of $\overleftarrow{T_3{C}_{5\text{proj}}}$ and ${\overrightarrow{v}}_5$ is no less than zero; otherwise, ${\overrightarrow{v}}_5$ will multiply by (−1) because of the opposite direction. The verified principal axis vector of link 5 is shown in equation (31)

$T_3=I_{23}+L_3\times {\overrightarrow{v}}_3$

30

${\overrightarrow{v}}_5=\{\begin{array}{l}{\overrightarrow{v}}_5,i\text{f }\overrightarrow{T_3{C}_{5\text{proj}}}\cdot {\overrightarrow{v}}_5>0\\ -{\overrightarrow{v}}_5,\text{if }\overrightarrow{T_3{C}_{5\text{proj}}}\cdot {\overrightarrow{v}}_5\le 0\end{array}$

31

Principal axis compensating method

In terms of link 2, other links are probably in the working space of link 2, while the range of angular of joint 2 is from −90° to −180° or from −90° to 0°. Link 2 is probably blocked so that there is not any PCD of link 2 within cluster 2. Therefore, it is useless to find the principal axis of link 2 with cylinder model segmentation. The translation of link 2 only moves on the plane which is parallel to XZ plane, and the intersection O between links 1 and 2 is known. As a result, the equation of the link 2 translational plane is $y=y_0$ , assuming that the intersection O is $(x_o,y_o,z_o)$ . In addition, the intersection O is able to project on the plane $y=y_0$ after projecting on the link 3 principal axis which is known. The projection is the end point $T_2$ of the principal axis of link 2 and the vector $\overrightarrow{O{T}_2}$ , as shown in Figure 4, is the principal axis vector of link 2. This method is the compensation, while the PCD of link 2 is not captured.

Figure 4.

The principal axis vector compensation of link 2.

For other links, if other links are blocked, as long as the cylindrical parameters of two adjacent links are known, the principal axis vectors of the links can also be found in the compensation method.

Angles of joint computing

In this article, the principal axis vectors obtained by clustering in cylindrical model segmentation are projected onto the rotational planes before computing the joint angles. According to the corresponding relationship between the joints and the links, the joint angles are computed from two projection vectors, as shown in Table 2.

In terms of the rotation plane, Figure 5 shows that the rotation plane of joint 1 is parallel to the XZ plane, and the rotation plane of joints 2 and 3 is the plane whose normal vector is the principal axis vector of joint 2 because of the same rotational directions, and because the rotational planes are vertical to the principal axis vector of link 2.

Figure 5.

The explanation of joint rotational planes.

In terms of vector projections, a vector $\overrightarrow{v}$ which projects to the normal vector $\overrightarrow{n}$ of the plane E is known ${\overrightarrow{v}}_{\text{proj}}$ .Assuming that two vectors within applications of the robot arms are $\overrightarrow{a}$ (the world coordinate vector or the link principal axis vector) and $\overrightarrow{b}$ (the link principal axis vector), $\overrightarrow{a}\ne \overrightarrow{0},\overrightarrow{b}\ne \overrightarrow{0},|\overrightarrow{a}|=|\overrightarrow{b}|=1$ , E is the rotational plane, $\overrightarrow{n}=(n_1,n_2,n_3)$ is the normal vector of the plane E, $\overrightarrow{a}\ne \overrightarrow{0}$ , and $|\overrightarrow{a}|=1$ . Besides, ${\overrightarrow{a}}_{\text{proj}}=(a_1,b_1,c_1)$ and ${\overrightarrow{b}}_{\text{proj}}=(a_1,b_1,c_1)$ are assumed the vector projections from $\overrightarrow{a}$ and $\overrightarrow{b}$ to the plane E, respectively, ${\overrightarrow{a}}_{\text{proj}}$ is the starting vector, and ϕ is the angle between ${\overrightarrow{a}}_{\text{proj}}$ and ${\overrightarrow{b}}_{\text{proj}}$ ; then, ϕ can be represented as in equations (32) and (33)

$M=\left[\begin{array}{l}{\overrightarrow{a}}_{\text{proj}}\\ {\overrightarrow{b}}_{\text{proj}}\\ \overrightarrow{n}\end{array}\right]=\left[\begin{array}{lll}{a}_1\hfill & a_2\hfill & a_3\hfill \\ b_1\hfill & b_2\hfill & b_3\hfill \\ n_1\hfill & n_2\hfill & n_3\hfill \end{array}\right]$

32

$\varphi ={\text{tan}}^{-1}\left[\frac{\text{det}(M)}{{\overrightarrow{a}}_{\text{proj}}\cdot {\overrightarrow{b}}_{\text{proj}}}\right]$

33

For joint 1, the angle is between the X-axis of the first RGB-D camera and the principal axis vector of link 2. The threshold within cluster 2 which is set in this article is due to the influence whether other links block link 2 or not. The angle of joint 1 will be output directly if point number of cluster 2 is no less than the threshold; otherwise, the angle of joint 1 will be indicated that it is computed to let users know which manipulator postures that link 2 is blocked.

It is known that the rotational planar equation of joint 1 is $y=y_0$ ; that is to say, the normal vector of the rotational plane is ${\overrightarrow{v}}_Y=(0,1,0)$ . Assuming that the X-axis of the first RGB-D camera is ${\overrightarrow{v}}_X=(0,1,0)$ , and the principal axis vector of link 2 which projects to the rotational plane is ${\overrightarrow{v}}_{2_{\text{proj}}}$ , shown in equation (34); the angle of joint 1 is ${\theta }_1$ , and $M_1$ is the arc tangent numerator of ${\theta }_1$ , shown in equation (35). The joint 1 angle ${\theta }_1$ is computed in equation (36)

${\overrightarrow{v}}_{2_{\text{proj}}}={\overrightarrow{v}}_2-\frac{{\overrightarrow{v}}_2\cdot {\overrightarrow{v}}_y}{{|{\overrightarrow{v}}_y|}^2}\times {\overrightarrow{v}}_y=(v_{2_{{\text{proj}}_x}},v_{2_{{\text{proj}}_y}},v_{2_{{\text{proj}}_z}})$

34

$M_1=\left[\begin{array}{l}{\overrightarrow{v}}_X\\ {\overrightarrow{v}}_{2_{\text{proj}}}\\ {\overrightarrow{v}}_Y\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ v_{2_{{\text{proj}}_x}}& v_{2_{{\text{proj}}_y}}& v_{2_{{\text{proj}}_z}}\\ 0& 1& 0\end{array}\right]$

35

${\theta }_1={\text{tan}}^{-1}\left[\frac{\text{det}(M_1)}{{\overrightarrow{v}}_X\cdot {\overrightarrow{v}}_{2_{\text{proj}}}}\right]$

36

For joint 2, the angle is the principal axis vectors between links 1 and 3 according to Table 2 with 90°, and which is difference from the manipulator system; that is, the angle of joint 2 of the manipulator system is the angle of the principal axis vectors between links 1 and 3 which minuses 90°.

Table 2.

The explanation of experimental methods.

Parameters        Method	Original method	Proposed method
Value of $k_{\text{EV}}$	50	$\left[\frac{7.96875n_{\text{EV}}}{2\pi L}\right]$
Threshold of principal axis compensating	None	500

It is known that ${\overrightarrow{v}}_{2_{\text{proj}}}$ shown in equation (34) is the normal vector of the joint 2 rotational plane, and the principal axis vectors of links 1 and 2 are ${\overrightarrow{v}}_1=(0,1,0)$ and ${\overrightarrow{v}}_2$ , respectively. The principal axis vector of link 3 is ${\overrightarrow{v}}_3$ and the principal axis vector of link 5 is ${\overrightarrow{v}}_5$ . Then, the principal axis vector of link 3 which projects to the rotational plane is ${\overrightarrow{v}}_{3_{\text{proj}}}$ , shown in equation (37), and the joint 2 angle is ${\theta }_2$ , and $M_2$ is the arc tangent numerator of ${\theta }_2$ , shown in equation (38). The angle of joint 2 ${\theta }_2$ is computed in equation (39)

${\overrightarrow{v}}_{3_{\text{proj}}}={\overrightarrow{v}}_3-\frac{{\overrightarrow{v}}_3\cdot {\overrightarrow{v}}_{2_{\text{proj}}}}{{|{\overrightarrow{v}}_{2_{\text{proj}}}|}^2}\times {\overrightarrow{v}}_{2_{\text{proj}}}=(v_{3_{{\text{proj}}_x}},v_{3_{{\text{proj}}_y}},v_{3_{{\text{proj}}_z}})$

37

$M_1=\left[\begin{array}{l}{\overrightarrow{v}}_{3_{{\text{proj}}_x}}\\ {\overrightarrow{v}}_{5_{{\text{proj}}_x}}\\ {\overrightarrow{v}}_{2_{{\text{proj}}_x}}\end{array}\right]=\left[\begin{array}{lll}{v}_{3_{{\text{proj}}_x}}\hfill & v_{3_{{\text{proj}}_y}}\hfill & v_{3_{{\text{proj}}_z}}\hfill \\ v_{5_{{\text{proj}}_x}}\hfill & v_{5_{{\text{proj}}_y}}\hfill & v_{5_{{}_{{\text{proj}}_z}}}\hfill \\ v_{2_{{\text{proj}}_x}}\hfill & v_{2_{{\text{proj}}_y}}\hfill & v_{5_{{}_{{\text{proj}}_z}}}\hfill \end{array}\right]$

38

${\theta }_3={\text{tan}}^{-1}\left[\frac{\text{det}(M_3)}{{\overrightarrow{v}}_{3_{\text{proj}}}\cdot {\overrightarrow{v}}_{5_{\text{proj}}}}\right]$

39

Experiments

The experiments are the application experiment of using manipulator as the representative cylindrical robot arms to compute three joint angles and compare errors with the angles shown from the manipulator system.

Experimental configuration

In this article, manipulator is used as the research object, and the cylindrical robot arm is studied. The first step is to convert images to PCD through the RGB-D cameras, Kinect for Windows SDK 2.0, and PCL, and then the images captured by the second Xbox One Kinect are converted to the images whose reference point is the first RGB-D camera with rotational matrix and translational matrix. Then, the images are combined after the converting. The second step is to divide the PCD of the combined images and filter noise. Finally, according to the normal vector of PCD points and the order of cylinder model segmentation in PCL, some principal axis vectors are computed, and then three angles of manipulator joint are calculated through the vectors afterward. Then, the feasibility of this method is verified by comparing the errors between the computed angles and the joint angles displayed on the manipulator system.

The cylinders of PCD captured by two Xbox Kinect sensors are not complete. Because of the different radius, length, and posture of the cylinder, the number of PCD points is also changed. In addition, Zhang et al. ⁸ use fixed value to compute the normal vectors of the PCD points. The normal vector of the PCD point may be incorrect and the results are probably corrupted due to the selection of asymmetric points or fixed values that are not suitable for some cylinders. Therefore, this article proposes a method for calculating the normal vector according to the radius, length, and postures of the cylinders before the segmentation of the cylinder model.

Figure 6(a) shows the definition of manipulator in this article, where the numbers 1–8 are called links or cylinders of the manipulator. Robot arms can represent different positions through the rotation of joints. There are eight links (or cylinders) and six joints in manipulator. The X-axis, shown in Table 2, is the world coordinate axis, which is also the X-axis of the first RGB-D camera.

Figure 6.

The definition of manipulator and the manipulator colored links. (a) The definitions of UR5 links. (b) The UR5 colored links.

The manipulator links are processed with different colors before capturing images. Links 1 and 4 and the arc below link 5 are kept gray. Links 2, 3, and 5 are processed with blue, red, and green, respectively. Links 6 to 8 and the blue caps of the links are processed with black. Figure 6(b) shows the colored links of the manipulator.

This paper finds the principal axis vectors of links 2, 3, and 5, and then computes the angles of joints 1 to 3 through vector computing.

In this paper, a posture measurement method is proposed. Firstly, two RGB-D cameras ^20,21 and PCL are used to model the images, and the angles of joints 1 to 3 postures in manipulator ^{21 –23} are calculated. The next step is to verify the feasibility of this method by comparing the errors between the calculated angle and the joint angle displayed on the manipulator system.

The reference point of the images captured by the second RGB-D camera is converted to that of the first RGB-D camera with rotational matrix and translational matrix, which are shown in equation (43)

$\begin{array}{l}\left[\begin{array}{l}{x^{\prime }}_{{\text{cloud}}_{\text{2}}}\\ {y^{\prime }}_{{\text{cloud}}_{\text{2}}}\\ {z^{\prime }}_{{\text{cloud}}_{\text{2}}}\end{array}\right]=\left[\begin{array}{ccc}\text{cos}{\theta }_{\text{rot}}& 0& \text{sin}{\theta }_{\text{rot}}\\ 0& 1& 0\\ -\text{sin}{\theta }_{\text{rot}}& 0& \text{cos}{\theta }_{\text{rot}}\end{array}\right]\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{2}}}\\ y_{{\text{cloud}}_{\text{2}}}\\ z_{{\text{cloud}}_{\text{2}}}\end{array}\right]+\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{mov}}}\\ y_{{\text{cloud}}_{\text{mov}}}\\ z_{{\text{cloud}}_{\text{mov}}}\end{array}\right]\end{array}$

40

After that the images are converted to the PCD and saved as the PCD files in order of the two RGB-D cameras, Kinect for Windows SDK 2.0, and PCL. $(x_{{\text{cloud}}_{\text{2}}},y_{{\text{cloud}}_{\text{2}}},z_{{\text{cloud}}_{\text{2}}})$ , shown in equation (43), is the PCD point of the images captured by the second RGB-D camera, and it can also represent $\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{2}}}\\ y_{{\text{cloud}}_{\text{2}}}\\ z_{{\text{cloud}}_{\text{2}}}\end{array}\right]$ with matrix. $({x^{\prime }}_{{\text{cloud}}_{\text{2}}},{y^{\prime }}_{{\text{cloud}}_{\text{2}}},{z^{\prime }}_{{\text{cloud}}_{\text{2}}})$ is the PCD point after the rotation and the translation, and it can represent $\left[\begin{array}{l}{x^{\prime }}_{{\text{cloud}}_{\text{2}}}\\ {y^{\prime }}_{{\text{cloud}}_{\text{2}}}\\ {z^{\prime }}_{{\text{cloud}}_{\text{2}}}\end{array}\right]$ with matrix; that is, it is also the PCD point whose reference point is the first RGB-D camera. ${\theta }_{\text{rot}}$ is the rotational angle of the second RGB-D camera relative to the first RGB-D camera. $(x_{{\text{cloud}}_{\text{mov}}},y_{{\text{cloud}}_{\text{mov}}},z_{{\text{cloud}}_{\text{mov}}})$ is the translation of the second RGB-D camera relative to the first RGB-D camera, and it can also represent $\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{mov}}}\\ y_{{\text{cloud}}_{\text{mov}}}\\ z_{{\text{cloud}}_{\text{mov}}}\end{array}\right]$ with matrix.

At last, the two PCD files are combined to a PCD file, and the combined image of the PCD is shown in Figure 7. The PCD image is divided into three different clusters according to colors and spatial information. Consequently, a Gaussian noise filtering process is performed for each cluster.

Figure 7.

The definition of manipulator and the manipulator colored links. (a) The definitions of UR5 links. (b) The UR5 colored links.

In the environment of the application experiment, the base of the manipulator is about 0.7 m from the ground. The distance between the first RGB-D camera and the floor is about 1.333 m. The distance between the second RGB-D camera and the floor is about 1.273 m. The shortest distance between the first RGB-D camera and the manipulator is about 1.29 m. The shortest distance between the second RGB-D camera and the manipulator is about 1.4 m, and the distances on X- and Z-axes between the two RGB-D cameras are about 1.22 mand 1.24 m, respectively. The parameters of ${\theta }_{\text{rot}}$ ²⁴ and the translational matrix $\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{mov}}}\\ y_{{\text{cloud}}_{\text{mov}}}\\ z_{{\text{cloud}}_{\text{mov}}}\end{array}\right]$ according to equation (43) where the PCD captured by the second RGB-D camera relative to the first RGB-D camera are ${\theta }_{\text{rot}}=\frac{\pi }{2}$ and $\left[\begin{array}{l}{x}_{{\text{cloud}}_{\text{mov}}}\\ y_{{\text{cloud}}_{\text{mov}}}\\ z_{{\text{cloud}}_{\text{mov}}}\end{array}\right]=\left[\begin{array}{l}-1.22\\ -0.06\\ 1.24\end{array}\right]$ .

The rotational angles of the manipulator joints are theoretically unlimited and the manipulator joints can rotate from 0° to 360°. Nevertheless, the manipulator joints are limited by the spatial environment and the manipulator black tubes. As a consequence, this article takes samples according to the symmetry of the rotational angles.

The samples are the manipulator postures where the manipulator only moves one joint by 1° once, and the moving trace is a semicircle. This article uses these samples to compute the rotational angles of joints 1 to 3. The samples of the manipulator postures are shown in Table 1, where ${\theta }_{1_{UR5}}$ , ${\theta }_{2_{UR5}}$ , and ${\theta }_{3_{UR5}}$ are the angles of joints 1 to 3, respectively, displayed on the manipulator system. The method steps of the application experiment are shown in Figure 8.

Table 1.

The samples of manipulator postures.

Rotational joint number	1	2	3
Rotational angular interval	1°	1°	1°
${\theta }_{1UR5}$	0°–180°	0°	$0^o$
${\theta }_{2UR5}$	−90°	0°–180°	−90°
${\theta }_{3UR5}$	0°	0°	−90° to 90°
Number of PCD files	181	181	181
Total number of PCD files	543

PCD: point cloud data.

Figure 8.

The method steps of the application experiment.

The principal axis of link 2 needs to be computed within the method of principal axis compensating because the posture of link 2 is probably not captured by RGB-D cameras in some manipulator postures. This experiment uses the threshold of the PCD point number of cluster 2 to determine whether the method of principal axis compensating should be used or not. As the setting of the threshold, it is gotten by using the comparison of cylinder model segmentation and principal axis compensating with link 2 of the samples where the manipulator rotates joint 2.

Experimental results

The metrics of comparison is the errors between the computed angles of joint 1 and the angles of joint 1 displayed on the manipulator system. The fixed $k_{\text{EV}}$ value within cylinder model segmentation is according to PCL Developers. ¹⁹ $k_{\text{EV}}=50$ is the normal vector computation of the PCD within cylinder model segmentation. The angular errors are the angles of joint 1 displayed on the manipulator system minus the angles of joint 1 within the two methods, respectively.

After experiments, the following results are obtained. The experimental results are shown in Figure 9. When the angle of joint 2 is −125°, which also means that the point number of cluster 2 is less than 328 points, the accuracy of the angles will be unstable within cylinder model segmentation, while principal axis compensating is not influenced by the point number of cluster 2. To increase the stability of the angular accuracy, the threshold is set to be 500 points.

Figure 9.

The comparison of two methods.

In the application experiment, this article compares the proposed method with the original method, where the explanation is shown in Table 2. The original method, shown in Table 2, is according to the fixed value $k_{\text{EV}}$ with PCL Developers. ¹⁹ −125° is the normal vector computation of the PCD within cylinder model segmentation, while the normal vector computation of the PCD within the proposed method is the variable $k_{\text{EV}}$ value according to equation (21). Additionally, the proposed method uses the threshold of principal axis compensating to determine the principal axis vector of link 2 gotten from cylinder model segmentation or from principal axis compensating.

Some of the experimental results of the samples are shown in Figures 10 to 12, where θ is the collective term of the control group ${\theta }_c$ and the experimental group ${\theta }_E,\theta =\left[\begin{array}{l}{\theta }_1\\ {\theta }_2\\ {\theta }_3\end{array}\right]$ , control group ${\theta }_c$ is the three computed angles within the original method, ${\theta }_c=\left[\begin{array}{l}{\theta }_{1c}\\ {\theta }_{2c}\\ {\theta }_{3c}\end{array}\right]$ , experimental group ${\theta }_E$ is the three computed angles within the proposed method, ${\theta }_E=\left[\begin{array}{l}{\theta }_{1E}\\ {\theta }_{2E}\\ {\theta }_{3E}\end{array}\right]$ , and ${\theta }_{UR5}$ in Figures 10 to 12 is the three angles displayed on the manipulator system, ${\theta }_{UR5}=\left[\begin{array}{l}{\theta }_{1UR5}\\ {\theta }_{2UR5}\\ {\theta }_{3UR5}\end{array}\right]$ ; the red, green, and blue line segments are the principal axes of links 2, 3, and 5, respectively.

Figure 10.

The experimental results while manipulator system. The three angles of UR5 system are 55°, −90°, and 0°.

Figure 11.

The experimental results while manipulator system. The three angles of UR5 system are 0°, −79°, and 0°.

Figure 12.

The experimental results while manipulator system. The three angles of UR5 system are 0°, −90°, and −48°.

Figure 10 shows the results within the original method and the proposed method with one of the samples chosen from rotational joint 1. The absolute errors of three joint angular of the proposed method are smaller than those of the original method according to the experimental results. Figure 11 shows the results within the original method and the proposed method with one of the samples chosen from rotational joint 2. The absolute errors of three joint angular of the proposed method are smaller than those of the original method according to the experimental results. Figure 12 shows the results within the original method and the proposed method with one of the samples chosen from rotational joint 3. The absolute errors of three joint angular of the proposed method are smaller than those of the original method according to the experimental results. All of the experimental results of the samples are shown in Figures 13 to 15 and Table 3. From the experimental results shown in Table 3, it can be seen that the proposed method reduces the error of posture measurement for the manipulator system from the two aspects of average error and standard deviation. But the average velocity of image processing is slower than that of the original method.

Figure 13.

The errors of joint 1 angles.

Figure 14.

The errors of joint 2 angles.

Figure 15.

The errors of joint 2 angles.

Table 3.

The explanation of experimental methods.

Items             Method	Original method
Average of joint 1 (°)	3.018
Sample standard deviation of joint 1 (°)	9.059
Average of joint 2 (°)	1.384
Sample standard deviation of joint 2 (°)	2.524
Average of joint 3 (°)	1.070
Sample standard deviation of joint 3 (°)	0.829
Average time of image processing (s/piece)	0.165
Average velocity of image processing (piece/s) (°)	6.048
Parameters            Method	Proposed method
Average of joint 1 (°)	1.134
Sample standard deviation of joint 1 (°)	0.885
Average of joint 2 (°)	1.040
Sample standard deviation of joint 2 (°)	0.672
Average of joint 3 (°)	1.106
Sample standard deviation of joint 3 (°)	0.837
Average time of image processing (piece/s)	0.196
Average velocity of image processing (piece/s) (°)	5.113

The horizontal axis of Figure 13 is the angle of the rotational joints in Table 1, and the vertical axis is the angle of joint 1 where the manipulator system minuses the original method and the proposed method, respectively; that is to say, ${\theta }_{1_{UR5}}-{\theta }_{1_C}$ and ${\theta }_{1_{UR5}}-{\theta }_{1_E}$ . There are obvious errors within the original method according to the experimental results with some samples of rotational joint 2, while there are almost the same errors between the original method and the proposed method with other samples. The horizontal and vertical axes in Figure 14 are defined in Figure 13. As well, there are large errors within the original method according to the experimental results with some samples of rotational joint 2. In the cases of Figure 15, the setting is similar, but takes account of ${\theta }_{3_{UR5}}-{\theta }_{3_C}$ and ${\theta }_{3_{UR5}}-{\theta }_{3_E}$ . Table 3 presents the absolute errors of joint angular and the image processing time of all the samples within the original method and the proposed method. The absolute errors of joint angular in Table 3 are the three joint angles where the manipulator system minuses the original method and the proposed method, respectively; that is to say, $|{\theta }_{3_{UR5}}-{\theta }_{3_C}|$ and $|{\theta }_{3_{UR5}}-{\theta }_{3_E}|$ . In terms of the angular absolute errors, some absolute errors within the original method are almost the same as those within the proposed method, while some absolute errors within the original method are bigger than those within the proposed method. The overall angular absolute errors of using the proposed method are within about 2°. In terms of the image processing time, the proposed method is about 0.031 s/piece of the images slower than the original method. By the way, the computations of the image processing time and the image processing velocity start after loading the PCD files, and end after computing the three joint angles in Table 3; that is, the image processing time and the image processing velocity in Table 3 are without loading the PCD files.

In this article, a method for posture measurement of the manipulator by processing PCD of different joints is presented. Firstly, the image of the manipulator is captured by two Kinect sensors, and then the PCD is preprocessed by image merging and noise filtering. Finally, the principal axis of the joint is calculated by a principal axis compensation method, and the angle of the joint is estimated by the principal axis. In this article, a multi-joint manipulator is studied, and the point cloud clusters of different joints can be obtained by model segmentation for different types of robots, such as underwater vehicle-manipulator system. ^25,26 If the perceptual noise of Kinect sensors in underwater environment can be overcome or the noise in PCD can be filtered better, this method can provide a new solution for the posture measurement of underwater vehicle-manipulator system.

The main factors affecting the precision of posture measurement are the influence of noise in PCD and the incompleteness of PCD. Our future research will focus on the following three aspects: Firstly, we will improve the noise filtering method in point cloud and design a robust noise filtering algorithm to increase the number of effective PCD. Therefore, the precision of posture measurement of manipulator will be improved. Secondly, the incompleteness of point cloud data will also affect the accuracy of posture measurement, and the incompleteness of point cloud data is often caused by external environmental factors such as occlusion or environmental noise. ^27,28 An effective method of PCD completion will bring new solutions to this problem. Therefore, we will study the method of PCD completion and reduce the impact of environment on image acquisition. Thirdly, a robust point cloud clustering algorithm can partition different types of PCD, which can also improve the processing speed of PCD. The higher the partition accuracy of PCD, the higher the accuracy of posture measurement.

Conclusions

Aiming at the safety problems where robots and operators are highly coupled in a working space, a posture measurement approach for an articulated manipulator is proposed in this article. In order to overcome the dilemma that the accuracy of cylindrical model segmentation method is not high when the number of PCD is incomplete, a principal axis compensation method is proposed in this article. Although the proposed method in this article is slower than the original method in terms of the image processing velocity due to the more one step of Gaussian noise filtering, where the image processing velocity of the proposed method is about 5 piece/s, and the range of the proposed method is smaller than that of the original method in terms of the overall errors, where the error range of the proposed method is between about 5° and −5°. That is, the accuracy of the proposed method is better than that of the original method. The experimental results show that the proposed method is more accurate than the cylindrical model segmentation in posture measurement when the PCD is incomplete. At the same time, under normal circumstances, the result of using cylindrical robot arm posture measurement method proposed in this article is as good as that of using cylinder model segmentation with the original method according to the experimental results in this article.