JP7808295B2 - Method for controlling a robotic device - Google Patents

Description

The present disclosure relates to a method for controlling a robotic device.

In many applications, it is desirable for robots to operate autonomously, sometimes in dynamic and unstructured environments. To do this, robots need to learn how to move and interact in their surroundings. To do so, robots may rely on a library of skills that can be used to perform simple actions or to perform complex tasks as a combination of multiple skills. A technique for learning behavioral skills is known as learning from demonstrations (LfD), which requires an expert (typically a human) to demonstrate one or more times the specific behavior to be imitated by the robot.

The publication "Using probabilistic movement primitives in robotics" by A. Paraschos et al., in Autonomous Robots, 42:529-551, 2018, describes probabilistic movement primitives (ProMP), a probabilistic framework for learning and synthesizing robot movement skills. ProMP represents trajectory distributions based on compact basis function representations. Its probabilistic formulation enables the exploitation of distributed information in motion modulation, parallel motion activation, and control.

While ProMP has been used to learn cartesian motion, its formulation cannot handle orientation motion in the form of quaternion trajectories. However, quaternions have favorable properties for robot control, such as the fact that they offer near-minimal representation and strong stability in closed-loop orientation control. Therefore, an approach that enables robot control learning from demonstrations containing quaternion trajectories is desirable.

DISCLOSURE OF THE INVENTION According to various embodiments, a method for controlling a robotic device is provided, the method including providing demonstrations for a robotic skill, where each demonstration demonstrates a trajectory including a sequence of robot configurations, where each robot configuration is described by an element of a predetermined configuration space having the structure of a Riemannian manifold. The method further includes, for each demonstrated trajectory, determining a representation of the trajectory as a weight vector of predetermined primitive motions of the robotic device by searching for a weight vector that minimizes a distance measure between the demonstrated trajectory and a combination of primitive motions according to the weight vector, where the combination is mapped to the manifold. The method further includes determining a probability distribution of the weight vector by fitting a probability distribution to the weight vector determined for the demonstrated trajectory, and controlling the robotic device by executing the primitive motions according to the determined probability distribution of the weight vector.

According to various embodiments, the above-described method provides robot control using a Riemannian manifold approach to encoding, reproducing, and adapting stochastic motion primitives (using multivariate geodesic regression, as described in more detail below). In particular, according to various embodiments, the space of quaternion trajectories is considered a Riemannian manifold. This approach is less likely to encode inaccurate data or reproduce distorted trajectories compared to geometry-unaware approaches (such as classical ProMP), allowing the robot to learn and reproduce skills. Because it does not rely on rough approximations, the model is also more explainable. Furthermore, this approach also offers additional adaptation capabilities, such as modulating trajectory distributions and blending motion primitives.

According to various embodiments, the demonstrated trajectories are represented as weight vectors that are geodesic regressions. This means that a geodesic curve can be seen to be fitted to each demonstrated trajectory.

Various examples are given below.

Example 1 is a method for controlling a robot device as described above.

Example 2 is a method according to Example 1, where the probability distribution of the weight vector is determined by fitting a Gaussian distribution to the weight vector determined for the demonstrated trajectory.

The use of Gaussian distributions for training and recall provides reliable control of control scenarios not seen in the demonstration.

Example 3 is the method according to example 1 or 2, wherein each demonstrated trajectory includes a robot configuration for each time point in a predetermined sequence of time points, and each combination of primitive motions according to a weight vector specifies a robot configuration for each time point in the predetermined sequence of time points;
For each demonstrated trajectory, a weight vector is determined by determining a weight vector for a combination of the basic movement according to the weight vector and the demonstrated trajectory from a set of possible weight vectors;
The combination is mapped onto the manifold and is the smallest among the set of possible weight vectors,
The distance between a combination of elementary motions mapped onto a manifold and a demonstrated trajectory is given by a sum over the time points of the sequence of time points, including a term for each time point that contains the value or power of the value of the manifold metric between the element of the manifold given by the combination of elementary motions at the time point when mapped onto the manifold and the demonstrated trajectory.

This provides an efficient way to represent the demonstrated trajectories with weight vectors by fitting a weight vector to the demonstrated trajectories. Combinations may be mapped to the manifold by selecting a point on the manifold and mapping the combination to the manifold by an exponential function of the tangent space of the manifold at the selected point.

Example 4 is a method according to any one of Examples 1 to 3, the method including: searching for a point and a weight vector on a manifold such that, for one of the demonstrated trajectories, a distance measure between a combination of primitive movements according to a weight vector and the demonstrated trajectory is minimized, where the combination is mapped onto the manifold from a tangent space at a point, and where, for each demonstrated trajectory, the mapping of each combination onto the manifold is performed by mapping the combination from a tangent space at a selected point.

