4、机器人静力学

i
f i −if i+1 + i mi g = 0
i +1 i i ci i i
（4-1）
力矩平衡方程为 M i − M i +1 −
i i
P× f i +1 + r× mi g = 0 （4-2）
通常需要根据末端连杆上的外界作用力和力矩，依次计算出 每个连杆上的受力情况，从末端连杆递推到基座。 如果忽略掉连杆本身的重量，上两式可写成反向迭代的形式
第四章、机器人静力学
机器人与外界接触会有力和力矩的作用，如灵巧手抓取鸡 蛋时；双足机器人上下楼梯时。 各关节的驱动力（广义力）与末端的作用力之间的关系？？
17.6m
如果外界的作用力太大，如机器人抓持的 物体太重，则机器人操作臂会发生变形。 外界作用力与操作臂各 连杆变形之间的关系？？ 本章讨论机器人在静止状态下的受力分析和平衡关系。
τi =iFiT ⋅i Z i
F3
Y3
X3
例：2自由度平面机器人末端对外施加 的作用力为F3，求各关节驱动力矩。
Y2
τ2
X2
l2 连杆2
连杆1
⎡ fx ⎤ ⎢ ⎥ 2 f 2 = F3 = ⎢ f y ⎥ ⎢0⎥ ⎣ ⎦
⎡ 0 ⎤ ⎢ ⎥ 2 2 M 2 = l2 x2 × f 2 = ⎢ 0 ⎥ ⎢l2 f y ⎥ ⎣ ⎦
S4
N(JT) 零空间
τ ∈R
N(JT)
n
F ∈ Rm
零空间：代表不需要任何关节驱动力矩而能承受的所有末端 操作力的集合；这时末端操作力完全由操作臂机构本身承 受。 R ( J T ) 值域空间：代表操作力能平衡的所有关节力矩矢量的集合。
这意味着在 q∈R J映射 不产生末端 S2 操作速度的 • S1 p =0 这些关节速 度方向上， N(J) R (J ) 关节力矩不 零空间 值域空间 能 被 末 端 操 作 力 所 平 JT 衡。为了保 持操作臂末 S3 端 静 止 不 动，在零空 N(JT) R( J T ) S4 间的关节力 零空间 值域空间 矩矢量必须 n F ∈ Rm τ ∈R 为 零 。 T 根据线性代数：零空间 N(J) 是值域空间 R( J ) 在Rn的正交补，
l1 X1 Y1
τ1
⎡c 2 − s 2 0 ⎤ ⎡ f x ⎤ ⎡ c 2 f x − s 2 f y ⎤ 1 f1 = ⎢ s 2 c 2 0⎥ ⎢ f y ⎥ = ⎢ s 2 f x + c 2 f y ⎥ ⎥ ⎥⎢ ⎥ ⎢ ⎢ ⎥ ⎢0 0 1⎥ ⎢ 0 ⎥ ⎢ 0 ⎦ ⎦⎣ ⎦ ⎣ ⎣ ⎡ ⎤ ⎡ 0 ⎤ 0 ⎢ ⎥ ⎢ ⎥ 1 1 0 M 1 = ⎢ 0 ⎥ + l1 x1 × f1 = ⎢ ⎥ ⎢l1 s 2 f x + l1c2 f y + l 2 f y ⎥ ⎢l 2 f y ⎥ ⎣ ⎦ ⎣ ⎦ τ1 = l1 s 2 f x + l1c2 f y + l 2 f y τ 2 = l2 y2 ⎡ τ1 ⎤ ⎡l1 s 2 ⎢τ ⎥ = ⎢ 0 ⎣ 2⎦ ⎣ l1c2 + l 2 ⎤ ⎡ f x ⎤ ⎥⎢ f ⎥ l2 ⎦ ⎣ y ⎦
⎡ τ1 ⎤ ⎡− l1 s1 − l 2 s12 τ = ⎢ ⎥ =⎢ ⎣τ 2 ⎦ ⎣ − l 2 s12
⎡−l1s1 − l2 s12 J =⎢ ⎣ l1c1 + l2c12
l1c1 + l 2 c12 ⎤ 0 ⎥ F2 l 2 c12 ⎦
−l2 s12 ⎤ ⎥ l2c12 ⎦
力雅可比
力雅可比刚好是运动学雅可比矩阵的转置
静力映射是从m维矢量空间向n维关节空间的映射；因此 关节力矩矢量总是由末端操作力F唯一确定。 τ = J T (q) F 但对于给定的关节力矩，与之平衡的末端操作力不一定存在。
q∈R
S1
•
n
J映射
p ∈ Rm
S2
•
p=0
•
速度：
N(J) 零空间
R (J )
值域空间
JT
S3
静力：
R( J T )
值域空间
•
n
p ∈ Rm
•
即对于任何非空的n维矢量 q ∈ N (J ) 则 q ⊥ R ( J T ) S1:N(J)在Rn的正交补
•
•
S1 ≡ R( J T )
S3:R(JT)在Rn的正交补
S3 ≡ N ( J )
q∈R
S1
•
n
J映射
p ∈ Rm
S2
•
p=0
•
N(J) 零空间
Rห้องสมุดไป่ตู้(J )
值域空间
JT
S3
B B ⎡d A ⎤ ⎡ A R − S ( BO P ) A R ⎤ ⎡d B ⎤ A =⎢ ⎥⎢ ⎥ ⎢δ ⎥ B ⎣ A⎦ ⎣ 0 AR ⎦ ⎣δ B ⎦
反 对 称 矩 阵
⎡ 0 ⎢ S ( P) = ⎢ p z ⎢− p y ⎣
− pz 0 px
py ⎤ ⎡ px ⎤ ⎥ − px ⎥ , P = ⎢ p y ⎥ ⎢ ⎥ ⎢ pz ⎥ 0 ⎥ ⎣ ⎦ ⎦
A T B T
两坐标系的 微运动变换
A A ⎡ B R S ( AO P) B R ⎤ A B B D=⎢ ⎥⋅ D A BR ⎣ 0 ⎦
A A ⎡ B R S ( AO P ) B R ⎤ B F = F ⎢ ⎥ A BR ⎣ 0 ⎦
从坐标系{B}的描述变换到坐标系{A}中的描述为：
B ⎡ fA ⎤ ⎡ AR ⎢m ⎥ = ⎢ BO B ⎣ A ⎦ ⎣S ( A P) A R
WO T
P⎤ ⎥ 1 ⎦
⎡ fT ⎤ ⎡ W R T = ⎢ WO W ⎢ ⎥ ⎣mT ⎦ ⎣ S ( T P ) T R
0 ⎤ ⎡ fW ⎤ ⎥ W ⎥⎢ R ⎦ ⎣ mW ⎦ T
⎡ 0 ⎡1 0 0 ⎤ ⎢ ⎥ WO ⎢ W 1 0⎥ S ( T P ) = ⎢ p z T R = ⎢0 ⎢− p y ⎢0 0 1 ⎥ ⎦ ⎣ ⎣
运动学 和静力 学的对 偶性
R( J T )
值域空间
S4
N(JT) 零空间
τ ∈R
n
F ∈ Rm
S2 ≡ N (J T )
T 在操作空间中，值域空间 R(J ) 是零空间 N(J ) 的正交补。
S 4 ≡ R( J )
不能由关节运动驱动产生的这些操作运动方向正是不需关节力矩平 衡的末端操作力的方向。反之，当外力作用方向沿着末端操作器能 够运动的方向时，外力完全可以由关节力矩来平衡。
4.4 力与力矩的坐标变换
q∈R
S1
•
n
J映射
p ∈ Rm
S2
•
p=0
•
N(J) 零空间
R (J )
值域空间
JT
S3
运动学 和静力 学的对 偶性
R( J T )
值域空间
S4
N(JT) 零空间
τ ∈R
n
F ∈ Rm
利用静力和瞬时运动的对偶关系，可以把静力学问题归结为相 应的微分运动问题来研究。
力和力矩的坐标变换与微分运动坐标变换之间同样存在对偶关系 假设6维广义力矢量
WO T
− pz 0 px
P = px
[
py
pz
]
py ⎤ ⎥ − px ⎥ 0 ⎥ ⎦
T
则工具端的广义力为：
⎡ T fx ⎤ ⎡ 1 ⎢T ⎥ ⎢ 0 ⎢ fy ⎥ ⎢ ⎢T f ⎥ ⎢ 0 T = ⎢T z ⎥ = ⎢ F ⎢ mx ⎥ ⎢ 0 ⎢ T m ⎥ ⎢ pz ⎢ y⎥ ⎢ ⎢ T mz ⎥ ⎣ − p y ⎣ ⎦
4.1 连杆的受力和平衡方程
机器人是由连杆和关节（低副机构）组成，这里将机器人的 连杆当成刚体，以其中一个连杆为对象对其进行静力分析， 连杆i及其相邻连杆之间的作用力和作用力矩关系如下图。
{i}
ci i
•
{i＋1}
Mi+1
r
i +1 i
•
-fi+1
•
Mi fi
P
fi+1
-Mi+1 mig
fi
：连杆i-1作用在连杆i上的力； ：连杆i-1作用在连杆i上的力矩；
i i +1 i i +1 i +1 i
！
根据此式可进一步求出每关节的驱动力和力矩
若不考虑关节中的摩擦，旋转关节除了绕转轴的扭矩外，其余 各方向的力和力矩都由机械构件承受，为保持连杆平衡，关节 驱动力矩应等于 τi =iM T ⋅i Z i i 对于移动关节，除了沿Z轴方向的力之外，其余方向的力和力 矩都由机械构件承受，关节驱动力为
τ T ⋅ δq = F T ⋅ D
由于 D = J ⋅ δq
τT ⋅ δq = F T ⋅ J ⋅ δq
τ = F ⋅J
T T
τ = JTF
上式表明：不考虑关节之间的摩擦力，在外力F的作用下， 操作臂平衡的条件是关节力矩满足上式。
τ=J F
T
力雅可比是运动雅可比的转置 力雅可比
注意：如果雅可比J不满秩，则末端操作器在某些方向上处于 失控，不能施加所需的力和力矩，即沿这些方向的广义力可 随意变化，而不会对关节力矩的大小产生影响。
0 ⎤⎡ f B ⎤ B ⎥⎢ mB ⎥ ⎦ A R⎦ ⎣
两坐标系的 力和力矩变换公式
例： 腕力传感器测出手腕上的六维
广义力 W F ，计算工具顶端的作用力 T F
ZT ZT YW XW XT YT
令坐标系{W}到坐标系{T}的齐次变换矩阵为
⎡ WR W T =⎢ T T ⎣0 0 0
根据静力传递的关系
将外界作用力从坐标系{2}表示转换到坐标系{0}中
0
F2 = R⋅ F2
2 0 2
2 0
⎡c12 R=⎢ ⎣ s12
− s12 ⎤ c12 ⎥ ⎦
2 0
R
−1
⎡ c12 =⎢ ⎣− s12
s12 ⎤ c12 ⎥ ⎦
⎡ τ1 ⎤ ⎡l1 s 2 ⎢τ ⎥ = ⎢ 0 ⎣ 2⎦ ⎣
l1c2 + l 2 ⎤ ⎡ f x ⎤ ⎥⎢ f ⎥ l2 ⎦ ⎣ y ⎦
将 f i +1 , M i +1 表示在其所在的 坐标系{i+1}中 i 采用旋转矩阵 写成静力从一 连杆向另一连 杆传递的形式
i
i i
i i
f i = f i +1
i
M i = M i +1 + P× f i +1
i i
i +1 i
f i = i +i1R i +1f i +1 M i = R M i +1 + P× f i
4.2 等效关节力和力雅可比
⎡ fn ⎤ 将操作臂末端所受到的力和力矩组成六维矢量 Fn = ⎢ ⎥ ⎣mn ⎦ ⎡ τ1 ⎤ 终端广义 ⎢ ⎥ 力矢量 τ=⎢M ⎥ 将各关节驱动力矩组成n维矢量 ⎢τn ⎥ ⎣ ⎦
n×1
将关节驱动力矩看成操作臂驱动装置的输入，末端产生的 广义力作为操作臂的输出。采用虚功原理推导它们之间的 关系。 虚位移是满足机械系统的几何约束条件的无限小位移。 令各关节的虚位移为 δqi ，末端的虚位移为D。
w = τ T ⋅ δq = τ 1δq1 + L + τ nδqn 各关节所做的虚功之和为
末端操作器所做的虚功为 ？
w = F T ⋅ D = f x dx + f y dy + f z dz + mxδ x + m yδ y + mzδ z
根据虚功原理，操作臂平衡情况下，由任意虚位移产 生的虚功和为零。即关节空间虚位移产生的虚功等于 操作空间虚位移产生的虚功
⎡f⎤ F =⎢ ⎥ ⎣m⎦
利用虚功原理推导从坐标系 {B}的描述变换到 {A}中的描述
A B
D, D,
A B
F F
坐标系 {A}中的虚位移、作用力 坐标系 {B}中的虚位移、作用力
根据虚功原理：外力和等效力所做的虚功之和为零，可得：
A
F T ⋅ A D= BF T ⋅B D
由微分运动的坐标变换公式
F
雅可比J不满秩
奇异状态
如右图所示的平面两杆机器人处于近 奇异状态，则此时可以很小的关节力矩 克服非常大的外界作用力。
τ2 τ1
连杆1
连杆2
4.3 微分运动与静力传播的对偶性
P = J (q) q
• •
τ = J T (q) F
由力雅可比和运动雅可比之间的关系可知操作臂的静力传递 关系和速度传递关系紧密相关。
关节空间 操作空间
q∈R
•
n
J映射
p ∈ Rm p=0
•
•
零空间N(J)
R (J )
值域空间
J的值域空间表示关节运动能产生的全部操作速度的集合
q∈R
•
n
J映射
p ∈ Rm p=0
•
•
速度：
N(J) 零空间
R (J )
值域空间
JT 静力：
R( J T )
值域空间
N(JT) 零空间
τ ∈ Rn
F ∈ Rm
0 0 1 0 0 1 − pz py − px 0 px 0
0 1 0 0 1 0 0
⎡ w fx ⎤ ⎤ ⎥⎢ w f ⎥ ⎥⎢ y ⎥ ⎥⎢ w fz ⎥ ⎢ ⎥ 0⎥ ⎢ w m ⎥ x ⎥ 0⎥ ⎢ w m ⎥ ⎥⎢ w y ⎥ 1⎦ ⎢ m ⎥ ⎣ z⎦
4.5 静力学的逆问题
i = i, i + 1
Mi
mi g
ci ：连杆i的重力，作用在质心上； i
r ：连杆i上的质心位置；
{i}
ci i
•
{i＋1}
Mi+1
r
i +1 i
•
-fi+1
•
Mi fi
i +1 i
P
fi+1
-Mi+1 mig
P ：表示坐标系{i+1}的原点相对于坐标系{i}的表示；
连杆i处于平衡状态时，所受合力为零，力平衡方程为