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

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

第五章:最优激励与测量数据处理

第四章已经确定了待估的基参数。接下来需要安排一段机械臂运动,让这些参数对力矩的影响充分表现出来。例如,改变姿态可以观察重力作用,反复加减速可以观察惯性作用,多关节协调运动可以呈现连杆之间的耦合。为辨识专门设计的这类运动,称为激励轨迹。

最优激励轨迹的目标,是在关节范围、驱动能力和碰撞约束内,让采集到的运动与力矩更有助于确定参数。整个过程从评价轨迹的信息量开始,再设计连续运动、求解轨迹系数,最后执行采集并验证辨识结果。

5.1 用信息矩阵评价激励轨迹

先看一个抬臂动作。机械臂缓慢抬起时,关节力矩主要呈现姿态变化引起的重力作用;在相近的姿态范围内增加加减速,惯性作用会更加明显。肩、肘同时运动时,各连杆对关节力矩的共同影响又会发生变化。设计激励轨迹,就是利用这些变化,让各个待估参数在数据中留下足够强、容易区分的作用。

沿用第四章处理好摩擦、偏置和接触后的刚体回归模型:

y τ = W b β + ε . y_\tau=W_b\beta+\varepsilon. yτ​=Wb​β+ε.

β \beta β 包含 r r r 个基参数, y τ y_\tau yτ​ 是整段采样的关节力矩, ε \varepsilon ε 是测量误差。 W b W_b Wb​ 由每帧的基参数回归矩阵 Y b , k = Y b ( q k , q ˙ k , q ¨ k ) Y_{b,k}=Y_b(q_k,\dot q_k,\ddot q_k) Yb,k​=Yb​(qk​,q˙​k​,q¨​k​) 竖向堆叠而成; k k k 是采样编号, q k , q ˙ k , q ¨ k q_k,\dot q_k,\ddot q_k qk​,q˙​k​,q¨​k​ 是这一帧的关节位置、速度和加速度。对于 n n n 轴机械臂,采集 N N N 帧后, W b W_b Wb​ 的大小为 n N × r nN\times r nN×r。

改变轨迹,就会改变这些运动状态以及 W b W_b Wb​ 中的系数。如果两列在整段运动中几乎成比例,对应参数的作用就很相似,测量噪声容易使它们的估计值相互补偿。通过调整各关节的姿态、速度和加速度,可以让这些列呈现更明显的差别。

统一比较尺度

评价轨迹时,要看参数变化引起的力矩变化,相对于测量噪声有多明显。这里需要先约定两件事:每个参数改变多少算作一个参考幅度,每条力矩测量有多大的噪声。前者决定回归矩阵各列的尺度,后者决定各行的权重。

沿用第四章的单轴抬臂例子,把绕关节轴的惯量简记为 I I I,一阶质量矩记为 h = m ℓ h=m\ell h=mℓ,两个待估参数组成 β = [ I , h ] T \beta=[I,h]^T β=[I,h]T。取两帧示意数据,角加速度分别为 2、4 rad/s²,重力系数 g 0 sin ⁡ q g_0\sin q g0​sinq 分别为 1、 − 2 -2 −2 m/s²,得到:

[ τ 1 τ 2 ] = [ 2 1 4 − 2 ] ⏟ W b [ I h ] + ε . \begin{bmatrix}\tau_1\\\tau_2\end{bmatrix} =\underbrace{\begin{bmatrix}2&1\\4&-2\end{bmatrix}}_{W_b} \begin{bmatrix}I\\h\end{bmatrix}+\varepsilon. [τ1​τ2​​]=Wb​ [24​1−2​]​​[Ih​]+ε.

τ 1 , τ 2 \tau_1,\tau_2 τ1​,τ2​ 表示两个采样时刻的同一关节力矩,单位为 N·m;第一列乘以惯量,第二列乘以一阶质量矩。下面的矩阵数值都按本例列出的单位填写。

本例假设 CAD 给出的惯量为 I C A D = 0.5 I_{\mathrm{CAD}}=0.5 ICAD​=0.5 kg·m²,一阶质量矩为 h C A D = 2 h_{\mathrm{CAD}}=2 hCAD​=2 kg·m。取这两个正值作为参考尺度,放在对角矩阵 S β S_\beta Sβ​ 中,并在比较候选轨迹时保持固定。将实际待估参数写成“CAD 参考值乘以待估倍数”:

β = S β β ˉ , S β = [ 0.5 0 0 2 ] . \beta=S_\beta\bar\beta,\qquad S_\beta=\begin{bmatrix}0.5&0\\0&2\end{bmatrix}. β=Sβ​βˉ​,Sβ​=[0.50​02​].

β ˉ \bar\beta βˉ​ 的两个分量是 β ˉ 1 = I / I C A D \bar\beta_1=I/I_{\mathrm{CAD}} βˉ​1​=I/ICAD​ 和 β ˉ 2 = h / h C A D \bar\beta_2=h/h_{\mathrm{CAD}} βˉ​2​=h/hCAD​,都没有单位。CAD 参数本身对应 β ˉ = [ 1 , 1 ] T \bar\beta=[1,1]^T βˉ​=[1,1]T;辨识时, β ˉ 1 , β ˉ 2 \bar\beta_1,\bar\beta_2 βˉ​1​,βˉ​2​ 是要由测量数据确定的倍数。把这组待估量保留在方程中,将 β = S β β ˉ \beta=S_\beta\bar\beta β=Sβ​βˉ​ 代入,有:

