US 6,456,901 B1Grant

Hybrid robot motion task level control system

Issue Date:2002-09-24

•20 Claims

•3 Drawing Sheets

Abstract

A hybrid control system is provided for controlling the movement of a robot. The hybrid control system includes a singularity detector; a task level controller that receives a motion plan and determines a first set of control commands which are defined in a task space; and a joint level controller that receives the motion plan and determines a second set of control commands which are defined in a joint space. The singularity detector monitors the movement of the robot and detects robot movement in a region about a singularity configuration. When robot movement occurs outside of this region, the task level controller is operable to issue the first set of control commands to the robot. When the robot movement occurs inside of this region, the joint level controller is operable to issue the second set of control commands to the robot. In this way, the hybrid control system ensures feasible robot motion in the neighborhood of and at kinematic singularity configuration.

Metadata

Inventors

Ning Xi
Jindong Tan

Application Information

Application Number:US 09/838,894

Filing Date:2001-04-20

Priority Date:2001-04-20

Art Unit:7

Classifications

IPC:

G06F 1900

Field of Search:

700245700258700260700250700263700 29700 31700 59700 6370026170026270026431856817318572318578901 9901 6901 33901 34901 35901 41901 46701 23

Patent Drawings (3 sheets)

Description

Background of the Invention

[0002] The present invention relates generally to robot controllers and, more particularly, a hybrid control system for controlling the movement of a robot in the neighbor of and at kinematic singular configurations.

[0003] A robot task is usually described in its task space. The direct implementation of a task level controller can provide significant application efficiency and flexibility to a robotic operation. Implementation of the task level controller becomes even more important when robots are working coordinately with humans. The human intuition on a task is always represented in the task space. However, the major problem in the application of the task level controller is the existence of kinematic singularities. While approaching a singular configuration, the task level controller generates high joint torques which result in instability or large errors in task space. The task level controller is not only invalid at the singular configuration, but also unstable in the neighborhood of the singular configuration. Therefore, a myriad of methods have been proposed to solve this problem.

[0004] For instance, the Singularity-Robust Inverse (SRI) method was developed to provide an approximate solution to the inverse kinematics problem around singular configurations. Since the Jacobian matrix becomes ill-conditioned around the singular configurations, the inverse or pseudoinverse of the Jacobian matrix results in unreasonable torques being applied by a task level controller. Instead, the SRI method uses a damped least-squares approach (DLS) to provide approximate motion close to the desired Cartesian trajectory path. The basic DLS approach has been refined by varying the damping factors to improve tracking errors from the desired trajectory path. By considering the velocity and acceleration variables, the SRI method can be further improved to reduce the torque applied to individual joints and achieve approximate motion. By allowing an error in the motion, the SRI method allows the robot to pass close to, but not go through a singularity point. It can be shown that such a system is unstable at the singular points. This means that robot movement can not start from or can not actually reach the singularity points. Obviously, this creates difficulties for many robot applications.

[0005] In another instance, a path tracking approach augments the joint space by adding virtual joints to the manipulator and allowing self motion. Based on the predictor-corrector method of path following, this approach provides a satisfactory solution to the path tracking problem at singular configurations. Timing is not considered at the time of planning and it is reparameterized in solving the problem. However, when a timing is imposed on the path, it forces the manipulator to slow down in the neighborhood of singular configurations and to stop at the singular configuration.

[0006] The normal form approach provides a solution of inverse kinematics in the entire joint space. With appropriately constructed local diffeomorphic coordinate changes around the singularity, the solution of inverse kinematics can be found and then transformed back to the original coordinates. All joint space solutions are obtained by gluing together the regular and the singular piece. The normal form method is heavily computationally involved. In addition, it is experimentally difficult to implement it in real-time.

[0007] Finally, a time re-scale transformation method for designing robot controllers also incorporates the dynamic poles of the system. This method achieves slow poles in the vicinity of a singularity configuration and fast poles in the regular area. It can be shown that this method results in similar error dynamics as found in the SRI method.

[0008] Therefore, it is desirable to design a hybrid robot motion control system which is stable in the entire robot workspace including at the singularity configurations. Based on the analysis of the singular configurations of nonredundant robot manipulators, the robot workspace is divided into subspaces by the singularity configurations. In different subspaces, different continuous robot controllers could be used. A hybrid system approach is used to integrate different continuous robot controllers and singularity conditions are adopted as switching conditions for discrete control. With the hybrid motion control system, a robot can pass by singular configurations and achieve a stable and continuous motion in the entire workspace.

Summary of the Invention

[0009] In accordance with the present invention, a hybrid control system is provided for controlling the movement of a robot. The hybrid control system includes a singularity detector; a task level controller that receives a motion plan and determines a first set of control commands which are defined in a task space; and a joint level controller that receives the motion plan and determines a second set of control commands which are defined in a joint space. The singularity detector monitors the movement of the robot and detects robot movement in a region about a singularity configuration. When robot movement occurs outside of this region, the task level controller is operable to issue the first set of control commands to the robot. When the robot movement occurs inside of this region, the joint level controller is operable to issue the second set of control commands to the robot. In this way, the hybrid control system of the present invention ensures feasible robot motion in the neighborhood of and at kinematic singularity configuration.

[0010] For a more complete understanding of the invention, its objects and advantages, reference may be had to the following specification and to the accompanying drawings.

Brief Description of the Drawings

[0011] FIG. 1 is a block diagram of a hybrid motion control system in accordance with the present invention;

[0012] FIG. 2 is a diagram illustrating an exemplary robot workspace about a singular configuration in accordance with the present invention;

[0013] FIG. 3 is a block diagram of a first preferred embodiment of the hybrid motion control system of the present invention; and

[0014] FIG. 4 illustrates a typical switching sequence for the hybrid control system of the present invention.

Description of the Preferred Embodiment

[0015] A hybrid control system 10 for controlling the movement of a robot 20 is shown in FIG. 1. The hybrid control system generally includes a singularity detector 12, a task level controller 14, and a joint level controller 16. In operation, a motion plan (or trajectory plan) is received by the task level controller 14 and the joint level controller 16. In response to the motion plan, the task level controller 14 determines a first set of control commands which are defined in a task space; whereas the joint level controller 16 determines a second set of control commands which are defined in a joint space.

[0016] The singularity detector 12 monitors the movement of the robot 20 and detects robot movement in a region about a singularity condition. When robot movement occurs outside of this region, the task level controller 14 is operable to issue the first set of control commands to the robot 20. When the robot movement occurs inside of this region, the joint level controller 16 is operable to issue the second set of control commands to the robot 20. In this way, the hybrid control system 10 of the present invention ensures feasible robot motion in the neighborhood of and at kinematic singularity conditions. A more detailed description for the hybrid control system 10 of the present invention is provided below.

[0017] The dynamic model for a nonredundant robot arm can be written as

[0018] D(q){umlaut over (q)}+c(q,{dot over (q)})+g(q)=u

[0019] where q is the 6×1 vector of joint displacements, u is the 6×1 vector of applied torques, D(q) is the 6×6 positive definite manipulator inertia matrix, c(q,{dot over (q)}) is the 6×1 centripetal and coroilis terms, and g(q) is the 6×1 vector of gravity term.

[0020] For a robot task given in joint space, denoted by q^d,{dot over (q)}^d,{umlaut over (q)}^da joint level robot controller in joint space can be derived by

[0021] u₁=D(q)({umlaut over (q)}^d+K_v({dot over (q)}^d−{dot over (q)})+K_P(q^d−q))+c(q,{dot over (q)})+g(q) (1.1)

[0022] where K_Vqand K_Pqare feedback gain matrices. Given e_q1=q^d−q,e_q2={dot over (q)}^d−{dot over (q)}, the error dynamics for this controller can be described by

[0023] {dot over (e)}_q1=e_q2

[0024] {dot over (e)}_q2=−K_Pqe_q1−K_Vqe_q2.

[0025] It is straightforward to show that this system is asymptotically stable for appropriate gain matrices K_Pq,K_Vq. Regardless of the current robot configuration, the system can track any reasonable trajectories given as q^d,{dot over (q)}^d,{umlaut over (q)}^d.

[0026] However, a robot task is generally represented by desired end-effector position and orientation. Therefore, it is desirable to provide a robot controller that operates in task space. To develop a task level controller, the above dynamic robot model needs to be represented in the form of task level variables. Let Yε⁶be a task space vector defined by Y=(x,y,z,O,A,T)^T, where (x,y,z)^Tdenotes the position of the end-effector and (O,A,T)^Tdenotes an orientation representation (Orientation, Attitude, Tool angles) of the end-effector in the task space. The relationship between the joint space variable q and task space variable Y can be represented by Y=h(q). Accordingly, the dynamic robot model in the form of task space variables can then be described as follows:

[0027] DJ₀⁻¹(Ÿ−J_o{dot over (q)})+c(q,{dot over (q)})+g(q)=u (1.2)

