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

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

第四章:动力学回归与最小参数集

前面计算力矩前馈、阻抗控制和外力估计时,都用到了机械臂的质量、质心和惯量。这些参数可以先从 CAD 模型得到,再通过实际运动与力矩数据加以修正。动力学参数辨识做的就是这件事:记录机械臂怎样运动、驱动器出了多大力矩,再寻找一组能解释这些数据的模型参数。

本章假定关节类型、连杆几何和坐标系定义已经确定,集中讨论惯性参数的估计。先把动力学整理成适合求解的形式,再说明哪些参数组合能由关节力矩确定,以及怎样利用测量数据和先验估计这些参数。

4.1 从动力学方程到参数回归

把运动量与待求参数分开

沿用第二章的抬臂例子:一根连杆绕水平关节轴在竖直平面内摆动,转角 q q q 从竖直向下的位置算起。忽略摩擦和外部接触,关节驱动力矩为:

τ a = I O , u q ¨ + ( m ℓ ) g 0 sin ⁡ q . \tau_a=I_{O,u}\ddot q+(m\ell)g_0\sin q. τa​=IO,u​q¨​+(mℓ)g0​sinq.

I O , u I_{O,u} IO,u​ 是连杆绕关节轴的惯量,单位为 kg·m²; m m m 是质量, ℓ \ell ℓ 是质心到关节轴的距离,单位分别为 kg、m; g 0 g_0 g0​ 是重力加速度大小,单位为 m/s²。采集运动数据后, q q q 和 q ¨ \ddot q q¨​ 已经有了测量或估计值,待求的量就集中在 I O , u I_{O,u} IO,u​ 和 m ℓ m\ell mℓ 这两个系数上。把式子重新排列,有:

τ a = [ q ¨ g 0 sin ⁡ q ] [ I O , u m ℓ ] . \tau_a= \begin{bmatrix}\ddot q&g_0\sin q\end{bmatrix} \begin{bmatrix}I_{O,u}\\m\ell\end{bmatrix}. τa​=[q¨​​g0​sinq​][IO,u​mℓ​].

左边是测得的力矩,右边第一项由已知运动计算,第二项装着待求参数。这就是参数回归的基本形式。换一个姿态或加速度,就得到一条新的力矩方程;把多次测量放在一起,便可以估计这两个系数。

这里的“线性”指力矩对待求参数呈线性关系:每个参数乘一个已知系数,再把各项相加。 sin ⁡ q \sin q sinq 可以随关节角非线性变化;当这一帧的 q q q 已知时,它就是一个确定的数。

一根连杆的十个标准惯性参数

六轴、七轴机械臂中的连杆会同时平移和转动,需要用完整的三维质量分布描述惯性。沿用第一章的记号,将第 i i i 根连杆的参数排列为:

π i = [ m i h m x , i h m y , i h m z , i I O , x x , i I O , y y , i I O , z z , i I O , x y , i I O , x z , i I O , y z , i ] T . \pi_i=\begin{bmatrix} m_i&h_{mx,i}&h_{my,i}&h_{mz,i}& I_{O,xx,i}&I_{O,yy,i}&I_{O,zz,i}&I_{O,xy,i}&I_{O,xz,i}&I_{O,yz,i} \end{bmatrix}^T. πi​=[mi​​hmx,i​​hmy,i​​hmz,i​​IO,xx,i​​IO,yy,i​​IO,zz,i​​IO,xy,i​​IO,xz,i​​IO,yz,i​​]T.

这个十维列向量称为标准惯性参数。各部分的含义如下。

参数物理含义单位
m i m_i mi​连杆质量kg
h m , i = m i r C , i h_{m,i}=m_i r_{C,i} hm,i​=mi​rC,i​ 的三个分量一阶质量矩,包含质量与质心位置的信息kg·m
I O , i I_{O,i} IO,i​ 的六个独立分量关于连杆坐标系原点 O O O 的惯量矩阵,包含三个对角项和三个非对角项kg·m²

r C , i r_{C,i} rC,i​ 是从连杆原点指向质心的向量,所有分量均用该连杆坐标轴表达。惯量矩阵是对称的,因此用六个分量就能描述。原点 O O O 可以选在建模时规定的连杆坐标系位置。

选择一阶质量矩的原因,在抬臂例子中已经出现:重力力矩里直接乘着 m ℓ m\ell mℓ。把这个乘积整体作为一个未知系数,就能保留线性形式。三维情况下采用 h m = m r C h_m=mr_C hm​=mrC​,并搭配关于同一原点的 I O I_O IO​,空间惯量便对这十个参数呈线性关系。给定参数后,可用 r C = h m / m r_C=h_m/m rC​=hm​/m 恢复质心位置,再通过平行轴定理换算质心惯量。

回归矩阵的每一列

将各连杆的十个参数按顺序竖向拼接,得到整台机械臂的参数向量 π = [ π 1 ; … ; π n ] \pi=[\pi_1;\ldots;\pi_n] π=[π1​;…;πn​],其中 n n n 是移动连杆数,分号表示竖向拼接。刚体动力学对这些参数呈线性关系,因此可以写成:

τ r b = M q ¨ + c + g = Y ( q , q ˙ , q ¨ ) π . \tau_{\mathrm{rb}}=M\ddot q+c+g =Y(q,\dot q,\ddot q)\pi. τrb​=Mq¨​+c+g=Y(q,q˙​,q¨​)π.

τ r b \tau_{\mathrm{rb}} τrb​ 是惯性、速度耦合和重力共同对应的关节力矩,下标 rb 表示刚体部分; Y Y Y 称为动力学回归矩阵。机械臂的几何和重力设置已知时,给定一帧 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​,就能计算这一帧的 Y Y Y。对于七根移动连杆组成的七轴模型, π \pi π 包含 70 个参数, Y Y Y 有 7 行、70 列,每行对应一个关节,每列对应一个参数。

前面的单连杆例子中,绕轴惯量的系数是 q ¨ \ddot q q¨​,质量矩 m ℓ m\ell mℓ 的系数是 g 0 sin ⁡ q g_0\sin q g0​sinq。多关节机械臂也按同样的方式组织系数,只是一个参数可能同时影响多个关节。

先用一个两行两列的小例子看清矩阵乘法。设待估参数为 p 1 , p 2 p_1,p_2 p1​,p2​,对应两个关节的力矩为 τ 1 , τ 2 \tau_1,\tau_2 τ1​,τ2​,这一帧的四个系数记为 a , b , c , d a,b,c,d a,b,c,d。按行计算,有:

[ τ 1 τ 2 ] = [ a b c d ] [ p 1 p 2 ] = [ a p 1 + b p 2 c p 1 + d p 2 ] . \begin{bmatrix}\tau_1\\\tau_2\end{bmatrix} =\begin{bmatrix}a&b\\c&d\end{bmatrix} \begin{bmatrix}p_1\\p_2\end{bmatrix} =\begin{bmatrix}ap_1+bp_2\\cp_1+dp_2\end{bmatrix}. [τ1​τ2​​]=[ac​bd​][p1​p2​​]=[ap1​+bp2​cp1​+dp2​​].