y τ = W b β + ε = W b ( S β β ˉ ) + ε = [ 2 1 4 − 2 ] [ 0.5 0 0 2 ] [ β ˉ 1 β ˉ 2 ] + ε = [ 1 2 2 − 4 ] ⏟ W b S β [ β ˉ 1 β ˉ 2 ] + ε . \begin{aligned} y_\tau &=W_b\beta+\varepsilon\\ &=W_b(S_\beta\bar\beta)+\varepsilon\\ &=\begin{bmatrix}2&1\\4&-2\end{bmatrix} \begin{bmatrix}0.5&0\\0&2\end{bmatrix} \begin{bmatrix}\bar\beta_1\\\bar\beta_2\end{bmatrix}+\varepsilon\\ &=\underbrace{\begin{bmatrix}1&2\\2&-4\end{bmatrix}}_{W_bS_\beta} \begin{bmatrix}\bar\beta_1\\\bar\beta_2\end{bmatrix}+\varepsilon. \end{aligned} yτ​​=Wb​β+ε=Wb​(Sβ​βˉ​)+ε=[24​1−2​][0.50​02​][βˉ​1​βˉ​2​​]+ε=Wb​Sβ​ [12​2−4​]​​[βˉ​1​βˉ​2​​]+ε.​

这里 y τ = [ τ 1 , τ 2 ] T y_\tau=[\tau_1,\tau_2]^T yτ​=[τ1​,τ2​]T。新的回归矩阵 W b S β W_bS_\beta Wb​Sβ​ 已由运动数据和参考尺度算出,后面的 β ˉ \bar\beta βˉ​ 保留为未知量。求得这些倍数后,再乘参考尺度还原参数。例如,若后续辨识得到 β ˉ ^ 1 = 0.8 \widehat{\bar\beta}_1=0.8 βˉ​ ​1​=0.8,则估计惯量为 0.5 × 0.8 = 0.4 0.5\times0.8=0.4 0.5×0.8=0.4 kg·m²。

右乘 S β S_\beta Sβ​,就是将第一列乘以 0.5、第二列乘以 2。现在第一列表示惯量增加一个参考幅度时,两帧力矩分别增加 1、2 N·m;第二列表示一阶质量矩增加一个参考幅度时,两帧力矩分别变化 2、 − 4 -4 −4 N·m。各列都以“参数改变一个参考幅度”来比较。

预先估计噪声并固定比较权重

参数尺度确定后,再用噪声标准差给力矩方程加权。本章采用以下流程:优化前先做少量预采样,给每个关节估计一个有代表性的标准差;同一轮轨迹优化固定使用这组数值,执行所得轨迹后再复核噪声水平。

预采样先采用静态保持。在预期工作区域内选几种常用姿态,例如工作区中部的弯臂姿态和正常伸展姿态,保持工具、负载与预期辨识工况一致。让机械臂在每个姿态静止,稳定后连续记录关节力矩,采用与后续辨识一致的采样频率和滤波设置。对第 j j j 个关节在一个姿态下的 N s N_s Ns​ 个样本,计算:

τ ˉ j = 1 N s ∑ s = 1 N s τ j , s , σ ^ j = 1 N s − 1 ∑ s = 1 N s ( τ j , s − τ ˉ j ) 2 . \bar\tau_j=\frac{1}{N_s}\sum_{s=1}^{N_s}\tau_{j,s},\qquad \widehat\sigma_j= \sqrt{\frac{1}{N_s-1}\sum_{s=1}^{N_s}(\tau_{j,s}-\bar\tau_j)^2}. τˉj​=Ns​1​s=1∑Ns​​τj,s​,σ j​=Ns​−11​s=1∑Ns​​(τj,s​−τˉj​)2 ​.

N s > 1 N_s>1 Ns​>1 是用于估计噪声的样本数, τ j , s \tau_{j,s} τj,s​ 是第 s s s 次记录的该关节力矩, τ ˉ j \bar\tau_j τˉj​ 是平均值, σ ^ j \widehat\sigma_j σ j​ 是标准差估计,单位为 N·m。平均值对应维持姿态所需的力矩,围绕均值的波动包含测量噪声、驱动波动和细小姿态扰动,可先作为辨识加权的噪声尺度。各姿态取相同数量的样本、分别去均值后,可以将各姿态的方差取平均再开平方,得到该关节的代表性标准差。

采用观测误差相互独立的近似时,将各条力矩方程对应的标准差平方填入 Σ \Sigma Σ 的对角线,其余元素取零。按上述固定关节噪声模型,同一关节的各帧采用相同权重;不同关节按各自的噪声水平加权。一次优化中的所有候选轨迹都使用这套 Σ \Sigma Σ,因此具有共同的比较依据。

为展示不同观测权重的矩阵运算,下面的两帧算例仍预先给定两个噪声水平:两帧噪声相互独立,标准差分别为 1 N·m 和 2 N·m。相同的 1 N·m 力矩变化,在第一帧中相当于一个噪声标准差,在第二帧中相当于半个。为体现这种差别,将每条力矩方程都除以自己的噪声标准差。