[0028] where J_ois called the OAT Jacobian matrix and {dot over (Y)}=J_o{dot over (q)}. Given a desired path in task space, Y^d,{dot over (Y)}^d,Ÿ^d, the task level robot controller in task space can be described by

[0029] u₂=DJ₀⁻¹(Ÿ^d−J_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c(q,{dot over (q)})+g(q) (1.3)

[0030] where e_x1=Y^d−Y and e_x2={dot over (Y)}^d−{dot over (Y)}. Accordingly, the error dynamics in task space can be described as {dot over (e)}_x1=e_x2and {dot over (e)}=−K_Pxe_x1−K_Vxe_x2. It can also be shown that this system is locally asymptotically stable for appropriate gain matrices K_Px,K_Vx.

[0031] A comparison of the joint level controller and the task level controller shows that the operation of the task level controller depends on the existence of J_o⁻¹whereas the joint level controller does not. If the determinant of J_ois very small or zero, then the determinant of J_o⁻¹could be very large which in turn will result in very large joint torques. Robot configurations where det(J_o)=0 are referred to as singular configurations. Robot singular configurations and their corresponding analytical singularity conditions are therefore obtained based on an analysis of the Jacobian matrix.

[0032] For a six degrees of freedom robot manipulator having a three degrees of freedom forearm and a three degrees of freedom spherical wrist, the Jacobian matrix, J_o, is a 6×6 matrix. While the following description is provided with reference to a six degrees of freedom robot manipulator, it is readily understood that the present invention is applicable to other types of robot manipulators.

[0033] With regards to the six degrees of freedom manipulator, the Jacobian matrix may be decoupled in such a way that the singular configurations resulting from arm joint angles and wrist joint angles are separated. As will be apparent to one skilled in the art, the Jacobian matrix, J_o, could be decoupled into:

\begin{matrix} J_{o} = [\begin{matrix} I & U \\ 0 & I \end{matrix}] [\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}] [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}] \\ = [\begin{matrix} I & U \\ 0 & I \end{matrix}] [\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}] J_{w} \end{matrix}

J_{w} [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}], Ψ = [\begin{matrix} - \frac{s_{o} s_{A}}{c_{A}} & \frac{c_{o} s_{A}}{c_{A}} & 1 \\ - c_{o} & - s_{o} & 0 \\ - \frac{s_{o}}{c_{A}} & \frac{c_{o}}{c_{A}} & 0 \end{matrix}], U = [\begin{matrix} 0 & d_{6} \cdot e_{k} & - d_{6} \cdot e_{j} \\ - d_{6} \cdot e_{k} & 0 & d_{6} \cdot e_{i} \\ d_{6} \cdot e_{j} & - d_{6} \cdot e_{i} & 0 \end{matrix}], J_{11} = [\begin{matrix} - d_{4} s_{1} s_{23} - a_{1} s_{1} c_{2} - a_{3} s_{1} c_{23} - d_{2} c_{1} & d_{4} c_{1} c_{23} - a_{2} c_{1} s_{2} - a_{3} c_{1} s_{23} & c_{1} (d_{4} c_{23} - a_{3} s_{23}) \\ d_{4} c_{1} s_{23} + a_{1} c_{1} c_{2} + a_{3} c_{1} c_{23} - d_{2} s_{1} & d_{4} s_{1} c_{23} - a_{2} s_{1} s_{2} - a_{3} s_{1} s_{23} & s_{1} (d_{4} c_{23} - a_{3} s_{23}) \\ 0 & - d_{4} s_{23} - a_{2} - a_{3} c_{23} & - d_{4} s_{23} - a_{3} c_{23} \end{matrix}], J_{21} = [\begin{matrix} 0 & - s_{1} & - s_{1} \\ 0 & - c_{1} & - c_{1} \\ 1 & 0 & 0 \end{matrix}], J_{22} = [\begin{matrix} c_{1} s_{23} & - c_{1} c_{23} s_{4} - s_{1} c_{4} & c_{1} c_{4} c_{23} s_{5} - s_{1} s_{4} s_{5} + c_{1} s_{23} c_{5} \\ s_{1} s_{23} & - s_{1} c_{23} s_{4} - c_{1} c_{4} & s_{1} c_{4} c_{23} s_{5} + c_{1} s_{4} s_{5} + s_{1} s_{23} c_{5} \\ c_{23} & s_{23} s_{4} & c_{23} c_{5} - s_{23} c_{4} s_{5} \end{matrix}], \begin{matrix} e_{i} = c_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) - s_{1} s_{4} s_{5}, \\ e_{j} = s_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) + c_{1} s_{4} s_{5}, \\ e_{k} = c_{23} c_{5} - s_{23} c_{4} s_{5} . \end{matrix}

[0034] such that s_iand c_istand for sin(q_i) and cos(q_i), respectively, and a₂,a₃,d₂,d₄are indicative of robot joint parameters. Since Ψ and ∪ are not singular, the singularity analysis can be obtained by checking J_w. Further assessment of J_wreveals that J₁₁involves only q₁,q₂,q₃which correspond to the arm joints and J₂₂involves only q₄,q₅,q₆which correspond to the wrist joints. Singular configurations caused by J₁₁=0 are called arm singularities and singular configurations caused by J₂₂=0 are called wrist singularities.

[0035] Since the determinant of J₁₁is (d₄{dot over (c)}₃−a₃s₃)(d₄s₂₃+a₂c₂+a₃c₂₃), two singular configurations can with the arm joints. A boundary singular configuration occurs when γ_b=d₄c₃−a₃s₃=0. This situation occurs when the elbow is fully extended or retracted. An interior singular configuration occurs when

[0036] γ_i=d₄s₂₃+a₂c₂+a₃c₂₃=0.

[0037] On the other hand, a wrist singularity can be identified by checking the determinant of the matrix J₂₂. The wrist singularity occurs when two wrist joint axes are collinear. The corresponding singularity condition is denoted by γ_w=−s₅=0.

[0038] Each singular condition corresponds to certain robot configurations in the workspace of the robot. The workspace of a robot is a complex volume calculated from the limit values of the joint variables. The workspace can be described by two concepts: reachable workspace and dexterous workspace. A reachable workspace is the volume in which every point can be reached by a reference point on the end-effector of the manipulator. The dexterous workspace is a subset of the reachable workspace and it does not include the singular configurations and their vicinities. The dexterous workspace is therefore a volume within which every point can be reached by a reference point on the manipulator's end-effector in any desired orientation.

[0039] Unfortunately, the dexterous workspace is not a connected space. It is separated into subspaces by the singular configurations. For example, the wrist singularity condition can be satisfied at almost any end-effector position. In the other word, almost at any point of the workspace, there is an orientation of the end-effector which will lead to the wrist singularity condition. A robot path may compose several segments in the dexterous subspaces as well as one or more segments in the vicinity of a singular configuration. In the dexterous subspaces, there is no problem to control the robot at task level. However, when the robot task requires the robot to go from one dexterous subspace to another dexterous subspace, the end-effector needs to go through the vicinity of a singular configuration. At these configurations, the manipulator loses one or more degrees of freedom and the determinant of J_oapproaches zero. In task space, impractically high joint velocities are required to generate a reasonable motion. Thus, it is desirable to provide a hybrid robot motion control system to achieve a stable and continuous motion in the entire workspace.

[0040] To design such a hybrid motion control system, the robot workspace, Ω, can be divided into two kinds of subspaces: the dexterous subspace, Ω₀and the subspaces in the vicinity of singular configurations as denoted by Ω₁and Ω₂. The definition of Ω₁and Ω₂are given as follows:

[0041] Ω₁={qεR⁶|α_b≦|γ_b|≦β_b∪α_i|γ_i|≦|γ_wβ_i∪α_w≦|γ_w|≦β_w}

[0042] Ω₂={qεR⁶∥γ_b|<α_b∪|γ_i|<α_i∪|γ_w<α_w}

[0043] where β_b>α_b>0,β_i>α_i>0,β_w>α_w>0. In other words, the subspace denoted by Ω₂is an area closer in proximity to the singular configuration than the subspace denoted by Ω₁. An exemplary robot workspace in the vicinity of a singular configuration, including each of the described subspaces, is shown in FIG. 2. It is worth noting that Ω₀∪Ω₁∪Ω₂=Ω and Ω₀∩Ω₁∩Ω₂=φ. addition, a region, δ, called the dwell region is defined in Ω₁. The dwell region may be defined as Δ={qεR⁶|α_i<|γ_i|<α_i+δ and/or α_b<|γ_b|<α_b+δ and/or α_w<|γ_w|<α_w+δ}, where δ is a constant greater than zero. As will be further described below, the dwell region is used to avoid chattering when switching occurs between the subspaces.

