Os sistemas de controle podem ser representados por um conjunto de equações matemáticas conhecidas como modelo matemático. Esses modelos são úteis para análise e projeto de sistemas de controle. Como descrevemos no post anterior, Testes reais do Bbot, o controlador PID não conseguiu sustentar o robô. Portanto, decidimos fazer um estudo mais aprofundado do modelo matemático e de controle do robô. Foi feito então uma busca de artigos e trabalhos similares aos robôs self-balancing, no fim, 2 artigos se destacaram e formaram uma base para o nosso desenvolvimento, sendo eles, Modeling and Control of Two-Legged Wheeled Robot por Adam Kollarčík e Dynamic modeling of a two-wheeled inverted pendulum balancing mobile robot por Sangtae Kim e Sang Joo Kwon

Modelo

Sabemos que os modelos da maioria dos sistemas semelhantes ao pêndulos são conhecidos, e o nosso modelo (Bbot) não é uma exceção. Portanto, como referenciado, nós utilizamos um modelo usado nos artigos anteriores. O modelo de espaço de estado é o seguinte:

\$\$ \begin{bmatrix}\ddot{x}\\ \ddot{φ}\\ \ddot{ψ} \end{bmatrix} = \begin{bmatrix}m_{11} & m_{12} & 0\\ m_{21} & m_{22} & 0\\ 0 & 0 & m_{33} \end{bmatrix}^{-1} \begin{pmatrix} \begin{bmatrix} \frac{1}{r_0}(u_{0R} + u_{0L}) \\ m_plg\sin φ - u_{0R} - u_{0L} \\ \frac{w}{2}(u_{0R} - u_{0L}) \end{bmatrix} - \begin{bmatrix} \frac{2b}{r_0^2} & d_{12} & d_{13}\\\ \frac{-2b}{r_0} & 2b & d_{23}\\ d_{31} & d_{32} & d_{33} \end{bmatrix} \begin{bmatrix}\dot{x}\\ \dot{φ}\\ \dot{ψ} \end{bmatrix} \end{pmatrix} \$\$

Onde:

\$\$ m_{11} = m_p + 2m_0 + 2\frac{I_0}{r_0^2}, m_{12} = m_{21} = m_pl\cos φ, m_{22} = I_{py} + m_pl^2, \$\$ \$\$ m_{33} = I_{pz} + 2I_{0xy} + (m_0 + \frac{I_0}{r_0^2})\frac{w^2}{2} - (I_{pz} - I_{px} - m_pl^2) \sin^2 φ, \$\$ \$\$ d_{12} = -m_pl\dot{φ}\sin φ - \frac{2b}{r_0}, d_{13} = m_pl\dot{ψ}\sin φ, d_{23} = (I_{pz} - I_{px} - m_pl^2)\dot{ψ}\sin φ\cos φ, \$\$ \$\$ d_{31} = m_pl\dot{ψ}\sin φ, d_{32} = -d_{23}, d_{33} = -(I_{pz} - I_{px} - m_pl^2)\dot{φ}\sin φ \cos φ + \frac{w^2}{2r_0^2}b \$\$

Temos parâmetros como inércia, massa, distância entre as rodas, torque nas rodas, gravidade e coeficiente de atrito. Vale mencionar que $x$ é a distância percorrida pelo pêndulo, $φ$ é o ângulo em pitch e $ψ$ é o ângulo em yaw.

Nós decidimos simplificar o modelo de forma que utilizamos as juntas estáticas. O movimento das juntas superiores será implementado nas próximas etapas. Todos os valores dos parâmetros citados foram retirados do desenho técnico feito no software Onshape.

A próxima etapa para o desenvolvimento é a linearização do sistema, obtendo as matrizes $Ac$ e $Bc$. Essas matrizes são encontradas a partir da jacobiana em relação ao vetor de estados e do torque das rodas. Essas matrizes são avaliadas em um ponto fixo, que no nosso caso, foram todas em 0.

O sistema então é discretizado, obtendo as matrizes $Ad$ e $Bd$, usando o método de zero-order hold com uma frequência setada para 25 Hz.

Controle

Em muitos artigos nos deparamos com o controlador LQR, e observamos o seu bom desempenho nos sistemas. Já que não tivemos uma implementação muito eficiente com o PID, optamos por seguir para implementação de um método semelhante, porém, com uma diferença, que é usar o LQR no sistema discretizado, usamos então a função Discrete-time Linear Quadratic Regulator (DLQR).

Já com o modelo linearizado, retiramos o ângulo em yaw e a distância percorrida pelo pêndulo, já que não são estados necessários para a estabilização do mesmo. Em seguida criamos um sistema aumentado ($Aaug$, $Baug$, $Caug$ e $Daug$), permitindo a implementação de uma ação integral para a velocidade linear e velocidade em yaw, a fim de controlá-la por um valor de referência definido pelo operador.

A partir destas matrizes, podemos fazer análises para saber se o sistema pode ser controlado e se o sistema é observável. A controlabilidade mede a capacidade de uma configuração particular do atuador controlar todos os estados do sistema, já a observabilidade mede a capacidade da configuração particular do sensor de fornecer todas as informações necessárias para estimar todos os estados do sistema. O sistema é controlável se a matriz de controlabilidade for full rank e observável se a matriz de observabilidade for full rank. Nosso sistema passou nos dois testes.

Então, seguimos para o procedimento de design do DLQR no sistema aumentado, obtendo a matriz de ganho K para a estabilização do pêndulo, e tendo como parâmetro as matriz de peso de entrada Q e R (já normalizadas). No final, temos uma matriz com os seguintes pesos:

\$\$ \tiny K = \begin{bmatrix} -0.359816139587012 & -0.16899712716524 & -0.062273242637461 & -1.04054845575376 & 0.00552059416223505 & 0.0178652255170376\\ -0.359816139587049 & -0.168997127165247 & 0.062273242637461 & -1.04054845575384 & 0.00552059416223684 & -0.0178652255170376 \end{bmatrix} \$\$

O dlqr pode ter sua estabilidade testada com seus autovalores. Se o valor absoluto deles estiver abaixo de 1, o sistema é estável. Nosso sistema retornou os seguintes valores:

\$\$ \tiny E = \begin{bmatrix} 0.00995189968460506 & 0.958224659846958 & 0.958224659846958 & 0.928541227517652 & 0.664161585872212 & 0.664161585872212 \end{bmatrix} \$\$

Os valores satisfazem a regra de estabilidade do sistema.

Simulação em python

Para validar o modelo foi importante realizar a simulação do mesmo. Utilizamos o software Matlab - Simulink para gerar o modelo linearizado (gif da esquerda) do sistema e com o Python, criamos o modelo não linear (gif da direita).

Após carregar o modelo na simulação, implementamos o controlador no sistema e ele respondeu muito bem aos testes.

ROS

Seguimos então para a simulação no Gazebo - ROS. E o controlador proposto pode ser visto no data flow em seguida.

Podemos ver nos vídeos seguintes que o controlador se comportou bem no simulador. Continuou estável contra distúrbios externos de até 800 N e se comportou bem ao receber comandos de movimentos lineares e angulares.

Próximos passos

Nesta etapa do projeto, nós apresentamos o modelo matemático do Bbot. 📚📖

Os resultados para os testes foram apresentados nas suas respectivas descrições. Para as próximas etapas, serão apresentados os testes com o robô real, demonstrando o controle e seus ajustes na prática.

Para uma abordagem mais abrangente do projeto e de como ele foi feito com o python control, siga para esse link.