机械臂能量分析

机械臂能量分析

• • [1.1 基本定义与符号](#1.1 基本定义与符号)
  • [1.2 动力学方程](#1.2 动力学方程)
  • • [1.2.1 关节空间动力学](#1.2.1 关节空间动力学)
    • [1.2.2 任务空间动力学方程：](#1.2.2 任务空间动力学方程：)
    • [1.2.3 运动学关系](#1.2.3 运动学关系)
  • [1.3 能量分析](#1.3 能量分析)
  • • [1.3.1 关节空间总机械能：](#1.3.1 关节空间总机械能：)
    • [1.3.2 任务空间总机械能：](#1.3.2 任务空间总机械能：)
    • [1.3.3 能量分解定理](#1.3.3 能量分解定理)
  • [1.4 功率与能量变化率](#1.4 功率与能量变化率)
  • • [1.4.1 功率平衡](#1.4.1 功率平衡)
    • [1.4.2 保守外力情况](#1.4.2 保守外力情况)
    • [1.4.3 非保守外力情况](#1.4.3 非保守外力情况)
    • 特殊情况
    • 奇异构型

1.1 基本定义与符号

关节空间变量：

q ∈ R n q \in \mathbb{R}^n q∈Rn：关节位置向量

q ˙ ∈ R n \dot{q} \in \mathbb{R}^n q˙∈Rn：关节速度向量

q ¨ ∈ R n \ddot{q} \in \mathbb{R}^n q¨∈Rn：关节加速度向量

任务空间变量：

x ∈ R m x \in \mathbb{R}^m x∈Rm：末端执行器位姿（ m ≤ 6 m \leq 6 m≤6）

x ˙ ∈ R m \dot{x} \in \mathbb{R}^m x˙∈Rm：任务空间速度

x ¨ ∈ R m \ddot{x} \in \mathbb{R}^m x¨∈Rm：任务空间加速度

运动学关系：

• 正运动学： x = f ( q ) x = f(q) x=f(q)

• 速度关系： x ˙ = J ( q ) q ˙ \dot{x} = J(q)\dot{q} x˙=J(q)q˙

• 加速度关系： x ¨ = J ( q ) q ¨ + J ˙ ( q ) q ˙ \ddot{x} = J(q)\ddot{q} + \dot{J}(q)\dot{q} x¨=J(q)q¨+J˙(q)q˙

• J ( q ) = ∂ f ∂ q J(q) = \frac{\partial f}{\partial q} J(q)=∂q∂f：雅可比矩阵（ m × n m \times n m×n）

1.2 动力学方程

1.2.1 关节空间动力学

一般形式：
M ( q ) q ¨ + C ( q , q ˙ ) q ˙ + g ( q ) = τ + J ( q ) T F ext M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau + J(q)^T F_{\text{ext}} M(q)q¨+C(q,q˙)q˙+g(q)=τ+J(q)TFext

其中：

• M ( q ) ∈ R n × n M(q) \in \mathbb{R}^{n \times n} M(q)∈Rn×n：对称正定惯量矩阵

• C ( q , q ˙ ) q ˙ ∈ R n C(q, \dot{q})\dot{q} \in \mathbb{R}^n C(q,q˙)q˙∈Rn：科氏力和离心力向量

• g ( q ) ∈ R n g(q) \in \mathbb{R}^n g(q)∈Rn：重力向量

• f ( q ˙ ) ∈ R n f(\dot{q}) \in \mathbb{R}^n f(q˙)∈Rn：摩擦力向量（通常忽略）

• τ ∈ R n \tau \in \mathbb{R}^n τ∈Rn：关节驱动力矩

• F ext ∈ R m F_{\text{ext}} \in \mathbb{R}^m Fext∈Rm：作用在末端的环境外力

1.2.2 任务空间动力学方程：

Λ ( q ) x ¨ + μ ( q , q ˙ ) + p ( q ) = F + F ext \Lambda(q)\ddot{x} + \mu(q, \dot{q}) + p(q) = F + F_{\text{ext}} Λ(q)x¨+μ(q,q˙)+p(q)=F+Fext

其中：

• Λ ( q ) ∈ R m × m \Lambda(q) \in \mathbb{R}^{m \times m} Λ(q)∈Rm×m：任务空间惯量矩阵
• μ ( q , q ˙ ) ∈ R m \mu(q, \dot{q}) \in \mathbb{R}^m μ(q,q˙)∈Rm：任务空间科氏力/离心力向量
• p ( q ) ∈ R m p(q) \in \mathbb{R}^m p(q)∈Rm：任务空间重力向量
• F ∈ R m F \in \mathbb{R}^m F∈Rm：任务空间控制力

1.2.3 运动学关系

x = f ( q ) x ˙ = J ( q ) q ˙ x ¨ = J ( q ) q ¨ + J ˙ ( q ) q ˙ τ = J ( q ) T F \begin{aligned} x &= f(q) \\ \dot{x} &= J(q)\dot{q} \\ \ddot{x} &= J(q)\ddot{q} + \dot{J}(q)\dot{q} \\ \tau &= J(q)^T F \end{aligned} xx˙x¨τ=f(q)=J(q)q˙=J(q)q¨+J˙(q)q˙=J(q)TF

推导过程如下：

将力映射关系 τ = J ( q ) T F \tau = J(q)^T F τ=J(q)TF 代入关节空间动力学(1)式：
M ( q ) q ¨ + C ( q , q ˙ ) q ˙ + g ( q ) = J ( q ) T F + J ( q ) T F ext M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = J(q)^T F + J(q)^T F_{\text{ext}} M(q)q¨+C(q,q˙)q˙+g(q)=J(q)TF+J(q)TFext

使用动态一致伪逆 J # = M − 1 J T Λ J^\# = M^{-1}J^T\Lambda J#=M−1JTΛ，左乘 J # J^\# J# 的转置：
J # T M q ¨ + J # T ( C q ˙ + g ) = J # T J T ( F + F ext ) {J^\#}^T M \ddot{q} +{J^\#}^T (C\dot{q} + g) = {J^\#}^T J^T (F + F_{\text{ext}}) J#TMq¨+J#T(Cq˙+g)=J#TJT(F+Fext)

利用 J # T J T = I m {J^\#}^T J^T = I_m J#TJT=Im 和 J # T M = Λ J {J^\#}^T M = \Lambda J J#TM=ΛJ，得到：
Λ ( q ) x ¨ + μ ( q , q ˙ ) + p ( q ) = F + F ext \Lambda(q)\ddot{x} + \mu(q, \dot{q}) + p(q) = F + F_{\text{ext}} Λ(q)x¨+μ(q,q˙)+p(q)=F+Fext

其中：

• Λ ( q ) = ( J ( q ) M ( q ) − 1 J ( q ) T ) − 1 \Lambda(q) = (J(q)M(q)^{-1}J(q)^T)^{-1} Λ(q)=(J(q)M(q)−1J(q)T)−1：任务空间惯量矩阵
• μ ( q , q ˙ ) = Λ ( J M − 1 C q ˙ − J ˙ q ˙ ) \mu(q, \dot{q}) = \Lambda(J M^{-1}C\dot{q} - \dot{J}\dot{q}) μ(q,q˙)=Λ(JM−1Cq˙−J˙q˙)：任务空间科氏力/离心力
• p ( q ) = Λ J M − 1 g ( q ) p(q) = \Lambda J M^{-1} g(q) p(q)=ΛJM−1g(q)：任务空间重力向量

1.3 能量分析

1.3.1 关节空间总机械能：

E joint ( q , q ˙ ) = T joint ( q , q ˙ ) + U joint ( q ) E_{\text{joint}}(q, \dot{q}) = T_{\text{joint}}(q, \dot{q}) + U_{\text{joint}}(q) Ejoint(q,q˙)=Tjoint(q,q˙)+Ujoint(q)

其中：

• 动能： T joint = 1 2 q ˙ T M ( q ) q ˙ T_{\text{joint}} = \frac{1}{2} \dot{q}^T M(q) \dot{q} Tjoint=21q˙TM(q)q˙
• 势能： U joint U_{\text{joint}} Ujoint（主要是重力势能）

1.3.2 任务空间总机械能：

E task ( x , x ˙ ) = T task ( q , x ˙ ) + U task ( x ) E_{\text{task}}(x, \dot{x}) = T_{\text{task}}(q, \dot{x}) + U_{\text{task}}(x) Etask(x,x˙)=Ttask(q,x˙)+Utask(x)

其中：

• 动能： T task = 1 2 x ˙ T Λ ( q ) x ˙ T_{\text{task}} = \frac{1}{2} \dot{x}^T \Lambda(q) \dot{x} Ttask=21x˙TΛ(q)x˙
• 势能： U task ( x ) = U joint ( f − 1 ( x ) ) U_{\text{task}}(x) = U_{\text{joint}}(f^{-1}(x)) Utask(x)=Ujoint(f−1(x))

1.3.3 能量分解定理

对于冗余机械臂（ m < n m < n m<n），关节空间总机械能可分解为：
E joint = E task + T null E_{\text{joint}} = E_{\text{task}} + T_{\text{null}} Ejoint=Etask+Tnull

其中：

• T null = 1 2 q ˙ null T M ( q ) q ˙ null T_{\text{null}} = \frac{1}{2} \dot{q}{\text{null}}^T M(q) \dot{q}{\text{null}} Tnull=21q˙nullTM(q)q˙null：零空间动能
• q ˙ null = ( I n − J # J ) q ˙ \dot{q}_{\text{null}} = (I_n - J^\# J) \dot{q} q˙null=(In−J#J)q˙：零空间速度分量
• J # = M − 1 J T Λ J^\# = M^{-1} J^T \Lambda J#=M−1JTΛ：动态一致伪逆

||证明：

定义任务空间速度分量： q ˙ task = J # x ˙ = J # J q ˙ \dot{q}_{\text{task}} = J^\# \dot{x} = J^\# J \dot{q} q˙task=J#x˙=J#Jq˙

速度分解： q ˙ = q ˙ task + q ˙ null \dot{q} = \dot{q}{\text{task}} + \dot{q}{\text{null}} q˙=q˙task+q˙null

动能分解：
T joint = 1 2 q ˙ T M q ˙ = 1 2 ( q ˙ task + q ˙ null ) T M ( q ˙ task + q ˙ null ) = 1 2 q ˙ task T M q ˙ task + 1 2 q ˙ null T M q ˙ null + q ˙ task T M q ˙ null \begin{aligned} T_{\text{joint}} &= \frac{1}{2} \dot{q}^T M \dot{q} \\ &= \frac{1}{2} (\dot{q}{\text{task}} + \dot{q}{\text{null}})^T M (\dot{q}{\text{task}} + \dot{q}{\text{null}}) \\ &= \frac{1}{2} \dot{q}{\text{task}}^T M \dot{q}{\text{task}} + \frac{1}{2} \dot{q}{\text{null}}^T M \dot{q}{\text{null}} + \dot{q}{\text{task}}^T M \dot{q}{\text{null}} \end{aligned} Tjoint=21q˙TMq˙=21(q˙task+q˙null)TM(q˙task+q˙null)=21q˙taskTMq˙task+21q˙nullTMq˙null+q˙taskTMq˙null

利用 M J # = J T Λ M J^\# = J^T \Lambda MJ#=JTΛ 和 J q ˙ null = 0 J \dot{q}{\text{null}} = 0 Jq˙null=0，可得 q ˙ task T M q ˙ null = 0 \dot{q}{\text{task}}^T M \dot{q}_{\text{null}} = 0 q˙taskTMq˙null=0

验证 1 2 q ˙ task T M q ˙ task = 1 2 x ˙ T Λ x ˙ = T task \frac{1}{2} \dot{q}{\text{task}}^T M \dot{q}{\text{task}} = \frac{1}{2} \dot{x}^T \Lambda \dot{x} = T_{\text{task}} 21q˙taskTMq˙task=21x˙TΛx˙=Ttask

势能相等： U joint ( q ) = U task ( f ( q ) ) U_{\text{joint}}(q) = U_{\text{task}}(f(q)) Ujoint(q)=Utask(f(q))

因此：
E joint = T task + T null + U task = E task + T null E_{\text{joint}} = T_{\text{task}} + T_{\text{null}} + U_{\text{task}} = E_{\text{task}} + T_{\text{null}} Ejoint=Ttask+Tnull+Utask=Etask+Tnull
证明结束||

1.4 功率与能量变化率

1.4.1 功率平衡

机械臂能量变化率满足：
d E joint d t = P act + P ext \begin{equation} \frac{dE_{\text{joint}}}{dt} = P_{\text{act}} + P_{\text{ext}} \end{equation} dtdEjoint=Pact+Pext

其中：

• P act = q ˙ T τ P_{\text{act}} = \dot{q}^T \tau Pact=q˙Tτ：驱动器输入功率
• P ext = q ˙ T J T F ext = x ˙ T F ext P_{\text{ext}} = \dot{q}^T J^T F_{\text{ext}} = \dot{x}^T F_{\text{ext}} Pext=q˙TJTFext=x˙TFext：外力功率

||证明

对 E joint E_{\text{joint}} Ejoint 求导：
d E joint d t = d d t ( 1 2 q ˙ T M q ˙ + U joint ) = q ˙ T M q ¨ + 1 2 q ˙ T M ˙ q ˙ + q ˙ T g \begin{align} \frac{dE_{\text{joint}}}{dt} &= \frac{d}{dt}\left(\frac{1}{2} \dot{q}^T M \dot{q} + U_{\text{joint}}\right) \\ &= \dot{q}^T M \ddot{q} + \frac{1}{2} \dot{q}^T \dot{M} \dot{q} + \dot{q}^T g \end{align} dtdEjoint=dtd(21q˙TMq˙+Ujoint)=q˙TMq¨+21q˙TM˙q˙+q˙Tg

代入动力学方程： M q ¨ = τ + J T F ext − C q ˙ − g M \ddot{q} = \tau + J^T F_{\text{ext}} - C \dot{q} - g Mq¨=τ+JTFext−Cq˙−g

利用性质： q ˙ T ( M ˙ − 2 C ) q ˙ = 0 \dot{q}^T (\dot{M} - 2C) \dot{q} = 0 q˙T(M˙−2C)q˙=0，即 q ˙ T C q ˙ = 1 2 q ˙ T M ˙ q ˙ \dot{q}^T C \dot{q} = \frac{1}{2} \dot{q}^T \dot{M} \dot{q} q˙TCq˙=21q˙TM˙q˙

简化得： d E joint d t = q ˙ T τ + q ˙ T J T F ext \frac{dE_{\text{joint}}}{dt} = \dot{q}^T \tau + \dot{q}^T J^T F_{\text{ext}} dtdEjoint=q˙Tτ+q˙TJTFext
证明结束||

1.4.2 保守外力情况

总能量守恒

如果外力 F ext F_{\text{ext}} Fext 是保守力，即存在势能函数 U env ( x ) U_{\text{env}}(x) Uenv(x) 使得 F ext = − ∇ U env ( x ) F_{\text{ext}} = -\nabla U_{\text{env}}(x) Fext=−∇Uenv(x)，且 P act = 0 P_{\text{act}} = 0 Pact=0，则：
d d t ( E joint + U env ) = 0 \begin{equation} \frac{d}{dt} \left( E_{\text{joint}} + U_{\text{env}} \right) = 0 \end{equation} dtd(Ejoint+Uenv)=0

保守外力功率： P ext = x ˙ T F ext = − x ˙ T ∇ U env = − d U env d t P_{\text{ext}} = \dot{x}^T F_{\text{ext}} = -\dot{x}^T \nabla U_{\text{env}} = -\frac{dU_{\text{env}}}{dt} Pext=x˙TFext=−x˙T∇Uenv=−dtdUenv

由定理2： d E joint d t = − d U env d t \frac{dE_{\text{joint}}}{dt} = -\frac{dU_{\text{env}}}{dt} dtdEjoint=−dtdUenv

因此： d d t ( E joint + U env ) = 0 \frac{d}{dt}(E_{\text{joint}} + U_{\text{env}}) = 0 dtd(Ejoint+Uenv)=0

1.4.3 非保守外力情况

对于非保守外力（如阻尼力 F ext = − B x ˙ F_{\text{ext}} = -B \dot{x} Fext=−Bx˙， B > 0 B > 0 B>0），能量耗散：
d E joint d t = P act − x ˙ T B x ˙ \begin{equation} \frac{dE_{\text{joint}}}{dt} = P_{\text{act}} - \dot{x}^T B \dot{x} \end{equation} dtdEjoint=Pact−x˙TBx˙

当 P act = 0 P_{\text{act}} = 0 Pact=0 时， d E joint d t ≤ 0 \frac{dE_{\text{joint}}}{dt} \leq 0 dtdEjoint≤0，系统总机械能减少。

特殊情况

非冗余机械臂（ m = n m = n m=n）

此时雅可比矩阵可逆，有：

• q ˙ = J − 1 x ˙ \dot{q} = J^{-1} \dot{x} q˙=J−1x˙
• Λ = J − T M J − 1 \Lambda = J^{-T} M J^{-1} Λ=J−TMJ−1
• 零空间维数为0， T null = 0 T_{\text{null}} =0 Tnull=0
• E joint = E task E_{\text{joint}} = E_{\text{task}} Ejoint=Etask

奇异构型

当 J J J 秩亏时， Λ − 1 = J M − 1 J T \Lambda^{-1} = J M^{-1} J^T Λ−1=JM−1JT 奇异， Λ \Lambda Λ 趋于无穷大，某些任务方向上的有效惯量无限大。