DYNAMICAL SYSTEM-BASED ROBOT VELOCITY CONTROL

[0001] The present disclosure relates to dynamical system-based robot velocity control.

[0002] Robots typically include a series of linkages that are interconnected via motor-driven robotic joints. Each robotic joint represents one or more independent control variables/degrees of freedom. End-effectors such as robotic hands or grippers are the particular end linkages in a serial robot which act directly on an object in the performance of a given work task, such as grasping and moving the object. Complex programming and motion control techniques are used in a variety of ways to achieve the required levels of robotic mobility, dexterity, and work task-related functionality. End-effectors approach and depart from a specified goal position according to a particular path or trajectory. Such paths are planned in logic using a variety of approaches. However, conventional end-effector path planning may be less than optimally robust in certain instances as explained below.

[0003] A robotic control system and method are disclosed herein that are suitable for controlling a robotic end-effector. While conventional robots are typically isolated from the surrounding work environment by gates or other barriers, direct interaction between robots and line operators and/or objects located in the robot's work environment is intended with some of the more complex emerging dexterous robot designs. At times, however, unexpected contact may still occur with the end-effector of such robots, which in turn may lead to end-effector perturbations. Such perturbations can cause the end-effector motion to diverge from the planned path, and thus require a more robust control processing functionality.

[0004] The present control framework is intended to addresses this particular control challenge via the use of a control system that combines functions of a velocity control module with those of a dynamical system module as explained below. As is well known in the art, end-effector control typically involves programming a controller with predefined approach and departure trajectories in a given task space, e.g., an XYZ Cartesian coordinate frame of reference. The position and velocity of the end-effector are measured and controlled in real time as the end-effector moves toward or away from the specified goal position. Dynamical systems offer an alternative approach. In a dynamical system, the end-effector movement trajectories are not pre-defined. Instead, such trajectories are automatically generated via a controller during a given end-effector movement. Unlike in the continuous closed-loop, programming-intensive control schemes noted above, only a few control commands are specified in a dynamical system. Differential equations ultimately generate the corresponding movement trajectories.

[0005] A dynamic movement primitive (DMP) framework may be used to implement such dynamical systems. In a DMP framework, the aforementioned differential equations are adapted to generate given movements. Together, these differential equations can form a library of so-called “movement primitives”. However, conventional end-effector control techniques may remain less than optimally robust, particularly when encountering perturbations such as those caused by external forces encountered by the end-effector while moving through planned trajectories.

[0006] The control system disclosed herein ultimately generates movement commands for the end-effector, possibly doing so in response to a previously operator-demonstrated work task. The dynamical system module generates desired velocity commands for movement of the end-effector, doing so via a flow vector field. The actual position in free space of the end-effector is measured and transmitted to the dynamical system module as a control input. The control system disclosed herein allows for more diverse or flexible robotic movements relative to the prevailing art, with the robot better able to adapt online or in real time to newly stated goal positions and/or encountering of dynamic or static obstacles in the robot's work environment.

[0007] In an example embodiment, a robotic system includes an end-effector and a control system. The control system, which is in communication with the end-effector, includes a processor, a dynamical system module (DSM), and a velocity control module (VCM), with the term “module” as used herein referring to the hardware and software portions of the control system dedicated to performing associated functions as explained below. In particular, the DSM processes a set of inputs and applies the flow vector field, and then outputs a control velocity command. The set of inputs may include a measured or actual position of the end-effector, a desired goal position of the end-effector, and an optional operator-demonstrated reference path of the end-effector, e.g., a path demonstrated via backdriving or teleoperation of the end-effector. The VCM receives or calculated an actual velocity of the end-effector and the control velocity command as inputs, and ultimately generates a motor torque command to the end-effector as an output command.

[0008] The control system employs a predetermined set of differential equations to generate a motion trajectory of the end-effector in real time. The motion trajectory approximates the demonstrated reference path. The control system is programmed to adapt or modify movement of the end-effector in real time via the VCM in response to experienced perturbations of movement of the end-effector.

