n自由度旋转关节串联机器人动态模型

🚀 n 自由度旋转关节串联机器人动态模型 🚀

一、➡️ 核心方程 ⬅️

nnn自由度旋转关节串联机器人的核心时域动力学模型如下所示：

q(t)q(t)q(t):关节位置向量q˙(t)dot{q}(t)q˙(t):关节角速度向量q¨(t)ddot{q}(t)q¨(t):关节角加速度向量M(q)M(q)M(q):惯性矩阵C(q,q˙)C(q,dot{q})C(q,q˙):科氏-离心力矩阵G(q)G(q)G(q):重力向量F(t,q,q˙)F(t,q,dot{q})F(t,q,q˙):摩擦力向量τ(t) au(t)τ(t):关节驱动力/力向量

方程的物理本质：“阻力总和=驱动力”

惯性阻力+科氏力离心阻力+重力阻力+摩擦阻力=驱动力矩

二、各参数详细解释（结合串联机器人特性） 🔋

方程中每个参数均对应机器人的特定物理属性，需结合“旋转关节”“串联结构”的特点来理解，具体解释如下表所示：

参数	维度	物理意义与串联机器人特性关联
q(t)q(t)q(t)	n×1向量n×1向量n×1向量	关节位置向量：q1(t),q2(t),…,qn(t)q_1(t),q_2(t),…,q_n(t)q1(t),q2(t),…,qn(t),每个元素表示对应旋转关节的角度（单位：radradrad），是描述机器人姿态的核心变量，关节角决定末端位置。
q˙(t)dot{q}(t)q˙(t)	n×1向量n×1向量n×1向量	关节角速度向量：q1˙(t),q2˙(t),…,qn˙(t)dot{q_1}(t),dot{q_2}(t),…,dot{q_n}(t)q1˙(t),q2˙(t),…,qn˙(t), （单位：rad/srad/srad/s），反映关节运动的 “快慢”，是产生科氏力、离心力的关键因素。
q¨(t)ddot{q}(t)q¨(t)	n×1向量n×1向量n×1向量	关节角加速度向量：q1¨(t),q2¨(t),…,qn¨(t)ddot{q_1}(t),ddot{q_2}(t),…,ddot{q_n}(t)q1¨(t),q2¨(t),…,qn¨(t),（单位：rad/s2rad/s_2rad/s2），反映关节运动的 “加速度”，与惯性力直接成正比。
M(q)M(q)M(q)	n×n对称正定矩阵n×n对称正定矩阵n×n对称正定矩阵	惯性矩阵：机器人最核心的动力学参数之一，其元素mij(q)m_{ij}(q)mij(q)表示 “关节 jjj 的角加速度对关节 iii 产生的惯性力矩”，体现了关节间的惯性耦合（串联机器人的关键特性）。 “对称正定” 是固有属性：确保惯性力始终与加速度方向相反，且无零惯性（避免运动失控）。矩阵对称:mij=mjim_{ij}=m_{ji}mij=mji
C(q,q˙)C(q,dot{q})C(q,q˙)	n×n矩阵n×n矩阵n×n矩阵	科氏 – 离心力矩阵：描述关节运动速度产生的附加力，分为两类： 1. 科氏力（Coriolis force）：由两个关节的角速度交叉作用产生（如关节 1 和关节 2 同时转动时，连杆间的交互力），元素与qi˙qj˙dot{q_i} dot{q_j}qi˙qj˙成正比； 2. 离心力（Centrifugal force）：由单个关节的角速度平方产生（如关节高速旋转时，连杆因离心效应产生的径向力），元素与qi˙2dot{q_i}^2qi˙2成正比串联机器人中，科氏 – 离心力随关节角度(q)(q)(q)和速度(q˙)(dot{q})(q˙) 动态变化，是导致系统非线性的主要因素之一。
G(q)G(q)G(q)	n×1向量n×1向量n×1向量	重力向量：描述机器人各连杆重力对关节产生的力矩，仅与关节位置(q)(q)(q)相关（与速度、加速度无关）。
F(t,q,q˙)F(t,q,dot{q})F(t,q,q˙)	n×1向量n×1向量n×1向量	摩擦力向量：阻碍关节运动的力，主要包括三类： 1. 库仑摩擦（静摩擦）：关节静止时的阻力，与运动方向相反； 2. 粘性摩擦：与关节角速度成正比(Fviscous=bq˙,b为粘性摩擦系数F_viscous=bdot{q},b为粘性摩擦系数Fviscous=bq˙,b为粘性摩擦系数); 3. 静摩擦：关节启动时的额外阻力。摩擦力虽小，但会影响低速运动精度（如精密装配机器人），需在控制中补偿。
τ(t) au(t)τ(t)	n×1向量n×1向量n×1向量	关节驱动力矩 / 力向量：控制器输出的核心信号，由电机提供，用于抵消M(q)q¨,C(q,q˙)q˙,G(q),FM(q)ddot{q} ,C(q,dot{q})dot{q} ,G(q),FM(q)q¨,C(q,q˙)q˙,G(q),F 的总和，使关节跟踪期望运动（如跟踪特定轨迹）。

