Doppelte Pendelbewegungsgleichung in Python

Was ist ein Doppelpendel?

Zuvor habe ich in dem Artikel Motorgleichung mit Sympy Sympy verwendet, um die kinematische Gleichung des Feder-Dämpfer-Systems abzuleiten. Dieses Mal möchte ich die Bewegungsgleichung des Doppelpendels ableiten. Doppelpendel sind in der physikalischen Mechanik üblich, und wie in der folgenden Abbildung gezeigt, befindet sich am Ende des Pendels ein zusätzliches Pendel. Es ist nur ein Doppelpendel, aber es ist eine komplexe Bewegung, die mit einem einzelnen Pendel nicht zu vergleichen ist. Daher wird sie häufig in Chaos und komplexen Systemen erwähnt.

Formel

Betrachten wir zunächst die Bewegungsgleichung eines einfachen Pendels. (Stellen Sie sich in der obigen Abbildung ein einzelnes Pendel vor.)

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

Es ist ersichtlich, dass ein Element den Trägheitsterm darstellt und zwei Elemente den Schwerkraftterm darstellen und die Winkelbeschleunigung des Pendels proportional zu dem $ \ sin $ des Pendelwinkels zu diesem Zeitpunkt ist. Als nächstes kommt das Doppelpendel. Wenn es darum geht, ernsthaft abzuleiten, wird es eine lange Geschichte mit der Lagrange-Funktion sein, also werde ich dieses Mal die Formel Wikipedia überprüfen. 8C% E9% 87% 8D% E6% 8C% AF% E3% 82% 8A% E5% AD% 90) wird schnell eingenommen.

(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

Wenn Sie sich die Gleichung ansehen, sehen Sie, dass sie erheblich komplizierter ist als die kinematische Gleichung eines einfachen Pendels.

SymPyBotics Das letzte Mal habe ich das Modul physics.mechanics von SymPy direkt verwendet, um die Formel abzuleiten, aber der starre Körper wie diesmal ist eine Klausel. Ich möchte SymPyBotics verwenden, um das Ableiten von Formeln für durch (Link) verbundene Objekte zu vereinfachen. Holen Sie es sich von Github und installieren Sie es.

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

Erstellen Sie mit SymPyBotics ein Doppelpendel. Unten ist der 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)

Der Ort, an dem Sie "sympybotics.RobotDef" aufrufen, ist der Ort, an dem Sie das Doppelpendel erstellen. [(0, 'l_1', 0, 'q'), (0, 'l_2', 0, 'q')] ist der eigentliche Doppelpendelparameter, aber Denavit-Hartenberg-Parameter ist eine Methode zur Angabe von Parametern, die von Robotern verwendet werden. Kurz gesagt geben "l_1" und "l_2" die Länge von Pendel 1 und Pendel 2 an, und "q" gibt den Winkel des Pendels an, die den Variablen "q1" und "q2" zugewiesen werden, nachdem das Pendel erstellt wurde. Der erste Winkel ist "q1-pi / 2", der hinzugefügt wird, um anzupassen, wo der Winkel gemessen wird. Dies liegt daran, dass SymPyBotics den Winkel von der positiven x-Achse misst, während die Abbildung den Winkel von der negativen y-Achse misst. Wenn Sie ein dreifaches Pendel wollen, "[(0," l_1 ", 0," q-pi / 2 "), (0," l_2 ", 0," q "), (0," l_3 ", 0," q ')] `. Der letzte Druck, den ich geschrieben habe, gibt die Formel aus, die die Position des Pendels ausdrückt, und lautet wie folgt.

[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))

Als nächstes wird jeder Term (Trägheitsterm, Zentrifugal- / Kolliolenkraftterm, Schwerkraftterm) abgeleitet, der ein Element der Bewegungsgleichung ist. Wird mit rbt.dyn.gen_all () generiert und löscht diesmal unnötige Parameter (Parameter, die sich auf das Trägheitsmoment beziehen).

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))

Schließlich berechnen wir tatsächlich die kinetische Gleichung. Zum Zeitpunkt der ersten Zeile wurde die kinetische Gleichung abgeleitet, aber [Wikipedia](http://ja.wikipedia.org/wiki/%E4%BA%8C%E9%87%8D%E6%8C% AF% E3% 82% 8A% E5% AD% 90) wurde vereinfacht und die Variablen wurden geändert, um die Ergebnisse zu vergleichen.

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))]])

Es ist etwas schwer zu erkennen, aber wenn Sie der Meinung sind, dass die Formel gleich 0 ist und unnötige Variablen entfernen, [Wikipedia's Doppelpendel-Bewegungsgleichung](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. Sie können sehen, dass es mit BC.8F übereinstimmt. Diesmal war es ein Doppelpendel, aber es scheint, dass es auch zur Ableitung von Bewegungsgleichungen für mehrere Pendel und Roboter angewendet werden kann.