[0009] The DSM may include a path transformation module programmed to transform a frame of reference of the flow vector field, and a path blending module that incorporates a tunable linear interpolation term into the differential equations. The tunable linear interpolation term blends together a path of the end-effector drawn forward from a start position with a path drawn backward from a goal position.

[0010] The control system appends a flow vector field function with a transform, with the flow vector field function defining the flow vector field noted above. The transform rotates a frame of reference of the flow vector field function to match an orientation of the demonstrated path with an orientation of the new start and goal positions.

[0011] The VCM may operate at a cycle rate that is at least five times (5×) higher than a cycle rate of the DSM. For example, the DSM may operate at about 20-40 Hz and the VCM may operate at about 100-1000 Hz, such that the differential equations are updated at the higher frequency.

[0012] An associated method includes processing a set of inputs via the DSM having a predetermined flow vector field. The set of inputs includes an actual position of an end-effector of a robotic system, a desired goal position of the end-effector, and a demonstrated reference path of the end-effector. Processing includes employing a predetermined set of differential equations to generate a motion trajectory of the end-effector in real time to approximate the demonstrated reference path. The method also includes outputting a control velocity command via the DSM, and receiving and processing an actual velocity of the end-effector and the control velocity command from the DSM via the VCM. The method may further include generating and transmitting a motor torque command to the end-effector via the VCM to thereby adapt or modify a movement of the end-effector in real time in response to perturbations of movement of the end-effector.

[0013] The above features and advantages and other features and advantages of the present invention are readily apparent from the following detailed description of the best modes for carrying out the invention when taken in connection with the accompanying drawings.

[0021] With reference to the drawings, wherein like reference numbers refer to the same or similar components throughout the several views, a robotic system 10 is shown schematically in FIG. 1. The robotic system 10 includes a robot 12, which is depicted as an example dexterous humanoid robot, but which may be alternatively embodied as any multi-axis robot. The robot 12 includes an end-effector 14 disposed at a distal end of a robot arm 16. Motion of the robot 12, particularly the end-effector 14 and the robot arm 16, is automatically controlled via a control system (C) 50 having an integrated dynamical system module (DSM) 52 and a velocity control module (VCM) 54, the specific programmed functions of which are described in detail below.

[0022] The robot 12 is configured in hardware and software to perform one or more automated tasks with multiple control degrees of freedom, and to perform other interactive tasks or control other integrated system components, for instance clamping, relays, task lighting, and the like. In the embodiment shown in FIG. 1, the robot 12 includes a plurality of independently and interdependently-moveable robotic joints to control the motion of the robot arm 16, and ultimately the end-effector 14. Although omitted from FIG. 1 for illustrative simplicity, each robotic joint contains and/or is driven by one or more joint actuators, such as a joint motor, linear actuator, rotary actuator, or the like.

[0023] The control system 50 provides precise motion control of the robot 12, including control over the fine and gross movements needed for manipulating an object (not shown) that may be acted on by the end-effector 14. Additionally, the control system 50 provides online movement generation and motor control for the robot 12. As described below, the control system 50 may be programmed to provide dynamic movement primitives or DMPs so as to generate a motion trajectory in real time using differential equations, doing so in a particular manner using the presently disclosed modification that adapt the movements to a large variety of new or unexpected contexts.

[0024] In particular, the control system 50 as shown in FIG. 1 may be programmed with a term appended to the differential equations that causes rotation of a generated movement of the end-effector 14, such that the generated movement aligns with new start and end/goal positions in a given end-effector path. The control system 50 also appends an additional term that gradually varies a goal parameter from an original goal position to a new goal position. In other words, known capabilities of DMPs are extended using the present framework such that a single movement of the end-effector 14 or other part of the robot 12 of FIG. 1 can be easily modulated to handle a much larger set of unforeseen circumstances. This approach, which may utilize path transform (PT) logic 55 and path blend (PB) logic 57 to respectively transform the reference frame of a flow vector field and incorporate linear interpolation, is discussed in detail below with reference to FIGS. 3A and 3B.

