A Global Obstacle-Avoidance Map for Anthropomorphic Arms

Abstract

More and more humanoid robots are used in human society, and they face a wide variety of complicated manipulation tasks, which are mainly to be achieved by their anthropomorphic arms. Obstacle avoidance for the anthropomorphic arm must be a fundamental consideration to guarantee the successful implementation of these tasks. Different from traditional methods searching for feasible or optimal collision-free solutions for the anthropomorphic arm, a global obstacle-avoidance map for the whole arm is proposed to indicate the complete set of feasible solutions. In this map, the motion of the arm can be appropriately planned to intuitively control the configuration of the arm in motion. First, the cubic spline function is adopted to interpolate some well-chosen path points to generate a smooth collision-free path for the wrist of the anthropomorphic arm. Second, based on the path function of the wrist, the time and the self-rotation angle of the arm about the “shoulder-wrist” axis are used to parameterize all possible configurations of the arm so that a global two-dimensional map considering the obstacle avoidance can be established. Subsequently, a collision-free self-rotation angle profile of the arm can be well planned. Finally, the joint trajectories of a specific anthropomorphic arm, which correspond to the planned path of the wrist and self-rotation angle profile of the arm, can be solved on the basis of the general kinematic analysis of the anthropomorphic arm, and the specific structure. Several simulations are conducted to verify that the proposed collision-free motion planning method for anthropomorphic arms has some advantages and can be regarded as a convenient and intuitive tool to control the configuration of the anthropomorphic arm in motion, without collision with obstacles in its surroundings.

1. Introduction

As many researchers and scholars have pointed out, robots are gradually being involved in our society, getting closer to our lives, interacting with us and helping us achieve a variety of chores in our daily lives. Due to the similarity in appearance with humans, humanoid robots are favoured in many household, medical and healthcare applications, and play an important role in the service robot family. As for the service function of humanoid robots, they must have two fundamental capacities: locomotion and manipulation. Locomotion is usually implemented by the robots' legs, and manipulation tasks are generally done by their arms, which are anthropomorphic. When it comes to the manipulation and motion planning problems of anthropomorphic arms, an important issue is obstacle avoidance, which is the research topic of this paper.

The existing motion planning methods considering obstacle avoidance for redundant manipulators can be divided into three major categories: path planning in configuration space (C-space), artificial potential field, and gradient projection method (GPM). C-space path planning is a generic and global planning method for various robots, which is employed widely. It comprises C-space obstacles which are characterized by the collection of configurations which yield collisions with obstacles. Path planning in C-space thus usually involves the construction of C-space obstacles for a specific manipulator and searching for a collision-free path between the start and goal configurations in the global C-space model. Due to the high-dimensional property of the configuration space of redundant manipulators, the most effective kind of technique is sampling-based C-space planning, two typical examples of which are the PRM [1, 2] and the RRT [3, 4] techniques. However, because the specific kinematic model of a redundant manipulator is not considered in searching for a collision-free path in C-space, the resulting motion process of the whole arm cannot be predicted in advance. In addition, the computation of this method is time-consuming due to the high-dimensional configuration space. The artificial potential field approach does not construct a global C-space model, but rather a differentiable real-valued function, termed a potential function of joint variables, which guides the motion of the manipulator. The potential includes two components: an attractive component which pulls the manipulator towards the goal state, and a repulsive component which pushes the manipulator away from the obstacles. The gradient of the resultant potential function points in the direction that locally maximally increases the potential, which can be used to compute the collision-free path from the start point to the goal point by applying the corresponding gradient descent [5, 6]. However, this gradient-descent approach does not guarantee a solution to the problem. It is also prone to reaching a local minimum of the potential to result in failing to reach the goal state [7]. Another local optimization method, gradient projection method, is based on the Moore-Penrose pseudo-inverse of the Jacobian of redundant manipulator, and its solution can be divided into minimum norm solution and null-space solution at the velocity level. The former can force the end-effector of the manipulator to follow the desired velocity at a minimum joint velocity, while the latter allows the manipulator to optimize some obstacle-avoidance criterion by its self-motion, which does not affect the motion of its end-effector. Many performance indices on obstacle avoidance have been proposed to allow the redundant manipulator in motion to avoid obstacles as far as possible [8–11]. However, similarly to the artificial potential method, this local optimization method is also prone to making a redundant manipulator become stuck in some local minima. In addition, the two local methods cannot predict and control the configuration of the arm in motion, which is very significant for the anthropomorphic arm.

Taking these disadvantages into account, in this paper a generic framework of collision-free motion planning is proposed for anthropomorphic arms, which is a special group of redundant manipulators. In contrast to traditional collision-free motion planning methods to find feasible or optimal solutions, in this framework we try to create a global map for an anthropomorphic arm indicating the information on all feasible and unfeasible configurations of the arm. On the basis of this, a feasible route from the original configuration to the goal configuration can be easily planned in the feasible region of the created map. What is most appealing is that the planned route is highly intuitive, which means the user can easily imagine the detailed motion process of the arm corresponding to the planned result in advance, and can conveniently modify the route if the planned result does not meet expectations. This advantage of the proposed method in intuitiveness and convenience is the main novelty and unique feature of this paper. It is also worth noting that the proposed motion planning method is suitable for various anthropomorphic arms due to common or similar geometry. Thus, the method is endowed with the advantage of generality.

2. Some statements on the research issue

The research issue we discuss here is how to make an anthropomorphic arm achieve a successful reaching movement without collision with obstacles in its surroundings, as illustrated in Figure 1. The orientation of the hand of anthropomorphic arm is not considered in this paper; we are concerned rather with the position of the wrist. When the wrist is steered towards the goal position, we should not only keep the wrist away from the obstacle but also prevent the whole arm from approaching the obstacles, that is, the geometry of the whole anthropomorphic arm needs to be considered. Since different kinds of anthropomorphic arms have similar or almost the same geometry, it is possible to propose a unified motion planning framework for different anthropomorphic arms, which is one major contribution of this paper. In order to facilitate the detection of collisions between an anthropomorphic arm and obstacles, some simple bounding volumes are adopted to encapsulate them as their approximate geometrical representations. Here, due to similarity in shape and inexpensive computation [12], the sphere-swept lines, one type of sphere-swept volume commonly termed “capsules”, are used to approximately represent the upper arm and lower arm. Specifically, the capsule for an upper arm can be generated by sweeping a sphere of appropriate radius along the line segment between the centres of the shoulder and elbow joints. Likewise, the capsule for a lower arm would be the resulting volume of sweeping an appropriate sphere along the line segment between the centres of the elbow and wrist joints. Since the collision detection between any two sphere-swept volumes can be converted to conveniently comparing the distances between their inner primitives, such as points, lines, and rectangles, against the sum of their radii, we also employ sphere-swept volumes to represent obstacles. It is obvious that any complex sphere-swept volume can comprise some basic sphere-swept points, namely spheres. Without loss of generality, we assume the sphere is the basic geometrical element for describing obstacles.

Figure 1.

Reaching movement of an anthropomorphic arm without collision with obstacles

3. Collision-free motion planning for anthropomorphic arms

In this section, the proposed general collision-free motion-planning method for anthropomorphic arms will be introduced in detail. This method has a total of three steps. First, find an appropriate collision-free path only for the wrist of the anthropomorphic arm. Second, construct a complete global two-dimensional map for the whole arm, including feasible regions and forbidden regions, and plan a reasonable route in the feasible region. Finally, find solutions for the joint trajectories of the anthropomorphic arm corresponding to the planned route in the second step. In the following subsections, the three steps will be introduced in turn.

3.1. Finding a collision-free path for the wrist of the anthropomorphic arm

