最近在研究多连杆的机器人建模,发现许多使用牛顿欧拉法的建模方式,都直接采用了如下的一条公式:
(0) F e x t j = M j V j ˙ + β j F_{extj}=M_j \dot{V_j}+\beta_j \tag{0} Fextj=MjVj˙+βj(0)
其中:
V j = [ U j Ω j ] , 这 是 连 杆 j 相 对 于 惯 性 系 的 速 度 和 角 速 度 组 成 的 六 维 向 量 ( 表 示 在 j 系 下 ) 。 V_j=\left[ \begin{matrix} U_j\\ \Omega_j \end{matrix} \right],这是连杆j相对于惯性系的速度和角速度组成的六维向量(表示在j系下)。 Vj=[UjΩj],这是连杆j相对于惯性系的速度和角速度组成的六维向量(表示在j系下)。
j H j + 1 , 代 表 的 就 是 一 个 6 维 的 变 换 矩 阵 , 将 j + 1 系 下 的 力 变 换 到 j 系 下 。 {\vphantom{}}^{j}H_{j+1}, 代表的就是一个6维的变换矩阵,将{j+1}系下的力变换到{j}系下。 jHj+1,代表的就是一个6维的变换矩阵,将j+1系下的力变换到j系下。
F e x t j , 连 杆 受 到 的 外 力 、 外 力 矩 在 j 系 下 的 表 示 。 F_{extj},连杆受到的外力、外力矩在j系下的表示。 Fextj,连杆受到的外力、外力矩在j系下的表示。
M j = [ m I 3 × 3 − m r j ^ m r j ^ I j ] M_j=\left[ \begin{matrix} m I_{3\times3} & -m\hat{r_j} \\ m\hat{r_j} & I_j\\ \end{matrix} \right] Mj=[mI3×3mrj^−mrj^Ij]
β j = [ m Ω j × U j + m Ω j × ( Ω j × r j ) m r j × ( Ω j × U j ) + Ω j × I j Ω j ] \beta_j=\left[ \begin{matrix} m\Omega_j \times U_j + m\Omega_j \times (\Omega_j \times r_j)\\ m r_j \times (\Omega_j \times U_j) + \Omega_j \times I_j \Omega_j \end{matrix} \right] βj=[mΩj×Uj+mΩj×(Ωj×rj)mrj×(Ωj×Uj)+Ωj×IjΩj]
x ^ = [ 0 − x 3 x 2 x 3 0 − x 1 − x 2 x 1 0 ] , x ^ p = x × p \hat{x}=\left[ \begin{matrix} 0 &-x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{matrix} \right] ,\hat{x}p=x\times p x^=⎣⎡0x3−x2−x30x1x2−x10⎦⎤,x^p=x×p
这个公式的形式很漂亮,用起来也比较方便。但是,如果从牛顿-欧拉方程来看,很难直接推出这个公式。看了很多文献,也没有找到推导过程,于是索性自己推导了一番。
图中蓝色的物体表示一个刚体,我们选择刚体上的某一点,定义了一个与刚体固连的坐标系B,即本体系。
我们假设刚体可被视作为很多微元的组合,并且假设刚体是均匀的。
最终,我们希望得到外力与刚体相对惯性系的速度和加速度的关系式。
T T T:代表刚体的动能
P b P_b Pb:代表刚体的动量,在{B}系下的表示
Π b \Pi_b Πb:代表刚体的角动量,在{B}系下的表示
U b U_b Ub:代表刚体相对于惯性系的速度,在{B}系下的表示
U s U_s Us:代表刚体相对于惯性系的速度,在{S}系下的表示
V i b V_{ib} Vib:代表刚体微元 i 相对于惯性系的速度,在{B}系下的表示
Ω b \Omega_b Ωb:代表刚体相对于惯性系的角速度,在{B}系下的表示
Ω s \Omega_s Ωs:代表刚体相对于惯性系的角速度,在{S}系下的表示
p b p_b pb:代表p向量在{B}系下的表示
p s p_s ps:代表p向量在{S}系下的表示
r c b r_{cb} rcb:代表 r c r_c rc向量在{B}系下的表示
r i b r_{ib} rib:代表 r i r_i ri向量在{B}系下的表示
m m m:刚体质量
I b I_b Ib:刚体的惯量矩阵,相对于{B}系的
F e x t b F_{extb} Fextb:刚体所受合外力在{B}系的表示
M e x t b M_{extb} Mextb:刚体所受合外力矩在{B}系的表示
第一步,计算刚体的动能。
动能是个标量,在任何坐标系下表示都相等,但这个动能必须是相对惯性系而言的。
也就是说,速度和角速度必须是相对与惯性系的。
第二步,动能对速度、角速度求导,得到动量、角动量。
速度可以是表示在{B}系下的,也可以是{S}系下的。如果是相对于表示在{B}系下的速度求导,那么求出来的动量也是表示在{B}系下的。反之亦然。
第三步,动量和角动量对时间求导,得到外力。
这一步的求导需要格外注意。我们需要在惯性系下对时间求导,这样出来的才会是外力。牛顿定律只在惯性系下有效。
如何在惯性系下求导,参考博文[关于机器人运动学与动力学建模的几点领悟](https://blog.csdn.net/handsome_for_kill/article/details/96473701)
链接:关于机器人运动学与动力学建模的几点领悟
刚体的动能会等于所有刚体微元的动能之和。
即
(1) T = ∫ 1 2 V i b 2 d m T=\int \frac{1}{2}V_{ib}^2\ dm \tag{1} T=∫21Vib2 dm(1)
我们知道, V i b V_{ib} Vib可以被表示为:
(2) V i b = U b + Ω b × r i b V_{ib}=U_b+\Omega_b \times r_{ib} \tag{2} Vib=Ub+Ωb×rib(2)
所以,我们把公式(2)代入公式(1),可以得到:
(3) T = ∫ 1 2 V i b 2 d m = ∫ 1 2 ( U b + Ω b × r i b ) ⋅ ( U b + Ω b × r i b ) d m = ∫ 1 2 ( U b ⋅ U b + 2 U b ⋅ Ω b × r i b + Ω b × r i b ⋅ Ω b × r i b ) d m = ∫ 1 2 U b 2 d v + ∫ U b ⋅ Ω b × r i b d m + ∫ 1 2 Ω b × r i b ⋅ Ω b × r i b d m = 1 2 m U b 2 + U b ⋅ Ω b × ∫ r i b d m + 1 2 Ω b ∫ r i b × Ω b × r i b d m = 1 2 m U b 2 + m U b ⋅ Ω b × r c b + 1 2 Ω b I b Ω b = 1 2 m U b 2 + m U b ⋅ Ω b × r c b + 1 2 I b Ω b 2 \begin{aligned} T &= \int \frac{1}{2} V_{ib}^2 dm \\ &= \int \frac{1}{2} (U_b+\Omega_b \times r_{ib})\cdot(U_b+\Omega_b \times r_{ib}) dm \\ &= \int \frac{1}{2} (U_b \cdot U_b+2U_b\cdot\Omega_b \times r_{ib}+ \Omega_b \times r_{ib}\cdot\Omega_b \times r_{ib}) dm \\ &= \int \frac{1}{2} U_b^2 dv + \int U_b\cdot\Omega_b \times r_{ib} dm + \int \frac{1}{2} \Omega_b \times r_{ib}\cdot\Omega_b \times r_{ib} dm \\ &=\frac{1}{2} m U_b^2 +U_b\cdot\Omega_b \times \int r_{ib} dm+\frac{1}{2} \Omega_b \int r_{ib} \times \Omega_b \times r_{ib} dm \\ &=\frac{1}{2} m U_b^2 +mU_b\cdot\Omega_b \times r_{cb}+\frac{1}{2} \Omega_b I_b \Omega_b\\ &=\frac{1}{2} m U_b^2 +mU_b\cdot\Omega_b \times r_{cb}+\frac{1}{2} I_b \Omega_b^2\\ \end{aligned} \tag{3} T=∫21Vib2dm=∫21(Ub+Ωb×rib)⋅(Ub+Ωb×rib)dm=∫21(Ub⋅Ub+2Ub⋅Ωb×rib+Ωb×rib⋅Ωb×rib)dm=∫21Ub2dv+∫Ub⋅Ωb×ribdm+∫21Ωb×rib⋅Ωb×ribdm=21mUb2+Ub⋅Ωb×∫ribdm+21Ωb∫rib×Ωb×ribdm=21mUb2+mUb⋅Ωb×rcb+21ΩbIbΩb=21mUb2+mUb⋅Ωb×rcb+21IbΩb2(3)
在这部分的推导中,用到了很多公式:
Ω b × r i b = − r i b × Ω b U b ⋅ Ω b × r i b = U b × Ω b ⋅ r i b m = ∫ 1 d m r c b = ∫ r i b d m ∫ 1 d m ∫ r i b × Ω b × r i b d m = ∫ [ r i b y 2 + r i b z 2 − r i b y r i b x − r i b z r i b x − r i b x r i b y r i b z 2 + r i b x 2 − r i b z r i b y − r i b x r i b z − r i b y r i b z r i b x 2 + r i b y 2 ] d m ⋅ Ω b = I b Ω b \begin{aligned} &\Omega_b \times r_{ib} =-r_{ib} \times \Omega_b \\ &U_b\cdot\Omega_b \times r_{ib} =U_b\times \Omega_b \cdot r_{ib} \\ &m={\int 1 dm} \\ &r_{cb}=\frac{\int r_{ib} dm}{\int 1 dm} \\ & \int r_{ib} \times \Omega_b \times r_{ib} dm=\int \left[ \begin{matrix} r_{iby}^2+r_{ibz}^2 & -r_{iby}r_{ibx}& -r_{ibz}r_{ibx}\\ -r_{ibx}r_{iby}&r_{ibz}^2+r_{ibx}^2 & -r_{ibz}r_{iby}\\ -r_{ibx}r_{ibz}& -r_{iby}r_{ibz} & r_{ibx}^2+r_{iby}^2 \\ \end{matrix} \right] dm \cdot \Omega_b = I_b\Omega_b \end{aligned} Ωb×rib=−rib×ΩbUb⋅Ωb×rib=Ub×Ωb⋅ribm=∫1dmrcb=∫1dm∫ribdm∫rib×Ωb×ribdm=∫⎣⎡riby2+ribz2−ribxriby−ribxribz−ribyribxribz2+ribx2−ribyribz−ribzribx−ribzribyribx2+riby2⎦⎤dm⋅Ωb=IbΩb
最终,我们得到了动能的表达式:
(4) T = 1 2 m U b 2 + m U b ⋅ Ω b × r c b + 1 2 I b Ω b 2 T =\frac{1}{2} m U_b^2 +mU_b\cdot\Omega_b \times r_{cb}+\frac{1}{2} I_b \Omega_b^2 \tag{4} T=21mUb2+mUb⋅Ωb×rcb+21IbΩb2(4)
动能对速度求导,得到动量:
(5) P b = ∂ T ∂ U b = m U b + m Ω b × r c b P_b=\frac{\partial T}{\partial U_b}=mU_b+m\Omega_b \times r_{cb} \tag{5} Pb=∂Ub∂T=mUb+mΩb×rcb(5)
动能对角速度求导,得到角动量:
(6) Π b = ∂ T ∂ Ω b = m r c b × U b + I b Ω b \Pi_b=\frac{\partial T}{\partial \Omega_b}=m r_{cb} \times U_b+I_b\Omega_b \tag{6} Πb=∂Ωb∂T=mrcb×Ub+IbΩb(6)
首先,我们要分析,上一步算出来的刚体动量和动量矩是什么?
动量的定义没有歧义,但是动量矩有。
我们知道,根据动量矩定理,只有刚体对固定点(惯性系原点S)的动量矩对时间求导等于刚体上的合外力对该固定点的合外力矩。即:
d d t Π s = M s \frac{{\rm d}}{{\rm dt}}\Pi_s=M_s dtdΠs=Ms
而我们上面计算的是对动点b的绝对动量矩,它和 M b M_b Mb是什么关系呢?
这篇文章中就不详细推导了,这部分推导可以参见文章:机器人动力学建模之刚体动力学基础学习
这里直接给出结论:
(7) d d t Π b = M e x t b − U b × P b \frac{{\rm d}}{{\rm dt}}\Pi_b=M_{extb}-U_b\times P_b \tag{7} dtdΠb=Mextb−Ub×Pb(7)
接下来,进入正题:
注意,之前提到,这里需要在惯性系下,将动量和角动量对时间求导:
1、动量对时间求导,得到合外力:
(8) F e x t b = d s P b d t = m d s U b d t + m d s ( Ω b × r c b ) d t = m ( U b ˙ + Ω b × U b ) + m Ω ˙ b × r c b + m Ω b × ( Ω b × r c b ) = m U b ˙ + m Ω b × U b + m Ω ˙ b × r c b + m Ω b × ( Ω b × r c b ) \begin{aligned} F_{extb} =\frac{d^s P_b}{dt}&=m\frac{d^s U_b}{dt}+m\frac{d^s(\Omega_b\times r_{cb})}{dt} \\ &=m(\dot{U_b}+\Omega_b \times U_b)+m\dot\Omega_b\times r_{cb}+m\Omega_b \times (\Omega_b\times r_{cb}) \\ &=m\dot{U_b}+m\Omega_b \times U_b+m\dot\Omega_b\times r_{cb}+m\Omega_b \times (\Omega_b\times r_{cb}) \\ \end{aligned} \tag{8} Fextb=dtdsPb=mdtdsUb+mdtds(Ωb×rcb)=m(Ub˙+Ωb×Ub)+mΩ˙b×rcb+mΩb×(Ωb×rcb)=mUb˙+mΩb×Ub+mΩ˙b×rcb+mΩb×(Ωb×rcb)(8)
2、根据公式(7),角动量对时间求导,得到合外力矩:
(9) M e x t b = d s Π b d t + U b × P b = m d s ( r c b × U b ) d t + d s ( I b Ω b ) d t + m U b × ( Ω b × r c b ) = m ( r c b × U ˙ b + Ω b × ( r c b × U b ) ) + I b Ω ˙ b + Ω b × ( I b Ω b ) + m U b × ( Ω b × r c b ) = m r c b × U b ˙ + I b Ω ˙ b + Ω b × ( I b Ω b ) + m [ Ω b × ( r c b × U b ) + U b × ( Ω b × r c b ) ] = m r c b × U b ˙ + I b Ω ˙ b + Ω b × ( I b Ω b ) + m r c b × ( Ω b × U b ) \begin{aligned} M_{extb} =\frac{d^s \Pi_b}{dt}+U_b \times P_b&=m \frac{d^s (r_{cb}\times U_b)}{dt}+ \frac{d^s (I_b\Omega_b)}{dt}+mU_b \times (\Omega_b\times r_{cb}) \\ &=m (r_{cb}\times\dot U_b+\Omega_b\times(r_{cb}\times U_b))+ I_b\dot\Omega_b+\Omega_b\times(I_b\Omega_b)+mU_b \times (\Omega_b\times r_{cb})\\ &=m r_{cb}\times \dot{U_b}+ I_b \dot{\Omega}_b+\Omega_b\times(I_b\Omega_b)+m [\Omega_b\times(r_{cb}\times U_b)+U_b \times (\Omega_b\times r_{cb})]\\ &=m r_{cb}\times \dot{U_b}+ I_b \dot{\Omega}_b+\Omega_b\times(I_b\Omega_b)+m r_{cb}\times (\Omega_b\times U_b)\\ \end{aligned} \tag{9} Mextb=dtdsΠb+Ub×Pb=mdtds(rcb×Ub)+dtds(IbΩb)+mUb×(Ωb×rcb)=m(rcb×U˙b+Ωb×(rcb×Ub))+IbΩ˙b+Ωb×(IbΩb)+mUb×(Ωb×rcb)=mrcb×Ub˙+IbΩ˙b+Ωb×(IbΩb)+m[Ωb×(rcb×Ub)+Ub×(Ωb×rcb)]=mrcb×Ub˙+IbΩ˙b+Ωb×(IbΩb)+mrcb×(Ωb×Ub)(9)
整理一下公式(7)和公式(8),得到:
(10) F e x t b = m U b ˙ + m Ω b × U b + m Ω ˙ b × r c b + m Ω b × ( Ω b × r c b ) M e x t b = m r c b × U b ˙ + I b Ω ˙ b + Ω b × ( I b Ω b ) + m r c b × ( Ω b × U b ) \begin{aligned} &F_{extb}=m\dot{U_b}+m\Omega_b \times U_b+m\dot\Omega_b\times r_{cb}+m\Omega_b \times (\Omega_b\times r_{cb})\\ &M_{extb}=m r_{cb}\times \dot{U_b}+ I_b \dot{\Omega}_b+\Omega_b\times(I_b\Omega_b)+m r_{cb}\times (\Omega_b\times U_b)\\ \end{aligned} \tag{10} Fextb=mUb˙+mΩb×Ub+mΩ˙b×rcb+mΩb×(Ωb×rcb)Mextb=mrcb×Ub˙+IbΩ˙b+Ωb×(IbΩb)+mrcb×(Ωb×Ub)(10)
整理成矩阵形式:
(11) [ F e x t b M e x t b ] = [ m I 3 × 3 − m r ^ c b m r ^ c b I b ] [ U b ˙ Ω ˙ b ] + [ m Ω b × U b + m Ω b × ( Ω b × r c b ) m r c b × ( Ω b × U b ) + Ω b × ( I b Ω b ) ] \left[ \begin{matrix} F_{extb}\\ M_{extb} \end{matrix} \right]= \left[ \begin{matrix} mI_{3\times 3} &-m\hat{r}_{cb}\\ m \hat{r}_{cb} & I_b \end{matrix} \right] \left[ \begin{matrix} \dot{U_b}\\ \dot\Omega_b \end{matrix} \right]+ \left[ \begin{matrix} m{\Omega}_b \times U_b+m\Omega_b \times (\Omega_b\times r_{cb})\\ m r_{cb}\times (\Omega_b\times U_b)+ \Omega_b\times(I_b\Omega_b) \end{matrix} \right] \tag{11} [FextbMextb]=[mI3×3mr^cb−mr^cbIb][Ub˙Ω˙b]+[mΩb×Ub+mΩb×(Ωb×rcb)mrcb×(Ωb×Ub)+Ωb×(IbΩb)](11)
可以看到,这个式子和公式(0)是完全一摸一样的,推到这里我们就得到了答案。
值得注意,这里的推导用到了下面的求导方法,其中 r c b r_{cb} rcb是常数向量。
Ω b × r c b = [ Ω b x , Ω b y , Ω b z ] × [ r x , r y , r z ] = [ Ω b y r z − Ω b z r y , Ω b z r x − Ω b x r z , Ω b x r y − Ω b y r x ] \Omega_b\times r_{cb}=[\Omega_{bx},\Omega_{by},\Omega_{bz}]\times[r_x,r_y,r_z]=[\Omega_{by}r_z-\Omega_{bz}r_y,\Omega_{bz}r_x-\Omega_{bx}r_z,\Omega_{bx}r_y-\Omega_{by}r_x] Ωb×rcb=[Ωbx,Ωby,Ωbz]×[rx,ry,rz]=[Ωbyrz−Ωbzry,Ωbzrx−Ωbxrz,Ωbxry−Ωbyrx]
d s ( Ω b × r c b ) d t = d s d t [ ( Ω b y r z − Ω b z r y ) ⋅ i b + ( Ω b z r x − Ω b x r z ) ⋅ j b + ( Ω b x r y − Ω b y r x ) ⋅ k b ] = d s ( Ω b y r z − Ω b z r y ) d t ⋅ i b + d s ( Ω b z r x − Ω b x r z ) d t ⋅ j b + d s ( Ω b x r y − Ω b y r x ) d t ⋅ k b + ( Ω b y r z − Ω b z r y ) ⋅ d s i b d t + ( Ω b z r x − Ω b x r z ) ⋅ d s j b d t + ( Ω b x r y − Ω b y r x ) ⋅ d s k b d t = ( Ω ˙ b y r z − Ω ˙ b z r y ) ⋅ i b + ( Ω ˙ b z r x − Ω ˙ b x r z ) ⋅ j b + ( Ω ˙ b x r y − Ω ˙ b y r x ) ⋅ k b + Ω b × ( Ω b y r z − Ω b z r y ) ⋅ i b + Ω b × ( Ω b z r x − Ω b x r z ) ⋅ j b + Ω b × ( Ω b x r y − Ω b y r x ) ⋅ k b = Ω ˙ b × r c b + Ω b × ( Ω b × r c b ) \begin{aligned} \frac{d^s(\Omega_b\times r_{cb})}{dt}&=\frac{d^s}{dt}\Big[ (\Omega_{by}r_z-\Omega_{bz}r_y)\cdot i_b + (\Omega_{bz}r_x-\Omega_{bx}r_z) \cdot j_b + (\Omega_{bx}r_y-\Omega_{by}r_x) \cdot k_b \Big] \\ &=\frac{d^s (\Omega_{by}r_z-\Omega_{bz}r_y)}{dt}\cdot i_b+\frac{d^s(\Omega_{bz}r_x-\Omega_{bx}r_z)}{dt} \cdot j_b+\frac{d^s(\Omega_{bx}r_y-\Omega_{by}r_x)}{dt} \cdot k_b \\ &\ \ \ \ \ \ \ +(\Omega_{by}r_z-\Omega_{bz}r_y)\cdot \frac{d^s i_b}{dt} + (\Omega_{bz}r_x-\Omega_{bx}r_z) \cdot\frac{d^s j_b}{dt} + (\Omega_{bx}r_y-\Omega_{by}r_x) \cdot \frac{d^s k_b}{dt} \\ &=( \dot{\Omega}_{by}r_z- \dot\Omega_{bz}r_y)\cdot i_b + ( \dot\Omega_{bz}r_x- \dot\Omega_{bx}r_z) \cdot j_b + ( \dot\Omega_{bx}r_y- \dot\Omega_{by}r_x) \cdot k_b \\ &\ \ \ \ \ \ \ +\Omega_{b}\times(\Omega_{by}r_z-\Omega_{bz}r_y)\cdot i_b +\Omega_{b}\times (\Omega_{bz}r_x-\Omega_{bx}r_z) \cdot j_b + \Omega_{b}\times(\Omega_{bx}r_y-\Omega_{by}r_x) \cdot k_b\\ &=\dot\Omega_b\times r_{cb}+\Omega_b \times (\Omega_b\times r_{cb}) \end{aligned} dtds(Ωb×rcb)=dtds[(Ωbyrz−Ωbzry)⋅ib+(Ωbzrx−Ωbxrz)⋅jb+(Ωbxry−Ωbyrx)⋅kb]=dtds(Ωbyrz−Ωbzry)⋅ib+dtds(Ωbzrx−Ωbxrz)⋅jb+dtds(Ωbxry−Ωbyrx)⋅kb +(Ωbyrz−Ωbzry)⋅dtdsib+(Ωbzrx−Ωbxrz)⋅dtdsjb+(Ωbxry−Ωbyrx)⋅dtdskb=(Ω˙byrz−Ω˙bzry)⋅ib+(Ω˙bzrx−Ω˙bxrz)⋅jb+(Ω˙bxry−Ω˙byrx)⋅kb +Ωb×(Ωbyrz−Ωbzry)⋅ib+Ωb×(Ωbzrx−Ωbxrz)⋅jb+Ωb×(Ωbxry−Ωbyrx)⋅kb=Ω˙b×rcb+Ωb×(Ωb×rcb)
同理,我们有:
d s ( r c b × U b ) d t = r c b × U ˙ b + Ω b × ( r c b × U b ) \frac{d^s (r_{cb}\times U_b)}{dt}=r_{cb}\times\dot U_b+\Omega_b\times(r_{cb}\times U_b) dtds(rcb×Ub)=rcb×U˙b+Ωb×(rcb×Ub)
此外,还有:
Ω b × ( r c b × U b ) + U b × ( Ω b × r c b ) = r c b × ( Ω b × U b ) \Omega_b\times(r_{cb}\times U_b)+U_b \times (\Omega_b\times r_{cb})=r_{cb}\times (\Omega_b\times U_b) Ωb×(rcb×Ub)+Ub×(Ωb×rcb)=rcb×(Ωb×Ub)
另外,由于对于{b}系而言, I b I_b Ib是常数矩阵,所以
d s ( I d Ω b ) d t = d s d t ( [ I x x I x y I x z I y x I y y I y z I z x I z y I z z ] [ Ω b x Ω b y Ω b z ] ) = d s d t ( I x x Ω b x + I x y Ω b y + I x z Ω b z I y x Ω b x + I y y Ω b y + I y z Ω b z I z x Ω b x + I z y Ω b y + I z z Ω b z ) = ( d s d t ( I x x Ω b x ⋅ i b + I x y Ω b y ⋅ j b + I x z Ω b z ⋅ k b ) d s d t ( I y x Ω b x ⋅ i b + I y y Ω b y ⋅ j b + I y z Ω b z ⋅ k b ) ) d s d t ( I z x Ω b x ⋅ i b + I z y Ω b y ⋅ j b + I z z Ω b z ⋅ k b ) ) ) = ( I x x Ω ˙ b x ⋅ i b + I x y Ω ˙ b y ⋅ j b + I x z Ω ˙ b z ⋅ k b + Ω b × ( I x x Ω b x ⋅ i b + I x y Ω b y ⋅ j b + I x z Ω b z ⋅ k b ) I y x Ω ˙ b x ⋅ i b + I y y Ω ˙ b y ⋅ j b + I y z Ω ˙ b z ⋅ k b + Ω b × ( I y x Ω b x ⋅ i b + I y y Ω b y ⋅ j b + I y z Ω b z ⋅ k b ) I z x Ω ˙ b x ⋅ i b + I z y Ω ˙ b y ⋅ j b + I z z Ω ˙ b z ⋅ k b + Ω b × ( I z x Ω b x ⋅ i b + I z y Ω b y ⋅ j b + I z z Ω b z ⋅ k b ) ) = I b Ω ˙ b + Ω b × ( I b Ω b ) \begin{aligned} \frac{d^s (I_d\Omega_b)}{dt}&=\frac{d^s}{dt} \left(\left[ \begin{matrix} I_{xx} & I_{xy} & I_{xz}\\ I_{yx} & I_{yy} & I_{yz}\\ I_{zx} & I_{zy} & I_{zz}\\ \end{matrix} \right] \left[ \begin{matrix} \Omega_{bx}\\ \Omega_{by}\\ \Omega_{bz} \end{matrix} \right]\right) \\ &=\frac{d^s}{dt} \left( \begin{matrix} I_{xx}\Omega_{bx}+I_{xy}\Omega_{by}+I_{xz}\Omega_{bz}\\ I_{yx}\Omega_{bx}+I_{yy}\Omega_{by}+I_{yz}\Omega_{bz}\\ I_{zx}\Omega_{bx}+I_{zy}\Omega_{by}+I_{zz}\Omega_{bz}\\ \end{matrix} \right)\\ &=\left( \begin{matrix} \frac{d^s}{dt}(I_{xx}\Omega_{bx}\cdot i_b+I_{xy}\Omega_{by}\cdot j_b+I_{xz}\Omega_{bz}\cdot k_b)\\ \frac{d^s}{dt}(I_{yx}\Omega_{bx}\cdot i_b+I_{yy}\Omega_{by}\cdot j_b+I_{yz}\Omega_{bz}\cdot k_b))\\ \frac{d^s}{dt}(I_{zx}\Omega_{bx}\cdot i_b+I_{zy}\Omega_{by}\cdot j_b+I_{zz}\Omega_{bz}\cdot k_b)) \end{matrix} \right)\\ &=\left( \begin{matrix} I_{xx}\dot \Omega_{bx}\cdot i_b+I_{xy}\dot \Omega_{by}\cdot j_b+I_{xz}\dot \Omega_{bz}\cdot k_b+\Omega_b\times(I_{xx}\Omega_{bx}\cdot i_b+I_{xy}\Omega_{by}\cdot j_b+I_{xz}\Omega_{bz}\cdot k_b)\\ I_{yx}\dot \Omega_{bx}\cdot i_b+I_{yy}\dot \Omega_{by}\cdot j_b+I_{yz}\dot \Omega_{bz}\cdot k_b+\Omega_b\times(I_{yx}\Omega_{bx}\cdot i_b+I_{yy}\Omega_{by}\cdot j_b+I_{yz}\Omega_{bz}\cdot k_b)\\ I_{zx}\dot \Omega_{bx}\cdot i_b+I_{zy}\dot \Omega_{by}\cdot j_b+I_{zz}\dot \Omega_{bz}\cdot k_b+\Omega_b\times(I_{zx}\Omega_{bx}\cdot i_b+I_{zy}\Omega_{by}\cdot j_b+I_{zz}\Omega_{bz}\cdot k_b) \end{matrix} \right)\\ &=I_b\dot\Omega_b+\Omega_b\times(I_b\Omega_b) \end{aligned} dtds(IdΩb)=dtds⎝⎛⎣⎡IxxIyxIzxIxyIyyIzyIxzIyzIzz⎦⎤⎣⎡ΩbxΩbyΩbz⎦⎤⎠⎞=dtds⎝⎛IxxΩbx+IxyΩby+IxzΩbzIyxΩbx+IyyΩby+IyzΩbzIzxΩbx+IzyΩby+IzzΩbz⎠⎞=⎝⎛dtds(IxxΩbx⋅ib+IxyΩby⋅jb+IxzΩbz⋅kb)dtds(IyxΩbx⋅ib+IyyΩby⋅jb+IyzΩbz⋅kb))dtds(IzxΩbx⋅ib+IzyΩby⋅jb+IzzΩbz⋅kb))⎠⎞=⎝⎛IxxΩ˙bx⋅ib+IxyΩ˙by⋅jb+IxzΩ˙bz⋅kb+Ωb×(IxxΩbx⋅ib+IxyΩby⋅jb+IxzΩbz⋅kb)IyxΩ˙bx⋅ib+IyyΩ˙by⋅jb+IyzΩ˙bz⋅kb+Ωb×(IyxΩbx⋅ib+IyyΩby⋅jb+IyzΩbz⋅kb)IzxΩ˙bx⋅ib+IzyΩ˙by⋅jb+IzzΩ˙bz⋅kb+Ωb×(IzxΩbx⋅ib+IzyΩby⋅jb+IzzΩbz⋅kb)⎠⎞=IbΩ˙b+Ωb×(IbΩb)
最近学习建模耽误了很多时间,其实这么老的方法,直接把论文里的公式拿来用就OK了,但是我就是控制不住自己,想要把来龙去脉搞清楚。查了很多资料都没有查到,最后只好自己动手。
当然这里也借鉴了很多地方的资料,参考如下:
惯性矩、惯性积、转动惯量、惯性张量
关于机器人运动学与动力学建模的几点领悟
牛顿—欧拉方程
UNDERWATER GLIDERS: DYNAMICS, CONTROL AND DESIGN
最后,希望本文能够对大家有所帮助!