求解双摆与单摆运动轨迹并绘制动图

忧郁奔向冷的天

已于 2022-07-04 20:16:37 修改

阅读量1.2w

点赞数 11

文章标签：力学

于 2021-06-14 16:14:11 首次发布

本文链接：https://blog.csdn.net/qq_37083038/article/details/117884311

版权

内容主要来自《python科学计算》一书

双摆

初始角度为 $\theta_1$ 与 $\theta_2$ ，无质量杆长 $L_1$ 与 $L_2$ ，小球质量 $m_1$ 与 $m_2$

拉格朗日力学求解

建立坐标系

设小球的坐标为 $x_1,y_1)$ 与 $x_2,y_2)$
$x_1 = L_1sin(\theta_1), y_1 = -L_1cos(\theta_1)$
$x_2 = L_1sin(\theta_1)+L_2sin(\theta_2), y_2 = -L_1cos(\theta_1)-L_2cos(\theta_2)$

双摆系统的动能
$T=\frac{m_1}{2}(\dot{x}_1^2 + \dot{y}_1^2) + \frac{m_2}{2}(\dot{x}_2^2 + \dot{y}_2^2)$
双摆系统的势能
$V=-m_1gy_1 - m_2gy_2$

拉格朗日方程
$\frac{d}{dt}\frac{\partial{L}}{\partial{\dot{\theta}_1}} - \frac{\partial{L}}{\partial{\theta_1}} = 0$
$\frac{d}{dt}\frac{\partial{L}}{\partial{\dot{\theta}_2}} - \frac{\partial{L}}{\partial{\theta_2}} = 0$
$L = T - V$