协方差矩阵的对角元素记录噪声方差,因此本例中 Σ = diag ⁡ ( 1 , 4 ) \Sigma=\operatorname{diag}(1,4) Σ=diag(1,4),方差的单位为 (N·m)²。取平方根后是标准差 1、2,再取逆就得到 Σ − 1 / 2 = diag ⁡ ( 1 , 1 / 2 ) \Sigma^{-1/2}=\operatorname{diag}(1,1/2) Σ−1/2=diag(1,1/2)。用它左乘刚才的矩阵:

A = Σ − 1 / 2 W b S β = [ 1 0 0 1 / 2 ] [ 1 2 2 − 4 ] = [ 1 2 1 − 2 ] . A=\Sigma^{-1/2}W_bS_\beta =\begin{bmatrix}1&0\\0&1/2\end{bmatrix} \begin{bmatrix}1&2\\2&-4\end{bmatrix} =\begin{bmatrix}1&2\\1&-2\end{bmatrix}. A=Σ−1/2Wb​Sβ​=[10​01/2​][12​2−4​]=[11​2−2​].

左乘这个对角矩阵,就是第一行除以 1、第二行除以 2。测量力矩和误差也接受相同处理,处理后的噪声标准差都为 1,这称为白化。至此, A A A 中的每个数都表示:一个参数改变一个参考幅度时,该条观测的力矩变化相当于几个噪声标准差。例如, A A A 的第二列为 [ 2 , − 2 ] T [2,-2]^T [2,−2]T,表示两帧变化方向相反,幅度都达到各自噪声标准差的两倍。

把各次观测汇成信息矩阵

固定 S β S_\beta Sβ​ 和噪声权重后,每条候选轨迹都会给出自己的 W b W_b Wb​ 与 A A A。最小二乘用平方误差衡量拟合,因此将 A A A 各列的平方和及相互关系汇总为:

F = A T A = [ 1 1 2 − 2 ] [ 1 2 1 − 2 ] = [ 2 0 0 8 ] . \mathcal F=A^TA =\begin{bmatrix}1&1\\2&-2\end{bmatrix} \begin{bmatrix}1&2\\1&-2\end{bmatrix} =\begin{bmatrix}2&0\\0&8\end{bmatrix}. F=ATA=[12​1−2​][11​2−2​]=[20​08​].

F \mathcal F F 称为信息矩阵。本例中的 2 来自第一列的平方和 1 2 + 1 2 1^2+1^2 12+12,8 来自第二列的平方和 2 2 + ( − 2 ) 2 2^2+(-2)^2 22+(−2)2。它们分别汇总了两个参数变化在全部观测中的作用强度。交叉项为 1 × 2 + 1 × ( − 2 ) = 0 1\times2+1\times(-2)=0 1×2+1×(−2)=0,表示两列正交:第一个参数让两帧力矩同向变化,第二个参数让它们反向变化,两种作用容易区分。

对于具有 r r r 个基参数的机械臂, A A A 有 r r r 列, F \mathcal F F 就是 r × r r\times r r×r 的矩阵。对角项是各列的平方和,交叉项是两列的内积;结合各列大小考察这些内积,可以判断不同参数的作用是否相似。

计算时,也可以逐帧累加信息。设各帧噪声相互独立,将第 k k k 帧处理后的回归矩阵记为 A k = Σ k − 1 / 2 Y b , k S β A_k=\Sigma_k^{-1/2}Y_{b,k}S_\beta Ak​=Σk−1/2​Yb,k​Sβ​,其中 Σ k \Sigma_k Σk​ 是这一帧的噪声协方差。整段矩阵 A A A 由各个 A k A_k Ak​ 竖向堆叠,直接做分块矩阵乘法就得到:

F = [ A 1 T ⋯ A N T ] [ A 1 ⋮ A N ] = ∑ k = 1 N A k T A k . \mathcal F= \begin{bmatrix}A_1^T&\cdots&A_N^T\end{bmatrix} \begin{bmatrix}A_1\\\vdots\\A_N\end{bmatrix} =\sum_{k=1}^{N}A_k^TA_k. F=[A1T​​⋯​ANT​​] ​A1​⋮AN​​ ​=k=1∑N​AkT​Ak​.

每一项表示一帧提供的信息,求和得到整段运动的信息。这与先堆叠全部数据再计算 A T A A^TA ATA 完全相同。重复相同状态会加强已有方向的信息,改变姿态、速度和加速度则有机会让其他参数方向也呈现出清楚的作用。

从参数不确定度得到 D 最优目标

给定一条候选轨迹,前面的计算得到它对应的 A A A 和 F \mathcal F F。接下来通过这些中间量,预测在给定测量噪声下参数估计的波动大小,以此评价轨迹的辨识效果。

即使重复执行同一条轨迹,每次测量中的噪声也会略有不同,求得的参数便会围绕真实值分散。前面的 Σ \Sigma Σ 描述力矩测量误差的波动;参数估计的协方差则描述这种噪声经过最小二乘求解后,引起的参数波动。其对角项是各参数估计的方差,非对角项反映估计误差之间的相关性。

