Path Tracking of a Wheeled Mobile Manipulator through Improved Localization and Calibration

Abstract

This chapter focuses on path tracking of a wheeled mobile manipulator designed for manufacturing processes such as drilling, riveting, or line drawing, which demand high accuracy. This problem can be solved by combining two approaches: improved localization and improved calibration. In the first approach, a full-scale kinematic equation is derived for calibration of each individual wheel’s geometrical parameters, as opposed to traditionally treating them identical for all wheels. To avoid the singularity problem in computation, a predefined square path is used to quantify the errors used for calibration considering the movement in different directions. Both statistical method and interval analysis method are adopted and compared for estimation of the calibration parameters. In the second approach, a vision-based deviation rectification solution is presented to localize the system in the global frame through a number of artificial reflectors that are identified by an onboard laser scanner. An improved tracking and localization algorithm is developed to meet the high positional accuracy requirement, improve the system’s repeatability in the traditional trilateral algorithm, and solve the problem of pose loss in path following. The developed methods have been verified and implemented on the mobile manipulators developed by Shanghai University.

1. Introduction

Recently, mobile manipulators have been used in various industries including aerospace or indoor decoration engineering, which requires a large workspace [1, 2, 3, 4]. The said mobile manipulator consists of an industrial manipulator mounted on a mobile platform to perform various manufacturing tasks such as drilling/riveting in aerospace industry or baseline drawing in decoration engineering. Wheeled mobile platforms with Mecanum wheels that can easily navigate through crowded spaces due to their omnidirectionality with a zero turning radius are commonly used. Path tracking is one of the important issues for mobile manipulators, in particular for performing manufacturing tasks. This chapter addresses this issue from the aspect of localization and calibration.

Localization is a key functionality of mobile manipulator in order to track and determine its position around the environment [5]. Many methods are proposed to address this issue [6]. They can be divided into two categories: absolute localization and relative localization.

Absolute localization relies on detecting and recognizing different features in the environment to obtain the position and posture. The features can be normally divided into two types: artificial landmarks and natural landmarks. Compared with the natural landmarks, artificial landmarks have advantages of high recognition, which will lead to high accuracy. There is no cumulative error problem when a localization method based on artificial landmarks is used. The key challenge is to identify and extract the needed information from the raw data of landmarks. For the relative localization, dead reckoning and inertial navigation are commonly carried out to obtain the systems’ position. It does not have to perceive the external environment, but the drift error accumulates over time.

Researchers have proposed several solutions, such as fuzzy reflector-based localization [7] and color reflector-based self-localization [8]. The main drawbacks of these two methods are that the anti-interference ability is poor and the computation is huge. To solve these problems pertaining to the localization method based on artificial landmarks, Madsen and Andersen [9] proposed a method using three reflectors and the triangulation principle. Betke and Gurvits [10] proposed a multichannel localization method with the three-dimensional localization principle and the least squares method. Because of the unavoidable errors in position and angle measurement of reflectors, the use of only a trilateral or triangular method will not achieve high accuracy [11]. Nevertheless, there are still many challenges for mobile manipulator working in industrial environments such as aerospace manufacturing or decoration engineering, which requires high maneuverability and high accuracy at the same time. The stationary industrial manipulator has high repeated localization accuracy, which the mobile manipulator cannot achieve. This chapter focuses on the improvement of path tracking of a mobile manipulator through enhanced localization combined with calibration. Calibration is required for the system kinematic model established based on nominal geometry parameter to improve motion accuracy. Muir and Neuman [12] proposed a kinematic error model for Mecanum wheeled platform and applied actuated inverse and sensed forward solutions to the kinematic control. Wang and Chang [13] carried out error analysis in terms of distribution among Mecanum wheels. Shimada et al. [14] presented a position-corrective feedback control method with vision system on Mecanum wheeled mobile platform. Qian et al. [15] conducted a more detailed analysis on the installation angle of rollers. An improved calibration method is presented in this chapter to improve the tracking accuracy of a mobile manipulator.

2. System modeling

The wheeled mobile manipulator, as shown in Figure 1, is built with a manipulator onto a wheeled mobile platform with four Mecanum wheels. This system aims to carry out fuselage drilling/riveting tasks at assembly stations in an adaptive and flexible way in the aerospace industry.

Figure 1.

The wheeled mobile manipulator for fuselage drilling/riveting.

This system needs to localize in real time during machining process. The global frame $\{{\mathrm{X}}_{\mathrm{G}},{\mathrm{Y}}_{\mathrm{G}},{\mathrm{Z}}_{\mathrm{G}}\}$ is attached to the ground in the environment. The platform frame $\{{\mathrm{X}}_{\mathrm{P}},{\mathrm{Y}}_{\mathrm{P}},{\mathrm{Z}}_{\mathrm{P}}\}$ is attached to the center of the mobile platform. The tool frame $\left\{{\mathrm{X}}_{\mathrm{M}},{\mathrm{Y}}_{\mathrm{M}},{\mathrm{Z}}_{\mathrm{M}}\right\}$ is attached to the tool of the manipulator. Two laser range finders are equipped and their frames $\left\{{\mathrm{X}}_{\mathrm{L}1},{\mathrm{Y}}_{\mathrm{L}1},{\mathrm{Z}}_{\mathrm{L}1}\right\}$ and $\left\{{\mathrm{X}}_{\mathrm{L}2},{\mathrm{Y}}_{\mathrm{L}2},{\mathrm{Z}}_{\mathrm{L}2}\right\}$ are attached to the left-front and right-rear of the mobile platform, respectively. The vision frame $\left\{{\mathrm{X}}_{\mathrm{V}},{\mathrm{Y}}_{\mathrm{V}},{\mathrm{Z}}_{\mathrm{V}}\right\}$ is attached to the industrial camera.

The position and posture of the system in the global frame can be defined as

$${}^{\mathrm{G}}\mathbf{P}=[{}^{\mathrm{G}}\mathrm{x},\hspace{0.17em}{}^{\mathrm{G}}\mathrm{y},\hspace{0.17em}{}^{\mathrm{G}}\theta ]$$E1

where ${}_{}{}^{\mathrm{G}}\mathrm{x}$ and ${}_{}{}^{\mathrm{G}}\mathrm{y}$ are the positions in two directions, respectively, and ${}_{}{}^{\mathrm{G}}\mathrm{\theta }$ is the azimuth angle, as shown in Figure 2.

Figure 2.

The position and posture of system.

3. Accuracy analysis of a wheeled mobile manipulator

3.1. Problem formulation

Figure 3 shows the kinematic model of the mobile platform with Mecanum wheels. According to kinematic analysis [16], the motion equation can be obtained as

$$\mathbf{V}={\mathbf{D}}_0\mathbf{W}$$E2

Figure 3.

The kinematic model of a Mecanum wheeled mobile platform.

where $\mathbf{V}={\left[{\mathrm{V}}_{\mathrm{X}},{\mathrm{V}}_{\mathrm{Y}},{\mathrm{\omega }}_0\right]}^{\mathrm{T}}$, ${\mathbf{D}}_{\mathbf{0}}=\frac{1}{4}\left[\begin{array}{cccc}-{\mathrm{R}}_0& {\mathrm{R}}_0& -{\mathrm{R}}_0& {\mathrm{R}}_0\\ {\mathrm{R}}_0& {\mathrm{R}}_0& {\mathrm{R}}_0& {\mathrm{R}}_0\\ \frac{{\mathrm{R}}_0}{{\mathrm{l}}_0+\mathrm{L}}& -\frac{{\mathrm{R}}_0}{{\mathrm{l}}_0+\mathrm{L}}& -\frac{{\mathrm{R}}_0}{{\mathrm{l}}_0+\mathrm{L}}& \frac{{\mathrm{R}}_0}{{\mathrm{l}}_0+\mathrm{L}}\end{array}\right]$, $\mathbf{W}={\left[{\mathrm{\omega }}_1,{\mathrm{\omega }}_2,{\mathrm{\omega }}_3,{\mathrm{\omega }}_4\right]}^{\mathrm{T}}$.

