Here are some more complex algorithms for landing algorithms similar to what is being discussed with some example code and online references to a technical paper and a thesis.
If you assume that the gravity acceleration is constant and no aerodynamics, then you can write an analytic expression for the final state after a finite duration burn where the direction of thrust is linear in time. Either constant thrust or constant acceleration segments can be used with the same algorithm through some trickery. Basically, it is a really quick way to get a good estimate of the final state after a finite burn. It turns out that the "direction of thrust is linear in time" constraint is close enough to the optimal profile to be useful for real work.
You can do the burn in (for example) 4 segments holding the gravity acceleration constant (and different) in each of the 4 segments, probably based on the estimated state in the middle of the segment. Or 10 pieces, or whatever you decide is necessary. It isn't usually necessary for guidance purposes to use too many, because the model becomes more accurate as you approach burnout.
This kind of algorithm is sometimes called a "Thrust Integral".
Once you have a Thrust Integral model, you can use it by solving a non-linear programming problem with 6 control variables (ignition time, two starting angles, two final angles and the burn time) and 6 constraints (desired position vector and velocity vector at cutoff).
The equations can look something like this:
Code:
//
// Name: _calcThrustIntegrals
// Class: GuidanceStrategy
// Namespace: MvjFlight
//
// Purpose: Reduced order method (analytic approximation,
// assuming constant thrust and average gravity)
// Ref: Dukeman Thesis, p.33..35
//
// Parameters: see below
//
// Exceptions: None
//
// Return: none
//
void
GuidanceStrategy::_calcThrustIntegrals(
MvjOrbiter::StateVector &svIn, // input state vector, starting state
MvjMath::Vector3 &Avec, // pointing direction rate vector
MvjMath::Vector3 &Bvec, // initial pointing direction
double t, // s, burn time
double m0, // kg, initial mass
double c, // m/s, exit vel
double T, // N, thrust
MvjOrbiter::StateVector &svOut) // output final state estimate
{
//
// state input
//
Vector3 rVec = svIn.getPosition();
Vector3 vVec = svIn.getVelocity();
// do separately, since will need Rmag^3 later
double Rmag2 = rVec.sqrLength();
double Rmag = dSqrt(Rmag2);
//
// solution vec
//
double A = Avec.length();
double B = Bvec.length();
double A2 = A*A;
double B2 = B*B;
BASE_ASSERT(A > 0.0);
BASE_ASSERT(c > 0.0);
if (!(A > 0.0 && c > 0.0)) {
svOut = svIn;
return;
}
// Vehicle Model
double mDot = T / c; // kg/s
/* // (need to do this on the input side)
bool accLimEna = false; // NOTE: this formulation has not yet been tested
if (accLimEna) {
// Overwrite mass flow rate to impose acceleration constraint
// using the same equations for non-constrained trajectory
// REF: Ref: Dukeman Thesis, p.35
mDot = 0.001 * m0 / t;
// NOTE: Input such that: T / m0 = accel limit, is probably required
}
*/
//
// Thrust Integral factors
//
Vector3 vecD = Avec * m0 + Bvec * mDot;
double d = dSqrt(vecD.dot(vecD));
double ATB = Avec.dot(Bvec);
double e = ATB + A*B;
Vector3 At_Plus_B = Avec * t + Bvec;
double f = At_Plus_B.length(); // dSqrt(At_Plus_B.dot(At_Plus_B));
double g = ATB + A2*t + A*f;
double BBMdot = B2*mDot;
double p = m0*(BBMdot + A2*m0*t+ATB*(m0+mDot*t)+d*f);
double mT = m0 - mDot * t;
double q = mT*(d*B + m0*ATB + BBMdot);
double tau = m0/mDot;
double Ainverse = 1.0/A;
double logEG = dLog(e/g);
double logPQ = dLog(p/q);
double f11 = (T/mDot) * (
(f - B)/A2
+ logEG*(t - tau + ATB/A2)*Ainverse
+ logPQ*m0*(t-tau)/d
);
double ca = (T/mDot)*(Ainverse*logEG + (m0/d)*logPQ);
double cb = (T/d)*logPQ;
Vector3 dVt = Avec * ca + Bvec * cb; // Thrust Integral delta velocity
// ca = f11 * T; // (??? This is an apparent typo in Dukeman's thesis: Remove redundant *T)
ca = f11; // ??? too many Thrusts? do units analyis
cb = T*( (-Ainverse/mDot) * logEG - (mT/d/mDot)*logPQ );
Vector3 dRt = Avec * ca + Bvec * cb; // Thrust Integral delta position
//
// Calculate predicted position (constant gravity per initial position)
//
double t2o2 = t*t*0.5;
double Rmag3= Rmag*Rmag2;
Vector3 Rp0g= rVec + vVec*t; // coast portion
Rp0g += dRt; // coast portion + thrust portion (still without gravity yet)
Vector3 gVecR0= rVec * (- _gb.mu() / Rmag3); // starting position gravity
// predicted position Rp with effect of starting constant gravity acceleration
Vector3 RpVec = Rp0g + gVecR0 * t2o2;
//
// Calculate average gravity (average of starting and predicted positions)
//
double RpMag2= RpVec.sqrLength();
double RpMag = dSqrt(RpMag2);
double RpMag3= RpMag*RpMag2;
Vector3 gVecRp= RpVec * (- _gb.mu() / RpMag3);
Vector3 gVecAv = (gVecR0 + gVecRp) * 0.5;
//
// Calculate final state estimate using average gravity
//
Vector3 rVecOut = Rp0g + gVecAv * t2o2;
Vector3 vVecOut = vVec + dVt + gVecAv * t;
svOut = StateVector(TimeTag(svIn.getTimeTag().getTimeSec() + t,0.0), rVecOut, vVecOut);
}
//
// Name: _calcThrustIntegralsN
// Class: GuidanceStrategy
// Namespace: MvjFlight
//
// Purpose: Reduced order method (analytic approximation,
// assuming constant thrust and average gravity)
// Ref: Dukeman Thesis, p.47..50
//
// Parameters: see below
//
// Exceptions: None
//
// Return: none
//
void
GuidanceStrategy::_calcThrustIntegralsN(
int n, // number of pieces to break propagation time t into
MvjOrbiter::StateVector &svIn, // input state vector, starting state
MvjMath::Vector3 &Avec, // pointing direction rate vector
MvjMath::Vector3 &Bvec, // initial pointing direction
double t, // s, burn time
double m0, // kg, initial mass
double c, // m/s, exit vel
double T, // N, thrust
MvjOrbiter::StateVector &svOut) // output final state estimate
{
svOut = svIn;
double tn = t / (double)n;
double te; // time elapsed to start of step
double m0n = m0;
double mDot = T/c;
Vector3 BvecN = Bvec;
for (int i=0;i<n;i++) {
_calcThrustIntegrals(svOut,Avec,BvecN,tn,m0n,c,T,svOut);
te = tn *(double)(i+1);
BvecN = Avec * te + Bvec;
// set up for next step
m0n = m0 - mDot * te;
}
}
See:
http://etd.gatech.edu/theses/available/etd-01032005-164555/
"Closed-Loop Nominal and Abort Atmospheric Ascent Guidance for Rocket-Powered Launch Vehicles," Greg A Dukeman, 2004.
Or use an algorithm specialized for vacuum planetary landings. I like:
"An Optimal Guidance Law for Planetary Landing," Cristopher N. D'Souza, Charles Stark Draper Lab, 1997
http://www.draper.com/digest98/paper9.pdf
This is an optimal calculus of variations based algorithm assuming constant gravity (low altitude) and no rotation of the coordinate systems (particularly good for Earth's moon).
The guts of this algorithm look like: (see below)
Here, _tgo has been calculated using ::_calcTgo and this value is used in ::getPointingDirection. The RA (range coordinates) frame is as described in the paper: has X along azimuth and Z up. e.g.: N,W,Up for azi = 0. See the D'Souza paper.
Note that the "getPointing Direction" function is somewhat misnamed. In this case, it is more properly the commanded thrust acceleration vector. I happened to fit this newer algorithm into an existing framework for testing various guidance algorithms. The others calculate the pointing direction and thrust separately. This one calculates them together, so it doesn't yet fit exactly into the existing framework at this point (the framework has to expand).
I have used this to guid a DG on approach to Brighton Beach using main engine and TVC based attitude control. I'm currently at the point of doing the flip over to hover engines as it reaches a spot over the landing site and then the vertical final touchdown. For that, I need to transition to a different attitude controller (RCS) and haven't done the logic for the transition yet. For an all vertical lander like the Lunar Module, you wouldn't have that extra work.
Code:
//
// Name: _calcTgo
// Class: GuidanceStrategy_PoweredLanding
// Namespace: MvjFlight
//
// Purpose: D'Souza analytic algorithm for optimum time-to-go calculation
//
// Parameters: None
//
// Exceptions: None
//
// Return: optimal time-to-go solution
//
//
double
GuidanceStrategy_PoweredLanding::_calcTgo(void)
{
double output = 0.0;
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// convert sv into Ra frame
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ***NYI***
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Calculate Tgo
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
double v_dot_r = _svNav.getVelocity().dot(_svNav.getPosition());
double v_dot_v = Vector3::sqrLength(_svNav.getVelocity());
double r_dot_r = Vector3::sqrLength(_svNav.getPosition());
// solve reduced fidelity (flat planet)
// powered landing problem for optimal time-to-go
// per I-Load _gamma selection
MVJ_ASSERT(_gamma >= 0.0);
double g = _gb.surfaceGravityAcceleration(); // m/s^2 equatorial surface
double a,b,c, denom;
denom = _gamma + g*g*0.5;
// t^4 + a* t^2 + b* t + c = 0
a= -2.0 * v_dot_v / denom;
b= -12.0 * v_dot_r / denom;
c= -18.0 * r_dot_r / denom;
double a2, a2m4c;
a2 = a*a;
a2m4c = a2 - 4.0 * c;
double alpha, beta, delta, Z;
alpha = a2m4c - 4.0*a2 / 3.0;
beta = (16.0*a2*a - 18.0*a*a2m4c)/27.0 -b*b;
delta = alpha*alpha*alpha/27.0 + beta*beta/4.0;
// (future sqrts)
MVJ_ASSERT(alpha >= 0.0);
MVJ_ASSERT(delta >= 0.0);
double deltah, halfbeta;
deltah= dSqrt(delta);
halfbeta = 0.5*beta;
// cube root = pow ( abs ( val ), 1.0 / 3.0 )
Z = dCubeRoot(-halfbeta+deltah) + dCubeRoot(-halfbeta-deltah);
double eta;
// invert eq.58
eta = Z - 2.0*a/3.0;
MVJ_ASSERT(eta >= 0.0); // future sqrt
// will be taking sqrt later, eta must be positive
if (eta > 0.0) {
double zeta, xi;
// MVJ: remove the minus sign from this zeta:
// It screws up the sign matching required to get the signs
// correct for the 4 solutions
// zeta = -0.5*b / eta;
zeta = 0.5*b / eta;
xi = 0.5*(a + eta);
// It appears from the requirement for at least 1 real root, that
// equation (75) has an error: the second sign of the right hand radical
// should match that of the left hand radical (after the zeta correction above).
// They certainly can NOT be the same at least, otherwise
// the radicals are the same and any negative
// radical would lead to 4 imaginary solutions.
// (Since we have to have at least 1 real soln, 2 must be real therefor
// can only have at most 2 imaginary solutions.
// per: (eq. 8') [URL]http://en.wikipedia.org/wiki/Quartic_equation#Special_cases[/URL]
// the rad >= 0 is required for a real root, so only consider those
double etah = dSqrt(eta);
double radp = eta - 4.0*(xi + etah*zeta);
double radn = eta - 4.0*(xi - etah*zeta);
double radh;
// store the up to 4 solutions into this array
double tgo[4]= {-1.0, -1.0, -1.0, -1.0};
unsigned int numRealSolns = 0;
if (radp >= 0.0) {
radh = dSqrt(radp);
// future optimization could include the 0.5* in the etah, radh factors
tgo[numRealSolns++] = 0.5*(+etah + radh);
tgo[numRealSolns++] = 0.5*(+etah - radh);
}
if (radn >= 0.0) {
radh = dSqrt(radn);
// future optimization could include the 0.5* in the etah, radh factors
tgo[numRealSolns++] = 0.5*(-etah + radh);
tgo[numRealSolns++] = 0.5*(-etah - radh);
}
// choose best tgo. (smallest positive)
output = 99999.0;
for (unsigned int i=0;i<numRealSolns;i++) {
if (tgo[i] > 0.0 && tgo[i] < output) {
output = tgo[i];
}
}
// for v_dot_r > 0 (v pointing away from target), only 1 real positive root.
printf("tgo solns = %7.2f %7.2f %7.2f %7.2f\n",tgo[0],tgo[1],tgo[2],tgo[3]);
output = output;// break point
#if 0// compute residual of polynomial to indicate any errors
{
// t^4 + a* t^2 + b* t + c = 0
double res = c+ output*(b + output*(a+ output*output));
printf("residual best soln (%g) = %g\n",output, res);
res=res;// breakpoint
}
#endif
#if 0 // compute the Jmin performance index = estimate integral[ a^2 dt ]
{
double Jmin = (_gamma + 0.5*g*g)*output
+ _svNav.getVelocityZ()*(-g) + 2.0*v_dot_v/output // v_dot_g
+ (6.0/output/output) * (v_dot_r + (6.0/output)*r_dot_r);
Jmin /= (0.3048*0.3048);
Jmin = Jmin; // debug break
}
#endif
} else {
output = output;// break point
MVJ_ASSERT(0);
}
return output;
}
// Ra-frame thrust acceleration vector
//
MvjMath::Vector3
GuidanceCommand_PoweredLanding::getPointingDirection( // NON-unit vector of thrust accel in Ra-Frame
double time, // sec, input time, relative to same reference as t0
const MvjMath::Vector3 &pos, // m, current position vector, Ra-frame (Range frame)
const MvjMath::Vector3 &vel, // m/s, current velocity vector, Ra-Frame (Range frame)
double surfaceGrav // m/s^2
)
{
// time-to-go is reduced as time moves on from _t0
double grav = surfaceGrav; // m/s^2
double tgo = _tgo - (time - _t0);
if (tgo < _minimumTimeToGo) {
// return vertical accel for time-to-go below the threshold
return MvjMath::Vector3(0.0,0.0,grav);
} else {
double tgo_inv = 1.0/tgo;
MvjMath::Vector3 output = vel *(-4.0*tgo_inv) + pos * (-6.0*tgo_inv*tgo_inv);
output.z += grav;
return output;
}
}
Hopefully these constant gravity vacuum algorithms will show that useful analytic solutions are possible and roughly how complicated they are. If anyone cares to recreate the algorithms from the papers, this code can serve as a reference implementation.
