2
$\begingroup$

I'm trying to develop a control for a quadrotor, but I'm struggling with position and velocity stabilization.

I believe that I have an issue with position and velocity states of the system.

Maybe someone has already done full state system. Unfortunately, I've found only stabilization of quaternion and angular velocity in articles.

I've attached my version of the system.

Is there any articles about rigid body full state integration and linearization in terms of quaternions?. Available from: https://www.researchgate.net/post/Is_there_any_articles_about_rigid_body_full_state_integration_and_linearization_in_terms_of_quaternions [accessed Jul 14, 2017].

Here

$\eta$ - position in Earth coordinate system

$\upsilon$ - velocity in Body frame

$\omega$ - angular velocity

$\lambda$ - quaternion

\begin{cases} \dot{\vec{\eta}}=\vec{\lambda}^{-1}\cdot\upsilon\cdot\vec{\lambda} & ,\\ \dot{\vec{\upsilon}}=\vec{\lambda}\cdot\begin{pmatrix}0\\ 0\\ 0\\ -g \end{pmatrix}\cdot\vec{\lambda}^{-1}+\frac{1}{m}\cdot F & ,\\ \dot{\vec{\omega}}=\begin{pmatrix}\frac{\tau_{1}}{I_{x}}\\ \frac{\tau_{2}}{I_{y}}\\ \frac{\tau_{3}}{I_{z}} \end{pmatrix}-\begin{pmatrix}\frac{I_{z}-I_{y}}{I_{x}}\dot{\theta}\dot{\psi}\\ \frac{I_{x}-I_{z}}{I_{y}}\dot{\phi}\dot{\psi}\\ \frac{I_{y}-I_{x}}{I_{z}}\dot{\phi}\dot{\theta} \end{pmatrix} & ,\\ \dot{\vec{\lambda}}=\frac{1}{2}\vec{\lambda}\circ\vec{\omega} & . \end{cases}

or detailed

\begin{cases} \dot{\eta}_{n}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((\lambda_{0}^{2}+\lambda_{1}^{2}-\lambda_{2}^{2}-\lambda_{3}^{2})v_{x}+(2\lambda_{0}\lambda_{3}+2\lambda_{1}\lambda_{2})v_{y}+(-2\lambda_{0}\lambda_{2}+2\lambda_{1}\lambda_{3})v_{z}) & ,\\ \dot{\eta}_{e}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((-2\lambda_{0}\lambda_{3}+2\lambda_{1}\lambda_{2})v_{x}+(\lambda_{0}^{2}-\lambda_{1}^{2}+\lambda_{2}^{2}-\lambda_{3}^{2})v_{y}+(2\lambda_{0}\lambda_{1}+2\lambda_{3}\lambda_{2})v_{z}) & ,\\ \dot{\eta}_{h}=\frac{1}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}((2\lambda_{0}\lambda_{2}+2\lambda_{1}\lambda_{3})v_{x}+(-2\lambda_{0}\lambda_{1}+2\lambda_{3}\lambda_{2})v_{y}+(\lambda_{0}^{2}-\lambda_{1}^{2}-\lambda_{2}^{2}+\lambda_{3}^{2})v_{z}) & ,\\ \dot{v}_{x}=\frac{2g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(-\lambda_{1}\lambda_{3}-\lambda_{0}\lambda_{2}) & ,\\ \dot{v}_{y}=\frac{2g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(\lambda_{0}\lambda_{1}-\lambda_{2}\lambda_{3}) & ,\\ \dot{v}_{z}=\frac{-g}{(\lambda_{0}^{2}+\lambda_{1}^{2}+\lambda_{2}^{2}+\lambda_{3}^{2})}(\lambda_{0}^{2}-\lambda_{1}^{2}-\lambda_{2}^{2}+\lambda_{3}^{2})+\frac{1}{m}F & ,\\ \ddot{\phi}=\frac{1}{I_{x}}\left[\tau_{1}-(I_{z}-I_{y})\dot{\theta}\dot{\psi}\right] & ,\\ \ddot{\theta}=\frac{1}{I_{y}}\left[\tau_{2}-(I_{x}-I_{z})\dot{\phi}\dot{\psi}\right] & ,\\ \ddot{\psi}=\frac{1}{I_{z}}\left[\tau_{3}-(I_{y}-I_{x})\dot{\phi}\dot{\theta}\right] & ,\\ \dot{\lambda_{0}}=\frac{1}{2}[-\lambda_{1}\dot{\phi}-\lambda_{2}\dot{\theta}-\lambda_{3}\dot{\psi}] & ,\\ \dot{\lambda_{1}}=\frac{1}{2}[\lambda_{0}\dot{\phi}-\lambda_{3}\dot{\theta}+\lambda_{2}\dot{\psi}] & ,\\ \dot{\lambda_{2}}=\frac{1}{2}[\lambda_{3}\dot{\phi}+\lambda_{0}\dot{\theta}-\lambda_{1}\dot{\psi}] & ,\\ \dot{\lambda_{3}}=\frac{1}{2}[-\lambda_{2}\dot{\phi}+\lambda_{1}\dot{\theta}+\lambda_{0}\dot{\psi}] & . \end{cases}

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.