${\left[{\mathrm{V}}_{\mathrm{X}},{\mathrm{V}}_{\mathrm{Y}},{\mathrm{\omega }}_0\right]}^{\mathrm{T}}$ is defined as the linear and angular speeds of the platform; L is the half distance from the front axle to the rear axle as shown in Figure 3; ${\mathrm{l}}_0$ is the transverse distance from the wheel centers to the platform center line; ${\mathrm{R}}_0$ is the radius of the Mecanum wheel; $\text{and}{\mathrm{\omega }}_1$, ${\mathrm{\omega }}_2$, ${\mathrm{\omega }}_3$, and ${\mathrm{\omega }}_4$ are angular velocities of the four wheels, respectively.

The Mecanum wheel and its roller are shown in Figure 4. The roller is fitted on the edge of the wheel at a certain angle (generally 45°) with its central axis. Each roller can rotate freely around the central axis. The wheel relies on the friction between the roller and the ground to move. The material of roller’s outer rim is usually rubber, which will deform under pressure between ground and the wheel.

Figure 4.

Mecanum wheel and roller.

Figure 5 shows the distribution of roller deformation. F and T are the force and driving torque on the roller, respectively. The radiuses of wheels reduce differently under different pressures. The deformation zone and the position of the wheel center change with the action of driving torque and the shifting [17]. Furthermore, with such deformation, ${\mathrm{R}}_0$ and ${\mathrm{l}}_0$ will change.

Figure 5.

Roller deformation.

As shown in Figure 6, based on the kinematic analysis and consideration on deformation of the roller, Eq. (2) can be revised as follows: [18].

$$\mathbf{V}={\mathbf{D}}_1\mathbf{W}$$E3

where ${\mathbf{D}}_1=\frac{1}{4}\left[\begin{array}{cccc}-{\mathrm{R}}_1& {\mathrm{R}}_2& -{\mathrm{R}}_3& {\mathrm{R}}_4\\ {\mathrm{R}}_1& {\mathrm{R}}_2& {\mathrm{R}}_3& {\mathrm{R}}_4\\ \frac{{\mathrm{R}}_1}{{\mathrm{l}}_1+\mathrm{L}}& -\frac{{\mathrm{R}}_2}{{\mathrm{l}}_2+\mathrm{L}}& -\frac{{\mathrm{R}}_3}{{\mathrm{l}}_3+\mathrm{L}}& \frac{{\mathrm{R}}_4}{{\mathrm{l}}_4+\mathrm{L}}\end{array}\right]$; here ${\mathbf{D}}_1$ is defined by ${\mathrm{R}}_1$, ${\mathrm{R}}_2$, ${\mathrm{R}}_3,$ and ${\mathrm{R}}_4$, which are individual radiuses of the four Mecanum wheels, respectively. In order to improve the system motion accuracy, the relative errors ${\mathbf{∆}\mathrm{R}}_{\mathrm{i}}$ between ${\mathrm{R}}_{\mathrm{i}}$ and ${\mathrm{R}}_0$, and relative errors $\mathbf{∆}{\mathrm{l}}_{\mathrm{i}}$ between ${\mathrm{l}}_{\mathrm{i}}$ and ${\mathrm{l}}_0$ ($\mathrm{i}=1,2,3,4$) should be obtained to revise matrix ${\mathbf{D}}_1$ (Figure 6).

Figure 6.

Motion model of the mobile platform.

3.2. Error modeling

By multiplying time $\mathrm{t}$, Eq. (3) becomes a displacement equation as

$$\mathbf{X}={\mathbf{D}}_{\mathbf{1}}\mathbf{\theta }$$E4

where $\mathbf{X}=\mathbf{V}\mathrm{t}$ is ${\left[\mathrm{X},\mathrm{Y},\mathrm{\theta }\right]}^{\mathrm{T}}$. $\mathbf{\theta }=\mathbf{W}\mathrm{t}$ is ${\left[{\mathrm{\theta }}_1,{\mathrm{\theta }}_2,{\mathrm{\theta }}_3,{\mathrm{\theta }}_4\right]}^{\mathrm{T}}$. Individual geometric parameters including ${\mathrm{l}}_1$, ${\mathrm{l}}_2$, ${\mathrm{l}}_3$, ${\mathrm{l}}_4$, ${\mathrm{R}}_1$, ${\mathrm{R}}_2$, ${\mathrm{R}}_{3,}$ and ${\mathrm{R}}_4$ are variables in Eq. (4). The relative errors can be determined from

$$\mathbf{X}=\frac{1}{4}\left[\begin{array}{cccc}-{\mathrm{R}}_1{\mathrm{\theta }}_1& {\mathrm{R}}_2{\mathrm{\theta }}_2& -{\mathrm{R}}_3{\mathrm{\theta }}_3& {\mathrm{R}}_4{\mathrm{\theta }}_4\\ {\mathrm{R}}_1{\mathrm{\theta }}_1& {\mathrm{R}}_2{\mathrm{\theta }}_2& {\mathrm{R}}_3{\mathrm{\theta }}_3& {\mathrm{R}}_4{\mathrm{\theta }}_4\\ \frac{{\mathrm{R}}_1}{{\mathrm{l}}_1+\mathrm{L}}{\mathrm{\theta }}_1& -\frac{{\mathrm{R}}_2}{{\mathrm{l}}_2+\mathrm{L}}{\mathrm{\theta }}_2& -\frac{{\mathrm{R}}_3}{{\mathrm{l}}_3+\mathrm{L}}{\mathrm{\theta }}_3& \frac{{\mathrm{R}}_4}{{\mathrm{l}}_4+\mathrm{L}}{\mathrm{\theta }}_4\end{array}\right]$$E5

leading to

$$∆\mathbf{X}=\mathbf{G}∆\mathbf{B}$$E6

where $\mathbf{∆X}={\left[\mathrm{∆X},\mathrm{∆Y},\mathrm{∆\theta }\right]}^{\mathrm{T}}$, $\mathbf{∆B}={\left[∆{\mathrm{R}}_1,∆{\mathrm{R}}_2,∆{\mathrm{R}}_3,∆{\mathrm{R}}_4,∆{\mathrm{l}}_1,∆{\mathrm{l}}_2,∆{\mathrm{l}}_3,∆{\mathrm{l}}_4\right]}^{\mathrm{T}}$, and