In other words, the tangent space (i.e., the points on the manifold that take up the tangent space) is determined for one demonstrated trajectory by performing an optimization over the weights and points. This tangent space is then used to map any combinations required during the combination or search onto the manifold for all demonstrated trajectories. In other words, the same tangent space, and therefore the same exponential mapping, is used for all demonstrated trajectories. This provides an effective technique for overcoming the problem that using different tangent spaces for different trajectories leads to very diverse tangent weight vectors.

Example 5 is the method according to any one of Examples 1 to 4, wherein the trajectories are orientation trajectories, each demonstration further demonstrates a position trajectory, and each robot configuration includes a pose described by a vector in three-dimensional space and an orientation described by elements of a predetermined configuration space.

Thus, skills may be learned by demonstrating sequences of robot poses, e.g., end-effector positions and orientations, where a model for orientation is learned using a Riemannian manifold-based approach.

Example 6 is a method according to any one of Examples 1 to 5, the method including the steps of providing demonstrations of more robot skills; determining, for each skill, a representation of a trajectory, a weight vector, and a probability distribution of the weight vectors; controlling the robotic device by determining (for each time point) a Riemannian-Gauss distribution of the manifold points from the probability distribution of the weight vectors for each skill; determining a product distribution of the Riemannian-Gauss distributions of the skills; and controlling the robotic device by sampling (for each time point) from the determined product probability distribution.

This allows for skill blending for skills learned from demonstrations on Riemannian manifolds.

Example 7 is a robot device controller configured to execute the method described in any one of claims 1 to 6.

Example 8 is a computer program comprising instructions that, when executed by a processor, cause the processor to perform a method according to any one of Examples 1 to 6.

Example 9 is a computer-readable medium having stored thereon instructions that, when executed by a processor, cause the processor to perform a method according to any one of Examples 1 to 6.

In the drawings, like reference numbers generally refer to the same parts throughout the different views. The drawings are not necessarily to scale, emphasis instead being placed upon generally illustrating the principles of the invention. In the following specification, various aspects are described with reference to the following drawings:

FIG. 1 is a diagram illustrating a robot. 1 is a diagram showing a spherical manifold ^S2 whose points can represent each possible orientation of, for example, a robot's end effector. FIG. 1 illustrates multivariate general linear regression on a spherical manifold ^S2 according to one embodiment. FIG. 10 is a diagram illustrating an example in which the embodiment is applied to characters on a spherical surface for the purpose of explanation. FIG. 10 illustrates the blending process according to one embodiment for characters on a sphere for illustrative purposes. 1 is a flowchart illustrating a method for controlling a robotic device.

The following detailed description refers to the accompanying drawings, which show, by way of example, specific details and aspects of the present disclosure in which the present invention may be practiced. Furthermore, other aspects may be used and structural, logical, and electrical changes may be made without departing from the scope of protection of the present invention. Various aspects of the present disclosure are not necessarily mutually exclusive, as some aspects of the present disclosure can be combined with one or more other aspects of the present disclosure to form new aspects.

Various examples are explained in more detail below.

Figure 1 shows the robot 100.

The robot 100 includes a robotic arm 101, e.g., an industrial robotic arm, for manipulating or assembling work pieces (or one or more other objects). The robotic arm 101 includes manipulators 102, 103, and 104 and a base (or support) 105 on which the manipulators 102, 103, and 104 are supported. The term "manipulator" refers to the movable members of the robotic arm 101, the manipulation of which allows for physical interaction with the environment, e.g., to perform a task. For control, the robot 100 includes a (robot) controller 106 configured to implement the interaction with the environment according to a control program. The final member 104 of the manipulators 102, 103, and 104 (furthest from the support 105), also referred to as the end effector 104, may include one or more tools, e.g., a welding torch, a gripping tool, painting equipment, etc.

The other manipulators 102, 103 (near the support base 105) can, for example, together with an end effector 104, form a positioning device having a robotic arm 101 at its end with the end effector 104. The robotic arm 101 is a mechanical arm (possibly with a tool at its end) that can provide functions similar to those of a human arm.

The robotic arm 101 may include joint elements 107, 108, and 109 that interconnect the manipulators 102, 103, and 104 with each other and with the support base 105. The joint elements 107, 108, and 109 may include one or more joints, each of which may provide rotatable (i.e., rotational) and/or translational (i.e., displacement) motion for the associated manipulators. Movement of the manipulators 102, 103, and 104 may be initiated using actuators controlled by the controller 106.

