Qual è il modo consigliato per vincolare la posizione del quaternione a base mobile e la velocità angolare?

Question

Per gli oggetti con base mobile, la rotazione nella posizione generalizzata è espressa come un quaternione 4D. Tuttavia, la rotazione nella velocità generalizzata è ancora espressa come vettore di rotazione spaziale 3D.

Esiste un modo consigliato per vincolarli, ad esempio Eulero all'indietro?

prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))

È appropriato convertire la velocità angolare in forma di quaternione, ad esempio qui ?

O come dice John T. Betts in Practical Methods for Optimal Control and Estimation Using Nonlinear Programming

Answer 1

Penso che per il tuo caso d'uso, tu voglia imporre il vincolo di integrazione tra waypoint adiacenti nella pianificazione. In questo caso, non suggerirei di utilizzare q[n] = q[n+1] - qdot[n+1] * dtdirettamente l'integrazione di Eulero. La ragione è che il quaternione dovrebbe sempre soddisfare il vincolo di lunghezza unitaria (cioè, q[n]e q[n+1]sono entrambi sulla superficie della sfera unitaria in 4D). Quindi la derivata temporale qdotè lungo la superficie tangente di questa sfera unitaria, vale a dire q[n+1] - qdot[n+1] * dtè su quella superficie tangente. Nessun punto (diverso da q[n+1]) sulla superficie tangente è anche sulla superficie del cerchio unitario. Quindi il vincolo q[n+1] = q[n] + qdot[n+1] * dtpuò essere soddisfatto solo quando dt=0, quando lo accoppi con il vincolo |q[n]| = |q[n+1]|=1. Dal punto di vista pittorico, potresti usare la figura nella sezione "tangente a un cerchio" dihttps://www.toppr.com/guides/maths/circles/tangents-to-the-circle/. Nota che q[n+1] - qdot[n+1] * dtè sulla linea tangente della sfera ed q[n]è sul cerchio, quindi non puoi avere q[n] = q[n+1] - qdot[n+1] * dt.

Suggerirei invece di considerare la seguente condizione

q[n+1] = q[n] ⊗ Δq

dove q[n]è il quaternione della base galleggiante al n'esimo waypoint, q[n+1]è il quaternione della base galleggiante al n+1'esimo waypoint. ⊗è il prodotto hamiltoniano tra due quaternioni q[n]e Δq. Quindi qui devi calcolare Δq dalla velocità angolare.

Supponiamo che la velocità angolare media tra l'n-esimo e l'n+1-esimo waypoint sia ω ∈ ℝ³. Ciò significa che la base mobile ruota attorno a un asse a = ω/|ω|alla velocità |ω|, quindi

Δq = [cos(|ω|Δt/2), a*sin(|ω|Δt/2)]

Vederehttps://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotationsper una spiegazione sull'equazione di cui sopra.

Se usi ω[n+1]come velocità angolare media dall'nesimo waypoint all'n + 1esimo waypoint (analogo all'uso di v[n+1] in Eulero all'indietro), allora il tuo vincolo di integrazione è

q[n+1] = q[n] ⊗ [cos(|ω[n+1]|Δt/2), a*sin(|ω[n+1]|Δt/2)]

Potresti anche usare le altre quantità come velocità angolare media, ad esempio (ω[n] + ω[n+1])/2.