本系列是我学习机器人动力学的讲义,我将它搬运到博客中。这系列文章默认读者已经掌握机器人运动学相关的知识。
主要内容计划安排如下,专栏地址:微鉴道长机器人动力学专栏
后文内容不定期更新,欢迎关注:

【机器人动力学】(一):刚体惯性与动能
【机器人动力学】(二):机械臂动力学方程与计算
【机器人动力学】(三):力矩控制与接触交互
【机器人动力学】(四):动力学回归与最小参数集
【机器人动力学】(五):最优激励与测量数据处理
【机器人动力学】(六):本体、摩擦与工具参数辨识

第六章:本体、摩擦与工具参数辨识

第四章说明了如何从运动和力矩建立参数回归,第五章介绍了轨迹设计与数据处理。本章把这些方法用于具体的辨识任务:先通过多姿态保持估计本体重力参数,再通过双向滑动分离摩擦,随后利用腕部六维传感器标定工具的质量、质心和转动惯量,最后将各部分合成完整模型。

这些实验通过改变运动条件,突出不同参数的作用。静止时主要观察重量和力臂;稳定滑动时观察摩擦随方向、速度的变化;转动加减速时观察惯量。

6.1 本体重力参数与测量偏置

由多姿态力矩估计重力参数

让机械臂在多个姿态下保持静止,工具和连杆都与环境脱离接触,记录关节位置及力矩。将力矩测量统一到关节侧,并沿用第二章的驱动力矩符号,单次观测可写为:

τ m = g ( q ) + τ s t a t i c + b + ε . \tau_m=g(q)+\tau_{\mathrm{static}}+b+\varepsilon. τm​=g(q)+τstatic​+b+ε.

τ m \tau_m τm​ 是测得的关节力矩, g ( q ) g(q) g(q) 是当前姿态下平衡重力所需的力矩, τ s t a t i c \tau_{\mathrm{static}} τstatic​ 表示静摩擦引起的驱动力矩变化,符号沿用第二章的摩擦约定; b b b 是各测量通道的常量零点偏置, ε \varepsilon ε 是测量噪声。这些向量中,每个关节各占一个分量,单位为 N·m。

重力力矩取决于各连杆的质量,以及质心到关节轴的力臂。例如,肩关节支撑着上臂、前臂、腕部和工具,它的保持力矩包含这些部分的共同贡献。改变肩、肘和腕的姿态,各部分的力臂随之改变,多姿态数据便提供了区分这些贡献的依据。

沿用第四章的参数形式,每根连杆取质量 m i m_i mi​ 和一阶质量矩 h m , i = m i r C , i h_{m,i}=m_i r_{C,i} hm,i​=mi​rC,i​ 的三个分量,组成四个重力相关参数。其中 r C , i r_{C,i} rC,i​ 从连杆原点指向质心, m i m_i mi​ 的单位为 kg, h m , i h_{m,i} hm,i​ 的单位为 kg·m。将所有连杆的这四个参数堆叠为 π g \pi_g πg​,重力模型写成:

g ( q ) = Y g ( q ) π g . g(q)=Y_g(q)\pi_g. g(q)=Yg​(q)πg​.

Y g ( q ) Y_g(q) Yg​(q) 是这一姿态的重力回归矩阵。它可以由第四章的回归矩阵在速度、加速度置零后,取出质量和一阶质量矩对应的列得到。静态辨识集中估计这些参数;六个转动惯量分量留待动态实验估计,或采用 CAD 先验。

采集 N N N 个姿态后,将测量与回归矩阵逐帧叠放,并给每个关节增加一个偏置参数:

y g = W g π g + B b + ε g , y g = [ τ m , 1 ⋮ τ m , N ] , W g = [ Y g ( q 1 ) ⋮ Y g ( q N ) ] , B = [ I n ⋮ I n ] . \begin{aligned} y_g&=W_g\pi_g+B b+\varepsilon_g,\\ y_g&=\begin{bmatrix}\tau_{m,1}\\\vdots\\\tau_{m,N}\end{bmatrix},\qquad W_g=\begin{bmatrix}Y_g(q_1)\\\vdots\\Y_g(q_N)\end{bmatrix},\qquad B=\begin{bmatrix}I_n\\\vdots\\I_n\end{bmatrix}. \end{aligned} yg​yg​​=Wg​πg​+Bb+εg​,= ​τm,1​⋮τm,N​​ ​,Wg​= ​Yg​(q1​)⋮Yg​(qN​)​ ​,B= ​In​⋮In​​ ​.​

n n n 是关节数, I n I_n In​ 是 n n n 阶单位矩阵, B B B 由 N N N 个 I n I_n In​ 纵向叠成,将同一组偏置 b b b 加到每帧测量中。 ε g \varepsilon_g εg​ 汇总静摩擦、噪声和其他残余误差。对七轴、七连杆模型, π g \pi_g πg​ 有 28 个分量, W g W_g Wg​ 为 7 N × 28 7N\times28 7N×28, B B B 为 7 N × 7 7N\times7 7N×7。待求的是 π g \pi_g πg​ 与 b b b,可按第四章的方法处理相关列,再结合先验求解。