The term "actuator" may be understood as a component adapted to affect a mechanism or process in response to being actuated. The actuator may execute commands (so-called actuations) output by the controller 106 as mechanical movements. An actuator, for example an electromechanical converter, may be configured to convert electrical energy into mechanical energy in response to being actuated.

The term "controller" may be understood as any type of logic-implemented item, which may include, for example, circuitry and/or a processor capable of executing software, firmware, or a combination thereof stored on a recording medium, and capable of outputting instructions to, for example, an actuator in this example. The controller may be configured, for example, by program code (e.g., software) to control the operation of a system, in this example, a robot.

In this example, the controller 106 includes one or more processors 110 and a memory 111 that stores code and data based on which the processor 110 controls the robotic arm 101. According to various embodiments, the controller 106 controls the robotic arm 101 based on a machine learning model 112 stored in the memory 111.

According to various embodiments, a Riemannian manifold approach is used to learn oriented motion primitives using ProMP. That is, an extension of classical ProMP is provided, denoted as "oriented ProMP" using a Riemannian manifold formulation.

The original (i.e., classical) Probabilistic Motion Primitives (ProMP) approach processes robot skills in Euclidean space, making it impossible to learn and reproduce quaternion trajectories (representing the robot's orientation).

The Riemannian formulation of ProMP described below allows for the learning and reproduction of quaternion data. Furthermore, the general treatment given here allows for its use on general Riemannian manifolds.

Below, we present an implementation of ProMP for processing robot skills in Euclidean space.

In what follows, the following notation is used:

Generally, for a single movement execution, a given trajectory
is denoted as a time series of the variable y. Here, _yt is also referred to as the robot configuration for time t and can represent either the joint angles or the Cartesian position in task space at time step t (the time derivative of y may additionally be considered). Following classical ProMP notation, _yt is a d-dimensional vector representing measurements of a d-degree-of-freedom (DoF) system, e.g., a robot arm 101 with 7 degrees of freedom. Each point of the trajectory τ can be represented as a linear basis function model as follows:
y _t =Ψ _t w+ε _y ⇒P(y _t │w)=N(y _t │Ψ _t w, Σ _y ) (1)
where w is a dN _φ -dimensional weight vector, Ψ _t is a d×dN _φ -dimensional block diagonal matrix containing time-dependent basis functions φ _t for each DoF (a basis function for one DoF is also called a basic motion (e.g., motion in a given direction, rotation around a given axis)), N _φ denotes the number of basis functions, and ε _y ∼N(0,Σ _y ) is zero-mean i.i.d. Gaussian noise with uncertainty Σ _y .

ProMP assumes that each demonstration is characterized by a different value of the weight vector w, resulting in a distribution P(w;θ) = N(w|μw,Σw). The complete trajectory can then be modeled as a composition of basis functions at each t with weights w drawn from P(w;θ). Thus, the distribution of states P(y _t ;θ) over time t can be calculated as follows:
This equation estimates both the mean and the variance at each time step t.

When learning from demonstrations, the example trajectories often vary in length in time. ProMP overcomes this problem by introducing a phase variable to separate the data from the time instance, which allows for time modulation. In this case, the demonstration ranges from z ₀ =0 to z _T =1, and the demonstrated trajectory is
The basis functions forming Ψ also depend on the phase variable z. Specifically, ProMP uses Gaussian basis functions for stroke-based motion, defined as b _i (z _t ) = exp((−(z _t −c _i ) ² )/2h), with width h and center c _i , which are often designed experimentally. These Gaussian basis functions are then normalized to yield:

Generally speaking, the learning process of ProMP mainly consists of estimating the weight distribution P(w;θ). To do so, the weight vector w _i representing the i-th demonstration as shown in Equation (1) is estimated by maximum likelihood estimation. This is expressed as follows:
w _i =(Ψ ^T Ψ+λI) ^-1 Ψ ^T Y _i (3)
leads to a linear ridge regression solution of the form
connects all observed trajectory points, and Ψ consists of all time instances for the basis function matrix Ψ _t . Then, given a set of N demonstrations, the weight distribution parameters θ = {μ _w , Σ _w } can be estimated by maximum likelihood. To adapt to new situations, ProMP estimates the weight distribution parameters θ = {μ w , Σ w } for the desired trajectory points with the associated covariance Σ _y ^* .
This allows for trajectory modulation to waypoints or target locations by conditioning the motion to reach
and its parameters can be calculated as follows (assuming a Gaussian distribution):

By computing the product of trajectory distributions, different motion primitives can be blended into a single motion. Specifically, for a set of S different ProMPs _Ps ( _yt ) = N( _yt | _μt,s ,Σt _,s ), whose contribution to the final motion varies according to blending weights αt _,s , the blended trajectory at each time step t is given by the following distribution:
Then,
The parameters of are easily estimated from a weighted product of Gaussians as follows:

Task parameters can, for example, adapt robot motion to a target object in order to accomplish a task. Such information is often obtained during demonstrations and can be integrated into the ProMP formulation. Formally, ProMP is a set of parameters that are based on external conditions.
Considering
The affine mapping from to the average weight vector μ _w is learned, resulting in the joint probability distribution:
where {O, o} is learned using linear ridge regression.

As mentioned above, quaternions have properties that make them suitable for robotic control. However, because quaternions (as used in robotic control) satisfy unit norm constraints, they do not form vector spaces, and therefore the use of traditional Euclidean space methods for processing and analyzing quaternion-valued variables (with unit norm) is insufficient.

According to various embodiments, Riemannian geometry is leveraged to formulate ProMP on quaternion space.

A Riemannian manifold M is a space in which each point is locally in Euclidean space.
It is an m-dimensional topological space with a globally defined differential structure similar to that of the Riemannian manifold. For each point x∈M, there exists a tangent space T _x M, which is a vector space of the tangent vectors of all possible smooth curves that pass through x. Riemannian manifolds have smoothly varying positive definite inner products called Riemann metrics, which allow us to define the length of a curve in M. These curves, called geodesics, represent the curve of minimum length between two points in M, and are a generalization of lines in Euclidean space to Riemannian manifolds.

FIG. 2 shows a diagram of a spherical manifold ^S2 whose points can represent each possible orientation of, for example, a robot end effector.

Two points x and y are shown on a sphere that may be used by the controller 106 to represent two different orientations of the robot end effector 104.

The shortest distance between two points in ambient space is a straight line 201, but the shortest path on the manifold is a geodesic line 202.

To use the Euclidean tangent space,
may be used, which are denoted as the exponential map and the logarithmic map, respectively.

Exponential map
starts at x and maps a point u in the tangent space of x to a point y on the manifold such that u lies on a geodesic in the direction of u such that the geodesic distance dM between x and y is equal to the norm of the distance between x and u. The inverse operation is the logarithmic map
That is,

Another useful operation on manifolds is the translation, which moves an element between tangent spaces so that the dot product between two elements in the tangent spaces remains constant.
There is.

For example, in FIG.
teeth,
from
The vector translated to
and
(For simplicity, the index
is omitted).

In the following, the Riemannian distribution of a random variable p∈M is expressed as follows:
where the mean μ∈M and the covariance Σ∈TμM
This Riemann-Guassian corresponds to an approximate maximum entropy distribution for a Riemannian manifold.

Below are formulas for the Riemannian distance, exponential map, logarithmic map, and translation operations for a spherical manifold S ^m .

According to various embodiments, geodesic regression, which generalizes linear regression to the Riemannian manifold setting, is used (e.g., controller 106). This geodesic regression model is defined as follows:
where y∈M and
are the output and input variables, respectively, p∈M is the base point on the manifold, u∈T _p M is a vector in the tangent space at p, and the error term ε is
is a random variable that takes values in the tangent space at p. Similar to linear regression, (p, u) can be interpreted as having an intercept p and a slope u.

where points {y ₁ , . . . , y _T }εM and
The goal of geodesic regression is to find a geodesic curve γ∈M that best models the relationship between all T pairs (x _i , y _i ). To achieve this, the sum of squared Riemannian distances (i.e., the error) between the model estimates and the observations is minimized, i.e.,
where:
is the model estimate on the manifold M,
is the Riemann error, and the pair (p, u) ∈ TM is an element of the tangent bundle TM. The least squares estimator for the geodesic model can be formulated as the minimization of the sum of squares of the Riemann distances above, i.e.,

However, equation (9) cannot be analytically solved like equation (3). A solution can be obtained by steepest descent, which requires computing the derivatives of the Riemann distance function and the exponential map. The latter can be separated into derivatives with respect to the initial point p and the initial velocity u. These gradients can be computed in terms of the Jacobi field (i.e., the solution of a quadratic equation under the Riemann curvature tensor subject to specific initial conditions).

The above geodesic model is based on the scalar independent variable
Note that we only consider . This means that the derivatives are obtained in terms of the Jacobi field along a single geodesic curve parameterized by a single tangent vector u. The computation of the Jacobi field relies on the so-called adjoint operator, which in effect plays the role of a translation of the error term of the geodesic regression.
The extension of to the multivariate case requires a slightly different approach, which involves identifying multiple geodesic curves (which can be thought of as "basis" vectors in Euclidean space). Multivariate general linear models (MGLMs) on Riemannian manifolds provide a solution to this problem.

The MLGM uses a geodesic basis U=[u ₁ ...u _n ] formed by multiple tangent vectors u j ∈ TpM, one for each dimension of x. Then, equation (9) in the problem becomes
can be reformulated as follows:
To solve equation (10), the corresponding gradient can be computed by leveraging the insight that the adjoint operator is similar to a translation operation. In this way, the hurdle of designing a special adjoint operator for the multivariate case can be overcome; instead, a translation operation may be performed to approximate the required gradient. This multivariate framework serves the purpose of computing a weight vector similar to equation (3) for each demonstration on the Riemannian manifold M.

Below we explain how MLGM can be used when the demonstration data corresponds to quaternion orbitals, i.e., when ^M≡S3 .

If the human demonstration is characterized by orthogonal movement patterns (either via kinesthetic instruction or teleoperation), it is necessary to have a learning model 112 that encompasses both translational and rotational movements of the robot end effector. This is because the robot end effector is able to learn the robot's motions based on the given demonstration trajectory.
where the data points representing the complete orthogonal pose of the end effector at time step t are
The problem in this case is that
Since the Euclidean case in follows classical ProMP, we train ProMP in the orientation space.

First
An equivalent expression for is introduced to resemble the linear basis function model in equation (1) in the MGLM framework. Specifically, the estimated value
and here it is as follows:

This equivalence proved useful when establishing the similarity between the classical formulation of ProMP and our proposed approach for orientation trajectories. Similar to equation (1), the points y _t ∈ M in τ can be expressed as a geodesic basis function model as follows:
P(y _t │w)=N _M (y _t │Exp _p (Ψ _t w), Σ _y ) (12)
where p is a fixed base point on M,
is a large weight vector concatenating N _φ weight vectors w _n ∈ _{T p} M, Ψt is a matrix of time-dependent basis functions the same as in equation (1), and Σ _y is
is a covariance matrix encoding the uncertainty above. Two special aspects of this formulation are particularly noteworthy: (i) the mean of the Riemann-Gassus distribution in equation (12), i.e., Exp _p (Ψ _t w)∈M, exploits the equivalent formulation of the MGLM discussed above, and (ii) the weight vector forming w in equation (12) corresponds to the vectors that make up the geodesic basis of the MGLM.

Since every demonstration is characterized by a different weight vector w, we again obtain the distribution P(w; θ) = N(w|μ _w , Σ _w ). The marginal distribution of y _t can therefore be calculated as follows:
P (y; θ) = ∫N _M (y│Exp ₍ Ψw), Σ _y ) N (w│μ _w , Σ _w ) dw (13)
where the marginal distributions depend on two probability distributions on different manifolds (for simplicity, we omit the time index here and below). However, the mean μ _y depends on a single fixed point p∈M and μ _w ∈T _p M. These two observations are exploited to solve the bound (13) on the tangent space T _p M as follows:
where:
is the translation covariance Σ _y from μ _y to p. Note that this marginal distribution is still on the tangent space T _p M, so it is mapped back to M using the exponential map. This finally gives us the marginal distribution
however,