每一行与参数列向量相乘,得到一个关节的力矩。再把含 p 1 p_1 p1​ 的项放在一起,含 p 2 p_2 p2​ 的项放在一起,同一个结果就写成:

[ τ 1 τ 2 ] = p 1 [ a c ] + p 2 [ b d ] . \begin{bmatrix}\tau_1\\\tau_2\end{bmatrix} =p_1\begin{bmatrix}a\\c\end{bmatrix} +p_2\begin{bmatrix}b\\d\end{bmatrix}. [τ1​τ2​​]=p1​[ac​]+p2​[bd​].

右侧的两个向量恰好是原矩阵的两列。第一列中的 a , c a,c a,c 分别乘以 p 1 p_1 p1​,得到这个参数对两个关节的力矩贡献;第二列对 p 2 p_2 p2​ 做同样的计算。将两个贡献向量相加,就得到完整的关节力矩。因此,按行看,是一个关节的力矩由哪些参数共同贡献;按列看,是一个参数分别对各个关节贡献多少力矩。

在七轴模型中,每个参数在七条关节力矩方程里各有一个系数,这七个系数就组成 Y Y Y 中对应的一列。将各列乘以相应参数后相加,与逐行计算 Y π Y\pi Yπ 得到相同结果。系数与参数的单位相互配合,乘积统一为 N·m。

这些系数可以利用逆动力学逐列计算。逆动力学除了使用运动状态,还会读取模型中的连杆惯性参数,并将它们组成空间惯量。沿用第一章的写法,第 i i i 根连杆的空间惯量为:

I i = [ I O , i [ h m , i ] × − [ h m , i ] × m i I 3 ] . \mathcal I_i= \begin{bmatrix} I_{O,i}&[h_{m,i}]_\times\\ -[h_{m,i}]_\times&m_iI_3 \end{bmatrix}. Ii​=[IO,i​−[hm,i​]×​​[hm,i​]×​mi​I3​​].

其中 m i m_i mi​ 是质量, h m , i h_{m,i} hm,i​ 是一阶质量矩, I O , i I_{O,i} IO,i​ 是关于连杆原点的惯量矩阵; I 3 I_3 I3​ 是三阶单位矩阵, [ h m , i ] × [h_{m,i}]_\times [hm,i​]×​ 是叉乘矩阵。十个参数直接填入这些位置,随后通过空间惯量参与递归受力计算。

以提取某根连杆的 I O , z z I_{O,zz} IO,zz​ 对应列为例:在计算程序中,将这个分量设为 1 kg·m²,整组 π \pi π 中其余 69 个参数全部设为 0。按上式填写后,该连杆的空间惯量只有第 3 行、第 3 列为 1,其余连杆的空间惯量全部为零。这样的赋值用于单独提取一个参数的系数。

保持机构几何、重力设置和本帧的 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​ 相同,用刚刚填写的空间惯量运行一次忽略摩擦和接触的逆动力学。前向递推计算各连杆的运动,后向递推汇总受力并提取关节力矩,最终得到七个力矩数值。在 Y π Y\pi Yπ 中,这次赋值使所选参数对应的列乘以 1,其余列均乘以 0,因此输出恰好就是所求列的数值。

将结果填入这一列后,再选择下一个参数,将它设为 1、其余参数清零,重复上述计算。每次的 1 都采用相应参数的单位。处理完 70 个参数,就得到当前这帧完整的 7 × 70 7\times70 7×70 回归矩阵。沿着运动逐帧计算,便能将各个时刻的回归矩阵汇集起来,用于参数估计。

把一段运动组织成方程组

先考虑摩擦、测量偏置和外部接触已经处理好的刚体力矩。记第 k k k 帧的力矩为 τ ( k ) \tau^{(k)} τ(k),对应回归矩阵为 Y ( k ) Y^{(k)} Y(k);括号上标标记采样帧。将 N N N 帧按时间叠放,得到:

W = [ Y ( 1 ) Y ( 2 ) ⋮ Y ( N ) ] , y τ = [ τ ( 1 ) τ ( 2 ) ⋮ τ ( N ) ] , y τ = W π . W=\begin{bmatrix}Y^{(1)}\\Y^{(2)}\\\vdots\\Y^{(N)}\end{bmatrix}, \qquad y_\tau=\begin{bmatrix}\tau^{(1)}\\\tau^{(2)}\\\vdots\\\tau^{(N)}\end{bmatrix}, \qquad y_\tau=W\pi. W= ​Y(1)Y(2)⋮Y(N)​ ​,yτ​= ​τ(1)τ(2)⋮τ(N)​ ​,yτ​=Wπ.

W W W 是整段运动的回归矩阵, y τ y_\tau yτ​ 是对应的力矩观测向量。七轴机械臂采集 N N N 帧,就有 7 N 7N 7N 条标量方程,用来估计 70 个标准参数。

接下来要考察这些方程提供了多少独立信息。例如,机器人一直停在同一姿态时,每帧主要重复相同的重力关系;姿态与加速度发生变化时,参数系数才会出现新的组合。方程之间的独立性,决定了数据能够确定哪些参数。

4.2 参数组合与最小参数集

关节力矩能够确定的量

回到抬臂例子。无论采集多少个转角和加速度,力矩方程中的两个系数始终是 I O , u I_{O,u} IO,u​ 与 m ℓ m\ell mℓ。若暂时只知道关节运动与力矩,可以从数据中求出绕关节轴的惯量,以及质量和质心距离的乘积。

例如,质量较小、质心较远的连杆,与质量较大、质心较近的连杆,可以具有相同的 m ℓ m\ell mℓ,因而产生相同的重力力矩。若它们绕关节轴的惯量也相同,在这个单轴实验中就会呈现相同的力矩响应。独立称重得到 m m m 后,便可用 ℓ = ( m ℓ ) / m \ell=(m\ell)/m ℓ=(mℓ)/m 进一步求出质心距离。

在多连杆机械臂中,也会出现类似的参数组合。以腕部的转接板和工具为例,两者始终刚性固定,关节力矩反映的是这个整体的惯性。将它们的参数都换算到同一原点和坐标轴后,合成参数就是质量、一阶质量矩与惯量的逐项相加;例如,合成质量为 m A + m B m_A+m_B mA​+mB​,其中 A , B A,B A,B 分别表示转接板和工具。保持合成参数相同的各种分配,对关节运动产生相同的动力学作用。拆下工具单独称重,就能补充区分两者所需的信息。

即使按每个关节划分连杆,相邻连杆的一些惯性作用也会因关节运动约束而合并到同一个系数中。机构几何和可测量的力矩共同决定了这些组合关系。

结构不可辨识的含义

用回归矩阵表示上述现象,设 z z z 是一组非零的参数变化量,与 π \pi π 同维,各分量具有相应参数的单位。若在所有允许的运动状态下都满足 Y z = 0 Yz=0 Yz=0,则:

Y ( π + z ) = Y π . Y(\pi+z)=Y\pi. Y(π+z)=Yπ.

这表示沿 z z z 调整参数后,关节力矩保持相同。由机构和测量方式带来的这种参数歧义,称为结构不可辨识;所有满足该条件的变化方向组成结构零空间。“零”指这些参数变化在力矩中产生的增量为零。