在模型和运动状态准确、噪声零均值且协方差为 Σ \Sigma Σ、 A A A 满列秩的条件下,由加权最小二乘的误差传播可以得到:

Cov ⁡ ( β ˉ ^ ) = ( A T A ) − 1 = F − 1 . \operatorname{Cov}(\widehat{\bar\beta}) =(A^TA)^{-1}=\mathcal F^{-1}. Cov(βˉ​ ​)=(ATA)−1=F−1.

β ˉ ^ \widehat{\bar\beta} βˉ​ ​ 是缩放基参数的估计值, Cov ⁡ \operatorname{Cov} Cov 表示协方差。这里直接使用这一最小二乘的统计结论。计算时,对候选轨迹的信息矩阵取逆,就得到这条轨迹对应的参数估计协方差。它描述的是在给定噪声水平下,重复实验时参数估计预计会怎样分散;轨迹设计阶段就可以根据 W b W_b Wb​、 S β S_\beta Sβ​ 和 Σ \Sigma Σ 算出这个预测结果。

同一轮优化中,力矩噪声协方差 Σ \Sigma Σ 固定;候选轨迹改变时, W b W_b Wb​、 A A A 和 F \mathcal F F 随之改变,参数估计协方差 F − 1 \mathcal F^{-1} F−1 也会变化。前者提供观测权重,后者预测该轨迹的辨识效果。

沿用前面的两参数例子:

Cov ⁡ ( β ˉ ^ ) = [ 2 0 0 8 ] − 1 = [ 1 / 2 0 0 1 / 8 ] . \operatorname{Cov}(\widehat{\bar\beta}) =\begin{bmatrix}2&0\\0&8\end{bmatrix}^{-1} =\begin{bmatrix}1/2&0\\0&1/8\end{bmatrix}. Cov(βˉ​ ​)=[20​08​]−1=[1/20​01/8​].

两个对角元素分别是缩放参数 β ˉ ^ 1 \widehat{\bar\beta}_1 βˉ​ ​1​、 β ˉ ^ 2 \widehat{\bar\beta}_2 βˉ​ ​2​ 的估计方差。第二个方向的信息强度为第一个方向的四倍,对应的方差为其四分之一。这把“参数变化更容易在力矩中观察到”与“参数估计更稳定”连接了起来。

只看两个参数方向时,可以把可能的估计结果想象成参数平面中的一片区域。两种参数的作用容易混淆时,这片区域沿某个方向拉得很长;轨迹让它们的作用容易区分后,区域就更紧凑。下图用协方差椭圆表示这种差别,多参数情况下则对应更高维的椭球。

在这里插入图片描述

一种常用设计准则是尽量缩小这个椭球的整体体积,称为 D 最优设计。按相同的标准差倍数绘制时,椭球体积与 det ⁡ Cov ⁡ ( β ˉ ^ ) \sqrt{\det\operatorname{Cov}(\widehat{\bar\beta})} detCov(βˉ​ ​) ​ 成正比,其中 det ⁡ \det det 表示行列式。因此,可以通过减小协方差矩阵的行列式来减小整体不确定度。

将刚才的协方差关系代入,就得到目标与信息矩阵之间的连接:

det ⁡ Cov ⁡ ( β ˉ ^ ) = det ⁡ ( F − 1 ) = 1 det ⁡ F , log ⁡ det ⁡ Cov ⁡ ( β ˉ ^ ) = − log ⁡ det ⁡ F . \begin{aligned} \det\operatorname{Cov}(\widehat{\bar\beta}) &=\det(\mathcal F^{-1})=\frac{1}{\det\mathcal F},\\ \log\det\operatorname{Cov}(\widehat{\bar\beta}) &=-\log\det\mathcal F. \end{aligned} detCov(βˉ​ ​)logdetCov(βˉ​ ​)​=det(F−1)=detF1​,=−logdetF.​

log ⁡ \log log 是自然对数。第一行说明,协方差行列式越小,信息矩阵行列式就越大;第二行把同一个目标写成对数形式,便于数值计算。于是,最小化参数协方差的对数行列式,就等价于最小化 − log ⁡ det ⁡ F -\log\det\mathcal F −logdetF,也就是最大化 log ⁡ det ⁡ F \log\det\mathcal F logdetF。

由这一等价关系,后续求解器直接用 − log ⁡ det ⁡ F -\log\det\mathcal F −logdetF 评价候选轨迹。需要查看各参数的预期方差时,再由 F \mathcal F F 计算协方差矩阵。

D 最优评价整体体积,最小奇异值则帮助观察最弱方向。若 A A A 的最小奇异值为 σ min ⁡ \sigma_{\min} σmin​,则 F \mathcal F F 的最小特征值为 σ min ⁡ 2 \sigma_{\min}^2 σmin2​,最大的方向方差为 1 / σ min ⁡ 2 1/\sigma_{\min}^2 1/σmin2​。比较轨迹时,把这两个指标放在一起看,就能同时了解整体改善和薄弱方向的变化。

整个比较过程采用同一套基参数映射、参数尺度、噪声权重、采样时长和采样规则。这样,目标值的变化就对应轨迹本身带来的信息变化。这里的基参数映射由第四章的结构分析预先确定,后续轨迹优化始终沿用它。