According to the assumption about the obstacles mentioned in section 2, the obstacles in the workspace of an anthropomorphic arm can be represented by a set of various spheres. Here, as an example, two spheres of different radii are employed as the obstacles in the environment, as shown in Figure 2. Given the start point and goal point of the wrist, our aim is to find a proper spatial path connecting the two points without any intersections with the two spheres. A great many research results from the field of neurophysiology show that human arms are inclined to perform a voluntary unconstrained reaching movement along a straight line rather than along other paths in free space [13, 14]. According to this conclusion, it is assumed that anthropomorphic arms should achieve a reaching movement along a straight line segment from the known start point to the goal point in space if the line segment does not intersect with any obstacle. A natural question then arises: how to design the path of the wrist if the line segment goes through some existing obstacles? Usually, we may use some via points in space that the planned path must pass through to control the shape of the path in order to avoid the obstacles. Different ways of choosing a set of via points will result in distinct feasible paths. Here, a convenient approach is proposed to obtain these via points.

Figure 2.

Diagram of a collision-free path for the wrist of an anthropomorphic arm

As illustrated in Figure 2, line segment SG goes through both the obstacle spheres, and the Cartesian coordinates of the centres of the spheres, $O_i(i=\mathrm{1,2})$ , are known. Viewing the centre as a repulsive source, the original straight line segment will be pushed out so it is bent. The repulsive direction is perpendicular to the line segment. Thus, the key via point $V_i$ can be obtained:

$\begin{array}{c}{SG}_u=\frac{SG}{‖SG‖}\\ {O_{i}F}_i=(S{O}_i\cdot {SG}_u){SG}_u-S{O}_i\\ V_i=O_i+\frac{(r_i+r_l+d_i{)O}_i{F}_i}{‖{O_{i}F}_i‖}\end{array}$

where upper-case letters indicate vectors and lower-case letters denote scalars. The symbol ‖ ‖ means the magnitude of vector, and the symbol ⋅ refers to the dot product between vectors. ${\mathrm{F}}_i$ indicates the foot of the perpendicular between ${\mathrm{O}}_i$ and SG. $r_i\hspace{0.17em}\text{and}\hspace{0.17em}{r}_l$ denote the radius of obstacle sphere i and the radius of swept sphere for the lower arm, and $d_i$ indicates the distance between sphere i and the bounding volume of the lower arm. The relationship between the three variables is as follows:

$d_i=\delta (r_i+r_l)$

δ can be viewed as a parameter to control the distance $d_i$ . When all via points are obtained in this way, along with the given start and goal points, some smooth functions can be used to interpolate these path points to generate a mathematical representation of the collision-free path. Here, three cubic spline functions will be employed to interpolate these path points respectively in terms of the different components of the position coordinate. That is, given the coordinates of the start, via, and goal points, $S\left(x_S,y_S,z_S\right),\hspace{0.17em}{V}_i\left(x_{V_i},y_{V_i},z_{V_i}\right),\hspace{0.17em}G\left(x_G,y_G,z_G\right)\hspace{0.17em}(i=1,...,n)$ (here, n refers to the number of the via points), three cubic spline interpolation functions will be constructed:

$x=x\left(t\right),\mathrm{\hspace{0.17em}}y=y\left(t\right),\mathrm{\hspace{0.17em}}z=z\left(t\right)$

(1)

where $x\left(t\right),y\left(t\right),z\left(t\right)\in C_{[0,T]}^2$ , T is the total time of the motion which can be specified in advance. The three cubic spline functions must satisfy the requirements as follows:

$\begin{array}{l}x\left(0\right)=x_S,\mathrm{\hspace{0.17em}}x\left(t_i\right)=x_{V_i},\mathrm{\hspace{0.17em}}x\left(T\right)=x_G,\hfill \\ y\left(0\right)=y_S,\mathrm{\hspace{0.17em}}y\left(t_i\right)=y_{V_i},\mathrm{\hspace{0.17em}}y\left(T\right)=y_G,\hfill \\ z\left(0\right)=z_S,\mathrm{\hspace{0.17em}}z\left(t_i\right)=z_{V_i},\mathrm{\hspace{0.17em}}z\left(T\right)=z_G.\hfill \end{array}$

Note that, when the three cubic spline functions pass through the same via point, the time, $t_i$ , must be set to be the same in order to guarantee the synchronization of these functions. $t_i$ can be determined as:

$t_i=\frac{T‖F_{i}S‖}{‖SG‖}=\frac{T\left(S{O}_i\cdot {SG}_u\right)}{‖SG‖}$

In addition, taking the actual initial and final velocities of the anthropomorphic arm motion into account, some additional boundary conditions are added for these spline functions:

$\begin{array}{l}{x^{\mathrm{\prime }}}^{}\left(0\right)=0,\mathrm{\hspace{0.17em}}{y^{\mathrm{\prime }}}^{}\left(0\right)=0,\mathrm{\hspace{0.17em}}{z^{\mathrm{\prime }}}^{}\left(0\right)=0,\hfill \\ {x^{\mathrm{\prime }}}^{}\left(T\right)=0,\mathrm{\hspace{0.17em}}{y^{\mathrm{\prime }}}^{}\left(T\right)=0,\mathrm{\hspace{0.17em}}{x^{\mathrm{\prime }}}^{}\left(T\right)=0.\hfill \end{array}$

Specific derivations of these spline functions can be obtained from [15], and thus are not presented here. By now, the spatial and temporal attributes of the planned collision-free path are both specified according to (1). Note that the proposed method is only suitable for cases in which the obstacles are distributed sparsely. Searching for collision-free paths in a complicated environment is still an open area for researchers, which is not the emphasis of this paper. Fortunately, the proposed global motion planning method for anthropomorphic arms can be effective based on any collision-free paths for the wrist. The creation of collision-free path used here is just a simple example for the introduction of the whole method.

3.2. Constructing a global obstacle-avoidance map for the whole anthropomorphic arm

After a specific path of the wrist of the anthropomorphic arm as a function of time is determined as in the preceding subsection, a two-dimensional global obstacle-avoidance map for the whole anthropomorphic arm is created. Each of the obstacles will generate a forbidden region in the map. The remaining regions of the map, apart from these forbidden regions, comprise the feasible region.

As shown in Figure 3, a simple triangle, $∆ShElWr$ , can be adopted to represent the abstract structure of an anthropomorphic arm without regard to the orientation of the end-effector (generally a hand). The vertex of the triangle, Sh, indicates the centre of the shoulder joint. Vertex El denotes the centre of the elbow joint. Wr means the centre of the wrist joint. The theoretical lengths of the upper and lower arms, lu and ll, are known. As the physiological shoulder joint of the human arm is a spherical joint it is usually simulated in the anthropomorphic arm by a set of three revolute joints whose axes intersect at one point. The elbow joint is often simulated by one revolute joint. So, in general, there are a total of four degrees of freedom for an anthropomorphic arm in locating the Cartesian position of its wrist, which results in one redundant degree of freedom. The redundant degree of freedom can be intuitively regarded as a self-rotation of the arm about the axis connecting the centres of the shoulder and wrist joints while keeping the position of the wrist static. Due to the existence of obstacles, the self-rotation of the arm is constrained within a limited range. Next, we will calculate the exact unfeasible range for the arm.

Figure 3.

Diagram of the relationship between the arm and the obstacle sphere