As mentioned above, the learning process of ProMP boils down to estimating the weight distribution P(w;θ). To do so, for each demonstration i, the controller 106 uses the MGLM to estimate the weight vector
First, the equivalent formula for _yt introduced earlier is used, where:
is the number of basis functions. Furthermore, the quaternion orbital demonstrated with y _t ∈ ^{S 3}
Then, in Euclidean space similar to equation (3), the weight estimates are obtained, here by exploiting equation (10), to give:
where φ _t is the vector of basis functions at time t, and W is the estimated tangent weight vector
(i.e., a set of N _φ tangent vectors emerging from a point pεM).

Figure 3 shows the multivariate general linear regression on the spherical manifold ^S2 used to train the weights of the orientation ProMP. Given the trajectory y, the origin p of the tangent space _TpM and the tangent weight vector _wn are estimated via equation (15).

To solve equation (15), the gradients of E(p, w _n ) with respect to p and each w _n are calculated. As explained above, these gradients depend on so-called adjoint operators, roughly speaking, for each error term
of,
Using
to T _p M. These adjoint operators can therefore be approximated as translation operations, which leads to the reformulation of the error function in equation (15) as

Then the approximate gradient of the error function E(p, w _n ) corresponds to:
Using the above gradient, the controller 106 can estimate, for each demonstration i, both the vector p _i formed by the N _φ vectors w _n and the weight matrix W _i . Note that each demonstration may lead to a different estimate of p, which defines the origin in the manifold M used to estimate each tangent weight vector w _n ∈T _p M. This may result in different tangent spaces across the demonstrations, and therefore a wide variety of tangent weight vectors. An effective approach to overcome this problem is to assume that all demonstrations share the same tangent space origin p, the same assumption made when defining the geodesic basis function model (Equation (12)). Thus, according to various embodiments, the controller 106 estimates p for a single demonstration and uses it to estimate all tangent weight vectors for the entire set of demonstrations. Then, given a set of N demonstrations, the weight distribution parameter θ={μ _w , Σ _w } is given by
can be estimated by standard maximum likelihood methods as

