自行车#
自行车是一个有趣的系统，因为它有多个刚体、非完整约束和一个完整约束。线性化运动方程如 [Meijaard2007]. 这个例子将在 sympy.physics.mechanics . ：：
>>> from sympy import *
>>> from sympy.physics.mechanics import *
>>> print('Calculation of Linearized Bicycle \"A\" Matrix, '
... 'with States: Roll, Steer, Roll Rate, Steer Rate')
Calculation of Linearized Bicycle "A" Matrix, with States: Roll, Steer, Roll Rate, Steer Rate
Note that this code has been crudely ported from Autolev, which is the reason for some of the unusual naming conventions. It was purposefully as similar as possible in order to aid initial porting & debugging.:
>>> mechanics_printing(pretty_print=False)
坐标和速度的声明：本代码使用qdots的一个简单定义qd=u。速度为：偏航框架和。速率，辊架角度。后轮架ang。速率（旋转运动），帧ang。速率（俯仰运动），转向架和。速度，前轮角度。速率（旋转运动）。车轮位置是可忽略的坐标，因此不引入。:
>>> q1, q2, q3, q4, q5 = dynamicsymbols('q1 q2 q3 q4 q5')
>>> q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
>>> u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
>>> u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
系统参数的声明：下面的符号应该是不言而喻的。:
>>> WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
>>> forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
>>> forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
>>> Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
>>> Iframe22, Iframe33, Iframe31, Ifork11 = \
... symbols('Iframe22 Iframe33 Iframe31 Ifork11')
>>> Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
>>> mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
为系统设置参考系：N-惯性Y-偏航R-横滚WR-后轮，旋转角度是可忽略的坐标，因此不定向框架-自行车车架TempFrame-静态旋转框架更容易参考惯性定义叉子-自行车叉TempFork-静态旋转框架更容易参考惯性定义WF-前轮，同样拥有一个可忽略坐标：
>>> N = ReferenceFrame('N')
>>> Y = N.orientnew('Y', 'Axis', [q1, N.z])
>>> R = Y.orientnew('R', 'Axis', [q2, Y.x])
>>> Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
>>> WR = ReferenceFrame('WR')
>>> TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
>>> Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
>>> TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
>>> WF = ReferenceFrame('WF')
前后轮质量中心点->后车轮质量中心点->后车轮质量中心点位置->后车轮质量中心点。:
>>> WR_cont = Point('WR_cont')
>>> WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
>>> Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
>>> Frame_mc = WR_mc.locatenew('Frame_mc', -framecg1 * Frame.x + framecg3 * Frame.z)
>>> Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z)
>>> WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
>>> WF_cont = WF_mc.locatenew('WF_cont', WFrad*(dot(Fork.y, Y.z)*Fork.y - \
... Y.z).normalize())
设置每一帧的角速度：角加速度最终是自动计算的微分角速度时，首次需要。：u1是横摆率u2是横摆率u3是后轮率u4是车架俯仰率u5是叉式转向率u6是前轮率：
>>> Y.set_ang_vel(N, u1 * Y.z)
>>> R.set_ang_vel(Y, u2 * R.x)
>>> WR.set_ang_vel(Frame, u3 * Frame.y)
>>> Frame.set_ang_vel(R, u4 * Frame.y)
>>> Fork.set_ang_vel(Frame, u5 * Fork.x)
>>> WF.set_ang_vel(Fork, u6 * Fork.y)
用两点定理求出点的速度。当第一次需要时，会自动计算加速度。:
>>> WR_cont.set_vel(N, 0)
>>> WR_mc.v2pt_theory(WR_cont, N, WR)
WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y
>>> Steer.v2pt_theory(WR_mc, N, Frame)
WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y + framelength*(u1*sin(q2) + u4)*Frame.x - framelength*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4))*Frame.y
>>> Frame_mc.v2pt_theory(WR_mc, N, Frame)
WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y + framecg3*(u1*sin(q2) + u4)*Frame.x + (-framecg1*(u1*cos(htangle + q4)*cos(q2) + u2*sin(htangle + q4)) - framecg3*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4)))*Frame.y + framecg1*(u1*sin(q2) + u4)*Frame.z
>>> Fork_mc.v2pt_theory(Steer, N, Fork)
WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y + framelength*(u1*sin(q2) + u4)*Frame.x - framelength*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4))*Frame.y + forkcg3*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5))*Fork.x + (-forkcg1*((-sin(q2)*sin(q5) + cos(htangle + q4)*cos(q2)*cos(q5))*u1 + u2*sin(htangle + q4)*cos(q5) - u4*sin(q5)) - forkcg3*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4) + u5))*Fork.y + forkcg1*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5))*Fork.z
>>> WF_mc.v2pt_theory(Steer, N, Fork)
WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y + framelength*(u1*sin(q2) + u4)*Frame.x - framelength*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4))*Frame.y + forkoffset*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5))*Fork.x + (forklength*((-sin(q2)*sin(q5) + cos(htangle + q4)*cos(q2)*cos(q5))*u1 + u2*sin(htangle + q4)*cos(q5) - u4*sin(q5)) - forkoffset*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4) + u5))*Fork.y - forklength*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5))*Fork.z
>>> WF_cont.v2pt_theory(WF_mc, N, WF)
- WFrad*((-sin(q2)*sin(q5)*cos(htangle + q4) + cos(q2)*cos(q5))*u6 + u4*cos(q2) + u5*sin(htangle + q4)*sin(q2))/sqrt((-sin(q2)*cos(q5) - sin(q5)*cos(htangle + q4)*cos(q2))*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2)) + 1)*Y.x + WFrad*(u2 + u5*cos(htangle + q4) + u6*sin(htangle + q4)*sin(q5))/sqrt((-sin(q2)*cos(q5) - sin(q5)*cos(htangle + q4)*cos(q2))*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2)) + 1)*Y.y + WRrad*(u1*sin(q2) + u3 + u4)*R.x - WRrad*u2*R.y + framelength*(u1*sin(q2) + u4)*Frame.x - framelength*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4))*Frame.y + (-WFrad*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*((-sin(q2)*sin(q5) + cos(htangle + q4)*cos(q2)*cos(q5))*u1 + u2*sin(htangle + q4)*cos(q5) - u4*sin(q5))/sqrt((-sin(q2)*cos(q5) - sin(q5)*cos(htangle + q4)*cos(q2))*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2)) + 1) + forkoffset*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5)))*Fork.x + (forklength*((-sin(q2)*sin(q5) + cos(htangle + q4)*cos(q2)*cos(q5))*u1 + u2*sin(htangle + q4)*cos(q5) - u4*sin(q5)) - forkoffset*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4) + u5))*Fork.y + (WFrad*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*(-u1*sin(htangle + q4)*cos(q2) + u2*cos(htangle + q4) + u5)/sqrt((-sin(q2)*cos(q5) - sin(q5)*cos(htangle + q4)*cos(q2))*(sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2)) + 1) - forklength*((sin(q2)*cos(q5) + sin(q5)*cos(htangle + q4)*cos(q2))*u1 + u2*sin(htangle + q4)*sin(q5) + u4*cos(q5)))*Fork.z
设置每个实体的惯量。利用惯性框架构造惯性并矢。车轮惯量仅由主惯性矩定义，实际上在框架和叉形参考系中是恒定的；因此，不需要定义车轮的方向。框架和叉子惯量在“Temp”框架中定义，这些框架固定在适当的主体框架上；这是为了更容易地输入基准文件的参考值。请注意，由于方向稍有不同，惯性积需要翻转其符号；稍后输入数值时会进行此操作。:
>>> Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0,
... Iframe31), Frame_mc)
>>> Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
>>> WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
>>> WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
刚体容器声明。:
>>> BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
>>> BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
>>> BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
>>> BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
>>> print('Before Forming the List of Nonholonomic Constraints.')
Before Forming the List of Nonholonomic Constraints.
运动学微分方程；它们的定义相当简单。此列表中的每个条目都等于零。:
>>> kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
非完整约束是前轮接触点在X、Y和Z方向上的速度；使用偏航框架是因为它“更靠近”前轮（连接它们的DCM少1个）。这些约束强制前轮接触点在惯性系中的速度为0；X和Y方向约束强制执行“防滑”条件，Z方向约束强制前轮接触点不离开地面框架，本质上是复制完整约束，不允许帧间距以无效方式改变。:
>>> conlist_speed = [dot(WF_cont.vel(N), Y.x),
... dot(WF_cont.vel(N), Y.y),
... dot(WF_cont.vel(N), Y.z)]
完整性约束条件是，从后轮接触点到前轮接触点在垂直于地平面方向上的点的位置必须为零；有效地说，前后车轮接触点始终接触地平面。这实际上不是动力学方程的一部分，而是线性化过程所必需的。:
>>> conlist_coord = [dot(WF_cont.pos_from(WR_cont), Y.z)]
力列表；每个物体的质心都有适当的重力。:
>>> FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z),
... (WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)]
>>> BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
N坐标系是惯性系，坐标系按独立、相依的顺序提供。运动微分方程也在这里输入。这里规定了独立速度，接着是相关速度，以及非完整约束。在完整约束条件下，给出了相关坐标系。同样，这只在线性化过程中发挥作用，但它是线性化正确工作所必需的。:
>>> KM = KanesMethod(N, q_ind=[q1, q2, q5],
... q_dependent=[q4], configuration_constraints=conlist_coord,
... u_ind=[u2, u3, u5],
... u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
... kd_eqs=kd)
>>> print('Before Forming Generalized Active and Inertia Forces, Fr and Fr*')
Before Forming Generalized Active and Inertia Forces, Fr and Fr*
>>> (fr, frstar) = KM.kanes_equations(BL, FL)
>>> print('Base Equations of Motion Computed')
Base Equations of Motion Computed
这是开始输入基准文件中的数值，以验证从该模型线性化方程的特征值到参考特征值。更多信息请参阅上述文件。其中一些是中间值，用于将纸张中的值转换为模型中使用的坐标系。:
>>> PaperRadRear = 0.3
>>> PaperRadFront = 0.35
>>> HTA = evalf.N(pi/2-pi/10)
>>> TrailPaper = 0.08
>>> rake = evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
>>> PaperWb = 1.02
>>> PaperFrameCgX = 0.3
>>> PaperFrameCgZ = 0.9
>>> PaperForkCgX = 0.9
>>> PaperForkCgZ = 0.7
>>> FrameLength = evalf.N(PaperWb*sin(HTA) - (rake - \
... (PaperRadFront - PaperRadRear)*cos(HTA)))
>>> FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear - \
... (PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
>>> FrameCGPar = evalf.N((PaperFrameCgX / sin(HTA) + \
... (PaperFrameCgZ - PaperRadRear - \
... PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
>>> tempa = evalf.N((PaperForkCgZ - PaperRadFront))
>>> tempb = evalf.N((PaperWb-PaperForkCgX))
>>> tempc = evalf.N(sqrt(tempa**2 + tempb**2))
>>> PaperForkL = evalf.N((PaperWb*cos(HTA) - \
... (PaperRadFront - PaperRadRear)*sin(HTA)))
>>> ForkCGNorm = evalf.N(rake + (tempc * sin(pi/2 - \
... HTA - acos(tempa/tempc))))
>>> ForkCGPar = evalf.N(tempc * cos((pi/2 - HTA) - \
... acos(tempa/tempc)) - PaperForkL)
这是数值的最终组合。符号“v”是自行车的前进速度（这个概念只有在直立、静态平衡的情况下才有意义？）。这些都在一本词典里，以后会被取代。又是上面的标志 产品 由于坐标系的不同方向，惯性值在这里翻转。:
>>> v = Symbol('v')
>>> val_dict = {
... WFrad: PaperRadFront,
... WRrad: PaperRadRear,
... htangle: HTA,
... forkoffset: rake,
... forklength: PaperForkL,
... framelength: FrameLength,
... forkcg1: ForkCGPar,
... forkcg3: ForkCGNorm,
... framecg1: FrameCGNorm,
... framecg3: FrameCGPar,
... Iwr11: 0.0603,
... Iwr22: 0.12,
... Iwf11: 0.1405,
... Iwf22: 0.28,
... Ifork11: 0.05892,
... Ifork22: 0.06,
... Ifork33: 0.00708,
... Ifork31: 0.00756,
... Iframe11: 9.2,
... Iframe22: 11,
... Iframe33: 2.8,
... Iframe31: -2.4,
... mfork: 4,
... mframe: 85,
... mwf: 3,
... mwr: 2,
... g: 9.81,
... q1: 0,
... q2: 0,
... q4: 0,
... q5: 0,
... u1: 0,
... u2: 0,
... u3: v/PaperRadRear,
... u4: 0,
... u5: 0,
... u6: v/PaperRadFront}
>>> kdd = KM.kindiffdict()
>>> print('Before Linearization of the \"Forcing\" Term')
Before Linearization of the "Forcing" Term
将强迫向量线性化；将方程设置为MM udot=强迫，其中MM是质量矩阵，udot是表示广义速度的时间导数的向量，而forcing是包含外部强迫项和内部强迫项的向量，例如向心力或科里奥利力。这实际上返回一个矩阵，其行数为 全部的 坐标和速度，但只有独立坐标和速度一样多的列。（请注意，下面的注释已被注释掉，因为它需要几分钟才能运行，这在执行doctest时是不好的）：
>>> # forcing_lin = KM.linearize()[0].subs(sub_dict)
如前所述，线性化强迫项的大小被扩展到包括q和u，所以质量矩阵也必须这样做。这可能会被改变为线性化过程的一部分，以供将来参考。:
>>> MM_full = (KM._k_kqdot).row_join(zeros(4, 6)).col_join(
... (zeros(6, 4)).row_join(KM.mass_matrix))
>>> print('Before Substitution of Numerical Values')
Before Substitution of Numerical Values
我认为这是不言自明的。但这需要很长时间。我尝试过使用带替换的evalf，但由于超过了最大递归深度而失败；我也尝试对其进行lambdizing，但也没有成功。（再次因速度而被评论）：
>>> # MM_full = MM_full.subs(val_dict)
>>> # forcing_lin = forcing_lin.subs(val_dict)
>>> # print('Before .evalf() call')
>>> # MM_full = MM_full.evalf()
>>> # forcing_lin = forcing_lin.evalf()
最后，我们构造一个xdot=ax的“A”矩阵（x是状态向量，尽管在本例中，大小有点偏离）。下一行只提取特征值分析所需的最小条目，这些条目对应于lean、steer、lean rate和steer rate的行和列。（由于依赖于上面的代码，这一切都被注释掉了，它也被注释掉了）：：
>>> # Amat = MM_full.inv() * forcing_lin
>>> # A = Amat.extract([1,2,4,6],[1,2,3,5])
>>> # print(A)
>>> # print('v = 1')
>>> # print(A.subs(v, 1).eigenvals())
>>> # print('v = 2')
>>> # print(A.subs(v, 2).eigenvals())
>>> # print('v = 3')
>>> # print(A.subs(v, 3).eigenvals())
>>> # print('v = 4')
>>> # print(A.subs(v, 4).eigenvals())
>>> # print('v = 5')
>>> # print(A.subs(v, 5).eigenvals())
在自己运行上述代码后，启用注释掉的行，将计算出的特征值与参考文献中的特征值进行比较。自行车示例到此结束。