初次设计可以从有代表性的噪声量级开始,重点估计各关节噪声的相对大小。对于这里的 D 最优目标,若所有标准差同时扩大两倍, F \mathcal F F 缩小为原来的四分之一,预测的参数协方差扩大四倍,而目标值只增加一个与轨迹无关的常数,候选轨迹的排序保持相同。各关节噪声比例的变化则会改变观测权重,进而影响轨迹选择。噪声的绝对大小决定了预测方差的数值,应结合后续实际数据复核。

5.2 用傅里叶级数描述连续轨迹

做点到点轨迹规划时,通常先给定起点、终点和运动时间,再用多项式或 S 曲线安排中间的运动。激励实验同样需要给出关节角随时间变化的函数 q ( t ) q(t) q(t),通过反复运动采集多种姿态、速度和加速度下的力矩。一个常用做法是让各关节在一定角度范围内往复摆动,再调整摆动幅度、快慢和各关节的配合时序。

从一次往返认识周期和频率

以六轴机械臂的肘关节为例,让它围绕 30° 前后各摆动 10°,每 4 秒完成一次往返。可以用一个正弦函数描述这段运动:

q ( t ) = q c + B sin ⁡ ( 2 π t T ) . q(t)=q_c+B\sin\left(\frac{2\pi t}{T}\right). q(t)=qc​+Bsin(T2πt​).

t t t 是时间, q c q_c qc​ 是摆动的中心角度, B B B 是角度幅值, T T T 是完成一轮运动所需的时间,称为周期。本例取 q c = 30 ∘ q_c=30^\circ qc​=30∘、 B = 10 ∘ B=10^\circ B=10∘、 T = 4 T=4 T=4 s。下面用角度制帮助观察动作,代入动力学计算时统一换成 rad。

时刻关节角运动情况
0 s30°从中心向角度增大的方向运动
1 s40°到达最大角度,随后反向
2 s30°经过中心,继续向角度减小的方向运动
3 s20°到达最小角度,随后反向
4 s30°回到与 0 s 相同的运动状态,开始下一轮

频率表示每秒完成几轮运动,单位为 Hz。因此,4 秒往返一次对应 f = 1 / T = 0.25 f=1/T=0.25 f=1/T=0.25 Hz。若把周期改成 2 秒,频率就变为 0.5 Hz,关节在相同角度范围内以更快的节奏摆动。改变 B B B 则改变摆幅,例如 B = 5 ∘ B=5^\circ B=5∘ 时,运动范围变为 25°~35°。

正弦函数每经过 2 π 2\pi 2π 就重复一次,因此常把 2 π / T 2\pi/T 2π/T 记为角频率。以本例的周期作为整段轨迹的基本周期 T 0 = 4 T_0=4 T0​=4 s,就有基频 f 0 = 0.25 f_0=0.25 f0​=0.25 Hz 和基角频率 ω 0 = 2 π / T 0 = π / 2 \omega_0=2\pi/T_0=\pi/2 ω0​=2π/T0​=π/2 rad/s。这里 ω 0 \omega_0 ω0​ 决定正弦波重复的快慢;关节自身的角速度由 q ( t ) q(t) q(t) 求导得到,会随时间变化。

把几种快慢不同的摆动叠加起来

单一正弦运动的形状由中心角度、幅值和周期决定。为了给优化器更多调整运动形状的余地,可以在每 4 秒一次的摆动上,叠加每 2 秒一次的小幅摆动:在每个时刻,将两种摆动相对中心的角度偏移相加,得到该时刻的关节角。合成后,加减速的变化会更丰富。

每 4 秒一次的分量称为基波;每 2 秒一次的分量频率是基频的两倍,称为第二次谐波。同样,第三次谐波在 4 秒内完成三轮。把有限个这样的正弦、余弦分量相加,就得到有限傅里叶(Fourier)级数。对第 j j j 个关节,写为:

q j ( t ) = q 0 , j + ∑ h = 1 H [ a j h sin ⁡ ( h ω 0 t ) + b j h cos ⁡ ( h ω 0 t ) ] . q_j(t)=q_{0,j}+\sum_{h=1}^{H}\left[ a_{jh}\sin(h\omega_0t)+b_{jh}\cos(h\omega_0t)\right]. qj​(t)=q0,j​+h=1∑H​[ajh​sin(hω0​t)+bjh​cos(hω0​t)].

q 0 , j q_{0,j} q0,j​ 是该关节的中心角度; h h h 是谐波编号, H H H 是采用的谐波数,第 h h h 次谐波在一个基本周期内完成 h h h 轮。 a j h , b j h a_{jh},b_{jh} ajh​,bjh​ 是对应的正弦和余弦系数,与 q 0 , j q_{0,j} q0,j​ 一样,单位均为 rad。各频率都是基频的整数倍,因此合成轨迹每隔 T 0 T_0 T0​ 重复一次。

同一频率的正弦和余弦在时间上错开四分之一周期。调整它们的系数,就能同时改变该分量的摆幅和摆动时序;这种时序差用相位描述。例如,可以让肩关节到达最大角度时,肘关节恰好经过中心角度。每个关节各有一组系数,优化器便可以安排多轴配合,形成有助于区分动力学参数的运动。

由位置函数计算速度和加速度