把始终一起起作用的参数合并,保留能够独立影响力矩的组合,就得到基参数集,也称最小参数集。用 β \beta β 表示这些组合,用 Y b Y_b Yb​ 表示对应的回归矩阵,有:

Y π = Y b β . Y\pi=Y_b\beta. Yπ=Yb​β.

下标 b b b 表示基参数形式。两边对同一运动给出相同力矩;右边用更少的独立系数表达这些作用。例如,两列完全相同时,它们的贡献可以合成一列,未知系数相应变成两个原参数之和。

基参数中的一个分量可能混合多根连杆的质量、质量矩和惯量,组合系数由机构几何决定。其数量等于回归模型中独立参数方向的数量,称为结构秩。六轴或七轴给出了关节数量,具体的结构秩还要结合关节布置、几何和测量方式计算。

关节运动的冗余与惯性参数的结构依赖分别描述两个层面。前者关心关节自由度与末端任务维数的关系,通过运动学 Jacobian 分析;后者关心各个惯性参数能否在测量力矩中体现出独立作用,通过回归矩阵分析。六根移动连杆组成的六轴模型有 60 个标准惯性参数,其中也会出现结构依赖,因此同样可以构造最小参数集。

结构依赖与运动信息不足

此外,一次实验能观察到多少信息,还取决于机械臂怎样运动。仍看抬臂例子:静止采样时 q ¨ = 0 \ddot q=0 q¨​=0,惯量前面的系数为零,数据只包含重力信息;加入加减速运动,绕轴惯量的作用才会显现出来。

这两类情况可以按下面的方式区分。

情况力矩数据中的表现获得更多信息的途径
结构依赖所有允许运动中,一些参数始终只以固定组合出现独立称重、已知零件参数、增加测量或改变装配条件
运动信息不足当前轨迹主要呈现部分参数组合,其余组合的作用为零或很弱增加姿态变化、加减速和多关节协调运动

完整参数与基参数的选择

辨识时,可以直接拟合完整参数模型 y τ = W π y_\tau=W\pi yτ​=Wπ,也可以拟合基参数模型 y τ = W b β y_\tau=W_b\beta yτ​=Wb​β。只考虑力矩拟合项时,在相同模型和数据下,两种表示可以得到相同的最优力矩预测。直接估计完整参数时,一种常用做法是同时参考 CAD 参数:

π ^ = argmin ⁡ π ( ∥ W π − y τ ∥ 2 + λ ∥ D π − 1 ( π − π C A D ) ∥ 2 ) . \widehat\pi=\underset{\pi}{\operatorname{argmin}} \left( \|W\pi-y_\tau\|^2 +\lambda\|D_\pi^{-1}(\pi-\pi_{\mathrm{CAD}})\|^2 \right). π =πargmin​(∥Wπ−yτ​∥2+λ∥Dπ−1​(π−πCAD​)∥2).

π C A D \pi_{\mathrm{CAD}} πCAD​ 是 CAD 给出的参数; D π D_\pi Dπ​ 是由各参数的正参考尺度组成的对角矩阵,用于统一质量、质量矩和惯量偏差的尺度; λ ≥ 0 \lambda\geq0 λ≥0 调节先验项的权重。 argmin ⁡ \operatorname{argmin} argmin 表示取使括号内目标最小的 π \pi π,帽号标记估计值。

第一项让预测力矩贴近测量,第二项让参数靠近 CAD。取 λ > 0 \lambda>0 λ>0 时,先验也为结构零方向上的自由分量提供选择依据。采用完整参数形式,还方便直接加入各连杆的质量、质心和惯量约束。

基参数形式先消去结构零方向,减少待估量;在激励充分时,回归矩阵具有满列秩,便于求解、分析误差和设计激励。例如,某七轴机械臂的 70 个标准惯性参数可归并为 43 个基参数。

4.3 从回归矩阵构造最小参数集

实际计算中,可以先根据已知机构几何,在较广的关节范围内生成多组位置、速度和加速度,计算并堆叠回归矩阵 W W W。这些状态用来检查动力学的代数关系,可以逐组生成;采集真实数据时,再使用连续可执行的运动轨迹。

随后需要完成两件事:判断独立参数方向的数量,并把原参数整理成相应的组合。奇异值分解(SVD)和列主元 QR 分解是常用工具。下面直接使用它们的分解结果,重点看每一步在辨识中的作用。

统一参数尺度

质量、一阶质量矩和惯量具有不同单位,典型数值也可能相差很多。为了比较这些参数变化对力矩的影响,可以先为每个参数选一个参考尺度,再将参数写成“参考尺度乘以一个待估倍数”。例如,以 5 kg 为某根连杆的质量尺度,倍数为 0.8 就表示质量为 4 kg。

将各个参考尺度放在对角矩阵 D π D_\pi Dπ​ 中,将对应的倍数组成向量 θ \theta θ,便有:

π = D π θ . \pi=D_\pi\theta. π=Dπ​θ.

π \pi π 沿用前文整台机械臂的参数向量。七轴模型中,它包含 70 个参数,因此 D π D_\pi Dπ​ 是 70 × 70 70\times70 70×70 的对角矩阵, θ \theta θ 是 70 维向量。 D π D_\pi Dπ​ 的每个对角元素都是正的参考量,并携带对应参数的单位; θ \theta θ 表示各参数相对于参考量的倍数,没有单位。计算前先选定 D π D_\pi Dπ​,后续待估的量写在 θ \theta θ 中。

参考尺度可以根据 CAD 数据和连杆尺寸选取。例如,某根连杆的质量量级为 5 kg,长度量级为 0.1 m,就可以给质量选取 5 kg,给一阶质量矩选取 5 × 0.1 = 0.5 5\times0.1=0.5 5×0.1=0.5 kg·m,给惯量选取 5 × 0.1 2 = 0.05 5\times0.1^2=0.05 5×0.12=0.05 kg·m²。不同连杆可以采用各自的参考尺度。一阶质量矩和非对角惯量可能为零或负数,仍可使用这种正的量级作尺度,参数本身的正负和零值由对应倍数保留。

只取其中三个分量,假设它们的实际值如下,可以看清这个换算过程:

参数选定的参考尺度用于演示的实际值对应倍数
质量 m m m5 kg4 kg0.8
一阶质量矩 h m x h_{mx} hmx​0.5 kg·m0.3 kg·m0.6
惯量 I O , z z I_{O,zz} IO,zz​0.05 kg·m²0.08 kg·m²1.6

将这三个分量写成矩阵形式,矩阵内暂时省略单位,有:

[ 4 0.3 0.08 ] = [ 5 0 0 0 0.5 0 0 0 0.05 ] [ 0.8 0.6 1.6 ] . \begin{bmatrix}4\\0.3\\0.08\end{bmatrix} =\begin{bmatrix}5&0&0\\0&0.5&0\\0&0&0.05\end{bmatrix} \begin{bmatrix}0.8\\0.6\\1.6\end{bmatrix}. ​40.30.08​ ​= ​500​00.50​000.05​ ​ ​0.80.61.6​ ​.