[0044] In subspace Ω₀, the inverse of J_oalways exists and thus the task level controller is effective in this subspace. In region Ω₁, det J_ois very small, and yet a feasible solution of the inverse Jacobian can be obtained by using a pseudo-inverse Jacobian matrix. As will be further described below, the task level controller can still be used after substituting J_o^# for J_o⁻¹, where J_o^# is a kind of pseudo-inverse Jacobian matrix. However, as is known in the art, the task level controller based on pseudo-inverse Jacobian matrix will cause instability in subspace Ω₂. If a robot motion controller can not make the robot go through singular subspace Ω₂, the singular configurations greatly restricts the dexterous workspace of the robot. Since the joint level controller works in the whole workspace, it can be used to maintain system stability and smooth trajectory in subspace Ω₂. In accordance with the present invention, the hybrid control system employs different controllers in different subspaces in order to achieve stable and continuous robot motion in the entire workspace.

[0045] A hybrid motion control system involves continuous and discrete dynamic systems. The evolution of such a system is given by equations of motion that generally depend on both continuous and discrete variables. The continuous dynamics of such a system is generally modeled by several sets of differential or difference equations; whereas the discrete dynamics describes the switching logic of the continuous dynamics. Thus, the task level controller and the joint level controller are the continuous controllers and the singularity conditions serves as the discrete switching conditions between these continuous controllers.

[0046] A general form of the hybrid robot motion controller is defined as follows. The continuous state variable is either joint angle, q, or the end-effector position and orientation, Y. The discrete state variables, denoted by mε¹or mε{m₁,m₂, . . . m₁}, represent the closeness to a singular configuration. The hybrid control system model of the robot can be described by

[0047] D(q){umlaut over (q)}+C(q,{dot over (q)})+g(q)=u

[0048] DJ_o⁻¹(Ÿ−{dot over (J)}_o{dot over (q)})+c(q,{dot over (q)})+g(q)=u

[0049] m(t)=f(Y(t),q(t),m(t⁻))

[0050] and the hybrid robot motion controller is given by

[0051] u(t)=f(Y(t),q(t),m(t)).

[0052] The dynamics, f, is governed by the singularity conditions. Depending on the current discrete state and the continuous states of the robot, f gives the next discrete state. t⁻ denotes that m(t) is piecewise continuous from the right. f discretizes the continuous states and switches between local controllers. h(t) integrates the task level controller, the joint level controller and the discrete state m(t). As will be further described below, Max-Plus algebra is used to describe the discrete event evolution. It can provide an analytical representation of a discrete event system.

[0053] In subspace Ω₁, the robot is in the neighborhood of singular configurations where the direct inverse of the Jacobian matrix will result in large torques. Therefore, the hybrid motion control system of the present invention uses a modified task level controller in this subspace. In particular, the modified task level controller employs a pseudoinverse Jacobian matrix which is computed using a damped least squares technique. A more detailed description of the modified task level controller is provided below.

[0054] The inverse Jacobian J_o⁻¹can be decomposed into

J_{o}^{- 1} = J_{w}^{- 1} \cdot {[\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}]}^{- 1} \cdot {[\begin{matrix} I & U \\ 0 & I \end{matrix}]}^{- 1},

[0055] where

J_{w} = [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}] .

[0056] In the neighborhood of singular configurations, J_wis ill-conditioned. However, the inverse of J_wcan be computed using a damped least squares technique, thereby yielding

[0057] J_w^#=(J_w·J_w^T+·m_s1(t))⁻¹·J_w^T, (1.4)

[0058] where m_s1(t) is a matrix of variable damping factors. The inverse of J_othat is based on J_w^#is called J_o^#.

[0059] m_s1defines the switching conditions for the task level controller. It is a function of the singularity conditions and can be represented by Max-plus algerbra. More specifically, the Max-Plus algebra is defined as m(t)εⁿ_max″, where _max=∪{−∞}, ⊕: max operation, and {circle around (x)}: plus operation. Some exemplary opperations may include (but are not limited to) a⊕b=max{a,b} and a{circle around (x)}b=a+b. It can be shown that {_max″:⊕{circle around (x)}} is an idempotent and commutative semifield with zero element α=−∞ and identity element e=0.

[0060] Based on the analysis of the inverse of J_w, m_s1is defined as follows:

\begin{matrix} m_{s1} (t) = diag ([m_{1} (t) m_{2} (t) m_{3} (t) m_{4} (t) m_{5} (t) m_{6} (t)]) \\ = [\begin{matrix} k_{i} \oplus 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & k_{i} \oplus k_{b} \oplus 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & k_{b} \oplus 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & k_{w} \oplus 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & k_{w} \oplus 0 \end{matrix}] \end{matrix}

[0061] where k_b=k_b0(1−|γ_b|/β_b), k_i=k_i0(1−|γ_i|/β_i), and k_w=k_w0(1−|γ_w|/β_w). In other words m_s1(t) is a matrix of positive damping factors which depend on the closeness to the singular configurations. Diag(v) denotes a matrix whose diagonal elements are vector, v, and the remaining elements are zeroes. At a dexterous configuration, the diagonal elements of m_s1are zeroes. In the vicinity of singularity conditions, some of the elements are nonzeroes and some of the elements are zeroes depending on the particular singularity condition. Accordingly, the task level controller of the hybrid control system can be synthesized by

[0062] u₃=DJ_o^#(Ÿ^d+J_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c+g (1.5)

[0063] A stability analysis for the modified task level controller is presented below. By substituting equation (1.5) into equation (1.2) and defining e_x1=Y^d−Y and e_x2={dot over (Y)}^d−{dot over (Y)}, the error dynamic for the task level controller can be described by:

[0064] {dot over (e)}_x1=e_x2

