线性化 Carvallo-Whipple 自行车模型¶
自行车是一个有趣的系统，因为它可以用多个刚体、非完整约束和完整约束来建模。Carvallo-Whipple自行车模型的线性化运动方程在[Meijaard2007]_中提出并进行了基准测试。本示例将使用 sympy.physics.mechanics 构建相同的线性运动方程。:
>>> import sympy as sm
>>> import sympy.physics.mechanics as me
>>> me.mechanics_printing(pretty_print=False)
坐标与速度的声明¶
在该模型中，使用了 \(\mathbf{u} = \dot{\mathbf{q}}\) 的简单定义。广义速度为：
偏航框架角速度 \(u_1\),
滚转帧角速度 \(u_2\),
后轮框架角速率（旋转运动） \(u_3\),
框架角速度（俯仰运动） \(u_4\),
转向架角速度 \(u_5\)，以及
前轮角速度（旋转运动） \(u_6\)。
轮子位置是可忽略的坐标，因此它们没有被引入。
>>> q1, q2, q3, q4, q5 = me.dynamicsymbols('q1 q2 q3 q4 q5')
>>> q1d, q2d, q4d, q5d = me.dynamicsymbols('q1 q2 q4 q5', 1)
>>> u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1 u2 u3 u4 u5 u6')
>>> u1d, u2d, u3d, u4d, u5d, u6d = me.dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
系统参数的声明¶
模型的常量参数是:
>>> WFrad, WRrad, htangle, forkoffset = sm.symbols('WFrad WRrad htangle forkoffset')
>>> forklength, framelength, forkcg1 = sm.symbols('forklength framelength forkcg1')
>>> forkcg3, framecg1, framecg3, Iwr11 = sm.symbols('forkcg3 framecg1 framecg3 Iwr11')
>>> Iwr22, Iwf11, Iwf22, Iframe11 = sm.symbols('Iwr22 Iwf11 Iwf22 Iframe11')
>>> Iframe22, Iframe33, Iframe31, Ifork11 = sm.symbols('Iframe22 Iframe33 Iframe31 Ifork11')
>>> Ifork22, Ifork33, Ifork31, g = sm.symbols('Ifork22 Ifork33 Ifork31 g')
>>> mframe, mfork, mwf, mwr = sm.symbols('mframe mfork mwf mwr')
自行车的运动学¶
为系统设置参考框架¶
N- 惯性Y- 偏航R- 滚动WR- 后轮，旋转角度是可忽略的坐标，因此不定向Frame- 自行车车架TempFrame- 静态旋转框架，便于参考惯性定义Fork- 自行车前叉TempFork- 静态旋转的参考系，便于定义惯性参考WF- 前轮，再次拥有一个可忽略的坐标
>>> N = me.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 = me.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 = me.ReferenceFrame('WF')
定义系统的相关要点¶
WR_cont - 后轮接触点 WR_mc - 后轮质心 Steer - 车架/前叉连接点 Frame_mc - 车架质心 Fork_mc - 前叉质心 WF_mc - 前轮质心 WF_cont - 前轮接触点
>>> WR_cont = me.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*(me.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
运动学微分方程如下。此列表中的每一项都等于零。
>>> kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
设置约束¶
非完整约束是前轮接触点的速度在X、Y和Z方向上的投影；使用偏航框架，因为它“更接近”前轮（连接它们的余弦矩阵少一个方向）。这些约束强制前轮接触点在惯性框架中的速度为零；X和Y方向的约束强制执行“无滑移”条件，而Z方向的约束强制前轮接触点不从地面框架移动，本质上复制了不允许框架俯仰以无效方式变化的完整约束。
>>> conlist_speed = [me.dot(WF_cont.vel(N), Y.x),
... me.dot(WF_cont.vel(N), Y.y),
... me.dot(WF_cont.vel(N), Y.z)]
全向约束是指从后轮接触点到前轮接触点的位置在地面法线方向上的点积必须为零；实际上，这意味着前后轮接触点始终接触地面平面。这实际上不是动力学微分方程的一部分，但对于线性化过程是必要的。
>>> conlist_coord = [me.dot(WF_cont.pos_from(WR_cont), Y.z)]
惯性和刚体¶
设置每个物体的惯性。使用惯性框架来构建惯性二重体。车轮的惯性仅由主惯性矩定义，并且在框架和叉子的参考框架中实际上是恒定的；正是由于这个原因，车轮的方向不需要定义。框架和叉子的惯性在固定到适当物体框架的’Temp’框架中定义；这是为了更容易输入基准论文的参考值。请注意，由于方向略有不同，惯性积需要翻转其符号；这在输入数值时稍后完成。:
>>> Frame_I = (me.inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0,
... Iframe31), Frame_mc)
>>> Fork_I = (me.inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
>>> WR_I = (me.inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
>>> WF_I = (me.inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
RigidBody 容器的声明。
>>> BodyFrame = me.RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
>>> BodyFork = me.RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
>>> BodyWR = me.RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
>>> BodyWF = me.RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
>>> bodies = [BodyFrame, BodyFork, BodyWR, BodyWF]
重力载荷¶
力列表；每个物体在其质心处施加适当的引力。:
>>> forces = [(Frame_mc, -mframe * g * Y.z),
... (Fork_mc, -mfork * g * Y.z),
... (WF_mc, -mwf * g * Y.z),
... (WR_mc, -mwr * g * Y.z)]
...
非线性运动方程¶
N 框架是惯性框架，坐标按独立、从属坐标的顺序提供。运动学微分方程也在这里输入。这里指定了独立速度，接着是从属速度，以及非完整约束。还提供了从属坐标，带有完整约束。同样，这仅在线性化过程中起作用，但对于线性化正确工作是必要的。:
>>> kane = me.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,
... constraint_solver='CRAMER')
>>> fr, frstar = kane.kanes_equations(bodies, loads=forces)
线性化运动方程¶
这是从基准论文中输入数值的开始，以验证该模型线性化方程的特征值与参考特征值的一致性。更多信息请参阅上述论文。其中一些是中间值，用于将论文中的数值转换为该模型中使用的坐标系。:
>>> PaperRadRear = 0.3
>>> PaperRadFront = 0.35
>>> HTA = sm.evalf.N(sm.pi/2 - sm.pi/10)
>>> TrailPaper = 0.08
>>> rake = sm.evalf.N(-(TrailPaper*sm.sin(HTA) - (PaperRadFront*sm.cos(HTA))))
>>> PaperWb = 1.02
>>> PaperFrameCgX = 0.3
>>> PaperFrameCgZ = 0.9
>>> PaperForkCgX = 0.9
>>> PaperForkCgZ = 0.7
>>> FrameLength = sm.evalf.N(PaperWb*sm.sin(HTA) - (rake -
... (PaperRadFront - PaperRadRear)*sm.cos(HTA)))
>>> FrameCGNorm = sm.evalf.N((PaperFrameCgZ - PaperRadRear -
... (PaperFrameCgX/sm.sin(HTA))*sm.cos(HTA))*sm.sin(HTA))
>>> FrameCGPar = sm.evalf.N((PaperFrameCgX / sm.sin(HTA) +
... (PaperFrameCgZ - PaperRadRear -
... PaperFrameCgX / sm.sin(HTA)*sm.cos(HTA))*sm.cos(HTA)))
>>> tempa = sm.evalf.N((PaperForkCgZ - PaperRadFront))
>>> tempb = sm.evalf.N((PaperWb-PaperForkCgX))
>>> tempc = sm.evalf.N(sm.sqrt(tempa**2 + tempb**2))
>>> PaperForkL = sm.evalf.N((PaperWb*sm.cos(HTA) -
... (PaperRadFront - PaperRadRear)*sm.sin(HTA)))
>>> ForkCGNorm = sm.evalf.N(rake + (tempc*sm.sin(sm.pi/2 -
... HTA - sm.acos(tempa/tempc))))
>>> ForkCGPar = sm.evalf.N(tempc*sm.cos((sm.pi/2 - HTA) -
... sm.acos(tempa/tempc)) - PaperForkL)
以下是数值的最终组合。符号 ‘v’ 是自行车的前进速度（这个概念仅在直立、静态平衡的情况下才有意义？）。这些数值在一个字典中，稍后将被替换。由于坐标系方向的不同，惯性积的符号在这里再次被翻转。:
>>> v = sm.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,
... }
...
在线性化平衡点附近的运动方程:
>>> eq_point = {
... u1d: 0,
... u2d: 0,
... u3d: 0,
... u4d: 0,
... u5d: 0,
... u6d: 0,
... q1: 0,
... q2: 0,
... q4: 0,
... q5: 0,
... u1: 0,
... u2: 0,
... u3: v/PaperRadRear,
... u4: 0,
... u5: 0,
... u6: v/PaperRadFront,
... }
...
>>> Amat, _, _ = kane.linearize(A_and_B=True, op_point=eq_point, linear_solver='CRAMER')
>>> Amat = me.msubs(Amat, val_dict)
计算特征值¶
最后，我们为形式 \(\dot{\mathbf{x}} = \mathbf{A} \mathbf{x}\) 构建一个“A”矩阵（\(\mathbf{x}\) 是状态向量，尽管在这种情况下，尺寸有些不匹配）。以下行仅提取了特征值分析所需的最小条目，这些条目对应于倾斜、转向、倾斜率和转向率的行和列。
>>> A = Amat.extract([1, 2, 3, 5], [1, 2, 3, 5])
>>> A
Matrix([
[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
[9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
[11.7194768719633, 30.9087533932407 - 1.97171508499972*v**2, 3.67680523332152*v, -3.08486552743311*v]])
>>> print('v = 1')
v = 1
>>> print(A.subs(v, 1).eigenvals())
{-3.13423125066578 - 1.05503732448615e-65*I: 1, 3.52696170990069 - 0.807740275199311*I: 1, 3.52696170990069 + 0.807740275199311*I: 1, -7.11008014637441: 1}
>>> print('v = 2')
v = 2
>>> print(A.subs(v, 2).eigenvals())
{2.68234517512745 - 1.68066296590676*I: 1, 2.68234517512745 + 1.68066296590676*I: 1, -3.07158645641514: 1, -8.67387984831737: 1}
>>> print('v = 3')
v = 3
>>> print(A.subs(v, 3).eigenvals())
{1.70675605663973 - 2.31582447384324*I: 1, 1.70675605663973 + 2.31582447384324*I: 1, -2.63366137253665: 1, -10.3510146724592: 1}
>>> print('v = 4')
v = 4
>>> print(A.subs(v, 4).eigenvals())
{0.413253315211239 - 3.07910818603205*I: 1, 0.413253315211239 + 3.07910818603205*I: 1, -1.42944427361326 + 1.65070329233125e-64*I: 1, -12.1586142657644: 1}
>>> print('v = 5')
v = 5
>>> print(A.subs(v, 5).eigenvals())
{-0.775341882195845 - 4.46486771378823*I: 1, -0.322866429004087 + 3.32140410564766e-64*I: 1, -0.775341882195845 + 4.46486771378823*I: 1, -14.0783896927982: 1}
上述特征值与 [Meijaard2007] 第1971页表2中的特征值一致。至此，自行车示例结束。
参考文献¶
[Meijaard2007]
Meijaard, J. P., Papadopoulos, J. M., Ruina, A., & Schwab, A. L. (2007). 自行车平衡与转向的线性化动力学方程：基准与回顾。《皇家学会学报A：数学、物理与工程科学》，463(2084)，1955–1982。https://doi.org/10.1098/rspa.2007.1857