$$\mathbf{G}=\left[\begin{array}{ccc}-{\mathrm{\theta }}_1& {\mathrm{\theta }}_2& -{\mathrm{\theta }}_3\\ {\mathrm{\theta }}_1& {\mathrm{\theta }}_2& {\mathrm{\theta }}_3\\ \frac{{\mathrm{\theta }}_1}{{\mathrm{M}}_1}& -\frac{{\mathrm{\theta }}_2}{{\mathrm{M}}_2}& -\frac{{\mathrm{\theta }}_3}{{\mathrm{M}}_3}\end{array}\begin{array}{ccc}{\mathrm{\theta }}_4& 0& 0\\ {\mathrm{\theta }}_4& 0& 0\\ \frac{{\mathrm{\theta }}_4}{{\mathrm{M}}_4}& -\frac{{\mathrm{R}}_1{\mathrm{\theta }}_1}{{{\mathrm{M}}_1}^2}& \frac{{\mathrm{R}}_2{\mathrm{\theta }}_2}{{{\mathrm{M}}_2}^2}\end{array}\begin{array}{cc}0& 0\\ 0& 0\\ \frac{{\mathrm{R}}_3{\mathrm{\theta }}_3}{{{\mathrm{M}}_3}^2}& -\frac{{\mathrm{R}}_4{\mathrm{\theta }}_4}{{{\mathrm{M}}_4}^2}\end{array}\right]$$

$${\mathrm{R}}_1={\mathrm{R}}_0+∆{\mathrm{R}}_1,{\mathrm{l}}_1={\mathrm{l}}_0+∆{\mathrm{l}}_1,{\mathrm{R}}_2={\mathrm{R}}_0+∆{\mathrm{R}}_2,{\mathrm{l}}_2={\mathrm{l}}_0+∆{\mathrm{l}}_2$$

$${\mathrm{R}}_3={\mathrm{R}}_0+∆{\mathrm{R}}_3,{\mathrm{l}}_3={\mathrm{l}}_0+∆{\mathrm{l}}_3,{\mathrm{R}}_4={\mathrm{R}}_0+∆{\mathrm{R}}_4,{\mathrm{l}}_4={\mathrm{l}}_0+∆{\mathrm{l}}_4$$

With the least squares method, the geometric errors can be solved as

$$∆\mathbf{B}={\left({\mathbf{G}}^{\mathrm{T}}\mathbf{G}\right)}^{-1}{\mathbf{G}}^{\mathrm{T}}∆\mathbf{X}$$E7

$∆{\mathrm{R}}_{\mathrm{i}}$ is generally defined as the tolerance resulting from manufacturing or assembly. With the deformation of the roller and $∆{\mathrm{l}}_{\mathrm{i}}$ limited to $\left[–\mathrm{h},\mathrm{h}\right]$ and $\left[–\mathrm{j},\mathrm{j}\right]$ $(\mathrm{h}=3\mathrm{mm}$, $\mathrm{j}=5\mathrm{mm}$) respectively, ${\mathrm{l}}_{\mathrm{i}}$ and ${\mathrm{R}}_{\mathrm{i}}$ can be defined as:

$${\mathrm{l}}_{\mathrm{i}}=2\mathrm{jr}+\left({\mathrm{l}}_0-\mathrm{j}\right)$$E8

$${\mathrm{R}}_{\mathrm{i}}=2\mathrm{hr}+\left({\mathrm{R}}_0-\mathrm{h}\right)$$E9

where ${\mathrm{l}}_{\mathrm{i}}$ and ${\mathrm{R}}_{\mathrm{i}}$ are in $\left[{\mathrm{l}}_0-\mathrm{j},{\mathrm{l}}_0+\mathrm{j}\right]$ and $\left[{\mathrm{R}}_0-\mathrm{h},{\mathrm{R}}_0+\mathrm{h}\right]$, respectively, and r is a random number between 0 and 1. The angular speeds of all wheels are set to $20\mathrm{rad}/\mathrm{s}$, and the time t is set to 1 s.

The rank of matrix $\mathbf{G}$ in Eq. (7) is generally 3, so $\mathbf{G}$ is a full rank matrix and $\mathbf{∆B}$ can be obtained. The following steps can be carried out. First, the displacement error measurement is analyzed. In order to obtain $\mathbf{∆B}$, it is needed to obtain the displacement error matrix $\mathbf{∆X}$ first.

The system is moved in the ${\mathrm{Y}}_{\mathrm{G}}$ direction and stopped every 2 s to obtain its position and posture $\left[{}_{}{}^{\mathrm{G}}{\mathrm{X}}_{\mathrm{k}},{}_{}{}^{\mathrm{G}}{\mathrm{Y}}_{\mathrm{k}},{}_{}{}^{\mathrm{G}}{\mathrm{\theta }}_{\mathrm{k}}\right]$. The principle of measurement is shown in Figure 7 and $∆\mathbf{X}$ can be calculated as

$$\mathbf{∆X}=\left[\begin{array}{c}\left({}_{}{}^{\mathrm{G}}{\mathrm{X}}_1+{{}_{}{}^{\mathrm{G}}\mathrm{X}}_2+{{}_{}{}^{\mathrm{G}}\mathrm{X}}_3+{{}_{}{}^{\mathrm{G}}\mathrm{X}}_4\right)/10-{\mathrm{X}}_{\mathrm{T}}\\ \left({{}_{}{}^{\mathrm{G}}\mathrm{Y}}_1+{{}_{}{}^{\mathrm{G}}\mathrm{Y}}_2+{{}_{}{}^{\mathrm{G}}\mathrm{Y}}_3+{{}_{}{}^{\mathrm{G}}\mathrm{Y}}_4\right)/10-{\mathrm{Y}}_{\mathrm{T}}\\ \left({{}_{}{}^{\mathrm{G}}\mathrm{\theta }}_1+{{}_{}{}^{\mathrm{G}}\mathrm{\theta }}_2+{{}_{}{}^{\mathrm{G}}\mathrm{\theta }}_3+{{}_{}{}^{\mathrm{G}}\mathrm{\theta }}_4\right)/10-{\mathrm{\theta }}_{\mathrm{T}}\end{array}\right]$$E10

Figure 7.

The principle of measuring displacement.

According to Eq. (2), ${\mathrm{X}}_{\mathrm{T}}$, ${\mathrm{Y}}_{\mathrm{T}}$, and ${\mathrm{\theta }}_{\mathrm{T}}$ are theoretical values which can be obtained as follows: ${\mathrm{X}}_{\mathrm{T}}={\mathrm{V}}_{\mathrm{x}}\mathrm{t}$, ${\mathrm{Y}}_{\mathrm{T}}={\mathrm{V}}_{\mathrm{y}}\mathrm{t}$, ${\mathrm{\theta }}_{\mathrm{T}}=0$. Finally, through the experiment, the displacement errors can be obtained.

$$\mathbf{∆X}=\left[\begin{array}{c}3\mathrm{mm}\\ 1\mathrm{mm}\\ 0^{°}\end{array}\right]$$E11

Substituting Eq. (11) in to Eq. (7), $\mathbf{∆B}$ can be determined.

The Monte Carlo analysis is applied to deal with 50 samples. While $∆\mathbf{X}$ has been given in Eq. (11), the corresponding $∆\mathbf{B}$ should also satisfy its own tolerance, i.e., $-3\le ∆{\mathrm{R}}_{\mathrm{i}}\le 3$ and $-5\le ∆{\mathrm{L}}_{\mathrm{i}}\le 5$.

The results are given in Figure 8. These data are averaged to form a new result: $∆\mathbf{B}={\left[-0.541,1.237,-0.605,1.055,0.458,0.683,-0.048,-0.683\right]}^{\mathrm{T}}.$ A process capability index is then used to estimate the value of $∆\mathbf{B}$. Process capability refers to the ability of a process to meet the required quality. It is a measure of the minimum fluctuation of the internal consistency of the process itself in the most stable state. When the process is stable, the quality characteristic value of the product is in the range $\left[\mathrm{\mu }-3\mathrm{\sigma },\mathrm{\mu }+3\mathrm{\sigma }\right]$, where μ is the ensemble average of the quality characteristic value of the product, and σ is the standard deviation of the quality characteristic value of the product.

