Bezier curve flightpath guidance using moving waypoints

[0001] The present invention relates generally to flightpath guidance, and, more particularly, to the use of Bezier curves to command flightpath guidance using moving waypoints for unmanned air vehicles.

[0002] To reduce the risk to human life, unmanned air vehicles (UAVs) are being developed for unpiloted, autonomous flight operations, such as surveillance or combat missions. Previously, drones were actively commanded and navigated with remote control during such operations or the vehicles were programmed to move according to a predetermined course.

[0003] UAV flight guidance systems are suited currently for formation flying, carrier landing, aerial refueling, and other common maneuvers.

[0004] Conventional navigation and guidance systems rely upon use of waypoints that are fixed in space and, typically, connected by straight line segments. This leads to ad-hoc modifications for guidance in turns, long linear queues for UAV and UCAV landing approaches that maximize the potential for dangerous wake vortex encounters, and excessively complex mission plans to achieve desired routes. Fixed waypoints are fundamentally incapable of effectively handling rendezvous problems such as carrier landing, aerial refueling, and formation flight, thus requiring separate modes for accomplishing each of these tasks. Other like conventional navigation and guidance systems are similarly limited by use of fixed waypoints or straight-line segments.

[0005] Thus it would be advantageous to design an improved flightpath guidance system and method for such applications as future unmanned vehicle systems and air traffic control systems.

[0006] In light of the foregoing background, embodiments of the present invention provide a flightpath guidance system and method using moving waypoints and curved line segments. One embodiment of a guidance system of the present invention includes a processing element for defining a path defined by a series of waypoints along which a vehicle may be guided, a current position of the vehicle relative to the intended path, a target position along the path, and a guidance command for steering the vehicle towards the target position. The processing element guidance command is capable of accounting for movement of at least one of the waypoints. A further embodiment of the present invention may include a target, or an ideal, position, defined by the processing element, for the vehicle along the path where the current position of the vehicle is perpendicular to a tangent to the path at the ideal position. A guidance subsystem may be included in an embodiment of the present invention to update the processing element definition of the current position and target position, thus creating a repeating loop in order to iterate the progression of the vehicle along the intended path through the series of waypoints.

[0007] A guidance subsystem for an embodiment of the present invention may be capable of checking waypoint sequencing, thus identifying which of the series of waypoints is the previous waypoint through which the vehicle has just passed and the current and next waypoints through which the vehicle will pass. Based upon checking waypoint sequencing, the guidance subsystem can identify that the current waypoint through which the vehicle has just passed should become a previous waypoint and, similarly, the next waypoint will become a current waypoint to which the vehicle is now guided along the ideal path. In embodiments of the present invention, the processing element may define the path between successive waypoints to be curved and the ideal path to be a Bezier curve. In further embodiments of the present invention successive waypoints along the series of waypoints of the path may be moving. The present invention also provides corresponding methods for controlling a vehicle in accordance with embodiments of the present invention.

[0008] A further embodiment of the present invention provides a method of navigating a vehicle including commanding guidance of the vehicle from a previous waypoint to a current waypoint, wherein departure from the previous waypoint is controlled by guidance commands based at least on the previous waypoint and approach to the current waypoint is controlled by guidance commands based at least on the current waypoint. Guidance of the vehicle may be commanded to reduce divergence from an intended position on the path based at least on the target distance along the intended path ahead of the intended position where the target distance ahead of the vehicle, or the intended position of the vehicle along the path, is constant.

[0009] Further embodiments of the present invention provide a vehicle capable of making adjustments to flight guidance based upon generated guidance commands derived from flight data such as movement of a waypoint defining an intended flight path. Such a vehicle may include a guidance system for controlling the movement of the vehicle, such as the hardware and electronics necessary to allow an operator or an automated system to control the flight of an airplane, a navigation processor capable of generating guidance commands for the guidance system, and a waypoint movement system, such as an optical tracking system or a communication system for receiving movement data from an external source such as a vehicle representing a moving waypoint. An associated method of the present invention is also provided for navigating a vehicle in flight. The vehicle may adjust crabbing into the wind to adjust for an intended flight path determined by the guidance commands of a navigational controller using a Bezier curve navigation process to account for movement of waypoints.