对角矩阵逐项完成“参考尺度乘以倍数”的运算,就恢复了原参数。表中的实际值用于演示;真正辨识时,先给定参考尺度,再利用运动与力矩数据估计参数。参考尺度规定参数的表示方式,迭代初值规定求解从哪组参数出发,两者承担不同的作用。若希望估计结果靠近 CAD 参数,可以加入第 4.4 节介绍的先验项。

接着将这种表示代入力矩方程。前面已经得到 y τ = W π y_\tau=W\pi yτ​=Wπ,用 D π θ D_\pi\theta Dπ​θ 替换其中的 π \pi π,便有:

y τ = W π = W ( D π θ ) = ( W D π ) θ . y_\tau=W\pi=W(D_\pi\theta)=(WD_\pi)\theta. yτ​=Wπ=W(Dπ​θ)=(WDπ​)θ.

W W W 由采样时的运动计算, D π D_\pi Dπ​ 已经选定,因此可以先计算它们的乘积,并记为 A A A:

A = W D π , y τ = A θ . A=WD_\pi,\qquad y_\tau=A\theta. A=WDπ​,yτ​=Aθ.

右乘对角矩阵 D π D_\pi Dπ​,相当于将 W W W 的每一列乘以该参数的参考尺度;随后再乘 θ \theta θ 中对应的倍数,得到的力矩与原来的 W π W\pi Wπ 相同。这样,待估参数就统一表示为相对于各自参考尺度的倍数。

构造 A A A 只用到已知的 W W W 和选定的 D π D_\pi Dπ​。接下来可以对 A A A 做 SVD,分析这些倍数组合中哪些能够从力矩数据独立确定,以及它们的影响有多强。比较不同运动数据时,始终沿用同一个 D π D_\pi Dπ​,使各次比较采用相同的参数尺度。

用 SVD 观察独立方向及其强弱

对缩放后的回归矩阵作完整的奇异值分解:

A = U Σ s V T , A v j = σ j u j . A=U\Sigma_sV^T,\qquad Av_j=\sigma_j u_j. A=UΣs​VT,Avj​=σj​uj​.

U , V U,V U,V 是正交矩阵, u j , v j u_j,v_j uj​,vj​ 分别是它们的第 j j j 列,长度均为 1; Σ s \Sigma_s Σs​ 的对角线上排列着从大到小的非负奇异值 σ j \sigma_j σj​。 v j v_j vj​ 给出一种缩放参数的组合变化方向, u j u_j uj​ 给出该变化在整段力矩数据中产生的方向。

第二个等式最能说明它的用途:当参数沿 v j v_j vj​ 改变一个单位时,力矩变化的大小就是 σ j \sigma_j σj​。奇异值大,说明这组参数变化会留下明显的力矩信号;奇异值小,说明力矩响应较弱;奇异值为零,说明这组变化在当前数据中的贡献为零。

在这里插入图片描述

浮点计算中,根据选定阈值将奇异值分为保留部分和视作零的部分,保留的个数记为 r r r。这个数反映当前矩阵中独立参数方向的数量,具体含义还要结合构造矩阵时采用的运动状态来判断。

计算结构秩时,可以先在计算机里生成一批覆盖较广范围的 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​,根据机构几何构造 A A A 并做分解;随后另行生成一批状态,计算它们的回归矩阵,检查前一批找到的零方向在新状态下是否仍然产生接近零的力矩变化。这两批状态用于分析和验证参数依赖关系,可以逐点选取,也可以从不同轨迹中取得。它们通过模型计算提供信息,实际辨识实验则通过执行连续轨迹、测量运动与力矩来估计参数。

运动状态覆盖充分、独立状态上的依赖关系也得到验证后,便有了将 r r r 作为结构秩数值结果的依据。若直接对某条实验轨迹构造的矩阵做分解,得到的秩还会受到这条轨迹的影响。例如,静止采样提供重力信息;加入加减速运动后,惯量参数的作用也会在力矩中显现。

接下来按已经确定的结构零方向,将 SVD 的结果分成两组。先按精确关系来写:前 r r r 个奇异值大于零,其余奇异值为零,非零奇异值的数量就是矩阵 A A A 的秩 r r r。对于有 70 个参数的模型,完整的 V V V 有 70 行、70 列,按相同顺序分组,有:

V = [ v 1 ⋯ v r v r + 1 ⋯ v 70 ] = [ V r V 0 ] . V=\begin{bmatrix}v_1&\cdots&v_r&v_{r+1}&\cdots&v_{70}\end{bmatrix} =\begin{bmatrix}V_r&V_0\end{bmatrix}. V=[v1​​⋯​vr​​vr+1​​⋯​v70​​]=[Vr​​V0​​].

V r V_r Vr​ 收集前 r r r 个非零奇异值对应的右奇异向量, V 0 V_0 V0​ 收集其余对应零奇异值的右奇异向量。两组列向量的长度都为 1; V 0 V_0 V0​ 的下标 0 0 0 标记的是这些方向对应的奇异值为零。

同样,将 U U U 的前 r r r 列记为 U r U_r Ur​,其余列记为 U 0 U_0 U0​;将非零奇异值 σ 1 , … , σ r \sigma_1,\ldots,\sigma_r σ1​,…,σr​ 放在对角线上,组成 r × r r\times r r×r 的矩阵 Σ r \Sigma_r Σr​。原来的 SVD 就可以展开为:

A = [ U r U 0 ] [ Σ r 0 0 0 ] [ V r T V 0 T ] = U r Σ r V r T . A= \begin{bmatrix}U_r&U_0\end{bmatrix} \begin{bmatrix}\Sigma_r&0\\0&0\end{bmatrix} \begin{bmatrix}V_r^T\\V_0^T\end{bmatrix} =U_r\Sigma_rV_r^T. A=[Ur​​U0​​][Σr​0​00​][VrT​V0T​​]=Ur​Σr​VrT​.

中间矩阵的零块使对应部分的乘积为零,保留下来的就是 U r Σ r V r T U_r\Sigma_rV_r^T Ur​Σr​VrT​。又因为 V r V_r Vr​ 与 V 0 V_0 V0​ 的列彼此正交,满足 V r T V 0 = 0 V_r^TV_0=0 VrT​V0​=0,所以上式给出 A V 0 = 0 AV_0=0 AV0​=0:沿 V 0 V_0 V0​ 中任何一个方向改变参数,力矩变化都为零。

有了这两组方向,就可以将参数向量换到它们构成的坐标系中。 V V V 的各列彼此正交、长度为 1, θ \theta θ 沿这些轴分解为:

θ = V r β + V 0 γ . \theta=V_r\beta+V_0\gamma. θ=Vr​β+V0​γ.

β \beta β 是沿保留方向的坐标,共有 r r r 个分量; γ \gamma γ 是沿结构零方向的坐标。对于七轴模型, θ \theta θ 有 70 个分量, V r V_r Vr​ 有 70 行、 r r r 列, V 0 V_0 V0​ 有 70 行、 70 − r 70-r 70−r 列, γ \gamma γ 相应有 70 − r 70-r 70−r 个分量。两个矩阵分别把各组坐标组合成 70 维向量,相加后恢复完整的 θ \theta θ。

