Equation de mouvement à double pendule en python

Qu'est-ce qu'un double pendule?

Auparavant, dans l'article Equation moteur avec sympy, j'ai utilisé sympy pour dériver l'équation cinématique du système ressort-amortisseur. Cette fois, je voudrais dériver l'équation de mouvement du double pendule. Les doubles pendules sont courants en mécanique physique, et comme le montre la figure ci-dessous, il y a un pendule supplémentaire à l'extrémité du pendule. C'est juste un double pendule, mais c'est un mouvement complexe qui est incomparable à un seul pendule, donc il est souvent mentionné dans le chaos et les systèmes complexes.

Formule

Examinons d'abord l'équation de mouvement d'un simple pendule. (Imaginez un seul pendule dans la figure ci-dessus.)

ml\ddot{\theta} + mg \sin\theta = 0

On peut voir qu'un élément représente le terme inertiel et deux éléments représentent le terme de gravité, et l'accélération angulaire du pendule est proportionnelle au $ \ sin $ de l'angle du pendule à ce moment. Vient ensuite le double pendule. Quand il s'agira de le dériver sérieusement, ce sera une longue histoire en utilisant la fonction Lagrange, donc cette fois je vais vérifier la formule Wikipedia 8C% E9% 87% 8D% E6% 8C% AF% E3% 82% 8A% E5% AD% 90) est pris rapidement.

(m_1+m_2)l_1\ddot{\theta}_1 + m_2l_2\ddot{\theta}_2\cos(\theta_1 - \theta_2) + m_2l_2\dot{\theta}_2^2\sin(\theta_1 - \theta_2) + (m_1 + m_2)g\sin\theta_1 = 0
\\\
l_1l_2\ddot{\theta}_1\cos(\theta_1- \theta_2) + l_2^2\ddot{\theta}_2 - l_1l_2\dot{\theta}_1^2\sin(\theta_1 - \theta_2) + gl_2\sin\theta_2 = 0

En regardant simplement l'équation, vous pouvez voir qu'elle est considérablement plus compliquée que l'équation cinématique d'un simple pendule.

SymPyBotics La dernière fois, j'ai utilisé le module physics.mechanics de SymPy directement pour dériver la formule, mais le corps rigide comme cette fois est une clause. Je voudrais utiliser SymPyBotics, ce qui facilite la dérivation de formules pour les objets connectés par (lien). Obtenez-le depuis github et installez-le.

git clone https://github.com/cdsousa/SymPyBotics.git
cd SymPyBotics
sudo python setup.py install

Créez un double pendule à l'aide de SymPyBotics. Ci-dessous le code.

import sympybotics
rbtdef = sympybotics.RobotDef('Double Pendulum',
                              [(0, 'l_1', 0, 'q-pi/2'),
                               (0, 'l_2', 0, 'q')],
                              dh_convention='standard')
from sympy import symbols, Matrix, simplify, pprint
g = symbols('g')
rbtdef.gravityacc = Matrix([[0.0],[-g],[0.0]])
rbt = sympybotics.RobotDynCode(rbtdef)
for p in rbt.geo.p:
    pprint(simplify(p), use_unicode=False)

L'endroit où vous appelez sympybotics.RobotDef est l'endroit où vous créez le double pendule. [(0, 'l_1', 0, 'q'), (0, 'l_2', 0, 'q')] est le paramètre de double pendule réel, mais paramètre Denavit – Hartenberg est une méthode pour donner des paramètres utilisés par les robots. Brièvement, "l_1" et "l_2" indiquent la longueur du pendule 1 et du pendule 2, et "q" indique l'angle du pendule, qui sont affectés aux variables "q1" et "q2" après la création du pendule. Le premier angle est «q1-pi / 2», qui est ajouté pour ajuster l'endroit où l'angle est mesuré. En effet, SymPyBotics mesure l'angle à partir de l'axe x positif, tandis que la figure mesure l'angle à partir de l'axe y négatif. Si vous voulez faire un triple pendule, [(0, 'l_1', 0, 'q-pi / 2'), (0, 'l_2', 0, 'q'), (0, 'l_3', 0, ' q ')] . Le dernier tirage que j'ai écrit produit la formule qui exprime la position du pendule, et c'est comme suit.

[l_1*sin(q1) ]
[            ]
[-l_1*cos(q1)]
[            ]
[     0      ]
[l_1*sin(q1) + l_2*sin(q1 + q2) ]
[                               ]
[-l_1*cos(q1) - l_2*cos(q1 + q2)]
[                               ]
[               0               ]

rbt.dyn.gen_all()
params = {'l_1x': 0.0, 'l_1y': 0.0,
          'l_2x': 0.0, 'l_2y': 0.0,
          'L_1zz': 0.0, 'L_2zz': 0.0}
mass = simplify(rbt.dyn.M.subs(params))
corioli = simplify(rbt.dyn.c.subs(params))
gravity = simplify(rbt.dyn.g.subs(params))

Ensuite, chaque terme (terme d'inertie, terme de force centrifuge / collioli, terme de gravité) qui est un élément de l'équation de mouvement est dérivé. Généré avec rbt.dyn.gen_all (), et supprimez les paramètres inutiles (paramètres liés au moment d'inertie) cette fois.

eq = (mass * rbtdef.ddq + corioli + gravity).as_mutable()
eq[0, 0] -= eq[1, 0]
params = {rbtdef.ddq[1, 0]: rbtdef.ddq[1, 0] - rbtdef.ddq[0, 0],
          rbtdef.dq[1, 0]: rbtdef.dq[1, 0] - rbtdef.dq[0, 0],
          rbtdef.q[1, 0]: rbtdef.q[1, 0] - rbtdef.q[0, 0]}
print simplify(eq.subs(params))

Enfin, nous calculons en fait l'équation cinétique. Au moment de la première ligne, l'équation cinétique a été dérivée, mais [Wikipedia](http://ja.wikipedia.org/wiki/%E4%BA%8C%E9%87%8D%E6%8C% AF% E3% 82% 8A% E5% AD% 90) a été simplifiée et les variables ont été modifiées pour comparer les résultats.

Matrix([
[l_1*(\ddot{q}_1*l_1*m_1 + \ddot{q}_1*l_1*m_2 + \ddot{q}_2*l_2*m_2*cos(q1 - q2) + \dot{q}_2**2*l_2*m_2*sin(q1 - q2) + g*m_1*sin(q1) + g*m_2*sin(q1))],
[                                                 l_2*m_2*(\ddot{q}_1*l_1*cos(q1 - q2) + \ddot{q}_2*l_2 - \dot{q}_1**2*l_1*sin(q1 - q2) + g*sin(q2))]])

C'est un peu difficile à voir, mais si vous pensez que la formule est égale à 0 et supprimez les variables inutiles, [l'équation de mouvement à double pendule de Wikipedia](http://ja.wikipedia.org/wiki/%E4%BA%8C%] E9% 87% 8D% E6% 8C% AF% E3% 82% 8A% E5% AD% 90 # .E9.81.8B.E5.8B.95.E6.96.B9.E7.A8.8B.E5. Vous pouvez voir qu'il correspond à BC.8F). Cette fois, il s'agissait d'un double pendule, mais il semble qu'il puisse également être appliqué à la dérivation d'équations de mouvement pour plusieurs balanciers et robots.