三、方程展开形式 🔖

为了更好地理解，可以将n维矩阵方程展开为“单个关节的力矩平衡方程”（以第i个关节为例）：

左侧第一项（∑j=1nmij(q)qj¨(t)sum_{j=1}^{n}m_{ij}(q)ddot{q_j}(t)∑j=1nmij(q)qj¨(t)）:所有关节的角加速度对第 i 个关节产生的总惯性力矩左侧第二项（∑j=1n∑k=1ncijk(q)qj˙(t)qk˙(k)sum_{j=1}^{n} sum_{k=1}^{n}c_{ijk}(q)dot{q_j}(t)dot{q_k}(k)∑j=1n∑k=1ncijk(q)qj˙(t)qk˙(k)）:所有关节的角速度组合对第 i 个关节产生的总科氏 – 离心力矩左侧第三、四项：第 i 个关节的重力矩与摩擦力矩右侧：第 i 个关节的驱动力矩

四、推导过程 📑

（1）推导前提与假设

结构与假设

机器人结构定义：n个旋转关节串联，连杆 i（i=1~n）通过关节 i 与连杆 i-1 连接（基座为连杆 0），所有关节旋转轴平行（平面机器人）。假设：连杆为刚体，无弹性形变；忽略关节间隙；广义坐标为关节角q=[q1,q2,…,qn]Tq = [q_1, q_2, …, q_n]^Tq=[q1,q2,…,qn]T（完全描述系统姿态）。

核心符号

符号	定义	维度
mim_imi	连杆 i 的质量	标量
IiI_iIi	连杆 i 绕自身质心的转动惯量	标量
rir_iri	连杆 i 的长度（从关节 i 到关节 i+1）	标量
pcip_{ci}pci	连杆 i 质心相对于基座的位置向量	2×1
vciv_{ci}vci	连杆 i 质心的速度向量	2×1
wiw_iwi	连杆 i 的角速度（绕旋转轴）	标量
TTT	系统总动能	标量
VVV	系统总势能（仅重力势能）	标量
L=T−VL=T-VL=T−V	拉格朗日函数	标量
τ auτ	关节驱动力矩向量	标量
FFF	关节摩擦力向量	n×1

（2）计算系统总动能T

动能是所有连杆 “平动动能 + 转动动能” 之和，即T=∑i=1nTiT= sum_{i=1}^{n} T_iT=∑i=1nTi,需逐个推导连杆 i 的动能TiT_iTi。

连杆i的位置与速度推导

假设连杆i的质心位于其几何中心，相对于关节 i 的位置为(ri2cos⁡θi,risin⁡θi)(frac{r_i}{2}cos heta_i,frac{r_i}{sin heta_i})(2ricosθi,sinθiri),其中θi=q1+q2+…+qi heta_i=q_1+q_2+…+q_iθi=q1+q2+…+qi(串联旋转，角度叠加)。

结合前序连杆的位置，质心相对于基座的坐标为：