将两个拉格朗日方程化简后得到两个常微分方程
$L_1[(m_1+m_2)L_1\theta_1'' + m_2L_2cos(\theta_1-\theta_2)\theta_2'' + m_2L_2sin(\theta_1-\theta_2)\theta_2' + (m_1+m_2)gsin(\theta_1)] = 0$
$m_2L_2[L_2\theta_2'' + L_1cos(\theta_1-\theta_2)\theta_1'' - L_1sin(\theta_1-\theta_2)\theta_1'^2 + gsin(\theta_2)] = 0$

注： $x'=x_t, x''=x_{tt}, \theta'=\theta_t, \theta''=\theta_{tt}$

sympy求解

得到化简后的常微分方程

# -*- coding: utf-8 -*-
"""
Created on Sun Jun 13 21:30:07 2021

@author: Leslie Lee  
"""

# -*- coding: utf-8 -*-
from sympy import Function
from sympy import sin, cos, var, diff, simplify, trigsimp
from sympy import Derivative as D

# theta1与theta2
f, h = Function('f'), Function('h')

# 定义常微分方程中的变量与常数
# var, symbols, Symbol都可以定义符号变量
var("x1 x2 y1 y2 l1 l2 m1 m2 th1 th2 dth1 dth2 ddth1 ddth2 t g")

# Derivative与diff不同, 前者是建立一个求导关系, 后者直接对表达式求导
# 如定义两个变量x与y, Derivative(x, y)返回Derivative(x, y), diff(x, y)返回0
sublist = [(D(f(t), t, t), ddth1),
           (D(f(t), t), dth1),
           (D(h(t), t, t), ddth2),
           (D(h(t),t), dth2),
           (f(t), th1),
           (h(t), th2)]

# 坐标与角度的关系
# sin与cos函数
x1 = l1*sin(f(t))
y1 = -l1*cos(f(t))
x2 = l1*sin(f(t)) + l2*sin(h(t))
y2 = -l1*cos(f(t)) - l2*cos(h(t))

# 对坐标求一阶导
vx1 = diff(x1, t)
vx2 = diff(x2, t)
vy1 = diff(y1, t)
vy2 = diff(y2, t)

# 拉格朗日量
L = m1/2*(vx1**2 + vy1**2) + m2/2*(vx2**2 + vy2**2) - m1*g*y1 - m2*g*y2

# 拉格朗日方程
def Leq(L, v):
    eq = diff(diff(L, diff(v, t)),t) - diff(L, v)
    eq = eq.subs(sublist)
    eq = trigsimp(simplify(eq))
    return eq

eq1 = Leq(L, f(t))
eq2 = Leq(L, h(t))
print(eq1)
print(eq2)

给定初始条件，求一段时间内两个小球的轨迹

# -*- coding: utf-8 -*-

from math import sin,cos
import numpy as np
from scipy.integrate import odeint


g = 9.8
class DoublePendulum(object):
    def __init__(self, m1, m2, l1, l2):
        self.m1, self.m2, self.l1, self.l2 = m1, m2, l1, l2
        # 初始状态: th1 th2 v1 v2
        self.init_status = np.array([0.0,0.0,0.0,0.0])
        
    def equations(self, w, t): 
        """
        微分方程公式
        """
        m1, m2, l1, l2 = self.m1, self.m2, self.l1, self.l2
        th1, th2, v1, v2 = w
        # dv1 = ddth1, dv2 = ddth2
        dth1 = v1
        dth2 = v2
        
        #eq of th1
        #eq1: a*dv1 + b*dv2 + c = 0
        a = l1*(m1+m2)  # dv1 parameter
        b = m2*l2*cos(th1-th2) # dv2 paramter
        c = m2*l2*sin(th1-th2)*dth2*dth2 + (m1+m2)*g*sin(th1)
        
        #eq of th2
        #eq2: d*dv1 + e*dv2 + f = 0
        d = l1*cos(th1-th2) # dv1 parameter
        e = l2 # dv2 parameter
        f = -l1*sin(th1-th2)*dth1*dth1 + g*sin(th2)
        
        # a*dv1 + b*dv2 = -c
        # d*dv1 + e*dv2 = -f
        # 求解线性方程组 dv1与dv2
        dv1, dv2 = np.linalg.solve([[a,b],[d,e]], [-c,-f])  
        
        return np.array([dth1, dth2, dv1, dv2])

      
def double_pendulum_odeint(pendulum, ts, te, tstep):
    """
    对双摆系统的微分方程组进行数值求解，返回两个小球的X-Y坐标
    """
    # 时间离散
    t = np.arange(ts, te, tstep)
    # pendulum.equations给出如何计算th1 th2 v1 v2 对t的一阶导 dth1, dth2, dv1, dv2
    # pendulum.init_status给出th1 th2 v1 v2的初始值
    track = odeint(pendulum.equations, pendulum.init_status, t)
    # t时间内 th1与th2的变化
    th1_array, th2_array = track[:,0], track[:, 1]
    # 由th1与th2的变化 得出两个小球坐标的变化
    l1, l2 = pendulum.l1, pendulum.l2
    x1 = l1*np.sin(th1_array)
    y1 = -l1*np.cos(th1_array)
    x2 = x1 + l2*np.sin(th2_array)
    y2 = y1 - l2*np.cos(th2_array)
    #将最后的状态赋给pendulum
    pendulum.init_status = track[-1,:].copy() 
    return x1, y1, x2, y2 


if __name__ == "__main__":    
    import matplotlib.pyplot as plt
    
    plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
    plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签

    pendulum = DoublePendulum(1.0, 2.0, 1.0, 2.0) 
    # th1, th2 = 0.2, 0.4 # 初始位置为小角度
    th1, th2 = 1*np.pi, 1.5*np.pi # 初始位置为大角度
    pendulum.init_status[:2] = th1, th2
    x1, y1, x2, y2 = double_pendulum_odeint(pendulum, 0, 30, 0.02)
    plt.plot(x1,y1, label = u"上球")
    plt.plot(x2,y2, label = u"下球")
    plt.legend()
    plt.axis("equal")
    plt.show()

重点在于odient函数的使用

def odeint(func, y0, t, args=(), Dfun=None, col_deriv=0, full_output=0,
           ml=None, mu=None, rtol=None, atol=None, tcrit=None, h0=0.0,
           hmax=0.0, hmin=0.0, ixpr=0, mxstep=0, mxhnil=0, mxordn=12,
           mxords=5, printmessg=0, tfirst=False)
求解一阶常微分方程
func 计算$\frac{y}{t}$
y0 给出y的初值
t 给出离散时间序列
args=() 额外传给func的参数, 例子中args=(1.0,)将杆长传给了func

单摆

单摆方程
$L*\theta_{tt} + gsin(\theta)= 0$

给定初始条件，求一段时间内小球的轨迹

from math import sin
import numpy as np
from scipy.integrate import odeint

g = 9.8

def pendulum_equations(w, t, l):
    th, v = w
    dth = v
    dv  = - g/l * sin(th)
    return dth, dv
    
if __name__ == "__main__":
    import pylab as pl
    t = np.arange(0, 10, 0.01)
    # 初始条件th=1, dth=0
    track = odeint(pendulum_equations, (1.0, 0), t, args=(1.0,))
    # 绘制th随时间的变化图
    pl.plot(t, track[:, 0])
    pl.xlabel(u"时间(秒)")
    pl.ylabel(u"震度角度(弧度)")
    pl.show()

单摆周期
最大摆动角度很小时， $sin(\theta) \approx \theta$ ，摆动周期约为 $T_0 = 2*\pi \sqrt{l/g}$
任意摆动角度时，精确周期为 $T=4\sqrt(l/g)K(sin(\theta_2/2)), K(k)=\int_0^{(k/2)}\frac{d\theta}{\sqrt{1-k^2sin^2(\theta)}}$

# -*- coding: utf-8 -*-
from math import sin, sqrt
import numpy as np
from scipy.integrate import odeint
from scipy.optimize import fsolve
import matplotlib
import matplotlib.pyplot as plt
from scipy.special import ellipk

matplotlib.use('Qt5Agg') 
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签

g = 9.8

def pendulum_equations(w, t, l):
    th, v = w
    dth = v
    dv  = - g/l * sin(th)
    return dth, dv

# 计算t时刻的th
# 给定初始条件th=th0, dth=0; 给定离散时间序列[0, t], odient内部会对时间序列再细分
def pendulum_th(t, l, th0):
    track = odeint(pendulum_equations, (th0, 0), [0, t], args=(l,))
    # 返回最后一个th值
    return track[-1, 0]

# 给定杆长与初始角度, 计算周期  从初始角度到0角度为摆动周期的1/4
# 找到使得track[-1, 0]=0的t, 即th从th0到0, 此时t为T/4
# fsolve会不断调用pendulum_th直到track[-1, 0]=0
def pendulum_period(l, th0):
    # T/4的估计值
    t0 = 2*np.pi*sqrt( l/g ) / 4
    # 得出T/4
    t = fsolve( pendulum_th, t0, args = (l, th0) ) 
    return t*4

# 摆动初始角从0到90度的摆动周期   l=1
ths = np.arange(0, np.pi/2.0, 0.01)
periods = [pendulum_period(1, th) for th in ths]

# 计算单摆周期的精确值 第一类椭圆积分
periods2 = 4*sqrt(1.0/g)*ellipk(np.sin(ths/2)**2) 

plt.plot(ths, periods, label = u"fsolve计算的单摆周期", linewidth=4.0)
plt.plot(ths, periods2, "r", label = u"单摆周期精确值", linewidth=2.0)
plt.legend(loc='upper left')
plt.xlabel(u"初始摆角(弧度)")
plt.ylabel(u"摆动周期(秒)")
plt.show()

重点在于fsolve函数

def fsolve(func, x0, args=(), fprime=None, full_output=0,
           col_deriv=0, xtol=1.49012e-8, maxfev=0, band=None,
           epsfcn=None, factor=100, diag=None)
寻根
func 方程或方程组
x0 根的估计值
args=() 传入函数中的额外参数

''' 举例1
from scipy.optimize import fsolve
def func(x):
    return [x[0] * np.cos(x[1]) - 4,
            x[1] * x[0] - x[1] - 5]
root = fsolve(func, [1, 1])
'''

'''举例2
# -*- coding: utf-8 -*-
from math import sin, sqrt
import numpy as np
from scipy.integrate import odeint
from scipy.optimize import fsolve

g = 9.8

def pendulum_equations(w, t, l):
    th, v = w
    dth = v
    dv  = - g/l * sin(th)
    return dth, dv

# 计算t时刻的th
# 给定初始条件th=th0, dth=0; 给定离散时间序列[0, t], odient内部会对时间序列再细分
def pendulum_th(t, l, th0):
    track = odeint(pendulum_equations, (th0, 0), [0, t], args=(l,))
    # 返回最后一个th值
    # print(track) 可以发现运行后输出的track不止1个 说明fsolve在不断调用pendulum_th, 直到pendulum_th的返回为0
    return track[-1, 0]

# 给定杆长与初始角度, 计算周期  从初始角度到0角度为摆动周期的1/4
# 找到使得track[-1, 0]=0的t, 即th从th0到0, 此时t为T/4
def pendulum_period(l, th0):
    # T/4的估计值
    t0 = 2*np.pi*sqrt( l/g ) / 4
    # 得出T/4
    t = fsolve( pendulum_th, t0, args = (l, th0) ) 
    return t*4

# 初始角度为1.5pi  l=1
periods = pendulum_period(1, np.array([1.5*np.pi]))
'''

matplotlib.animation制作动画

https://www.cnblogs.com/hellowzl/p/9773277.html

from numpy import sin, cos
import numpy as np
import matplotlib.pyplot as plt
import scipy.integrate as integrate
import matplotlib.animation as animation

# 常量
G = 9.8  # acceleration due to gravity, in m/s^2
L1 = 1.0  # length of pendulum 1 in m
L2 = 1.0  # length of pendulum 2 in m
M1 = 1.0  # mass of pendulum 1 in kg
M2 = 1.0  # mass of pendulum 2 in kg

# dydx = [dth1, ddth1, dth2, ddth2]
def derivs(state, t):
    dydx = np.zeros_like(state)
    dydx[0] = state[1]

    del_ = state[2] - state[0]
    den1 = (M1 + M2)*L1 - M2*L1*cos(del_)*cos(del_)
    dydx[1] = (M2*L1*state[1]*state[1]*sin(del_)*cos(del_) +
               M2*G*sin(state[2])*cos(del_) +
               M2*L2*state[3]*state[3]*sin(del_) -
               (M1 + M2)*G*sin(state[0]))/den1

    dydx[2] = state[3]
    
    den2 = (L2/L1)*den1
    dydx[3] = (-M2*L2*state[3]*state[3]*sin(del_)*cos(del_) +
               (M1 + M2)*G*sin(state[0])*cos(del_) -
               (M1 + M2)*L1*state[1]*state[1]*sin(del_) -
               (M1 + M2)*G*sin(state[2]))/den2
    return dydx

# 离散时间序列
dt = 0.05
t = np.arange(0.0, 20, dt)

# 初始条件
th1 = 120.0
w1 = 0.0
th2 = -10.0
w2 = 0.0
state = np.radians([th1, w1, th2, w2]) # 角度转弧度

# 使用odient求出[th1, dth1, th2, dth2]
y = integrate.odeint(derivs, state, t)
# 由th1与th2求出坐标
x1 = L1*sin(y[:, 0])
y1 = -L1*cos(y[:, 0])
x2 = L2*sin(y[:, 2]) + x1
y2 = -L2*cos(y[:, 2]) + y1

# ----------------------------------
# 用计算出的小球运动轨迹来制作动画
# 重要的是后面的这几行代码
# -----------------------------------
# 建立fig
fig = plt.figure()
ax = fig.add_subplot(111, autoscale_on=False, xlim=(-2, 2), ylim=(-2, 2))
ax.grid()
line, = ax.plot([], [], 'o-', lw=2)
time_template = 'time = %.1fs'
time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes)

def init():
    line.set_data([], [])
    time_text.set_text('')
    return line, time_text

def animate(i):
    thisx = [0, x1[i], x2[i]]
    thisy = [0, y1[i], y2[i]]

    line.set_data(thisx, thisy)
    time_text.set_text(time_template % (i*dt))
    return line, time_text

ani = animation.FuncAnimation(fig, animate, np.arange(1, len(y)),
                              interval=25, blit=True, init_func=init)

# ani.save('double_pendulum.mp4', fps=15)
plt.show()