Keeping the position of the wrist static, the angle of the self-rotation can be regarded as a simple parameter to represent the configuration of the arm. Naturally, a reference configuration needs to be defined to calculate the specific angle value of a particular configuration of the arm. Here, the half-plane determined by vectors ShWr and V is defined as a reference plane for the arm, that is, the angle value of the self-rotation is zero when the arm is on this half-plane. V is the instantaneous linear velocity of the wrist. So, the reference configuration of the arm at every path point can be easily determined after the path of the wrist is defined. Obviously, the reference configuration changes as the path point varies. In fact, the defined path provides a complete and continuous reference system for the configuration description of the whole arm. In order to calculate the angle value between the reference half-plane and another half-plane containing the arm, that is, the angle of self-rotation of the arm, the direction of a half-plane is defined by the cross product of vectors X and ShWr, where X indicates any vector in the half-plane, the tail of which is on line segment ShWr. According to this definition, the unit direction vector of the reference half-plane, $P_r$ , can be obtained:

$P_r=\frac{V\times ShWr}{‖V\times ShWr‖}$

where symbol × indicates the cross-product between vectors. It can be noticed that the wrist velocities are equal to zero when the arm is at the start or goal point. So, in order to ensure the consistency of the reference half-plane, we choose the reference half-plane of the arm at the moment, $t=∆t$ , as the initial reference half-plane and choose $t=T-∆t$ , as the final reference plane for the arm, where $∆t$ denotes a very short period of time. It is not difficult to prove that the volume produced by sweeping the anthropomorphic arm triangle about axis ShWr within the unfeasible range is symmetrical about the half-plane α, which is determined by vector ShWr and the centre of the obstacle sphere $O_i$ . The unit direction vector of this half-plane, $P_{\alpha }$ , can be computed as:

$P_{\alpha }=\frac{Sh{O}_i\times ShWr}{‖Sh{O}_i\times ShWr‖}$

The positive direction of the self-rotation of the arm is defined by the right-hand rule. Specifically, providing that the right-hand screw direction is the self-rotation direction, the self-rotation angle is positive if the direction of the thumb is along the direction of vector ShWr. Therefore, when the triangle of the anthropomorphic arm is on the half-plane α, the self-rotation angle, ${\beta }_{\alpha }$ , can be used to describe the configuration of the arm:

$\begin{array}{rl}& \mathbf{i}\mathbf{f}\hspace{0.17em}(P_r\times P_{\alpha })\cdot ShWr>0\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\beta }_{\alpha }=arccos(P_r\cdot P_{\alpha })\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\hspace{0.17em}\mathbf{i}\mathbf{f}\hspace{0.17em}(P_r\times P_{\alpha })\cdot ShWr<0\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\beta }_{\alpha }=-arccos(P_r\cdot P_{\alpha })\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ & \hspace{0.17em}\mathbf{i}\mathbf{f}\hspace{0.17em}{P}_{\alpha }=P_r\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & \hspace{0.17em}\hspace{0.17em}{\beta }_{\alpha }=0\\ & \hspace{0.17em}\mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ & \hspace{0.17em}\hspace{0.17em}{\beta }_{\alpha }=\pi \\ & \hspace{0.17em}\mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\\ & \mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\end{array}$

(2)

As the forbidden region of the arm for an obstacle sphere is symmetrical about the half-plane α, the plane will be used as a reference half-plane to calculate the specific range of the unfeasible self-rotation angle for the arm. As shown in Figure 3, the arm triangle $∆ShEl1Wr$ on the half-plane α is rotated about axis ShWr by self-rotation angle θ to triangle $∆ShEl2Wr$ . So, our goal is to calculate the angle ${\theta }_b$ for the boundary condition under which the anthropomorphic arm is in contact with the obstacle sphere. On the basis of the structure of the anthropomorphic arm, this problem can be further decomposed into two subproblems: finding a solution for the angles ${\theta }_{b\_u}$ and ${\theta }_{b\_l}$ in the boundary states in which the upper arm and lower arm are just in contact with the sphere, respectively, and setting the larger one as the angle ${\theta }_b$ . Therefore, the relationship between a point (the centre of the sphere) and a line segment (the axis of the upper arm or lower arm) in space will be analysed in the next part.

Using the lower arm (the situation of the upper arm is similar), with two perpendiculars of El1Wr drawn on plane α from points El1 and Wr, respectively, plane α can be divided into four parts: its border line ShWr, and regions 1, 2, 3. When point ${\mathrm{O}}_{\mathrm{i}}$ is in the border line, the shortest distance between ${\mathrm{O}}_{\mathrm{i}}$ and ElWr cannot be changed by rotating the arm about the “shoulder-wrist” axis. When ${\mathrm{O}}_{\mathrm{i}}$ is in region 1, the shortest distance will occur between points ${\mathrm{O}}_{\mathrm{i}}$ and Wr. So, the arm can rotate about the “shoulder-wrist” axis freely since the length of ${\mathrm{O}}_{\mathrm{i}}\mathrm{W}\mathrm{r}$ is always larger than $(r_i+r_l)$ , which is guaranteed by the path planning for the wrist. When ${\mathrm{O}}_{\mathrm{i}}$ is located in region 2, the shortest distance is the length of the perpendicular between ${\mathrm{O}}_{\mathrm{i}}$ and ElWr – see ${\mathrm{O}}_{\mathrm{i}}{\mathrm{A}}_1$ in Figure 3. If $‖O_i{A}_1‖\ge (r_i+r_l)$ , the arm will never collide with the obstacle sphere; if $‖O_i{A}_1‖<(r_i+r_l)$ , the arm is not safe until the self-rotation angle θ is larger than ${\theta }_a$ , by which the arm is rotated about ShWr to make the length of ${\mathrm{O}}_{\mathrm{i}}{\mathrm{A}}_2$ equal to $(r_i+r_l)$ . Finally, when ${\mathrm{O}}_{\mathrm{i}}$ is in region 3, the length of ${\mathrm{O}}_{\mathrm{i}}{\mathrm{E}\mathrm{l}}_1$ is the shortest distance. Likewise, if $‖O_i{El}_1‖\ge (r_i+r_l)$ , the arm can be rotated freely. And if $‖O_i{El}_1‖<(r_i+r_l)$ , the arm is not away from the obstacle until ${\theta >\theta }_{el}$ if the foot of the perpendicular ${\mathrm{O}}_{\mathrm{i}}{\mathrm{A}}_2$ ( $‖O_i{A}_2‖=(r_i+r_l)$ ) is in the extended line of line segment WrEl2, or until ${\theta >\theta }_a$ if ${\mathrm{A}}_2$ is in the middle of line segment WrEl2, where ${\theta }_{el}$ is the angle by which the arm is rotated to make the length of ${\mathrm{O}}_{\mathrm{i}}{\mathrm{E}\mathrm{l}}_2$ equal to $(r_i+r_l)$ . Based on the analysis mentioned above, a brief algorithm description of solving for ${\theta }_{b\_l}$ can be presented as follows:

$\begin{array}{rl}& \mathbf{i}\mathbf{f}\hspace{0.17em}{O}_i\hspace{0.17em}\text{is in line ShWr}\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & \hspace{0.17em}\mathbf{i}\mathbf{f}\hspace{0.17em}\text{the shortest distance between}\hspace{0.17em}{\mathrm{O}}_{\mathrm{i}}\hspace{0.17em}\text{and ElWr},\\ & \hspace{0.17em}{d}_{min}\ge (r_i+r_l)\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & \hspace{0.17em}{\theta }_{b\mathrm{\_}l}=0\\ & \hspace{0.17em}\mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ & {\theta }_{b\mathrm{\_}l}=\pi \\ & \hspace{0.17em}\mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\hspace{0.17em}\mathbf{i}\mathbf{f}\hspace{0.17em}{\mathrm{O}}_{\mathrm{i}}\hspace{0.17em}\text{is in region}\hspace{0.17em}1\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\theta }_{b\mathrm{\_}l}=0\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\hspace{0.17em}\mathbf{i}\mathbf{f}\hspace{0.17em}{\mathrm{O}}_{\mathrm{i}}\hspace{0.17em}\text{is in region}\hspace{0.17em}2\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & \mathbf{i}\mathbf{f}\hspace{0.17em}{d}_{min}\ge (r_i+r_l)\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\theta }_{b\mathrm{\_}l}=0\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ & {\theta }_{b\mathrm{\_}l}={\theta }_a\\ & \mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\hspace{0.17em}{\mathrm{O}}_{\mathrm{i}}\hspace{0.17em}\text{is in region}\hspace{0.17em}3\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & \mathbf{i}\mathbf{f}\hspace{0.17em}{d}_{min}\ge (r_i+r_l)\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\theta }_{b\mathrm{\_}l}=0\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ & \mathbf{i}\mathbf{f}\hspace{0.17em}{\mathrm{A}}_2\hspace{0.17em}\text{is in the extended line of wrEL}2\hspace{0.17em}\text{when}\\ & \parallel O_i{A}_2\parallel =(r_i+r_l)\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\theta }_{b\mathrm{\_}l}={\theta }_{el}\\ & \mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\hspace{0.17em}{\mathrm{A}}_2\hspace{0.17em}\text{is in the middle of WrEl}2\hspace{0.17em}\text{when}\\ & \parallel O_i{A}_2\parallel =(r_i+r_l)\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ & {\theta }_{b\mathrm{\_}l}={\theta }_a\\ & \mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\\ & \mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\\ & \mathbf{e}\mathbf{n}\mathbf{d}\hspace{0.17em}\mathbf{i}\mathbf{f}\end{array}$

(3)

Next, the specific algorithms for ${\theta }_a$ and ${\theta }_{el}$ will be introduced. We establish a new frame, $\left\{O\prime x\prime y\prime z\prime \right\}$ , whose origin is coincident with that of the original one, $\left\{Oxyz\right\}$ . The direction of axis y' is along the vector ShWr, and axis z' is in the direction of the half-plane α. The coordinates of points Wr and El in terms of the new frame are $(0,\mathrm{\hspace{0.17em}}‖ShWr‖,\mathrm{\hspace{0.17em}}0)$ and ( $‖ElB‖cos\theta ,\hspace{0.17em}‖ShB‖,\mathrm{\hspace{0.17em}}-‖ElB‖sin\theta )$ respectively. The coordinate of $O_i$ relative to frame $\left\{O\prime x\prime y\prime z\prime \right\}$ can be obtained by simple coordinate transformation. So, we have:

$\begin{array}{l}{\hspace{0.17em}}^{O\mathrm{\prime }}Wr{O}_i={\hspace{0.17em}}^{O\mathrm{\prime }}{O}_i-{\hspace{0.17em}}^{O\mathrm{\prime }}Wr\mathrm{\hspace{0.17em}}\hfill \\ {\hspace{0.17em}}^{O\mathrm{\prime }}{Wr{O}_i}_u=\frac{{\hspace{0.17em}}^{O\mathrm{\prime }}Wr{O}_i}{‖{\hspace{0.17em}}^{O\mathrm{\prime }}Wr{O}_i‖}\hfill \end{array}$

Set $(x_u,y_u,0)$ as the coordinates of ${\hspace{0.17em}}^{O^{\prime }}Wr{o}_{i_{u^{\prime }}}$ , and set φ as the angle between vectors ${\hspace{0.17em}}^{O^{\prime }}Wr{o}_i$ and ${\hspace{0.17em}}^{O^{\prime }}WrEl$ . Assume ${\phi }_2=arcsin\left(\frac{r_i+r_l}{\Vert {\hspace{0.17em}}^{O^{\prime }}Wr{o}_i\Vert }\right)$ . Then:

$\begin{array}{c}{{\hspace{0.17em}}^{O\mathrm{\prime }}Wr{O}_i}_u\cdot {\hspace{0.17em}}^{O\mathrm{\prime }}Wr{}^{}{El}_2={{\hspace{0.17em}}^{O\mathrm{\prime }}Wr{}^{}{O}_i}_u\cdot \left({\hspace{0.17em}}^{O\mathrm{\prime }}{El}_2{}^{}-{\hspace{0.17em}}^{O\mathrm{\prime }}Wr\right)\\ =l_{l}cos{\phi }_2\Rightarrow \\ \begin{array}{c}(x_u,y_u,0)\cdot (‖ElB‖cos{\theta }_a,\mathrm{\hspace{0.17em}}‖ShB‖-‖ShWr‖,\mathrm{\hspace{0.17em}}-‖ElB‖sin{\theta }_a)\\ \begin{array}{c}=l_{l}cos{\phi }_2\Rightarrow \\ {\theta }_a=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}\left(\frac{l_{l}cos{\phi }_2-y_u\left(‖ShB‖-‖ShWr‖\right)}{x_u‖ElB‖}\right)\end{array}\end{array}\end{array}$

(4)

Assume $\Vert {\hspace{0.17em}}^{O^{\prime }}{O}_{i}E{l}_2\Vert =\left(r_i+r_l\right)$ , and set $(x,y,0)$ as the coordinates of ${\hspace{0.17em}}^{O^{\prime }}{O}_i$ . Then:

$\begin{array}{c}{(‖ElB‖cos{\theta }_{el}-x)}^2+{(‖ShB‖-y)}^2+{‖ElB‖}^2{sin}^2{\theta }_{el}\\ ={(r_i+r_l)}^2\Rightarrow \\ {\theta }_{el}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}\left(\frac{{‖ElB‖}^2+x^2+{\left(‖ShB‖-y\right)}^2-{\left(r_i+r_l\right)}^2}{2x‖ElB‖}\right)\end{array}$

(5)

The algorithm for calculating the shortest distance between a point and a line segment has already been introduced in our previous paper, [11]; therefore, it is not presented again here due to limited space. Based on formulas (4) and (5) and algorithm (3), ${\theta }_{b\_l}$ can be obtained. In almost the same way, ${\theta }_{b\_u}$ can also be computed easily. Then, the larger of ${\theta }_{b\_l}$ and ${\theta }_{b\_u}$ will be chosen as the angle ${\theta }_b$ . So, the configuration of the arm is not feasible if the self-rotation angle of the arm, β, is within the range:

With regard to the algorithm of calculating the shortest distance between a point and a line segment, it has already been introduced in our previous paper [11]. Therefore, it is not presented again here due to the limited space. Based on formulae (4) and (5) and algorithm (3), ${\theta }_{b\_l}$ can be obtained. In almost the same way, ${\theta }_{b\_u}$ can be also computed easily. Then, the larger one between ${\theta }_{b\_l}$ and ${\theta }_{b\_u}$ will be chosen as the angle ${\theta }_b$ . So, the configuration of the arm is not feasible if the self-rotation angle of the arm, β, is within the range:

${\beta }_{\alpha }-{\theta }_b\le \beta \le {\beta }_{\alpha }+{\theta }_{\mathrm{b}}$

(6)

Then, we construct a two-dimensional map whose horizontal axis is the time and whose vertical axis is the self-rotation angle of an arm. Since the relationship between the position of the path point and the time is defined according to (1), given the time, we can easily solve for the Cartesian coordinate of the corresponding path point of the wrist. So, a point on the map represents a specific configuration of the arm when the centre of the wrist is at a certain path point. For each path point of the wrist, there is an unfeasible configuration range of the arm, as (6). Therefore, for each obstacle sphere, there is a continuous forbidden region for the arm. As shown in Figure 4, there is one red forbidden region and one blue forbidden region caused by the obstacle spheres 1 and 2 respectively, and the remaining region of the map can be defined as feasible regions where the arm does not collide with any obstacles. Based on the created map, any continuous collision-free self-rotation angle profile can be planned within the feasible regions, as in the profiles above the blue forbidden region or below the red forbidden region shown in Figure 4. The feasible regions between the two forbidden regions are unavailable due to their incomplete connectivity along the time axis.