由于这些坐标轴正交且长度为 1,用 v j T θ v_j^T\theta vjT​θ 做投影,就能得到 θ \theta θ 沿第 j j j 个方向的坐标。将保留方向的投影排在一起,得到:

β = V r T θ . \beta=V_r^T\theta. β=VrT​θ.

这个式子定义了基参数与原参数之间的关系。 β \beta β 的每个分量都是原来那些缩放参数的线性组合。例如,第一个分量 v 1 T θ v_1^T\theta v1T​θ,就是按 v 1 v_1 v1​ 中的系数对 θ \theta θ 的各分量加权求和。

再将完整分解代入力矩方程。结构零方向满足 A V 0 = 0 AV_0=0 AV0​=0,因此:

y τ = A θ = A ( V r β + V 0 γ ) = A V r β + A V 0 γ = A V r β . \begin{aligned} y_\tau &=A\theta\\ &=A(V_r\beta+V_0\gamma)\\ &=AV_r\beta+AV_0\gamma\\ &=AV_r\beta. \end{aligned} yτ​​=Aθ=A(Vr​β+V0​γ)=AVr​β+AV0​γ=AVr​β.​

沿结构零方向的参数部分对力矩贡献为零,力矩便可以完全通过 β \beta β 计算。把乘在 β \beta β 前面的矩阵记为 W b W_b Wb​,再代入前面的 A = W D π A=WD_\pi A=WDπ​,就得到:

W b = A V r = W D π V r , y τ = W b β . W_b=AV_r=WD_\pi V_r,\qquad y_\tau=W_b\beta. Wb​=AVr​=WDπ​Vr​,yτ​=Wb​β.

沿用前面 43 个基参数的七轴例子,结构秩为 r = 43 r=43 r=43。完整的 V V V 为 70 × 70 70\times70 70×70,分成 70 × 43 70\times43 70×43 的 V r V_r Vr​ 和 70 × 27 70\times27 70×27 的 V 0 V_0 V0​。相应地, W b W_b Wb​ 有 43 列,待估的 β \beta β 有 43 个分量。原来用 70 个参数表示的力矩,便改用 43 个独立组合表示。由于 θ \theta θ 已经按参考尺度缩放,这些 SVD 基参数也没有单位。

实际计算时,先由结构分析得到 V r V_r Vr​。随后对辨识轨迹构造 W W W,沿用选定的 D π D_\pi Dπ​ 和 V r V_r Vr​,计算 W b = W D π V r W_b=WD_\pi V_r Wb​=WDπ​Vr​,就能用采集到的力矩数据直接估计 β \beta β。浮点运算中用残差检查上述等式的精度;若进一步截去较小的非零奇异值,则得到用于抑制噪声的近似模型。

用 QR 从原回归列构造基参数

列主元 QR 提供另一种构造基参数模型的方法:从原回归矩阵中选出独立列,把其余列的贡献合并到这些列上。

先看一个三列的例子。设 W = [ w 1   w 2   w 3 ] W=[w_1\ w_2\ w_3] W=[w1​ w2​ w3​],对应三个同单位参数 p 1 , p 2 , p 3 p_1,p_2,p_3 p1​,p2​,p3​。其中 w 1 , w 2 w_1,w_2 w1​,w2​ 独立,第三列满足 w 3 = 2 w 1 + w 2 w_3=2w_1+w_2 w3​=2w1​+w2​。代入力矩方程,有:

y τ = w 1 p 1 + w 2 p 2 + w 3 p 3 = w 1 p 1 + w 2 p 2 + ( 2 w 1 + w 2 ) p 3 = w 1 ( p 1 + 2 p 3 ) + w 2 ( p 2 + p 3 ) . \begin{aligned} y_\tau &=w_1p_1+w_2p_2+w_3p_3\\ &=w_1p_1+w_2p_2+(2w_1+w_2)p_3\\ &=w_1(p_1+2p_3)+w_2(p_2+p_3). \end{aligned} yτ​​=w1​p1​+w2​p2​+w3​p3​=w1​p1​+w2​p2​+(2w1​+w2​)p3​=w1​(p1​+2p3​)+w2​(p2​+p3​).​

把两个括号分别记为 β 1 , β 2 \beta_1,\beta_2 β1​,β2​,就得到:

β = [ p 1 + 2 p 3 p 2 + p 3 ] , W b = [ w 1   w 2 ] , y τ = W b β . \beta=\begin{bmatrix}p_1+2p_3\\p_2+p_3\end{bmatrix},\qquad W_b=[w_1\ w_2],\qquad y_\tau=W_b\beta. β=[p1​+2p3​p2​+p3​​],Wb​=[w1​ w2​],yτ​=Wb​β.

后续直接估计 β 1 , β 2 \beta_1,\beta_2 β1​,β2​,待估参数由三个减少到两个。 推广到机械臂,需要先确定 W b W_b Wb​ 的列,以及 β \beta β 对应的参数组合,再用力矩数据估计这些组合的数值。

先假设已选出一组能够表示 W W W 全部列的独立列,组成 W s W_{\mathrm{s}} Ws​,数量记为 r r r;其余列组成 W d W_{\mathrm{d}} Wd​。两组列满足:

W d = W s K . W_{\mathrm{d}}=W_{\mathrm{s}}K. Wd​=Ws​K.

K K K 的每一列记录 W d W_{\mathrm{d}} Wd​ 中对应列的组合系数。开头的例子中, W s = [ w 1   w 2 ] W_{\mathrm{s}}=[w_1\ w_2] Ws​=[w1​ w2​]、 W d = w 3 W_{\mathrm{d}}=w_3 Wd​=w3​,相应的 K = [ 2   1 ] T K=[2\ 1]^T K=[2 1]T。

将原参数也分成两组: π s \pi_{\mathrm{s}} πs​ 对应 W s W_{\mathrm{s}} Ws​, π d \pi_{\mathrm{d}} πd​ 对应 W d W_{\mathrm{d}} Wd​。把列之间的关系代入力矩方程,有:

y τ = W s π s + W d π d = W s π s + W s K π d = W s ( π s + K π d ) = W b β . \begin{aligned} y_\tau &=W_{\mathrm{s}}\pi_{\mathrm{s}}+W_{\mathrm{d}}\pi_{\mathrm{d}}\\ &=W_{\mathrm{s}}\pi_{\mathrm{s}}+W_{\mathrm{s}}K\pi_{\mathrm{d}}\\ &=W_{\mathrm{s}}(\pi_{\mathrm{s}}+K\pi_{\mathrm{d}}) =W_b\beta. \end{aligned} yτ​​=Ws​πs​+Wd​πd​=Ws​πs​+Ws​Kπd​=Ws​(πs​+Kπd​)=Wb​β.​

这就给出了两个量的构造方式:

W b = W s , β = π s + K π d . W_b=W_{\mathrm{s}},\qquad \beta=\pi_{\mathrm{s}}+K\pi_{\mathrm{d}}. Wb​=Ws​,β=πs​+Kπd​.