多姿态的作用在这里很直观:重力回归系数随姿态变化,偏置列始终保持相同。充分改变力臂有助于区分二者。某些参数仍会以固定组合出现,这些依赖关系同样可以通过第四章的秩分析确认。

静摩擦与测量尺度的影响

静止的关节中,摩擦可以在一定范围内随受力变化,帮助维持平衡。以单关节为例,若保持位置所需的力矩有一部分由静摩擦承担,传感器读到的驱动力矩就会相应减小。从相反方向到达同一姿态时,摩擦和传动弹性留下的作用也可能不同。因此,采集时可以从多个方向缓慢到位,等待稳定后重复记录;同一姿态各组读数之间的差异,有助于判断这部分误差的大小。下一节利用持续滑动时更明确的摩擦方向,进一步分离重力与摩擦。

姿态选择还要结合关节轴方向。例如,机械臂第一关节绕世界竖直轴转动时,单独改变该角度会保持各连杆质心的高度,势能对这个角度的导数为零,该轴理想重力力矩也为零。这类关节的惯量信息主要来自动态运动。

若力矩由电流换算,还需要确定换算尺度。先扣除偏置与摩擦,并将各通道已知的相对换算比例计入电流向量 i i i;假设剩余一个共同的未知换算系数 k I k_I kI​,则有:

k I i = W g π g . k_I i=W_g\pi_g. kI​i=Wg​πg​.

i i i 按姿态堆叠,单位为 A; k I k_I kI​ 的单位为 N·m/A,包含电机及传动侧的换算。将 k I k_I kI​ 和 π g \pi_g πg​ 同时放大相同倍数,等式仍然成立。因此,已知工具质量、独立力矩标定或可靠的质量先验,需要为这组参数提供绝对尺度。

6.2 关节摩擦的分离与拟合

用双向、多速度运动分离摩擦

先固定其他关节,让一个转动关节往复运动,用标量 q q q 表示它的角度。稳定滑动时,一个常用模型为:

τ f = b v q ˙ + τ c sign ⁡ ( q ˙ ) . \tau_f=b_v\dot q+\tau_c\operatorname{sign}(\dot q). τf​=bv​q˙​+τc​sign(q˙​).

b v b_v bv​ 是黏性摩擦系数,单位为 N·m·s/rad,表示随速度增大的阻力; τ c \tau_c τc​ 是库仑摩擦力矩的非负幅值,单位为 N·m,表示滑动时近似固定的一部分阻力。 sign ⁡ ( q ˙ ) \operatorname{sign}(\dot q) sign(q˙​) 在正、负速度下分别取 + 1 , − 1 +1,-1 +1,−1。这里 τ f \tau_f τf​ 表示驱动器克服摩擦所需的力矩,实际摩擦作用方向与它相反。

在同一关节位置附近,分别取速度为 + v +v +v 和 − v -v −v 的样本, v > 0 v>0 v>0 表示速度大小。固定其他关节后,运动关节在加减速阶段仍需要惯性力矩,因此这里选取低速匀速段,使加速度项与速度耦合项足够小。此时两次观测的重力相同,摩擦方向相反,近似有:

τ ( + v ) ≈ g ( q ) + b 0 + b v v + τ c , τ ( − v ) ≈ g ( q ) + b 0 − b v v − τ c . \begin{aligned} \tau(+v)&\approx g(q)+b_0+b_vv+\tau_c,\\ \tau(-v)&\approx g(q)+b_0-b_vv-\tau_c. \end{aligned} τ(+v)τ(−v)​≈g(q)+b0​+bv​v+τc​,≈g(q)+b0​−bv​v−τc​.​

τ ( + v ) , τ ( − v ) \tau(+v),\tau(-v) τ(+v),τ(−v) 是该关节的两次测量力矩, b 0 b_0 b0​ 是这个通道的常量偏置, g ( q ) g(q) g(q) 表示该关节在共同姿态下的重力力矩。将两式相加、相减,得到:

τ ( + v ) + τ ( − v ) 2 ≈ g ( q ) + b 0 , τ ( + v ) − τ ( − v ) 2 ≈ b v v + τ c . \frac{\tau(+v)+\tau(-v)}{2}\approx g(q)+b_0,\qquad \frac{\tau(+v)-\tau(-v)}{2}\approx b_vv+\tau_c. 2τ(+v)+τ(−v)​≈g(q)+b0​,2τ(+v)−τ(−v)​≈bv​v+τc​.

半差直接消去了共同的重力与偏置,因此初次辨识时,可以先通过双向配对估计摩擦,再用多个姿态下的低速配对平均值估计重力参数与偏置。这样就能从实验数据逐步建立模型。配对时还应保持负载、温度等条件接近。

更一般地,两次采样的姿态、加速度相同,速度等幅反向时,共同的刚体动态力矩也会在半差中抵消。平均值中仍保留这部分贡献,所以用平均值估计重力时,应选取动态作用足够小的运动段。

对多个速度大小分别求半差,就得到“速度大小—摩擦力矩”的一组点。用直线拟合这些点,斜率给出 b v b_v bv​,截距给出 τ c \tau_c τc​。多个速度档提供了区分这两个参数的信息。

已有较可靠的重力与动态模型时,还可以从测量中扣除这些模型预测的力矩,再联合拟合摩擦与偏置。将剩余力矩记为 r r r,有:

r = b v q ˙ + τ c sign ⁡ ( q ˙ ) + b 0 . r=b_v\dot q+\tau_c\operatorname{sign}(\dot q)+b_0. r=bv​q˙​+τc​sign(q˙​)+b0​.

例如,在两个不同的速度大小 v 1 , v 2 > 0 v_1,v_2>0 v1​,v2​>0 下分别采集正、反向数据,四类样本写成:

[ r + 1 r − 1 r + 2 r − 2 ] = [ v 1 1 1 − v 1 − 1 1 v 2 1 1 − v 2 − 1 1 ] [ b v τ c b 0 ] . \begin{bmatrix}r_{+1}\\r_{-1}\\r_{+2}\\r_{-2}\end{bmatrix} =\begin{bmatrix} v_1&1&1\\-v_1&-1&1\\v_2&1&1\\-v_2&-1&1 \end{bmatrix} \begin{bmatrix}b_v\\\tau_c\\b_0\end{bmatrix}. ​r+1​r−1​r+2​r−2​​ ​= ​v1​−v1​v2​−v2​​1−11−1​1111​ ​ ​bv​τc​b0​​ ​.

r + 1 , r − 1 r_{+1},r_{-1} r+1​,r−1​ 对应速度 + v 1 , − v 1 +v_1,-v_1 +v1​,−v1​, r + 2 , r − 2 r_{+2},r_{-2} r+2​,r−2​ 对应 + v 2 , − v 2 +v_2,-v_2 +v2​,−v2​。第一列随速度变化,第二列随方向变号,第三列始终为 1。当 v 1 ≠ v 2 v_1\ne v_2 v1​=v2​ 时,三列独立,可以求出这三个参数。只取正向数据时,后两列相同;只取同一个速度大小时,第一列与第二列成比例。这说明双向和多速度各自提供了必要的信息。

在这里插入图片描述

图中正负两段直线的斜率对应 b v b_v bv​,两段向零速延伸后的截距之差为 2 τ c 2\tau_c 2τc​,两个截距的平均值为 b 0 b_0 b0​;零速处的阴影表示静摩擦作用下读数的可能范围。采集更多速度档后,可以用最小二乘估计这些量,并约束 b v , τ c ≥ 0 b_v,\tau_c\geq0 bv​,τc​≥0。

低速变化与摩擦补偿

有些传动在刚开始滑动时阻力较大,速度提高后摩擦先下降,再逐渐由黏性作用主导。这种低速下降现象称为 Stribeck 效应,可用下式描述:

τ f = [ τ c + ( τ s − τ c ) e − ( ∣ q ˙ ∣ / v s ) p ] sign ⁡ ( q ˙ ) + b v q ˙ . \tau_f=\left[\tau_c+(\tau_s-\tau_c)e^{-(|\dot q|/v_s)^p}\right] \operatorname{sign}(\dot q)+b_v\dot q. τf​=[τc​+(τs​−τc​)e−(∣q˙​∣/vs​)p]sign(q˙​)+bv​q˙​.

τ s \tau_s τs​ 是接近零速滑动时的摩擦幅值,单位为 N·m,通常取 τ s ≥ τ c \tau_s\geq\tau_c τs​≥τc​; v s > 0 v_s>0 vs​>0 是下降过程的特征速度,单位为 rad/s; p > 0 p>0 p>0 是控制曲线形状的无量纲指数。速度从零附近增大时,指数项逐渐减小,括号内的摩擦幅值便从 τ s \tau_s τs​ 过渡到 τ c \tau_c τc​。

实际辨识可以先拟合库仑加黏性模型,再根据低速数据是否出现明显的下降趋势,决定是否加入这几个参数。长时间静止与换向附近的样本受静摩擦、弹性和运动历史影响较大,宜单独分析;稳定滑动段更适合拟合上述速度模型。温度与负载也会改变摩擦,采样条件应覆盖后续使用范围。

将模型用于控制时,前馈补偿会减轻传动阻力。补偿偏大则可能在换向附近推动关节反复运动。因而应结合低速跟踪和换向表现调整补偿量,并为零速附近设置平滑过渡。拟合效果与实际运动手感共同决定模型的适用范围。

6.3 工具质量、质心与六维偏置

腕部六维力传感器提供三个力分量和三个力矩分量。让工具在几个朝向下保持静止,并与环境脱离接触,就可以利用重力同时估计工具质量、质心和六个通道的零偏。这里的“工具”包含传感器测量截面外侧随末端一起运动的负载,例如转接件、夹具和夹持物。

把工具重量写成线性回归

以传感器测力参考点为原点 S S S,所有向量用传感器固连坐标轴表达。将传感器符号统一为“工具作用在传感器上的载荷”,静止时读到的是工具重量及其力矩,再加测量偏置。

令 m m m 为工具质量, r C r_C rC​ 为从 S S S 指向工具质心的向量, h m = m r C h_m=mr_C hm​=mrC​ 为一阶质量矩;单位分别为 kg、m 和 kg·m。 g S g_S gS​ 是物理重力加速度在传感器轴中的表达,单位为 m/s²,可以根据关节位置计算传感器朝向后得到。例如世界重力为 g W = ( 0 , 0 , − 9.81 ) T g_W=(0,0,-9.81)^T gW​=(0,0,−9.81)T,则:

g S = R W S T g W . g_S=R_{WS}^Tg_W. gS​=RWST​gW​.

R W S R_{WS} RWS​ 将传感器坐标分量旋转到世界坐标,转置矩阵完成反向转换。工具翻转时, g S g_S gS​ 的方向随之改变;工具质量、质心和通道偏置在传感器轴下保持相同。

将测量的力与力矩分别记为 f S , n S f_S,n_S fS​,nS​,对应偏置记为 b f , b n b_f,b_n bf​,bn​。忽略随机噪声,静态关系为:

f S = m g S + b f , n S = r C × m g S + b n = − [ g S ] × h m + b n . f_S=mg_S+b_f,\qquad n_S=r_C\times mg_S+b_n=-[g_S]_\times h_m+b_n. fS​=mgS​+bf​,nS​=rC​×mgS​+bn​=−[gS​]×​hm​+bn​.

f S , b f f_S,b_f fS​,bf​ 的单位为 N, n S , b n n_S,b_n nS​,bn​ 的单位为 N·m。力矩项表示重量通过质心力臂作用于传感器原点;叉乘交换顺序后,便将未知的一阶质量矩放到了右侧。 [ g S ] × [g_S]_\times [gS​]×​ 是叉乘矩阵,满足 [ g S ] × h m = g S × h m [g_S]_\times h_m=g_S\times h_m [gS​]×​hm​=gS​×hm​。

传感器自身也有重量,哪些部分的自重进入读数,取决于内部承力结构和厂家采用的补偿方式。其中可等效为测量端固定负载的部分,可以与工具一起计入这里的 m , h m m,h_m m,hm​。这样,随姿态变化的自重作用由重力项描述, b f , b n b_f,b_n bf​,bn​ 表示温度和安装条件稳定时近似恒定的通道零偏。若要单独得到工具参数,可以先做空载多姿态标定,再从装有工具的测量中扣除相应姿态下的空载响应。

将两式放在一起,得到关于 m , h m , b f , b n m,h_m,b_f,b_n m,hm​,bf​,bn​ 共十个未知量的线性回归:

[ f S n S ] = [ g S 0 3 × 3 I 3 0 3 × 3 0 3 × 1 − [ g S ] × 0 3 × 3 I 3 ] [ m h m b f b n ] . \begin{bmatrix}f_S\\n_S\end{bmatrix} =\begin{bmatrix} g_S&0_{3\times3}&I_3&0_{3\times3}\\ 0_{3\times1}&-[g_S]_\times&0_{3\times3}&I_3 \end{bmatrix} \begin{bmatrix}m\\h_m\\b_f\\b_n\end{bmatrix}. [fS​nS​​]=[gS​03×1​​03×3​−[gS​]×​​I3​03×3​​03×3​I3​​] ​mhm​bf​bn​​ ​.

I 3 I_3 I3​ 是三阶单位矩阵,零块的下标给出行数和列数。前三行对应三个力通道,用来估计质量和力偏置;后三行对应三个力矩通道,用来估计一阶质量矩和力矩偏置。本节为了对应传感器读数采用“力在前、力矩在后”的排列,第二章空间力采用“力矩在前、力在后”的排列,转换时交换这两组分量即可。

通过改变朝向分离负载与偏置

在单一朝向下,力测量中质量和偏置共同形成一个固定读数。若质量增加 Δ m \Delta m Δm,同时让力偏置减少 Δ m   g S \Delta m\,g_S ΔmgS​,读数仍保持相同。改变工具朝向后,重量在各通道的分量随 g S g_S gS​ 改变,偏置保持相同,这就提供了分离两者的依据。

取两个姿态的测量之差,可以直接看到偏置被消去:

Δ f = m   Δ g S , Δ n = − [ Δ g S ] × h m . \begin{aligned} \Delta f&=m\,\Delta g_S,\\ \Delta n&=-[\Delta g_S]_\times h_m. \end{aligned} ΔfΔn​=mΔgS​,=−[ΔgS​]×​hm​.​

这里 Δ f = f S , 2 − f S , 1 \Delta f=f_{S,2}-f_{S,1} Δf=fS,2​−fS,1​、 Δ n = n S , 2 − n S , 1 \Delta n=n_{S,2}-n_{S,1} Δn=nS,2​−nS,1​, Δ g S = g S , 2 − g S , 1 \Delta g_S=g_{S,2}-g_{S,1} ΔgS​=gS,2​−gS,1​。各次读数都按传感器自身的同名通道相减。第一式用力的变化估计质量;第二式利用重量力矩的变化估计一阶质量矩。

叉乘对平行方向的分量为零,所以一次姿态差主要提供 h m h_m hm​ 垂直于 Δ g S \Delta g_S ΔgS​ 的分量信息。加入更多朝向,使重力差向量具有至少两个不平行方向,就能补齐三个分量的信息。实际可让传感器的正负三个轴分别大致朝向重力,采集六个姿态,堆叠回归矩阵后检查其秩与奇异值,再统一求解十个未知量。

在这里插入图片描述

