1 仿生分析

仿人机器人是基于仿生原理，在机械结构和运动过程上模仿人的行为以达到预期性能的机电装置。如图1.1所示，一般地，仿人机器人结构可分为七连杆，主导运动的腿部主要由髋关节、膝关节、踝关节组成，其余为上肢。

与人类似，在机器人行走运动过程中分为三个阶段：单脚支撑、落地碰撞与双脚支撑阶段。机器人处于单脚支撑阶段时，摆动腿膝关节被动摆动，但在落地碰撞进入双脚支撑阶段之前，摆动腿会绷直以更好地支撑身体重量 [ 1 ] ^{[1]} [1]。有研究指出 [ 2 ] ^{[2]} [2]，髋关节、膝关节在机器人行走、跑步等常规运动——即单脚支撑与双脚支撑阶段占有主导地位；而踝关节的分析在这两个阶段中并不必要，其作用体现在落地碰撞瞬间的驱动力上。

因此，仿人机器人运动系统模型可以分为两个方面：
(1) 单脚支撑与双脚支撑阶段，此时不考虑踝关节驱动，机器人简化为五连杆模型，如图1.2所示；
(2) 落地碰撞阶段，此时踝关节提供驱动并产生自由度变化。
仿人机器人的综合模型可视为上述两个方面的叠加 [ 3 ] ^{[3]} [3]。

2 模型假设

2.1 单脚支撑与双脚支撑阶段

此时机器人为五连杆模型，转动关节本身产生五个转动自由度 θ i ( i = 1 , 2 , 3 , 4 , 5 ) \theta _i\left( i=1,2,3,4,5 \right) θi​(i=1,2,3,4,5)，分别由五个驱动力矩 τ i ( i = 1 , 2 , 3 , 4 , 5 ) \tau _i\left( i=1,2,3,4,5 \right) τi​(i=1,2,3,4,5)作用。假设基坐标系位于髋部，当髋关节位置不变时，机器人的位姿完全由上述五个自由度表征。但当机器人运动产生髋部位移时，基坐标系随之移动，此时需要额外增加两个自由度来描述这种变化，这里把这两个自由度体现在髋部坐标上。综上所述，仿人机器人在单脚支撑和双脚支撑阶段共7个自由度构成关节向量：

θ = [ θ 1 θ 2 θ 3 θ 4 θ 5 x z ] T \boldsymbol{\theta }=\left[ \begin{matrix} \theta _1& \theta _2& \theta _3& \theta _4& \theta _5& x& z\\\end{matrix} \right] ^T θ=[θ1​​θ2​​θ3​​θ4​​θ5​​x​z​]T

5个驱动力矩构成控制输入：

τ = [ τ 1 τ 2 τ 3 τ 4 τ 5 ] T \boldsymbol{\tau }=\left[ \begin{matrix} \tau _1& \tau _2& \tau _3& \tau _4& \tau _5\\\end{matrix} \right] ^T τ=[τ1​​τ2​​τ3​​τ4​​τ5​​]T

2.2 落地碰撞阶段

建立仿人机器人多体动力学模型最重要的一点就是怎样处理脚和地面的接触面。一般地，认为机器人是多刚体系统，且一般地面刚度很大，因此机器人足端与地面为刚性接触。落地碰撞瞬时发生，且为完全非弹性碰撞，从而碰撞前后，机器人仅发生速度突变而没有位置的突变，且碰撞后摆动腿不发生弹起和滑移。有研究采用弹簧防震器和误差项来控制地面和系统的反作用 [ 4 ] ^{[4]} [4]；有研究采用Hertz模型来表示地面接触力 [ 5 ] ^{[5]} [5]。这里采用Hertz模型：