Figure 8.

The values obtained with the Monte Carlo method.

There are two kinds of situations for which the process capability index has to be solved. First, $\mathrm{\mu }=\mathrm{M}$ as shown in Figure 9: ${\mathrm{U}}_{\mathrm{SL}}$ is the maximum error of quality and ${\mathrm{L}}_{\mathrm{sl}}$ is the minimum error of quality; $\mathrm{M}=\left({\mathrm{U}}_{\mathrm{SL}}+{\mathrm{L}}_{\mathrm{SL}}\right)/2$; μ is the average value of process machining; and ${\mathrm{C}}_{\mathrm{p}}$ indicates the process capability index, and ${\mathrm{C}}_{\mathrm{p}}=\left({\mathrm{U}}_{\mathrm{SL}}-{\mathrm{L}}_{\mathrm{SL}}\right)/6\mathrm{\sigma }.$

Figure 9.

$\mathrm{\mu }=\mathrm{M}$.

Second, $\mathrm{\mu }\ne \mathrm{M}$ as shown in Figure 10: ${\mathrm{C}}_{\mathrm{pk}}$ indicates the process capability index, and ${\mathrm{C}}_{\mathrm{pk}}=\left({\mathrm{U}}_{\mathrm{SL}}-{\mathrm{L}}_{\mathrm{SL}}\right)/6\mathrm{\sigma }-\mid \mathrm{M}-\mathrm{\mu }\mid /3\mathrm{\sigma }$. Only process capability index greater than 1 is valid. As $-3\le {\mathrm{∆R}}_{\mathrm{i}}\le 3$ and $-5\le {\mathrm{∆L}}_{\mathrm{i}}\le 5$, $\mathrm{M}=0$ and $\mathrm{\mu }\ne \mathrm{M}$, the following results can be obtained: ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_1\right)=1.58$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_1\right)=0.57$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_2\right)=1.06$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_2\right)=0.58$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_3\right)=1.54$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_3\right)=0.63$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_4\right)=1.17$, $\text{and}{\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_4\right)=0.5$.

Figure 10.

$\mathrm{\mu }\ne \mathrm{M}$.

The values of $∆{\mathrm{R}}_1$, $∆{\mathrm{R}}_2$, $∆{\mathrm{R}}_3$, and $∆{\mathrm{R}}_4$ are closer to the real values than the values of $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_3$, and $∆{\mathrm{l}}_4$. Now, taking $∆{\mathbf{B}}_{\mathbf{1}}\left(\mathbf{a}\right)={\left[-0.541,1.237,-0.605,1.055,0,0,0,0\right]}^{\mathrm{T}}$, $∆{\mathbf{B}}_{\mathbf{1}}\left(\mathbf{b}\right)={\left[0,0,0,0,0.458,0.683,-0.048,-0.683\right]}^{\mathrm{T}}$ in Eq. (5), it can be seen that $∆{\mathbf{X}}_{\mathbf{1}}={\left[-0.4119,,4,-0.0003\right]}^{\mathrm{T}}$, $∆{\mathbf{X}}_{\mathbf{1}}\left(\mathbf{b}\right)={\left[0,0,-0.0003\right]}^{\mathrm{T}}$. According to the change rate relative to Eq. (10), the influence of $∆{\mathrm{R}}_1$,$∆{\mathrm{R}}_2$, $∆{\mathrm{R}}_{3,}$ and $∆{\mathrm{R}}_4$ is much bigger than that of $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_{3,}$ and $∆{\mathrm{l}}_4$. Although the values of $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_3$, and $∆{\mathrm{l}}_4$ are not accurate enough for the real values, ${∆\mathbf{B}}_{\mathbf{1}}$ is valid.

The interval analysis is also carried out. In Eq. (6), $\text{the value of}\mathbf{G}$ is uncertain because of the change of ${\mathrm{∆R}}_{\mathrm{i}}$ and ${\mathrm{∆l}}_{\mathrm{i}}$ in $\left[-3,3\right]$ and $\left[-5,5\right]$, respectively. Now, the increment of ${\mathrm{∆R}}_{\mathrm{i}}$ and ${\mathrm{∆l}}_{\mathrm{i}}$ is set to 1 mm; there are 7 values of ${\mathrm{∆R}}_{\mathrm{i}}$, and 11 values of ${\mathrm{∆l}}_{\mathrm{i}}$, so there are $7^4\times {11}^4$ groups of combinations of matrix $\mathbf{G}.$ Take all the combinations of matrix $\mathbf{G}$ to Eq. (6) to obtain $\mathbf{∆B}$ and exclude the cases for which ${\mathrm{∆R}}_{\mathrm{i}}$ and ${\mathrm{∆l}}_{\mathrm{i}}$ are not in $\left[-3,3\right]$ and $\left[-5,5\right]$, respectively. Thus, 277 groups of $\mathbf{∆B}$ can be obtained. As shown in Figure 11, the average values of $∆{\mathrm{R}}_1$, $∆{\mathrm{R}}_2$, $∆{\mathrm{R}}_3$, $∆{\mathrm{R}}_4$, $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_3$, and $∆{\mathrm{l}}_4$, lead to a new $\mathbf{∆}{\mathbf{B}}_{\mathbf{2}}={\left[-0.233,1.467,-0.932,0.824,0.2,-0.213,-0.068,-0.039\right]}^{\mathrm{T}}$, which is close to $\mathbf{∆}{\mathbf{B}}_{\mathbf{1}}$.

Figure 11.

The values obtained in interval analysis.

The same method is used to solve the process capability index of ${\mathbf{∆B}}_{\mathbf{2}}$. One obtains: ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_1\right)=3.34$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_1\right)=0.68$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_2\right)=1.22$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_2\right)=0.59$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆R}}_3\right)=2.5$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{∆l}}_3\right)=0.63$, ${\mathrm{C}}_{\mathrm{pk}}\left({\mathrm{R}}_4\right)=1.73$, $\text{and}{\mathrm{C}}_{\mathrm{pk}}\left(∆{\mathrm{l}}_4\right)=0.6$. The values of $∆{\mathrm{R}}_1$, $∆{\mathrm{R}}_2$, $∆{\mathrm{R}}_{3,}$ and $∆{\mathrm{R}}_4$ in ${\mathbf{∆B}}_{\mathbf{2}}$ are closer to the real ones than the values of $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_{3,}$ and $∆{\mathrm{l}}_4$ in ${\mathbf{∆B}}_{\mathbf{2}}$. As the influence of $∆{\mathrm{R}}_1$, $∆{\mathrm{R}}_2$, $∆{\mathrm{R}}_3$, $\text{and}∆{\mathrm{R}}_4$ is bigger than that of $∆{\mathrm{l}}_1$, $∆{\mathrm{l}}_2$, $∆{\mathrm{l}}_{3,}$ and $∆{\mathrm{l}}_4$, ${\mathbf{∆B}}_{\mathbf{2}}$ is valid.

The result is verified as well. ${\mathbf{∆B}}_{\mathbf{1}}$ is used to revise the parameters of ${\mathbf{D}}_{\mathbf{1}}$ in Eq. (2) as

