Online Sensor Information and Redundancy Resolution Based Obstacle Avoidance for High DOF Mobile Manipulator Teleoperation

Abstract

Abstract High degrees of freedom (DOF) mobile manipulators provide more flexibility than conventional manipulators. They also provide manipulation operations with a mobility capacity and have potential in many applications. However, due to high redundancy, planning and control become more complicated and difficult, especially when obstacles occur. Most existing obstacle avoidance methods are based on off-line algorithms and most of them mainly focus on planning a new collision-free path, which is not appropriate for some applications, such as teleoperation and uses many system resources as well. Therefore, this paper presents an online planning and control method for obstacle avoidance in mobile manipulators using online sensor information and redundancy resolution. An obstacle contour reconstruction approach employing a mobile manipulator equipped with an active laser scanner system is also introduced in this paper. This method is implemented using a mobile manipulator with a seven-DOF manipulator and a four-wheel drive mobile base. The experimental results demonstrate the effectiveness of this method.

1. Introduction

High DOF mobile manipulators are widely used in many fields including industry, the military and in space [1, 2]. They consist of a high DOF manipulator and a mobile base. The mobile base greatly expands the workspace of the manipulator and the high redundancy also provides more flexibility. Although a wider workspace and flexibility are desired features, this combined structure and high redundancy also makes some classical control problems become more complicated. Obstacle avoidance is one of these classical problems in robotics.

The teleoperator always wants to focus on the main task and not to be distracted by other events while operating the mobile manipulator. For instance, if the task is to move the mobile manipulator to a target position, the best method for the teleoperator is if they only need to control the direction of the movement to arrive at the designated position without worrying about any risk of collision by the end-effector or other critical points of the mobile manipulator. Therefore, automatic obstacle avoidance ability is very important to improve teleoperation performance.

Obstacle avoidance for mobile manipulator teleoperation has been researched for decades. The process can be generally divided into two steps. The first step is obstacle detection; the mobile manipulator needs to know the obstacle position and geometry size to plan the motion to avoid collision. At an earlier stage, ultrasonic sensors were used to detect the obstacle distance [3], but they cannot be used to describe the 3D information of the obstacle. Nowadays, both laser scanners and stereovision have been widely used to accomplish detection tasks [4–6] and both of the methods have their own pros and cons. The vision system is lightweight and relatively power-efficient, while lasers provide much more precise details of environmental information. The second step is the obstacle avoidance scheme. Obstacle avoidance has been viewed as a path-planning problem or simply as a reflex action. Planning a path, which is free of any risk of collision, is still the major problem in this field. Kuwata [7] presented the design of an efficient and reliable motion planning system based on rapidly-exploring random trees for urban vehicle driving. Jaillet and Simeon [8] proposed a method to build compact roadmaps, which are representative of the different varieties of free paths. These alternative paths are switched when one path becomes impossible due to environmental changes. Yoshida [9] proposed an online replanning and execution method for reactive collision avoidance in a changing environment. These methods [7–9] can deal with the sudden appearance or displacement of obstacles. However, planning a new path that is free of collisions in a complex environment increases the system operation load and may not be finished in the control period. Therefore, it is generally difficult for these methods to handle continuously moving obstacles due to their huge computational cost. Diankov and Kuffner [10] proposed a path-planning algorithm that can return a semi-optimal path with respect to several distance metrics that encode all the domain information. Burns and Brock [11] developed an approach of multi-query motion planning, which uses information from its previous experience to guide sampling to more relevant configurations. Toussaint [12] presented an approach to use approximate inference methods for robot trajectory optimization and stochastic optimal control problems. These algorithms [10–12] reduce the computational costs of searches for a possible path, but they cannot provide the capacity to avoid an obstacle that appears suddenly.

