Geometric Pseudospectral Method on SE(3) for Rigid-Body Dynamics with Application to Aircraft

Algorithm 2

Problem formulation of the NLP.

Compute: 𝑔_𝑎𝑏,𝑉^𝑏_𝑎𝑏,𝑇,𝛿_𝑎,𝛿_𝑟,𝛿_𝑒 at 𝜏_𝑗, 𝑗=1,…,𝑁+1

Minimizing: Cost Function

Subject to:

𝑔₀=𝑔_𝑖,𝑉₀=𝑉_𝑖, 𝑔_𝑁+1=𝑔_𝑓,𝑉_𝑁+1=𝑉_𝑓,

𝑖,𝑓 denote initial time and terminal time.

and

(i) dynamics constraints: (25), (26)

(ii) path constraints

(iii) decision variables bounds:

     𝑔𝜏_𝑖∈𝑔_𝑙,𝑔_𝑢,𝑉𝜏_𝑖∈𝑉_𝑙,𝑉_𝑢

𝑇𝜏_𝑖∈𝑇_𝑙,𝑇_𝑢,𝛿_𝑎∈𝛿_𝑎𝑙,𝛿_𝑎𝑢,

𝛿_𝑟∈𝛿_𝑟𝑙,𝛿_𝑟𝑢,𝛿_𝑒∈𝛿_𝑒𝑙,𝛿_𝑒𝑢

    where 𝑙,𝑢 denote lower limit and upper limit.