图中的 O O O 对应传感器原点 S S S,蓝色线段表示质心位置,橙色箭头表示工具重量在传感器轴中的方向。朝不同方向翻转工具会改变 g S g_S gS​,使质量、一阶质量矩与偏置的贡献更容易区分。保持朝向沿桌面平移、或只绕世界竖直轴旋转时, g S g_S gS​ 保持相同;实验应包含绕其他方向的翻转。

得到质量和一阶质量矩后,用 r C = h m / m r_C=h_m/m rC​=hm​/m 恢复质心。质量较小时,同样的一阶质量矩误差会带来更大的质心误差,因此轻工具通常需要更细致的偏置标定和重复采样。最后换一组朝向检查预测的重力载荷,便能验证这组工具参数。

6.4 工具转动惯量的动态辨识

工具的质量、质心和传感器偏置确定后,接下来让工具随腕部做转动加减速,观察产生这些运动所需的力矩。工具与末端保持刚性连接,实验中与环境脱离接触。仍沿用上一节的传感器坐标轴,这样质心位置与惯量分量在整个实验中保持常数。

从传感器读数得到质心处的转动力矩

上一节将读数定义为工具对传感器的作用。扣除偏置并取反,就得到机械臂对工具的支撑力 f s u p f_{\mathrm{sup}} fsup​ 和关于传感器原点的支撑力矩 n s u p n_{\mathrm{sup}} nsup​:

f s u p = − ( f S − b f ) , n s u p = − ( n S − b n ) . f_{\mathrm{sup}}=-(f_S-b_f),\qquad n_{\mathrm{sup}}=-(n_S-b_n). fsup​=−(fS​−bf​),nsup​=−(nS​−bn​).

两者的单位分别为 N 和 N·m。接下来将力矩的参考点从传感器原点移到质心。沿用从传感器原点指向质心的 r C r_C rC​,有:

n s u p = n C + r C × f s u p , n C = n s u p − r C × f s u p . n_{\mathrm{sup}}=n_C+r_C\times f_{\mathrm{sup}},\qquad n_C=n_{\mathrm{sup}}-r_C\times f_{\mathrm{sup}}. nsup​=nC​+rC​×fsup​,nC​=nsup​−rC​×fsup​.

n C n_C nC​ 是支撑作用关于工具质心的力矩。传感器原点与质心有一段距离,支撑力通过这段力臂也会产生力矩;减去这一部分,就得到用于分析工具自身转动的力矩。这一步用上一节估计的质心位置和六维传感器数据完成,所有分量仍沿传感器轴表达。

根据第一章的牛顿–欧拉方程,工具满足:

f s u p = m ( a C − g S ) , n C = I C α + ω × ( I C ω ) . f_{\mathrm{sup}}=m(a_C-g_S),\qquad n_C=I_C\alpha+\omega\times(I_C\omega). fsup​=m(aC​−gS​),nC​=IC​α+ω×(IC​ω).

a C a_C aC​ 是工具质心相对惯性系的物理线加速度, g S g_S gS​ 是重力加速度,两者均用传感器轴表达,单位为 m/s²。第一式来自 f s u p + m g S = m a C f_{\mathrm{sup}}+mg_S=ma_C fsup​+mgS​=maC​,可以用于检查质量、质心和力测量的对应关系。 I C I_C IC​ 是关于质心、沿传感器轴表达的惯量矩阵,单位为 kg·m²; ω , α \omega,\alpha ω,α 是工具角速度和角加速度,单位为 rad/s、rad/s²。

工具在实验中受到机械臂的支撑作用和重力。均匀重力可以等效为作用在质心上的力,对质心的力矩为零。因此,工具所受的合外力矩(关于质心)就是第二式左侧的 n C n_C nC​,即支撑作用关于质心的力矩;它等于右侧两项之和。

由关节位置、速度和加速度,通过第二章的前向递推可以计算工具的 ω , α \omega,\alpha ω,α。配合上面从传感器得到的 n C n_C nC​,每一帧就形成了一组“已知运动—测得力矩”的惯量辨识数据。数据求导、时间对齐和滤波沿用第五章的方法。

构造六个惯量分量的回归矩阵

将质心惯量的六个独立元素排列为:

ι = [ I x x I y y I z z I x y I x z I y z ] T . \iota=\begin{bmatrix}I_{xx}&I_{yy}&I_{zz}&I_{xy}&I_{xz}&I_{yz}\end{bmatrix}^T. ι=[Ixx​​Iyy​​Izz​​Ixy​​Ixz​​Iyz​​]T.

每个元素的单位都是 kg·m²,非对角元素按惯量矩阵本身的符号填写。转动方程对这些元素呈线性关系,因此可以写成 n C = Y I ι n_C=Y_I\iota nC​=YI​ι,其中 Y I Y_I YI​ 为 3 × 6 3\times6 3×6 矩阵。

先把这六个参数在惯量矩阵中的位置写出来,行、列均按 x , y , z x,y,z x,y,z 排列:

I C = [ I x x I x y I x z I x y I y y I y z I x z I y z I z z ] . I_C=\begin{bmatrix} I_{xx}&I_{xy}&I_{xz}\\ I_{xy}&I_{yy}&I_{yz}\\ I_{xz}&I_{yz}&I_{zz} \end{bmatrix}. IC​= ​Ixx​Ixy​Ixz​​Ixy​Iyy​Iyz​​Ixz​Iyz​Izz​​ ​.