[0010] Curved flightpath guidance using moving waypoints of the present invention may provide improved automated aerial rendezvous and refueling and carrier landing capabilities for UAVs and UCAVs. The present invention may also be used for multiple vehicle applications, including conventionally manned and actively controlled vehicles, in the air and on the battlefield, such as by reducing cycle time for advance control systems for coordination of the multiple vehicles. The present invention may also provide increased separation and air traffic control and flexibility for greater safety and simplify commercial flight management systems and improve air transport efficiency.

[0016] The present inventions now will be described more fully hereinafter with reference to the accompanying drawings, in which some, but not all embodiments of the invention are shown.

[0017] Indeed, these inventions may be embodied in many different forms and should not be construed as limited to the embodiments set forth herein; rather, these embodiments are provided so that this disclosure will satisfy applicable legal requirements. Like numbers and variables refer to like elements and parameters throughout.

[0018] While embodiments of the present invention may be particularly useful in the aircraft industry for flightpath guidance, the invention is also useful for many domains and for a variety of applications, including, for example, all manner of different types of unmanned vehicles, including combat flight, ground, and naval vehicles, and similar automated navigation systems, including applications for satellites in space and submarines. Example applications for the present invention include future combat systems (FCS), control for multi-mission UAVs (CMUS), Navy UCAV (UCAV-N), Canard rotor wing (CRW), sensor craft, data exploitation mission planning and communications UAVs (DEMPC), and commercial flight management systems.

[0019] Different methods have been developed to represent curves, particularly for modeling waypoint routes, flight trajectories, and other applications. Some methods use straight line segment representations, others use curve-form trajectories. Parametric polynomial curves, such as B-Spline, Bezier, and Hermite curves, may be used to represent curve-form trajectories. Typically waypoints are a trench variety where the corners of a polygon define the waypoints and a curved arc forms the corner within the polygon without passing through the waypoint. Bezier curves are widely used in computer graphics and similar computer aided design (CAD) systems. For example, drawing objects in presentation software may use Bezier curves. A Bezier curve is a polynomial parametric curve function defined by a set of control points. Various forms of Bezier curves, typically defined by the number of control points and, therefore, the degree of the representative polynomial parametric equations, form curves between anchor points as defined by the control points. Bezier curves may be used to define a path defined by a number of successive curved segments that are defined by guidance through successive waypoints, rather than a path where waypoints define corners of the path. Waypoints define a position, and control points define the curve of a path through a waypoint. For example, the path will cross through a waypoint, but the control point determines the manner in which the path approaches and departs from the waypoint. As used herein, waypoints define an intended path between a relative starting point and a relative ending point or target and control points define and modify the shape of the curve between, approaching, and departing from anchor points. Typically, a pair of control points define a line through the waypoint. The path passes through the waypoint at a tangent to the line defined by the pair of control points. A Bezier curve will leave a waypoint in the direction of a control point, but will curve away from the control point towards the next successive waypoint. The relative distance a control point extends from the waypoint defines how strongly the path will curve away from the control point. Relative weighting of the control points refers to the distance and orientation with respect to the waypoint and defines how strong the curvature of the path will be on either side of the waypoint. Essentially, the distance of the control point from the associated waypoint determines how strongly the curve is bent after leaving the waypoint. For example, if a control point approaching a waypoint is relatively large, the path may approach the waypoint and sharply turn towards the tangent to the line defined by the control points. And if the departure waypoint is relatively weak, the path may slowly diverge from the tangent line towards the next successive waypoint. Embodiments of the present invention are not limited to a particular type of Bezier curve or the equations and solutions provided for the Bezier curves of the example embodiments provided herein. Embodiments of the present invention may employ different Bezier curves in accordance with the present invention for flightpath guidance using moving waypoints.

[0020] FIG. 1 is an example Bezier curve flightpath guidance diagram generated by a system or method of one embodiment of the present invention. The path or route is comprised of a series of waypoints. At any given time, at least three waypoints are in consideration, shown as triangles in FIG. 1. Specifically, more than three waypoints could be considered in accordance with the present invention; however, for simplicity of understanding, the example embodiments provided herein include only three waypoints for consideration at one time. In FIG. 1, a previous waypoint, shown as the triangle identified with a P, is defined by control points identified as an approach control point for the previous waypoint CP_PA and a departure control point for the previous waypoint CP_PD. The current waypoint C and next waypoint N are similarly shown as triangles and defined by approach and departure control points CP_CA, CP_CD, CP_NA, and CP_ND. These control points define the curve of the path through the waypoint. FIG. 1 shows the segments between any two waypoints being defined or represented by Bezier curves.

