EP-4739466-A1 - METHOD AND APPARATUS FOR LOW-COMPLEXITY TRAJECTORY PLANNING

EP4739466A1EP 4739466 A1EP4739466 A1EP 4739466A1EP-4739466-A1

Abstract

The present disclosure relates to a method for planning a trajectory for at least one actuator. The trajectory is based on a continuous spline of degree n within a planning time frame comprising a plurality of time intervals. A plurality of initial values and a trajectory objective for the planning time frame are provided. Furthermore, the method includes providing a plurality of discrete trajectory constraints enforced at a respective start and/or end of the time intervals, that are related to the physical parameter and its first to n-th derivative with respect to time. Based on the trajectory objective, the plurality of discrete trajectory constraints, and the plurality of initial values, a piecewise constant function is computed, each piece representing a constant value for the n -th derivative of the physical parameter within the respective time interval. A continuous trajectory function in the form of a continuous spline of degree n is obtained for the planning time frame by integration calculations of the piecewise constant function. The continuous trajectory function is sampled in accordance with a sampling time interval length, and the actuator is controlled based on the samples.

Inventors

RICHTER, STEFAN

Assignees

Sony Group Corporation
Sony Europe B.V.

Dates

Publication Date: 20260513
Application Date: 20240619

Claims (19)