Figure 4.

An obstacle-avoidance two-dimensional map for the anthropomorphic arm

$\theta ={\theta }_{feasible}\left(t\right)$

(7)

When the obstacle-avoidance map dependent on the path of the wrist is created, the complete motion process of the arm will be uniquely determined after the suitable self-rotation angle profile in the map is planned. Different self-rotation angle profiles will yield different motion processes of the arm. Since the self-rotation angle directly reflects the extent of the rotation of the arm about the “shoulder-wrist” axis, the self-rotation angle profile can be regarded as a very intuitive tool to control the configuration of the arm in motion.

3.3. Solving for the joint trajectories according to the planned self-rotation angle profile

In this subsection, we will introduce how to solve for the specific joint trajectories for an anthropomorphic arm to realize the path of the wrist and the self-rotation angle profile of the whole arm planned in the previous two steps. First, we analyse the kinematics of anthropomorphic arms.

As shown in Figure 5, the absolute linear velocity of the wrist, $v_w^a$ , is caused by the transport angular velocity of the shoulder, ${\omega }_s^t$ , and the relative angular velocity of the elbow, ${\omega }_e^r$ . $v_w^a$ can be decomposed into one component ${\hspace{0.17em}}^{\Vert SW}{v}_w^a$ along the “shoulder-wrist” axis and the other component ${\hspace{0.17em}}^{\perp SW}{v}_w^a$ perpendicular to that axis. Because only the motion of the elbow joint can change the distance between the centres of the shoulder and wrist, ${\hspace{0.17em}}^{\parallel SW}{v}_w^a$ can be only provided by one component of $v_e^r$ along the “shoulder-wrist” axis produced by ${\omega }_e^r$ . ${\hspace{0.17em}}^{\perp SW}{v}_w^a$ can be composed of the other component of $v_e^r$ perpendicular to the “shoulder-wrist” axis and ${\hspace{0.17em}}^{\perp SW}{\nu }_s^t$ yielded by ${\hspace{0.17em}}^{\perp SW}{\omega }_s^t$ , which is one component of ${\omega }_s^t$ . The other component of ${\omega }_s^t$ , ${\hspace{0.17em}}^{\parallel SW}{\omega }_s^t$ , does not influence $v_w^a$ . In fact, ${\hspace{0.17em}}^{\parallel SW}{\omega }_s^t$ is used to control the orientation of the plane of the arm so as to force the arm to follow its planned self-rotation profile.

Figure 5.

Diagram of kinematics analysis for anthropomorphic arms

According to the planned path for the wrist, the absolute velocity of the wrist at any time can be easily obtained:

$v_w^a(t)={\left[x^{\mathrm{\prime }}\left(t\right)\mathrm{\hspace{0.17em}}{y}^{\mathrm{\prime }}\left(t\right)\mathrm{\hspace{0.17em}}z\mathrm{\prime }\right(t\left)\right]}^{\mathrm{T}}$

Define the symbol γ as the angle between the upper arm and lower arm; it can then be obtained thus:

$\gamma =arccos\frac{l_u^2+l_l^2-{‖ShWr‖}^2}{2l_u{l}_l}$

The unit direction vector of the half-plane, P, which the arm is on, can be calculated as:

$P=\frac{ShEl\times ShWr}{‖ShEl\times ShWr‖}$

According to the analysis mentioned above, the angular velocities of the shoulder and elbow joints can be solved by the algorithm as follows:

$\begin{array}{c}{ShEl}_u=\frac{ShEl}{‖ShEl‖}\\ {ElWr}_u=\left(\begin{array}{ccc}{ShEl}_u,& P\times {ShEl}_u,& P\end{array}\right)\mathrm{\hspace{0.17em}}R\left(z,(180-\gamma )\right)\\ \begin{array}{c}{(\mathrm{1,0},0)}^T\\ ElWr=l_l{ElWr}_u\\ \begin{array}{c}ShEl=l_u{ShEl}_u\\ ShWr=ShEl+ElWr\\ \begin{array}{c}{ShWr}_u=\frac{ShWr}{‖ShWr‖}\\ {\hspace{0.17em}}^{\parallel SW}{v}_w^a=(v_w^a\cdot {ShWr}_u){ShWr}_u\\ \begin{array}{c}{\hspace{0.17em}}^{\perp SW}{v}_w^a=v_w^a-{\hspace{0.17em}}^{\parallel SW}{v}_w^a\\ \begin{array}{c}{v}_e^r=\frac{‖{\hspace{0.17em}}^{\parallel SW}{v}_w^a‖}{(P\times {ElWr}_u)\cdot {ShWr}_u}(P\times {ElWr}_u)\\ {\omega }_e^r=\frac{{ElWr}_u\times v_e^r}{l_l}\end{array}\end{array}\end{array}\end{array}\end{array}\end{array}$

(8)

$\begin{array}{c}{\hspace{0.17em}}^{\perp SW}{v}_s^t={\hspace{0.17em}}^{\perp SW}{v}_w^a-\left(v_e^r-{\hspace{0.17em}}^{\parallel SW}{v}_w^a\right)\\ {\hspace{0.17em}}^{\perp SW}{\omega }_s^t=\frac{{ShWr}_u\times {\hspace{0.17em}}^{\perp SW}{v}_s^t}{‖ShWr‖}\end{array}\mathrm{\hspace{0.17em}}$

(9)

where $\left(\begin{array}{ccc}{ShEl}_u,& P\times {ShEl}_u,& P\end{array}\right)$ is a three-dimensional square matrix composed of three three-dimensional column vectors. $R\left(z,(180-\gamma )\right)$ refers to a rotation matrix that performs a rotation about z by $180-\gamma$ degrees. ${(1,\mathrm{0,0})}^T$ denotes a transpose of horizontal vector $(1,\mathrm{0,0})$ .

In order to calculate the angular velocity of the shoulder joint, ${\omega }_s^t$ , we also need to determine the value of ${\hspace{0.17em}}^{\parallel SW}{\omega }_s^t$ . As mentioned above, the choice of ${\hspace{0.17em}}^{\parallel SW}{\omega }_s^t$ depends on the planned self-rotation angle profile for the arm, that is, the change in the orientation of the half-plane of the arm. It is worth noting that the reference half-plane to which the arm half-plane is referenced will change as the path point varies. Taking the instantaneous self-rotation angular velocity of the reference half-plane about axis $\mathrm{S}\mathrm{h}\mathrm{W}\mathrm{r}$ , ${\omega }_r$ , into consideration, we have the following relationship:

${{\omega }_{rel}={\hspace{0.17em}}^{\parallel SW}\omega }_s^t-{\omega }_r$

where ${\omega }_{rel}$ indicates the relative angular velocity of the arm plane about axis $\mathrm{S}\mathrm{h}\mathrm{W}\mathrm{r}$ relative to the reference plane. Define that $\widehat{{\omega }_{rel}},\mathit{\hspace{0.17em}}{\hspace{0.17em}}^{\parallel }\widehat{{\hspace{0.17em}}^{SW}{\omega }_s^t},\hspace{0.17em}\text{and}\hspace{0.17em}\widehat{{\omega }_r}$ are the signed magnitudes of the related angular velocities along the vector direction $ShWr$ , and the equation below also holds:

$\widehat{{\omega }_{rel}}=\widehat{{\hspace{0.17em}}^{\parallel SW}{\omega }_s^t}-\widehat{{\omega }_r}$

(10)

$\widehat{{\omega }_{rel}}$ can be computed according to the planned self-rotation angle profile for the arm:

$\widehat{{\omega }_{rel}}={\theta }_{feasible}^{\mathrm{\prime }}$

(11)

Next, the method of calculating the self-rotation angular velocity of the reference half-plane, $\widehat{{\omega }_r}$ , will be introduced in order to compute ${\hspace{0.17em}}^{\parallel }\widehat{{\hspace{0.17em}}^{SW}{\omega }_s^t}$ . As shown in Figure 6, after an infinitesimal time d t, reference plane ${\mathrm{P}}_1$ , determined by $ShWr$ and $v_w^a$ , is changed into another reference plane ${\mathrm{P}}_2$ , defined by $ShWr\prime$ and $v_w^a\prime$ , where ${v_w^a}^{\prime }=v_w^a+a_w^{a}dt$ . Vector $a_w^a$ denotes the instantaneous linear acceleration of the wrist. Because the length of $\mathrm{W}\mathrm{r}\mathrm{W}\mathrm{r}\mathrm{\prime }$ is much shorter than that of $\mathrm{S}\mathrm{h}\mathrm{W}\mathrm{r}$ , $ShWr\prime$ can be approximately considered to be coincident with $ShWr$ . Then, line segment $\mathrm{K}\mathrm{J}$ is drawn perpendicular to $\mathrm{S}\mathrm{h}\mathrm{W}\mathrm{r}$ , and a perpendicular of ${\mathrm{P}}_1$ from point K (the head of vector $v_w^a$ ). The perpendicular and reference plane ${\mathrm{P}}_2$ intersect at point L, connecting points J and L. So, $d\phi =\angle KJL$ is the self-rotation angle of the reference half-plane. The unit normal vector of ${\mathrm{P}}_1$ , i, denotes the positive self-rotation direction of the plane. Define $\widehat{KL}$ as the signed magnitude of vector K L along the direction i. Therefore, since d φ is infinitesimal, it can be computed as:

Figure 6.

Diagram of the calculation of the self-rotation angular velocity of the reference plane

$d\phi =\frac{KL}{‖KJ‖}=\frac{KL}{‖v_w^a‖sin(arccos(\frac{v_w^a}{‖v_w^a‖}\cdot \frac{ShWr}{‖ShWr‖}\left)\right)}\mathrm{\hspace{0.17em}}\hspace{0.17em}$

(12)

Drawing a perpendicular of ${\mathrm{P}}_1$ from point Q (the head of vector $v_w^a\prime$ ), it and ${\mathrm{P}}_1$ intersect at point M. Drawing a line from L parallel to KM, the lines LN and MQ intersect at point N. The difference between the lengths of vectors KL and MQ can then be calculated:

$‖MQ‖-‖KL‖=tan\left(d\varphi \right)‖LN‖$

As $dt\to 0,\mathrm{\hspace{0.17em}}d\varphi \to 0$ and $(‖MQ‖-‖KL‖)\to 0$ . Thus, (12) can be improved to be:

$\begin{array}{c}d\phi =\frac{{(a}_w^{a}dt\mathrm{)}\cdot i}{‖v_w^a‖\mathit{sin}\left(\mathit{arccos}\left(\frac{v_w^a}{‖v_w^a‖}\cdot \frac{ShWr}{‖ShWr‖}\right)\right)}\\ =\frac{[{v_w^a}^{\mathrm{\prime }}\cdot (\frac{ShWr\times v_w^a}{‖ShWr\times v_w^a‖})]dt}{‖v_w^a‖sin(arccos(\frac{v_w^a}{‖v_w^a‖}\cdot \frac{ShWr}{‖ShWr‖}))}\end{array}$

Then, we can easily obtain:

$\widehat{{\omega }_r}=\frac{d\phi }{dt}=\frac{{v_w^a}^{\mathrm{\prime }}\cdot (\frac{ShWr\times v_w^a}{‖ShWr\times v_w^a‖})}{‖v_w^a‖sin(arccos(\frac{v_w^a}{‖v_w^a‖}\cdot \frac{ShWr}{‖ShWr‖}))}\hspace{0.17em}$

(13)

So, combining (10), (11) and (13), we can obtain ${\hspace{0.17em}}^{\parallel }\widehat{{\hspace{0.17em}}^{SW}{\omega }_s^t}$ and ${\hspace{0.17em}}^{\parallel SW}{\omega }_s^t$ :

$\begin{array}{c}\widehat{{\hspace{0.17em}}^{\parallel SW}{\omega }_s^t}=\widehat{{\omega }_{rel}}+\widehat{{\omega }_r}\\ {\hspace{0.17em}}^{\parallel SW}{\omega }_s^t=\frac{\widehat{{\hspace{0.17em}}^{\parallel SW}{\omega }_s^t}ShWr}{‖ShWr‖}\end{array}$

(14)

According to (8), (9), and (14), the angular velocities of the shoulder and elbow joints, ${\omega }_s^t\hspace{0.17em}\text{and}\hspace{0.17em}{\omega }_e^r$ , can be uniquely determined. Moreover, ${\omega }_s^t\hspace{0.17em}\text{and}\hspace{0.17em}{\omega }_e^r$ can be transformed into joint trajectories for various anthropomorphic arms with different structures, which endows the method with desirable generality. In the next part, a particular anthropomorphic arm with a typical structure will be used as an example to demonstrate how to solve for the specific joint trajectories. The structure of the typical arm without regard to the wrist joint is shown in Figure 7 (a). The link frames of the arm are established by using the D-H method with the post-position manner.

Figure 7.

(a) Schematic diagram of the structure of a typical anthropomorphic arm. (b) Local diagram for the shoulder.

For the shoulder joint, the schematic diagram of angular velocities of the first three joints can be obtained as shown in Figure 7(b), derived from the structure of the shoulder shown in Figure 7(a). The transport angular velocity vector of the shoulder ${\omega }_s^t$ is realized by the coordinated motions of the first three joints. In other words, ${\omega }_s^t$ needs to be written as a linear combination of unit vectors $z_0,z_1,\hspace{0.17em}\mathrm{a}\mathrm{n}\mathrm{d}\hspace{0.17em}{z}_2$ , which are the axes of the first three joints. Our strategy is to decompose ${\omega }_s^t$ into two components: one component ${\hspace{0.17em}}^{\parallel Z_2}{\omega }_s^t$ along $z_2$ and the other component ${\hspace{0.17em}}^{\perp Z_2}{\omega }_s^t$ perpendicular to $z_2$ . Meanwhile, the angular velocity vector of joint 1, $\stackrel{.}{{\theta }_1}{z}_0$ , is also decomposed into ${\dot{\theta }}_1{\hspace{0.17em}}^{\parallel Z_2}{Z}_0$ and ${\dot{\theta }}_1{\hspace{0.17em}}^{\perp Z_2}{Z}_0$ . It is not difficult to prove that ${\dot{\theta }}_1{\hspace{0.17em}}^{\perp Z_2}{Z}_0$ is always coincident with $x_2$ . Accordingly, ${\hspace{0.17em}}^{\perp Z_2}{\omega }_s^t$ can be composed of ${\dot{\theta }}_1{\hspace{0.17em}}^{\perp Z_2}{Z}_0$ and $\stackrel{.}{{\theta }_2}{z}_1$ , and ${\hspace{0.17em}}^{\parallel Z_2}{\omega }_s^t$ can be composed of ${\dot{\theta }}_1{\hspace{0.17em}}^{\perp Z_2}{Z}_0$ and $\stackrel{.}{{\theta }_3}{z}_2$ . On the basis of the analysis, the specific algorithm for the angular velocities of the first three joints is as follows:

$\begin{array}{c}{z}_0-{(1,0,0)}^T\\ z_1={(0,sin{\theta }_1,cos{\theta }_1)}^T\\ z_1={(cos{\theta }_2,cos{\theta }_1,sin{\theta }_1,sin{\theta }_2)}^T\\ {\hspace{0.17em}}^{\left|\right|z_2}{z}_0=(z_0\cdot z_2)z_2\\ {\hspace{0.17em}}^{\perp z_2}{z}_0=z_0-{\hspace{0.17em}}^{\left|\right|z_2}{z}_0\\ {\hspace{0.17em}}^{\perp z_2}{z}_{0u}=\frac{{\hspace{0.17em}}^{\perp z_2}{z}_0}{\Vert {\hspace{0.17em}}^{\perp z_2}{z}_0\Vert }\\ {\hspace{0.17em}}^{\left|\right|z_2}{\omega }_s^t=({\omega }_s^t\cdot z_2)z_2\\ {\hspace{0.17em}}^{\perp z_2}{\omega }_s^t={\omega }_s^t-{\hspace{0.17em}}^{\left|\right|z_2}{\omega }_s^t\\ {\stackrel{.}{\theta }}_1=\frac{{\hspace{0.17em}}^{\perp z_2}{\omega }_s^t\cdot {\hspace{0.17em}}^{\perp z_2}{z}_{ou}}{\Vert {\hspace{0.17em}}^{\perp z_2}{z}_o\Vert }\end{array}$

(15)

$\stackrel{.}{{\theta }_2}={\hspace{0.17em}}^{\perp {\mathrm{z}}_2}{\omega }_s^t\cdot z_1\mathrm{\hspace{0.17em}}$

(16)

$\stackrel{.}{{\theta }_3}=({\omega }_s^t\cdot z_2)-(\stackrel{.}{{\theta }_1}{z}_0\cdot z_2)$

(17)

where all vectors are described in terms of the base frame $\left\{O_b{x}_b{y}_b{z}_b\right\}$ . For the elbow joint, ${\omega }_e^r$ is implemented only by joint 4. According to the structure of the arm in Figure 7(a), the positive rotational direction of joint 4 is opposite to the direction of the half-plane which the arm is on. Assuming $x_w$ denotes the path point vector of the wrist, the angular velocity of joint 4, $\stackrel{.}{{\theta }_4}$ , can be obtained:

$\begin{array}{c}P=\frac{z_2\times x_w}{‖z_2\times x_w‖}\\ \mathbf{i}\mathbf{f}\mathrm{\hspace{0.17em}}{\omega }_e^r\cdot P\ge 0\hspace{0.17em}\mathbf{t}\mathbf{h}\mathbf{e}\mathbf{n}\\ \begin{array}{c}\begin{array}{c}\stackrel{.}{{\theta }_4}=-‖{\omega }_e^r‖\\ \begin{array}{c}\mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}\\ \stackrel{.}{{\mathrm{\hspace{0.17em}}\theta }_4}=‖{\omega }_e^r‖\end{array}\end{array}\\ \mathbf{e}\mathbf{n}\mathbf{d}\mathbf{\hspace{0.17em}}\mathbf{i}\mathbf{f}\end{array}\end{array}$

(18)

Accordingly, the angular velocities of all joints for this typical anthropomorphic arm can be calculated by (15)-(18), which constitute a differential equation group. The differential equation group can be solved for the joint trajectories of the arm by employing the Runge-Kutta approach with given initial values of joint angles. For different anthropomorphic arms with distinct structures, when their link frames are established, their joint trajectories can be easily solved in a similar way.

4. Simulations

4.1. Constructing the obstacle-avoidance map for a typical anthropomorphic arm

Figure 8 shows a virtual 3D human upper-body model whose right arm has the kinematic model shown in Figure 7(a). The lengths of the upper arm and lower arm are 72.7 mm and 68.0 mm respectively. The task is to make the right arm perform a reaching movement from start point S (0, 60, 0)(mm) to goal point G (70, 100, 30)(mm). In order to achieve the movement successfully, the right arm must avoid two obstacles whose centres are located at (33, 63, 0)(mm) and (45, 95, 10)(mm), respectively, and whose radii are 20 mm and 15 mm.

Figure 8.

A virtual 3D human upper body model

Because the line segment SG goes through the two spheres, a collision-free path for the wrist must be generated by using the method introduced in subsection 3.1. The positions of two via points can be calculated thus: V1(12.9, 83.4, 19.8)(mm) and V2(46.9, 75.7, 31.3)(mm). Then, the cubic spline function is employed to interpolate the four path points to generate a smooth path for the wrist, and the specific cubic spline function is obtained and rearranged as follows:

if t ≤ 1.64 then x(t) = 0.14t³ +4.55t² y(t) = −8.65t³ + 22.24t² + 0.03t + 60 z(t) = −3.78t³ + 13.56t² else if t ≤ 3.28 then x(t) = −1.38t³ + 11.85t² − 11.67t + 623 y(t) = 7.59t³ − 56.17t² + 128.84t − 10.28 z(t) = 0.45t³ − 724t² + 34.03t − 18.54 else x(t) = −175t³ + 15.67t² − 2488t + 20.68 y(t) = −9.54t³ + 11577t² − 446.04t + 629.66 z(t) = 0.95t³ − 12.02t² + 49.36t − 35.02 end if

The spatial curve described by this cubic spline function is shown in Figure 8. The total time of the reaching movement is 5 s. Based on the path for the wrist, a two-dimensional obstacle-avoidance map for the whole anthropomorphic arm can be obtained as introduced in subsection 3.2, illustrated in Figure 9:

Figure 9.

The obstacle-avoidance map for the typical anthropomorphic arm

In order to verify the correctness of the established obstacle-avoidance map, we select 20 key configurations, ${\mathrm{C}}_{\mathrm{A}}$ to ${\mathrm{C}}_{\mathrm{T}}$ , to properly outline the obtained map and then solve for their corresponding simulation results to check whether the results are in accordance with the information shown in the map. The obtained results are shown in Figure 10. From these simulations, the effectiveness of the established obstacle-avoidance map can be validated. For example, the key configuration, ${\mathrm{C}}_{\mathrm{E}}$ , is at the intersection of the two upper boundaries for the two obstacles, respectively. Correspondingly, as shown in Figure 10, the lower arm just contacts both the surfaces of the two obstacle spheres in this configuration.

Figure 10.

Simulation results of the 20 key configurations, ${\mathrm{C}}_{\mathrm{A}}$ to ${\mathrm{C}}_{\mathrm{T}}$

Next, based on the effective obstacle-avoidance map, we can plan arbitrary self-rotation profiles for the arm. Two feasible and one unfeasible self-rotation profiles are planned according to the feasible region and forbidden region of the map. The purple self-rotation profile in the top feasible region is drawn by the cubic spline curve interpolating the four points: (0, 2.5), (1.5, 3), (2.5, 3.3), (5, 2), while the green profile in the bottom feasible region is represented by another cubic spline function interpolating the points: (0, 0), (2, −0.3), (3.2, 0), (5, −1.5). The black unfeasible self-rotation profile is a horizontal line from the start point (0, 2.5). According to these cubic spline functions of the three self-rotation profiles, each group of joint trajectories corresponding to each self-rotation profile can be obtained by adopting the method introduced in subsection 3.3. The frame sequences of the simulation results are shown in Figure 11. The first, second, and fourth rows are for the purple feasible solution, black unfeasible solution, and green feasible solution, respectively. The third and last lines show alternative views of the black and green profiles, respectively.