[0065] {dot over (e)}_x2=(I−J_oJ_o^#)(Ÿ^d−{dot over (J)}{dot over (q)})−J_oJ_o^#(K_Pxe_x1+K_Ve_x2)

[0066] To analyze the stability of this system, the term J_oJ_o^# is a key component. It can be simplified by the singular value decomposition (SVD) of Jacobian matrix, J_w, as follows

J_{w} = \sum_{i = 1}^{6} σ_{i} u_{i} v_{i}^{T} = U \cdot \sum_{1} \cdot V^{T} .

[0067] Therefore, equation (1.4) can be expressed by

J_{w}^{#} = \sum_{i = 1}^{6} \frac{σ_{i}}{σ_{i}^{2} + m_{i}} v_{i} u_{i}^{T} = V^{T} \cdot \sum_{2} \cdot U

[0068] where v_i,u_i,i=1, . . . ,6 are orthonormal basis of IR⁶; σ_i,i=1, . . . ,6 are the singular values of J_w.; U and V are orthonormal matrices; and Σ_i,i=1,2 are diagonal. J_oJ_o^# can then be simplified as follows:

\begin{matrix} k = J_{o} J_{o}^{#} \\ = diag [\frac{σ_{1}^{2}}{σ_{1}^{2} + m_{1}} \frac{σ_{2}^{2}}{σ_{2}^{2} + m_{2}} \frac{σ_{3}^{2}}{σ_{3}^{2} + m_{3}} \frac{σ_{4}^{2}}{σ_{4}^{2} + m_{4}} \frac{σ_{5}^{2}}{σ_{5}^{2} + m_{5}} \frac{σ_{6}^{2}}{σ_{6}^{2} + m_{6}}] \end{matrix}

[0069] By defining

k_{\min} = \underset{i = 1}{\min^{6}} {\frac{σ_{i}^{2}}{σ_{i}^{2} + m_{i}}},

[0070] it can be seen that k_minbecomes smaller and smaller when the robot configuration is approaching a singular point. The stability of controller in subspace Ω₀and Ω₁will depend on k. In subspace Ω₀, k=I, the error dynamics in region Ω₀becomes

[0071] {dot over (e)}_x1=e_x2

[0072] {dot over (e)}_x2=−K_Pxe_x1−K_Vxe_x2 (1.6)

[0073] Accordingly, the modified task level controller is asymptotically stable in subspace Ω₀for appropriate gain matrices.

[0074] While in subspace Ω₁, the error dynamics of the system can be described by

[0075] {dot over (e)}_x1=e_x2

[0076] {dot over (e)}_x2=(I=k)(Ÿ^d−{dot over (J)}{dot over (q)})−k(K_Pxe_x1+K_Vxe_x2)

[0077] Defining the candidate Lyapunov function of equation (1.6) as

[0078] V=½[e_x1^TKe_x1+(μe_x1+e_x2)^T(μe_x1+e_x2)]

[0079] where K is positive definite matrix and μ is positive constant. The derivative of the candidate Lyapunov Function can be derived by

[0080] {dot over (V)}=e_x1^TKe_x2+(μe_x1+e_x2)^T

[0081] (μe_x2+(I−k)(Ÿ^d−{dot over (J)}{dot over (q)})−

[0082] k(K_Pxe_x1−K_Vxe_x2))=−μe_x1^TkK_Pxe_x1−

[0083] e_x2^T(kK_Vx−μI)e_x2+(μe_x1+e_x2)^T

[0084] (I−k)(Ÿ^d−{dot over (J)}{dot over (q)})≦−μe_x1^Tk_minK_Pxe_x1−

[0085] e_x2^T(k_minK_Vx−μI)e_x2+(1−k_min)·∥e_x∥η{square root over (1+μ²)}=

[0086] −k_minλ_min∥e_x∥+(1−k_min)·∥e_x∥η{square root over (1+μ²)}=−

[0087] (k_minλ_min−(1−k_{min)·η{square root over (1+μ}_x∥

[0088] where K=K_Px+μK_Vx−μ²I and μ is chosen such that K_Vx−μI and K are positive definite. λ_minis the minimum singular value of μK_Pxand K_Vx−μI/K_min, which is positive. η is the bound of ÿ^d−{dot over (J)}{dot over (q)}. From the above derivation, it can be seen that the stability depends on the value of k_min. In subspace Ω₀, k_min=1, {dot over (V)} is the negative definite and the system is asymptotically stable. In subspace Ω₁, k_minλ_min−(1−k_min)·η{square root over (1+μ²)}>0, the system is still asymptotically stable. However, in subspace Ω₂, the value of k_minis close to zero, k_minλ_min−(1−k_min)·η{square root over (1+μ²)}≦0, such that system stability can not be ensured.

[0089] In region Ω₂, two observations can be made from the error dynamics. First, at the singular points, because some singular values in Σ₂are zeros, the corresponding gains in gain matrices become zeroes. Assuming σ_iis zero, the i^thelement of the simplified Jacobian matrix J_wcan be written as:

[0090] {dot over (e)}_x1i=e_x2i

[0091] {dot over (e)}_x2i=Ÿ_i^d−({dot over (J)}{dot over (q)})_i

[0092] Thus, it can be seen that the system is an open loop system. Though it can be proven the system is ultimately bounded, big errors in the task space are expected when the measurement of the singular conditions are small enough. When a wrist singularity is met, the errors of O,A,T angles are very big. Accompanied with the errors in task space, large joint velocities are experienced which are unacceptable in most applications.

[0093] Second, when the robot approaches close to singular configurations, such as in region Ω₂, some of the elements of k become very small and the corresponding elements of the gain matrices also become small. Though the system is stable at this region, large task error is expected and output torque is reduced in certain directions.

[0094] To achieve a stable control in region Ω₂, a switching control is needed such that a joint level control can be enabled in Ω₂. Two discrete variables, m₇and m₈, are defined to represent arm and wrist singularities, respectively,

[0095] (m₇(t)=m₇(t⁻)^sgn(α^+δ−γ^)⊕sgn(α^+δ−γ^)⊕0⊕sgn((α_i−γ_i)⊕sgn((α_b−γ_b)⊕0

[0096] m₈(t)=m₈(t⁻)^sgn(α^+δ−γ^)⊕0⊕sgn(α_w−γ_w)⊕0.

[0097] where, m₇(t⁻) and m₈(t⁻) represent the values of m₇and m₈before a time instances t, respectively, and α_i,α_b,α_wspecify subspace Ω₂. Depending on the values of γ_b,γ_i, and γ_w, m₇and m₈will determine which of the continuous controllers should be used in the hybrid control system. It is worth noting that in the dwell region, Δ, the values of m₇and m₈not only depend on γ_b,γ_iand γ_w, but also depend on the previous value of m₇(t⁻) and m₈(t⁻). The dwell region, Δ, is introduced to avoid chattering in the switching surface. In other words, the switching surface for switching into joint level control and switching out of a joint level control is different. After the controller switches into joint space control, it can not switch back to task space control unless the robot reaches a larger region. This strategy can effectively avoid chattering.

[0098] A switching matrix, m_s2, is based on m₇and m₈, as follows:

m_{s2} (t) = [\begin{matrix} m_{7} (t) I_{3 \times 3} & 0 \\ 0 & (m_{7} (t) \oplus m_{8} (t)) I_{3 \times 3} \end{matrix}] .

[0099] This switching matrix is then used to form a hybrid motion control system.

[0100] In summary, the hybrid motion control system of the present invention can be represented by

[0101] u=(I−m_s2)u₁+m_s2u₃

[0102] The system involves both continuous controllers and switching controls as shown in FIG. 3. In operation, the switching conditions are dependent on the closeness to the singular configurations and the previous controller status. When the robot approaches a singular configuration, the hybrid motion control system first uses damped least squares to achieve an approximate motion of the end-effector. The hybrid motion control system will then switch into joint level control if the robot reaches the singular configurations.

[0103] It is worth noting that the joint level control and task level control can coexist. This happens when m₇=0 and m₈=1. It means that the position is controlled at task level and the orientation is controlled at joint level. For a given task, Y^d={x^d,y^d,z^d,O^d,A^d,T^d}, the desired position at the wrist center, denoted by {p_x^d,p_y^d,p_z^d}, can be obtained and taken as the desired value in the position control at task level. The orientation control will be obtained by controlling q₄,q₅,q₆at joint level. The end-effector position and orientation can therefore be controlled separately. Control in task level and joint level will coexist in this situation. Since the wrist singular configuration may happen at almost any end-effector position, the separated position control and orientation control can ensure a relatively larger continuous dexterous workspace for position control.

[0104] In subspaces Ω₀and Ω₁, the robot task is represented in Cartesian space. In region Ω₂. however, the robot task needs to be transformed into joint space. Given Y^din task space, joint level command q^dneeds to be calculated in subspace Ω₂. As will be apparent to one skilled in the art, the normal form approach and equivalence transformation can be used to map the task from Cartesian space to joint space at singular configuration. However, the normal form approach is highly computationally intensive.

[0105] Alternatively, since region Ω₂is very small, the desired joint level command q^dcan be obtained computationally as follows. At a singular configuration, some of the joint angles can be obtained by inverse kinematics. For example, at a wrist singular configuration, q₁^d,q₂^d,q₃^d,q₅^dcan be computed from Y^dby the normal inverse kinematic approach. q₄^dand q₆^dcan be obtained computationally considering the current value of q₄and q₆. For each singular configuration, only two of the joint commands can not be obtained by inverse kinematics. To compute these joint level commands, the following criteria are considered.

[0106] min{w₁|Y^d−h(q^d)|+w₂|q^d−q|}q^d

[0107] min{|{dot over (Y)}^d−J{dot over (q)}^d|}{dot over (q)}^d≦{dot over (q)}max,{dot over (q)}^d

[0108] where w₁and w₂are positive weight factors.

[0109] The first constraint minimizes the deviation after the task is mapped into joint space, and tries to find a q^dsuch that the least joint movement is needed. It is worth noting that not all desired joint level command need to be calculated from the criteria. Only the joint angles that can not be obtained by the inverse kinematics are calculated using the criteria The values of singular conditions γ_b,γ_b,γ_bwill determine the joint to be found by the optimization criteria.

[0110] The second constraint ensures the planned joint velocities are within the joint velocity limits. The path from q to q^dis planned based on the desired velocity obtained from the second constraints. The continuity of joint velocities and task level velocities are considered in the planning. At joint level, the initial velocity for every joint is the joint velocity prior to switching.

[0111] Next, the hybrid motion control system of the present invention is shown to be stable when switching between the task level control and joint level control. The difficulty at proving the stability of the hybrid motion control system lies in that the state variables in the error dynamics in joint space and task space are different. The errors in joint space and task space are defined as

e_{q} = (\begin{matrix} e_{q1} \\ e_{q2} \end{matrix}) = (\begin{matrix} q^{d} - q \\ {\dot{q}}^{d} - \dot{q} \end{matrix}), e_{x} = (\begin{matrix} e_{x1} \\ e_{x2} \end{matrix}) = (\begin{matrix} y^{d} - y \\ {\dot{y}}^{d} - \dot{y} \end{matrix}) .

[0112] The error dynamics of the manipulator in joint space can be written by

[0113] {dot over (e)}_q1=e_q2

[0114] {dot over (e)}_q2=−K_Pqe_q1−K_Vqe_q2 (2.1)

[0115] or {dot over (e)}_q=f_q(e_q). The error dynamic in the task space is

[0116] {dot over (e)}_x1=e_x2

[0117] {dot over (e)}_x2=(I−k)(ÿ^d−J{dot over (q)})−k(K_Pxe_x1+K_Vxe_x2) (2.2)

[0118] or {dot over (e)}_x=f_x(e_x). If the system switches between the two different controllers, the stability of the switching system needs to be proven. Since the state variables in error dynamic models are different, the relationship between the state variables in models is derived first.

[0119] It is easy to prove that y=h(q) and {dot over (y)}=J{dot over (q)} are globally Lipschitz continuous in their defined domain. Thus the following relationship can be obtained

[0120] ∥e_x1∥=∥y^d−y∥=∥h(q^d)−h(q)∥≦L₁∥q^d−q∥=L₁∥e_q1∥

[0121] ∥e_x2∥=∥{dot over (y)}^d−{dot over (y)}∥=∥J(q^d){dot over (q)}^d−J(q){dot over (q)}∥≦L₂∥{dot over (q)}^d−{dot over (q)}∥=L₂∥e_q2∥

[0122] where L₁and L₂are constants. In summary, the following relation between joint space error and task space error can be obtained:

[0123] ∥e_x∥≦L_b∥e_q∥

[0124] where L_bis constant. Similar to the above inequality, the following relationship can also be obtained based on the Lipschitz continuity of the function q=h⁻¹(y),{dot over (q)}=J⁻{dot over (y)} in Ω₀∪Ω₁.

[0125] ∥e_q∥≦L_a∥e_x∥

[0126] Therefore, the errors in joint space and task space satisfy the following inequality in Ω₀∪Ω₁.

[0127] ∥e_q∥≦L_a∥e_x∥, ∥e_x∥≦L_b∥e_q∥

[0128] Defining the Lyapunov function of system (2.1) and system (2.2) as

[0129] V₁=½(e_q1^TK_Pqe_q1+e_q2^Te_q2)

[0130] V₂=½[e_x1^TKe_x1+(μe_x1+e_x2)^T(μe_x1+e_x2)].

[0131] Since the system (2.1) and (2.2) have been proven to be stable individually, the Lyapunov functions satisfy the inequalities

a_{1} ‖ e_{q} ‖ \leq V_{1} (e_{q}) \leq b_{1} ‖ e_{q} ‖

\frac{\partial V_{1} (e_{q})}{\partial x} f_{q} (e_{q}) \leq - c_{1} ‖ e_{q} ‖

a_{2} ‖ e_{x} ‖ \leq V_{2} (e_{x}) \leq b_{2} ‖ e_{x} ‖

\frac{\partial V_{2} (e_{x})}{\partial x} f_{x} (e_{x}) \leq - c_{2} ‖ e_{x} ‖

[0132] For an initial time t₀the following inequalities can be obtained.

[0133] V₁(e_q(t₀+τ))≦e^−λ^τV₁(e_q(t₀))

[0134] V₂(e_x(t₀+τ))≦e^−λ^τV₂(e_q(t₀))

[0135] where λ₁c₁/b₁,λ₂=c₂/b₂are positive scalars. Having set forth the above equalities, the characteristic of the Lyapunov function under switching can be studied.

[0136] In order to prove the hybrid control system is stable, it is essential to show that V₁is monotone decreasing at all odd number of switch instances, t₁,t₃,t₅. . . , and V₂is monotone decreasing at all even switching instances, t₀,t₂,t₄, . . . A typical switching sequence for the hybrid control system is shown in FIG. 4. Based on this switching sequence,

\begin{matrix} V_{1} (e_{q} (t_{1})) \leq b_{1} ‖ e_{q} (t_{1}) ‖ \leq b_{1} L_{a} ‖ e_{x} (t_{1}) ‖ \\ \leq \frac{b_{1} L_{a}}{a_{2}} V_{2} (e_{x} (t_{1})) \leq \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0})) \end{matrix}

\begin{matrix} V_{2} (e_{x} (t_{2})) \leq b_{2} ‖ e_{x} (t_{2}) ‖ \leq b_{2} L_{b} ‖ e_{q} (t_{2}) ‖ \\ \leq \frac{b_{2} L_{b}}{a_{1}} V_{1} (e_{q} (t_{2})) \leq \frac{b_{2} L_{b}}{a_{1}} ⅇ^{- λ_{1} (t_{2} - t_{1})} V_{1} (e_{q} (t_{1})) \\ \leq \frac{b_{2} L_{b}}{a_{1}} \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{1} (t_{2} - t_{1})} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0})) \end{matrix}

[0137] From the above equalities, it can be seen that

V_{2} (e_{x} (t_{2})) \leq \frac{b_{2} L_{b}}{a_{1}} \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{1} (t_{2} - t_{1})} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0}))