[0025] The control system 50 may be embodied as a computer device or multiple such devices, and is programmed to plan and generate robotic movements of the robot arm 16 and the end-effector 14. The control system 50 may include one or more processors (P) and memory (M), including sufficient amounts of tangible, non-transitory memory. Memory types may include optical or magnetic read only memory (ROM), random access memory (RAM), erasable electrically-programmable read only memory (EEPROM), and the like. The control system 50 may also include a high-speed clock, analog-to-digital (A/D) circuitry, digital-to-analog (D/A) circuitry, and any required input/output (I/O) circuitry and devices, as well as signal conditioning and buffer electronics. Individual control algorithms resident in the controller 50 or readily accessible thereby, such as instructions embodying a method 100 shown in FIG. 5, may be stored in memory (M) and automatically executed via the processor (P) at one or more different control levels to provide the respective control functionality.

[0026] A dynamic movement primitive or DMP, as is well known in the art of robotic control, can be used to generate a particular robotic movement trajectory x(t) with a given velocity v(t). The equations of motion for a DMP are motivated by the dynamics of a damped spring attached to a goal position g and perturbed by a non-linear acceleration:

{dot over (v)}=K(g−x)−Dv+(g−x₀)ƒ(s)

{dot over (x)}=v

where x₀ is the start point of a given movement, K is the spring constant, D is the damping constant, and ƒ is a parameterized non-linear function.

[0027] In general, the control system 50 shown in FIG. 1 receives a desired goal (g_d), i.e., a target end position of a given end-effector movement, either via programming or via a reference path provided via demonstration learning, and executes a set of differential equations via the DSM 52. The DSM 52 includes the set of non-linear differential equations as specified below, which are solved via the processor (P). In certain existing systems, differential equations are solved in real time to produce desired joint angles or robot end-effector positions, which are then tracked by another robot controller. Such an approach is conducive to demonstration learning.

[0028] However, it is recognized herein that deviations may occur between an actual position and a desired position of the end-effector 14. These deviations are often ignored in conventional end-effector planning. As a result, existing planning approaches may not be possible to use if sufficiently large perturbations are present, such as an external force applied to the end-effector 14, for instance by an object unexpectedly encountered in the workspace of the robot 12 of FIG. 1. In other words, the robot 12 of FIG. 1 may fail to properly track the desired motion of its end-effector 14, and may instead produce an unexpected undesired motion. The control system 50 of the present invention is therefore intended to produce smooth motion of the robot arm 16 and the end-effector 14 in spite of such perturbations.

[0029] The DSM 52 shown schematically in FIG. 1 ultimately outputs desired joint velocity (v_d), and interacts with the velocity control module (VCM) 54, which in turn sends a commanded motor torque command (τ_m) to the robot 12 and receives an actual joint velocity (v_a) from the robot 12. The DSM 52 also receives measured actual joint positions (P_a) from the robot 12, e.g., from one or more position sensors S_p. As is known in the art, the actual velocity (v_a) may be readily calculated from the measured joint positions.

[0030] The VCM 54 may be run at a faster rate than the communication rate with the DSM 52, such as at least five times (5×) faster, and as much as fifty times (50×) faster in another embodiment. In an example approach, the DSM 52 may run at a cycle rate of between 20-40 Hz, while the VCM 54 may run at a cycle rate of between 100-1000 Hz. The processor (P) updates the dynamical system equations of the DSM 52 at the higher rate. In the present invention, the DSM 52 creates, for each time step in the movement, a DMP as a movement trajectory, which is defined herein as a desired position (x) and velocity (v) of the end-effector 14. Such a dynamical system can be described as follows, using terms similar to those used above to describe DMPs generally:

{dot over (v)}=K(g−x)−Dv−K(g−x₀)s+Kƒ(s)+p(x,v)

{dot over (x)}=v