对同一个位置函数求导,直接得到速度与加速度:

q ˙ j ( t ) = ∑ h = 1 H h ω 0 [ a j h cos ⁡ ( h ω 0 t ) − b j h sin ⁡ ( h ω 0 t ) ] , q ¨ j ( t ) = − ∑ h = 1 H ( h ω 0 ) 2 [ a j h sin ⁡ ( h ω 0 t ) + b j h cos ⁡ ( h ω 0 t ) ] . \begin{aligned} \dot q_j(t)&=\sum_{h=1}^{H}h\omega_0 \left[a_{jh}\cos(h\omega_0t)-b_{jh}\sin(h\omega_0t)\right],\\ \ddot q_j(t)&=-\sum_{h=1}^{H}(h\omega_0)^2 \left[a_{jh}\sin(h\omega_0t)+b_{jh}\cos(h\omega_0t)\right]. \end{aligned} q˙​j​(t)q¨​j​(t)​=h=1∑H​hω0​[ajh​cos(hω0​t)−bjh​sin(hω0​t)],=−h=1∑H​(hω0​)2[ajh​sin(hω0​t)+bjh​cos(hω0​t)].​

速度的单位为 rad/s,加速度的单位为 rad/s²。由于它们都来自同一个位置函数,三种运动量在每个时刻自然对应,便于构造回归矩阵和检查运动边界。

将所有关节的 q 0 , j , a j h , b j h q_{0,j},a_{jh},b_{jh} q0,j​,ajh​,bjh​ 堆叠成系数向量 a a a,它就是轨迹优化的变量。例如,七轴机械臂每轴采用五个谐波,每个关节有一个中心角度和十个正余弦系数,共有 7 ( 1 + 2 × 5 ) = 77 7(1+2\times5)=77 7(1+2×5)=77 个变量。给定这 77 个数,就能计算整个周期内任意时刻的运动状态。

频率、运动幅值与驱动能力

保持角度幅值相同时,速度幅值随频率成比例增加,加速度幅值随频率的平方增加。下图沿用 4 秒的基本周期和 10° 的角度幅值,分别画出第一次与第五次谐波。第一幅图表示相对中心角度的偏移;第五次谐波在 4 秒内摆动五轮,速度幅值为第一次的五倍,加速度幅值为其二十五倍。

在这里插入图片描述

较高频率可以在有限角度范围内产生明显的惯性力矩,同时也会增加驱动负担,并更容易激发传动柔性。通常先结合驱动的跟随能力和机械结构选定周期 T 0 T_0 T0​ 与最高谐波数 H H H,再优化各分量的系数。求得系数后,按控制周期计算 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​,就能沿用常规轨迹跟踪接口执行这段运动。重复相同周期,还便于对采集数据进行对齐和平均。

周期运动与起停连接

完整周期首尾的正弦、余弦值相同,因此位置、速度和加速度在相邻周期之间连续。本节开头的肘关节例子从经过中心的时刻开始计时,此时关节已有速度。实际执行时,可以先用平滑的引入段接入周期运动,稳定后采集完整周期,最后通过退出段停下。

也可以直接让周期轨迹满足指定的起始位置、零速度和零加速度。把 t = 0 t=0 t=0 代入位置及其导数,得到:

q 0 , j + ∑ h = 1 H b j h = q j , s t a r t , ∑ h = 1 H h ω 0 a j h = 0 , ∑ h = 1 H ( h ω 0 ) 2 b j h = 0. q_{0,j}+\sum_{h=1}^{H}b_{jh}=q_{j,\mathrm{start}},\qquad \sum_{h=1}^{H}h\omega_0a_{jh}=0,\qquad \sum_{h=1}^{H}(h\omega_0)^2b_{jh}=0. q0,j​+h=1∑H​bjh​=qj,start​,h=1∑H​hω0​ajh​=0,h=1∑H​(hω0​)2bjh​=0.

q j , s t a r t q_{j,\mathrm{start}} qj,start​ 是指定的起始关节角。三式分别对应起始位置、零速度和零加速度;周期末端也满足相同条件。基频固定后,这些都是关于轨迹系数的线性等式。

5.3 构造并求解最优激励问题

轨迹系数 a a a 决定运动,运动决定回归矩阵,回归矩阵又决定信息量。将这条关系和机械臂的运动限制放在一起,就得到一个轨迹优化问题:

min ⁡ a − log ⁡ det ⁡ F ( a ) , s.t. q min ⁡ ≤ q ( t ; a ) ≤ q max ⁡ , ∣ q ˙ ( t ; a ) ∣ ≤ v max ⁡ , ∣ q ¨ ( t ; a ) ∣ ≤ a max ⁡ , ∣ τ p r e d ( t ; a ) ∣ ≤ τ max ⁡ , d ( q ( t ; a ) ) ≥ d min ⁡ . \begin{aligned} \min_a\quad&-\log\det\mathcal F(a),\\ \text{s.t.}\quad&q_{\min}\leq q(t;a)\leq q_{\max},\\ &|\dot q(t;a)|\leq v_{\max},\quad |\ddot q(t;a)|\leq a_{\max},\\ &|\tau_{\mathrm{pred}}(t;a)|\leq\tau_{\max},\\ &d(q(t;a))\geq d_{\min}. \end{aligned} amin​s.t.​−logdetF(a),qmin​≤q(t;a)≤qmax​,∣q˙​(t;a)∣≤vmax​,∣q¨​(t;a)∣≤amax​,∣τpred​(t;a)∣≤τmax​,d(q(t;a))≥dmin​.​

