求解抛向空中的短棒的运动方程

`$\begin{array}{l}\left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\stackrel{¨}{\mathit{x}}-{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\stackrel{¨}{\theta }\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}\theta -{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\stackrel{˙}{\theta }}^{2}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}\theta =0,\\ \left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\stackrel{¨}{\mathit{y}}-{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\stackrel{¨}{\theta }\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}\theta -{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\stackrel{˙}{\theta }}^{2}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}\theta +\left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\mathit{g}=0,\\ {\mathit{L}}^{2}\stackrel{¨}{\theta }-\mathit{L}\text{\hspace{0.17em}}\stackrel{¨}{\mathit{x}}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}\theta +\mathit{L}\text{\hspace{0.17em}}\stackrel{¨}{\mathit{y}}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}\theta +\mathit{g}\text{\hspace{0.17em}}\mathit{L}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}\theta =0.\end{array}$`

编写方程代码

`ode45` 求解器要求将方程写作 $\stackrel{˙}{\mathit{q}}=\mathit{f}\left(\mathit{t},\mathit{q}\right)$ 形式，其中 $\stackrel{˙}{\mathit{q}}$ 是每个坐标的一阶导数。在此问题中，解向量有六个分量：$\mathit{x}$$\mathit{y}$、角度 $\theta$ 及各自的一阶导数：

`$\mathit{q}=\left[\begin{array}{c}{\mathit{q}}_{1}\\ {\mathit{q}}_{2}\\ {\mathit{q}}_{3}\\ {\mathit{q}}_{4}\\ {\mathit{q}}_{5}\\ {\mathit{q}}_{6}\end{array}\right]=\left[\begin{array}{c}\mathit{x}\\ \stackrel{˙}{\mathit{x}}\\ \mathit{y}\\ \stackrel{˙}{\mathit{y}}\\ \theta \\ \stackrel{˙}{\theta }\end{array}\right].$`

`$\begin{array}{l}\left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\stackrel{˙}{{\mathit{q}}_{2}}-{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\stackrel{˙}{{\mathit{q}}_{6}}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}={\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\mathit{q}}_{6}^{2}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5},\\ \left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\stackrel{˙}{{\mathit{q}}_{4}}-{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\stackrel{˙}{{\mathit{q}}_{6}}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}={\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\mathit{q}}_{6}^{2}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}-\left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\mathit{g},\\ {\mathit{L}}^{2}\stackrel{˙}{{\mathit{q}}_{6}}-\mathit{L}\text{\hspace{0.17em}}\stackrel{˙}{{\mathit{q}}_{2}}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}+\mathit{L}\text{\hspace{0.17em}}\stackrel{˙}{{\mathit{q}}_{4}}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}=-\mathit{g}\text{\hspace{0.17em}}\mathit{L}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}.\end{array}$`

`$\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& {\mathit{m}}_{1}+{\mathit{m}}_{2}& 0& 0& 0& -{\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& {\mathit{m}}_{1}+{\mathit{m}}_{2}& 0& {\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}\\ 0& 0& 0& 0& 1& 0\\ 0& -\mathit{L}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}& 0& \mathit{L}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}& 0& {\mathit{L}}^{2}\end{array}\right]\left[\begin{array}{c}\stackrel{˙}{{\mathit{q}}_{1}}\\ \stackrel{˙}{{\mathit{q}}_{2}}\\ \stackrel{˙}{{\mathit{q}}_{3}}\\ \stackrel{˙}{{\mathit{q}}_{4}}\\ \stackrel{˙}{{\mathit{q}}_{5}}\\ \stackrel{˙}{{\mathit{q}}_{6}}\end{array}\right]=\left[\begin{array}{c}{\mathit{q}}_{2}\\ {\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\mathit{q}}_{6}^{2}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}\\ {\mathit{q}}_{4}\\ {\mathit{m}}_{2}\mathit{L}\text{\hspace{0.17em}}{\mathit{q}}_{6}^{2}\text{\hspace{0.17em}}\mathrm{sin}\text{\hspace{0.17em}}{\mathit{q}}_{5}-\left({\mathit{m}}_{1}+{\mathit{m}}_{2}\right)\mathit{g}\\ {\mathit{q}}_{6}\\ -\mathit{g}\text{\hspace{0.17em}}\mathit{L}\text{\hspace{0.17em}}\mathrm{cos}\text{\hspace{0.17em}}{\mathit{q}}_{5}\end{array}\right]$`

