Abstract:
A method for controlling a robot having at least one visual sensor. A target for a motion of the robot is defined. A motion control signal adapted for the robot reaching the target is calculated. A collision avoidance control signal based on the closest points of segments of the robot and a virtual object between the visual sensing means and the target is calculated. The motion control signal and the collision avoidance control signal are weighted and combined. The weight of the motion control signal is higher when a calculated collision risk is lower. The motion of the robot is controlled according to the combined signal so that no segment of the robot enters the space defined by the virtual object.

Description:
RELATED APPLICATIONS 
       [0001]    This application claims priority under 35 U.S.C. § 119(a) to European Patent Application number 07 104 820, filed on Mar. 23, 2007, which is incorporated by reference herein in its entirety. This application is related to U.S. patent application Ser. No. 12/052,483, filed on Mar. 20, 2008 (Attorney Dkt No.: 23077-13969); and U.S. patent application Ser. No. 12/050,948, filed on Mar. 19, 2008 (Attorney Dkt No.: 23077-13970), which are incorporated by reference herein in their entirety. 
     
    
     FIELD OF THE INVENTION 
       [0002]    The present invention generally relates to robots, and specifically to robots acting autonomously. 
       BACKGROUND OF THE INVENTION 
       [0003]    When controlling a trajectory of an effector such as a robot, a target state needs to be defined. The target state is, for example, defined as an object that is to be handled by a manipulating arm of a robot. In general, the position of the object can be described by three parameters. In addition to the object position, it is necessary to describe a spatial orientation which is often described by Kardan-angles or Euler-angles. 
         [0004]    To carry out the movement of an effector in a robot, the trajectory is usually generated by mapping increments from a control parameter space to configuration space. 
         [0005]    The control parameter space or task space is the space of the command elements. The control parameter space is composed of the command elements. The command (also “target” or “task”) elements are the elements of a respective command vector. These elements define a useful description of what should be controlled, for example, the position of a hand or the tilting angle of a head. The configuration space is the space of controllable degrees of freedom. The configuration space can be composed of individual joints of a robot and/or more complex kinematics mechanisms to which controllable degrees of freedom can be assigned. 
         [0006]    “Null space” represents the dimensional difference between the joint space and the task space. The null space contains the redundant degrees of freedom, in which movements can be carried out without affecting the task space motion. The null space is defined as the set of arguments of a linear operator such that the corresponding function value is zero. Redundant systems have a (local) null space that can be used to address a secondary objective, such as kinematic conditioning, without disturbing a primary task. 
         [0007]    Conventionally, targets of robots were provided by operators and the robots merely tracked planned trajectories. The safety mechanism in conventional robots is an emergency stop mechanism that simply freezes motions, if any motions were being performed. Modern robots, in particular humanoid robots, are expected to work outside the typical environment such as factories. The modern robots need to interact with dynamic environment that is less predictable. The advantage of using the collision avoidance is not only safety. The collision avoidance does not necessarily stop the robot&#39;s target reaching motions and may expand the robot&#39;s working range. 
         [0008]    The known collision avoidance mechanism may be divided into two categories. One category of collision avoidance mechanism is a planning (non real-time) method which generates trajectories taking obstacles into account. 
         [0009]    James Kuffner et al., “Self-collision detection and prevention for humanoid robots,” In proceedings of the IEEE International Conference on Robotics and Automation, 2002, which is incorporated by reference herein in its entirety, proposes a collision avoidance on a humanoid robot. This document proposes a fast collision detection method and a real-time planning for gait generations taking leg interference into account. 
         [0010]    James Kuffner et al., “Dynamically-stable motion planning for humanoid robots,” Autonomous Robots, volume 12, pages 105-118, 2002, which is incorporated by reference herein in its entirety, proposes a dynamics based collision free planning method using Rapidly exploring Random Trees (RRTs). However, these methods are difficult to apply to interactive motions because the computation time becomes longer as the degrees of freedom of robots (e.g., humanoid robots) increase. 
         [0011]    Another category of the collision avoidance is reactive (real-time) collision avoidance. The reactive collision avoidance modifies trajectories that are quite simple such as line segments connecting current positions and target positions. 
         [0012]    In order to use this method, the direction of avoidance and how to switch the priority between target reaching motions and collision avoidance motions depending on the magnitude of danger of collisions in real-time must be decided. For instance, if the distance between segments is large enough, target reaching motions should have higher priority than collision avoidance motions. 
         [0013]    Null space optimization criteria may be used for collision avoidance (see, for example, Michael Gienger, Herbert Janssen, and Christian Goerick, “Task-oriented whole body motion for humanoid robots,” In proceedings of the IEEERAS International Conference on Humanoid Robots, 2005; and Michael Gienger, Herbert Janssen, and Christian Goerick, “Exploiting task intervals for whole body robot control,” In proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006, which are incorporated by reference herein in their entirety). 
         [0014]    A particular case of collision avoidance is occlusion avoidance. When a robot arm moves towards a target that is captured by its visual sensors (e.g., cameras), sometimes an arm segment blocks the target by its own movements. This sometimes causes oscillations. For example, if the robot loses the target, it retracts the arm. After the target reappears and the robot again reaches for the target. 
       SUMMARY OF THE INVENTION 
       [0015]    It is an object of the present invention to provide an occlusion avoidance technique for a robot having visual sensors. 
         [0016]    Embodiments of the present invention set up a virtual object between cameras and targets. Then a real-time collision avoidance control is performed taking into account the virtual object. The robot&#39;s segments such as arms will not penetrate a space defined by the virtual object. Thus, the trajectory of the arm moves to the target while avoiding the space defined by the virtual object. When the hand comes close to a target, the space defined by the virtual object becomes smaller. 
         [0017]    The motion control calculation includes the step of mapping a collision avoidance motion in null space and a target reaching motion in task space. Thus, the motion control calculation takes into account collision avoidance in the null space. 
         [0018]    One embodiment of the present invention relates to controlling a robot having at least one visual sensor. The target of a motion of the robot is defined. A motion control signal adapted for the robot reaching the target is then calculated. A collision avoidance control signal based on the closest points of segments of the robot and a virtual object between the visual sensor and the target are calculated. The motion control signal and the collision avoidance control signal are combined. The weight of the motion control output signal is higher when a calculated collision risk is lower. A motion of the robot is controlled according to the combined signal such that no segment of the robot enters the space defined by the virtual object. 
         [0019]    In one embodiment of the present invention, the virtual object may be symmetrical around an axis connecting the center of the visual sensor and the target center position. 
         [0020]    In one embodiment of the present invention, the virtual object may define a space having the contours of a three dimensional ellipsoid. 
         [0021]    In one embodiment of the present invention, the weights can be changed gradually in order to perform a soft task switching between target reaching by motion and collision avoidance. 
         [0022]    In one embodiment of the present invention, the motion control calculation includes the step of mapping a collision avoidance motion in the null space and a target reaching motion in the task space. 
         [0023]    In one embodiment of the present invention, the weight of the collision avoidance output signal can be zero as long as the distance between the closest points is larger than a preset avoidance threshold distance. 
         [0024]    In one embodiment of the present invention, the method comprises the steps of carrying out a collision avoidance control. The collision avoidance control is achieved by calculating two closest points of different segments of the robot connected to each other via at least one joint or two closest points between a segment of the robot and another object. Then the collision avoidance motion of the robot is controlled in the direction parallel along a line connecting between the two closest points. 
         [0025]    In one embodiment of the present invention, the null space of the collision avoidance control may be used for a motion associated with reaching the target. 
         [0026]    In one embodiment of the present invention, the collision avoidance control may map a collision avoidance motion to the task space and a target reaching motion to the null space. 
         [0027]    One embodiment of the present invention also relates to a computer program product, implementing the above technique. 
         [0028]    One embodiment of the present invention relates to a robot having visual sensor and a motion control unit. The motion control unit of the robot includes a distance computing module, a motion control module, a collision avoidance module, a blending control unit for combining the output control signals of the motion control module and the collision avoidance control module to generate a motion control signal for the robot. The distance computing module calculates the two closest points between a segment of the robot and a virtual object defining a space between the visual sensor and a target. The motion control module calculates a motion control signal commanding a robot to reach a target. The collision avoidance module calculates a collision avoidance control signal based on the closest points of the segment of the robot and the virtual object. The blending control unit assigns weights to the motion control signal and the collision avoidance control signal to generate weighted motion control signal and weighted collision avoidance signal. The weight of the motion control signal is assigned a higher value when the risk of collision is lower. The weight of the collision avoidance control signal assigned a higher value when the risk of collision is higher. The blending control unit also combines the weighted motion control signal and the weighted collision avoidance control signal to generate a combined weighted signal according to which a motion of the robot is controlled. 
         [0029]    In one embodiment of the present invention, the robot is a humanoid biped robot. 
         [0030]    In one embodiment of the present invention, the technique is implemented on a humanoid robot such as a bipedal walking robot. The arm may avoid occlusion of the visual sensor of the robot. 
         [0031]    The features and advantages described in the specification are not all inclusive and, in particular, many additional features and advantages will be apparent to one of ordinary skill in the art in view of the drawings, specification, and claims. Moreover, it should be noted that the language used in the specification has been principally selected for readability and instructional purposes, and may not have been selected to delineate or circumscribe the inventive subject matter. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0032]    The teachings of the present invention can be readily understood by considering the following detailed description in conjunction with the accompanying drawings. 
           [0033]      FIG. 1  is a diagram illustrating a humanoid robot having seventeen (17) segments where each segment comprising one or more sphere swept-lines or spheres, according to one embodiment of the present invention. 
           [0034]      FIG. 2  is a diagram illustrating a collision avoidance coordinate system, according to one embodiment of the present invention. 
           [0035]      FIG. 3  is a block diagram illustrating control modules of a robot, according to one embodiment of the present invention. 
           [0036]      FIG. 4  illustrates virtual force as a function of the closest distance, according to one embodiment of the present invention. 
           [0037]      FIG. 5  illustrates occlusion avoidance, according to one embodiment of the present invention. 
       
    
    
     DETAILED DESCRIPTION OF THE INVENTION 
       [0038]    A preferred embodiment of the present invention is now described with reference to the figures where like reference numbers indicate identical or functionally similar elements. 
         [0039]    Reference in the specification to “one embodiment” or to “an embodiment” means that a particular feature, structure, or characteristic described in connection with the embodiments is included in at least one embodiment of the invention. The appearances of the phrase “in one embodiment” in various places in the specification are not necessarily all referring to the same embodiment. 
         [0040]    Some portions of the detailed description that follows are presented in terms of algorithms and symbolic representations of operations on data bits within a computer memory. These algorithmic descriptions and representations are the means used by those skilled in the data processing arts to most effectively convey the substance of their work to others skilled in the art. An algorithm is here, and generally, conceived to be a self-consistent sequence of steps (instructions) leading to a desired result. The steps are those requiring physical manipulations of physical quantities. Usually, though not necessarily, these quantities take the form of electrical, magnetic or optical signals capable of being stored, transferred, combined, compared and otherwise manipulated. It is convenient at times, principally for reasons of common usage, to refer to these signals as bits, values, elements, symbols, characters, terms, numbers, or the like. Furthermore, it is also convenient at times, to refer to certain arrangements of steps requiring physical manipulations of physical quantities as modules or code devices, without loss of generality. 
         [0041]    However, all of these and similar terms are to be associated with the appropriate physical quantities and are merely convenient labels applied to these quantities. Unless specifically stated otherwise as apparent from the following discussion, it is appreciated that throughout the description, discussions utilizing terms such as “processing” or “computing” or “calculating” or “determining” or “displaying” or “determining” or the like, refer to the action and processes of a computer system, or similar electronic computing device, that manipulates and transforms data represented as physical (electronic) quantities within the computer system memories or registers or other such information storage, transmission or display devices. 
         [0042]    Certain aspects of the present invention include process steps and instructions described herein in the form of an algorithm. It should be noted that the process steps and instructions of the present invention could be embodied in software, firmware or hardware, and when embodied in software, could be downloaded to reside on and be operated from different platforms used by a variety of operating systems. 
         [0043]    The present invention also relates to an apparatus for performing the operations herein. This apparatus may be specially constructed for the required purposes, or it may comprise a general-purpose computer selectively activated or reconfigured by a computer program stored in the computer. Such a computer program may be stored in a computer readable storage medium, such as, but is not limited to, any type of disk including floppy disks, optical disks, CD-ROMs, magnetic-optical disks, read-only memories (ROMs), random access memories (RAMs), EPROMs, EEPROMs, magnetic or optical cards, application specific integrated circuits (ASICs), or any type of media suitable for storing electronic instructions, and each coupled to a computer system bus. Furthermore, the computers referred to in the specification may include a single processor or may be architectures employing multiple processor designs for increased computing capability. 
         [0044]    The algorithms and displays presented herein are not inherently related to any particular computer or other apparatus. Various general-purpose systems may also be used with programs in accordance with the teachings herein, or it may prove convenient to construct more specialized apparatus to perform the required method steps. The required structure for a variety of these systems will appear from the description below. In addition, the present invention is not described with reference to any particular programming language. It will be appreciated that a variety of programming languages may be used to implement the teachings of the present invention as described herein, and any references below to specific languages are provided for disclosure of enablement and best mode of the present invention. 
         [0045]    In addition, the language used in the specification has been principally selected for readability and instructional purposes, and may not have been selected to delineate or circumscribe the inventive subject matter. Accordingly, the disclosure of the present invention is intended to be illustrative, but not limiting, of the scope of the invention, which is set forth in the following claims. Effectors include manipulators in industrial robots. In humanoid robotics, the effector is often defined as a reference point of the hand such as the finger tip. The effector could also be the head, which is controlled to face a certain point or in certain direction. 
         [0046]    Task coordinates refer to coordinates in which the movement of the effector is described. There are many ways to describe effector motions. To describe the positions of the effectors, the x, y and z elements of a position vector are commonly chosen. For spatial orientations, the task is often described in Euler angles or quaternions. In many cases, special descriptions for a task are used. 
         [0047]    Task space refers to the space that is described by the task coordinates. For example, if the hand position of a robot is controlled in x, y and z direction, the task space has a dimension of three (3) and is described by these coordinates. 
         [0048]    Null space refers to the space where a motion does not influence the task space motion. For example, if a robot has seven (7) degrees of freedom, and the task vector is three (3) dimensional hand position, then the null space has four (4) dimensions. The system is redundant with respect to the task. All motions of the arm that do not interfere with the task motion are referred to as the null space motion herein. 
         [0049]    Trajectory is a continuous path describing the motion of a system. The trajectory can describe the path of the individual joints or a path represented in the task coordinates. 
       Distance Computation 
       [0050]    For collision avoidance, it is necessary to compute distances and closest points between segments which are physical links separated by joints. It is more complex to compute actual distances and closest points for all possible segment pairs of humanoid robots within a constrained computation time, especially when the computation is performed on an embedded computer of the robot. 
         [0051]    Therefore, in one embodiment of the present invention, a collision model with primitive objects as shown in  FIG. 1  is defined. Each segment (Head, Right Shoulder and so on) is composed of one or more spheres or swept sphere lines (SSLs) in order to cover the shape of the robot. The distances and closest points are then calculated based on this model. 
         [0052]    In addition, a virtual object as shown in  FIG. 5  is taken into account. Therefore, computation of distances and closest points between a segment of the robot (for example, an arm) and the virtual object is also performed. 
         [0053]    As illustrated in  FIG. 5 , the robot is a humanoid robot with a moveable head having two cameras at its fronts. The virtual object to be avoided by segments of the robot is adaptively arranged between the cameras and the target. “Adaptively” means that the virtual object may be changed according to the position of the target associated with the robot&#39;s reaching motion relative to the cameras. 
       Collision Avoidance 
       [0054]    The role of collision avoidance is to move away segments that are close to each other. One of the most efficient ways to move away the segments apart is by moving the segments in a direction parallel to the vector that connects closest two points of the segments. A collision avoidance coordinate system is defined for task space motions in the collision avoidance control. An example of the collision avoidance coordinate system is defined as illustrated in  FIG. 2 . The origin is the closest point on the lower arm. y-axis is aligned to a line that connects the closest points so that the direction of avoidance is aligned to this axis. x-axis is aligned to the vector that extends from the elbow to the wrist position. z-axis is an outer product of a unit vector of x direction and a unit vector in y direction. The collision avoidance moves segments only in the y direction on this coordinate system. 
         [0055]    In one embodiment of the present invention, only one degree of freedom is used for the collision avoidance instead of three degrees of freedom. The other degrees of freedom remain available for the null space and for target reaching motions. 
         [0056]      FIG. 3  is a diagram illustrating main computing blocks, according to one embodiment of the present invention. A target (defined externally or by the robot itself) is provided to a motion control unit such as a whole body motion (WBM) control unit and a collision avoidance (CA) unit. The whole body motion is to be understood as being merely an example of a motion of the robot. 
         [0057]    The motion control unit outputs a first joint velocity vector signal to a blending unit which combines this signal with a second joint velocity vector signal from the CA control unit. The blending control unit outputs a combined joint velocity vector basis on which the robot&#39;s motion is controlled. 
         [0058]    The combined joint velocity vector is furthermore provided to a distance computing unit that calculates the two closest points of different segments of the robot connected to each other via at least one joint or a segment of the robot and another object. The distance computing unit outputs closest point data and distance information to the CA control unit. The distance computing unit also outputs distance information to the motion control unit and to the blending unit. The blending unit calculates the blending ratio between the first and the second joint velocity vector on the basis of the supplied distance information. 
       A. Collision Avoidance Control Using Null Space Criteria 
       [0059]    Generally, the Cartesian position and orientation x of an effector of a robot can be described as a function of the vector of joint variables q of the manipulator. 
         [0000]        x=f ( q ) 
         [0060]    While this equation can be obtained easily, the inverse problem is crucial. In redundant cases, it is generally not possible to find an inverse mapping f 1 . Instead of constructing an inverse function g(x) with 
         [0000]        f ( g ( x ))= x    
         [0000]    analytically, the problem is often reformulated in the velocities utilizing the partial derivation of f(q) 
         [0000]    
       
         
           
             
               x 
               . 
             
             = 
             
               
                 J 
                  
                 
                   ( 
                   q 
                   ) 
                 
               
                
               
                 q 
                 . 
               
             
           
         
       
       
         
           
             J 
             = 
             
               
                 ∂ 
                 
                   f 
                    
                   
                     ( 
                     q 
                     ) 
                   
                 
               
               
                 ∂ 
                 q 
               
             
           
         
       
     
         [0061]    Because the inverse of the non-square (analytical) Jacobian J(q) does not exist in the redundant case, the pseudo inverse is utilized. 
         [0062]    The task space is used for collision avoidance and the null space is used for moving towards the target using a potential function (cost function). In this control method, each joint velocity is computed as: 
         [0000]        {dot over (q)}   ca   =J   ca   # ( q )| row.y   {dot over (y)}   ca   +N   ca ξ t   (1) 
         [0000]    , where J ca (q) is the collision avoidance Jacobian between closest points, N ca  is a matrix which maps ξ to the null space, J ca   # (q)| row.y  is the row vector extracted from the pseudo inverse Jacobian J ca (q) and which is in the y axis in the collision avoidance coordinate system illustrated in  FIG. 2 , and {dot over (y)} ca  is an avoidance velocity which is derived from a virtual force F virtual , 
         [0063]    In this example, the collision avoidance takes over control for only one degree-of-freedom (specifically, the y-direction) in order to avoid the closest segment. Zje collision avoidance does not affect other degrees of freedom, which remain available for the task execution (e.g., target reaching motions related to x and z direction). This means that the collision avoidance uses one degree of freedom for each arm respectively. 
         [0064]    In the following equation (2), D is a pre-set non zero dampening coefficient. 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       y 
                       . 
                     
                     ca 
                   
                   = 
                   
                     
                       F 
                       virtual 
                     
                     D 
                   
                 
               
               
                 
                   ( 
                   2 
                   ) 
                 
               
             
             
               
                 
                   
                     F 
                     virtual 
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               d 
                               a 
                             
                             - 
                             d 
                           
                         
                         
                           
                             
                               if 
                                
                               
                                   
                               
                                
                               d 
                             
                             &lt; 
                             da 
                           
                         
                       
                       
                         
                           0 
                         
                         
                           otherwise 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   3 
                   ) 
                 
               
             
           
         
       
     
         [0065]    If the distance d between the closest points is shorter than a threshold distance d a  set for activating/deactivating the collision avoidance, the virtual force value and the avoidance velocity is greater than zero. The virtual force and the avoidance velocity increase proportional (e.g., linearly) to the difference between d and d a . Thus the collision avoidance is activated as soon as the closest point distance d is smaller than the preset threshold value d a . (In the example of  FIG. 2 , the distance d between the closest points of the robot is indicated as the distance between the right arm and the front of the body of the humanoid robot). 
         [0066]    N ca  matrix which maps ξ into null space and I is the identity matrix. 
         [0000]        N   ca   =I−J   ca ( q ) #   J   ca ( q )  (4) 
         [0067]    The potential function H t (r) leads to the target for the null space. Let α(t) be a step width. 
         [0000]    
       
         
           
             
               
                 
                   
                     ξ 
                     t 
                   
                   = 
                   
                     - 
                     
                       
                         
                           α 
                           t 
                         
                         ( 
                         
                           
                             ∂ 
                             
                               
                                 H 
                                 t 
                               
                                
                               
                                 ( 
                                 r 
                                 ) 
                               
                             
                           
                           
                             ∂ 
                             q 
                           
                         
                         ) 
                       
                       T 
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             ∂ 
                             
                               
                                 H 
                                 t 
                               
                                
                               
                                 ( 
                                 r 
                                 ) 
                               
                             
                           
                           
                             ∂ 
                             q 
                           
                         
                         = 
                         
                           
                             
                               ∂ 
                               
                                 
                                   H 
                                   t 
                                 
                                  
                                 
                                   ( 
                                   r 
                                   ) 
                                 
                               
                             
                             
                               ∂ 
                               r 
                             
                           
                            
                           
                             
                               ∂ 
                               r 
                             
                             
                               ∂ 
                               q 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               ∂ 
                               
                                 
                                   H 
                                   t 
                                 
                                  
                                 
                                   ( 
                                   r 
                                   ) 
                                 
                               
                             
                             
                               ∂ 
                               r 
                             
                           
                            
                           
                             J 
                              
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   6 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       H 
                       t 
                     
                      
                     
                       ( 
                       r 
                       ) 
                     
                   
                   = 
                   
                     
                       1 
                       2 
                     
                      
                     
                       
                         ( 
                         
                           
                             r 
                             curr 
                           
                           - 
                           
                             r 
                             t 
                           
                         
                         ) 
                       
                       T 
                     
                      
                     
                       
                         W 
                         t 
                       
                        
                       
                         ( 
                         
                           
                             r 
                             curr 
                           
                           - 
                           
                             r 
                             t 
                           
                         
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   7 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where W t  is a weighting matrix, and r t  is a target vector 
         [0068]    Thus, the collision avoidance controls joints for the collision avoidance in task space and the target reaching in null space. 
       B. Motion Control 
       [0069]    On the other hand, the whole body motion control is used in order to control the robot that has redundant degrees of freedom also with null space optimization criteria as, 
         [0000]        {dot over (q)}   wbm   =J   wbm   # ( q ) {dot over (x)}   task   +N   wbm ξ wbm   (8) 
         [0000]        N   wbm   =I−J   wbm   # ( q ) J   wbm ( q )  (9) 
         [0000]    where {dot over (q)} wbm  is a joint velocity vector, J wbm   # (q) is pseudo inverse Jacobian of overall joints, and {dot over (x)} task  is task velocity vector. 
         [0070]    The matrix N wbm  maps an arbitrary joint velocity vector ξ wbm  into the null space. Two cost functions can be used for the null space. Let α jc  and α ca  the step width, then: 
         [0000]    
       
         
           
             
               
                 
                   
                     ξ 
                     wbm 
                   
                   = 
                   
                     
                       - 
                       
                         
                           
                             α 
                             jc 
                           
                            
                           
                             ( 
                             
                               
                                 ∂ 
                                 
                                   
                                     H 
                                     jc 
                                   
                                    
                                   
                                     ( 
                                     q 
                                     ) 
                                   
                                 
                               
                               
                                 ∂ 
                                 q 
                               
                             
                             ) 
                           
                         
                         T 
                       
                     
                     - 
                     
                       
                         
                           α 
                           ca 
                         
                          
                         
                           ( 
                           
                             
                               ∂ 
                               
                                 
                                   H 
                                   ca 
                                 
                                  
                                 
                                   ( 
                                   q 
                                   ) 
                                 
                               
                             
                             
                               ∂ 
                               q 
                             
                           
                           ) 
                         
                       
                       T 
                     
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
         [0071]    The first cost function penalizes deviations from an arbitrary joint center vector {tilde over (q)}. This cost function seeks to control any joint close to its neutral position. In this regard, a joint limit avoidance cost function is used. 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         H 
                         jc 
                       
                        
                       
                         ( 
                         q 
                         ) 
                       
                     
                     = 
                     
                       
                         1 
                         2 
                       
                        
                       
                         
                           ( 
                           
                             q 
                             - 
                             
                               q 
                               ~ 
                             
                           
                           ) 
                         
                         T 
                       
                        
                       
                         
                           W 
                           jc 
                         
                          
                         
                           ( 
                           
                             q 
                             - 
                             
                               q 
                               ~ 
                             
                           
                           ) 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where W jc  is a weighting matrix. In the simplest case, we choose joint center {tilde over (q)} accordingly. This cost function allows the joint limits to be avoided easily. 
         [0072]    The second one cost function implements the collision avoidance. 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             ∂ 
                             
                               
                                 H 
                                 ca 
                               
                                
                               
                                 ( 
                                 d 
                                 ) 
                               
                             
                           
                           
                             ∂ 
                             q 
                           
                         
                         = 
                         
                           
                             
                               ∂ 
                               
                                 
                                   H 
                                   ca 
                                 
                                  
                                 
                                   ( 
                                   d 
                                   ) 
                                 
                               
                             
                             
                               ∂ 
                               d 
                             
                           
                            
                           
                             
                               ∂ 
                               d 
                             
                             
                               ∂ 
                               q 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               ∂ 
                               
                                 
                                   H 
                                   ca 
                                 
                                  
                                 
                                   ( 
                                   d 
                                   ) 
                                 
                               
                             
                             
                               ∂ 
                               d 
                             
                           
                            
                           
                             
                               J 
                               ca 
                               # 
                             
                              
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         H 
                         ca 
                       
                        
                       
                         ( 
                         d 
                         ) 
                       
                     
                     = 
                     
                       
                         1 
                         2 
                       
                        
                       
                         
                           ( 
                           
                             
                               d 
                               safe 
                             
                             - 
                             d 
                           
                           ) 
                         
                         T 
                       
                        
                       
                         
                           W 
                           ca 
                         
                          
                         
                           ( 
                           
                             
                               d 
                               safe 
                             
                             - 
                             d 
                           
                           ) 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where W ca  is a weighting matrix for the collision avoidance. Thus, the whole body motion controls joints for the target reaching in null space, the collision avoidance and joint limit avoidance in null space. 
       C. Integration of Motion Control and Collision Avoidance 
       [0073]    The output of the whole body motion control and the output of the collision avoidance motion control are blended into joint velocities according to the closest distance. The final joint velocity vector {dot over (q)} is 
         [0000]        {dot over (q)}={ 1− f ( d )} {dot over (q)}   wbm   +f ( d ) {dot over (q)}   ca   (14) 
         [0000]    , where {dot over (q)} control  is the joint velocity vector computed by a robot controller such as whole body motion control, and {dot over (q)} ca  is the joint velocity vector computed by the collision avoidance controller, f(d) is a gain factor which can assume values between 0 and 1 to represent the magnitude of danger of collisions, for example, as a function of the shortest distance between segments. If f(d) is equal to “1”, the collision avoidance entirely takes over control. If f(d) is equal to “0”, the robot controller entirely takes over control. f(d) is defined as following: 
         [0000]    
       
         
           
             
               f 
                
               
                 ( 
                 d 
                 ) 
               
             
             = 
             
               { 
               
                 
                   
                     
                       
                         d 
                         - 
                         
                           d 
                           a 
                         
                       
                       
                         
                           d 
                           b 
                         
                         - 
                         
                           d 
                           a 
                         
                       
                     
                   
                   
                     
                       
                         if 
                          
                         
                             
                         
                          
                         d 
                       
                       &lt; 
                       
                         d 
                         a 
                       
                     
                   
                 
                 
                   
                     0 
                   
                   
                     otherwise 
                   
                 
               
             
           
         
       
     
         [0074]    d b  is a preset constant threshold distance where the whole body motion control is switched off, wherein d a &gt;d b . 
         [0075]    If d is larger than d a , the collision avoidance control is deactivated and the robot tracks the trajectory which is generated by the whole body motion control. However, trajectories computed by the whole body motion take into account collision avoidance in null space. If d is smaller than d a , the collision avoidance is activated. The ratio between them depends on the shortest distance between the closest segments. 
         [0076]    Both the collision avoidance control and the robot control use null space optimization criteria. These criteria are composed of the task space and the null space. The task space always has higher priority than the null space. On one hand, the robot control maps a target reaching motion in the task space and a collision avoidance motion in the null space. On the other hand, the collision avoidance control maps a collision avoidance motion in task space and a target reaching motion in the null space. 
         [0000]    
       
         
               
               
               
             
               
               
               
             
           
               
                   
                   
               
               
                   
                 motion in task space 
                 motion in null space 
               
               
                   
                   
               
             
             
               
                   
               
             
          
           
               
                 robot control ({dot over (q)} control ) 
                 target reaching 
                 collision avoidance 
               
               
                 collision avoidance control 
                 collision avoidance 
                 target reaching 
               
               
                 ({dot over (q)} ca ) 
               
               
                   
               
             
          
         
       
     
         [0077]    As illustrated by the above equation (14), the robot control contribution and the collision avoidance control contribution are blended depending on f(d). When f(d) increases, then the collision avoidance motion becomes more important with respect to robot motions. However, target reaching motions are compiled in the null space in the collision avoidance control and if f(d) is not “1”, the robot control contributes to move towards the target. This enables a smooth task priority switching. 
       EXAMPLE 
       [0078]    In order to avoid occlusion problems, a “virtual object” in space is defined and taken into account when performing the collision avoidance. Thus, space can be defined where a robot segment is not allowed to penetrate. This means that an area can be defined where a robot segment is not allowed to enter. Such area may be used as an “occlusion avoidance.” One of the major problems when robots grasp something is occlusions that are caused by their hands hiding target objects. In one embodiment of the present invention, a virtual obstacle can be configured and placed between the robot head and the target so that the hands of the robot do not interfere with the robot&#39;s view of the target. The virtual object may be inactivated right before the robot reaches to the object. 
         [0079]    Embodiments of the present invention relate to a technique for a safety mechanism that protects articulated robots and the environment in which the robot is operating. In particular, embodiments may be employed in robots that interact with dynamic environment that is difficult to predict in real-time. The following are illustrative examples where embodiments of the present invention may be used: 
         [0000]    (1) Tracking moving targets that are captured by cameras: Robots need to track moving targets movement of which cannot be predicted while avoiding self collisions at the same time.
 
(2) Bimanual manipulations: Robots move both arms at the same time according to tasks. Each arm has a target respectively and they sometimes cross each other.
 
(3) Avoid moving obstacles: Robots need to avoid moving objects such as humans.
 
         [0080]    The collision avoidance uses only one degree of freedom. On the other hand, target reaching motions are achieved by (1) null space in the collision avoidance control, and (2) task space in the robot control. 
         [0081]    By limiting the collision avoidance motion to a single degree-of-freedom, the null space motion of the collision avoidance control allows for a target reaching motion is increased although the collision avoidance control allocates higher priority to the collision avoidance task compared to the target reaching task. Thus, the movement towards a target is more efficient compared to conventional methods 
         [0082]    While particular embodiments and applications of the present invention have been illustrated and described herein, it is to be understood that the invention is not limited to the precise construction and components disclosed herein and that various modifications, changes, and variations may be made in the arrangement, operation, and details of the methods and apparatuses of the present invention without departing from the spirit and scope of the invention as it is defined in the appended claims.