例：i=1(第一连杆)：θ1=q1,pc1=r12[cos⁡q1,sin⁡q1]T;i=1(第一连杆)： heta_1=q_1,p_{c1}=frac{r_1}{2}[cos q_1,sin q_1]^T;i=1(第一连杆)：θ1=q1,pc1=2r1[cosq1,sinq1]T;例：i=2(第二连杆)：θ2=q1+q2,pc2=r1[cos⁡q1,sin⁡q1]T+r22[cos⁡(q1+q2),sin⁡(q1+q2)]T。i=2(第二连杆)： heta_2=q_1+q_2,p_{c2}=r_1[cos q_1,sin q_1]^T + frac{r_2}{2}[cos (q_1+q_2),sin (q_1+q_2)]^T。i=2(第二连杆)：θ2=q1+q2,pc2=r1[cosq1,sinq1]T+2r2[cos(q1+q2),sin(q1+q2)]T。

图片[1] - n自由度旋转关节串联机器人动态模型 - 宋马

速度是位置对时间的导数，利用链式法则展开：

其中偏导数∂pci∂qjfrac{partial p_{ci}}{partial q_j}∂qj∂pci(雅可比矩阵列向量)为：

δkj为克罗内克函数：k=j时为1，否则为0delta_{kj}为克罗内克函数：k=j 时为 1，否则为 0δkj为克罗内克函数：k=j时为1，否则为0

简化速度的模长平方（平动动能需vci2=x˙ci2+y˙ci2v_{ci}^2 = dot{x}_{ci}^2 + dot{y}_{ci}^2vci2=x˙ci2+y˙ci2):

其中Jij=[Jijx,Jijy]T=∂pci∂qjJ_{ij} = left[ J_{ijx}, J_{ijy}
ight]^T = frac{partial p_{ci}}{partial q_j}Jij=[Jijx,Jijy]T=∂qj∂pci(雅可比列向量)

串联旋转关节的角速度叠加：连杆 i 的角速度等于所有前序关节角速度之和（旋转轴同向）：

连杆i的动能TiT_iTi

平动动能 + 转动动能：

代入vci2v_{ci}^2vci2和ωiomega_iωi的表达式：

系统总动能T

总动能为所有连杆动能之和T=∑i=1nTiT = sum_{i=1}^n T_iT=∑i=1nTi,展开后合并同类项(q˙jq˙k项)(dot{q}_j dot{q}_k 项)(q˙jq˙k项):

定义惯性矩阵M(q)=[mjk(q)]n×nM(q) = [m_{jk}(q)]_{n×n}M(q)=[mjk(q)]n×n,其中元素：

则总动能可简化为二次型形式(核心简化结果)：

（3）计算系统总势能V(重力势能求和)

仅考虑重力势能，以基座为零势能面，总势能为各连杆质心重力势能之和V=∑i=1nViV = sum_{i=1}^n V_iV=∑i=1nVi

单个连杆i的势能ViV_iVi

重力势能公式：Vi=migyciV_i = m_i g y_{ci}Vi=migyci,其中g为重力加速度,yciy_{ci}yci为连杆 iii 质心的 yyy 坐标（竖直方向高度）。

代入yciy_{ci}yci的表达式（来自第一步的质心位置）:

系统总势能V

（4）定义拉格朗日函数并应用拉格朗日方程

拉格朗日方程核心形式（考虑摩擦力）：

其中L=T−VL = T – VL=T−V（拉格朗日函数）,需分别计算∂L∂q˙、ddt(∂L∂q˙)、∂L∂qfrac{partial L}{partial dot{q}}、frac{d}{dt}left( frac{partial L}{partial dot{q}}
ight)、frac{partial L}{partial q}∂q˙∂L、dtd(∂q˙∂L)、∂q∂L

计算∂L∂q˙frac{partial L}{partial dot{q}}∂q˙∂L（对角速度的偏导数）

因L=T−VL = T – VL=T−V,且VVV与q˙dot{q}q˙无关(∂V∂q˙=0frac{partial V}{partial dot{q}} = 0∂q˙∂V=0),故:

代入总动能的二次型（公式 1）,利用矩阵偏导数规则：对于T=12q˙TMq˙T = frac{1}{2} dot{q}^T M dot{q}T=21q˙TMq˙,有∂T∂q˙=Mq˙frac{partial T}{partial dot{q}} = M dot{q}∂q˙∂T=Mq˙（证明见附录）。因此：