为了分别计算每个参数对应的回归列,用只含 0 和 1 的矩阵 B k B_k Bk​ 记录第 k k k 个参数应填入的位置,编号与 ι \iota ι 中的顺序一致。例如,第一个参数是 I x x I_{xx} Ixx​,第四个参数是 I x y I_{xy} Ixy​,对应:

B 1 = [ 1 0 0 0 0 0 0 0 0 ] , B 4 = [ 0 1 0 1 0 0 0 0 0 ] . B_1=\begin{bmatrix}1&0&0\\0&0&0\\0&0&0\end{bmatrix},\qquad B_4=\begin{bmatrix}0&1&0\\1&0&0\\0&0&0\end{bmatrix}. B1​= ​100​000​000​ ​,B4​= ​010​100​000​ ​.

I x x B 1 I_{xx}B_1 Ixx​B1​ 将 I x x I_{xx} Ixx​ 填入第 1 行第 1 列; I x y B 4 I_{xy}B_4 Ixy​B4​ 将同一个 I x y I_{xy} Ixy​ 同时填入第 1 行第 2 列和第 2 行第 1 列,保持惯量矩阵的对称性。同理, B 2 , B 3 B_2,B_3 B2​,B3​ 分别在第 2、第 3 个对角位置取 1; B 5 B_5 B5​ 在 ( 1 , 3 ) , ( 3 , 1 ) (1,3),(3,1) (1,3),(3,1) 处取 1, B 6 B_6 B6​ 在 ( 2 , 3 ) , ( 3 , 2 ) (2,3),(3,2) (2,3),(3,2) 处取 1,其余位置均为零。

这些 B k B_k Bk​ 都是无量纲的固定矩阵。将六个参数各自乘上对应的矩阵再相加,就恢复了 I C I_C IC​,即 I C = ∑ k = 1 6 ι k B k I_C=\sum_{k=1}^{6}\iota_kB_k IC​=∑k=16​ιk​Bk​,其中 ι k \iota_k ιk​ 是参数向量的第 k k k 个分量。代入转动方程并按参数合并,得到:

Y I = [ y 1 ⋯ y 6 ] , y k = B k α + ω × ( B k ω ) . Y_I=\begin{bmatrix}y_1&\cdots&y_6\end{bmatrix},\qquad y_k=B_k\alpha+\omega\times(B_k\omega). YI​=[y1​​⋯​y6​​],yk​=Bk​α+ω×(Bk​ω).

y k y_k yk​ 是三维列向量,描述第 k k k 个惯量分量对三个方向力矩的贡献。计算时固定这一帧的 ω , α \omega,\alpha ω,α,依次将 B 1 B_1 B1​ 至 B 6 B_6 B6​ 代入上式,把所得向量填入 Y I Y_I YI​ 的第 1 至第 6 列。列向量中的第一项来自角加速度,第二项来自角动量方向的变化。将多帧的 Y I Y_I YI​ 和 n C n_C nC​ 分别纵向叠放,就可以用最小二乘估计 ι \iota ι,并加入下一节的物理约束。

用多轴运动提供完整的惯量信息

以只绕工具 z z z 轴转动为例,令 ω = ( 0 , 0 , ω z ) T \omega=(0,0,\omega_z)^T ω=(0,0,ωz​)T、 α = ( 0 , 0 , α z ) T \alpha=(0,0,\alpha_z)^T α=(0,0,αz​)T。代入转动方程,质心力矩的三个分量为:

n C = [ I x z α z − I y z ω z 2 I y z α z + I x z ω z 2 I z z α z ] . n_C=\begin{bmatrix} I_{xz}\alpha_z-I_{yz}\omega_z^2\\ I_{yz}\alpha_z+I_{xz}\omega_z^2\\ I_{zz}\alpha_z \end{bmatrix}. nC​= ​Ixz​αz​−Iyz​ωz2​Iyz​αz​+Ixz​ωz2​Izz​αz​​ ​.

ω z , α z \omega_z,\alpha_z ωz​,αz​ 是该轴的角速度与角加速度, I x z , I y z , I z z I_{xz},I_{yz},I_{zz} Ixz​,Iyz​,Izz​ 是惯量矩阵中的相应元素。这个实验提供的是这三个分量的信息。保持匀速时, α z = 0 \alpha_z=0 αz​=0,数据中只剩下 I x z , I y z I_{xz},I_{yz} Ixz​,Iyz​ 的作用。改变旋转轴并安排加减速,才能让其余惯量分量也参与观测。

因此,可利用六轴、七轴机械臂的腕部安排多轴角运动,再按照第五章的方法评价堆叠回归矩阵的奇异值,选择信息较充分的轨迹。角加速度应足以产生清晰的惯性力矩,同时与驱动带宽、传感器量程和运动测量质量相适应。

6.5 参数的物理一致性与组合模型验证

辨识结果将用于计算重力、动能和运动力矩,因此还要检查这些参数能否共同描述一份真实的质量分布。第四章从基参数恢复完整连杆参数时,也需要用到这里的条件。

质量、质心与惯量之间的物理关系