{ F l = [ F x l F z l ] = [ F x l K ∣ z l ∣ n + λ z ˙ l ∣ z l ∣ n ] F r = [ F x r F z r ] = [ F x r K ∣ z r ∣ n + λ z ˙ r ∣ z r ∣ n ] \begin{cases} \boldsymbol{F}_l=\left[ \begin{array}{c} F_{xl}\\ F_{zl}\\\end{array} \right] =\left[ \begin{array}{c} F_{xl}\\ K\left| z_l \right|^n+\lambda \dot{z}_l\left| z_l \right|^n\\\end{array} \right]\\ \boldsymbol{F}_r=\left[ \begin{array}{c} F_{xr}\\ F_{zr}\\\end{array} \right] =\left[ \begin{array}{c} F_{xr}\\ K\left| z_r \right|^n+\lambda \dot{z}_r\left| z_r \right|^n\\\end{array} \right]\\\end{cases} ⎩⎪⎪⎨⎪⎪⎧​Fl​=[Fxl​Fzl​​]=[Fxl​K∣zl​∣n+λz˙l​∣zl​∣n​]Fr​=[Fxr​Fzr​​]=[Fxr​K∣zr​∣n+λz˙r​∣zr​∣n​]​

其中 K K K为地面刚度系数， λ \lambda λ为阻尼系数， n n n为赫兹系数， ( x l , z l ) (x_l, z_l) (xl​,zl​)、 ( x r , z r ) (x_r, z_r) (xr​,zr​)分别为左、右足的空间坐标，对于关节向量 ，将碰撞时的约束力映射为约束力矩向量(对移动自由度而言为力)：

{ τ l = C l F l τ r = C r F r \begin{cases} \boldsymbol{\tau }_l=\boldsymbol{C}_l\boldsymbol{F}_l\\ \boldsymbol{\tau }_r=\boldsymbol{C}_r\boldsymbol{F}_r\\\end{cases} {τl​=Cl​Fl​τr​=Cr​Fr​​

3 模型建立

采用拉格朗日方程建立动力学模型，列出各连杆重心坐标如下：

{ x ˉ 1 = x + l 3 sin ⁡ θ 3 + s 1 sin ⁡ θ 1 x ˉ 2 = x − l 4 sin ⁡ θ 4 − s 2 sin ⁡ θ 2 x ˉ 3 = x + s 3 sin ⁡ θ 3 x ˉ 4 = x − s 4 sin ⁡ θ 4 x ˉ 5 = x − s 5 sin ⁡ θ 5    { z ˉ 1 = z − l 3 cos ⁡ θ 3 − s 1 cos ⁡ θ 1 z ˉ 2 = z − l 4 cos ⁡ θ 4 − s 2 cos ⁡ θ 2 z ˉ 3 = z − s 3 cos ⁡ θ 3 z ˉ 4 = z − s 4 cos ⁡ θ 4 z ˉ 5 = z + s 5 cos ⁡ θ 5 \begin{cases} \bar{x}_1=x+l_3\sin \theta _3+s_1\sin \theta _1\\ \bar{x}_2=x-l_4\sin \theta _4-s_2\sin \theta _2\\ \bar{x}_3=x+s_3\sin \theta _3\\ \bar{x}_4=x-s_4\sin \theta _4\\ \bar{x}_5=x-s_5\sin \theta _5\\\end{cases}\,\, \begin{cases} \bar{z}_1=z-l_3\cos \theta _3-s_1\cos \theta _1\\ \bar{z}_2=z-l_4\cos \theta _4-s_2\cos \theta _2\\ \bar{z}_3=z-s_3\cos \theta _3\\ \bar{z}_4=z-s_4\cos \theta _4\\ \bar{z}_5=z+s_5\cos \theta _5\\\end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​xˉ1​=x+l3​sinθ3​+s1​sinθ1​xˉ2​=x−l4​sinθ4​−s2​sinθ2​xˉ3​=x+s3​sinθ3​xˉ4​=x−s4​sinθ4​xˉ5​=x−s5​sinθ5​​⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​zˉ1​=z−l3​cosθ3​−s1​cosθ1​zˉ2​=z−l4​cosθ4​−s2​cosθ2​zˉ3​=z−s3​cosθ3​zˉ4​=z−s4​cosθ4​zˉ5​=z+s5​cosθ5​​

其中 s i ( i = 1 , 2 , 3 , 4 , 5 ) s_i\left( i=1,2,3,4,5 \right) si​(i=1,2,3,4,5)为连杆重心位置。
设髋部为零势能点，则系统总势能为：

U = ∑ i = 1 5 m i g z ˉ i = − m 1 g ( l 3 cos ⁡ θ 3 + s 1 cos ⁡ θ 1 ) − m 2 g ( l 4 cos ⁡ θ 4 + s 2 cos ⁡ θ 2 ) − m 3 g s 3 cos ⁡ θ 3 − m 4 g s 4 cos ⁡ θ 4 + m 5 g s 5 cos ⁡ θ 5 U=\sum_{i=1}^5{m_ig\bar{z}_i}\\=-m_1g\left( l_3\cos \theta _3+s_1\cos \theta _1 \right) -m_2g\left( l_4\cos \theta _4+s_2\cos \theta _2 \right) -m_3gs_3\cos \theta _3-m_4gs_4\cos \theta _4+m_5gs_5\cos \theta _5 U=i=1∑5​mi​gzˉi​=−m1​g(l3​cosθ3​+s1​cosθ1​)−m2​g(l4​cosθ4​+s2​cosθ2​)−m3​gs3​cosθ3​−m4​gs4​cosθ4​+m5​gs5​cosθ5​

将系统连杆动能分解为平动动能和转动动能,根据 K i = 1 2 m i ( x ˉ ˙ i 2 + z ˉ ˙ i 2 ) + 1 2 J i θ ˙ i 2 K_i=\frac{1}{2}m_i\left( \dot{\bar{x}}_{i}^{2}+\dot{\bar{z}}_{i}^{2} \right) +\frac{1}{2}J_i\dot{\theta}_{i}^{2} Ki​=21​mi​(xˉ˙i2​+zˉ˙i2​)+21​Ji​θ˙i2​，其中 J i J_i Ji​为第 i i i根连杆的转动惯量，可得：

{ K 1 = 1 2 m 1 [ x ˙ 2 + z ˙ 2 + 2 l 3 θ ˙ 3 x ˙ cos ⁡ θ 3 + 2 s 1 θ ˙ 1 x ˙ cos ⁡ θ 1 + 2 l 3 θ ˙ 3 z ˙ sin ⁡ θ 3 + 2 s 1 θ ˙ 1 z ˙ sin ⁡ θ 1 + 2 l 3 s 1 θ ˙ 1 θ ˙ 3 cos ⁡ ( θ 1 − θ 3 ) + l 3 2 θ ˙ 3 2 + s 1 2 θ ˙ 1 2 ] + 1 2 J 1 θ ˙ 1 2 K 2 = 1 2 m 2 [ x ˙ 2 + z ˙ 2 − 2 l 4 θ ˙ 4 x ˙ cos ⁡ θ 4 − 2 s 2 θ ˙ 2 x ˙ cos ⁡ θ 2 + 2 l 4 θ ˙ 4 z ˙ sin ⁡ θ 4 + 2 s 2 θ ˙ 2 z ˙ sin ⁡ θ 2 + 2 l 4 s 2 θ ˙ 2 θ ˙ 4 cos ⁡ ( θ 2 − θ 4 ) + l 4 2 θ ˙ 4 2 + s 2 2 θ ˙ 2 2 ] + 1 2 J 2 θ ˙ 2 2 K 3 = 1 2 m 3 [ x ˙ 2 + z ˙ 2 + 2 s 3 θ ˙ 3 x ˙ cos ⁡ θ 3 + 2 s 3 θ ˙ 3 z ˙ sin ⁡ θ 3 + s 3 2 θ ˙ 3 2 ] + 1 2 J 3 θ ˙ 3 2 K 4 = 1 2 m 4 [ x ˙ 2 + z ˙ 2 − 2 s 4 θ ˙ 4 x ˙ cos ⁡ θ 4 + 2 s 4 θ ˙ 4 z ˙ sin ⁡ θ 4 + s 4 2 θ ˙ 4 2 ] + 1 2 J 4 θ ˙ 4 2 K 5 = 1 2 m 5 [ x ˙ 2 + z ˙ 2 − 2 s 5 θ ˙ 5 x ˙ cos ⁡ θ 5 − 2 s 5 θ ˙ 5 z ˙ sin ⁡ θ 5 + s 5 2 θ ˙ 5 2 ] + 1 2 J 5 θ ˙ 5 2 \begin{cases} K_1=\frac{1}{2}m_1\left[ \dot{x}^2+\dot{z}^2+2l_3\dot{\theta}_3\dot{x}\cos \theta _3+2s_1\dot{\theta}_1\dot{x}\cos \theta _1+2l_3\dot{\theta}_3\dot{z}\sin \theta _3+2s_1\dot{\theta}_1\dot{z}\sin \theta _1+2l_3s_1\dot{\theta}_1\dot{\theta}_3\cos \left( \theta _1-\theta _3 \right) +l_{3}^{2}\dot{\theta}_{3}^{2}+s_{1}^{2}\dot{\theta}_{1}^{2} \right] +\frac{1}{2}J_1\dot{\theta}_{1}^{2}\\ K_2=\frac{1}{2}m_2\left[ \dot{x}^2+\dot{z}^2-2l_4\dot{\theta}_4\dot{x}\cos \theta _4-2s_2\dot{\theta}_2\dot{x}\cos \theta _2+2l_4\dot{\theta}_4\dot{z}\sin \theta _4+2s_2\dot{\theta}_2\dot{z}\sin \theta _2+2l_4s_2\dot{\theta}_2\dot{\theta}_4\cos \left( \theta _2-\theta _4 \right) +l_{4}^{2}\dot{\theta}_{4}^{2}+s_{2}^{2}\dot{\theta}_{2}^{2} \right] +\frac{1}{2}J_2\dot{\theta}_{2}^{2}\\ K_3=\frac{1}{2}m_3\left[ \dot{x}^2+\dot{z}^2+2s_3\dot{\theta}_3\dot{x}\cos \theta _3+2s_3\dot{\theta}_3\dot{z}\sin \theta _3+s_{3}^{2}\dot{\theta}_{3}^{2} \right] +\frac{1}{2}J_3\dot{\theta}_{3}^{2}\\ K_4=\frac{1}{2}m_4\left[ \dot{x}^2+\dot{z}^2-2s_4\dot{\theta}_4\dot{x}\cos \theta _4+2s_4\dot{\theta}_4\dot{z}\sin \theta _4+s_{4}^{2}\dot{\theta}_{4}^{2} \right] +\frac{1}{2}J_4\dot{\theta}_{4}^{2}\\ K_5=\frac{1}{2}m_5\left[ \dot{x}^2+\dot{z}^2-2s_5\dot{\theta}_5\dot{x}\cos \theta _5-2s_5\dot{\theta}_5\dot{z}\sin \theta _5+s_{5}^{2}\dot{\theta}_{5}^{2} \right] +\frac{1}{2}J_5\dot{\theta}_{5}^{2}\\\end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​K1​=21​m1​[x˙2+z˙2+2l3​θ˙3​x˙cosθ3​+2s1​θ˙1​x˙cosθ1​+2l3​θ˙3​z˙sinθ3​+2s1​θ˙1​z˙sinθ1​+2l3​s1​θ˙1​θ˙3​cos(θ1​−θ3​)+l32​θ˙32​+s12​θ˙12​]+21​J1​θ˙12​K2​=21​m2​[x˙2+z˙2−2l4​θ˙4​x˙cosθ4​−2s2​θ˙2​x˙cosθ2​+2l4​θ˙4​z˙sinθ4​+2s2​θ˙2​z˙sinθ2​+2l4​s2​θ˙2​θ˙4​cos(θ2​−θ4​)+l42​θ˙42​+s22​θ˙22​]+21​J2​θ˙22​K3​=21​m3​[x˙2+z˙2+2s3​θ˙3​x˙cosθ3​+2s3​θ˙3​z˙sinθ3​+s32​θ˙32​]+21​J3​θ˙32​K4​=21​m4​[x˙2+z˙2−2s4​θ˙4​x˙cosθ4​+2s4​θ˙4​z˙sinθ4​+s42​θ˙42​]+21​J4​θ˙42​K5​=21​m5​[x˙2+z˙2−2s5​θ˙5​x˙cosθ5​−2s5​θ˙5​z˙sinθ5​+s52​θ˙52​]+21​J5​θ˙52​​

由拉格朗日函数 L = K − U L=K-U L=K−U知

{ d d t ( ∂ L ∂ θ ˙ 1 ) − ∂ L ∂ θ 1 = ( m 1 s 1 2 + J 1 ) θ ¨ 1 + m 1 l 3 s 1 θ ¨ 3 cos ⁡ ( θ 1 − θ 3 ) + m 1 s 1 x ¨ cos ⁡ θ 1 + m 1 s 1 z ¨ sin ⁡ θ 1 + m 1 l 3 s 1 θ ˙ 3 2 sin ⁡ ( θ 1 − θ 3 ) d d t ( ∂ L ∂ θ ˙ 2 ) − ∂ L ∂ θ 2 = ( m 2 s 2 2 + J 2 ) θ ¨ 2 + m 2 l 4 s 2 θ ¨ 4 cos ⁡ ( θ 2 − θ 4 ) − m 2 s 2 x ¨ cos ⁡ θ 2 + m 2 s 2 z ¨ sin ⁡ θ 2 + m 2 l 4 s 2 θ ˙ 4 2 sin ⁡ ( θ 2 − θ 4 ) d d t ( ∂ L ∂ θ ˙ 3 ) − ∂ L ∂ θ 3 = ( m 1 l 3 2 + m 3 s 3 2 + J 3 ) θ ¨ 3 + m 1 l 3 s 1 θ ¨ 1 cos ⁡ ( θ 1 − θ 3 ) + ( m 1 l 3 + m 3 s 3 ) x ¨ cos ⁡ θ 3 + ( m 1 l 3 + m 3 s 3 ) z ¨ sin ⁡ θ 3 − m 1 l 3 s 1 θ ˙ 1 2 sin ⁡ ( θ 1 − θ 3 ) d d t ( ∂ L ∂ θ ˙ 4 ) − ∂ L ∂ θ 4 = ( m 2 l 4 2 + m 4 s 4 2 + J 4 ) θ ¨ 4 + m 2 l 4 s 2 θ ¨ 2 cos ⁡ ( θ 2 − θ 4 ) − ( m 2 l 4 + m 4 s 4 ) x ¨ cos ⁡ θ 4 + ( m 2 l 4 + m 4 s 4 ) z ¨ sin ⁡ θ 4 − m 2 l 4 s 2 θ ˙ 2 2 sin ⁡ ( θ 2 − θ 4 ) d d t ( ∂ L ∂ θ ˙ 5 ) − ∂ L ∂ θ 5 = ( m 5 s 5 2 + J 5 ) θ ¨ 5 − m 5 s 5 x ¨ cos ⁡ θ 5 − m 5 s 5 z ¨ sin ⁡ θ 5 d d t ( ∂ L ∂ x ˙ ) − ∂ L ∂ x = ( m 1 + m 2 + m 3 + m 4 + m 5 ) x ¨ + m 1 s 1 θ ¨ 1 cos ⁡ θ 1 − m 2 s 2 θ ¨ 2 cos ⁡ θ 2 + ( m 1 l 3 + m 3 s 3 ) θ ¨ 3 cos ⁡ θ 3 − ( m 2 l 4 + m 4 s 4 ) θ ¨ 4 cos ⁡ θ 4    − m 5 s 5 θ ¨ 5 cos ⁡ θ 5 − m 1 s 1 θ ˙ 1 2 sin ⁡ θ 1 + m 2 s 2 θ ˙ 2 2 sin ⁡ θ 2 − ( m 1 l 3 + m 3 s 3 ) θ ˙ 3 2 sin ⁡ θ 3 + ( m 2 l 4 + m 4 s 4 ) θ ˙ 4 2 sin ⁡ θ 4 + m 5 s 5 θ ˙ 5 2 sin ⁡ θ 5 d d t ( ∂ L ∂ z ˙ ) − ∂ L ∂ z = ( m 1 + m 2 + m 3 + m 4 + m 5 ) z ¨ + m 1 s 1 θ ¨ 1 sin ⁡ θ 1 + m 2 s 2 θ ¨ 2 sin ⁡ θ 2 + ( m 1 l 3 + m 3 s 3 ) θ ¨ 3 sin ⁡ θ 3 + ( m 2 l 4 + m 4 s 4 ) θ ¨ 4 sin ⁡ θ 4    − m 5 s 5 θ ¨ 5 sin ⁡ θ 5 + m 1 s 1 θ ˙ 1 2 cos ⁡ θ 1 + m 2 s 2 θ ˙ 2 2 cos ⁡ θ 2 + ( m 1 l 3 + m 3 s 3 ) θ ˙ 3 2 cos ⁡ θ 3 + ( m 2 l 4 + m 4 s 4 ) θ ˙ 4 2 cos ⁡ θ 4 − m 5 s 5 θ ˙ 5 2 sin ⁡ θ 5 \begin{cases} \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_1} \right) -\frac{\partial L}{\partial \theta _1}=\left( m_1s_{1}^{2}+J_1 \right) \ddot{\theta}_1+m_1l_3s_1\ddot{\theta}_3\cos \left( \theta _1-\theta _3 \right) +m_1s_1\ddot{x}\cos \theta _1+m_1s_1\ddot{z}\sin \theta _1+m_1l_3s_1\dot{\theta}_{3}^{2}\sin \left( \theta _1-\theta _3 \right)\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_2} \right) -\frac{\partial L}{\partial \theta _2}=\left( m_2s_{2}^{2}+J_2 \right) \ddot{\theta}_2+m_2l_4s_2\ddot{\theta}_4\cos \left( \theta _2-\theta _4 \right) -m_2s_2\ddot{x}\cos \theta _2+m_2s_2\ddot{z}\sin \theta _2+m_2l_4s_2\dot{\theta}_{4}^{2}\sin \left( \theta _2-\theta _4 \right)\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_3} \right) -\frac{\partial L}{\partial \theta _3}=\left( m_1l_{3}^{2}+m_3s_{3}^{2}+J_3 \right) \ddot{\theta}_3+m_1l_3s_1\ddot{\theta}_1\cos \left( \theta _1-\theta _3 \right) +\left( m_1l_3+m_3s_3 \right) \ddot{x}\cos \theta _3+\left( m_1l_3+m_3s_3 \right) \ddot{z}\sin \theta _3-m_1l_3s_1\dot{\theta}_{1}^{2}\sin \left( \theta _1-\theta _3 \right)\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_4} \right) -\frac{\partial L}{\partial \theta _4}=\left( m_2l_{4}^{2}+m_4s_{4}^{2}+J_4 \right) \ddot{\theta}_4+m_2l_4s_2\ddot{\theta}_2\cos \left( \theta _2-\theta _4 \right) -\left( m_2l_4+m_4s_4 \right) \ddot{x}\cos \theta _4+\left( m_2l_4+m_4s_4 \right) \ddot{z}\sin \theta _4-m_2l_4s_2\dot{\theta}_{2}^{2}\sin \left( \theta _2-\theta _4 \right)\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_5} \right) -\frac{\partial L}{\partial \theta _5}=\left( m_5s_{5}^{2}+J_5 \right) \ddot{\theta}_5-m_5s_5\ddot{x}\cos \theta _5-m_5s_5\ddot{z}\sin \theta _5\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{x}} \right) -\frac{\partial L}{\partial x}=\left( m_1+m_2+m_3+m_4+m_5 \right) \ddot{x}+m_1s_1\ddot{\theta}_1\cos \theta _1-m_2s_2\ddot{\theta}_2\cos \theta _2+\left( m_1l_3+m_3s_3 \right) \ddot{\theta}_3\cos \theta _3-\left( m_2l_4+m_4s_4 \right) \ddot{\theta}_4\cos \theta _4\\ \,\, -m_5s_5\ddot{\theta}_5\cos \theta _5-m_1s_1\dot{\theta}_{1}^{2}\sin \theta _1+m_2s_2\dot{\theta}_{2}^{2}\sin \theta _2-\left( m_1l_3+m_3s_3 \right) \dot{\theta}_{3}^{2}\sin \theta _3+\left( m_2l_4+m_4s_4 \right) \dot{\theta}_{4}^{2}\sin \theta _4+m_5s_5\dot{\theta}_{5}^{2}\sin \theta _5\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{z}} \right) -\frac{\partial L}{\partial z}=\left( m_1+m_2+m_3+m_4+m_5 \right) \ddot{z}+m_1s_1\ddot{\theta}_1\sin \theta _1+m_2s_2\ddot{\theta}_2\sin \theta _2+\left( m_1l_3+m_3s_3 \right) \ddot{\theta}_3\sin \theta _3+\left( m_2l_4+m_4s_4 \right) \ddot{\theta}_4\sin \theta _4\\ \,\, -m_5s_5\ddot{\theta}_5\sin \theta _5+m_1s_1\dot{\theta}_{1}^{2}\cos \theta _1+m_2s_2\dot{\theta}_{2}^{2}\cos \theta _2+\left( m_1l_3+m_3s_3 \right) \dot{\theta}_{3}^{2}\cos \theta _3+\left( m_2l_4+m_4s_4 \right) \dot{\theta}_{4}^{2}\cos \theta _4-m_5s_5\dot{\theta}_{5}^{2}\sin \theta _5\\\end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​dtd​(∂θ˙1​∂L​)−∂θ1​∂L​=(m1​s12​+J1​)θ¨1​+m1​l3​s1​θ¨3​cos(θ1​−θ3​)+m1​s1​x¨cosθ1​+m1​s1​z¨sinθ1​+m1​l3​s1​θ˙32​sin(θ1​−θ3​)dtd​(∂θ˙2​∂L​)−∂θ2​∂L​=(m2​s22​+J2​)θ¨2​+m2​l4​s2​θ¨4​cos(θ2​−θ4​)−m2​s2​x¨cosθ2​+m2​s2​z¨sinθ2​+m2​l4​s2​θ˙42​sin(θ2​−θ4​)dtd​(∂θ˙3​∂L​)−∂θ3​∂L​=(m1​l32​+m3​s32​+J3​)θ¨3​+m1​l3​s1​θ¨1​cos(θ1​−θ3​)+(m1​l3​+m3​s3​)x¨cosθ3​+(m1​l3​+m3​s3​)z¨sinθ3​−m1​l3​s1​θ˙12​sin(θ1​−θ3​)dtd​(∂θ˙4​∂L​)−∂θ4​∂L​=(m2​l42​+m4​s42​+J4​)θ¨4​+m2​l4​s2​θ¨2​cos(θ2​−θ4​)−(m2​l4​+m4​s4​)x¨cosθ4​+(m2​l4​+m4​s4​)z¨sinθ4​−m2​l4​s2​θ˙22​sin(θ2​−θ4​)dtd​(∂θ˙5​∂L​)−∂θ5​∂L​=(m5​s52​+J5​)θ¨5​−m5​s5​x¨cosθ5​−m5​s5​z¨sinθ5​dtd​(∂x˙∂L​)−∂x∂L​=(m1​+m2​+m3​+m4​+m5​)x¨+m1​s1​θ¨1​cosθ1​−m2​s2​θ¨2​cosθ2​+(m1​l3​+m3​s3​)θ¨3​cosθ3​−(m2​l4​+m4​s4​)θ¨4​cosθ4​−m5​s5​θ¨5​cosθ5​−m1​s1​θ˙12​sinθ1​+m2​s2​θ˙22​sinθ2​−(m1​l3​+m3​s3​)θ˙32​sinθ3​+(m2​l4​+m4​s4​)θ˙42​sinθ4​+m5​s5​θ˙52​sinθ5​dtd​(∂z˙∂L​)−∂z∂L​=(m1​+m2​+m3​+m4​+m5​)z¨+m1​s1​θ¨1​sinθ1​+m2​s2​θ¨2​sinθ2​+(m1​l3​+m3​s3​)θ¨3​sinθ3​+(m2​l4​+m4​s4​)θ¨4​sinθ4​−m5​s5​θ¨5​sinθ5​+m1​s1​θ˙12​cosθ1​+m2​s2​θ˙22​cosθ2​+(m1​l3​+m3​s3​)θ˙32​cosθ3​+(m2​l4​+m4​s4​)θ˙42​cosθ4​−m5​s5​θ˙52​sinθ5​​