[0031] In the above equations, x₀ is the starting position of a movement, g is the desired goal/target of the movement, s is a phase variable that decays exponentially during the movement, K is the spring constant, D is a damping constant, ƒ is an adaptable non-linear function, and p is an optional term that allows the inclusion of an obstacle avoidance technique as set forth below. ƒ may be defined as follows:

$f\left(s\right)=\frac{\sum _i\psi i\left(s\right)\mathrm{wi}}{\sum _i\psi i\left(s\right)}s$

with ψi(s) being a Gaussian function. ƒ depends explicitly on the phase variable (s), i.e., τ{dot over (s)}=−αs, where α is a predefined constant. In other words, the non-linear function ƒ does not depend on time, and therefore ƒ can be simply adjusted such that the equations generate a desired trajectory for the end-effector 14. This is an underlying principle of the discussion that follows with reference to FIGS. 2-4B.

[0032] Referring to FIG. 2, one advantage of the control system 50 of FIG. 1 over the use of a static trajectory is the flexibility provided against perturbations in motion of the end-effector 14, as a desired position/velocity can be readily provided for each location in state space. This advantage also allows the inclusion of obstacle avoidance capabilities, thereby enabling real-time planning of movements around any dynamic, i.e., moving, obstacles in the operating space of the robot 12.

[0033] To further illustrate this concept, an example vector flow field (FF) 20 is depicted for perturbations in XY position in state space, with the X axis representing changes in position occurring in the X direction and the Y axis representing changes in position occurring in the Y direction. The flow field 20 acts a velocity vector field, e.g., a field of individual velocity vectors 22, which helps to guide the end-effector 14 from its current position P_c to a goal position P_g. Arrow A in FIG. 2 represents the perturbation in different directions.

[0034] Referring again to FIG. 1, the control system 50 of FIG. 1 calculates the required motor torque needed to achieve a particular velocity (v) of the end-effector 14. For control of the robot 12, the DSM 52 of FIG. 1 generates the desired velocity v_d, which is then converted via the VCM 54 into corresponding control torque τ. In a possible embodiment, the desired velocity (v_d) is defined in Cartesian space of the end-effector 14. In such a case, the VCM 54 may be realized mathematically as follows:

τ=PJ^T(x_d−x)+DJ^T(v_d−v)

where once again τ is the commanded control torque, x_d and v_d are the respective desired position and velocity, x and v are the measured position and velocity, J is the Jacobian matrix of the robot arm 16, and P and D are selectable or tunable parameters. The Jacobian J may be computed either inside or outside of the velocity control loop. Alternatively, x and v may describe the motion in joint space as in FIG. 1. In such an instance, the Jacobian J can be omitted.

[0035] In an embodiment, the tunable parameter P may be zero or near zero, and thus the control system 50 is dominated by velocity error. Still, the control system 50 will continue to command movements that converge to the desired position (x_d), since the position error is provided as feedback to the DSM 52, which in turn is designed to converge at the desired target. The sensor(s) S_p of the robot 12 provide feedback about the actual robot position, e.g., of the end-effector 14. Therefore, the combination of the VCM 54 with the DSM 52 allows for small or zero gains on the position error. This in turn has the advantage of making the movements of the end-effector 14 smoother and more tolerant to perturbations.

[0036] The above approach may be further optimized by extending the framework to allow for modulation of the individual dynamic movement primitives. A dynamic movement primitive generates a trajectory in real time using a differential equation, as is known in the art and noted above. The remaining discussion with reference to FIGS. 3A-4B relates to the modifications that can be made to adapt movements of the end-effector 14 to a larger variety of contexts. Particularly, first and second terms are added to the differential equations. The first term rotates a generated movement such that the movement aligns with a new desired start and goal positions, while the second term gradually varies a goal parameter from the original goal to a new goal.