Figure 11.

Simulation results of the three different self-rotation profiles shown in Figure 9. (a) The purple self-rotation profile. (b) The black self-rotation profile. (c) Another view of the black profile. (d) The green self-rotation profile. (e) Another view of the green profile.

According to the simulation results in Figure 11, the purple and green feasible self-rotation profiles can successfully keep the whole arm away from the obstacle spheres. The arm will collide with the bigger obstacle sphere from 1 s to 3 s if the arm performs the black self-rotation profile. So, all the results are in accordance with the constructed obstacle-avoidance map.

4.2. Comparison between the proposed method and a traditional method

In this part, for comparison, a traditional method, Gradient Projection Method (GPM), is used to plan the motion of the virtual arm to achieve the same reaching movement along the same path curve of the wrist shown in the preceding part. The differential equation of the joint velocity vector in the GPM is as follows:

$\stackrel{.}{Q}=J^{+}\stackrel{.}{X}+k(I-J^{+}J)\nabla H(Q)$

(19)

where $\stackrel{.}{Q}={[\stackrel{.}{q_1},\stackrel{.}{q_2},\stackrel{.}{q_3},\stackrel{.}{q_4}]}^T$ denotes the joint velocity column vector, $\stackrel{.}{X}$ indicates the linear velocity column vector of the wrist, J and $J^{+}$ refer to the Jacobian matrix and its pseudo-inverse, and I is the $4\times 4$ identity matrix.

$\nabla H\left(Q\right)={[\frac{\partial H}{\partial q_1}(Q),\frac{\partial H}{\partial q_2}(Q),\frac{\partial H}{\partial q_3}(Q),\frac{\partial H}{\partial q_4}(Q)]}^T$

means the gradient column vector of criterion H, which is the optimization objective function of GPM. k is the coefficient to control the optimization effect of the gradient vector. When k is set to zero, the solution is transformed into the minimum norm solution, which no longer optimizes the criterion H.

Here, H is defined as the shortest distance between the arm and the obstacles, $D_{min}$ . If the defining variables are $d_{\mathrm{m}\mathrm{i}\mathrm{n}\_ui}$ and $d_{\mathrm{m}\mathrm{i}\mathrm{n}\_li}$ , the shortest distance between the obstacle sphere i and the upper arm and that between sphere i and the lower arm, respectively, $D_{min}$ can be calculated as:

$\begin{array}{l}{D}_{min}=min(d_{min\_u1},d_{min\_l1},d_{min\_u2},d_{min\_l1},\dots \hfill \\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{d}_{min\_un},d_{min\_ln})(n=2\hspace{0.17em}in\hspace{0.17em}this\hspace{0.17em}case)\hfill \end{array}$

The initial joint vector is arbitrarily set as $Q=(4.18,-1.28,-1.40,-2.26)$ , and k is set as 0 and 10, respectively. The joint trajectories of the arm can thus be obtained by solving differential equation (19) with the Runge-Kutta approach. Thus, the self-rotation profile and the variation curve of the criterion $D_{min}$ for the minimum norm solution and GPM solution can be further obtained as shown in Figure 12.

Figure 12.

Comparison between three feasible solutions of the arm. (a) Self-rotation profiles. (b) Variation curves of criterion $D_{min}$ .

As shown in Figure 12(b), the value of the $D_{min}$ of the GPM solution is always larger than that of the minimum norm solution, which means the GPM has an optimization effect on the criterion $D_{min}$ . In Figure 12(a), it is clear that the feasible region of the map can be viewed as a complete set of feasible solutions, and the minimum norm solution and GPM solution are two solutions belonging to this set. Based on the information of the obstacle-avoidance map, the initial joint angles can be adjusted to make the initial self-rotation angle of the arm far away from the forbidden region. The red profile in Figure 12(a) is generated by improved GPM solution, obtained by adopting the improved initial self-rotation angle. The value of $D_{min}$ in the improved GPM solution is better than in the previous two solutions as shown in Figure 12(b). Therefore, the effect of the GPM can be improved by using the information of the proposed obstacle-avoidance map.

In addition to improving the GPM, another important advantage of the proposed obstacle-avoidance map is that it allows us to control the orientation of the anthropomorphic arm plane in order to produce more natural and human-like arm motion. As shown in Figure 11, after the path of the wrist is determined, the orientation of the arm plane can be arbitrarily adjusted within the feasible region to comply with some requirements at any moment. Specifically, the y-axis of the map indicates the rotation angle of the arm about the “shoulder-wrist” axis relative to the reference half-plane. We can simply modify the y coordinates of some key points through which the self-rotation angle profile must pass, to intuitively adjust the motion process of the arm if the previously planned result was not consistent with the habit of human arm motion, or did not meet the user's expectation. Obviously, in Figure 11, the green feasible solution is more human-like than the other two solutions.

Finally, we will draw a comparison between the proposed obstacle-avoidance map method and the GPM in terms of the computation effort and the likeliness of finding a feasible solution. It is not difficult to notice that solving for the joint trajectories in each of the two methods is realized by establishing the differential equation of the joint velocity first and then solving this equation with the Runge-Kutta approach. However, the difference between the two methods is that the differential equation is established based on the global information from the obtained map in the proposed method, and the similar equation is established based on the local information from the distance criterion, H. From formulae (15)-(19), we find that the joint velocities are obtained by simple operations among vectors and matrices in the proposed method; thus, it is an analytical method. In contrast, the GPM is a numerical method, due to the computation of the pseudo inverse of the Jacobian matrix. So, when the obstacle-avoidance map is established, the computation time of the proposed method will be shorter than that of the GPM in general if we ignore the computation effort for planning the self-rotation profile in the proposed method. For the likeliness of finding a feasible solution, the GPM will get into trouble and fail to complete the motion planning if there are any singularities during the planning. However, as a global method, it is easy to revise the self-rotation angles to allow the arm to avoid singularities to continue the motion planning in the proposed method. For instance, see the local diagram of the shoulder joint in Figure 7(b). If the axis of rotation for joint 3, $z_2$ , is close to or even coincides with the axis of rotation for joint 1, $z_0$ , by rotating joint 2, the shoulder joint will lose a degree of freedom, resulting in a singular configuration for the arm. In this case, the GPM cannot continue to finish motion planning, but in the proposed method, a self-rotation of the arm about the “shoulder-wrist” can be employed to move the shoulder away from the singular configuration to ensure the success of the motion planning. In this sense, the likelihood of finding a feasible solution in the proposed global method is much larger than in the local GPM.

5. Conclusions

In this paper, a novel collision-free motion planning method for anthropomorphic arms has been discussed. A general concept of an obstacle-avoidance map for various anthropomorphic arms was proposed. After the collision-free path for the wrist of the arm was defined, the time and the self-rotation angle of the arm about the “shoulder-wrist” axis were employed to parameterize any possible configurations of the arm, producing a global two-dimensional obstacle-avoidance map. The effectiveness of the constructed map was verified by several simulations. It was also proven that the configuration of the arm in motion could be intuitively controlled by planning an appropriate self-rotation angle profile within the feasible region of the map to make the anthropomorphic arm motion more human-like. Compared with other traditional collision-free motion planning methods, the map actually provides us with a complete set of all feasible solutions, rather than only one particular solution.

The effect of the hand of the arm on the collision-free motion planning is not discussed in this paper. The obstacle-avoidance issue of the anthropomorphic arm with all the seven joints will be further investigated in future research.