An example of a learning algorithm for the robot control model 112 with oriented ProMP that the controller 106 can execute after a set of N demonstrations has been provided (e.g., provided by a user by manually moving the robot arm 101) is as follows:

Similar to classical ProMP, the controller 106 calculates the associated covariance
A desired trajectory point with
Trajectory modulation (i.e., control scenarios to adapt to new situations) can be performed by adjusting the operation to reach . This results in a conditional probability that depends on two probability distributions on different manifolds, similar to equation (13).
Here again we exploit the fact that the mean μ _y depends on a single, fixed p∈M, which is a basis for the tangent space T _p M in which the weight distribution lies. This allows us to rewrite the conditional distribution as
where:
are the parameters to estimate for the resulting conditional distribution. Since both distributions now reside on T _p M embedded in Euclidean space, the new distribution parameters can be estimated similarly to the classical ProMP conditioning procedure, with special attention to the translation of the covariance matrix. The new weight distribution parameters are then:

From the resulting new weight distribution, new marginal distributions P(y;θ ^* ) may also be obtained, here via equation (14).

Regarding blending, classical ProMP blends a set of motion primitives by using a product of Gaussian distributions. When blending primitives in M, it is necessary to consider that each trajectory distribution is parameterized by a set of weight vectors on a different tangent space TpM. Therefore, the weighted product of Gaussian distributions needs to be reformulated. To do so, according to various embodiments, a formulation of a Gaussian product on a Riemannian manifold is used, where the log-likelihood of the product is iteratively maximized using a gradient-based approach.