[0138] The values of λ₁and λ₂are related to the gain matrices K_px,K_vx,K_pqand K_vq. The gain matrices can be selected such that V₂(e_x(t₂))<V₂(e_x(t₀)). Accordingly, the switched system can be proven to be asymptotically stable as is known in the art. In the implementation of the controller, height gains and sampling frequency are chosen to ensure appropriate λ₁,λ₂. The role of δ is to avoid chattering at switching. In accordance with the definition of a Dwell Region, the value of m₇or m₈can not be changed immediately. Only when robot configuration reaches another boundary from the current one can the value of m₇or m₈change. This avoids chattering. From a different point of view, δ can also be designed to ensure V₂(e_x(t₂))<V₂(e_x(t₀)), thereby ensuring the stability of the switching mechanism.

[0139] While the invention has been described in its presently preferred form, it will be understood that the invention is capable of modification without departing from the spirit of the invention as set forth in the appended claims.

Claims

Post-issuance certificates present

The text shown below may not reflect later post-issuance changes.

What is claimed is:

1. A hybrid control system for controlling the movement of a robot, comprising:

a singularity detector operable to monitor the movement of the robot and detect robot movement in a region about a singularity configuration;

a task level controller connected to the singularity detector, the task level controller receptive of a motion plan and operable to issue a first set of control commands to the robot when robot movement occurs outside said region, where the first set of control commands are defined a task space; and

a joint level controller connected to the singularity detector, the joint level controller receptive of the motion plan and operable to issue a second set of control commands to the robot when robot movement occurs inside said region, where the second set of control commands are defined in a joint space.

2. The hybrid control system of claim 1 wherein the robot is operable at the singularity configuration.

3. The hybrid control system of claim 1 wherein the robot includes a manipulator having a plurality of joints and an end-effector, such that the manipulator is operable to position and orientate the end-effector within a reachable workspace.

4. The hybrid control system of claim 3 wherein the singularity detector is operable to monitor robot joint parameters associated with said plurality of joint, such that the robot joint parameters are indicative of robot movement in the region about the singularity configuration.

5. The hybrid control system of claim 3 wherein the manipulator further includes a three degrees of freedom forearm and a three degrees of freedom spherical wrist.

6. The hybrid control system of claim 5 wherein the singularity configuration may be selected from the group consisting of a boundary arm singular configuration, an interior arm singular configuration, and a wrist singularity configuration.

7. The hybrid control system of claim 1 wherein the task level controller is further defined by