[0021] Because at any given time three waypoints are under consideration, the three waypoints under consideration will change based upon progression along the path. For example, after passing through waypoint C, waypoint C will become the previous waypoint and waypoint N will become the current waypoint with a subsequent waypoint becoming the next waypoint.

[0022] FIG. 2 shows an isolated segment of a Bezier curve. A previous end point or waypoint (X₁, Y₁), also known as a first waypoint, is defined by a departure control point (ΔX₁, ΔY₁). A current end point or waypoint (X₂, Y₂), also known as a second end point, is defined by an approach control point (ΔX₂, ΔY₂). The curve S between the first end point and the second end point may be defined by a Bezier curve. In that manner, FIG. 2 may be applied to the route and waypoint sequencing of FIG. 1. As will be evident in the following description of an embodiment of the present invention, the mathematical symbols represented in FIGS. 1 and 2 are used for solving for guidance using movement waypoints.

[0023] One of ordinary skill in the art will recognize that FIG. 1 only represents two dimensional aspects of movement along a path and will understand corresponding three dimensional characteristics and variables not represented in FIG. 1 but included within the scope of the present invention. In this regard, an embodiment of the present invention is described herein in three dimensions providing for six degrees of freedom from which one of ordinary skill in the art may identify parameters not represented in the simplification of FIG. 1. The embodiment described herein uses symbolic mathematic systems based on well established principles to define and describe a new approach to waypoint guidance.

[0024] The Bezier curve B of the route in FIG. 1 is further defined by an independent variable u which varies from 0 to 1. Similarly, a point B(u) along the Bezier curve may be defined by the independent variable u. The velocity of movement along the path defined by the B curve may also be defined by the independent variable u and represented by V(u). The course of movement along the path may be defined relative to an azimuthal angle represented by Ψ and an elevation angle represented by γ.

[0025] Although movement would ideally follow the path, movement, for example a vehicle being guided along the path, may be disturbed from the path such as an aircraft being blown off track by a crosswind or a vehicle maneuvering around an obstacle. In such a case where movement is disturbed from the path, corrections must be provided to guide the movement in order to bring the actual movement back to the path. In order to determine correction guidance commands, a current location along the path for the ideal position and the actual position such as a present position of the vehicle may be defined. The correction back to the path generally may be referred to as crosstrack (xtk) guidance, referring to a linear path which would cross over the intended track or path. To determine crosstrack guidance commands, the present position along the path, or ideal position along the path, P_u may define a perpendicular vector ΔY_xtk from the intended route to the current position such as the location of the vehicle that is intended to be guided along the path.

[0026] The actual position or current location of a vehicle is then defined as being the point on the curve P_u where a position vector to the vehicle is perpendicular to the desired path. A crosstrack guidance command to correct back to the path may be defined by both the actual position or current location of a vehicle and a target position along the path ahead of the P_u position on the path. This target position may be defined by a lead time Δt and the velocity V_u of movement along the path. The ideal position along the path or the commanded position along the path defines the guidance command to bring a vehicle back to the path determined by a commanded course Ψ_c and a commanded angle γ_c from the actual location of the vehicle. The commanded course and commanded angle provide a crosstrack gain determined by the independent variable u and the lead time Δt.

[0027] The previous discussion, however, does not consider the potential for waypoints to be moving during guidance based upon those waypoints. Conventionally, waypoints are fixed in space relative to a reference frame such as the Earth. Moving generally describes the property of a change in position or orientation of a waypoint in the reference frame in which the position and orientation of a vehicle is measured such that guidance of the vehicle may account for the change in position of the waypoint. However, waypoints may be moving such as where they are fixed with reference to a moving object such as defined by an aircraft carrier or moving in a fixed reference frame such as defined by Earth. For example, in FIG. 1 the current waypoint is defined as having movement X_Wc along the x-axis at angle Ψ of attitude, with a change in attitude of Δ Ψ. A waypoint may move in any manner, such as along a single axis or across multiple axes. As described herein, even after a vehicle may have passed a waypoint, the movement of the waypoint may remain as a variable in an equation to control the movement of the vehicle. In some situations, even rotational movement of a waypoint would be important to control the guidance of a vehicle.