Formally, the log-likelihood of a product of Riemann-Gausian distributions is given by (excluding the constant term):
where μ _y,s and Σ _y,s are parameters of the marginal distribution P _s (y; θ) for skill s. Note that the logarithmic map in (20) is
Note that this operates on. To perform log-likelihood maximization, the cardinality and argument of the map are swapped while leaving the original log-likelihood function unchanged. To do so, the relationship Log _x (y) = -Log _y (x) as well as a translation operation can be exploited to overcome this issue, resulting in:
where μ ⁺ is the mean of the resulting (estimated) Gaussian,
Equation (21) is the vector
and a block diagonal matrix
As a result, J has the form of an objective function used to compute the empirical mean v of a Gaussian distribution on a Riemannian manifold M,
From there, it is possible to iteratively calculate the average as follows:
where J is the Jacobian of ε(ν) with respect to the basis of the tangent space of M at ν _k . The controller 106 can now perform a similar iterative estimation of the mean μ ⁺ as shown below:
however,
After convergence in iteration K, the controller 106 obtains the final parameters of the distribution P(y ⁺ )=N _M (y ⁺ |μ ⁺ ,Σ ⁺ ) as shown below:

As explained above, in classical ProMP, the weight distribution P(w; θ) = N(w|μ _w , Σ _w ) is defined as the external task parameter
can be fitted as a function of , where for each demonstration
The task parameterization is assumed to have access to the values of the weight vector
This also applies to the orientation ProMP as
can be applied directly as long as is Euclidean, except that
If belongs to a Riemannian manifold, a more general approach is needed.

Task parameters
, the controller 106 uses a Gaussian mixture model on a Riemannian manifold to obtain the joint probability distribution
The controller 106 can then learn the new task parameters
If provided, during playback
Gaussian mixture regression can be employed to calculate