由拉格朗日方程 τ = d d t ( ∂ L ∂ θ ˙ ) − ∂ L ∂ θ \boldsymbol{\tau }=\frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \boldsymbol{\dot{\theta}}} \right) -\frac{\partial L}{\partial \boldsymbol{\theta }} τ=dtd​(∂θ˙∂L​)−∂θ∂L​结合第二节的理论，关节向量 可视为单脚支撑、双脚支撑阶段与落地碰撞阶段的叠加：

B τ + τ l + τ r = D 1 ( θ ) θ ¨ + D 2 ( θ ) θ ˙ 2 + D 4 ( θ ) \boldsymbol{B\tau }+\boldsymbol{\tau }_l+\boldsymbol{\tau }_r=\boldsymbol{D}_1\left( \boldsymbol{\theta } \right) \boldsymbol{\ddot{\theta}}+\boldsymbol{D}_2\left( \boldsymbol{\theta } \right) \boldsymbol{\dot{\theta}}^2+\boldsymbol{D}_4\left( \boldsymbol{\theta } \right) Bτ+τl​+τr​=D1​(θ)θ¨+D2​(θ)θ˙2+D4​(θ)

其中

D 1 ( θ ) = [ m 1 s 1 2 + J 1 0 m 1 l 3 s 1 cos ⁡ ( θ 1 − θ 3 ) 0 0 m 1 s 1 cos ⁡ θ 1 m 1 s 1 sin ⁡ θ 1 m 2 s 2 2 + J 2 0 m 2 l 4 s 2 cos ⁡ ( θ 2 − θ 4 ) 0 − m 2 s 2 cos ⁡ θ 2 m 2 s 2 sin ⁡ θ 2 ⋮ m 1 l 3 2 + m 3 s 3 2 + J 3 0 0 ( m 1 l 3 + m 3 s 3 ) cos ⁡ θ 3 ( m 1 l 3 + m 3 s 3 ) sin ⁡ θ 3 m 2 l 4 2 + m 4 s 4 2 + J 4 0 − ( m 2 l 4 + m 4 s 4 ) cos ⁡ θ 4 ( m 2 l 4 + m 4 s 4 ) sin ⁡ θ 4 ⋮ ⋱ m 5 s 5 2 + J 5 − m 5 s 5 cos ⁡ θ 5 − m 5 s 5 sin ⁡ θ 5 ⋱ m 1 + m 2 + m 3 + m 4 + m 5 0 ⋯ ⋯ m 1 + m 2 + m 3 + m 4 + m 5 ] \boldsymbol{D}_1\left( \boldsymbol{\theta } \right) =\left[ \begin{matrix} m_1s_{1}^{2}+J_1& 0& m_1l_3s_1\cos \left( \theta _1-\theta _3 \right)& 0& 0& m_1s_1\cos \theta _1& m_1s_1\sin \theta _1\\ & m_2s_{2}^{2}+J_2& 0& m_2l_4s_2\cos \left( \theta _2-\theta _4 \right)& 0& -m_2s_2\cos \theta _2& m_2s_2\sin \theta _2\\ \vdots& & m_1l_{3}^{2}+m_3s_{3}^{2}+J_3& 0& 0& \left( m_1l_3+m_3s_3 \right) \cos \theta _3& \left( m_1l_3+m_3s_3 \right) \sin \theta _3\\ & & & m_2l_{4}^{2}+m_4s_{4}^{2}+J_4& 0& -\left( m_2l_4+m_4s_4 \right) \cos \theta _4& \left( m_2l_4+m_4s_4 \right) \sin \theta _4\\ \vdots& & \ddots& & m_5s_{5}^{2}+J_5& -m_5s_5\cos \theta _5& -m_5s_5\sin \theta _5\\ & \ddots& & & & m_1+m_2+m_3+m_4+m_5& 0\\ & & \cdots& & \cdots& & m_1+m_2+m_3+m_4+m_5\\\end{matrix} \right] D1​(θ)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​m1​s12​+J1​⋮⋮​0m2​s22​+J2​⋱​m1​l3​s1​cos(θ1​−θ3​)0m1​l32​+m3​s32​+J3​⋱⋯​0m2​l4​s2​cos(θ2​−θ4​)0m2​l42​+m4​s42​+J4​​0000m5​s52​+J5​⋯​m1​s1​cosθ1​−m2​s2​cosθ2​(m1​l3​+m3​s3​)cosθ3​−(m2​l4​+m4​s4​)cosθ4​−m5​s5​cosθ5​m1​+m2​+m3​+m4​+m5​​m1​s1​sinθ1​m2​s2​sinθ2​(m1​l3​+m3​s3​)sinθ3​(m2​l4​+m4​s4​)sinθ4​−m5​s5​sinθ5​0m1​+m2​+m3​+m4​+m5​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