每个刚体的质量应满足 m > 0 m>0 m>0。将质心惯量 I C I_C IC​ 的三个特征值记为 I C , 1 , I C , 2 , I C , 3 I_{C,1},I_{C,2},I_{C,3} IC,1​,IC,2​,IC,3​,它们就是沿三条惯性主轴的主惯量,单位为 kg·m²。主惯量应非负,并满足:

I C , 1 + I C , 2 ≥ I C , 3 , I C , 2 + I C , 3 ≥ I C , 1 , I C , 3 + I C , 1 ≥ I C , 2 . \begin{aligned} I_{C,1}+I_{C,2}&\geq I_{C,3},\\ I_{C,2}+I_{C,3}&\geq I_{C,1},\\ I_{C,3}+I_{C,1}&\geq I_{C,2}. \end{aligned} IC,1​+IC,2​IC,2​+IC,3​IC,3​+IC,1​​≥IC,3​,≥IC,1​,≥IC,2​.​

这些三角不等式来自同一份质量分布。沿质心主轴,用 x , y , z x,y,z x,y,z 表示质量微元的位置,则 I C , 1 = ∫ ( y 2 + z 2 )   d m I_{C,1}=\int(y^2+z^2)\,dm IC,1​=∫(y2+z2)dm,其他两项依次类推。因此:

I C , 1 + I C , 2 − I C , 3 = 2 ∫ z 2   d m ≥ 0. I_{C,1}+I_{C,2}-I_{C,3}=2\int z^2\,dm\geq0. IC,1​+IC,2​−IC,3​=2∫z2dm≥0.

另外两条关系也可同样得到。它们把三个方向的惯量联系起来:每一项都由同一批质量微元到各轴的距离决定。

在参数估计中加入物理约束

可以直接把主惯量三角不等式作为优化约束。采用第四章的标准参数时,每根连杆的待估量是质量 m m m、一阶质量矩 h m h_m hm​ 和关于连杆原点的惯量 I O I_O IO​。要检查主惯量,需先通过平行轴定理计算质心惯量:

I C = I O − 1 m ( ∥ h m ∥ 2 I 3 − h m h m T ) , I_C=I_O-\frac{1}{m}\left(\|h_m\|^2I_3-h_mh_m^T\right), IC​=IO​−m1​(∥hm​∥2I3​−hm​hmT​),

再求 I C I_C IC​ 的三个特征值,代入三角不等式。这里 I 3 I_3 I3​ 是三阶单位矩阵, h m = m r C h_m=mr_C hm​=mrC​, r C r_C rC​ 从连杆原点指向质心。由于质量和质心也在变化,这种写法包含参数乘积、除法和特征值计算,求导与数值处理较为复杂。

伪惯量矩阵提供了另一种组织方式:将同样的物理关系写成对标准参数呈线性的矩阵约束。先定义关于连杆原点 O O O 的二阶质量矩:

Σ m = ∫ r r T   d m = 1 2 tr ⁡ ( I O ) I 3 − I O . \Sigma_m=\int rr^T\,dm =\frac12\operatorname{tr}(I_O)I_3-I_O. Σm​=∫rrTdm=21​tr(IO​)I3​−IO​.

r r r 从原点指向质量微元, tr ⁡ ( I O ) \operatorname{tr}(I_O) tr(IO​) 是惯量矩阵三个对角元素之和。 Σ m \Sigma_m Σm​ 的对角元素分别是 ∫ x 2 d m , ∫ y 2 d m , ∫ z 2 d m \int x^2dm,\int y^2dm,\int z^2dm ∫x2dm,∫y2dm,∫z2dm,单位为 kg·m²,描述质量沿各方向的分布。将它与一阶质量矩、质量放在一起,得到:

P ( π i ) = [ Σ m h m h m T m ] = ∫ [ r 1 ] [ r 1 ] T d m . \mathcal P(\pi_i)= \begin{bmatrix}\Sigma_m&h_m\\h_m^T&m\end{bmatrix} =\int\begin{bmatrix}r\\1\end{bmatrix} \begin{bmatrix}r\\1\end{bmatrix}^Tdm. P(πi​)=[Σm​hmT​​hm​m​]=∫[r1​][r1​]Tdm.

π i \pi_i πi​ 是第 i i i 根连杆的十维标准参数,四阶矩阵 P \mathcal P P 称为伪惯量矩阵。积分式表明,对任意四维实向量 u u u, u T P u u^T\mathcal Pu uTPu 都是平方乘以非负质量后再相加,因此满足 u T P u ≥ 0 u^T\mathcal Pu\geq0 uTPu≥0。这个性质称为半正定,记为 P ⪰ 0 \mathcal P\succeq0 P⪰0,等价于矩阵的所有特征值非负。它允许零特征值,也包含理想薄片、细杆等质量分布。

要求 m > 0 m>0 m>0 且 P ⪰ 0 \mathcal P\succeq0 P⪰0,就能把质量、质心和惯量约束在同一份非负质量分布中。它与三角不等式的联系可以通过质心处的二阶质量矩看出来:当 m > 0 m>0 m>0 时, P ⪰ 0 \mathcal P\succeq0 P⪰0 等价于

Σ C = Σ m − h m h m T m = 1 2 tr ⁡ ( I C ) I 3 − I C ⪰ 0. \Sigma_C=\Sigma_m-\frac{h_mh_m^T}{m} =\frac12\operatorname{tr}(I_C)I_3-I_C\succeq0. ΣC​=Σm​−mhm​hmT​​=21​tr(IC​)I3​−IC​⪰0.

