Correction Blending of Path Ends

There are optimal methods for resolving the differences in initial and final con- ditions, most notably those of neighboring optimal control (NOC) [13, 119]. For systems where these differences are critical, NOC is recommended. Experimentation with this system suggested that the differences in initial conditions were very small (as it will be for systems where the calculation time is fairly predictable). During flight test, the longest calculation time was only 0.11 seconds beyond what was an- ticipated, leading to very small initial discontinuities. Changes at the “tail” of the path can be substantial, depending on how far the target estimate moves during each measurement update.

Stability for the quadrotor system in the face of a discontinuity in commanded trajectory was never a question, as the autopilot was designed with velocity limits to be stable for any size command step. The tail of the path was certainly more sensitive to measurement updates, but until the end-game, the tail portion of the path will be replaced each epoch before it is actually flown. As a result, the computational expense of NOC was forgone for a simple and efficient strategy that ensured the path would always end at the most current target estimate, but without discernible delay. This correction is critical for the last seconds of the flight, but is a nice feature for robustness as well, as the path “in hand” is always based on the best information at the time, and is the best plan to follow in the case of mechanical failure of the optimal solver, or a delay caused by an inability to converge on a solution.

The initial condition discontinuities can occur when the optimal solution for epoch 𝑘 + 1, available at 𝑡𝑘+1, arrives later than the expected time of 𝑡0𝑘+1. Until 𝑡𝑘+1, the system continues to fly the solution that was produced for epoch 𝑘. The final condition discontinuities occur when the trajectory planner delivers a path for epoch 𝑘 + 1 to the assumed target,_ˆx−_𝑡𝑘+1, that has been updated by the estimation algorithm tox_ˆ+_𝑡𝑘+1 during the calculation time of the path planner. Sample results of the blending can be seen in Figure 37, where the dark black line indicates the path that is sent to the vehicle at the actual update time 𝑡𝑘+1. The path sent is a composite of the solid

optimal solution at x∗_𝑘, the dashed solution at x∗_𝑘+1, and the blending correction as a result of updating the target to _ˆx+_𝑡

𝑘+1during calculation time.

Figure 37. Cosine Blending Corrections

To produce the blending without generating the sharp changes of trajectory with a linear blending method, a cosine wave was used to “round the corners” and smoothly transition the head or tail of the path to the corrected point. The calculations are described here for the tail of the path with a discrete time series vector, as it is actually applied in the physical system. The length of the blending segment, 𝑡𝑏𝑙𝑚𝑎𝑥,

is selected by the desired segment time, 𝑡𝑠𝑒𝑔, limited to the amount of time remaining

if the path is already within the final window:

𝑡𝑏𝑙𝑚𝑎𝑥 = min [𝑡𝑠𝑒𝑔, 𝑡𝑓 − 𝑡𝑘− rem (tf − tk, Δt)] (131)

The remainder function (rem) is used to ensure an even division by Δ𝑡 (𝑡𝑠𝑒𝑔 is

chosen this way as well). In practice, it was found that blending initial differences (if they exist) over one second, and final differences over 5 seconds was efficient and effective. A time vector is then produced:

𝑡𝑏𝑙 = [0, Δ𝑡, 2Δ𝑡, . . . , 𝑡𝑏𝑙𝑚𝑎𝑥] 𝑇

(132)

For corrections at the tail of the path, the point at which the new path departs from the old should be smooth. A correction wave vector from zero to one with a slow initial transition is created using one-quarter of a cosine wave period, and is directionally scaled by the amount the target estimate was moved in each state at the last batch update to create a correction matrix:

Ξ0→1 = 1 − cos (𝜋𝑡𝑏𝑙/2𝑡𝑏𝑙𝑚𝑎𝑥) Ξ𝑐𝑜𝑟𝑘 = Ξ0→1 ( ˆ x+_𝑡 𝑘 −ˆx − 𝑡𝑘 )𝑇 (133)

The correction matrix is then used to update the path segment:

x+_𝑘[𝑡𝑎𝑝𝑝− 𝑡𝑏𝑙𝑚𝑎𝑥, 𝑡𝑎𝑝𝑝] = x −

𝑘[𝑡𝑎𝑝𝑝− 𝑡𝑏𝑙𝑚𝑎𝑥, 𝑡𝑎𝑝𝑝] + Ξ𝑐𝑜𝑟𝑘 (134)

When this technique is used to correct discontinuities in initial conditions, the error to be rectified is measured from beyond the moment when the new path becomes available, at 𝑡𝑘+1+ 𝑡𝑏𝑙𝑚𝑎𝑥. During the flare segment of the flight, a similar technique is

also used to generate a horizontal profile, providing a smooth path from the approach point to the point where the vehicle actually hooks the wire. In both of these cases, both ends of the blend are desired to be smooth, so a correction wave from zero to one such as the one in Equation 133 is used with a higher frequency (one-half of a full cosine wave). For the initial condition blending, this allows both the initial move from the old path and the blend into the new path to both have smooth transitions:

Ξ0→1𝑓 𝑙𝑎𝑟𝑒 =1/2−1/2cos (𝜋𝑡𝑏/𝑡𝑏𝑙𝑚𝑎𝑥)

Ξ𝑐𝑜𝑟𝑓 𝑙𝑎𝑟𝑒 = Ξ0→1𝑓 𝑙𝑎𝑟𝑒(ˆ𝑥𝑡− 𝑥𝑎𝑝𝑝)

(135)

7.1.3.1 Process Threading.

The last noteworthy implementation lesson came from timing synchronization problems stemming from using a flexible calculation time for the optimal path planner on actual hardware. The UKF, trajectory planner, communication paths, autopilot processes, and speed control servos are each running iterative loops, but all at different rates. Threading loops with known rates is not difficult, but the trajectory planner has a variable cycle time (just over 1 Hz for this application). Working at a much faster rate (50 Hz), the autopilot must have a buffer of future commands to process, and a “dealer” function was implemented as a solution to run between the programs

as a storage place for each epoch’s optimal path time history. This allowed the

flight control and optimization algorithms to be carried out on separate processors, and handled the asynchronous timing between them without resorting to slowing the process by saving the path to a file. A dealer function can be run at high speeds, checking for a complete path update (the “deck” if you will) without ceasing to provide a list of commanded positions and heading at each time step of the autopilot.

When a new optimal path is formed, it is sent via TCP packets using a blocking protocol to stop processing on the low, variable rate processor until the deck is picked

up. This makes any delay less than one time step of the higher rate function (the dealer runs at 100 Hz), and ensures the new path can be used as soon as it is ready. A similar method was used in the other direction to get measurements, limits, and initial conditions into the RTOC process, stopping processing after sending a “ready” poll, checked for during each loop of the dealer. With this technique, slowing down the trajectory planner to allow a fixed calculation time is not necessary.