【机器人动力学】(五):最优激励与测量数据处理
本系列是我学习机器人动力学的讲义,我将它搬运到博客中。这系列文章默认读者已经掌握机器人运动学相关的知识。
主要内容计划安排如下,专栏地址:微鉴道长机器人动力学专栏
后文内容不定期更新,欢迎关注:
【机器人动力学】(一):刚体惯性与动能
【机器人动力学】(二):机械臂动力学方程与计算
【机器人动力学】(三):力矩控制与接触交互
【机器人动力学】(四):动力学回归与最小参数集
【机器人动力学】(五):最优激励与测量数据处理
【机器人动力学】(六):本体、摩擦与工具参数辨识
第五章:最优激励与测量数据处理
第四章已经确定了待估的基参数。接下来需要安排一段机械臂运动,让这些参数对力矩的影响充分表现出来。例如,改变姿态可以观察重力作用,反复加减速可以观察惯性作用,多关节协调运动可以呈现连杆之间的耦合。为辨识专门设计的这类运动,称为激励轨迹。
最优激励轨迹的目标,是在关节范围、驱动能力和碰撞约束内,让采集到的运动与力矩更有助于确定参数。整个过程从评价轨迹的信息量开始,再设计连续运动、求解轨迹系数,最后执行采集并验证辨识结果。
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 g0sinq 分别为 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 [241−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.5002].
β ˉ \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ββˉ)+ε=[241−2][0.5002][βˉ1βˉ2]+ε=WbSβ [122−4][βˉ1βˉ2]+ε.
这里 y τ = [ τ 1 , τ 2 ] T y_\tau=[\tau_1,\tau_2]^T yτ=[τ1,τ2]T。新的回归矩阵 W b S β W_bS_\beta WbSβ 已由运动数据和参考尺度算出,后面的 β ˉ \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=Ns1s=1∑Nsτj,s,σ j=Ns−11s=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/2WbSβ=[1001/2][122−4]=[112−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=[121−2][112−2]=[2008].
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/2Yb,kSβ,其中 Σ 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∑NAkTAk.
每一项表示一帧提供的信息,求和得到整段运动的信息。这与先堆叠全部数据再计算 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(βˉ )=[2008]−1=[1/2001/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 s | 30° | 从中心向角度增大的方向运动 |
| 1 s | 40° | 到达最大角度,随后反向 |
| 2 s | 30° | 经过中心,继续向角度减小的方向运动 |
| 3 s | 20° | 到达最小角度,随后反向 |
| 4 s | 30° | 回到与 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[ajhsin(hω0t)+bjhcos(hω0t)].
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∑Hhω0[ajhcos(hω0t)−bjhsin(hω0t)],=−h=1∑H(hω0)2[ajhsin(hω0t)+bjhcos(hω0t)].
速度的单位为 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∑Hbjh=qj,start,h=1∑Hhω0ajh=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} amins.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,就按下面的顺序评价它:
- 在 N N N 个采样时刻计算 q k , q ˙ k , q ¨ k q_k,\dot q_k,\ddot q_k qk,q˙k,q¨k。
- 按第四章的方法逐帧构造回归矩阵 Y k Y_k Yk,通过固定的基参数映射得到 Y b , k Y_{b,k} Yb,k,堆叠成 W b W_b Wb。
- 使用选定的 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。
- 用同一组运动状态检查位置、速度、加速度、预测力矩和碰撞距离,得到各项约束的余量。
这个过程相当于在计算机里预先检查一次实验会提供什么信息。它使用回归系数和测量噪声的统计水平。设计完成后,实际传感器记录才用于参数估计。
改变中心姿态,会改变重力力臂和连杆耦合;调整谐波幅值,会改变速度与加速度的大小;调整各轴相位,会改变这些作用同时出现的方式。求解器通过调整这些系数,在运动范围内寻找信息量更高的轨迹。
从初始轨迹到可执行结果
实际求解时,可以先生成几组中心姿态、幅值和相位不同的候选轨迹,选出满足运动约束且 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τ 是预测与测量的差。
按关节查看均方根误差、平均偏差和峰值误差,分别反映整体误差、持续偏移和局部最大误差。再将误差与运动曲线对照:静态保持时的偏差可从重力模型和力矩零偏查起,换向附近可重点检查摩擦,快速加减速时可重点检查惯性参数与时间对齐。
验证运动应接近模型的预期用途:重力补偿侧重各姿态的静态力矩,运动前馈侧重不同速度和加速度下的预测效果。下一章继续讨论重力、摩擦和工具参数的具体辨识方法。
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐



所有评论(0)