$${\mathbf{D}}_1=\frac{1}{4}\left[\begin{array}{ccc}-186.959& 188.737& -186.895\\ 186.959& 188.737& 186.895\\ 0.1369& -0.1382& -0.1369\end{array}\begin{array}{c}188.555\\ 188.555\\ 0.1382\end{array}\right]$$

By setting two sets of four wheel speeds ${\mathbf{W}}_1={\left[10\mathrm{\pi }/21,10\mathrm{\pi }/21,10\mathrm{\pi }/21,10\mathrm{\pi }/21\right]}^{\mathrm{T}}$ and ${\mathbf{W}}_2={\left[4\mathrm{\pi }/7,4\mathrm{\pi }/7,4\mathrm{\pi }/7,4\mathrm{\pi }/7\right]}^{\mathrm{T}}$, the displacement errors can be computed as ${\mathbf{S}}_1=\left({\mathbf{D}}_1-{\mathbf{D}}_0\right){\mathbf{W}}_2\mathrm{t}$ and ${\mathbf{S}}_2=\left({\mathbf{D}}_2-{\mathbf{D}}_0\right){\mathbf{W}}_2\mathrm{t}$. The results of the two correction methods are almost identical in theory.

The experiment with four movements is shown in Figure 12(a), while the actual movement is shown in Figure 12(b). It can be found that ${\mathbf{S}}_1$ and ${\mathbf{S}}_2$ are close to the measured displacement errors, so both ${\mathbf{∆B}}_{\mathbf{1}}$ and ${\mathbf{∆B}}_{\mathbf{2}}$ are satisfied to revise the matrix ${\mathbf{D}}_{\mathbf{1}}$.

Figure 12.

(a) Schematic diagram of measuring and (b) the actual measuring.

4. Localization

4.1. System configuration

The localization component of mobile manipulator includes two laser range finders and a number of reflectors that are of cylindrical shape placed in the environment. Each reflector is covered by a reflective tape with high reflectivity (Figure 13).

Figure 13.

Localization system for the mobile manipulator.

4.2. Dynamic tracking

To achieve accurate localization, the system should have the ability to perceive external information through extracting the reflector features. In this research, as shown in Figure 13, there are n reflectors, and the feature of each reflector ${\mathrm{B}}_{\mathrm{i}}\left(\mathrm{i}=1,2,\dots ,\mathrm{n}\right)$ is extracted from the raw data. The feature extraction algorithm consists of three steps: (i) filtering and clustering, (ii) identification, and (iii) feature extraction.

The first step is filtering and clustering. The raw data obtained by each finder are a set of discrete data sequences $\left\{{\left\{\left(\mathrm{\gamma },\varnothing \right),\mathrm{\lambda }\right\}}_{\mathrm{i}}|\mathrm{i}=1,2,\dots ,\mathrm{n}\right\}$. γ is the distance from the target point to the finder. ∅ is the polar angle. ${\mathrm{\lambda }}_{\mathrm{i}}$ is the intensity value of the $\mathrm{ith}$ data point. In the process of data analysis, the outlier points that are contaminated by noise will be filtered out.

The density of the collected data points is proportional to the distance from the target point to the laser range finder. To improve the efficiency of the feature extraction process, an adaptive clustering method is adopted as given by Eq. (12). Unless the distance between two data points is less than the threshold δ, these data points are clustered for one reflector.

$$\mathrm{\delta }={\mathrm{\gamma }}_{\mathrm{i}-1}\left(\left(sin∆\varnothing \right)/\left(sin\left(\mathrm{\beta }-∆\varnothing \right)\right)\right)+3{\mathrm{\sigma }}_{\mathrm{\gamma }}$$E12

where ${\mathrm{\gamma }}_{\mathrm{i}-1}$ is the distance value of the $\left(\mathrm{i}-1\right)\mathrm{th}$ data point, $∆\varnothing$ is the angle resolution of the laser range finder, β is an auxiliary constant parameter, and ${\mathrm{\sigma }}_{\mathrm{\gamma }}$ is the measurement error. The values of parameters ${\mathrm{\sigma }}_{\mathrm{\gamma }}$ and β are given as 0.01 m and ${10}^{°},$ respectively.

The second step is identification. After clustering, the data set can be correlated to each observable reflector. Each reflector data set is then used to calculate the position of the reflector in the laser range finder frame [19]. Let ${\mathrm{\lambda }}_{\mathrm{\delta }}$ be the reflected intensity threshold and $\left[\mathrm{D}-{\mathrm{D}}_{\mathrm{\delta }},\mathrm{D}+{\mathrm{D}}_{\mathrm{\delta }}\right]$ be the diameter range of a reflector, where D is a nominal reflector diameter and ${\mathrm{D}}_{\mathrm{\delta }}$ is the tolerance. The values of ${\mathrm{\lambda }}_{\mathrm{\delta }}$ and ${\mathrm{D}}_{\mathrm{\delta }}$ are selected based on the actual situation. Considering that ${\mathbf{W}}_{\mathbf{c}}$ represents a set after clustering, i.e., ${\mathbf{W}}_{\mathrm{c}}=\left\{{\left\{\left(\mathrm{\gamma },\varnothing \right),\mathrm{\lambda }\right\}}_{\mathrm{i}}|\mathrm{i}=\mathrm{m},\dots ,\mathrm{n}\right\}$, for each reflector, the measured diameter ${\mathrm{D}}_{\mathrm{c}}$ is calculated as ${\mathrm{D}}_{\mathrm{c}}=\sqrt{{\mathrm{\gamma }}_{\mathrm{n}}^2+{\mathrm{\gamma }}_{\mathrm{m}}^2-2{\mathrm{\gamma }}_{\mathrm{n}}{\mathrm{\gamma }}_{\mathrm{m}}cos\left({\varnothing }_{\mathrm{n}}-{\varnothing }_{\mathrm{m}}\right)}$, where ${\left(\mathrm{\gamma },\varnothing \right)}_{\mathrm{m}}$ and ${\left(\mathrm{\gamma },\varnothing \right)}_{\mathrm{n}}$ are the beginning and end data of a reflector set, respectively. When the set ${\mathbf{W}}_{\mathbf{c}}$ satisfies the following two conditions