[0028] By way of further describing the present invention, the following is an embodiment for guidance along a Bezier curve route using waypoints that may be fixed or moving. Presented first for comparison is an embodiment of the present invention for fixed waypoints. Following the fixed waypoint solution is an embodiment of the present invention that accommodates moving waypoints. Generally, these embodiments may be referred to as a solution to Bezier curve route geometry for moving waypoints. The embodiments, however, are much more than geometry and include commands for guidance along the Bezier curve route including divergence from the route and waypoints that are moving.

[0029] As previously described the independent parameter u varies from 0 to 1, or 0≦u≦1. The solution for the Bezier curve using fixed waypoints (X₁, Y₁, Z₁) and (X₂, Y₂, Z₂) can be represented by

[0030] $B\left(u\right)=\left[\begin{array}{c}{X}_1\\ Y_1\\ Z_1\end{array}\right]·{\left(1-u\right)}^3+\left[\begin{array}{c}{X}_1+\Delta X_1\\ Y_1+\Delta Y_1\\ Z_1+\Delta Z_1\end{array}\right]·3·u·{\left(1-u\right)}^2+\left[\begin{array}{c}{X}_2+\Delta X_2\\ Y_2+\Delta Y_2\\ Z_2+\Delta Z_2\end{array}\right]·3·u^2·\left(1-u\right)+\left[\begin{array}{c}{X}_2\\ Y_2\\ Z_2\end{array}\right]·u^3.$
Expanding the solution provides

[0031] $B\left(u\right)=\left[\begin{array}{c}\left(2·u+1\right)·{\left(u-1\right)}^2·X_1+3·u·{\left(u-1\right)}^2·\Delta X_1+-u^2·\\ \left(-3+2·u\right)·X_2+-3·u^2·\left(u-1\right)·\Delta X_2\\ \left(2·u+1\right)·{\left(u-1\right)}^2·Y_1+3·u·{\left(u-1\right)}^2·\Delta Y_1+-u^2·\\ \left(-3+2·u\right)·Y_2+-3·u^2·\left(u-1\right)·\Delta Y_2\\ \left(2·u+1\right)·{\left(u-1\right)}^2·Z_1+3·u·{\left(u-1\right)}^2·\Delta Z_1+-u^2·\\ \left(-3+2·u\right)·Z_2+3·u^2·\left(u-1\right)·\Delta Z_2\end{array}\right].$
Further regrouping the solution provides

[0032] $B\left(u\right)=\left[\begin{array}{c}\left(3·\Delta X_1+2·X_1-2·X_2-3·\Delta X_2\right)·\\ u^3+\left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·\\ u^2+3·\Delta X_1·u+X_1\\ \left(3·\Delta Y_1+2·Y_1-2·Y_2-3·\Delta Y_2\right)·\\ u^3+\left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·\\ u^2+3·\Delta Y_1·u+Y_1\\ \left(3·\Delta Z_1+2·Z_1-2·Z_2-3·\Delta Z_2\right)·\\ u^3+\left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·\\ u^2+3·\Delta Z_1·u+Z_1\end{array}\right].$

[0033] The velocity of movement along the curve route using fixed waypoints may be solved by applying the chain rule to differentiating the Bezier curve, represented by

[0034] $\frac{d}{du}B\left(u\right)=\left[\begin{array}{c}6·u·\left(u-1\right)·X_1+3·\left(3·u-1\right)·\left(u-1\right)·\\ \Delta X_1+-6·u·\left(u-1\right)·\\ X_2+-3·u·\left(-2+3·u\right)·\Delta X_2\\ 6·u·\left(u-1\right)·Y_1+3·\left(3·u-1\right)·\left(u-1\right)·\\ \Delta Y_1+-6·u·\left(u-1\right)·\\ Y_2+-3·u·\left(-2+3·u\right)·\Delta Y_2\\ 6·u·\left(u-1\right)·Z_1+3·\left(3·u-1\right)·\left(u-1\right)·\\ \Delta Z_1+-6·u·\left(u-1\right)·\\ Z_2+-3·u·\left(-2+3·u\right)·\Delta Z_2\end{array}\right].$
Again, regrouping the solution provides