u₂=DJ₀⁻¹(Ÿ^d−{dot over (J)}_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c(q,{dot over (q)})+g(q)

where u is a vector of applied torques; q is a vector denoting joint displacements of the robot; D is an inertia matrix for the robot; J_ois a Jacobian matrix which defines the relationship between the joint space and the task space; Y is a vector denoting the position and orientation of the end-effector of the robot in task space; Y^d,{dot over (Y)}^d,Ÿ^ddenotes a desired robot task expressed in task space; K_Vxand K_Pxare feedback gain matrices; c(q,{dot over (q)}) is centripetal and coriolis terms; and g(q) is a gravity term.

8. The hybrid control system of claim 1 wherein the joint level controller is further defined by

u₁=D(q)({umlaut over (q)}^d+K_v({dot over (q)}^d−{dot over (q)})+K_P(q^d−q))+c(q,{dot over (q)})+g(q)

where u is a vector of applied torques; q is a vector denoting of joint displacements of the robot; D(q) is an inertia matrix for the robot; q^d,{dot over (q)}^d,{umlaut over (q)}^ddenotes a desired robot task expressed in the joint space; K_Vqand K_Pqare feedback gain matrices; c(q,{dot over (q)}) is centripetal and coriolis terms; and g(q) is a gravity term.

9. The hybrid control system of claim 1 wherein the task level controller operable to use a pseudoinverse jacobian matrix to determine the first set of control commands along a boundary of said region.

10. A hybrid control system for controlling the movement of a robot manipulator, the manipulator having a plurality of joints and an end-effector, comprising:

a singularity detector operable to detect movement of the manipulator in a first region proximate to a singularity configuration and further operable to detect movement of the manipulator in a second region proximate to the singularity configuration, where the second region is a subset of the first region;

a task level controller connected to the singularity detector, the task level controller receptive of a motion plan and operable to issue a first set of control commands to the manipulator when manipulator movement is outside of the first region, where the first set of control commands are defined in a task space using an inverse Jacobian matrix;

said task level controller further operable to issue a second set of control commands to the manipulator when manipulator movement is inside the first region but outside of the second region, where the second set of control commands are defined in the task space using a pseudoinverse Jacobian matrix; and

a joint level controller connected to the singularity detector, the joint level controller receptive of the motion plan and operable to issue a third set of control commands to the manipulator when manipulator movement is inside said second region, where the third set of control commands are defined in a joint space.

11. The hybrid control system of claim 10 wherein the robot manipulator is operable at the singularity configuration.

12. The hybrid control system of claim 10 wherein the singularity detector is operable to monitor joint parameters associated with said plurality of joint, such that the joint parameters are indicative of manipulator movement proximate to the singularity configuration.

13. The hybrid control system of claim 10 wherein the pseudoinverse Jacobian matrix is computed using a damped least squares technique.

14. The hybrid control system of claim 10 wherein the task level controller is further defined by

u₃=DJ_o^#(−Ÿ^d+J_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c+g

where u is a vector of applied torques; q is a vector denoting joint displacements of the manipulator; D(q) is an inertia matrix for the manipulator; J_ois a Jacobian matrix which defines the relationship between the joint space and the task space; J_o^# is the pseudoinverse Jacobian matrix; Y is a vector denoting the position and orientation of the end-effector in task space; Y^d,{dot over (Y)}^d,Ÿ^ddenotes a desired robot task expressed in task space; K_Vxand K_Pxare feedback gain matrices; c(q,{dot over (q)}) is centripetal and coriolis terms; and g(q) is a gravity term.

15. The hybrid control system of claim 14 wherein the pseudoinverse Jacobian matrix, J_o^#, is further defined as

J_w^#=(J_w·J_w^T+·m_s1(t))⁻¹·J_w^T

where J_wis the Jacobian matrix, J_w^Tis a modified Jacobian matrix computed in accordance with a damped least squares technique, and m_s1(t) is a vector of variable damping factors.

16. The hybrid control system of claim 15 wherein the variable damping factors are defined to be zero when manipulator movement is out side of the first region.

17. The hybrid control system of claim 10 wherein the joint level controller is further defined by

u₁=D(q)({umlaut over (q)}^d+K_v({dot over (q)}^d−{dot over (q)})+K_P(q^d−q))+c(q,{dot over (q)})+g(q)

where u is a vector of applied torques; q is a vector denoting of joint displacements of the manipulator; D(q) is an inertia matrix for the manipulator; q^d,{dot over (q)}^d,{umlaut over (q)}^ddenotes a desired robot task expressed in the joint space; K_Vqand K_Pqare feedback gain matrices; c(q,{dot over (q)}) is centripetal and coriolis terms; and g(q) is a gravity term.

18. A method for controlling the movement of a robot using a hybrid motion control system, comprising the steps of:

providing a robot manipulator having a plurality of joints and an end-effector, such that the manipulator is operable to position and orientate the end-effector within a workspace;

monitoring movement of the manipulator;

detecting manipulator movement in a first region proximate to a singularity configuration associated with the manipulator;

issuing a first set of control commands to the manipulator when manipulator movement falls outside of the first region, where the first set of control commands are defined in a task space; and

issuing a second set of control commands to the manipulator when manipulator manipulator falls within the first region, where the second set of control commands are defined in a joint space.

19. The method of claim 18 further comprises the step of detecting manipulator movement in a second region proximate to the singularity configuration, where the first region is a subset of the second region.

20. The method of claim 19 wherein the step of issuing a first set of control commands further comprises using an inverse Jacobian matrix to determine the first set of control commands when manipulator movement falls outside of the second region and using a pseudoinverse Jacobian matrix when the manipulator movement falls inside the second region but outside of the first region.

Patent Citations (18)

Patent	Date	Inventor	Cited By
US36929(A)	1862-11-01	Workman	Applicant
US4356440(A)	1982-10-01	Curtiss et al.	Examiner
US4621332(A)	1986-11-01	Sugimoto et al.	Examiner
US4987356(A)	1991-01-01	Yamada et al.	Examiner
US5265295(A)	1993-11-01	Jinno et al.	Examiner
US5276390(A)	1994-01-01	Fisher et al.	Applicant
US5442269(A)	1995-08-01	Takayama et al.	Applicant
US5509847(A)	1996-04-01	Jinno et al.	Examiner
US5579442(A)	1996-11-01	Kimoto et al.	Applicant
US5590034(A)	1996-12-01	Snell	Applicant
US5646493(A)	1997-07-01	Hara et al.	Applicant
US5880956(A)	1999-03-01	Graf	Applicant
US6039290(A)	2000-03-01	Wie et al.	Applicant
US6067862(A)	2000-05-01	Murray et al.	Examiner
US6092004(A)	2000-07-01	Harima	Applicant
US6181983(B1)	2001-01-01	Schlemmer et al.	Applicant
US6204620(B1)	2001-03-01	McGee et al.	Applicant
US6278906(B1)	2001-08-01	Piepmeier et al.	Examiner

Non-Patent Literature (24)

Osumi et al., Cooperative control of two industrial robots with force control devices, 1995, IEEE, pp. 550-555.Applicant
Egeland et al., A note Lyapunov stability for an adaptive robot controller, 1994, IEEE, pp. 1671-1673.Applicant
Fjellstad et al., Singularity-free tracking of unmanned underwater vehicles in 6 DOF, 1994, IEEE, pp. 1128-1133.Applicant
Osumi et al., Cooperative control between multiple manipulators with flexibility, 1993, IEEE, pp. 1935-1940.Applicant
A. K. Bejczy, S. Lee, “Robot Arm Dynamic Model Reduction for Control,” Proceedings of IEEE Conference on Decision and Control, San Antonio, Tex, pp. 1466-1476, 1983.Applicant
B. E. Bishop, M. W. Spong, “Control of Redundant Manipulators Using Logic-Based Switching,” Proceedings of IEEE Conference on Decision and Control, Leuven, Belgium, pp2664-2669, May 1998.Applicant
M. S. Branicky, “Multiple Lyapunov Functions and Other Analysis Tools for Switched and Hybrid Systems,” IEEE Transactions on Automatic Control, vol. 43, No. 4, 1998, pp. 475-482.Applicant
F. Cheng, T. Hour, Y. Sun, T. H. Chen, “Study and Rosolution of Singularities for a 6 DOF PUMA Manipulators,” IEEE Transactions on systems, Man and Cybernetics B: Cybernetics, vol.27, No.2, Apr. 1997, pp. 332-343.Applicant
S. Chiaverini, B. Siciliano, O. Egeland, “Review of the Damped Least-Squares Inverse Kinematics with Experiments on an Industrial Robot Manipulator,” IEEE Trans. Control System Technology, vol.2, No.2, Jun. 1994, pp. 123-134.Applicant
Fabrizio Caccavale, Setfano Chiaverini, and Bruno Siciliano, “Second-Order Kinematic Control of Robot Manipulators with Jacobian Damped Least-Squares Inverse: Theory and Experiments,” IEEE/ASME Transactions on Mechatronics, vol.2, No.3, Sep., 1997, pp. 188-194.Applicant
J. Kieffer, “Manipulator Inverse Kinematics for Untimed End-Effector Trajectories With Ordinary Singularities,” The International Journal of Robotics Research, vol., 11, Jun. 1992, pp. 225-237.Applicant
J. Kieffer, “Differential Analysis of Bifurcations and Isolated Singualarities for Robots and Mechanisms,” IEEE Transactions on Robotics and Automations, vol.10, No.1, Feb. 1994, pp1-10.Applicant
M. Kircanski, N. Kircanshi, D. Lekovic, M. Vukobratovic, “An Experimental Study of Resolved Acceleration Control of Robots at Singularities: Damped Least-Squares Approach,” Journal of Dynamic Systems, Measurement, and Control, vol.119, Mar., 1997, pp. 97-101.Applicant
Y. Nakamura, H. Hanafusa, “Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control,” Journal of Dynamic Systems, Measurement, and Control, vol. 108, Sep., 1986, pp. 163-171.Applicant
D.N. Nenchev, M. Uchiyama, “Singularity-Consistent Path Tracking: A Null Space Based Approach,” International Journal of Robotics Research, vol.14, No.4, 1995, pp. 399-404.Applicant
K. O'Neil, Y. C. Chen, “Instability of Pseudoinverse Acceleration Control of Redundant Mechanisms”, Proceedings of IEEE International Conference on Robotics and Automation, San Francisco, Apr. 2000, pp. 2575-2582.Applicant
P. Peleties, R. A. Decarlo, “Asymptotic Stability of m-switched Systems Using Lyapunov-like Functions,” Proceeding of American Control Conference, 1991, pp. 1679-1684.Applicant
M. Sampei and K. Furuata, “Robot Control in the Neighborhood of Singular Points,” IEEE Journal Robotics and Automation, vol. 14, No. 3, Jun. 1988, pp. 303-309.Applicant
T. J. Tarn, A. K. Bejczy, G. T. Marth, A. K. Ramadorai, “Performance Comparison of Four Manipulator Servo Schemes,” IEEE Control Systems Magazine vol. 13, Feb. 1993, pp. 22-29.Applicant
Krzysztof Tochon, “Calibration of Manipulator Kinematics: A Singularity Theory Approach”, IEEE Transactions on Robotics and Automation, vol. 8, No. 5, Oct. 1992, pp. 671-678.Applicant
Krzysztof Tochon, “A Normal Form of Singular Kinematics of Robot Manipulators with Smallest Degeneracy”, IEEE Transactions on Robotics and Automation, vol. 11, No. 3, Jun. 1995, pp. 401-404.Applicant
Krzysztof Tochon, “Quadratic Normal Forms of Redundant Robot Kinematics with Application to Singularity Avoidance”, IEEE Transactions on Robotics and Automation, vol. 14, No. 5, Oct. 1998, pp. 834-387.Applicant
Krzysztof Tochon, R. Muszynski, “Singular Inverse Kinematic Problem for Robotic Manipulators: A Normal Form Approach,” IEEE Transactions on Robotics and Automation, vol. 14, No. 1, Oct. 1998, pp. 93-104.Applicant
Tzyh-Jong Tarn, Yunying Wu, Xing Xi, A. Isidori, “Force Regulation and Contact Transition Control,” IEEE Control Systems Magazine, vol. 16, Feb. 1996, pp. 32-40.Applicant

Abstract

Description

Background of the Invention

Summary of the Invention

[0010] For a more complete understanding of the invention, its objects and advantages, reference may be had to the following specification and to the accompanying drawings.

Brief Description of the Drawings

[0011] FIG. 1 is a block diagram of a hybrid motion control system in accordance with the present invention;

[0012] FIG. 2 is a diagram illustrating an exemplary robot workspace about a singular configuration in accordance with the present invention;

[0013] FIG. 3 is a block diagram of a first preferred embodiment of the hybrid motion control system of the present invention; and

[0014] FIG. 4 illustrates a typical switching sequence for the hybrid control system of the present invention.

Description of the Preferred Embodiment

[0017] The dynamic model for a nonredundant robot arm can be written as

[0018] D(q){umlaut over (q)}+c(q,{dot over (q)})+g(q)=u

[0020] For a robot task given in joint space, denoted by q^d,{dot over (q)}^d,{umlaut over (q)}^da joint level robot controller in joint space can be derived by

[0021] u₁=D(q)({umlaut over (q)}^d+K_v({dot over (q)}^d−{dot over (q)})+K_P(q^d−q))+c(q,{dot over (q)})+g(q) (1.1)

[0022] where K_Vqand K_Pqare feedback gain matrices. Given e_q1=q^d−q,e_q2={dot over (q)}^d−{dot over (q)}, the error dynamics for this controller can be described by

[0023] {dot over (e)}_q1=e_q2

[0024] {dot over (e)}_q2=−K_Pqe_q1−K_Vqe_q2.

[0027] DJ₀⁻¹(Ÿ−J_o{dot over (q)})+c(q,{dot over (q)})+g(q)=u (1.2)

[0029] u₂=DJ₀⁻¹(Ÿ^d−J_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c(q,{dot over (q)})+g(q) (1.3)

\begin{matrix} J_{o} = [\begin{matrix} I & U \\ 0 & I \end{matrix}] [\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}] [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}] \\ = [\begin{matrix} I & U \\ 0 & I \end{matrix}] [\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}] J_{w} \end{matrix}