To better illustrate how model learning, trajectory recovery, waypoint matching, and skill blending work in Oriented ProMP, a dataset of handwritten characters was used. The original trajectory is
The trajectories were generated in and then projected to ^S2 by a simple mapping to a unit-norm vector. Each character in the dataset was demonstrated N = 8 times, and a simple smoothing filter was applied to each trajectory, primarily for visualization purposes. Four ProMP models were trained, one for each character in the set {G, I, J, S}. N _φ = 30 basis functions with uniformly distributed centers were used for the models trained for I and J, and N _φ = 60 basis functions were used for the characters G and S. The oriented ProMP models were trained according to the algorithm given above, with an initial learning rate α = 0.005 and a corresponding upper limit α _max = 0.03.

Figure 4 shows the demonstration data, the marginal distributions P(y;θ) calculated via equation (13), and the waypoint fits obtained from equations (18) and (19) corresponding to models trained on the letters G and S. The means of the marginal distributions follow the demonstration patterns, and the corresponding covariance profiles capture the variability of the demonstrations in ^S2 . The trajectories of letters G and S exhibit very elaborate "movement" patterns that may be even more complex than those observed in realistic robotic settings, and their complexity is noteworthy. For waypoint fits, the covariance
A random point y ^* ∈ ^S2 associated with y* was used (i.e., high accuracy was required when passing through y ^* ).

As shown in Figure 4, Orientation ProMP can smoothly fit both the trajectory and the associated covariance profile while accurately passing through a given waypoint.

Figure 5 shows the blending process of oriented ProMP for {G,I} and {S,J}.

The goal was to generate trajectories that start by following the profile of the first character in the set, and then smoothly switch to the trajectory distribution of the second character midway. Figure 5 shows the resulting blended trajectories for the two aforementioned cases, where Oriented ProMP smoothly blends the two given trajectory distributions by following the blending procedure for Oriented ProMP introduced above. Note that the blending behavior strongly depends on the temporal occurrence of the weights α _s ∈ [0,1] associated with each skill s. In this set of experiments,
While the weight
and
A sigmoid-like function was used for . The preceding results show that OrientedProMP successfully learns and reproduces the trajectory distribution on ^S2 , providing perfect waypoint matching and blending capabilities.

Experiments have shown that this holds true equally well in a robotic setup for a reorientation skill, which corresponds to, for example, picking up a previously grasped object, rotating the end effector 104, and placing the object back in its original location, but with a different orientation. This robotic skill features large changes in position and orientation, and is therefore well suited to showcasing the functionality of OrientationProMP.