[0035] $\frac{d}{du}B\left(u\right)=\left[\begin{array}{c}\left(9·\Delta X_1+6·X_1-6·X_2-9·\Delta X_2\right)·u^2+\\ \left(-6·X_1+6·X_2-12·\Delta X_1+6·\Delta X_2\right)·u+3·\Delta X_1\\ \left(9·\Delta Y_1+6·Y_1-6·Y_2-9·\Delta Y_2\right)·u^2+\\ \left(-6·Y_1+6·Y_2-12·\Delta Y_1+6·\Delta Y_2\right)·u+3·\Delta Y_1\\ \left(9·\Delta Z_1+6·Z_1-6·Z_2-9·\Delta Z_2\right)·u^2+\\ \left(-6·Z_1+6·Z_2-12·\Delta Z_1+6·\Delta Z_2\right)·u+3·\Delta Z_1\end{array}\right].$

[0036] The velocity then may be represented by

[0037] $V=\left(\frac{d}{du}B\left(u\right)\right)\frac{d}{dt}u.$
Substituting for d/du B(u) provides

[0038] $V=\left[\begin{array}{c}\left(-6·u+6·u^2\right)·X_1+\left(3-12·u+9·u^2\right)·\Delta X_1+\\ \left(6·u-6·u^2\right)·X_2+\left(6·u-9·u^2\right)·\Delta X_2\\ \left(-6·u+6·u^2\right)·Y_1+\left(3-12·u+9·u^2\right)·\Delta Y_1+\\ \left(6·u-6·u^2\right)·Y_2+\left(6·u-9·u^2\right)·\Delta Y_2\\ \left(-6·u+6·u^2\right)·Z_1+\left(3-12·u+9·u^2\right)·\Delta Z_1+\\ \left(6·u-6·u^2\right)·Z_2+\left(6·u-9·u^2\right)·\Delta Z_2\end{array}\right]·\left(\frac{d}{dt}u\right).$
Given that the speed of the vehicle is known, progress along the path or route will match the inertial vehicle speed and may be solved for an ordinary nonlinear differential equation that can be integrated over time to get the position of a vehicle progressing at a given speed along the path, represented by

[0039] $\frac{d}{dt}u=\frac{V_i}{3·\sqrt{\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}[\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ {\left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2]}^2\dots +\end{array}\\ [\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\end{array}\\ {\left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2]}^2\dots +\end{array}\\ [\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\end{array}\\ {\left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2]}^2\end{array}}}.$
It may be noted that the ellipses, . . . , in the previous equation are meant to represent line wraps and not continuations that do not appear. This convention is used herein throughout.

[0040] The distance between subsequent waypoints along the path is solved by the arclength formula,

[0041] $S={\int }_0^1\sqrt{{\left(\frac{d}{du}X\right)}^2+{\left(\frac{d}{du}Y\right)}^2+{\left(dZ\right)}^2}du,$
expanded and substituted to derive