However, for a high redundant mobile manipulator, besides the above-mentioned two steps, another main issue of teleoperation is the redundancy resolution problem. In past decades, many redundancy resolution methods were proposed, including the projected gradient method [13], the extended Jacobian method [14] and the operational space extension method [15]. Recently, some research has focused on the motion planning algorithm to solve more additional tasks. Lin [16] proposed an alternative approach using a forward kinematics and optimization technique to solve the motion planning problem with task priority requirements such as, singularity avoidance and obstacle avoidance. White and Bhatt [17] solved the end-effector trajectory-tracking problem including obstacles avoidance. Liu [18] developed a self-motion control method for redundant nonholonomic mobile manipulators to execute multiple subtasks. Kumar et al. [19] designed neural network based nonlinear tracking controls for kinematically redundant manipulators. However, all the above-mentioned redundancy resolution methods generally focused on an off-line algorithm and did not consider external disturbance. They are more like off-line path planning methods, rather than online trajectory generation and most of them are single objective and used for manipulators or low DOF mobile manipulators. In our previous work [20, 21], we developed some methods for obstacle avoidance, but they only considered the elbow of the manipulator and ignored the movement of the mobile base. In [22], we proposed a method, which is online sensor based and has the ability to handle multiple classical problems such as singularity avoidance, joint limits and maximum manipulability simultaneously. This method also can be used to improve teleoperation performance while solving the obstacle avoidance problem.

In this paper, we propose a new obstacle avoidance method for a mobile manipulator with a seven-DOF manipulator and a four-wheel drive mobile base. This method employs real-time environment information gathered by an online sensor system to detect and extract the 3D contours of an obstacle. According to these 3D contours and the position of the critical points of the manipulator, the control system changes the manipulator configuration automatically to adapt to the environment to avoid collision. Our approach is an online reflex action planning method for obstacle avoidance. Compared with the aforementioned methods, this method is much simpler and computationally efficient, both in implementation and system operation load. It can be applied in any complex and dynamic environment. The effectiveness of this method is also demonstrated by experimental results.

The contributions of this paper are: (1) we provide a simple method to detect the obstacle using real-time sensor information and plan the motion using redundancy resolution and (2) this is an online planning and control method rather than an off-line algorithm.

This paper is organized as follows. Section 2 presents the system structure and the kinematic model of a mobile manipulator with a seven-DOF manipulator and a four-wheel drive nonholonomic mobile base. Section 3 presents the obstacle detection and extraction method. Section 4 illustrates the obstacle avoidance scheme for the mobile manipulator. In Section 5, the experiment results will be given. Finally, the conclusion will be shown in Section 6.

2. System Structure and Modelling

2.1 System Structure

The mobile manipulator with a seven-DOF manipulator and a four-wheel drive mobile base is shown in Fig. 1. We can see that every joint of this manipulator is a revolute joint and any two adjacent joints are perpendicular to each other. A gripper is mounted on the end of the seventh link. The structure of the online sensor based control system is shown in Fig. 2. The system is composed of four parts: a four-wheel drive mobile manipulator, an online sensor system and teleoperator and communication systems. The control procedure is described as follows. The four-wheel drive mobile manipulator receives the six dimensional position and orientation commands of the end-effector from the teleoperator through the Internet, which is the communication system. Then the online sensor system gathers the environmental information and sends it to the mobile manipulator at the same time. Then, with the information and the commands, the mobile manipulator controller finds out the best motion path and makes the mobile manipulator respond to implement the task. The feedback data is important for the online sensor-based mobile manipulator to have the ability to perform the redundancy resolution autonomously, which can reduce the burden of the teleoperator effectively.

Figure 1.

Mobile manipulator structure

Figure 2.

Control system structure

Several pieces of information are needed in order to control this mobile manipulator, such as the velocity information of the mobile base, the joint angle information of the manipulator and the 3D information of the environment. Therefore, an acceleration sensor and gyroscope are used to provide the velocity and orientation information and a joint angle transducer is used to offer the joint angle information and a laser scanner with pan-tilt is used as a 3D environment information measurement system. In order to get the correct integrated information from the online sensor system, the coordinate frame of the laser measurement system needs to be calibrated with the coordinate frame of the mobile manipulator [20].

2.2 System Modelling and Redundancy Resolution

