一 间接力控 Indirect Force Control
处理机械臂的末端执行器与环境之间相互作用问题。
约定:
定义 | 描述 |
---|---|
Σ e \Sigma_e Σe | 末端执行器坐标系 |
p e p_e pe | 相对于坐标系e原点的位置矢量 |
R e R_e Re | 相对于坐标系e的旋转矩阵 |
v e = ( p ˙ e T w e T ) T v_e=(\dot{p}_e^T w_e^T)^T ve=(p˙eTweT)T | 末端执行器的刚体广义速度/运动旋量 |
p ˙ e \dot{p}_e p˙e | 位移矢量的倒数,即速度 |
w e w_e we | 角速度 |
q ˙ \dot q q˙ | 关节角速度 |
J J J | 末端执行器几何雅克比矩阵 |
v e = J ( q ) q ˙ v_e=J(q)\dot q ve=J(q)q˙ | 速度与关节角速度的线性映射 |
h e = ( f e T m e T ) T h_e=(f_e^T m_e^T)^T he=(feTmeT)T | 力旋量(wrench):由作用在末端执行器上的力 f f f和力矩 m m m组成 |
v e = ( p ˙ e T w e T ) T v_e=(\dot{p}_e^T w_e^T)^T ve=(p˙eTweT)T表达方式的作用或者意义? 定义一个向量 x = ( x 1 , x 2 , x 3 ) T ∈ R 3 x=(x_1,x_2,x_3)^T\in \mathbb{R}^3 x=(x1,x2,x3)T∈R3,那么我们有如下定义 [ x ] = [ 0 − x 3 x 2 x 3 0 − x 1 − x 2 x 1 0 ] ∈ S O ( 3 ) [x]=\left[ \begin{matrix} 0 & -x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{matrix} \right] \in SO(3) [x]= 0x3−x2−x30x1x2−x10 ∈SO(3) [ x ] [x] [x]称为 x x x的反对称矩阵(skew-symmetric matrix)。 反对称矩阵的主要性质为
[ x ] = − [ x ] T [x]=-[x]^T [x]=−[x]T
反对称矩阵在刚体角速度描述中的体现:该文1.7节我们现在考虑运动坐标系的线速度和角速度。 s {s} s和 b {b} b分别表示固定(space)坐标系和移动(body)坐标系帧。齐次坐标变换为
T s b ( t ) = T ( t ) = [ R ( t ) p ( t ) 0 1 ] T_{sb}(t)=T(t)=\left[ \begin{matrix} R(t) & p(t) \\ 0 & 1 \\ \end{matrix} \right] Tsb(t)=T(t)=[R(t)0p(t)1] 根据反对称矩阵的性质,对于旋转矩阵 R R R有 S = R ˙ R − 1 S=\dot RR^{-1} S=R˙R−1 S S S这里的反对称矩阵在刚体运动中通常称为角速度矩阵。 同样的对于 T T T也有如下性质 T − 1 T ˙ = [ R T − R T p 0 1 ] [ R ˙ p ˙ 0 0 ] = [ R T R ˙ R T p ˙ 0 0 ] = [ [ w b ] v b 0 0 ] T^{-1}\dot T=\left[ \begin{matrix} R^T& -R^Tp \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \dot R& \dot p \\ 0 & 0 \\ \end{matrix} \right] =\left[ \begin{matrix} R^T\dot R& R^T\dot p \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} [w_b]&v_b \\ 0 & 0 \\ \end{matrix} \right] T−1T˙=[RT0−RTp1][R˙0p˙0]=[RTR˙0RTp˙0]=[[wb]0vb0] 分析上式可以观察得出一下结论
R T R ˙ = [ w b ] R^T\dot R=[w_b] RTR˙=[wb]是角速度矢量 w b = ( w x , w y , w z ) T w_b=(w_x,w_y,w_z)^T wb=(wx,wy,wz)T在坐标系 b {b} b中以反对称矩阵表示。
R T p ˙ = v b R^T\dot p=v_b RTp˙=vb是线速度矢量在坐标系 b {b} b中的表示。 那么 [ w b ] [w_b] [wb]和 v b v_b vb就可以通过 T − 1 T ˙ T^{-1}\dot T T−1T˙联系在一起描述和计算。 数学就是这么奇妙,角速度和线速度的联合也是后续动力学一些重要思想的基础,比如力旋量这个概念 w r e n c h wrench wrench.
对于运动坐标系 ( b o d y ) (body) (body) b {b} b,我们定义有 V b = [ w b v b ] ∈ R 6 \mathcal{V}_b=\left[ \begin{matrix} w_b \\ v_b \\ \end{matrix} \right]\in \mathbb{R}^6 Vb=[wbvb]∈R6
可以称作spatial velocity in the body frame或者body twist 同理, T − 1 T ˙ = [ V b ] = [ [ w b ] v b 0 0 ] ∈ S E ( 3 ) T^{-1}\dot T=[\mathcal{V}_b]= \left[ \begin{matrix} [w_b]&v_b \\ 0 & 0 \\ \end{matrix} \right] \in SE(3) T−1T˙=[Vb]=[[wb]0vb0]∈SE(3) 其中, [ w b ] ∈ S O ( 3 ) , v b ∈ R 3 [w_b]\in SO(3),v_b\in \mathbb{R}^3 [wb]∈SO(3),vb∈R3
至此,任何刚体运动都可以通过六个量来表示,即三个线速度和三个角速度,并且两者可以有联系地放在一起描述。
T ˙ T − 1 = [ R ˙ p ˙ 0 0 ] [ R T − R T p 0 1 ] = [ R ˙ R T p ˙ − R ˙ R T p 0 0 ] = [ [ w s ] v s 0 0 ] \dot TT^{-1}= \left[ \begin{matrix} \dot R& \dot p \\ 0 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} R^T& -R^Tp \\ 0 & 1 \\ \end{matrix} \right] =\left[ \begin{matrix} \dot RR^T&\dot p- \dot RR^Tp \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} [w_s]&v_s \\ 0 & 0 \\ \end{matrix} \right] T˙T−1=[R˙0p˙0][RT0−RTp1]=[R˙RT0p˙−R˙RTp0]=[[ws]0vs0]
上式表示的是 s s s坐标系下的速度与角速度,但这里的 v s v_s vs不是直接对位移量 p p p求导,因为 p p p是相对于 v b v_b vb而言的位移量,故
v S = p ˙ − R ˙ R T p = p ˙ − w s × p = p ˙ + w s × ( − p ) v_S=\dot p- \dot RR^Tp=\dot p-w_s\times p=\dot p + w_s\times(-p) vS=p˙−R˙RTp=p˙−ws×p=p˙+ws×(−p)
反映的是运动物体上的点相对于静止坐标系的描述。 V s = [ w s v s ] ∈ R 6 \mathcal{V}_s=\left[ \begin{matrix} w_s \\ v_s \\ \end{matrix} \right]\in \mathbb{R}^6 Vs=[wsvs]∈R6 [ V s ] = [ [ w s ] v s 0 0 ] ∈ S E ( 3 ) [\mathcal{V}_s]= \left[ \begin{matrix} [w_s]&v_s \\ 0 & 0 \\ \end{matrix} \right] \in SE(3) [Vs]=[[ws]0vs0]∈SE(3)
可以称作spatial velocity in the space frame或者spatial twist
那么,可以推导出两个坐标系之间的运动旋量之间的转换关系
[ V b ] = T − 1 T ˙ = T − 1 [ V s ] T [\mathcal{V}_b]=T^{-1} \dot T=T^{-1} [\mathcal{V}_s]T [Vb]=T−1T˙=T−1[Vs]T
[ V s ] = T [ V s ] T − 1 [\mathcal{V}_s]=T [\mathcal{V}_s]T^{-1} [Vs]=T[Vs]T−1
展开 [ V s ] [\mathcal{V}_s] [Vs]可以得到
[ V s ] = [ R [ w b ] R T − R [ w b ] R T p + R v b 0 0 ] [\mathcal{V}_s]= \left[ \begin{matrix} R[w_b]R^T& -R[w_b]R^Tp+Rv_b \\ 0 & 0 \\ \end{matrix} \right] [Vs]=[R[wb]RT0−R[wb]RTp+Rvb0] 变形得到 V s \mathcal{V}_s Vs和 V b \mathcal{V}_b Vb之间的转换关系 [ w s v s ] = [ R 0 [ p ] R R ] [ w b v b ] \left[ \begin{matrix} w_s \\ v_s \\ \end{matrix} \right] =\left[ \begin{matrix} R&0 \\ [p]R & R \\ \end{matrix} \right] \left[ \begin{matrix} w_b \\ v_b \\ \end{matrix} \right] [wsvs]=[R[p]R0R][wbvb] 关于这个转换矩阵,我们有了新的定义 对于给定的 T = ( R , p ) ∈ S E ( 3 ) T=(R,p)\in SE(3) T=(R,p)∈SE(3),其伴随矩阵(adjoint representation) [ A d T ] [Ad_T] [AdT]定义如下 [ A d T ] = [ R 0 [ p ] R R ] ∈ R 6 × 6 [Ad_T]= \left[ \begin{matrix} R&0 \\ [p]R & R \\ \end{matrix}\right]\in \mathbb{R}^{6\times6} [AdT]=[R[p]R0R]∈R6×6 且 V ′ = [ A d T ] V \mathcal{V}'= [Ad_T]\mathcal{V} V′=[AdT]V 也可以写成
V ′ = A d T ( V ) \mathcal{V}'= Ad_T(\mathcal{V}) V′=AdT(V)
定义一个坐标系 a {a} a,一个线性力作用于刚体上的某点 r r r, r a ∈ R 3 r_a\in \mathbb{R}^3 ra∈R3,则力 f a ∈ R 3 f_a\in \mathbb{R}^3 fa∈R3,则扭矩或者力矩可以写作 m a = r a × f a m_a=r_a\times f_a ma=ra×fa,与速度旋量一样的表达方式,我们可以约定 F s = [ m a f a ] ∈ R 6 \mathcal{F}_s=\left[ \begin{matrix} m_a \\ f_a \\ \end{matrix} \right]\in \mathbb{R}^6 Fs=[mafa]∈R6
可以称作spatial force或者wrench,力旋量。
那么相对于 3 × 1 3\times1 3×1的速度/角速度和力/力矩表达方式, 6 × 1 6\times1 6×1的力旋量和速度旋量这种表达方式先进性体现在哪里?
首先,假设坐标系 a a a到坐标系 b b b的坐标变化 T T T已知,如果要进行不同坐标系之间力的表达变化,我们可能要进行繁琐的推导,但是有一种更加高效简便的推导方法。
这边我们考虑到功率,通常与速度和力的乘积有关,并且功率是标量,在同一个系统中,不管是在哪个坐标系表示,系统的总能量总是保持不变的,那么可以用数学公式表示为
V b T F b = V a T F a \mathcal{V}_b^T\mathcal{F}_b =\mathcal{V}_a^T\mathcal{F}_a VbTFb=VaTFa
V b T F b = ( [ A d T a b ] V b ) T F a = V b T ( [ A d T a b ] ) T F a \mathcal{V}_b^T\mathcal{F}_b =([Ad_{T_{ab}}]\mathcal{V}_b)^T\mathcal{F}_a=\mathcal{V}_b^T([Ad_{T_{ab}}])^T\mathcal{F}_a VbTFb=([AdTab]Vb)TFa=VbT([AdTab])TFa
化简得到 F b = [ A d T a b ] T F a = A d T a b T ( F a ) \mathcal{F}_b =[Ad_{T_{ab}}]^T\mathcal{F}_a=Ad_{T_{ab}}^T(\mathcal{F}_a) Fb=[AdTab]TFa=AdTabT(Fa)
F a = [ A d T b a ] T F b = A d T b a T ( F b ) \mathcal{F}_a =[Ad_{T_{ba}}]^T\mathcal{F}_b=Ad_{T_{ba}}^T(\mathcal{F}_b) Fa=[AdTba]TFb=AdTbaT(Fb)
通常,使用固定坐标系 fixed space frame s s s和运动坐标系body frame b b b,
即spatial wrench F s \mathcal{F}_s Fs和body wrench F b \mathcal{F}_b Fb
在关节空间中,刚体动力学方程的形式如下
τ
−
τ
e
=
M
(
q
)
q
¨
+
C
(
q
,
q
˙
)
q
˙
+
g
(
q
)
\tau-\tau_e=M(q)\ddot q+C(q,\dot q)\dot q+g(q)
τ−τe=M(q)q¨+C(q,q˙)q˙+g(q)
对于六轴机械臂,
q
˙
,
q
¨
∈
R
6
\dot q,\ddot q \in \mathbb{R}^6
q˙,q¨∈R6分别是关节空间下的角速度和角加速度;
M
(
q
)
∈
R
6
×
6
M(q)\in \mathbb{R}^{6\times6}
M(q)∈R6×6是质量矩阵,对称正定惯性矩阵;
C
(
q
)
∈
R
6
×
6
C(q)\in \mathbb{R}^{6\times6}
C(q)∈R6×6是离心力和哥氏力矩阵;
g
(
q
)
∈
R
6
g(q)\in \mathbb{R}^{6}
g(q)∈R6是重力矢量;
τ
∈
R
6
\tau\in \mathbb{R}^{6}
τ∈R6是关节转矩矢量;
τ
e
∈
R
6
\tau_e\in \mathbb{R}^{6}
τe∈R6是作用在末端执行器上的外部力旋量对应的关节转矩矢量。
对应到笛卡尔空间下的刚体动力学方程
h
c
−
h
e
=
Λ
(
q
)
v
˙
e
+
Γ
(
q
,
q
˙
)
v
e
+
η
(
q
)
h_c-h_e=\Lambda(q)\dot v_e+\Gamma(q,\dot q)v_e+\eta(q)
hc−he=Λ(q)v˙e+Γ(q,q˙)ve+η(q)
作用在末端执行器上的虚拟力
F
\mathcal F
F实际上可以用关节驱动器的驱动力表示,即
τ
=
J
T
(
q
)
h
\tau=J^T(q)h
τ=JT(q)h
关节空间和笛卡尔空间之间的可以进过如下变化得到
τ
−
τ
e
=
M
(
q
)
q
¨
+
C
(
q
,
q
˙
)
q
˙
+
g
(
q
)
\tau-\tau_e=M(q)\ddot q+C(q,\dot q)\dot q+g(q)
τ−τe=M(q)q¨+C(q,q˙)q˙+g(q)
J
−
T
τ
−
J
−
T
τ
e
=
J
−
T
M
(
q
)
q
¨
+
J
−
T
C
(
q
,
q
˙
)
q
˙
+
J
−
T
g
(
q
)
J^{-T}\tau-J^{-T}\tau_e=J^{-T}M(q)\ddot q+J^{-T}C(q,\dot q)\dot q+J^{-T}g(q)
J−Tτ−J−Tτe=J−TM(q)q¨+J−TC(q,q˙)q˙+J−Tg(q)
将
v
e
=
J
q
˙
,
v
˙
e
=
J
˙
q
˙
+
J
q
¨
v_e=J\dot q,\dot v_e=\dot J\dot q+J\ddot q
ve=Jq˙,v˙e=J˙q˙+Jq¨,即
q
¨
=
J
−
1
v
˙
e
−
J
−
1
J
˙
q
˙
\ddot q=J^{-1}\dot v_e-J^{-1}\dot J\dot q
q¨=J−1v˙e−J−1J˙q˙,代入得到
J
−
T
τ
−
J
−
T
τ
e
=
J
−
T
M
(
q
)
J
−
1
v
˙
e
−
J
−
T
M
(
q
)
J
−
1
J
˙
q
˙
+
J
−
T
C
(
q
,
q
˙
)
J
−
1
v
˙
e
+
J
−
T
g
(
q
)
J^{-T}\tau-J^{-T}\tau_e=J^{-T}M(q)J^{-1}\dot v_e-J^{-T}M(q)J^{-1}\dot J\dot q+J^{-T}C(q,\dot q)J^{-1}\dot v_e+J^{-T}g(q)
J−Tτ−J−Tτe=J−TM(q)J−1v˙e−J−TM(q)J−1J˙q˙+J−TC(q,q˙)J−1v˙e+J−Tg(q)
那么,记
Λ
(
q
)
=
J
−
T
M
(
q
)
J
−
1
\Lambda(q)=J^{-T}M(q)J^{-1}
Λ(q)=J−TM(q)J−1表示笛卡尔空间下的惯性矩阵;
Γ
(
q
,
q
˙
)
=
J
−
T
C
(
q
,
q
˙
)
J
−
1
−
Λ
(
q
)
J
−
1
J
˙
\Gamma(q,\dot q)=J^{-T}C(q,\dot q)J^{-1}-\Lambda(q)J^{-1}\dot J
Γ(q,q˙)=J−TC(q,q˙)J−1−Λ(q)J−1J˙表示包含离心力和科氏力影响的旋量;
η
(
q
)
=
J
−
T
g
(
q
)
\eta(q)=J^{-T}g(q)
η(q)=J−Tg(q)表示重力影响的旋量;
以上是关节空间和笛卡尔空间之间刚体动力学的转化。
笛卡尔空间下的动力学方程表示为:
h
c
−
h
e
=
Λ
(
q
)
v
˙
+
Γ
(
q
,
q
˙
)
v
˙
+
η
(
q
)
(1)
h_c-h_e=\Lambda(q)\dot v+\Gamma(q,\dot q)\dot v+\eta(q) \tag{1}
hc−he=Λ(q)v˙+Γ(q,q˙)v˙+η(q)(1)
1 刚度控制(Stiffness Control)
末端执行器的位姿可以用 x e = ( p e T , ϕ e T ) T x_e=(p_e^T,\phi_e^T)^T xe=(peT,ϕeT)T,同样可以用 x d x_d xd表示末端执行器期望位姿。
2 导纳控制
阻抗控制在机器人动力学的基础上,在加速度层面上对机器人动力学进行解耦和建模。当机器人与环境交互时,阻抗控制方程描述为:
h
c
=
Λ
(
q
)
α
+
Γ
(
q
,
q
˙
)
v
˙
+
h
e
(2)
h_c=\Lambda(q)\alpha+\Gamma(q,\dot q)\dot v+h_e \tag{2}
hc=Λ(q)α+Γ(q,q˙)v˙+he(2)
那么其实
α
=
v
˙
\alpha=\dot v
α=v˙
α
\alpha
α的物理意义是相对于基座标系的末端执行器加速度旋量,是阻抗控制律的控制量。可以设置控制律:
α
=
v
˙
d
+
M
d
−
1
[
B
d
(
v
d
−
v
e
)
+
h
Δ
−
h
e
]
(3)
\alpha=\dot v_d+M_d^{-1}[B_d(v_d-v_e)+h_{\Delta}-h_e] \tag{3}
α=v˙d+Md−1[Bd(vd−ve)+hΔ−he](3)
M
d
(
v
˙
d
−
v
˙
e
)
+
B
d
(
v
d
−
v
e
)
+
h
Δ
=
h
e
(4)
M_d(\dot v_d-\dot v_e)+B_d(v_d-v_e)+h_{\Delta}=h_e \tag{4}
Md(v˙d−v˙e)+Bd(vd−ve)+hΔ=he(4)
其中,
M
d
,
B
d
M_d,B_d
Md,Bd分别表示惯性矩阵和阻尼矩阵,
h
e
h_e
he表示与环境接触的弹性力旋量,
h
Δ
h_{\Delta}
hΔ由期望坐标系
Σ
d
\Sigma_d
Σd相对于末端执行器坐标
Σ
e
\Sigma_e
Σe的位姿变化产生的。在与刚性环境接触时,公式(4)转化为:
M
d
(
v
˙
d
−
v
˙
e
)
+
B
d
(
v
d
−
v
e
)
+
K
d
(
x
d
−
x
e
)
=
h
d
−
h
e
(5)
M_d(\dot v_d-\dot v_e)+B_d(v_d-v_e)+K_d(x_d-x_e)=h_d-h_e \tag{5}
Md(v˙d−v˙e)+Bd(vd−ve)+Kd(xd−xe)=hd−he(5)
其中,
D
d
D_d
Dd表示刚度矩阵,
x
d
,
x
e
x_d,x_e
xd,xe分别表示期望位姿和实际位姿。将位姿偏差量
(
x
d
−
x
e
)
(x_d-x_e)
(xd−xe)和末端力误差量
(
h
d
−
h
e
)
(h_d-h_e)
(hd−he)作为导纳控制的输入,为了得到导纳控制的输出量
α
\alpha
α,公式(5)可以转化为:
α
=
v
˙
d
+
M
d
−
1
[
(
h
d
−
h
e
)
⏟
力空间相关量
+
B
d
(
v
d
−
v
e
)
+
K
d
(
x
d
−
x
e
)
⏟
位姿空间相关量
]
(6)
\alpha=\dot v_d+M_d^{-1}[\underbrace{(h_d-h_e)}_\text{力空间相关量}+\underbrace{B_d(v_d-v_e)+K_d(x_d-x_e)}_\text{位姿空间相关量}] \tag{6}
α=v˙d+Md−1[力空间相关量
(hd−he)+位姿空间相关量
Bd(vd−ve)+Kd(xd−xe)](6)
在每个控制周期内对
α
\alpha
α进行积分和二次积分分别可以得到期望速度
v
d
v_d
vd和期望位姿
x
d
x_d
xd,经过逆运动学控制机器人达到期望位姿,实现对机器人位姿的导纳控制,伪代码如表所示,其中导纳参数
K
d
,
B
d
,
M
d
K_d,B_d,M_d
Kd,Bd,Md需要根据经验和实验整定,其他输入参数均为实时获取或计算得到。
位姿导纳控制 |
---|
输入:导纳参数 K d , B d , M d K_d,B_d,M_d Kd,Bd,Md,期望速度旋量 v d v_d vd,期望位姿 x d x_d xd,实际位姿 x d x_d xd,实际力旋量 h e h_e he |
输出: 机器人末端位姿 |
1: 计算期望位姿与实际位姿的偏差量 e r r = x d − x e err =x_d-x_e err=xd−xe |
2: 计算式(6)中的位姿空间相关量 w r e n c h p = B d ( v d − v e ) + K d ( x d − x e ) wrench_p=B_d(v_d-v_e)+K_d(x_d-x_e) wrenchp=Bd(vd−ve)+Kd(xd−xe) |
3: 计算式(6)中的期望加速度 α d = M d − 1 ( h d − h e + w r e n c h p ) \alpha_d=M_d^{-1}(h_d-h_e+wrench_p) αd=Md−1(hd−he+wrenchp) |
4: 设置期望加速度阈值 |
5: 对期望加速度积分得到期望速度旋量 v d v_d vd并纪录用于下一轮迭代 |
6: 对期望速度旋量 v d v_d vd积分得到期望位姿 x d x_d xd并纪录用于下一轮迭代 |
7: 设置期望位姿阈值 |
8: return x d x_d xd |