计算ddt(∂L∂q˙)frac{d}{dt} left( frac{partial L}{partial dot{q}}
ight)dtd(∂q˙∂L)对时间的全导数）

需考虑M(q)M(q)M(q)随时间变化（M与q相关，q随时间变化），应用乘积求导法则：

其中M˙(q)dot{M}(q)M˙(q)是惯性矩阵的时间导数，展开为：

每个元素m˙jk=∑l=1n∂mjk∂qlq˙ldot{m}_{jk} = sum_{l=1}^n frac{partial m_{jk}}{partial q_l} dot{q}_lm˙jk=∑l=1n∂ql∂mjkq˙l

计算∂L∂qfrac{partial L}{partial q}∂q∂L（对关节角的偏导数）

因L=T−VL = T – VL=T−V，故：

需分别计算∂T∂q和∂V∂qfrac{partial T}{partial q}和frac{partial V}{partial q}∂q∂T和∂q∂V。

代入T=12q˙TMq˙T = frac{1}{2} dot{q}^T M dot{q}T=21q˙TMq˙，对单个关节角qlq_lql求偏导（扩展到向量形式）：

向量形式为：

其中C(q,q˙)C(q, dot{q})C(q,q˙)为科氏 – 离心力矩阵，其元素定义为：

物理意义：分离出与角速度相关的耦合项，确保后续化简消去M˙dot{M}M˙。关键特性：M˙(q)=C(q,q˙)+C(q,q˙)Tdot{M}(q) = C(q, dot{q}) + C(q, dot{q})^TM˙(q)=C(q,q˙)+C(q,q˙)T（对称矩阵导数特性，证明见附录）。

代入VVV的表达式（公式 2），对单个关节角qlq_lql求偏导：

向量形式定义为重力向量G(q)G(q)G(q)：

物理意义：G(q)G(q)G(q)的第 lll 个元素是关节 lll 需克服的重力矩。

将公式（7）、（8）代入公式（6）：

联立拉格朗日方程并化简

将公式（4）、（5）、（9）代入公式（3）：

利用M˙=C+CTdot{M} = C + C^TM˙=C+CT（科氏矩阵特性），且q˙TM˙q˙=q˙T(C+CT)q˙=2q˙TCq˙dot{q}^T dot{M} dot{q} = dot{q}^T (C + C^T) dot{q} = 2 dot{q}^T C dot{q}q˙TM˙q˙=q˙T(C+CT)q˙=2q˙TCq˙（对称矩阵二次型特性），代入后：

化简消去M˙q˙dot{M}dot{q}M˙q˙和q˙TCq˙dot{q}^T C dot{q}q˙TCq˙项（交叉抵消），最终得到：

附录

公式∂T∂q˙=Mq˙frac{partial T}{partial dot{q}} = M dot{q}∂q˙∂T=Mq˙的证明

1.前置知识：标量对向量的偏导数定义

若存在标量函数f(q˙)f(dot{q})f(q˙)，其中q˙=[q˙1,q˙2,…,q˙n]Tdot{q} = [dot{q}_1, dot{q}_2, …, dot{q}_n]^Tq˙=[q˙1,q˙2,…,q˙n]T（n×1n×1n×1 向量），则标量对向量的偏导数定义为：

即结果为 n×1n×1n×1 向量，第 lll 个元素是 fff 对q˙ldot{q}_lq˙l的偏导数（l=1,2,…,nl=1,2,…,nl=1,2,…,n）。

2.证明步骤

已知系统总动能的二次型形式为：

其中M(q)=[mij(q)]n×nM(q) = [m_{ij}(q)]_{n×n}M(q)=[mij(q)]n×n是 n×nn×nn×n 对称正定矩阵（mij=mjim_{ij}=m_{ji}mij=mji），q˙Tdot{q}^Tq˙T是q˙dot{q}q˙的转置（1×n1×n1×n 行向量）。

(1)将二次型展开为标量形式

将矩阵乘法展开，明确 TTT 的标量表达式。因q˙TMq˙dot{q}^T M dot{q}q˙TMq˙是 1×1 标量，展开后：