The coordinate frame of every link is established according to the classical Denavit-Hartenberg model [23] in order to build the forward kinematic model.

Let the configuration vector of the seven-DOF manipulator be q =[q₁,q₂,q₃,q₄,q₅,q₆,q₇]^T, where q_i(i = 1,2…7) is the ith joint angle of the manipulator. Define the configuration vector of the nonholonomic mobile base as q_b = [x,y,β]^T, where x,y is the position and β is the orientation of the base. Let the configuration vector be q = [q_b,q_a]^T and let the task vector be ξ = [x_e,y_e,z_e,ϕ_e,θ_e,ϕ_e]^T, where x_e,y_e,z_e is the position of the end-effector and ϕ_e,θ_e,ϕ_e is the orientation defined as Euler Angles.

For this mobile manipulator, the transformation matrix of the mobile base coordinate frame is given as:

$\mathrm{B}=\left[\begin{array}{cccc}\mathrm{C}\beta & -\mathrm{S}\beta & 0& \mathrm{x}\\ \mathrm{S}\beta & \mathrm{C}\beta & 0& \mathrm{y}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

(1)

The transformation matrix A of the coordinate frame from the base to the end-effector is given in [21]. The forward kinematics of this mobile manipulator can be obtained with these two matrixes.

The task can be described as

$\xi =\mathrm{f}(\mathrm{q})=\mathrm{f}({\mathrm{q}}_{\mathrm{b}},{\mathrm{q}}_{\mathrm{a}})$

(2)

With the constraint of the rolling without slipping condition, the velocity input of this nonholonomic mobile base can be set as η_b = [v,ω] ^T , where v is the linear velocity and ω is the angular velocity. Therefore, the kinematic model of the mobile base is given as

${\stackrel{.}{q}}_{\mathrm{b}}=\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\beta }\end{array}\right]=\left[\begin{array}{cc}\cos \beta & 0\\ \sin \beta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\mathrm{v}& \omega \end{array}\right]=\mathrm{G}({\mathrm{q}}_{\mathrm{b}}){\eta }_{\mathrm{b}}$

(3)

Since the manipulator is a completely unconstrained system, the velocity input can be set as

${\stackrel{.}{q}}_{\mathrm{a}}={\eta }_{\mathrm{a}}$

(4)

Then the velocity input for the whole nonholonomic mobile manipulator is given as

$\eta ={\left[{\eta }_{\mathrm{b}},{\eta }_{\mathrm{a}}\right]}^{\mathrm{T}}$

(5)

Hence, the velocity kinematic for the nonholonomic mobile manipulator with respect to time can be calculated by

$\stackrel{.}{\xi }={\mathrm{J}}_{\mathrm{n}}(\mathrm{q})\eta$

(6)

where the 6×9 matrix J_n is the Jacobian of the nonholonomic mobile manipulator.

Since η ∈ ℜ⁹ and ξ ∈ ℜ⁶, the whole system is redundant. For given task $\stackrel{.}{\xi }$ , all solutions η to the velocity kinematics in Eq. (6) can be expressed as

$\eta ={\mathrm{J}}_{\mathrm{n}}^{†}(\mathrm{q})\stackrel{.}{\xi }+(\mathrm{I}-{\mathrm{J}}_{\mathrm{n}}^{†}(\mathrm{q}){\mathrm{J}}_{\mathrm{n}}(\mathrm{q})){\eta }_{\mathrm{s}}$

(7)

where the J_n⁺=J_n^T(J_nJ_n^T)⁻¹ is the right pseudo inverse of matrix J_n, I-J_n⁺J_n is the null-space of J_n and η_s ∈ ℜ⁹ is an arbitrary vector.

The design of the velocity kinematic controller is based on the velocity kinematic model of the nonholonomic mobile manipulator in Eq. (7). The structure of the control law is shown in Fig. 3. The control law is a design based on the principle that the end-effector should reach the desired position and orientation within the minimum possible time.

Figure 3.

Control Law Structure