To train robotic skills such as reorientation skills, each demonstration includes, e.g., a full-pose robot end-effector trajectory.
where,
represents the pose of the end effector at time step t. Thus, each demonstration is a position trajectory (each
We demonstrate the use of S3 trajectories (each containing a time series of positions described by elements of S3) and orientation trajectories (each containing a time series of orientations described by elements of ^S3 ). The raw data from these trajectories included a sub-model for position and a sub-model for orientation.
The ProMP model 112 may be used to train the ProMP model 112, where the position model is learned using the classical ProMP approach and the orientation model is learned using the orientation ProMP approach (e.g., the algorithm described above). For both of these sub-models, the same (e.g., N _φ = 40) set of basis functions may be used, but for different components (for each position component in the position sub-model and each orientation component in the orientation sub-model).

In summary, according to various embodiments, the method is provided as shown in Figure 6.

Figure 6 shows a flowchart 600 illustrating a method for controlling a robotic device.

In step 601, demonstrations are provided for a robot skill, where each demonstration demonstrates a trajectory including a sequence of robot configurations, where each robot configuration is described by an element of a predetermined configuration space having the structure of a Riemannian manifold.

In step 602, for each demonstrated trajectory, a representation of the trajectory as a weight vector of predetermined primitive motions of the robotic device is determined by searching for a weight vector that minimizes a distance measure between the combination of primitive motions according to the weight vector and the demonstrated trajectory, where the combination is mapped to a manifold.

In step 603, a probability distribution for the weight vector is determined by fitting a probability distribution to the weight vector determined for the demonstrated trajectory.

In step 604, the robotic device is controlled by performing primitive movements according to the determined probability distribution of the weight vector.

This can involve sampling from a probability distribution of weight vectors (according to equation (1)) and performing primitive movements according to the sampled vector. It is also possible to derive probability distributions of trajectories (according to equation (14)), one of which can be sampled for control, which may be used for more advanced control such as blending trajectories as described above.

The method of FIG. 6 may be performed by one or more computers including one or more data processing units. The term "data processing unit" may be understood as any type of item that enables processing of data or signals. For example, data or signals may be processed according to at least one (i.e., one or more) specific functions performed by the data processing unit. The data processing unit may include or be formed from an analog circuit, a digital circuit, a composite signal circuit, a logic circuit, a microprocessor, a microcontroller, a central processing unit (CPU), a graphics processing unit (GPU), a digital signal processor (DSP), a programmable gate array (FPGA) integrated circuit, or any combination thereof. Any other manner of implementing the respective functions may be understood as a data processing unit or logic circuit. It will be understood that one or more of the method steps detailed herein may be performed (e.g., implemented) by a data processing unit through one or more specific functions performed by the data processing unit.

Various embodiments may receive and use image data from various visual sensors (cameras), such as video, radar, LiDAR, ultrasound, thermal imaging, sonar, etc., to acquire data for demonstration purposes.

The approach of FIG. 6 can be used to compute control signals for controlling a physical system, such as a computer-controlled machine, such as a robot, a vehicle, an appliance, a power tool, a manufacturing machine, a personal assistant, or an access control system. According to various embodiments, a policy for controlling the physical system may be learned, and the physical system may then be operated accordingly.

According to one embodiment, this method is computer-implemented.

While specific embodiments have been shown and described herein, it will be apparent to those skilled in the art that the specific embodiments shown and described may be substituted with various alternative and/or equivalent implementations without departing from the scope of protection of the present invention. This application is intended to cover any adaptations or variations of the specific embodiments discussed herein. Therefore, it is intended that the present invention be limited only by the claims of this application and their equivalents.

Claims (9)

1. A method for controlling a robotic device, the method comprising:
providing demonstrations for a robotic skill, each demonstration demonstrating a trajectory including a sequence of robot configurations, each robot configuration being described by an element of a predetermined configuration space having the structure of a Riemannian manifold;
determining, for each demonstrated trajectory, a representation of the trajectory as a weight vector of predetermined primitive movements of the robotic device by searching for a weight vector that minimizes a distance measure between the demonstrated trajectory and a combination of primitive movements according to the weight vector, the combination being mapped onto the manifold;
determining a probability distribution for a weight vector by fitting a probability distribution to the weight vector determined for the demonstrated trajectory;
and controlling the robotic device by executing primitive movements according to the determined probability distribution of the weight vector. The method of claim 1, wherein the probability distribution of the weight vector is determined by fitting a Gaussian distribution to the weight vector determined for the demonstrated trajectory. The method of claim 1 or 2, wherein each of the demonstrated trajectories includes a robot configuration for each time point in a predetermined sequence of time points, and each combination of primitive movements according to a weight vector specifies a robot configuration for each time point in the predetermined sequence of time points, and for each of the demonstrated trajectories, the weight vector is determined by determining a weight vector for the combination of primitive movements according to the weight vector and the demonstrated trajectory from a set of possible weight vectors, the combination being mapped to a manifold that is smallest among the set of possible weight vectors, and the distance between the combination of primitive movements mapped to the manifold and the demonstrated trajectory is given by a sum over time points in the sequence of time points, including a term for each time point that includes a value or a power of a value of the manifold metric between the demonstrated trajectory and an element of the manifold given by the combination of primitive movements at the time point when mapped to the manifold. 3. The method of claim 1, further comprising: searching for a point and a weight vector of a manifold such that, for one of the demonstrated trajectories, a distance measure between a combination of primitive movements according to the weight vector and the demonstrated trajectory is minimized, the combination being mapped onto the manifold from a tangent space at a point; and , for each of the demonstrated trajectories, mapping each combination onto the manifold is performed by mapping the combination from a tangent space at a selected point. 3. The method of claim 1, wherein the trajectories are orientation trajectories, and wherein each of the demonstrations further demonstrates a position trajectory, and wherein each of the robot configurations includes a pose described by a vector in three-dimensional space and an orientation described by elements of a predetermined configuration space. 3. The method of claim 1, further comprising the steps of: providing a demonstration of more robot skills; determining, for each skill, a representation of a trajectory, a weight vector, and a probability distribution of the weight vectors; controlling the robotic device by determining, for each skill, a Riemannian-Gauss distribution of manifold points from the probability distribution of the weight vectors; determining a product distribution of the Riemannian- Gauss distributions of the skills; and controlling the robotic device by sampling from the determined product probability distribution. A robotic device controller configured to perform the method of claim 1 or 2 . A computer program comprising instructions which, when executed by a processor, cause the processor to carry out a method according to claim 1 or 2 . A computer readable medium having stored thereon instructions which, when executed by a processor, cause the processor to perform the method of claim 1 or 2 .