[0037] An objective of this two-term approach is to provide added flexibility in the generalizing of a human-demonstrated robotic motion to new movement contexts. For instance, a motion in which an object is lifted vertically by the robot 12 of FIG. 1, driven towards a goal position, and dropped cannot be readily generalized using DMPs unless the direction of movement is similar to the demonstrated movement. With the following approach modifications, however, an applied transformation induces a rotational invariance about the vertical axis of movement and generates a similar motion for any approach direction. Additionally, the approach shown in FIGS. 3A-4B offers an approach for adjusting the speed with which a generalized path conforms to the demonstrated path. These additional features introduce added flexibility to existing DMPs that allow a relatively small library of DMPs to represent a much larger range of motion.

[0038] With respect to demonstration generally, as is known in the art an input device (not shown) such as a 3D mouse, a joystick, or another control device may be used to teleoperate the robot 12, or the robot 12 may be manually backdriven, through a given work task. That is, the robot 12 may be “taught” to execute a particular work task sequence via human-assisted demonstration of that task. Therefore, the robot 12 may be taught all required grasp positions and approach/departure directions while learning how to grasp an object.

[0039] An example of task demonstration/learning is a simple “grasp and lift” task, wherein the robot 12 of FIG. 1 uses its end-effector 14 to grasp and lift an object 25, shown as an example sphere in FIG. 1. An operator of the robot 12 may move the end-effector 14 into a desired position with respect to the object 25, and then correctly grasp the object 27, for instance by manually or automatically actuating the end-effector 14 with sufficient force to establish a desired grasp pose. The operator then moves the robot arm 16 and the end-effector 14 to pick up and move the object 25. Although the robot 12 shown in FIG. 1 could potentially identify task sequences and some relevant perceptual data simply by observing a human operator performing the same task, one of the more challenging aspects of handling new objects and creating new assemblies is determining precisely where to place the end-effector 14. By having a human operator manually move the robot arm 16 and end-effector 14 through each task while the control system 50 records its own raw sensor data, each task demonstration can provide a valuable data stream of experiences from which the robot 12 can ultimately solve these difficult control problems in post-processing subsequent to human-assisted learning.

[0040] The robot 12, particularly when embodied as a humanoid robot as shown and operating in the presence of human operators, should be able to adapt quickly in a dynamic environment. Unlike conventional robots that may be access-restricted or protected by a fence, light curtain, or other barrier, a dexterous humanoid may be expected to interact directly with the human operators. Control of such a robot 12 should be sufficiently robust to any uncertainty about the location and pose of the object 25 of FIG. 1. The present approach addresses these challenges by describing robotic movements as flow vector fields/differential equations instead of static trajectories. The flow vector fields are learned via task demonstration, and they generalize to new contexts, such as new start and end positions of a given movement. Such movements are schematically depicted in FIGS. 3A-4B.

[0041] The DSM 52 of FIG. 1 may include a path transformation module 55 (PT) and a path blending module 57, which is abbreviated PB in FIG. 1. Again, the term “module” as used here, as well as with respect to the DSM 52 and the VCM 54, refers to a suite of requisite hardware and software needed for performing the designated functions described below. The path transformation module 55 is programmed or otherwise configured to transform the frame of reference of the flow field, e.g., the example flow vector field 20 depicted in FIG. 2. The path blending module 57 incorporates linear interpolation so as to blend together a path drawn forward from a start position (P_S) with a path drawn backward from a goal position P_g. Tuning of the interpolation provides the ability to select just how quickly an actual path conforms to a path drawn from the goal.

[0042] The original dynamical system differential equation may be restated as follows:

{dot over (v)}=K(g−x)−Dv−K(g−x₀)s+Kƒ(s)

Each parameter of the above-stated equation is defined elsewhere above. The path transformation module 55 then modulates this dynamical expression by appending the flow vector field function ƒ with a transform T:

{dot over (v)}=K(g−x)−Dv−K(g−x₀)s+KTƒ(s)