```function M = mass(t,q,P) % Extract parameters m1 = P.m1; m2 = P.m2; L = P.L; g = P.g; % Mass matrix function M = zeros(6,6); M(1,1) = 1; M(2,2) = m1 + m2; M(2,6) = -m2*L*sin(q(5)); M(3,3) = 1; M(4,4) = m1 + m2; M(4,6) = m2*L*cos(q(5)); M(5,5) = 1; M(6,2) = -L*sin(q(5)); M(6,4) = L*cos(q(5)); M(6,6) = L^2; end ```

```function dydt = f(t,q,P) % Extract parameters m1 = P.m1; m2 = P.m2; L = P.L; g = P.g; % Equation to solve dydt = [q(2) m2*L*q(6)^2*cos(q(5)) q(4) m2*L*q(6)^2*sin(q(5))-(m1+m2)*g q(6) -g*L*cos(q(5))]; end ```

求解方程组

```P.m1 = 0.1; P.m2 = 0.1; P.L = 1; P.g = 9.81```
```P = struct with fields: m1: 0.1000 m2: 0.1000 L: 1 g: 9.8100 ```

`tspan = linspace(0,4,25);`

`y0 = [0; 4; P.L; 20; -pi/2; 2];`

`opts = odeset('Mass',@(t,q) mass(t,q,P));`

• 方程 `@(t,q) f(t,q,P)` 的匿名函数

• 请求解的时间组成的向量 `tspan`

• 初始条件组成的向量 `y0`

• 引用质量矩阵的 options 结构体 `opts`

`[t,q] = ode45(@(t,q) f(t,q,P),tspan,y0,opts);`

绘制结果

`ode45` 的输出包含运动方程在每个请求的时间步的解。为了检验结果，对短棒随时间的运动轨迹绘图。

```figure title('Motion of a Thrown Baton, Solved by ODE45'); axis([0 22 0 25]) hold on for j = 1:length(t) theta = q(j,5); X = q(j,1); Y = q(j,3); xvals = [X X+P.L*cos(theta)]; yvals = [Y Y+P.L*sin(theta)]; plot(xvals,yvals,xvals(1),yvals(1),'ro',xvals(2),yvals(2),'go') end hold off```

参考

[1] Wells, Dare A. Schaum’s Outline of Theory and Problems of Lagrangian Dynamics:With a Treatment of Euler’s Equations of Motion, Hamilton’s Equations and Hamilton’s Principle.Schaum's Outline Series.New York:Schaum Pub.Co, 1967.

局部函数

```function dydt = f(t,q,P) % Equations to solve % Extract parameters m1 = P.m1; m2 = P.m2; L = P.L; g = P.g; % RHS of system of equations dydt = [q(2) m2*L*q(6)^2*cos(q(5)) q(4) m2*L*q(6)^2*sin(q(5))-(m1+m2)*g q(6) -g*L*cos(q(5))]; end %------------------------------------------------ function M = mass(t,q,P) % Mass matrix function % Extract parameters m1 = P.m1; m2 = P.m2; L = P.L; g = P.g; % Set nonzero elements in mass matrix M = zeros(6,6); M(1,1) = 1; M(2,2) = m1 + m2; M(2,6) = -m2*L*sin(q(5)); M(3,3) = 1; M(4,4) = m1 + m2; M(4,6) = m2*L*cos(q(5)); M(5,5) = 1; M(6,2) = -L*sin(q(5)); M(6,4) = L*cos(q(5)); M(6,6) = L^2; end```