Method and system of generating a feasible smooth reference trajectory for an actuator
A method of controlling an actuator having a maximum velocity and a maximum acceleration includes accessing a reference trajectory signal comprising temporally-spaced reference positions for the actuator and accessing a trajectory template having at least a third derivative continuity and at least one characteristic constrained by the maximum velocity and the maximum acceleration. The method includes generating a smooth reference trajectory signal based on the reference trajectory signal and the trajectory template and outputting the smooth reference trajectory signal. Controls for the actuator can be generated based on the smooth reference trajectory signal.
1 . A method comprising:
obtaining a first reference trajectory signal comprising first temporally-spaced reference positions for an actuator of a robot;
generating a trajectory template having at least a third derivative continuity and at least one characteristic constrained by a maximum velocity and a maximum acceleration associated with the actuator;
generating a delayed trajectory from the first reference trajectory signal comprising first temporally-spaced reference positions, wherein the delayed trajectory comprises an array of delayed signals sampled from the first reference trajectory signal at a first sampling rate;
generating a smooth reference trajectory signal based on the delayed trajectory and the trajectory template;
generating controls for the actuator based on the smooth reference trajectory signal; and
outputting the controls for the actuator to a driver of the actuator.
2 . The method of claim 1 , further comprising:
receiving a second reference trajectory signal comprising second temporally-spaced reference positions for the actuator; and
deriving the first reference trajectory signal from the second reference trajectory signal.
3 . The method of claim 2 , wherein receiving the second reference trajectory signal comprises sampling the second reference trajectory signal at a second sampling rate, and wherein deriving the first reference trajectory signal from the second reference trajectory signal comprises enforcing the maximum velocity on the second reference trajectory signal to obtain the first reference trajectory signal.
4 . The method of claim 3 , wherein enforcing the maximum velocity on the second reference trajectory signal to obtain the first reference trajectory signal comprises:
determining that a slope of the second reference trajectory signal at a given reference position represented by a given sample of the second reference trajectory signal exceeds a threshold;
determining a new value for the given reference position such that the slope of the second reference trajectory signal at the new value does not exceed the threshold; and
generating a portion of the first reference trajectory signal corresponding to the given sample of the second reference trajectory signal using the new value for the given reference position.
5 . The method of claim 4 , wherein the threshold is the maximum velocity.
6 . The method of claim 4 , wherein the new value for the given reference position is determined such that the slope at the new value for the given reference position is equal to the maximum velocity.
7 . The method of claim 3 , wherein enforcing the maximum velocity on the second reference trajectory signal to obtain the first reference trajectory signal comprises replacing a step function in the second reference trajectory signal with a ramp having a ramp rate equal to the maximum velocity.
8 . The method of claim 3 , wherein the smooth reference trajectory signal is outputted to a controller of the actuator, and wherein sampling the second reference trajectory signal at the second sampling rate comprises sampling the second reference trajectory signal from an output of a controller at a higher level compared to the controller of the actuator.
9 . The method of claim 1 , wherein generating the smooth reference trajectory signal based on the delayed trajectory and the trajectory template comprises:
determining a delay time from a ratio of the maximum velocity to the maximum acceleration; and
determining a number of the plurality of delayed signals to use in generating the smooth reference trajectory based on the delay time and the first sampling rate.
10 . The method of claim 9 , wherein the at least one characteristic is a time length of the trajectory template, and further comprising determining the time length of the trajectory template based on the delay time.
11 . The method of claim 9 , wherein the time length of the trajectory template is determined as twice the delay time.
12 . The method of claim 1 , wherein generating the smooth reference trajectory signal based on the delayed trajectory and the trajectory template comprises:
determining a position difference between a first delayed signal and a second delayed signal from the array of the delayed signals at a current time step;
generating a scaled trajectory template based on the trajectory template and the position difference; and
generating a trajectory plan at the current time step based on the scaled trajectory template and a trajectory plan at a previous time step.
13 . The method of claim 12 , wherein the first delayed signal and the second delayed signal have a least amount of delay in the array of the delayed signals.
14 . The method of claim 12 , wherein the trajectory plan at the previous time step does not exist, and further comprising forming the trajectory plan at the previous time step from the delayed trajectory prior to generating the trajectory plan at the current time step.
15 . The method of claim 12 , wherein outputting the smooth reference trajectory comprises outputting a value from the trajectory plan at the current time step.
16 . The method of claim 1 , wherein adjusting at least one characteristic of the trajectory template based on the maximum velocity and the maximum acceleration associated with the actuator comprises adjusting a time length of the trajectory template.
17 . The method of claim 1 , further comprising generating the trajectory template from a jerk profile comprising a sequence of alternating non-zero-jerk segments and zero-jerk segments.
18 . The method of claim 17 , wherein each non-zero-jerk segment has a first time length and each zero-jerk segment has a second time length, and wherein a ratio of the first time length to the second time length is in a range from 0 to 1.
19 . The method of claim 18 , wherein the ratio of the first time length to the second time length is 0.5 or less.
20 . A system comprising:
an actuator coupled to a joint of a robot, the actuator comprising a motor and a motor driver, wherein the actuator has a maximum velocity and a maximum acceleration associated therewith;
a computing system communicatively coupled to the robot, the computing system comprising one or more hardware processors and one or more computer-readable storage devices that store computer-readable instructions executed by the one or more hardware processors;
a first controller implemented in the computing system, the first controller configured to output a first reference trajectory signal comprising temporally-spaced reference positions for the actuator;
a second controller implemented in the computer system, the second controller configured to receive a smooth reference trajectory signal and generate controls for the motor driver based on the smooth reference trajectory signal; and
a trajectory preprocessor implemented in the computer system and communicatively coupled to the first controller and the second controller, wherein the trajectory preprocessor comprises:
a first preprocessing block configured to receive samples of the first reference trajectory signal outputted by the first controller and enforce the maximum velocity on the samples of the first reference trajectory signal to obtain a second reference trajectory signal;
a second preprocessing block configured to generate a delayed trajectory comprising an array of delayed signals sampled from the second reference trajectory signal;
a third preprocessing block configured to generate a trajectory template having at least a third derivative continuity and at least one characteristic constrained by the maximum velocity and the maximum acceleration; and
a fourth preprocessing block configured to generate the smooth reference trajectory signal based on the delayed trajectory and the trajectory template and output the smooth reference trajectory signal.
21 . The system of claim 20 , wherein the second controller comprises an impedance controller.
22 . The system of claim 20 , wherein the third preprocessing block comprises a tapped delay block.
23 . The system of claim 20 , wherein the first controller is communicatively coupled to a teleoperator system or a cognitive platform associated with the robot and outputs the first reference trajectory signal based on commands from the teleoperator system or the cognitive platform.