$$\left\{\begin{array}{c}{\mathrm{\lambda }}_{\max }\ge {\mathrm{\lambda }}_{\mathrm{\delta }}\\ {\mathrm{D}}_{\mathrm{c}}\in \left[{\mathrm{D}}_{\min },{\mathrm{D}}_{\max }\right]\end{array}\right.$$E13

then the set ${\mathbf{W}}_{\mathrm{c}}$ is established for all reflectors.

The third step is feature extraction. The feature extraction algorithm of a reflector extracts its central position $\left({\mathrm{\gamma }}_{\mathrm{L},\mathrm{B}},{\mathrm{\beta }}_{\mathrm{L},\mathrm{B}}\right)$ in the laser range finder frame $\left\{{\mathrm{X}}_{\mathrm{L}},{\mathrm{Y}}_{\mathrm{L}},{\mathrm{Z}}_{\mathrm{L}}\right\}$, where ${\mathrm{\gamma }}_{\mathrm{L},\mathrm{B}}$ and ${\mathrm{\beta }}_{\mathrm{L},\mathrm{B}}$ are the distance and azimuth of each reflector, respectively. In the process of extracting the feature of the circular reflector, only a small portion of the entire circle was scanned with noise, so the accuracy of the fitting result would be low if a general least square circle fitting method is used. Since the radius of the reflector is known, the known radius is used as a constraint to improve the accuracy.

First, the value of ${\mathrm{\beta }}_{\mathrm{L},\mathrm{B}}$ is obtained from $\left(\mathrm{n}-\mathrm{m}\right)$ consecutive data points of the reflector set

$${}^{\mathrm{L}}{\mathrm{\beta }}_B=\frac{1}{\left(\mathrm{n}-\mathrm{m}\right)}\sum _{\mathrm{i}=\mathrm{m}}^{\mathrm{n}}{}^{\mathrm{L}}{\mathrm{\beta }}_i$$E14

As shown in Figure 14, ${\mathrm{M}}_{\mathrm{i}}$ represents the ith data and $\overline{{\mathrm{O}}_{\mathrm{L}2}\mathrm{B}}$ is a line between the reflector center B and the laser range finder center ${\mathrm{O}}_{\mathrm{L}2}$; the projected angle of the line $\overline{{\mathrm{O}}_{\mathrm{L}2}\mathrm{B}}$ in the laser range finder frame is ${}^{\mathrm{L}}{\mathrm{\beta }}_B$, while the angle between the line $\overline{{\mathrm{O}}_{\mathrm{L}2}\mathrm{B}}$ and the line $\overline{{\mathrm{O}}_{\mathrm{L}2}{\mathrm{M}}_{\mathrm{i}}}$ is ${\mathrm{\theta }}_{\mathrm{i}}$. The distance from ${\mathrm{M}}_{\mathrm{i}}$ to B is approximately the radius of the reflector, i.e., $\left|\overline{\mathrm{B}{\mathrm{M}}_{\mathrm{i}}}\right|=\mathrm{D}/2.$

$${\mathrm{\theta }}_{\mathrm{i}}={\varnothing }_{\mathrm{i}}-{}^{\mathrm{L}}{\mathrm{\beta }}_B$$E15

$${\left|\overline{{\mathrm{O}}_{\mathrm{L}2}\mathrm{B}}\right|}_{\mathrm{i}}=\left|\overline{{\mathrm{O}}_{\mathrm{L}2}{\mathrm{M}}_{\mathrm{i}}}\right|cos\left|{\mathrm{\theta }}_{\mathrm{i}}\right|+\left|\overline{\mathrm{B}{\mathrm{M}}_{\mathrm{i}}}\right|cos\left(arcsin\left(\left(2\bullet \left|\overline{{\mathrm{O}}_{\mathrm{L}2}{\mathrm{M}}_{\mathrm{i}}}\right|sin{\mathrm{\theta }}_{\mathrm{i}}\right)/\mathrm{D}\right)\right)$$E16

$${}^{\mathrm{L}}{\mathrm{\gamma }}_B=\frac{1}{\mathrm{n}-\mathrm{m}}\sum _{\mathrm{i}=\mathrm{m}}^{\mathrm{n}}{\left|\overline{{\mathrm{O}}_{\mathrm{L}2}\mathrm{B}}\right|}_{\mathrm{i}},\mathrm{i}=1,2,\cdots \mathrm{m}$$E17

Figure 14.

Extraction of the center of the cylindrical reflector.

Furthermore, as mentioned before, the position of the finder in the platform frame is $\left({{}_{}{}^{\mathrm{P}}\mathrm{x}}_{\mathrm{L}2},{{}_{}{}^{\mathrm{P}}\mathrm{y}}_{\mathrm{L}2}\right)$. The position of reflector center B $\left({{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{B}},{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}}\right)$ in the platform frame can be expressed as

$$\left\{\begin{array}{c}{}^{\mathrm{P}}{\mathrm{\gamma }}_B=\sqrt{{\left({{}_{}{}^{\mathrm{P}}\mathrm{x}}_{\mathrm{L}2}+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{B}}cos{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}}\right)}^2+{\left({{}_{}{}^{\mathrm{P}}\mathrm{y}}_{\mathrm{L}2}+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{B}}sin{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}}\right)}^2}\\ {}^{\mathrm{P}}{\mathrm{\beta }}_B=arctan({{}_{}{}^{\mathrm{P}}\mathrm{y}}_{\mathrm{L}2}-{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{B}}sin{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B})}/\left({{}_{}{}^{\mathrm{P}}\mathrm{x}}_{\mathrm{L}2}-{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{B}}cos{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}}\right)\end{array}\right.$$E18

The optimal triangulation localization algorithm based on angle measurement is carried out. A number of experiments were carried out on the reflector feature extraction algorithm before proposing a localization algorithm for the system. A single reflector was placed in the measuring range of the finder. The finder performed repeated scanning when it was placed at difference places. Two different feature extraction algorithms were tested to calculate the position of the reflector, and the results are shown in Figure 15. Algorithm A is the feature extraction algorithm proposed in this chapter, while algorithm B is the least square circle fitting method without a radius constraint. Apparently, the former one yields a better result.

Figure 15.

Effect diagram using different feature extraction algorithms.

After extracting the reflector center, the positions of all the reflectors $\mathrm{G}=\left\{{\left({{}_{}{}^{\mathrm{L}}\mathrm{\gamma }}_{\mathrm{B}},{}_{}{}^{\mathrm{L}}{\mathrm{\beta }}_{\mathrm{B}}\right)}_{\mathrm{i}}|\mathrm{i}=1,2,\dots ,\mathrm{n}\right\}$ can be obtained and used to calculate the distance measurement range $\mathrm{R\gamma }$ and the angler measurement range ${\mathrm{R}}_{\mathrm{\beta }}$ of the reflector, as given below:

$${\mathrm{R}}_{\mathrm{\gamma }}={\left({{}_{}{}^{\mathrm{L}}\mathrm{\gamma }}_{\mathrm{B}}\right)}_{\max }-{\left({{}_{}{}^{\mathrm{L}}\mathrm{\gamma }}_{\mathrm{B}}\right)}_{\min }$$E19

$${\mathrm{R}}_{\mathrm{\beta }}=(\left({\left({{}_{}{}^{\mathrm{L}}\mathrm{\beta }}_{\mathrm{B}}\right)}_{\max }-{\left({{}_{}{}^{\mathrm{L}}\mathrm{\beta }}_{\mathrm{B}}\right)}_{\min }\right)\mathrm{\pi }\bullet \frac{1}{\mathrm{n}}\sum _{\mathrm{i}=1}^{\mathrm{n}}{}_{}{}^{\mathrm{L}}{\mathrm{\gamma }}_{\mathrm{B}})/180$$E20

where ${\left({{}_{}{}^{\mathrm{L}}\mathrm{\gamma }}_{\mathrm{B}}\right)}_{\max }\in \mathrm{G}$, ${\left({{}_{}{}^{\mathrm{L}}\mathrm{\gamma }}_{\mathrm{B}}\right)}_{\min }\in \mathrm{G}$, ${\left({}_{}{}^{\mathrm{L}}{\mathrm{\beta }}_{\mathrm{B}}\right)}_{\max }\in \mathrm{G}$, ${\left({}_{}{}^{\mathrm{L}}{\mathrm{\beta }}_{\mathrm{B}}\right)}_{\min }\in \mathrm{G}$.

It can be seen from Figure 16 that the angle measurement accuracy is better than the distance measurement accuracy. Therefore, based on these results, this chapter proposes an optimal trilateral localization algorithm based on angle measurement. The idea is to use the azimuth angle ${\mathrm{\beta }}_{\mathrm{b}}$ of the reflector in the platform frame to find the optima ${\mathrm{\gamma }}_{\mathrm{b}}$ by the cosine theorem. The global pose of the mobile manipulator is then calculated based on the triangular method. The algorithm details are given as follows.

