论文阅读-Physical feasibility of robot base inertial parameter identification: A linear matrix inequalit

概要

这篇论文主要讨论在考虑物理一致性的情况下的机器人动力学辨识。给出了三个方法，1用来检查给定的参数估计是否具有物理意义。2修正不可行的物理参数找到最近的可行的参数。这两个方法可以用到已存在的辨识上。3用最小二乘回归在考虑物理约束的情况下求解最优参数。

惯性参数的物理可行性

机器人的物理模型一般写成
$$M(q)\ddot{q}+c(q,\dot{q})+g(q)=\tau \text{\hspace{1em}(1)}$$
其中各项取决于几何参数和动力学参数。所谓满足物理可行性，既是要求质量大于零，惯性张量正定
$$\{\begin{array}{l}{m}_k>0\\ I_k\succ 0\end{array}\text{\hspace{1em}(2)}$$
惯性张量是
$$I_k=\left[\begin{array}{ccc}{I}_{k,xx}& I_{k,xy}& I_{k,xz}\\ I_{k,xy}& I_{k,yy}& I_{k,yz}\\ I_{k,xz}& I_{k,yz}& I_{k,zz}\end{array}\right]\text{\hspace{1em}(3)}$$
在link坐标系k下的惯性张量是
$$L_k=I_k+m_{k}S(r_k{)}^{T}S(r_k)\text{\hspace{1em}(5)}$$
其中$S(\cdot )$是反对称矩阵， $r_k$是link坐标系k中的质心。$L_k$必须为正定对称矩。
为了进行线性回归，需要使用转矩向量$l_k$
$$l_k=\left[\begin{array}{c}{l}_{k,x}\\ l_{k,y}\\ l_{k,z}\end{array}\right]=m_k{r}_k=m_k\left[\begin{array}{c}{r}_{k,x}\\ l_{k,y}\\ l_{k,z}\end{array}\right]\text{\hspace{1em}(8)}$$
所以,(5)需要改写为
$$L_k=I_k+m_{k}S(\frac{l_k}{m_k}{)}^{T}S(\frac{l_k}{m_k})\text{\hspace{1em}(9)}$$
因为反对称是线性操作，所以
$$I_k=L_k-\frac{l_k}{m_k}S(l_k{)}^{T}S(l_k)$$
因此，物理约束可以写为
$$\{\begin{array}{l}{m}_k>0\\ L_k-\frac{1}{m_k}S(l_k{)}^{T}S(l_k)\succ 0\end{array}\text{\hspace{1em}(11)}$$
将每个轴的惯性参数写到一起
$$\begin{gathered} {\delta }_k={\left[\begin{array}{cccccccccc}l{L}_{k,xx}& L_{k,xy}& L_{k,xz}& L_{k,yy}& L_{k,yz}& L_{k,zz}& l_{k,x}& l_{k,y}& l_{k,z}& m_k\end{array}\right]}^T \\ \delta ={\left[{\delta }_1^T{\delta }_2^T\cdots {\delta }_k^T\cdots {\delta }_N^T\right]}^T \end{gathered}$$
共有$n=10N$个变量
集合$\mathcal{D}$为
$$\mathcal{D}=\left\{\delta \in {\mathbb{R}}^n|m_k>0,L_k-m_k^{-1}S(l_k{)}^{T}S(l_k)\succ 0,k=1,...,N\right\}$$

机器人link的物理可行性

前面的公式可以写成
$$L_k-S(l_k{)}^T{\left(m_{k}1\right)}^{-1}S(l_k)\succ 0\text{\hspace{1em}(15)}$$
其中$1$是3x3单位矩阵，利用舒尔定理，可以写成
$$D_k\left({\delta }_k\right)=\left[\begin{array}{cc}{L}_k& S(l_k{)}^T\\ S(l_k)& m_{k}1\end{array}\right]\succ 0$$
其中
$$D_k\left({\delta }_k\right)=\left[\begin{array}{cc}{L}_k& S(l_k{)}^T\\ S(l_k)& m_{k}1\end{array}\right]=\left[\begin{array}{cccccc}l{L}_{k,xx}& L_{k,xy}& L_{k,xz}& 0& -l_{k,z}& l_{k,y}\\ L_{k,xy}& L_{k,yy}& L_{k,yz}& l_{k,z}& 0& -l_{k,x}\\ L_{k,xz}& L_{k,yz}& L_{k,zz}& -l_{k,y}& l_{k,x}& 0\\ 0& l_{k,z}& -l_{k,y}& m_k& 0& 0\\ -l_{k,z}& 0& l_{k,x}& 0& m_k& 0\\ l_{k,y}& -l_{k,x}& 0& 0& 0& m_k\end{array}\right]$$
使用时可将正定放宽为半正定，而不会对结果有什么影响。
也可根据需要添加一些关于质量和质量矩的上下限约束，这些约束都是凸的。
同样，也可将摩擦系数添加到参数中。

基参数的物理一致性

在已知$\delta$后检查其是否满足物理一致性是很容易的，直接将其带入所有不等式看是否满足。而参数辨识必须工作在最小惯性集上，因此需要讨论最小惯性参数的物理一致性。

构造最小惯性参数

机械臂的动力学是$\delta$的线性函数
$$H\left(q,\dot{q},\ddot{q}\right)\delta =\tau$$
将其写为
$$\left[\begin{array}{cc}{H}_b& H_d\end{array}\right]\left[\begin{array}{c}{\delta }_b\\ {\delta }_d\end{array}\right]=\tau \text{\hspace{1em}(30)}$$
其中$H_b$是$H$中$n_b$个线性无关的列，$H_d$是剩下$n_d=n-n_b$个列。
$H_d$可以写成$H_b$的线性组合
$$H_d=H_b{K}_d$$
中$K_d=R_1^{-1}{R}_2$是一个常量矩阵。
${\delta }_b$和${\delta }_d$是经过重新排序的参数，有
$$\begin{gathered} H_b=H{P}_b \\ {H}_d=H{P}_d \\ {\delta }_b=P_b^T\delta \\ {\delta }_d=P_d^T\delta \end{gathered}$$
$P_b$和$P_d$是排序矩阵$P$的前$n_b$和后$n_d$列 $HP=[H_b{H}_d]$
因为线性相关性，公式(30)可以写为
$$H_b\beta =\tau$$
其中$\beta =[{\beta }_1{\beta }_2{\beta }_3...{\beta }_{n_b}]$是最小惯性参数,有
$$\beta ={\delta }_b+K_d{\delta }_d$$
于是有
$$\begin{gathered} \beta =K\delta \\ K=P_b^T+K_d{P}_d^T=[I_b,R_1^{-1}{R}_2]P^T \end{gathered}$$
这里的$R_1$和$R_2$是$H$的堆叠矩阵$Z$在QR分解时产生
$$ZP=QR=Q\left[\begin{array}{cc}{R}_1& R_2\\ 0& 0\end{array}\right]$$

基参数可行性测试 BPFT

$K$矩阵将原始惯性参数$\delta$映射到了基参数$\beta =K\delta$。但是$K$不是一个方阵，因此这个映射不是双射，一个$\beta$对应着多个$\delta$。满足物理一致性的基参数集合为
B = { β ∈ R n b : ∃ δ ∈ D   s . t .   β = K δ } \mathcal{B}=\{\beta\in\mathbb{R}^{n_b}:\exist \delta\in \mathcal{D}\ s.t. \ \beta=K\delta\} B={β∈Rnb​:∃δ∈D s.t. β=Kδ}
但是这个定义不能直接用来判断物理一致性，因为$K$不是方阵。
定义一个新的映射
$$\begin{gathered} \mathfrak{m}:{\mathbb{R}}^n\to {\mathbb{R}}^n\delta \mapsto (\beta ,{\delta }_d) \\ \mathfrak{m}(\delta )=G\delta \end{gathered}$$
这里$G=K_G{P}^T$，
$$K_G=\left[\begin{array}{cc}1& K_d\\ 0& 1\end{array}\right]$$
所以有
$$\begin{array}{rl}G\delta & =K_G{P}^T\delta \\ & =\left[\begin{array}{cc}1& K_d\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\delta }_b\\ {\delta }_d\end{array}\right]\\ & =\left[\begin{array}{c}\beta \\ {\delta }_d\end{array}\right]=\left(\beta ,{\delta }_d\right)\end{array}$$
因为$K_G$是上三角矩阵，$P^T$是排序矩阵，因此$G$是可逆的，$\mathfrak{m}$是个一对一的映射。所以这个扩展空间上的真正定性等价与原空间的正定性，满足物理一致性的集合可以写为
D β = { ( β , δ d ) ∈ R n : D β ( β , δ d ) ⪰ 0 } D β ( β , δ d ) = D ( m − 1 ( β , δ d ) ) \mathcal{D} _{\beta}=\left\{ \left( \beta ,\delta _d \right) \in \mathbb{R} ^n:D_{\beta}\left( \beta ,\delta _d \right) \succeq 0 \right\} \\ D_{\beta}\left( \beta ,\delta _d \right) =D\left( \mathfrak{m} ^{-1}\left( \beta ,\delta _d \right) \right) Dβ​={(β,δd​)∈Rn:Dβ​(β,δd​)⪰0}Dβ​(β,δd​)=D(m−1(β,δd​))
也就是说，先用${\mathfrak{m}}^{-1}$反向映射得到$\delta$，用$\delta$的物理一致性来定义$(\beta ,{\delta }_d)$的物理一致性
$$\begin{gathered} {\mathfrak{m}}^{-1}\left(\beta ,{\delta }_d\right)=G^{-1}\left[\begin{array}{c}\beta \\ {\delta }_d\end{array}\right] \\ =P^{-T}{G}_K^{-1}\left[\begin{array}{c}\beta \\ {\delta }_d\end{array}\right] \\ =P\left[\begin{array}{cc}1& -K_d\\ 0& 1\end{array}\right]\left[\begin{array}{c}\beta \\ {\delta }_d\end{array}\right] \end{gathered}$$
因此，基参数的物理一致性集合可以改写为
B = { β ∈ R n b : ∃ δ d ∈ R n d    s . t . D β ( β , δ d ) ⪰ 0 } \mathcal{B} =\{\beta \in \mathbb{R} ^{n_b}:\exists \delta _d\in \mathbb{R} ^{n_d}\,\,s.t. D_{\beta}\left( \beta ,\delta _d \right) \succeq 0\} B={β∈Rnb​:∃δd​∈Rnd​s.t.Dβ​(β,δd​)⪰0}
这个定义中仍然包含${\delta }_d$，无法直接用来判断$\beta$的物理一致性。

若一个给定的$\hat{\beta }$满足物理一致性，则至少存在一个${\delta }_d$，使得$D_{\beta }\left(\beta ,{\delta }_d\right)⪰0$。描述为优化问题既为
$$\begin{gathered} \mathrm{find}{\delta }_d \\ \mathrm{s}.\mathrm{t}.D_{\beta }\left(\beta ,{\delta }_d\right)⪰0 \end{gathered}$$
这个优化问题叫做基参数可行性测试(base parameter feasibility test, BPFT)。

基参数可行性修正 BPFC

类似的，可以寻找距离给定基参数$\hat{\beta }$最接近的满足物理一致性的$\beta$
$$\begin{gathered} \underset{\left(\beta ,{\delta }_d\right)}{\min }||\hat{\beta }-\beta || \\ \mathrm{s}.\mathrm{t}.D_{\beta }\left(\beta ,{\delta }_d\right)⪰0 \end{gathered}$$
这个问题称作基参数可行性修正(base parameter feasibility correction， BPFC)。

基参数优化

在满足物理一致性的条件下辨识最小惯性集参数。收集$s$个点的关节角度和力矩数据之后堆叠成矩阵
$$W=\left[\begin{array}{c}{H}_b\left(q_1,{\dot{q}}_1,{\ddot{q}}_1\right)\\ H_b\left(q_2,{\dot{q}}_2,{\ddot{q}}_2\right)\\ ⋮\\ H_b\left(q_s,{\dot{q}}_s,{\ddot{q}}_s\right)\end{array}\right]w=\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ ⋮\\ {\tau }_s\end{array}\right]$$
目标函数为$||ϵ|{|}^2=||w-W\beta |{|}^2$
所以优化问题可以写为
$$\begin{gathered} \underset{\left(\beta ,{\delta }_d\right)}{\min }||w-W\beta |{|}^2 \\ \mathrm{s}.\mathrm{t}.D_{\beta }\left(\beta ,{\delta }_d\right)⪰0 \end{gathered}$$
论文里给出了SDP的形式，如果需要进一步提升求解速度，使用SDP的形式更好。