q ( t ; a ) q(t;a) q(t;a) 表示系数 a a a 在时刻 t t t 生成的关节位置, F ( a ) \mathcal F(a) F(a) 是按照 5.1 节计算的信息矩阵。目标前的负号把“最大化信息”写成求解器常用的最小化形式。 s.t. \text{s.t.} s.t. 表示满足后列约束,并根据接入方式加入上一节的起停条件。

q min ⁡ , q max ⁡ q_{\min},q_{\max} qmin​,qmax​ 是关节角下、上界, v max ⁡ , a max ⁡ v_{\max},a_{\max} vmax​,amax​ 是速度和加速度幅值上限, τ max ⁡ \tau_{\max} τmax​ 是力矩上限。绝对值和这些不等式都逐关节应用。 τ p r e d \tau_{\mathrm{pred}} τpred​ 是模型预测的驱动力矩; d ( q ) d(q) d(q) 是需要检查的机械臂自身及环境间的最小碰撞距离, d min ⁡ d_{\min} dmin​ 是要求保留的间距。

这里待求的是轨迹系数;执行轨迹并采集数据后,再估计基参数 β \beta β。在已知机构几何和重力设置的刚体模型中,回归矩阵 Y ( q , q ˙ , q ¨ ) Y(q,\dot q,\ddot q) Y(q,q˙​,q¨​) 的系数由运动状态决定,因此设计阶段就可以计算候选轨迹的信息量。质量和惯量的名义值则用于预测执行时的力矩:

τ p r e d ( t ; a ) = ID ⁡ ( q ( t ; a ) , q ˙ ( t ; a ) , q ¨ ( t ; a ) ; π n o m ) . \tau_{\mathrm{pred}}(t;a) =\operatorname{ID}\bigl(q(t;a),\dot q(t;a),\ddot q(t;a);\pi_{\mathrm{nom}}\bigr). τpred​(t;a)=ID(q(t;a),q˙​(t;a),q¨​(t;a);πnom​).

ID ⁡ \operatorname{ID} ID 是第二章的逆动力学计算, π n o m \pi_{\mathrm{nom}} πnom​ 是 CAD 或已有模型提供的惯性参数。式中包含重力,外部接触取零;驱动能力检查还可加入已有的摩擦估计。根据这些名义参数的误差范围,在力矩上限中预留相应余量。

一次轨迹评价的计算过程

先固定周期、谐波数和设计采样时刻。求解器每给出一组系数 a a a,就按下面的顺序评价它:

  1. 在 N N N 个采样时刻计算 q k , q ˙ k , q ¨ k q_k,\dot q_k,\ddot q_k qk​,q˙​k​,q¨​k​。
  2. 按第四章的方法逐帧构造回归矩阵 Y k Y_k Yk​,通过固定的基参数映射得到 Y b , k Y_{b,k} Yb,k​,堆叠成 W b W_b Wb​。
  3. 使用选定的 S β S_\beta Sβ​ 和 Σ \Sigma Σ 计算 A A A,再形成 F = A T A \mathcal F=A^TA F=ATA,求出目标值 − log ⁡ det ⁡ F -\log\det\mathcal F −logdetF。
  4. 用同一组运动状态检查位置、速度、加速度、预测力矩和碰撞距离,得到各项约束的余量。

这个过程相当于在计算机里预先检查一次实验会提供什么信息。它使用回归系数和测量噪声的统计水平。设计完成后,实际传感器记录才用于参数估计。

改变中心姿态,会改变重力力臂和连杆耦合;调整谐波幅值,会改变速度与加速度的大小;调整各轴相位,会改变这些作用同时出现的方式。求解器通过调整这些系数,在运动范围内寻找信息量更高的轨迹。

从初始轨迹到可执行结果

实际求解时,可以先生成几组中心姿态、幅值和相位不同的候选轨迹,选出满足运动约束且 A A A 满列秩的一组作为初值。此时 F \mathcal F F 正定,可以计算对数行列式。SQP 或内点法随后根据目标和约束对系数的变化率,反复更新系数;这些变化率可以通过解析求导、自动微分或有限差分获得。

在基频固定的时间网格上,位置、速度和加速度都对系数呈线性关系,相应的上下界也是线性约束。信息目标、力矩和碰撞距离通常对系数呈非线性关系,整个问题一般是非凸的。可以从几组初值分别求解,再比较所得轨迹的信息量和约束余量。

约束写的是整段连续运动,数值求解通常先在时间网格上检查。得到候选结果后,再用更密的网格检查峰值和最小间距,并在接近边界的时段补充约束点、重新优化。这样逐步把离散计算落实到整段轨迹的运动边界。最终保存的是轨迹系数、周期和接入方式,以及对应的信息指标与约束余量。

5.4 整理与实际运动对应的测量数据