J_{w} [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}], Ψ = [\begin{matrix} - \frac{s_{o} s_{A}}{c_{A}} & \frac{c_{o} s_{A}}{c_{A}} & 1 \\ - c_{o} & - s_{o} & 0 \\ - \frac{s_{o}}{c_{A}} & \frac{c_{o}}{c_{A}} & 0 \end{matrix}], U = [\begin{matrix} 0 & d_{6} \cdot e_{k} & - d_{6} \cdot e_{j} \\ - d_{6} \cdot e_{k} & 0 & d_{6} \cdot e_{i} \\ d_{6} \cdot e_{j} & - d_{6} \cdot e_{i} & 0 \end{matrix}], J_{11} = [\begin{matrix} - d_{4} s_{1} s_{23} - a_{1} s_{1} c_{2} - a_{3} s_{1} c_{23} - d_{2} c_{1} & d_{4} c_{1} c_{23} - a_{2} c_{1} s_{2} - a_{3} c_{1} s_{23} & c_{1} (d_{4} c_{23} - a_{3} s_{23}) \\ d_{4} c_{1} s_{23} + a_{1} c_{1} c_{2} + a_{3} c_{1} c_{23} - d_{2} s_{1} & d_{4} s_{1} c_{23} - a_{2} s_{1} s_{2} - a_{3} s_{1} s_{23} & s_{1} (d_{4} c_{23} - a_{3} s_{23}) \\ 0 & - d_{4} s_{23} - a_{2} - a_{3} c_{23} & - d_{4} s_{23} - a_{3} c_{23} \end{matrix}], J_{21} = [\begin{matrix} 0 & - s_{1} & - s_{1} \\ 0 & - c_{1} & - c_{1} \\ 1 & 0 & 0 \end{matrix}], J_{22} = [\begin{matrix} c_{1} s_{23} & - c_{1} c_{23} s_{4} - s_{1} c_{4} & c_{1} c_{4} c_{23} s_{5} - s_{1} s_{4} s_{5} + c_{1} s_{23} c_{5} \\ s_{1} s_{23} & - s_{1} c_{23} s_{4} - c_{1} c_{4} & s_{1} c_{4} c_{23} s_{5} + c_{1} s_{4} s_{5} + s_{1} s_{23} c_{5} \\ c_{23} & s_{23} s_{4} & c_{23} c_{5} - s_{23} c_{4} s_{5} \end{matrix}], \begin{matrix} e_{i} = c_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) - s_{1} s_{4} s_{5}, \\ e_{j} = s_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) + c_{1} s_{4} s_{5}, \\ e_{k} = c_{23} c_{5} - s_{23} c_{4} s_{5} . \end{matrix}

[0036] γ_i=d₄s₂₃+a₂c₂+a₃c₂₃=0.

[0041] Ω₁={qεR⁶|α_b≦|γ_b|≦β_b∪α_i|γ_i|≦|γ_wβ_i∪α_w≦|γ_w|≦β_w}

[0042] Ω₂={qεR⁶∥γ_b|<α_b∪|γ_i|<α_i∪|γ_w<α_w}

[0047] D(q){umlaut over (q)}+C(q,{dot over (q)})+g(q)=u

[0048] DJ_o⁻¹(Ÿ−{dot over (J)}_o{dot over (q)})+c(q,{dot over (q)})+g(q)=u

[0049] m(t)=f(Y(t),q(t),m(t⁻))

[0050] and the hybrid robot motion controller is given by

[0051] u(t)=f(Y(t),q(t),m(t)).

[0054] The inverse Jacobian J_o⁻¹can be decomposed into

J_{o}^{- 1} = J_{w}^{- 1} \cdot {[\begin{matrix} I & 0 \\ 0 & Ψ \end{matrix}]}^{- 1} \cdot {[\begin{matrix} I & U \\ 0 & I \end{matrix}]}^{- 1},

[0055] where

J_{w} = [\begin{matrix} J_{11} & 0 \\ J_{21} & J_{22} \end{matrix}] .

[0056] In the neighborhood of singular configurations, J_wis ill-conditioned. However, the inverse of J_wcan be computed using a damped least squares technique, thereby yielding

[0057] J_w^#=(J_w·J_w^T+·m_s1(t))⁻¹·J_w^T, (1.4)

[0058] where m_s1(t) is a matrix of variable damping factors. The inverse of J_othat is based on J_w^#is called J_o^#.

[0060] Based on the analysis of the inverse of J_w, m_s1is defined as follows:

\begin{matrix} m_{s1} (t) = diag ([m_{1} (t) m_{2} (t) m_{3} (t) m_{4} (t) m_{5} (t) m_{6} (t)]) \\ = [\begin{matrix} k_{i} \oplus 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & k_{i} \oplus k_{b} \oplus 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & k_{b} \oplus 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & k_{w} \oplus 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & k_{w} \oplus 0 \end{matrix}] \end{matrix}

[0062] u₃=DJ_o^#(Ÿ^d+J_o{dot over (q)}+K_Vxe_x2+K_Pxe_x1)+c+g (1.5)

[0064] {dot over (e)}_x1=e_x2