1. A method for planning a trajectory for at least one actuator, the trajectory being based on a continuous spline of degree n within a planning time frame comprising a plurality of time intervals, the method comprising: providing a plurality of initial values including an initial value for a physical parameter of the actuator and a respective initial value for a first to an (n-1 J -th derivative of the physical parameter with respect to time at the beginning of the planning time frame; providing a trajectory objective for the planning time frame; providing a plurality of discrete trajectory constraints at a respective start and/or end of the time intervals, the trajectory constraints being related to the physical parameter and its first to //-th derivative with respect to time; computing a piecewise constant function, each piece representing a constant value for the //-th derivative of the physical parameter within the respective time interval, wherein the piecewise constant function is computed based on the trajectory objective, the plurality of discrete trajectory constraints, and the plurality of initial values; computing a continuous trajectory function in the form of a continuous spline of degree n for the planning time frame by integration calculations of the piecewise constant function; sampling the continuous trajectory function to obtain samples in accordance with a sampling time interval length Z; and controlling the actuator based on the samples of the continuous trajectory function.
2. The method of claim 1, wherein each time interval of the plurality of time intervals in the planning time frame comprises a pre-determined spline time interval length T.
3. The method of claim 2, wherein the continuous trajectory function corresponds to a mathematical spline comprising a plurality of spline states defined at respective multiples of the pre-determined spline time interval length T, wherein a spline state at a time corresponding to a respective multiple is defined by the value for the physical parameter and the respective values for its first to (n-1 )-th derivatives, and wherein the spline state at a second time corresponding to a multiple [/ + 1] of the pre-determined spline time interval length T is computed based on the spline state at a first time corresponding to the previous multiple [7] and based on the value for the //-th derivative between the first and second time.
4. The method of claim 1, wherein the plurality of initial values include an initial value for the physical parameter w(0) and an initial value for the first derivative of the physical parameter, w (1) (0), and optionally a respective initial value for one or more higher order derivatives of the physical parameter, and wherein computing the continuous trajectory function by integration calculations is performed for each interval of the piecewise constant function at least in part by applying the following formula:
5. The method of claim 1, wherein the physical parameter is a translational or rotational position,/?, a velocity, v, or an acceleration, a, of the actuator or directly proportional thereto.
6. The method of claim 1, wherein the trajectory is based on a continuous spline of the third degree, wherein the physical parameter corresponds to a position, /?, of the actuator and the first, second, and third derivatives of the physical parameter correspond to a velocity, v, an acceleration, a, and a jerk, j, of the actuator, respectively, and wherein the plurality of initial values include an initial value for the position, p(0), for the velocity, v(0), and for the acceleration, a(0), and wherein computing the continuous trajectory function by integration calculations is performed for each interval of the piecewise constant function at least in part by applying the following formula:
7. The method of claim 1, wherein the trajectory objective is encoded in terms of an objective function to be maximized or minimized and/or additional trajectory constraints.
8. The method of claim 2, wherein the pre-determined spline time interval length, T, is larger than the sampling time interval length, , and equivalent to a value = m- T. wherein m is a real number greater than one, and wherein the value for m is chosen based on a desired computation time.
9. The method of claim 1, wherein the samples are used to obtain a discrete trajectory function corresponding to the physical parameter, wherein the discrete trajectory function is formed with the physical parameter confined within predefined limits, and wherein values obtained by backward difference calculations approximating the first to //-th derivatives based on the sampling time interval length T are confined within respective predefined limits.
10. The method of claim 9, wherein each value of the discrete trajectory function is constant within a time period equivalent to the sampling time interval length T.
11. The method of claim 9, wherein the plurality of discrete trajectory constraints comprises one or more discrete trajectory constraint equations to be enforced at multiples of the pre-determined spline time interval length T and respective ranges corresponding to a respective range of acceptable values for the physical parameter and its first to //-th derivatives of the physical parameter at the multiples of the pre-determined spline time interval length T, wherein the respective ranges are enforced during computation of the piecewise constant function.
12. The method of claim 11, wherein the respective ranges corresponding to the physical parameter and the first to (z/-2)-th parameters each comprise shrinkage values A for further confining the respective range of acceptable values during computation of the piecewise constant function, wherein the shrinkage values A are determined based on the degree of the spline, and wherein the shrinkage values A are determined for confining the physical parameter within predefined values and implicitly confining all values obtained by backward difference calculations approximating the first to //-th derivatives based on the sampling time interval length T within respective predefined limits.
13. The method of claim 12, wherein the predefined limits are determined at least in part for safety -critical applications.
14. The method of claim 9, wherein the actuator is a motor and the discrete trajectory function is a motor signal corresponding to at least one of a motor position signal, a motor speed signal, or a motor torque signal, and wherein the motor is configured to control a robot or drone or a part of a robot or drone based on the motor signal.
15. The method of claim 1, wherein the actuator is used for controlling a robot or drone or a part of a robot or drone along one or more rotational and/or translational axes.
16. The method of claim 15, further comprising Generating sensor data by one or more sensors of a surrounding environment of the robot or drone and updating the discrete trajectory constraints, the trajectory objective, the initial conditions and/or the pre-determined spline time interval length r based on the sensor data for further iterations of computing the continuous trajectory function and/or the discrete trajectory function.
17. A program having a program code for performing the method according to any of the previous claims when the program is executed on a processor or a programmable hardware.
18. A non-transitory machine-readable medium having stored thereon a program having a program code for performing the method according to any of the previous claims when the program is executed on a processor or a programmable hardware.
19. An apparatus for planning a trajectory for at least one actuator, the trajectory based on a continuous spline of degree n within a planning time frame comprising a plurality of time intervals, the apparatus comprising interface circuitry configured to: receive a plurality of initial values including an initial value for a physical parameter of the actuator and a respective initial value for a first to an (n-1 J -th derivative of the physical parameter with respect to time at the beginning of the planning time frame; receive a trajectory objective for the planning time frame; and receive a plurality of discrete trajectory constraints at a respective start and/or end of the time intervals, the trajectory constraints being related to the physical parameter of the actuator and its first to (//)-th derivative with respect to time; and further comprising processing circuitry configured to: compute a piecewise constant function, each piece representing a constant value for the //-th derivative of the physical parameter within the respective time interval, wherein the piecewise constant function is computed based on the trajectory objective, the plurality of discrete trajectory constraints, and the plurality of initial values; compute a continuous trajectory function in the form of a continuous spline of degree n for the planning time frame by integration calculations of the piecewise constant function; sample the continuous trajectory function in accordance with a sampling time interval length; and control the actuator based on the samples of the continuous trajectory function.

Description

Method and Apparatus for Low-Complexity Trajectory Planning Field The present disclosure is related to a method and an apparatus for planning a trajectory of an actuator. Background As technical systems increasingly interact with their environment, it is of foremost importance to enable them to react quickly to dynamically changing conditions or goals. This calls for trajectory planning algorithms that can be applied in real-time. One candidate to achieve this is optimization-based planning, which solves an optimization problem to derive a new trajectory. Optimization-based planning has the advantage of allowing one to specify the solution characteristics transparently by means of equality and inequality constraints and to specify the goal by means of an appropriate objective function. For many technical systems, a subset of the constraints that must be strictly met are formulated as constraints on a physical parameter, such as a position and its time derivatives, such as a velocity, acceleration, jerk, and/or generally any time derivative up to order n. Since nowadays most control systems are zero-order-hold sampled with a sampling time interval length T, these time derivatives are approximated by so-called backward differences, with bk ^ = wherein b^ = uk and uk is the physical parameter at time t = k - T, recursively defining the backward difference of order j at time step k. A control algorithm receiving, for example, a desired sequence of positions to be tracked, usually first verifies backward differences of orders up to n at all time steps k and only processes them if these backward differences - as valid approximations of the time derivatives of the physical parameter, such as the velocity, acceleration, jerk, etc. - satisfy hard lower and upper bounds on them. In a case where only one of these bounds is violated, the control system cannot guarantee safe operation anymore and may bring the system to a halt. This implies that the current goal cannot be achieved and that potentially tedious procedures must be undertaken to bring the technical system into a safe state from which it may resume its action. The state of the art is to solve this problem by enforcing lower and upper bounds on each backward difference in the optimization problem. This leads to a large number of decision variables and equality constraints defining backward differences and makes real-time planning of trajectories for longer trajectory lengths difficult to impossible. This may especially be the case if a robot has multiple axes that must be coordinated in a central manner, which would prevent the technical system to meaningfully interact with its environment. Another problem to consider is that the hardware and software of the technical system may be subject to change from the manufacturer, such as by increasing the sampling rate to increase control performance. Even if a complex problem can be solved at the original sampling time interval length in real-time, there is a potential that a decreased sampling time interval length would lead to longer computation times or create difficulties in real-time trajectory planning. In the best case scenario, the computation time increases linearly with the increased sampling rate. Thus, there is a demand for technical systems that apply optimization-based trajectory planning with a decreased complexity for enabling continued interaction with a surrounding environment. Summary This demand is addressed by a method and an apparatus in accordance with the independent claims. Possibly advantageous embodiments are addressed by the dependent claims. According to a first aspect, the present disclosure proposes a method for planning a trajectory for at least one actuator. The trajectory is based on a continuous spline of degree n within a planning time frame comprising a plurality of time intervals. The method includes providing a plurality of initial values including an initial value for a physical parameter of the actuator and a respective initial value for a first to an (//- I -th derivative of the physical parameter with respect to time at the beginning of the planning time frame. The method further includes providing a trajectory objective for the planning time frame. Furthermore, the method includes providing a plurality of discrete trajectory constraints at a respective start and/or end of the time intervals that are related to the physical parameter and its first to //-th derivative with respect to time. The method further includes computing a piecewise constant function. Each piece represents a constant value for the //-th derivative of the physical parameter within the respective time interval. The piecewise constant function is computed based on the trajectory objective, the plurality of discrete trajectory constraints, and the plurality of initial values. The method further includes computing a continuous trajectory function in the form of a continuous spline of degree n for the