${\eta }_{\mathrm{d}}={\mathrm{J}}_{\mathrm{n}}^{†}(\mathrm{S}(\mathrm{t}))({\stackrel{.}{\xi }}_{\mathrm{d}}-\mathrm{K}{\xi }_{err})+(\mathrm{I}-{\mathrm{J}}_{\mathrm{n}}^{†}(\mathrm{S}(\mathrm{t})){\mathrm{J}}_{\mathrm{n}}(\mathrm{S}(\mathrm{t})))\cdot \alpha {\eta }_{\mathrm{s}}$

(8)

where ${\stackrel{.}{\xi }}_{\mathrm{d}}={[{\stackrel{.}{x}}_{ed},{\stackrel{.}{y}}_{ed},{\stackrel{.}{z}}_{ed},{\stackrel{.}{\varphi }}_{ed},{\stackrel{.}{\theta }}_{ed},{\stackrel{.}{\phi }}_{ed}]}^{\mathrm{T}}$ is the desired velocity vector of the end-effector, ξ_err = ξ_d – ξ = [x_err,y_err,z_err,ϕ_err,β_err,ϕ_err]^T is the vector of error from the end-effector's current position and orientation to the desired target, S(t) is the online sensor information, K and a are suitable constant gain factors and η_s is an arbitrary vector, which is designed to control the secondary task. Any value given to η_s will have effects on the internal structure of the manipulator only and will not affect the final control of the end-effector at all. By using this controller we can achieve secondary tasks, such as joint limit avoidance, singularity removing, maximum manipulability and obstacle avoidance.

3. Obstacle Detection and Extraction

There are several methods to detect an obstacle, in this paper; we use a laser scanner to obtain the 3D information of the environment.

The laser scanner is mounted on a pan-tilt system in front of the mobile base, so the scanner can detect different 3D information of the environment with the rotation of the pan-tilt. In our approach, we just use the tilt function (the laser scanning area is shown in Fig. 4). The control system reads the data from the laser scanner every tilt-step angle, which is defined as θ_ts. The entire tilt angle is defined as

Figure 4.

Laser scanning area

${\theta }_{tilt}=\mathrm{N}\times {\theta }_{ts}$

(9)

where N is the number of tilt-steps.

The distance from O_s to the detected object can be defined as d_i. If d_i s smaller than a cut-off distance, which is defined as 1, the object should be seen as obstacle.

Before using the obstacle information, the coordinate frame of the laser system needs to be calibrated with the coordinate frame of the mobile manipulator, which is shown in Fig. 5. The calibration can transfer the point coordinates (x_s,y_s,z_s), which relate the laser system frame to the coordinates (x_m,y_m,z_m) in the manipulator frame.

Figure 5.

Coordinate frame

${\left({\mathrm{x}}_{\mathrm{m}},{\mathrm{y}}_{\mathrm{m}},{\mathrm{z}}_{\mathrm{m}}\right)}^{\mathrm{T}}={\mathrm{R}}_{\mathrm{s}}^{\mathrm{m}}{\left({\mathrm{x}}_{\mathrm{s}},{\mathrm{y}}_{\mathrm{s}},{\mathrm{z}}_{\mathrm{s}}\right)}^{\mathrm{T}}+{\mathrm{O}}_{\mathrm{s}}^{\mathrm{m}}$

(10)

where R_s^m is the transform matrix from the laser system coordinates to the manipulator coordinates and O_s^m is the coordinate of the origin of the laser system frame in the manipulator coordinate frame.

Sometimes, the manipulator will block the laser beam and the system will make a mistake in defining the manipulator as an obstacle. In this case, we can check the joint position of the manipulator according to the forward kinematic model. If the obstacle point is too close to the joint position, it must be the manipulator itself. So we can delete this kind of point from the obstacle information and then interpolate some points with linear fit according to the rest of the obstacle information.