行向量q˙Tdot{q}^Tq˙T的第 iii 个元素与矩阵 MMM 的第 iii 行第 jjj 列元素mijm_{ij}mij相乘，再与列向量q˙dot{q}q˙的第 jjj 个元素q˙jdot{q}_jq˙j相乘，最后对所有 i、ji、ji、j 求和

代入动能公式（1），得标量形式：

mijm_{ij}mij是关节角 qqq 的函数（与q˙dot{q}q˙无关），求偏导时视为常数。

(2)计算 TTT 对单个q˙ldot{q}_lq˙l的偏导数（∂T∂q˙lfrac{partial T}{partial dot{q}_l}∂q˙l∂T）

根据偏导数的线性性质和乘积法则，对公式（2）中q˙ldot{q}_lq˙l求偏导（lll 为固定索引，1≤l≤n1≤l≤n1≤l≤n）：

关键：分析∂∂q˙l(q˙iq˙j)frac{partial}{partial dot{q}_l} (dot{q}_i dot{q}_j)∂q˙l∂(q˙iq˙j)的结果（分 3 种情况）：

情况 1：i≠li≠li=l 且 j≠l→q˙ij≠l → dot{q}_ij=l→q˙i和q˙jdot{q}_jq˙j均与q˙ldot{q}_lq˙l无关，偏导数为 0；情况 2：i=li=li=l 且 j≠l→乘积为q˙lq˙jj≠l → 乘积为dot{q}_l dot{q}_jj=l→乘积为q˙lq˙j，对q˙ldot{q}_lq˙l的偏导数为q˙jdot{q}_jq˙j；情况 3：i≠li≠li=l 且 j=l→乘积为q˙iq˙lj=l → 乘积为dot{q}_i dot{q}_lj=l→乘积为q˙iq˙l，对q˙ldot{q}_lq˙l的偏导数为q˙idot{q}_iq˙i；情况 4：i=li=li=l 且 j=l→乘积为q˙l2j=l → 乘积为dot{q}_l^2j=l→乘积为q˙l2，对q˙ldot{q}_lq˙l的偏导数为2q˙l2dot{q}_l2q˙l（可合并到情况 2、3 中）。

因此，双重求和可简化为仅保留 i=li=li=l 或 j=lj=lj=l 的项：

第一项是 i=l、ji=l、ji=l、j 遍历所有值的和，第二项是 j=l、ij=l、ij=l、i 遍历所有值的和

(3)利用 MMM 的对称性化简，合并为向量形式

因惯性矩阵 MMM 对称，即mil=mlim_{il}=m_{li}mil=mli（对所有 i、li、li、l成立），故第二项中的mil=mlim_{il}=m_{li}mil=mli，可将第二项的索引 iii 替换为 jjj，得：

代入上式，两项求和结果相等，因此：

根据标量对向量的偏导数定义，将所有∂T∂q˙lfrac{partial T}{partial dot{q}_l}∂q˙l∂T（lll=1 ~ nnn）组成向量：

观察右侧向量：其本质是矩阵 MMM 与向量q˙dot{q}q˙的乘积（MMM 为 n×nn×nn×n 矩阵，q˙dot{q}q˙为 n×1n×1n×1 向量，乘积为$ n×1 $向量），即：

因此，最终得：

公式M˙(q)=C(q,q˙)+C(q,q˙)Tdot{M}(q) = C(q, dot{q}) + C(q, dot{q})^TM˙(q)=C(q,q˙)+C(q,q˙)T的详细证明

1.背景与定义

在拉格朗日机器人动力学中，动能可写为：

2.科氏矩阵C(q,q˙)C(q, dot{q})C(q,q˙)的定义

定义C(q,q˙)C(q, dot{q})C(q,q˙)的元素为：

另一种常见写法：设c(k,i,j)=∂Mij∂qk−12∂Mjk∂qic(k, i, j) = frac{partial M_{ij}}{partial q_k} – frac{1}{2} frac{partial M_{jk}}{partial q_i}c(k,i,j)=∂qk∂Mij−21∂qi∂Mjk，但这里我们直接用标准结果。