执行轨迹后,要把数据整理成同一时刻的 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​ 和关节力矩,再构造辨识方程。运动状态由编码器记录重建,这样实际跟踪中的变化也会进入回归矩阵。位置与力矩按第三章的约定统一到模型所采用的关节侧,接下来重点处理时间对齐、运动求导和滤波。

对齐运动与力矩的采集时刻

每一帧回归方程描述同一时刻的运动和受力。采集时采用统一时钟或同步触发,并校准各反馈通道的延迟,再按时间戳配对数据。机械臂加减速越快,时间错位造成的力矩误差通常越明显。

下图用一个只含惯性力矩的单关节例子生成理想数据,真实惯量设为 0.4 kg·m²,黏性摩擦系数为零。关节以 2 Hz 做正弦运动,将力矩记录延后 15 ms;左图中,两条曲线因此在时间上错开。

第 6.2 节会进一步介绍摩擦辨识,这里先看延迟对这类估计的影响。将当前的速度、加速度与这批延迟力矩配对,按 τ = I q ¨ + b q ˙ \tau=I\ddot q+b\dot q τ=Iq¨​+bq˙​ 同时估计惯量 I I I 和黏性摩擦系数 b b b。两图中的蓝线都是真实模型力矩,橙色点都来自同一批延迟记录;右图新增的绿色线是拟合模型力矩。在 t = 0 t=0 t=0 处,蓝线为零,绿线与橙色点同为约 1.18 N·m。绿线贴合了记录,表中的惯量发生偏移,原本为零的摩擦系数也被估成了正值。因此应在参数估计前对齐时间。

在这里插入图片描述

本例只估计惯量时,仍得到约 0.3929 kg·m²,并留下峰值约 1.18 N·m 的残差,有助于发现时间错位。加入摩擦参数后,这部分残差也被拟合吸收,问题就更隐蔽了。

从位置反馈估计速度和加速度

位置噪声会在求导时放大,二阶求导得到的加速度尤其敏感。对于周期运动,可以用 5.2 节的傅里叶级数形式拟合编码器记录的关节位置,再对拟合函数求导,得到平滑且相互对应的 q , q ˙ , q ¨ q,\dot q,\ddot q q,q˙​,q¨​。这里的拟合系数由实际记录求得;保留的频率范围应覆盖主要运动成分,同时压低测量中的高频抖动。

由这组运动状态逐帧构造 Y b , k Y_{b,k} Yb,k​,堆叠成 W b W_b Wb​,并配上相同时刻的力矩 y τ y_\tau yτ​,就得到第四章用于估计参数的数据。

对力矩与回归系数采用一致的滤波

需要进一步平滑辨识数据时,对力矩方程两侧施加相同的线性滤波。用 H \mathcal H H 表示沿时间进行的滤波运算,由于参数 β \beta β 在实验中恒定,有:

H [ y τ ] = H [ W b ] β + H [ ε ] . \mathcal H[y_\tau]=\mathcal H[W_b]\beta+\mathcal H[\varepsilon]. H[yτ​]=H[Wb​]β+H[ε].

ε \varepsilon ε 是原方程中的测量误差。实际处理时,对每个关节的力矩序列、以及对应的每一列回归系数使用同一个滤波器,保留等式两侧的对应关系。

离线处理可采用零相位滤波,并去除首尾受边界影响较大的样本;降低采样率时,先按新采样率做低通滤波,再抽取样本。滤波会使相邻误差相关,评估参数不确定度时可将这种相关性计入噪声协方差,或利用多个完整周期的重复结果评估。

5.5 用独立运动验证辨识结果

数据整理好后,先用重复周期复核 5.1 节预估的噪声。例如,取各周期开始后第 1 秒的力矩读数,在实际运动接近的条件下去均值、计算标准差,再查看其他相位的波动。若与预估水平有明显差异,就汇总新的代表性标准差,用于参数估计和下一轮轨迹设计;同一轮优化保持权重固定。

随后用第四章的最小二乘或正则化方法求得 β ^ \widehat\beta β ​,并留出另一段完整轨迹用于验证。验证轨迹可以改变中心姿态、往复快慢或各轴配合方式。保持参数固定,按 5.4 节处理这段新数据,再比较预测力矩与测量力矩:

e τ = W b , v a l β ^ − y τ , v a l . e_\tau=W_{b,\mathrm{val}}\widehat\beta-y_{\tau,\mathrm{val}}. eτ​=Wb,val​β ​−yτ,val​.

下标 v a l \mathrm{val} val 表示验证数据, W b , v a l W_{b,\mathrm{val}} Wb,val​ 是由验证运动构造的基参数回归矩阵, y τ , v a l y_{\tau,\mathrm{val}} yτ,val​ 是对应的力矩观测, e τ e_\tau eτ​ 是预测与测量的差。

按关节查看均方根误差、平均偏差和峰值误差,分别反映整体误差、持续偏移和局部最大误差。再将误差与运动曲线对照:静态保持时的偏差可从重力模型和力矩零偏查起,换向附近可重点检查摩擦,快速加减速时可重点检查惯性参数与时间对齐。

验证运动应接近模型的预期用途:重力补偿侧重各姿态的静态力矩,运动前馈侧重不同速度和加速度下的预测效果。下一章继续讨论重力、摩擦和工具参数的具体辨识方法。

Logo

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

更多推荐