Asshown in Fig. 5, a rectangular plane, which parallels the Y_mZ_m plane of the manipulator coordinate frame, is used in order to extract the contours of the obstacle at the front of the mobile manipulator. All the points, which can be projected to this plane are likely to block the movement of the mobile manipulator, otherwise these points are not a threat to the mobile manipulator. This 1m by 2m rectangle is divided into a 20 × 40 grid; every unit of this grid is a 50mm × 50mm square. The projection of the obstacle points is also divided into different units. In each unit, we only select the nearest point, which has the smallest x_m value in the manipulator coordinate frame, as the most threatening obstacle point.

After the process mentioned above, a 20 × 40 matrix P is obtained to describe the contours of the obstacle, in which every element is defined as

${\mathrm{p}}_{\mathrm{i},\mathrm{j}}=\{\begin{array}{cc}\min ({\mathrm{x}}_{\mathrm{m}}(\mathrm{i},\mathrm{j}))& \exists {\mathrm{x}}_{\mathrm{m}}(\mathrm{i},\mathrm{j})\\ 0& else\end{array}$

(11)

where i = 1,2…20, j = 1,2…40 and x_m (i,j) is the x_m value of the obstacle point in corresponding (i,j) unit.

4. Obstacle Avoidance Scheme

Obstacle avoidance for mobile manipulators can be divided into three classes: (1) obstacle avoidance for the manipulator, (2) obstacle avoidance for the mobile base and (3) obstacle avoidance for the whole mobile manipulator. In our previous work [22], we discussed obstacle avoidance for the mobile base, while in this paper we just focus on the obstacle avoidance for the manipulator. Our goal is to try to make the manipulator change its configuration automatically to avoid the obstacle while the teleoperator is controlling the mobile manipulator.

As shown in Fig. 6, the seven-DOF manipulator has three critical points for obstacle avoidance: the shoulder, the elbow and the end-effector. The shoulder is a fixed point in the manipulator coordinate frame, the elbow and the end-effector are mobile points. The end-effector should be connected to the shoulder with a straight line and the same for between the elbow and the shoulder and the elbow and the end-effector. Suppose the mobile base moves straight ahead, the manipulator will be safe if these three lines are not blocked by any obstacles. In other words, the manipulator is safe when the projection of these three lines on the rectangular plane is not interfered with by any projection of an obstacle point.

Figure 6.

Projection of critical points of manipulator

As shown in Fig. 7, the three lines are projected to the rectangular plane, which is mentioned in the last section. Point P_s is the projection of the shoulder, point P_eb is the projection of the elbow and point P_ee is the projection of the end-effector. Three projection line segments P_sP_ee, P_sP_eb and P_ebP_ee respectively have their own forbidden area, denoted as F₁, F₂ and F₃. The forbidden areas consist of the units where the line segment lies and the neighbours of these units. The projection of an obstacle point is not allowed to exist in these forbidden areas. Matrix L is defined to describe the forbidden area in the rectangular plane. The element of matrix L is defined as

Figure 7.

Interference detection

${\mathrm{l}}_{\mathrm{i},\mathrm{j}}=\{\begin{array}{cc}1& (\mathrm{i},\mathrm{j})\in \mathrm{F}\\ 0& (\mathrm{i},\mathrm{j})\notin \mathrm{F}\end{array}$

(12)

where i = 1,2…20, j = 1,2…40 and F = F₁ ∪ F₂ ∪ F₃.

From the two matrixes P and L, a matrix E, which is used to describe the interference area, can be obtained. The element is defined as

${\mathrm{e}}_{\mathrm{i},\mathrm{j}}={\mathrm{p}}_{\mathrm{i},\mathrm{j}}\times {\mathrm{l}}_{\mathrm{i},\mathrm{j}}$

(13)

If E ≠ 0, this means interference occurred and the manipulator is not safe if the mobile manipulator keeps going forward without making any changes and the collision will occur in the corresponding (i,j) unit where e_i,j ≠ 0. In this case we should check these three forbidden areas F₁, F₂ and F₃ separately.

Considering importance and flexibility, we check F₁ first and then F₂, followed by F₃. If interference occurs in F₁, the control system needs to go over every unit in the working area of the end-effector to find the safe unit that is closest to the current position of the end-effector P_ee and then make the end-effector move to this unit directly. The checking procedure of F₂ is almost the same; the only difference is the elbow movement, which is described in detail in [21]. If the interference only occurs in F₃, the easiest way to avoid the obstacle is to move the end-effector to make the two lines P_sP_ee and P_sP_eb coincide.

5. Experimental Results

To verify this obstacle avoidance method, several experiments are conducted on a nonholonomic mobile manipulator. To perform the experiment, a seven-DOF manipulator named LWA3, which is produced by Schunk Company, is mounted on a Segway nonholonomic mobile base, as shown in Fig. 1 and a laser scanner system named LMS111, which is produced by SICK, is used as the 3D environment information detection system with a pan-tilt.

In this experiment, the mobile manipulator needs to change the configuration of the manipulator automatically in order to pass through a bridge structure. First of all, we verify that the control system is still able to obtain the correct environment information and extract the contours of the obstacle even if the manipulator blocks the laser beam. The initial configuration vector of the manipulator is set as

${\mathrm{q}}_{\mathrm{a}}(0)={[0,0,0,\pi /2,0,-\pi /4,0]}^{\mathrm{T}}$

(14)

The posture of the manipulator and the bridge structure are shown in Fig. 8. So the end-effector will block part of the laser beams when the laser system scans upper area. Fig. 9 shows the raw data obtained by the laser system. It can be seen that there are a lot of obstacle points from the manipulator itself because the manipulator blocks the laser beam.

Figure 8.

Blocking laser beam posture

Figure 9.

Raw data of laser system

Fig. 10 shows the revised data for the environment. As indicated by arrows, some points are interpolated based on the linear fitting of the remaining environment data. The results show it reconstructed the original look of the bridge structure. Fig. 11 shows the extraction of the contours of the bridge structure.

Figure 10.

Modified data of laser system

Figure 11.

Contour extraction of bridge structure

Secondly, we need to verify that the manipulator can change its configuration to avoid the obstacle and go through the bridge structure. The initial configuration vector of the manipulator is set as

${\mathrm{q}}_{\mathrm{a}}(0)={[0,-\pi /4,0,\pi /2,0,0,0]}^{\mathrm{T}}$

(15)

The posture of the manipulator is shown in Fig. 12. The end-effector and the elbow are higher than the top of the bridge structure. The manipulator will collide with the bridge structure if it keeps going forward. The obstacle avoidance scheme mentioned in Section IV is used to adjust the configuration of the manipulator. Fig. 13 shows the position of the end-effector and elbow. It is clear that the end-effector moves down to a safe position first and then the elbow is turned to avoid the obstacle. Fig. 14 shows the 3D trajectory of the end-effector and elbow. The smallest distance from the obstacle to the end-effector and the elbow is shown in Fig. 15. We can see that the manipulator passes through the bridge structure safely.

Figure 12.

Posture of manipulator

Figure 13.

Position of end-effector and elbow

Figure 14.

3D trajectory of end-effector and elbow

Figure 15.

Smallest distance from obstacle to end-effector and elbow

In summary, this experiment illustrates that the designed obstacle detection and avoidance method achieves the purpose of adjusting the configuration of the manipulator automatically to adapt to the environment to avoid collision.

6. Conclusion

This paper investigated the obstacle avoidance method for a mobile manipulator with a seven-DOF manipulator and a four-wheel drive mobile base. This method employs real-time environment information gathered by an online sensor system to detect and extract the 3D contours of the obstacle. According to these 3D contours and the position of the critical points of the manipulator, the control system changes the manipulator configuration automatically to adapt to the environment to avoid collision. This method is easy to implement and reduces the computational cost for the control system. The effectiveness of this method and the real-time performance is also demonstrated by experimental results. This method also can be applied further in unknown environments for obstacle avoidance and guidance control tasks.

7. Acknowledgment

This paper is supported by the major innovation project of the Ministry of Education Training Funds Project (NO.708045).