Figure 16.

The position of single reflector in the laser range finder frame.

First, it is assumed that the finder can cover at least three reflectors at each position. After feature extraction, the positions of three reflectors ${\mathrm{B}}_1$, ${\mathrm{B}}_{2,}$ and ${\mathrm{B}}_3$ are calculated as $\left({{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_1},{}_{}{}^{\mathrm{P}}{\mathrm{\beta }}_{{\mathrm{B}}_1}\right)$; $\left({{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{R},{\mathrm{B}}_2},{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{R},{\mathrm{B}}_2}\right)$; and $\left({{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{\mathrm{R},{\mathrm{B}}_3},{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{R},{\mathrm{B}}_3}\right)$, respectively. In the global frame, these positions can also be obtained as ${}_{}{}^{\mathrm{G}}{\mathrm{B}}_1\left({\mathrm{x}}_1,{\mathrm{y}}_1\right)$; ${}_{}{}^{\mathrm{G}}{\mathrm{B}}_2\left({\mathrm{x}}_2,{\mathrm{y}}_2\right)$; and ${}_{}{}^{\mathrm{G}}{\mathrm{B}}_3\left({\mathrm{x}}_3,{\mathrm{y}}_3\right)$. Since they are measured from the same finder, three circles must intersect at the same point ${}_{}{}^{\mathrm{G}}{\mathrm{O}}_{\mathrm{MP}}$, and the value of ${}_{}{}^{\mathrm{G}}{\mathrm{O}}_{\mathrm{MP}}\left({}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}},{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right),$ represents the position of the mobile platform, as shown in Figure 17.

Figure 17.

Schematic diagram of the localization algorithm.

According to the cosine theorem, the relations between the above variables can be expressed by the following equations:

$$\left\{\begin{array}{c}{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}^2+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}^2-2{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}{\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}cos\left({{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}1}-{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}2}\right)={\left({\mathrm{x}}_1-{\mathrm{x}}_2\right)}^2+{\left({\mathrm{y}}_1-{\mathrm{y}}_2\right)}^2\\ {{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}^2+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}^2-2{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}{\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}cos\left({{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{R}}-{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}3}\right)={\left({\mathrm{x}}_2-{\mathrm{x}}_3\right)}^2+{\left({\mathrm{y}}_2-{\mathrm{y}}_3\right)}^2\\ {{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}^2+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}^2-2{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}cos\left({{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}1}-{{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}3}\right)={\left({\mathrm{x}}_1-{\mathrm{x}}_3\right)}^2+{\left({\mathrm{y}}_1-{\mathrm{y}}_3\right)}^2\end{array}\right.$$E21

In Eq. (21), the known parameters ${{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}1}$, ${{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}2},$ and ${{}_{}{}^{\mathrm{P}}\mathrm{\beta }}_{\mathrm{B}3}$ are used to solve ${{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}$, ${{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}},$ and ${{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}$. Since the above equations are nonlinear, the steepest descent method is adopted. The three circle equations can be expressed as

$$\left\{\begin{array}{c}{\left({\mathrm{x}}_1-{}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}}\right)}^2+{\left({\mathrm{y}}_1-{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)}^2={{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}^2\\ {\left({\mathrm{x}}_2-{}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}}\right)}^2+{\left({\mathrm{y}}_2-{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)}^2={{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}^2\\ {\left({\mathrm{x}}_3-{}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}}\right)}^2+{\left({\mathrm{y}}_3-{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)}^2={{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{3\mathrm{a}}}^2\end{array}\right.$$E22

The position ${}_{}{}^{\mathrm{G}}{\mathrm{O}}_{\mathrm{MP}}\left({}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}},{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)$ of the system can be obtained in the global frame by solving the above equations. Since the actual position of the reflector in the global environment deviates from the theoretical position, these circles may not intersect at the same point. In order to minimize the difference between the calculated position of the system and the actual position, the least squares estimation principle is applied. Assuming that the coordinate of the reflector is ${}_{}{}^{\mathrm{G}}{\mathrm{B}}_{\mathrm{i}}\left({\mathrm{x}}_{\mathrm{i}},{\mathrm{y}}_{\mathrm{i}}\right)\left(\mathrm{i}=1,2,\dots ,\mathrm{n}\right)$, n is the number of reflectors detected by laser range finder. The position value ${}_{}{}^{\mathrm{G}}{\mathrm{O}}_{\mathrm{MP}}\left({}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}},{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)$ of the system is calculated as

$${\left({}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}},{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)}^{\mathrm{T}}={\left({\mathbf{A}}^{\mathrm{T}}\mathbf{A}\right)}^{-1}{\mathbf{A}}^{\mathrm{T}}\mathbf{b}$$E23

where

$$\mathbf{A}=\left[\begin{array}{ccc}2\left({\mathrm{x}}_1-{\mathrm{x}}_{\mathrm{n}}\right)& \cdots & 2\left({\mathrm{y}}_1-{\mathrm{y}}_{\mathrm{n}}\right)\\ ⋮& \cdots & ⋮\\ 2\left({\mathrm{x}}_{\mathrm{n}-1}-{\mathrm{x}}_{\mathrm{n}}\right)& \cdots & 2\left({\mathrm{y}}_{\mathrm{n}-1}-{\mathrm{y}}_{\mathrm{n}}\right)\end{array}\right]$$E24

$$\mathbf{b}=\left[\begin{array}{c}{\mathrm{x}}_1^2-{\mathrm{x}}_{\mathrm{n}}^2+{\mathrm{y}}_1^2-{\mathrm{y}}_{\mathrm{n}}^2+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}^2-{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}^2\\ ⋮\\ {\mathrm{x}}_{\mathrm{n}-1}^2-{\mathrm{x}}_{\mathrm{n}}^2+{\mathrm{y}}_{\mathrm{n}-1}^2-{\mathrm{y}}_{\mathrm{n}}^2+{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{2\mathrm{a}}}^2-{{}_{}{}^{\mathrm{P}}\mathrm{\gamma }}_{{\mathrm{B}}_{1\mathrm{a}}}^2\end{array}\right]$$E25

The posture of the system also includes an azimuth angle ${}_{}{}^{\mathrm{G}}\mathrm{\theta }$ in the global frame. First, ${}_{}{}^{\mathrm{G}}{\mathrm{\theta }}_{\mathrm{i}}$ is obtained from the $\mathrm{ith}$ reflector as

$${}_{}{}^{\mathrm{G}}{\mathrm{\theta }}_{\mathrm{i}}=arctan\left(\left({\mathrm{y}}_{\mathrm{i}}-{}_{}{}^{\mathrm{G}}{\mathrm{y}}_{\mathrm{MP}}\right)/\left({\mathrm{x}}_{\mathrm{i}}-{}_{}{}^{\mathrm{G}}{\mathrm{x}}_{\mathrm{MP}}\right)\right)-{}_{}{}^{\mathrm{P}}{\mathrm{\beta }}_{{\mathrm{B}}_{\mathrm{i}}}$$E26

The azimuth angle ${}_{}{}^{\mathrm{G}}\mathrm{\theta }$ of the system is the averaged value from all the reflectors, as in

$${}_{}{}^{\mathrm{G}}\mathrm{\theta }=\frac{1}{\mathrm{n}}\sum _{\mathrm{i}=1}^{\mathrm{n}}{}_{}{}^{\mathrm{G}}{\mathrm{\theta }}_{\mathrm{i}}$$E27

The dynamic tracking algorithm is then carried out. Localization of the system is based on landmarks. The system needs to meet the following two conditions to complete real-time localization:

1. The system requires real-time localization of the reflector in the environment.

2. The localization system correctly matches the real-time observed reflectors.

During the matching process, the reflectors observed in real time are made to correspond to all reflectors in the last moment in the environment one by one to extract the effective reflectors [20]. The localization can be achieved.

During localization, owing to the fact that some of the reflectors are obscured by obstacles or confused with a highly reflective metal surface object, the loss of position of the system is observed. To address the above problems, a dynamic tracking algorithm is proposed as shown in Figure 18.

Figure 18.

Schematic diagram of the dynamic tracking algorithm.

After placing n reflectors in the global environment, the system actually observes q reflectors at the tth moment, and the coordinate value of the ith reflector in the platform frame is ${\mathbf{M}}_{\mathrm{t},\mathrm{i}}\left({\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}},{\mathrm{\beta }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}}\right)\left(0\ll \mathrm{i}\ll \mathrm{q}\right)$. The sampling time of the laser range finder is 0.1 s. Therefore, the theoretical position value ${\mathbf{M}}_{\mathrm{t}-1,\mathrm{j}-1}\left({\mathrm{\gamma }}_{\mathrm{t}-1,{\mathrm{B}}_j},{\mathrm{\beta }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}\right)$ of the $\mathrm{jth}$ reflector in the platform frame can be deduced by the position ${\mathrm{O}}_{\mathrm{t}-1,\mathrm{R}}\left({\mathrm{x}}_{\mathrm{t}-1},{\mathrm{y}}_{\mathrm{t}-1}\right)$ of the system at the $\left(\mathrm{t}-1\right)$th moment and the position ${{}_{}{}^{\mathrm{G}}\mathrm{O}}_{\mathrm{j}}\left({\mathrm{x}}_{\mathrm{j}},{\mathrm{y}}_{\mathrm{j}}\right)$ of the $\mathrm{jth}$ reflector in the global environment.