D 2 ( θ ) = [ 0 0 m 1 l 3 s 1 sin ⁡ ( θ 1 − θ 3 ) 0 0 0 0 0 0 0 m 2 l 4 s 2 sin ⁡ ( θ 2 − θ 4 ) 0 0 0 − m 1 l 3 s 1 sin ⁡ ( θ 1 − θ 3 ) 0 0 0 0 0 0 0 − m 2 l 4 s 2 sin ⁡ ( θ 2 − θ 4 ) 0 0 0 0 0 0 0 0 0 0 0 0 − m 1 s 1 sin ⁡ θ 1 m 2 s 2 sin ⁡ θ 2 − ( m 1 l 3 + m 3 s 3 ) sin ⁡ θ 3 ( m 2 l 4 + m 4 s 4 ) sin ⁡ θ 4 m 5 s 5 sin ⁡ θ 5 0 0 m 1 s 1 cos ⁡ θ 1 m 2 s 2 cos ⁡ θ 2 ( m 1 l 3 + m 3 s 3 ) cos ⁡ θ 3 ( m 2 l 4 + m 4 s 4 ) cos ⁡ θ 4 − m 5 s 5 sin ⁡ θ 5 0 0 ] \boldsymbol{D}_2\left( \boldsymbol{\theta } \right) =\left[ \begin{matrix} 0& 0& m_1l_3s_1\sin \left( \theta _1-\theta _3 \right)& 0& 0& 0& 0\\ 0& 0& 0& m_2l_4s_2\sin \left( \theta _2-\theta _4 \right)& 0& 0& 0\\ -m_1l_3s_1\sin \left( \theta _1-\theta _3 \right)& 0& 0& 0& 0& 0& 0\\ 0& -m_2l_4s_2\sin \left( \theta _2-\theta _4 \right)& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ -m_1s_1\sin \theta _1& m_2s_2\sin \theta _2& -\left( m_1l_3+m_3s_3 \right) \sin \theta _3& \left( m_2l_4+m_4s_4 \right) \sin \theta _4& m_5s_5\sin \theta _5& 0& 0\\ m_1s_1\cos \theta _1& m_2s_2\cos \theta _2& \left( m_1l_3+m_3s_3 \right) \cos \theta _3& \left( m_2l_4+m_4s_4 \right) \cos \theta _4& -m_5s_5\sin \theta _5& 0& 0\\\end{matrix} \right] D2​(θ)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​00−m1​l3​s1​sin(θ1​−θ3​)00−m1​s1​sinθ1​m1​s1​cosθ1​​000−m2​l4​s2​sin(θ2​−θ4​)0m2​s2​sinθ2​m2​s2​cosθ2​​m1​l3​s1​sin(θ1​−θ3​)0000−(m1​l3​+m3​s3​)sinθ3​(m1​l3​+m3​s3​)cosθ3​​0m2​l4​s2​sin(θ2​−θ4​)000(m2​l4​+m4​s4​)sinθ4​(m2​l4​+m4​s4​)cosθ4​​00000m5​s5​sinθ5​−m5​s5​sinθ5​​0000000​0000000​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​