接下来计算独立列及其组合系数。QR 分解将矩阵写成两个矩阵的乘积: Q Q Q 的列彼此正交、长度为 1, R R R 记录各列沿这些方向的系数,主对角线下方为零。处理一列时,先扣除它沿已有正交方向的分量,剩余部分体现这列增加的独立信息。

普通 QR 和带列主元的 QR,区别在于处理列的顺序:

分解方式处理列的顺序分解关系
普通 QR按输入矩阵的原列顺序处理 W = Q R W=QR W=QR
带列主元的 QR每一步优先选择剩余部分最长的列,用 P P P 记录列重排 W P = Q R WP=QR WP=QR

本节采用带列主元的 QR,使提供新独立信息的列优先排到前面,便于选出基参数所需的回归列。

P P P 可以由单位矩阵交换列得到,每行、每列恰好有一个 1,其余元素为零。例如,将三列按第 3、1、2 列重新排列,可写成:

P = [ 0 1 0 0 0 1 1 0 0 ] , [ w 1   w 2   w 3 ] P = [ w 3   w 1   w 2 ] . P=\begin{bmatrix}0&1&0\\0&0&1\\1&0&0\end{bmatrix},\qquad [w_1\ w_2\ w_3]P=[w_3\ w_1\ w_2]. P= ​001​100​010​ ​,[w1​ w2​ w3​]P=[w3​ w1​ w2​].

P P P 的第一列为 [ 0   0   1 ] T [0\ 0\ 1]^T [0 0 1]T,乘积的第一列便是 0 w 1 + 0 w 2 + 1 w 3 = w 3 0w_1+0w_2+1w_3=w_3 0w1​+0w2​+1w3​=w3​,其余两列同理。实际调用带列主元的 QR 函数时,输入 W W W,输出 Q , R Q,R Q,R 和列排列。 程序通常用索引数组 piv 保存排列,W[:, piv] 就实现了 W P WP WP。

第 j j j 步选中列的剩余长度,等于第 j j j 个对角元素 R j , j R_{j,j} Rj,j​ 的绝对值。每次优先选最长的剩余列,随后再从其余列中扣除新方向的分量,因此在精确计算下,对角元素的绝对值满足:

∣ R 1 , 1 ∣ ≥ ∣ R 2 , 2 ∣ ≥ ∣ R 3 , 3 ∣ ≥ ⋯   . |R_{1,1}|\geq|R_{2,2}|\geq|R_{3,3}|\geq\cdots. ∣R1,1​∣≥∣R2,2​∣≥∣R3,3​∣≥⋯.

这里的重排作用在 W W W 的列上, R R R 随分解过程一同算出。上述顺序表示各步新增独立信息的大小; R R R 的非对角元素记录沿已有方向的系数,按各列对应的位置保存。根据阈值判断这些对角元素是否接近零,就能确定数值秩 r r r,并保留前 r r r 个独立列。

重排后的两组列及其对应参数为:

W P = [ W s   W d ] , P T π = [ π s π d ] . WP=[W_{\mathrm{s}}\ W_{\mathrm{d}}],\qquad P^T\pi=\begin{bmatrix}\pi_{\mathrm{s}}\\\pi_{\mathrm{d}}\end{bmatrix}. WP=[Ws​ Wd​],PTπ=[πs​πd​​].

右乘 P P P 调整回归列,左乘 P T P^T PT 同步调整参数顺序。由于 P P T = I PP^T=I PPT=I,有 ( W P ) ( P T π ) = W π (WP)(P^T\pi)=W\pi (WP)(PTπ)=Wπ,原力矩关系保持相同。

最后求 K K K。取 Q Q Q 的前 r r r 列为 Q r Q_r Qr​,将 R R R 的前 r r r 行分成两块:前 r r r 列组成 R 11 R_{11} R11​,其余列组成 R 12 R_{12} R12​。按列依赖的精确关系,有:

W P = Q r [ R 11   R 12 ] , W s = Q r R 11 , W d = Q r R 12 . WP=Q_r[ R_{11}\ R_{12}],\qquad W_{\mathrm{s}}=Q_rR_{11},\quad W_{\mathrm{d}}=Q_rR_{12}. WP=Qr​[R11​ R12​],Ws​=Qr​R11​,Wd​=Qr​R12​.

把后两式代入要满足的 W d = W s K W_{\mathrm{d}}=W_{\mathrm{s}}K Wd​=Ws​K,再左乘 Q r T Q_r^T QrT​。由于 Q r T Q r = I Q_r^TQ_r=I QrT​Qr​=I,得到:

Q r R 12 = Q r R 11 K ⟹ R 11 K = R 12 . Q_rR_{12}=Q_rR_{11}K \quad\Longrightarrow\quad R_{11}K=R_{12}. Qr​R12​=Qr​R11​K⟹R11​K=R12​.

R 11 R_{11} R11​ 是可逆的上三角矩阵, R 12 R_{12} R12​ 也已由分解得到,求解这个方程就能算出 K K K。

以七轴模型为例, N N N 表示采样时刻数,每个时刻提供七个关节的力矩方程。因此,一帧的 Y Y Y 为 7 × 70 7\times70 7×70,堆叠后的 W W W 为 7 N × 70 7N\times70 7N×70。当 7 N ≥ 70 7N\geq70 7N≥70 时,采用精简 QR, Q Q Q 为 7 N × 70 7N\times70 7N×70, R R R 为 70 × 70 70\times70 70×70。

若判断得到 r = 43 r=43 r=43, R R R 保持上述尺寸,取出的 R 11 R_{11} R11​ 为 43 × 43 43\times43 43×43, R 12 R_{12} R12​ 与 K K K 为 43 × 27 43\times27 43×27。此时 W b W_b Wb​ 有 43 列,待估向量 β \beta β 有 43 个分量,原来的 70 个参数便合并为 43 个组合。

上述结构分析采用覆盖广泛运动的状态,并用另一批状态验证列依赖关系。确定这套关系后,对实际辨识轨迹构造 W W W、取出相同列组成 W b W_b Wb​,再用测得的力矩直接估计 β \beta β。

回归模型与连杆参数的对应关系

将本节的几种表示放在一起,忽略测量误差时,有:

y τ = W π = W D π θ = W b β . y_\tau=W\pi=WD_\pi\theta=W_b\beta. yτ​=Wπ=WDπ​θ=Wb​β.

W W W 由各帧的 Y ( q , q ˙ , q ¨ ) Y(q,\dot q,\ddot q) Y(q,q˙​,q¨​) 按行堆叠而成,包含全部标准参数对应的回归列; W b W_b Wb​ 则与最小参数集 β \beta β 配套。单帧的关系写成 τ r b = Y b β \tau_{\mathrm{rb}}=Y_b\beta τrb​=Yb​β,堆叠整段数据后写成 y τ = W b β y_\tau=W_b\beta yτ​=Wb​β。

完整连杆参数用 π \pi π 表示,每根连杆占十个分量,包含质量、一阶质量矩和关于连杆原点的惯量。 θ \theta θ 表示这些参数相对于参考尺度的无量纲倍数。七轴模型的尺度矩阵为:

D π = diag ⁡ ( d 1 , … , d 70 ) , π = D π θ . D_\pi=\operatorname{diag}(d_1,\ldots,d_{70}),\qquad \pi=D_\pi\theta. Dπ​=diag(d1​,…,d70​),π=Dπ​θ.

每个 d j d_j dj​ 都是预先选定的正参考量,携带对应参数的单位,可由 CAD 数据的量级和连杆尺寸确定。 D π D_\pi Dπ​ 在辨识过程中保持固定。两种基参数构造方法分别给出:

方法基参数回归矩阵 W b W_b Wb​基参数的定义
对 W D π WD_\pi WDπ​ 作 SVD W D π V r WD_\pi V_r WDπ​Vr​ β = V r T θ \beta=V_r^T\theta β=VrT​θ
对原矩阵 W W W 作列主元 QR W s W_{\mathrm{s}} Ws​ β = π s + K π d \beta=\pi_{\mathrm{s}}+K\pi_{\mathrm{d}} β=πs​+Kπd​

两行各自定义一套 W b , β W_b,\beta Wb​,β,计算时按所选方法配套使用。上文的 QR 直接合并带单位的原参数, D π D_\pi Dπ​ 用于 π \pi π 与 θ \theta θ 之间的换算。

估计得到 β ^ \widehat\beta β ​ 后,可以按下面的关系构造完整参数,帽号标记估计值。采用 SVD 表示时:

θ ^ = V r β ^ + V 0 γ , π ^ = D π θ ^ . \widehat\theta=V_r\widehat\beta+V_0\gamma,\qquad \widehat\pi=D_\pi\widehat\theta. θ =Vr​β ​+V0​γ,π =Dπ​θ .

V r β ^ V_r\widehat\beta Vr​β ​ 给出由基参数确定的部分, γ \gamma γ 则决定结构零方向上的分量。由于 W D π V 0 = 0 WD_\pi V_0=0 WDπ​V0​=0,改变 γ \gamma γ 会得到具有相同预测力矩的不同连杆参数。

已有 CAD 参数时,可以保留它在结构零方向上的坐标。将 CAD 参数记为 π C A D \pi_{\mathrm{CAD}} πCAD​,先换成同一套缩放坐标 θ C A D = D π − 1 π C A D \theta_{\mathrm{CAD}}=D_\pi^{-1}\pi_{\mathrm{CAD}} θCAD​=Dπ−1​πCAD​,再取:

γ = V 0 T θ C A D , θ ^ = V r β ^ + V 0 V 0 T θ C A D . \gamma=V_0^T\theta_{\mathrm{CAD}},\qquad \widehat\theta =V_r\widehat\beta+V_0V_0^T\theta_{\mathrm{CAD}}. γ=V0T​θCAD​,θ =Vr​β ​+V0​V0T​θCAD​.

这样,力矩数据可以观察到的坐标由辨识结果更新,结构零方向上的坐标由 CAD 补充。由于 V r V_r Vr​ 与 V 0 V_0 V0​ 正交,在满足 V r T θ ^ = β ^ V_r^T\widehat\theta=\widehat\beta VrT​θ =β ​ 的解中,这一选择使缩放参数与 θ C A D \theta_{\mathrm{CAD}} θCAD​ 的欧氏距离最小。若取 γ = 0 \gamma=0 γ=0,则得到 θ ^ = V r β ^ \widehat\theta=V_r\widehat\beta θ =Vr​β ​,对应缩放空间中的最小范数解;它可作为缺少先验时的一种计算选择。

采用 QR 表示时,由 β = π s + K π d \beta=\pi_{\mathrm{s}}+K\pi_{\mathrm{d}} β=πs​+Kπd​ 可知,选定剩余参数 π d \pi_{\mathrm{d}} πd​ 后,就能依次恢复:

π ^ s = β ^ − K π d , π ^ = P [ π ^ s π d ] , θ ^ = D π − 1 π ^ . \widehat\pi_{\mathrm{s}}=\widehat\beta-K\pi_{\mathrm{d}},\qquad \widehat\pi=P\begin{bmatrix}\widehat\pi_{\mathrm{s}}\\\pi_{\mathrm{d}}\end{bmatrix},\qquad \widehat\theta=D_\pi^{-1}\widehat\pi. π s​=β ​−Kπd​,π =P[π s​πd​​],θ =Dπ−1​π .

第二步用 P P P 将两组参数放回原连杆顺序,第三步再换算为无量纲表示。一种容易实现的选择,是固定 π d \pi_{\mathrm{d}} πd​ 为对应的 CAD 值。将 CAD 参数按 P T P^T PT 重排,把其中与 W d W_{\mathrm{d}} Wd​ 对应的分量记为 π d , C A D \pi_{\mathrm{d,CAD}} πd,CAD​,令:

π d = π d , C A D , π ^ s = β ^ − K π d , C A D . \pi_{\mathrm{d}}=\pi_{\mathrm{d,CAD}},\qquad \widehat\pi_{\mathrm{s}}=\widehat\beta-K\pi_{\mathrm{d,CAD}}. πd​=πd,CAD​,π s​=β ​−Kπd,CAD​.

这会保留选定的原参数值,并根据辨识结果调整其余参数。若希望整组参数共同围绕 CAD 调整,可以将这些自由分量交给优化求解,再加入相应的先验和物理条件。

七轴例子中,SVD 的 γ \gamma γ 和 QR 的 π d \pi_{\mathrm{d}} πd​ 都有 27 个自由分量。43 个基参数确定了力矩中的独立组合,这 27 个自由分量则由先验和独立测量补充取值依据。 恢复实际连杆模型时,还要检查质量为正、质心范围和惯量一致性,并在需要时把这些条件一起纳入参数恢复的优化。已知某根连杆的称重结果时,也可将其作为约束;含测量噪声时,可在允许的拟合误差内恢复物理参数。相关物理约束见第 6.5 节。

得到 π ^ \widehat\pi π 后,按每根连杆的十个分量取出质量 m m m、一阶质量矩 h m h_m hm​ 和惯量 I O I_O IO​,用 r C = h m / m r_C=h_m/m rC​=hm​/m 恢复质心,再通过平行轴定理换算质心惯量 I C I_C IC​。这样就接回了前面用于逆动力学的连杆模型。下一节先介绍怎样从实测力矩求出 β ^ \widehat\beta β ​。

4.4 从测量力矩估计基参数

整理实际测量值

有了基参数形式,便可以使用实验数据估计 β \beta β。每个采样时刻需要对应的 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​ 和关节驱动力矩;其中加速度常由运动数据估计。力矩可以来自关节输出端传感器,也可以由实际电机电流换算。模型包含的惯量与摩擦应和力矩测量位置对应,沿用第三章的约定。

驱动器测得的力矩中还可能含有摩擦和测量偏置。以第 j j j 个转动关节为例,在运动速度离开零点时,采用黏性与库仑摩擦模型,可写成:

τ m , j = ( Y π ) j + f v , j q ˙ j + f c , j sgn ⁡ ( q ˙ j ) + b j + ε j . \tau_{m,j}=(Y\pi)_j +f_{v,j}\dot q_j+f_{c,j}\operatorname{sgn}(\dot q_j) +b_j+\varepsilon_j. τm,j​=(Yπ)j​+fv,j​q˙​j​+fc,j​sgn(q˙​j​)+bj​+εj​.

τ m , j \tau_{m,j} τm,j​ 是测得的驱动力矩, ( Y π ) j (Y\pi)_j (Yπ)j​ 是该关节的刚体力矩。 f v , j f_{v,j} fv,j​ 是黏性摩擦系数,单位为 N·m·s/rad; f c , j f_{c,j} fc,j​ 是库仑摩擦幅值,单位为 N·m。符号函数 sgn ⁡ \operatorname{sgn} sgn 在速度为正时取 1、为负时取 − 1 -1 −1,用于表示运动方向。 b j b_j bj​ 是常量测量偏置, ε j \varepsilon_j εj​ 汇集噪声和未建模作用,二者单位均为 N·m。本式取外部接触为零;若有已知环境力矩,按第二章的符号约定,将它加到测得的驱动力矩上再建立回归。

摩擦与偏置参数已知时,先从测量中扣除它们;需要一起辨识时,就把速度、速度符号和常数 1 作为额外回归列,与惯性参数一同求解。后面用 y τ y_\tau yτ​ 表示处理后的力矩观测,集中说明基参数的估计方法。

用最小二乘拟合整段力矩

考虑测量误差后,回归关系写为:

y τ = W b β + ε , β ^ = argmin ⁡ β    1 2 ∥ W b β − y τ ∥ 2 . y_\tau=W_b\beta+\varepsilon,\qquad \widehat\beta=\underset{\beta}{\operatorname{argmin}}\; \frac12\|W_b\beta-y_\tau\|^2. yτ​=Wb​β+ε,β ​=βargmin​21​∥Wb​β−yτ​∥2.

ε \varepsilon ε 是整段数据的误差向量,帽号表示估计值, argmin ⁡ \operatorname{argmin} argmin 表示取使目标最小的参数。 W b β − y τ W_b\beta-y_\tau Wb​β−yτ​ 是预测力矩与测量力矩的差;二范数平方就是把各帧、各关节的差分别平方再相加。最小二乘寻找一组参数,让整段力矩尽量贴近测量。

若各关节的测量噪声大小不同,可以按噪声水平加权。设误差协方差为正定矩阵 Σ \Sigma Σ,其对角项记录误差方差,非对角项记录误差之间的相关性。取:

A = Σ − 1 / 2 W b , y = Σ − 1 / 2 y τ , min ⁡ β 1 2 ∥ A β − y ∥ 2 . A=\Sigma^{-1/2}W_b,\qquad y=\Sigma^{-1/2}y_\tau, \qquad \min_\beta\frac12\|A\beta-y\|^2. A=Σ−1/2Wb​,y=Σ−1/2yτ​,βmin​21​∥Aβ−y∥2.

Σ − 1 / 2 \Sigma^{-1/2} Σ−1/2 是协方差矩阵平方根的逆,这个变换称为白化。对相互独立的误差,它相当于将每条力矩方程除以该测量的噪声标准差。噪声较小的观测因而获得更大的权重。这里 A , y A,y A,y 表示本节加权后的矩阵和观测。

求解时通常直接对 A A A 使用 QR 或 SVD。正规方程 A T A β = A T y A^TA\beta=A^Ty ATAβ=ATy 也能表达最优条件;直接分解 A A A 有利于保留数值精度,因为形成 A T A A^TA ATA 会把矩阵的条件数平方。对满列秩矩阵,二范数条件数为 κ 2 ( A ) = σ max ⁡ / σ min ⁡ \kappa_2(A)=\sigma_{\max}/\sigma_{\min} κ2​(A)=σmax​/σmin​,其中最大、最小奇异值的比值反映不同参数方向的强弱差异。

弱信息方向与正则化

当一个参数方向的奇异值很小时,即使沿这个方向较大幅度地改变参数,预测力矩也只变化一点。测量中的小幅噪声因此可能对应很大的参数偏移。在回归矩阵准确、参数尺度固定、力矩噪声已白化的条件下,SVD 各方向的参数估计方差与 1 / σ j 2 1/\sigma_j^2 1/σj2​ 成正比。奇异值缩小十倍,该方向的方差就放大一百倍。

如果有 CAD 或先前标定结果,可以加入先验,限制参数在弱信息方向上的偏移:

min ⁡ β 1 2 ∥ A β − y ∥ 2 + λ 2 ∥ L ( β − β 0 ) ∥ 2 . \min_\beta\frac12\|A\beta-y\|^2 +\frac{\lambda}{2}\|L(\beta-\beta_0)\|^2. βmin​21​∥Aβ−y∥2+2λ​∥L(β−β0​)∥2.

β 0 \beta_0 β0​ 是先验参数, L L L 决定如何衡量各方向的参数偏差, λ ≥ 0 \lambda\geq0 λ≥0 控制先验项的整体权重。这种处理称为正则化。第一项鼓励模型贴近测量,第二项鼓励参数靠近先验; λ \lambda λ 越大,先验的影响越强。数据充分的方向主要由测量确定,信息较弱的方向更多借助先验。

一种常用设置是为每个基参数选一个正的参考偏差 s j s_j sj​,取:

L = diag ⁡ ( 1 / s 1 , … , 1 / s r ) , ∥ L ( β − β 0 ) ∥ 2 = ∑ j = 1 r ( β j − β 0 , j s j ) 2 . \begin{aligned} L&=\operatorname{diag}(1/s_1,\ldots,1/s_r),\\ \|L(\beta-\beta_0)\|^2 &=\sum_{j=1}^{r}\left(\frac{\beta_j-\beta_{0,j}}{s_j}\right)^2. \end{aligned} L∥L(β−β0​)∥2​=diag(1/s1​,…,1/sr​),=j=1∑r​(sj​βj​−β0,j​​)2.​

这里 r r r 是基参数个数, β 0 , j \beta_{0,j} β0,j​ 是第 j j j 个参数的先验值。每个参数的偏差先除以自己的参考偏差,再平方相加,便统一了偏差尺度。 s j s_j sj​ 与对应参数具有相同单位,可根据 CAD 的可信程度或以往标定误差选取;对同样的偏差, s j s_j sj​ 越小,惩罚越大,表示越希望该参数贴近先验。若只约束部分参数,可以保留 L L L 中对应的行;若要专门约束弱信息方向,则先统一参数尺度,再用 SVD 找出弱方向,将沿这些方向的投影与尺度缩放一起写入 L L L。

正则化改善的是现有数据下的求解稳定性。若希望从测量本身更准确地确定可辨识的参数组合,就需要让机械臂做出能明显激发相应惯性作用的运动,这也把参数求解与激励设计联系了起来。

实际中,回归矩阵还依赖测得的运动:速度、加速度的估计误差会改变参数前面的系数,运动与力矩的时间错位也会改变拟合结果。因此,数据同步、滤波和导数估计直接关系到辨识精度,第五章会结合采样过程继续展开。

Logo

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

更多推荐