Trajectory Optimization with Orientations
Quaternion optimal control with acados, kRRT on manifold, combinatorial optimization
Astrobee attitude-slew MPC
This project gave me a much clearer intuition for unit quaternions and Lie-group formulation as parameterizations of attitude. It is important that the cost be invariant to the sign of the quaternion because of topological identification property $q \sim -q$ and reflect the true relative rotation angle. In the Astrobee attitude-slew MPC below, cost was firstly built from the scalar product $q^\top q_g$,
\[e_q = 1 - (q^\top q_g)^2.\]After increasing the terminal weight on this attitude error term, the MPC drives Astrobee to the desired attitude with small residual angular rates as in the figure.
\[\begin{aligned} \min_{\{q_k, \omega_k, M_k\}_{k=0}^{N-1}} \quad & \sum_{k=0}^{N-1} \Big( Q_q\,\big[1 - (q_k^\top q_g)^2\big] + \omega_k^\top Q_\omega\, \omega_k + M_k^\top R\, M_k \Big) \\ &\quad + Q_q^N\,\big[1 - (q_N^\top q_g)^2\big] + \omega_N^\top Q_\omega^N\, \omega_N \\[0.5em] \text{s.t.} \quad & q_{k+1} = \frac{q_k + \tfrac{1}{2}\,\Omega(\omega_k)\, q_k\, \Delta t} {\big\|q_k + \tfrac{1}{2}\,\Omega(\omega_k)\, q_k\, \Delta t\big\|}, \qquad k = 0,\dots, N-1, \\[0.3em] & \omega_{k+1} = \omega_k + \Delta t\, J^{-1}\!\big( M_k - \omega_k \times (J \omega_k) \big), \qquad k = 0,\dots, N-1, \\[0.3em] & \| \omega_k \|_\infty \le \omega_{\max}, \quad \| M_k \|_\infty \le M_{\max}, \qquad k = 0,\dots, N-1, \\[0.3em] & q_0 = q_{\mathrm{init}}, \quad \omega_0 = \omega_{\mathrm{init}}, \quad q_k \in S^3, \ \omega_k, M_k \in \mathbb{R}^3. \end{aligned}\], where \(\Omega(\omega) = \begin{bmatrix} 0 & -\omega^\top \\ \omega & -[\omega]_\times \end{bmatrix}.\)
The MPC problem above is implemented in acados OCP class using a multiple-shooting SQP based on HPIPM with a small L-M regularization. With an initial $q_0 = [0.5,\,-0.5,\,0.5,\,0.5]$ and goal $q_g = [1,0,0,0]$, the plot shows the resulting quaternion attitude trajectories.
Lie-group-theoretic nonholonomic RRT for flexible needle
In practice, when I replace the quaternion-based orientation residual with the intrinsic geodesic distance on the manifold $ | \,(\log(R_{\text{goal}}^{\top} R))^{\vee} | ^{2} $, the optimization behaves noticeably better even without imposing a large terminal penalty.
Therefore next following project I further explore the Lie-group theoretic method of $SE(2)$ to design geometry-aware planning methods.
In this project, a Lie-group-theoretic nonholonomic RRT was implemented on $SE(2)$ for a flexible needle modeled as a unicycle. The kinematics are
\[\dot x = s\cos\theta = r\,u_{\phi}\cos\theta,\\ \dot y = s\sin\theta = r\,u_{\phi}\sin\theta,\\ \dot\theta = \omega = u_{\omega},\]where the state is $q = (x,y,\theta)^{\mathsf T} \in \mathbb R^2\times S^1$ and the control input is $u = (u_{\phi},u_{\omega})^{\mathsf T}$ with $u_{\phi}$ the wheel (needle) angular speed and $u_{\omega}$ the yaw rate. In compact form, \(\dot q = B(q)\,u\)
Let $g\in SE(2)$ be the pose of the robot and define the body twist \(V^{\mathrm b} = \xi^{\mathrm b} = (g^{-1}\dot g)^{\vee} = \begin{pmatrix} v_x^{\mathrm b}\\[2pt] v_y^{\mathrm b}\\[2pt] \omega \end{pmatrix} = J^b(q)\,\dot q = J^b(q)\,B(q)\,u,\) with body Jacobian \(J^b(q).\) This gives \(\xi = (g^{-1}\dot g)^{\vee} = J^b(q)\,B(q)\,u = \begin{pmatrix} r\,u_{\phi}\\ 0\\ u_{\omega} \end{pmatrix},\) and the Lie-theoretic control–affine model \(\dot g = g\,\hat\xi,\qquad \hat\xi = \begin{pmatrix} 0 & -u_{\omega} & r u_{\phi}\\ u_{\omega} & 0 & 0\\ 0 & 0 & 0 \end{pmatrix} \in \mathfrak{se}(2)\) has the exact closed-form solution for piecewise-constant controls \(g(t+\Delta t) = g(t)\,\exp\bigl(\hat\xi\,\Delta t\bigr).\)
In the RRT, each tree extension evaluates a finite control set $u\in\mathcal U_d$ and propagates the nearest node by this exact Lie-group update $g_{k+1}=g_k\exp(\hat\xi(u)\Delta t)$, rather than by Euler/RK4.
Combinatorial optimization
