{ "cells": [ { "cell_type": "code", "execution_count": 120, "metadata": {}, "outputs": [], "source": [ "import sympy as sm\n", "import sympy.physics.mechanics as me\n", "me.init_vprinting(use_latex='mathjax')" ] }, { "cell_type": "code", "execution_count": 121, "metadata": {}, "outputs": [], "source": [ "E = me.ReferenceFrame('E')\n", "A = me.ReferenceFrame('A')\n", "# B = me.ReferenceFrame('B')\n", "D = me.ReferenceFrame('D')\n", "C = me.ReferenceFrame('C')\n", "G = me.ReferenceFrame('G')\n", "G_prime1 = me.ReferenceFrame('G_prime1')\n", "G_prime2 = me.ReferenceFrame('G_prime2')\n", "G_prime3 = me.ReferenceFrame('G_prime3')\n", "# N1 = me.ReferenceFrame('N1')\n", "# N2 = me.ReferenceFrame('N2')\n", "# N3 = me.ReferenceFrame('N3')" ] }, { "cell_type": "code", "execution_count": 122, "metadata": {}, "outputs": [], "source": [ "# 广义坐标\n", "q_yaw, q_drtr, q_geaz= me.dynamicsymbols('q_yaw, q_drtr, q_geaz')\n", "u_yaw, u_drtr, u_geaz = me.dynamicsymbols('u_yaw, u_drtr, u_geaz')\n", "D.orient_axis(A, q_yaw, A.y)\n", "D.set_ang_vel(A, u_yaw * A.y)\n", "\n", "# shaft C \n", "theta_tilt = sm.symbols(\"theta_tilt\")\n", "C.orient_axis(D, theta_tilt, D.z)\n", "\n", "# Hub G = Azimuth E\n", "Azimuth = q_drtr + q_geaz\n", "G.orient_axis(C, Azimuth, C.x)\n", "G.set_ang_vel(C, (u_drtr + u_geaz) * C.x)\n", "\n", "# 到每只叶片\n", "angle = 2*sm.pi/3\n", "G_prime1.orient_axis(G, angle * 0, G.x)\n", "G_prime2.orient_axis(G, angle * 1, G.x)\n", "G_prime3.orient_axis(G, angle * 2, G.x)" ] }, { "cell_type": "code", "execution_count": 123, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle 0$" ], "text/plain": [ "0" ] }, "execution_count": 123, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# 设置点\n", "Z = me.Point('Z') # 塔基\n", "Z.set_vel(A, 0)\n", "\n", "O = me.Point('O') # 塔顶\n", "Tower_length = sm.symbols(\"L_T\")\n", "O.set_pos(Z, Tower_length * A.y)\n", "\n", "O.v2pt_theory(Z, A, D)" ] }, { "cell_type": "code", "execution_count": 124, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\cos{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_x} - \\sin{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_z}$" ], "text/plain": [ "cos(q_yaw) a_x + -sin(q_yaw) a_z" ] }, "execution_count": 124, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# 机舱\n", "# 机舱质心位置\n", "N_x = sm.symbols(\"Nx\")\n", "N_y = sm.symbols(\"Ny\")\n", "N_z = sm.symbols(\"Nz\")\n", "U = me.Point('U')\n", "U.set_pos(O, N_x * D.x + N_y * D.y + N_z * D.z)\n", "\n", "# 机舱IMU\n", "IMU_x = sm.symbols(\"I_x\")\n", "IMU_y = sm.symbols(\"I_y\")\n", "IMU_z = sm.symbols(\"I_z\")\n", "IMU = me.Point('I')\n", "IMU.set_pos(O, IMU_x * D.x + IMU_y * D.y + IMU_z * D.z)\n", "D.x.express(A)" ] }, { "cell_type": "code", "execution_count": 125, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_x} + (L_{overhang} \\sin{\\left(\\theta_{tilt} \\right)} + L_{t2s})\\mathbf{\\hat{a}_y} - L_{overhang} \\sin{\\left(q_{yaw} \\right)} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{a}_z}$" ], "text/plain": [ "L_overhang⋅cos(θₜᵢₗₜ)⋅cos(q_yaw) a_x + (L_overhang⋅sin(θₜᵢₗₜ) + Lₜ₂ₛ) a_y + -L ↪\n", "\n", "↪ _overhang⋅sin(q_yaw)⋅cos(θₜᵢₗₜ) a_z" ] }, "execution_count": 125, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# point P\n", "P = me.Point('P')\n", "L_t2s, L_overhang = sm.symbols(\"L_t2s, L_overhang\")\n", "P.set_pos(O, L_t2s * D.y + L_overhang * C.x)\n", "r_o_p = P.pos_from(O)\n", "r_o_p.express(A)" ] }, { "cell_type": "code", "execution_count": 126, "metadata": {}, "outputs": [], "source": [ "# blade tip\n", "S_1 = me.Point('S_1')\n", "S_2 = me.Point('S_2')\n", "S_3 = me.Point('S_3')\n", "Blade_length = sm.symbols(\"L_B\")\n", "S_1.set_pos(P, Blade_length * G_prime1.y)\n", "S_2.set_pos(P, Blade_length * G_prime2.y)\n", "S_3.set_pos(P, Blade_length * G_prime3.y)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 获得点的速度、坐标系/刚体的角速度" ] }, { "cell_type": "code", "execution_count": 127, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( Nz u_{yaw}\\mathbf{\\hat{d}_x} - Nx u_{yaw}\\mathbf{\\hat{d}_z}, \\ I_{z} u_{yaw}\\mathbf{\\hat{d}_x} - I_{x} u_{yaw}\\mathbf{\\hat{d}_z}, \\ - L_{overhang} u_{yaw} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_z}, \\ u_{yaw}\\mathbf{\\hat{a}_y}, \\ u_{yaw}\\mathbf{\\hat{a}_y}, \\ (u_{drtr} + u_{geaz})\\mathbf{\\hat{c}_x} + u_{yaw}\\mathbf{\\hat{a}_y}\\right)$" ], "text/plain": [ "(Nz⋅u_yaw d_x + -Nx⋅u_yaw d_z, I_z⋅u_yaw d_x + -Iₓ⋅u_yaw d_z, -L_overhang⋅u_ya ↪\n", "\n", "↪ w⋅cos(θₜᵢₗₜ) c_z, u_yaw a_y, u_yaw a_y, (u_drtr + u_geaz) c_x + u_yaw a_y)" ] }, "execution_count": 127, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# 速度\n", "U.v2pt_theory(O, A, D)\n", "IMU.v2pt_theory(O, A, D)\n", "\n", "P.v2pt_theory(O, A, D)\n", "P.vel(A).express(D)\n", "# r_P_O.express(D)\n", "\n", "E_v_P = D.ang_vel_in(A).cross(r_o_p)\n", "\n", "U.vel(A), IMU.vel(A), P.vel(A), D.ang_vel_in(A), C.ang_vel_in(A), G.ang_vel_in(A)" ] }, { "cell_type": "code", "execution_count": 128, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( (- Nx u_{yaw}^{2} + Nz \\dot{u}_{yaw})\\mathbf{\\hat{d}_x} + (- Nx \\dot{u}_{yaw} - Nz u_{yaw}^{2})\\mathbf{\\hat{d}_z}, \\ (- I_{x} u_{yaw}^{2} + I_{z} \\dot{u}_{yaw})\\mathbf{\\hat{d}_x} + (- I_{x} \\dot{u}_{yaw} - I_{z} u_{yaw}^{2})\\mathbf{\\hat{d}_z}, \\ - L_{overhang} u_{yaw}^{2} \\cos^{2}{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_x} + L_{overhang} u_{yaw}^{2} \\sin{\\left(\\theta_{tilt} \\right)} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_y} - L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw}\\mathbf{\\hat{c}_z}, \\ \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}, \\ \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}, \\ (\\dot{u}_{drtr} + \\dot{u}_{geaz})\\mathbf{\\hat{c}_x} - \\left(u_{drtr} + u_{geaz}\\right) u_{yaw} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_z} + \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}\\right)$" ], "text/plain": [ "⎛⎛ 2 ⎞ ⎛ 2⎞ ⎛ 2 ↪\n", "⎝⎝- Nx⋅u_yaw + Nz⋅u_ẏaw⎠ d_x + ⎝-Nx⋅u_ẏaw - Nz⋅u_yaw ⎠ d_z, ⎝- Iₓ⋅u_yaw + I_ ↪\n", "\n", "↪ ⎞ ⎛ 2⎞ 2 2 ↪\n", "↪ z⋅u_ẏaw⎠ d_x + ⎝-Iₓ⋅u_ẏaw - I_z⋅u_yaw ⎠ d_z, -L_overhang⋅u_yaw ⋅cos (θₜᵢₗₜ) ↪\n", "\n", "↪ 2 ↪\n", "↪ c_x + L_overhang⋅u_yaw ⋅sin(θₜᵢₗₜ)⋅cos(θₜᵢₗₜ) c_y + -L_overhang⋅cos(θₜᵢₗₜ)⋅u ↪\n", "\n", "↪ ↪\n", "↪ _ẏaw c_z, u_ẏaw a_y, u_ẏaw a_y, (u_dṙtr + u_gėaz) c_x + -(u_drtr + u_geaz)⋅u ↪\n", "\n", "↪ ⎞\n", "↪ _yaw⋅cos(θₜᵢₗₜ) c_z + u_ẏaw a_y⎠" ] }, "execution_count": 128, "metadata": {}, "output_type": "execute_result" } ], "source": [ "U.acc(A), IMU.acc(A), P.acc(A), D.ang_acc_in(A), C.ang_acc_in(A), G.ang_acc_in(A)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 偏线速度,偏角速度" ] }, { "cell_type": "code", "execution_count": 129, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( \\mathbf{\\hat{d}_y}, \\ 0, \\ 0\\right)$" ], "text/plain": [ "(d_y, 0, 0)" ] }, "execution_count": 129, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# 机舱 D = E^w_r^N\n", "w_D_1 = D.ang_vel_in(A).diff(u_yaw, A).express(D)\n", "w_D_2 = D.ang_vel_in(A).diff(u_drtr, A).express(D)\n", "w_D_3 = D.ang_vel_in(A).diff(u_geaz, A).express(D)\n", "w_D_1, w_D_2, w_D_3" ] }, { "cell_type": "code", "execution_count": 130, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( (u_{drtr} + u_{geaz})\\mathbf{\\hat{c}_x} + u_{yaw}\\mathbf{\\hat{a}_y}, \\ \\sin{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_x} + \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_y}, \\ \\mathbf{\\hat{c}_x}, \\ \\mathbf{\\hat{c}_x}\\right)$" ], "text/plain": [ "((u_drtr + u_geaz) c_x + u_yaw a_y, sin(θₜᵢₗₜ) c_x + cos(θₜᵢₗₜ) c_y, c_x, c_x)" ] }, "execution_count": 130, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# 风轮 G = E^w_r^L\n", "w_C_1 = G.ang_vel_in(A).diff(u_yaw, A).express(C)\n", "w_C_2 = G.ang_vel_in(A).diff(u_drtr, A).express(C)\n", "w_C_3 = G.ang_vel_in(A).diff(u_geaz, A).express(C)\n", "G.ang_vel_in(A), w_C_1, w_C_2, w_C_3" ] }, { "cell_type": "code", "execution_count": 131, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( Nx\\mathbf{\\hat{d}_x} + Ny\\mathbf{\\hat{d}_y} + Nz\\mathbf{\\hat{d}_z}, \\ Nz\\mathbf{\\hat{d}_x} - Nx\\mathbf{\\hat{d}_z}, \\ Nz\\mathbf{\\hat{d}_x} - Nx\\mathbf{\\hat{d}_z}, \\ 0, \\ 0\\right)$" ], "text/plain": [ "(Nx d_x + Ny d_y + Nz d_z, Nz d_x + -Nx d_z, Nz d_x + -Nx d_z, 0, 0)" ] }, "execution_count": 131, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# U点\n", "v_U_1 = U.vel(A).diff(u_yaw, A).express(D)\n", "v_U_2 = U.vel(A).diff(u_drtr, A).express(D)\n", "v_U_3 = U.vel(A).diff(u_geaz, A).express(D)\n", "U.pos_from(O).express(D), w_D_1.cross(U.pos_from(O).express(D)), v_U_1, v_U_2, v_U_3" ] }, { "cell_type": "code", "execution_count": 132, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( I_{x}\\mathbf{\\hat{d}_x} + I_{y}\\mathbf{\\hat{d}_y} + I_{z}\\mathbf{\\hat{d}_z}, \\ I_{z}\\mathbf{\\hat{d}_x} - I_{x}\\mathbf{\\hat{d}_z}, \\ I_{z}\\mathbf{\\hat{d}_x} - I_{x}\\mathbf{\\hat{d}_z}, \\ 0, \\ 0\\right)$" ], "text/plain": [ "(Iₓ d_x + I_y d_y + I_z d_z, I_z d_x + -Iₓ d_z, I_z d_x + -Iₓ d_z, 0, 0)" ] }, "execution_count": 132, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# IMU点\n", "v_I_1 = IMU.vel(A).diff(u_yaw, A).express(D)\n", "v_I_2 = IMU.vel(A).diff(u_drtr, A).express(D)\n", "v_I_3 = IMU.vel(A).diff(u_geaz, A).express(D)\n", "IMU.pos_from(O).express(D), w_D_1.cross(IMU.pos_from(O).express(D)), v_I_1, v_I_2, v_I_3" ] }, { "cell_type": "code", "execution_count": 133, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{d}_x} + (L_{overhang} \\sin{\\left(\\theta_{tilt} \\right)} + L_{t2s})\\mathbf{\\hat{d}_y}, \\ - L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{d}_z}, \\ - L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{d}_z}, \\ 0, \\ 0\\right)$" ], "text/plain": [ "(L_overhang⋅cos(θₜᵢₗₜ) d_x + (L_overhang⋅sin(θₜᵢₗₜ) + Lₜ₂ₛ) d_y, -L_overhang⋅c ↪\n", "\n", "↪ os(θₜᵢₗₜ) d_z, -L_overhang⋅cos(θₜᵢₗₜ) d_z, 0, 0)" ] }, "execution_count": 133, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# P点\n", "v_P_1 = P.vel(A).diff(u_yaw, A).express(D)\n", "v_P_2 = P.vel(A).diff(u_drtr, A).express(D)\n", "v_P_3 = P.vel(A).diff(u_geaz, A).express(D)\n", "P.pos_from(O).express(D), w_D_1.cross(P.pos_from(O).express(D)), v_P_1, v_P_2, v_P_3" ] } ], "metadata": { "kernelspec": { "display_name": "MinerU", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.10.16" } }, "nbformat": 4, "nbformat_minor": 2 }