减去 h m h m T / m h_mh_m^T/m hm​hmT​/m,移除了质心偏离原点的部分。在质心主轴下, Σ C \Sigma_C ΣC​ 的三个对角元素依次为 ( I C , 2 + I C , 3 − I C , 1 ) / 2 (I_{C,2}+I_{C,3}-I_{C,1})/2 (IC,2​+IC,3​−IC,1​)/2 及其循环排列;它们非负,正好对应前面的三角不等式。

采用 P \mathcal P P 的计算优势在于,它的每个元素都对待估参数呈线性关系。 这种“矩阵关于变量呈线性关系,并要求矩阵半正定”的约束称为线性矩阵不等式,简称 LMI。它保留了参数估计的凸结构,便于使用专门的求解器。若质量、质心已经确定,只估计第 6.4 节的六个工具惯量分量,直接对上式的三阶矩阵 Σ C \Sigma_C ΣC​ 施加半正定约束就可以。

为了理解矩阵约束如何进入优化,先看一个二阶对称矩阵:

P = [ a b b c ] ⪰ 0 ⟺ a ≥ 0 , c ≥ 0 , a c − b 2 ≥ 0. P=\begin{bmatrix}a&b\\b&c\end{bmatrix}\succeq0 \quad\Longleftrightarrow\quad a\geq0,\quad c\geq0,\quad ac-b^2\geq0. P=[ab​bc​]⪰0⟺a≥0,c≥0,ac−b2≥0.

a , b , c a,b,c a,b,c 是这个示例矩阵的元素。半正定条件同时约束了对角元素和它们与非对角元素之间的关系,矩阵写法把这些条件放在一起表达。半定规划求解器直接处理这种矩阵约束,并利用其中的结构进行计算。

回到完整机械臂,最小二乘估计可以写为:

min ⁡ π ∥ W π − y τ ∥ 2 , s.t. m i ≥ m i , min ⁡ , P ( π i ) ⪰ 0 , i = 1 , … , n . \begin{aligned} \min_\pi\quad &\|W\pi-y_\tau\|^2,\\ \text{s.t.}\quad&m_i\geq m_{i,\min},\qquad \mathcal P(\pi_i)\succeq0,\quad i=1,\ldots,n. \end{aligned} πmin​s.t.​∥Wπ−yτ​∥2,mi​≥mi,min​,P(πi​)⪰0,i=1,…,n.​

π \pi π 堆叠全部 n n n 根连杆的参数, W W W 是对应的回归矩阵, y τ y_\tau yτ​ 是处理后的力矩观测; m i , min ⁡ > 0 m_{i,\min}>0 mi,min​>0 是第 i i i 根连杆设定的正质量下界。还可加入第四章的 CAD 先验,以及已知的质量范围、质心范围。二次拟合目标配合这些线性矩阵不等式,可以采用凸半定优化方法求解。

以内点法为例,先按固定的质量、长度尺度将 P \mathcal P P 化为无量纲矩阵 P ‾ \overline{\mathcal P} P,再为每个连杆加入 − μ log ⁡ det ⁡ P ‾ -\mu\log\det\overline{\mathcal P} −μlogdetP 这样的障碍项, μ > 0 \mu>0 μ>0 控制障碍项的强度。迭代从矩阵正定的内部开始;某个特征值接近零时,障碍项迅速增大。逐步减小 μ \mu μ,就可以逼近原问题的最优解,包括位于半正定边界上的解。这与普通不等式约束的内点法思路相通。

也可以先求基参数,再在测量误差允许的范围内恢复满足物理条件的完整参数。基参数是多个原参数的线性组合,分量可正可负;上述物理条件施加在恢复出的各连杆参数上。

合成本体与工具模型并验证

本体和工具的参数需要有明确的装配边界。可以在已知空载配置下建立本体模型,再独立标定工具;装上工具后,通过装配变换将其质心、惯量换算到末端连杆坐标系,再与本体模型组合。也可以联合处理多组工具数据,让本体参数在各组之间共享,工具参数分别估计。

静态重力、双向摩擦和动态惯量估计完成后,就能按第二章的动力学方程计算总驱动力矩。本体的完整惯性参数使用第四、第五章的多关节动态回归估计,工具惯量则可采用本章的六维传感器方法。各阶段得到的参数共同参与最终预测。

分阶段估计时,前一步的误差会传入后一步。例如,工具质心估计偏远,会使 r C × f s u p r_C\times f_{\mathrm{sup}} rC​×fsup​ 的扣除量产生偏差,后续拟合可能通过调整惯量来吸收这部分误差。验证时改变旋转方向和加速度组合,就有助于发现这种相互补偿。

最后按第 5.5 节的方法保留独立数据,分别检查静态多姿态、双向多速度和多轴动态运动的预测误差,再检查组合模型在目标工作范围内的总力矩预测。静态偏差重点检查重力与零偏,换向偏差重点检查摩擦,加减速偏差重点检查惯量、质心和时间对齐。这样可以将误差落到具体环节,再决定补充哪类数据或调整哪部分模型。

Logo

DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