[0065] {dot over (e)}_x2=(I−J_oJ_o^#)(Ÿ^d−{dot over (J)}{dot over (q)})−J_oJ_o^#(K_Pxe_x1+K_Ve_x2)

[0066] To analyze the stability of this system, the term J_oJ_o^# is a key component. It can be simplified by the singular value decomposition (SVD) of Jacobian matrix, J_w, as follows

J_{w} = \sum_{i = 1}^{6} σ_{i} u_{i} v_{i}^{T} = U \cdot \sum_{1} \cdot V^{T} .

[0067] Therefore, equation (1.4) can be expressed by

J_{w}^{#} = \sum_{i = 1}^{6} \frac{σ_{i}}{σ_{i}^{2} + m_{i}} v_{i} u_{i}^{T} = V^{T} \cdot \sum_{2} \cdot U

\begin{matrix} k = J_{o} J_{o}^{#} \\ = diag [\frac{σ_{1}^{2}}{σ_{1}^{2} + m_{1}} \frac{σ_{2}^{2}}{σ_{2}^{2} + m_{2}} \frac{σ_{3}^{2}}{σ_{3}^{2} + m_{3}} \frac{σ_{4}^{2}}{σ_{4}^{2} + m_{4}} \frac{σ_{5}^{2}}{σ_{5}^{2} + m_{5}} \frac{σ_{6}^{2}}{σ_{6}^{2} + m_{6}}] \end{matrix}

[0069] By defining

k_{\min} = \underset{i = 1}{\min^{6}} {\frac{σ_{i}^{2}}{σ_{i}^{2} + m_{i}}},

[0071] {dot over (e)}_x1=e_x2

[0072] {dot over (e)}_x2=−K_Pxe_x1−K_Vxe_x2 (1.6)

[0073] Accordingly, the modified task level controller is asymptotically stable in subspace Ω₀for appropriate gain matrices.

[0074] While in subspace Ω₁, the error dynamics of the system can be described by

[0075] {dot over (e)}_x1=e_x2

[0076] {dot over (e)}_x2=(I=k)(Ÿ^d−{dot over (J)}{dot over (q)})−k(K_Pxe_x1+K_Vxe_x2)

[0077] Defining the candidate Lyapunov function of equation (1.6) as

[0078] V=½[e_x1^TKe_x1+(μe_x1+e_x2)^T(μe_x1+e_x2)]

[0079] where K is positive definite matrix and μ is positive constant. The derivative of the candidate Lyapunov Function can be derived by

[0080] {dot over (V)}=e_x1^TKe_x2+(μe_x1+e_x2)^T

[0081] (μe_x2+(I−k)(Ÿ^d−{dot over (J)}{dot over (q)})−

[0082] k(K_Pxe_x1−K_Vxe_x2))=−μe_x1^TkK_Pxe_x1−

[0083] e_x2^T(kK_Vx−μI)e_x2+(μe_x1+e_x2)^T

[0084] (I−k)(Ÿ^d−{dot over (J)}{dot over (q)})≦−μe_x1^Tk_minK_Pxe_x1−

[0085] e_x2^T(k_minK_Vx−μI)e_x2+(1−k_min)·∥e_x∥η{square root over (1+μ²)}=

[0086] −k_minλ_min∥e_x∥+(1−k_min)·∥e_x∥η{square root over (1+μ²)}=−

[0087] (k_minλ_min−(1−k_{min)·η{square root over (1+μ}_x∥

[0090] {dot over (e)}_x1i=e_x2i

[0091] {dot over (e)}_x2i=Ÿ_i^d−({dot over (J)}{dot over (q)})_i

[0095] (m₇(t)=m₇(t⁻)^sgn(α^+δ−γ^)⊕sgn(α^+δ−γ^)⊕0⊕sgn((α_i−γ_i)⊕sgn((α_b−γ_b)⊕0

[0096] m₈(t)=m₈(t⁻)^sgn(α^+δ−γ^)⊕0⊕sgn(α_w−γ_w)⊕0.

[0098] A switching matrix, m_s2, is based on m₇and m₈, as follows:

m_{s2} (t) = [\begin{matrix} m_{7} (t) I_{3 \times 3} & 0 \\ 0 & (m_{7} (t) \oplus m_{8} (t)) I_{3 \times 3} \end{matrix}] .

[0099] This switching matrix is then used to form a hybrid motion control system.

[0100] In summary, the hybrid motion control system of the present invention can be represented by

[0101] u=(I−m_s2)u₁+m_s2u₃

[0106] min{w₁|Y^d−h(q^d)|+w₂|q^d−q|}q^d

[0107] min{|{dot over (Y)}^d−J{dot over (q)}^d|}{dot over (q)}^d≦{dot over (q)}max,{dot over (q)}^d

[0108] where w₁and w₂are positive weight factors.

e_{q} = (\begin{matrix} e_{q1} \\ e_{q2} \end{matrix}) = (\begin{matrix} q^{d} - q \\ {\dot{q}}^{d} - \dot{q} \end{matrix}), e_{x} = (\begin{matrix} e_{x1} \\ e_{x2} \end{matrix}) = (\begin{matrix} y^{d} - y \\ {\dot{y}}^{d} - \dot{y} \end{matrix}) .

[0112] The error dynamics of the manipulator in joint space can be written by

[0113] {dot over (e)}_q1=e_q2

[0114] {dot over (e)}_q2=−K_Pqe_q1−K_Vqe_q2 (2.1)

[0115] or {dot over (e)}_q=f_q(e_q). The error dynamic in the task space is

[0116] {dot over (e)}_x1=e_x2

[0117] {dot over (e)}_x2=(I−k)(ÿ^d−J{dot over (q)})−k(K_Pxe_x1+K_Vxe_x2) (2.2)

[0119] It is easy to prove that y=h(q) and {dot over (y)}=J{dot over (q)} are globally Lipschitz continuous in their defined domain. Thus the following relationship can be obtained

[0120] ∥e_x1∥=∥y^d−y∥=∥h(q^d)−h(q)∥≦L₁∥q^d−q∥=L₁∥e_q1∥

[0121] ∥e_x2∥=∥{dot over (y)}^d−{dot over (y)}∥=∥J(q^d){dot over (q)}^d−J(q){dot over (q)}∥≦L₂∥{dot over (q)}^d−{dot over (q)}∥=L₂∥e_q2∥

[0122] where L₁and L₂are constants. In summary, the following relation between joint space error and task space error can be obtained:

[0123] ∥e_x∥≦L_b∥e_q∥

[0125] ∥e_q∥≦L_a∥e_x∥

[0126] Therefore, the errors in joint space and task space satisfy the following inequality in Ω₀∪Ω₁.

[0127] ∥e_q∥≦L_a∥e_x∥, ∥e_x∥≦L_b∥e_q∥

[0128] Defining the Lyapunov function of system (2.1) and system (2.2) as

[0129] V₁=½(e_q1^TK_Pqe_q1+e_q2^Te_q2)

[0130] V₂=½[e_x1^TKe_x1+(μe_x1+e_x2)^T(μe_x1+e_x2)].

[0131] Since the system (2.1) and (2.2) have been proven to be stable individually, the Lyapunov functions satisfy the inequalities

a_{1} ‖ e_{q} ‖ \leq V_{1} (e_{q}) \leq b_{1} ‖ e_{q} ‖

\frac{\partial V_{1} (e_{q})}{\partial x} f_{q} (e_{q}) \leq - c_{1} ‖ e_{q} ‖

a_{2} ‖ e_{x} ‖ \leq V_{2} (e_{x}) \leq b_{2} ‖ e_{x} ‖

\frac{\partial V_{2} (e_{x})}{\partial x} f_{x} (e_{x}) \leq - c_{2} ‖ e_{x} ‖

[0132] For an initial time t₀the following inequalities can be obtained.

[0133] V₁(e_q(t₀+τ))≦e^−λ^τV₁(e_q(t₀))

[0134] V₂(e_x(t₀+τ))≦e^−λ^τV₂(e_q(t₀))

[0135] where λ₁c₁/b₁,λ₂=c₂/b₂are positive scalars. Having set forth the above equalities, the characteristic of the Lyapunov function under switching can be studied.

\begin{matrix} V_{1} (e_{q} (t_{1})) \leq b_{1} ‖ e_{q} (t_{1}) ‖ \leq b_{1} L_{a} ‖ e_{x} (t_{1}) ‖ \\ \leq \frac{b_{1} L_{a}}{a_{2}} V_{2} (e_{x} (t_{1})) \leq \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0})) \end{matrix}

\begin{matrix} V_{2} (e_{x} (t_{2})) \leq b_{2} ‖ e_{x} (t_{2}) ‖ \leq b_{2} L_{b} ‖ e_{q} (t_{2}) ‖ \\ \leq \frac{b_{2} L_{b}}{a_{1}} V_{1} (e_{q} (t_{2})) \leq \frac{b_{2} L_{b}}{a_{1}} ⅇ^{- λ_{1} (t_{2} - t_{1})} V_{1} (e_{q} (t_{1})) \\ \leq \frac{b_{2} L_{b}}{a_{1}} \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{1} (t_{2} - t_{1})} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0})) \end{matrix}

[0137] From the above equalities, it can be seen that

V_{2} (e_{x} (t_{2})) \leq \frac{b_{2} L_{b}}{a_{1}} \frac{b_{1} L_{a}}{a_{2}} ⅇ^{- λ_{1} (t_{2} - t_{1})} ⅇ^{- λ_{2} (t_{1} - t_{0})} V_{2} (e_{x} (t_{0}))