[0042] $S=3·{\int }_0^1\sqrt{\begin{array}{c}[\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ {\left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ {\left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\\ {\left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2]}^2\end{array}}du.$
As evidenced from the above equation, the distance or spacing between waypoints does not impair the present invention. Rather, the nature of Bezier curves accounts for the distance between waypoints in the polynomial parametric curve equations for a Bezier curve.

[0043] The course Ψ and angle γ at any point u is found by solving for the tangent to the curved path, as follows:

[0044] $\cos \left(\Psi \right)=\frac{\begin{array}{c}\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ \left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2\end{array}}{\sqrt{\begin{array}{c}[\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ {\left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ {\left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\\ {\left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2]}^2\end{array}}},\sin \left(\Psi \right)=\frac{\begin{array}{c}\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ \left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2\end{array}}{\sqrt{\begin{array}{c}[\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ {\left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ {\left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\\ {\left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2]}^2\end{array}}},\tan \left(\Psi \right)=\frac{\begin{array}{c}\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ \left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2\end{array}}{\begin{array}{c}\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ \left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2\end{array}},\mathrm{and}$ $\sin \left(\gamma \right)=\frac{\begin{array}{c}\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\\ \left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2\end{array}}{\sqrt{\begin{array}{c}[\left(-2·u+2·u^2\right)·X_1+\left(1-4·u+3·u^2\right)·\Delta X_1+\\ {\left(2·u-2·u^2\right)·X_2+\left(2·u-3·u^2\right)·\Delta X_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Y_1+\left(1-4·u+3·u^2\right)·\Delta Y_1+\\ {\left(2·u-2·u^2\right)·Y_2+\left(2·u-3·u^2\right)·\Delta Y_2]}^2\dots +\\ [\left(-2·u+2·u^2\right)·Z_1+\left(1-4·u+3·u^2\right)·\Delta Z_1+\\ {\left(2·u-2·u^2\right)·Z_2+\left(2·u-3·u^2\right)·\Delta Z_2]}^2\end{array}}}.$

[0045] The formulae provided and described above assume that the vehicle accurately follows the intended path, course, or route. It is unlikely that accurate path trajectory is achieved in practical applications. Rather, a vehicle is likely to be disturbed from the intended path. Thus, a guidance system must be capable of accounting for such deviations and generating correction guidance commands to bring the vehicle back to the path.

[0046] Accordingly, because a vehicle may be disturbed from an intended path, the deviation results in an actual position or current location of the vehicle and an ideal or current position along the path or commanded position along the path. The position vector from the path to the actual position of the vehicle may be represented by r_u=P−B(u). Substituting for P and B(u) results in a solution represented by

[0047] $r_u=\left[\begin{array}{c}X+-\left[\begin{array}{c}\left(3·\Delta X_1+2·X_1-2·X_2-3·\Delta X_2\right)·u^3\dots +\\ \left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·u^2+\\ 3·\Delta X_1·u+X_1\end{array}\right]\\ Y+-\left[\begin{array}{c}\left(3·\Delta Y_1+2·Y_1-2·Y_2-3·\Delta Y_2\right)·u^3\dots +\\ \left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·u^2+\\ 3·\Delta Y_1·u+Y_1\end{array}\right]\\ Z+-\left[\begin{array}{c}\left(3·\Delta Z_1+2·Z_1-2·Z_2-3·\Delta Z_2\right)·u^3\dots +\\ \left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·u^2+\\ 3·\Delta Z_1·u+Z_1\end{array}\right]\end{array}\right].$

[0048] From the actual position of the vehicle and the position vector from that position to the path, the ideal or current position along the path may be defined by the point on the curve where the position vector to the actual position of the vehicle is perpendicular to the path, represented by

[0049] $r_u^T·\frac{d}{du}P\left(u\right)=0$
where r_u^T represents the transpose of r_u.
From before,

[0050] $P\left(u\right)=\left[\begin{array}{c}\left(3·\Delta X_1+2·X_1-2·X_2-3·\Delta X_2\right)·u^3+\\ \left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·\\ u^2+3·\Delta X_1·u+X_1\\ \left(3·\Delta Y_1+2·Y_1-2·Y_2-3·\Delta Y_2\right)·u^3+\\ \left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·\\ u^2+3·\Delta Y_1·u+Y_1\\ \left(3·\Delta Z_1+2·Z_1-2·Z_2-3·\Delta Z_2\right)·u^3+\\ \left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·\\ u^2+3·\Delta Z_1·u+Z_1\end{array}\right],\frac{d}{du}{P}_u=\left[\begin{array}{c}3·\left(3·\Delta X_1+2·X_1-2·X_2-3·\Delta X_2\right)·\\ u^2+2·\\ \left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·\\ u+3·\Delta X_1\\ 3·\left(3·\Delta Y_1+2·Y_1-2·Y_2-3·\Delta Y_2\right)·\\ u^2+2·\\ \left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·\\ u+3·\Delta Y_1\\ 3·\left(3·\Delta Z_1+2·Z_1-2·Z_2-3·\Delta Z_2\right)·\\ u^2+2·\\ \left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·\\ u+3·\Delta Z_1\end{array}\right],\mathrm{and}$ $r_u=\left[\begin{array}{c}X+-\left[\begin{array}{c}\left(3·\Delta X_1+2·X_1-2·X_2-3·\Delta X_2\right)·u^3+\\ \left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·u^2\dots +\\ 3·\Delta X_1·u+X_1\end{array}\right]\\ Y+-\left[\begin{array}{c}\left(3·\Delta Y_1+2·Y_1-2·Y_2-3·\Delta Y_2\right)·u^3+\\ \left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·u^2\dots \\ 3·\Delta Y_1·u+Y_1\end{array}+\right]\\ Z+-\left[\begin{array}{c}\left(3·\Delta Z_1+2·Z_1-2·Z_2-3·\Delta Z_2\right)·u^3+\\ \left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·u^2\dots \\ 3·\Delta Z_1·u+Z_1\end{array}+\right]\end{array}\right].$
Substituting where
a_X=3·ΔX₁+2·X₁−2·X₂−3·ΔX₂,
b_X=−3·X₁+3·X₂−6·ΔX₁+3·ΔX₂,
c_X=9·ΔX₁+6·X₁−6·X₂−9·ΔX₂,
d_X=−6·X₁+6·X₂−12·ΔX₁+6·ΔX₂,
a_Y=3·ΔY₁+2·Y₁−2·Y₂−3·ΔY₂,
b_Y=−3·Y₁+3·Y₂−6·ΔY₁+3·ΔY₂,
c_Y=9·ΔY₁+6·Y₁−6·Y₂−9·ΔY₂,
d_Y=−6·Y₁+6·Y₂−12·ΔY₁+6·ΔY₂,
a_Z=3·ΔZ₁+2·Z₁−2·Z₂−3·ΔZ₂,
b_Z=−3·Z₁+3·Z₂−6·ΔZ₁+3·ΔZ₂,
c_Z=9·ΔZ₁+6·Z₁−6·Z₂−9·Z₂, and
d_Z=−6·Z₁+6·Z₂−12·ΔZ₁+6·ΔZ₂
yields the representative solution for the ideal position along the path as

[0051] ${\left[\begin{array}{c}X+-\left(\begin{array}{c}{a}_X·u^3+b_X·u^2\dots +\\ 3·\Delta X_1·u+X_1\end{array}\right)\\ Y+-\left(\begin{array}{c}{a}_Y·u^3+b_Y·u^2\dots +\\ 3·\Delta Y_1·u+Y_1\end{array}\right)\\ Z+-\left(\begin{array}{c}{a}_Z·u^3+b_Z·u^2\dots +\\ 3·\Delta Z_1·u+Z_1\end{array}\right)\end{array}\right]}^T·\left[\begin{array}{c}{c}_X·u^2+d_X·u+3·\Delta X_1\\ c_Y·u^2+d_Y·u+3·\Delta Y_1\\ c_Z·u^2+d_Z·u+3·\Delta Z_1\end{array}\right]=0.$

[0052] Evaluating and collecting powers of u from the previous equation yields a quintic polynomial that can be solved for residual. When the path is kinked into a sharp loop, there could be a point in the middle of the loop that satisfies the normal vector criterion at five distinct points. However, the intent is to solve for the ideal position along the path. The resulting quintic polynomial may be represented by

[0053] $\left(-a_Z·c_Z-a_X·c_X-a_Y·c_Y\right)·u^5\dots +\left(-b_Y·c_Y-a_Y·d_Y-b_X·c_X·a_Z·d_Z-b_Z·c_Z-a_X·d_X\right)·u^4\dots +\left[\begin{array}{c}\left(-3·a_X-3·c_X\right)·\Delta X_1+\left(-3·c_Y-3·a_Y\right)·\Delta Y_1+\\ \left(-3·c_Z-3·a_Z\right)·\Delta Z_1\dots +\\ -b_Y·d_Y-b_Z·d_Z-b_X·d_X\end{array}\right]·u^3\dots +\left[\begin{array}{c}\left(-3·d_X-3·b_X\right)·\Delta X_1+\left(-3·d_Y-3·b_Y\right)·\Delta Y_1+\\ \left(-3·d_Z-3·b_Z\right)·\Delta Z_1\dots \\ +-Z_1·c_Z-Y_1·c_Y+X·c_X-X_1·c_X+Y·c_Y+Z·c_Z\end{array}\right]·u^2\dots +\left(-9·\Delta X_1^2+Y·d_Y-Z_1·d_Z-X_1·d_X-9·\Delta Z_1^2+Z·d_Z-Y_1·d_Y+X·d_X-9·\Delta Y_1^2\right)·u\dots +3·X·\Delta X_1+3·Y·\Delta Y_1-3·X_1·\Delta X_1-3·Y_1·\Delta Y_1-3·Z_1·\Delta Z_1+3·Z·\Delta Z_1=0.$
Substituting, collecting variables, and setting the quintic equal to residual provides the ability to determine u, or at least attempt to determine u. For example, the current u is the first real root which is greater than the previous u, and u is limited to the interval from the previous u to 1. One of ordinary skill in the art will recognize that the first three coefficients are independent of present position, and may be pre-computed. The representative solution is

[0054] $\left[\begin{array}{c}-3·(-3·\Delta X_1+3·\\ {\Delta X_2-2·X_1+2·X_2)}^2\dots +\\ -3·(3·\Delta Y_2-3·\\ {\Delta Y_1-2·Y_1+2·Y_2)}^2\dots +\\ -3·(3·\Delta Z_2-3·\\ {\Delta Z_1-2·Z_1+2·Z_2)}^2\end{array}\right]·u^5\dots +\left[\begin{array}{c}15·\left(3·\Delta X_2+2·X_2-3·\Delta X_1-2·X_1\right)·\\ \left(\Delta X_2+X_2-2·\Delta X_1-X_1\right)\dots +\\ 15·\left(3·\Delta Y_2-2·Y_1-3·\Delta Y_1+2·Y_2\right)·\\ \left(\Delta Y_2-Y_1-2·\Delta Y_1+Y_2\right)\dots +\\ 15·\left(3·\Delta Z_2-2·Z_1-3·\Delta Z_1+2·Z_2\right)·\\ \left(\Delta Z_2-Z_1-2·\Delta Z_1+Z_2\right)\end{array}\right]·u^4\dots +\left[\begin{array}{c}-\left(-3·X_1+3·X_2-6·\Delta X_1+3·\Delta X_2\right)·\\ \left(-6·X_1+6·X_2-12·\Delta X_1+6·\Delta X_2\right)\dots +\\ 12·\Delta X_1·\left(-3·\Delta X_1-2·X_1+2·X_2+3·\Delta X_2\right)\dots \\ -\left(-3·Y_1+3·Y_2-6·\Delta Y_1+3·\Delta Y_2\right)·\\ \left(-6·Y_1+6·Y_2-12·\Delta Y_1+6·\Delta Y_2\right)\dots +\\ 12·\Delta Y_1·\left(-3·\Delta Y_1-2·Y_1+2·Y_2+3·\Delta Y_2\right)\dots +\\ -\left(-3·Z_1+3·Z_2-6·\Delta Z_1+3·\Delta Z_2\right)·\\ \left(-6·Z_1+6·Z_2-12·\Delta Z_1+6·\Delta Z_2\right)\dots +\\ 12·\Delta Z_1·\left(-3·\Delta Z_1-2·Z_1+2·Z_2+3·\Delta Z_2\right)\end{array}\right]·u^3\dots +\left[\begin{array}{c}\left(9·\Delta X_1+6·X_1-6·X_2-9·\Delta X_2\right)·\\ \left(X-X_1\right)+-27·\Delta X_1·\\ \left(-2·\Delta X_1-X_1+X_2+\Delta X_2\right)\dots +\\ \left(9·\Delta Y_1+6·Y_1-6·Y_2-9·\Delta Y_2\right)·\\ \left(Y-Y_1\right)+-27·\Delta Y_1·\\ \left(-2·\Delta Y_1-Y_1+Y_2+\Delta Y_2\right)\dots +\\ \left(9·\Delta Z_1+6·Z_1-6·Z_2-9·\Delta Z_2\right)·\\ \left(Z-Z_1\right)+-27·\Delta Z_1·\\ \left(-2·\Delta Z_1-Z_1+Z_2+\Delta Z_2\right)\end{array}\right]·u^2\dots +\left[\begin{array}{c}\left(-6·X_1+6·X_2-12·\Delta X_1+6·\Delta X_2\right)·\left(X-X_1\right)-9·\Delta X_1^2\dots +\\ \left(-6·Y_1+6·Y_2-12·\Delta Y_1+6·\Delta Y_2\right)·\left(Y-Y_1\right)-9·\Delta Y_1^2\dots \\ \left(-6·Z_1+6·Z_2-12·\Delta Z_1+6·\Delta Z_2\right)·\left(Z-Z_1\right)-9·\Delta Z_1^2\end{array}\right]·u\dots +3·\left(X-X_1\right)·\Delta X_1+3·\left(Y-Y_1\right)·\Delta Y_1+3·\left(Z-Z_1\right)·\Delta Z_1=\mathrm{res}.$
The slope of the residual (res), represented by