Here, the transform T can have both scalar and rotational components. The transform T can also be set as a function of the decaying phase variable s so as to represent a time-varying transform. By rotating the frame of reference of a flow vector field 20 defined by the function ƒ so as to match the orientation of demonstrated paths with an orientation of a new start and goal, rotational invariance is induced into the generalized motion of the robot 12.

[0043] FIG. 3A shows such generalized motion trajectories 40 of the end-effector 14 in an example pick and place work task, without the added benefit of the path transformation module 55 shown in FIG. 1. FIG. 3B shows the same motion trajectories 40 after processing via the path transformation module 55. In FIGS. 3A and 3B, the human-demonstrated reference path is trace 42, while the generalized path realized from a start position P_S toward a goal position P_g is depicted as trace 44. The current position of the end-effector 14 is labeled as P_c. A comparison of FIGS. 3A and 3B will show that the depiction in FIG. 3B is a more intuitive adaptation of the demonstrated reference path (trace 42).

[0044] Implementation of the path blending module 57 of the control system 50 is somewhat more complex. The differential equations are restated as follows:

$\stackrel{.}{v}=K\left(g_m-x\right)-\mathrm{Dv}-K\left(g_m-x_0\right)s+\mathrm{Kf}\left(s\right)$ $g_m=g-\left(g-g_d+x_{\mathrm{od}}-x_0\right)\left(1-\frac{\ln s}{\ln s_m}\right)$

where g_d is the goal position of the demonstrated path (trace 44), x_0d is the start position of the demonstrated path (trace 44), and the phase variable s_m is an adjustable parameter that determines how quickly the position g_m approaches g, i.e., the desired goal or target position of the movement. The variable s_m can be thought of as the value of the variable s at which g_m and g are equal, and therefore when blending is finished.

[0045] FIGS. 4A and 4B depict the effects on the motion trajectories 40 of FIGS. 3A and 3B of path blending when the start position and end goal positions are closer to each other than they are in the demonstrated path (trace 44). FIG. 4A shows motion trajectories 140 with a generalized path 144 without the benefit of blending via the path blending module 57, while FIG. 4B shows the same generalized path 144 after such blending. Note that in FIG. 4B, processing via the path blending module 57 causes the generalized path 144 of the motion trajectories 140 to maintain the structure of the demonstrated path 142 without causing the robot 12 to move backward, as it does in FIG. 4A as indicated by arrow B. Adjusting the variable s_m in this case will scale the shape of the path between the generalized paths 144 in FIGS. 4A and 4B.

[0046] Referring briefly to FIG. 5, a method 100 of using the above-described control system 50 begins with step 102. Step 102 may entail processing a set of inputs via the DSM 52 and its predetermined flow vector field 20. As noted above, such inputs may include the actual position P_X of the end-effector 14 of the robotic system 10, as well as a desired goal position (g_d) of the end-effector 14 and possibly a demonstrated reference path of the end-effector 14. Step 102 includes employing a predetermined set of differential equations to generate a motion trajectory of the end-effector 14 in real time to approximate a demonstrated reference path.

[0047] At step 104, the control system 50 of FIG. 1 outputs a desired control velocity command V_d via the DSM 52. The method 100 then proceeds to step 106, where the VCM 54 receives and processes an actual velocity V_a of the end-effector and the control velocity command V_d from the DSM 52.

[0048] At step 108, the VCM 54 generates and transmits a motor torque command τ_M to the end-effector 14 to thereby adapt or modify a movement of the end-effector 14 in real time in response to perturbations of movement of the end-effector 14.

[0049] The above-described control system 50 and method 100 thus provides a generic approach toward controlling the movement for any type of end-effector or other robotic manipulator. Using flow vector fields specifically defined by the modified differential equations described herein, desired velocities are computed in a manner that accounts for any perturbances of the end-effector 14. In this manner, the disclosed invention seeks to improve upon conventional DMPs by extending the framework to allow for modulation of individual primitives.

[0050] While the best modes for carrying out the invention have been described in detail, those familiar with the art to which this invention relates will recognize various alternative designs and embodiments for practicing the invention within the scope of the appended claims.