D 4 ( θ ) = [ − m 1 g s 1 sin ⁡ θ 1 − m 2 g s 2 sin ⁡ θ 2 − m 3 g s 3 sin ⁡ θ 3 − m 1 g l 3 sin ⁡ θ 3 − m 4 g s 4 sin ⁡ θ 4 − m 2 g l 4 sin ⁡ θ 4 m 5 g s 5 sin ⁡ θ 5 0 0 ] \boldsymbol{D}_4\left( \boldsymbol{\theta } \right) =\left[ \begin{array}{c} -m_1gs_1\sin \theta _1\\ -m_2gs_2\sin \theta _2\\ -m_3gs_3\sin \theta _3-m_1gl_3\sin \theta _3\\ -m_4gs_4\sin \theta _4-m_2gl_4\sin \theta _4\\ m_5gs_5\sin \theta _5\\ 0\\ 0\\\end{array} \right] D4​(θ)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​−m1​gs1​sinθ1​−m2​gs2​sinθ2​−m3​gs3​sinθ3​−m1​gl3​sinθ3​−m4​gs4​sinθ4​−m2​gl4​sinθ4​m5​gs5​sinθ5​00​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​

τ l 、 τ r \boldsymbol{\tau }_l\text{、}\boldsymbol{\tau }_r τl​、τr​为碰撞接触约束力矩， B B B为转矩连接矩阵

4 参考文献

[1]王健美, 付成龙, 黄元林,等. 基于Matlab的双足机器人动力学仿真及仿生控制平台[J]. 系统仿真学报, 2011(05):977-983.
[2] Chevallereau C , Abba G , Aoustin Y , et al. RABBIT: A Testbed for Advanced Control Theory[J]. Control Systems IEEE, 2013, 23(5):57-79.
[3]槐创锋, 刘平安. 七连杆双足机器人建模和控制系统仿真[J]. 计算机仿真, 2010.
[4]槐创锋, 方跃法. 5连杆双足机器人建模和控制系统仿真[J]. 系统仿真学报, 2008, 27(020):5682-5686.