If the difference between the theoretical value ${\mathbf{M}}_{\mathrm{t}-1,\mathrm{j}}\left({\mathrm{\gamma }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}},{\mathrm{\beta }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}\right)$ and the observed value ${\mathbf{M}}_{\mathrm{t},\mathrm{i}}\left({\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}},{\mathrm{\beta }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}}\right)$ is less than η, then the $\mathrm{ith}$ observed reflector is an effective reflector and is considered matched with the reference reflector ${\mathrm{B}}_{\mathrm{j}}$ as follows:

$$\left|\sqrt{{\left({\mathrm{r}}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}cos{\mathrm{\beta }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}-{\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{j}}}cos{\mathrm{\beta }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}}\right)}^2+{\left({\mathrm{\gamma }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}sin{\mathrm{\beta }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}-{\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}}sin{\mathrm{\beta }}_{\mathrm{t}-1,{\mathrm{B}}_{\mathrm{j}}}\right)}^2}\right|\le \mathrm{\eta }$$E28

Therefore, a set A of effective reflectors at the tth moment can be obtained, and $\mathbf{A}=\left\{{\mathbf{M}}_{\mathrm{t},\mathrm{o}}\left({\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{o}}}{\mathrm{\beta }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{o}}}\right),\dots ,{\mathbf{M}}_{\mathrm{t},\mathrm{i}}\left({\mathrm{\gamma }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{o}}},{\mathrm{\beta }}_{\mathrm{t},{\mathrm{B}}_{\mathrm{i}}}\right)|\mathrm{i}=0,1,\dots ,\mathrm{q}\right\}$. Taking the mobile manipulator’s moving speed into account, the value of η depends on the actual situation. Through the use of the optimal triangulation localization algorithm based on angle measurement, the pose ${\mathrm{O}}_{\mathrm{t},\mathrm{R}}\left({\mathrm{x}}_{\mathrm{t}},{\mathrm{y}}_{\mathrm{t}}\right)$ of the mobile manipulator can be calculated at the tth moment.

4.3. Experimental verification

The experimental data are obtained by the LMS 100 laser range finder with a scanning range of 270° and an angular resolution of 0.25°. The experimental platform is shown in Figure 19. The outside of the reflector is wrapped by reflective tape. In the experimental environment, five reflectors are placed around the system. Their global coordinate values are (0,995); (0, 0); (0, 1774); (2905, −2449); and (3956, −2032), and the unit is mm, as shown in Figure 20.

Figure 19.

Experimental platform.

Figure 20.

Experimental environment.

The optimal triangulation method based on angle measurement is used for validation by the repeatability of the system. In the stationary state of the system, the environment is scanned by finders. Each result is indicated by a red dot in Figure 21. The repeatability obtained by the trilateral method is nearly 18 mm, while the repeatability of the optimal method is only 9 mm. It can be shown that the optimal method is better than the traditional method.

Figure 21.

Repeatability localization of the system at the same location.

The mobile manipulator moves in the direction of the arrow in Figure 19, and each time the system moves a certain distance, the localization system will perform an experiment, i.e., it will use the left rear finder to calculate the current position. An average of 30 samples is taken for each experiment.

Figure 22 shows the results of static localization accuracy. The maximum distance error is 18 mm and the maximum angle error is 2°, which satisfies the localization requirement of the system.

Figure 22.

System localization error.

The mobile manipulator moves in the designated route, and it needs to constantly calculate and record its own position in the moving process. As shown in Figure 23, the trajectory of the moving system based on the localization method is smoother.

Figure 23.

Tracking results.

This chapter demonstrates the feasibility of a tracking and localization algorithm for mobile manipulators. The following conclusions can be drawn from this study: (i) In the detection of a reflector in the laser range finder frame, the angle repeatability of the reflector is better than that of the distance repeatability based on the feature extraction algorithm; (ii) The repeatability localization accuracy using the optimal triangulation method based on the angle measurement is nearly 9 mm, which is better than that of the trilateral method; (iii) The localization error of the system is 18 mm, which satisfies the localization requirement of system. Improvements in the location method based on reflectors, such as optimizing the layout of reflectors and the map of reflectors selection strategy for localization, are still needed.

5. Summary

In this chapter, through analyzing the roller deformation of the Mecanum wheel, the changed parameters of the motion equation of mobile system are found. The relative variation of the parameters in the motion equation of the Mecanum motion platform is solved by Monte Carlo analysis and interval analysis. Using the relative variation of the parameters to revise the motion equation, the displacement errors in different spaces in theory are solved for and compared with the measured displacement errors. From the comparison, both the methods are found to satisfy the system’s requirement. Then, the feasibility of a tracking and locating algorithm for mobile manipulator is demonstrated. The following conclusions can be drawn from this study: (i) In the detection of a reflector in the laser range finder frame, the angle repeatability of the reflector is better than that of the distance repeatability based on the feature extraction algorithm; (ii) The repeatability localization accuracy using the optimal triangulation method based on the angle measurement is nearly 9 mm, which is better than that of the trilateral method; (iii) The localization error of the system is 18 mm, which satisfies the localization requirement of system. Improvements in the localization method based on reflectors, such as optimizing the layout of reflectors and the map of reflectors selection strategy for localization, are still needed.

The method in this chapter is also used in the research of MoMaCo (Mobile manipulator in Construction), which can draw baseline for architectural decoration engineering as shown in Figure 24. The application result also verified the effectiveness of the method.

Figure 24.

MoMaCo (mobile manipulator in construction).