-
摘要: 微零件的姿态测量对微装配具有重要的作用.但对于微球零件，其姿态的精确测量存在困难，影响了装配精度.针对带有微孔的微球，本文提出了一种基于单目显微视觉的微球姿态高精度测量方法.设计了一种由粗到精的微孔检测算法，实现了高精度的微孔定位.通过对相机光轴方向的标定，在相机运动后对微球图像坐标进行补偿，提高了在相机坐标系下的微球定位精度.通过对微球和微孔的精确定位，计算出微球球心与微孔圆心的空间相对位置，实现了相机坐标系下高精度的微球姿态测量.同时，根据标定出的相机坐标系与调整平台坐标系之间的旋转关系，将微球姿态转换到调整平台坐标系.实验结果表明，最大姿态测量误差0.3度，验证了本文方法的有效性.Abstract: Pose measurement for micro-components is very important to micro assembly. But it is quite difficult to measure the pose of a micro sphere with high precision, which impacts the assembly accuracy. An accurate pose measurement method for sphere with micro hole is proposed. A coarse-to-fine detection method is designed for the micro-hole on the micro-sphere. It realizes the high precision location of the micro-hole. The direction of the microscopic camera's optical axis is calibrated. It is used to compensate the micro-sphere's image coordinates after the microscopic camera moves in order to improve the location accuracy of the micro-sphere in the camera's frame. The relative position between the center of sphere and the center of hole can be easily derived from the accurately located micro-sphere and micro-hole. Then the pose of micro-sphere can be gotten. Meanwhile, the pose of micro-sphere can be transformed from the camera's frame into the frame of adjusting platform according to the calibrated rotation relationship between the two frames. The maximum error in pose measurements is 0.3 degree. Experimental results verify the effectiveness of the proposed method.
-
随机噪声与有界噪声 同时干扰自动控制系统的现象普遍存在, 例如, 在雷达跟踪系统中, 接收系统的热噪声以及天线受到阵风的影响是典型的随机噪声 [1], 而加速度的物理特性、外界未知环境不确定干扰等噪声因素的影响, 相比其随机统计特性获得边界是更加可行的 [2]; 单晶在生长控制过程中, 一方面受到随机对流的作用, 另一方面温度场按偏微分方程演化, 而在用有限元方法求解偏微分方程时, 需要对不规则区域用三角形进行分割, 对于分割接近边界邻域的狭小部分通常会忽略, 忽略的这部分是有界不确定性; 对于非线性随机系统, 在用扩展卡尔曼滤波(Extended Kalman filter, EKF)时, 线性化过程中同样也忽略了高阶余项. 这些实例都表明, 系统存在两种不确定性, 一种是概率统计特性已知的随机不确定性, 而另一种是已知边界的有界不确定性, 当两种不确定性同时存在时, 本文称之为双重不确定性.
系统的不确定性分析存在两种方法 [3], 概率方法和非概率方法, 而选择采用哪种方法往往取决于样本统计数据的多少及其性质. 概率方法需要知道随机变量的概率统计特性, 而区间分析理论(非概率方法)解决不确定问题需要已知有界不确定参数或变量所在范围的边界. 概率方法适合解决随机不确定, 而非概率方法适合解决有界不确定. 在不确定系统的状态估计问题中, 卡尔曼滤波是一种典型的概率方法, 集员滤波(Set-membership filter, SMF) [4-7]是一种典型的非概率方法. 两种滤波方法中, 卡尔曼滤波以贝叶斯推理为基础, 解决了高斯白噪声假设下线性系统状态的最优估计问题; SMF以包含系统真实状态的外定界椭球集合为基础, 只要求系统噪声有界, 且噪声界已知, 而不需要知道边界内噪声的统计特性. 相比卡尔曼滤波在每一步估计得到的是一个状态值, SMF得到的是一个椭球集合, 且此集合内的值都是可行解. 因此, 两种滤波方法有各自的适用范围, 且都在工程实践中得到广泛的应用 [8-10]. 对于存在两种不确定性的混合噪声系统, 非高斯白噪声的存在使单一卡尔曼滤波的估计结果往往过分乐观 [11], 甚至收敛性也不能保证; 高斯白噪声的存在导致单一SMF选取的噪声边界会过分保守, 估计精度下降. 总之, 现有方法一般都是假设一种噪声的存在, 意味着人为地排除了另一种噪声. 为克服这些单一滤波算法的局限性, 研究具有双重不确定性系统的滤波问题有重要的理论价值和实际应用前景.
然而处理双重不确定性噪声模型问题的研究还处于起步阶段, 如何将随机不确定性和有界不确定性整合成一个混合的数学描述, 是一个挑战性问题 [12]. 面对这个难题, 相关文献已经提出的能够结合两种不确定的数学形式有: 随机集合、集合概率密度以及其他不精确概率描述方法. Hanbeck等 [13]利用随机集合提出的统计和集合论信息(Statistical and set-theoretic information, SSI)滤波器, 其特点是传统单一的滤波方法得到的值只是SSI滤波器的边界情况, 即当随机噪声为零时, 收敛于集合估计; 当有界误差为零时, 收敛于贝叶斯估计, 该方法的局限性在于只能解决线性标量状态. Noack等 [14]采用集合密度的概念来描述双重不确定性, 即用适合描述不确定集合的集合概率密度取代了单一概率密度函数. Klumpp等 [15]比较了基于随机集合的SSI滤波器和基于集合概率密度的CS (Credal state)滤波器, 得出CS滤波器具有更加保守的特点. Henningsson [16]用椭球包含混合噪声中有界集合部分的估计误差, 通过线性矩阵不等式得到最优滤波增益, 该算法的性能受到一个权系数的影响, 而该系数的取值取决于实际系统总体噪声中随机不确定和有界不确定的某种权重关系, 且这种权重关系并没有定量地给出. Liu等 [17]提出的椭球集合滤波算法在解决纯方位机动目标的跟踪问题时, 在集员椭球更新的过程中考虑了随机不确定性, 得到了跟踪性能优于单一EKF的结论.
本文根据随机不确定性和有界不确定性对估计结果影响的各自特点, 将统计特性未知但有界的(Unknown but bounded, UBB)噪声引入到卡尔曼滤波模型中, 得到一组包含集合运算的卡尔曼滤波方程. 其中的UBB噪声应用SMF的思想进行处理, 而随机噪声应用卡尔曼滤波的思想进行处理, 实现了两种滤波方法的联合处理. 文章的最后将该算法推广到非线性系统中.
1. 数学基础
1.1 数学模型
为了处理包含随机噪声和UBB噪声系统, 需要建立能够正确描述该系统的数学模型, 因此, 系统状态方程可写成如下形式
${ x}_{k}=f_{k}( {x}_{k-1}, {w}_{k} , {d}_{k}) $
(1) 式中${x}_{k}$为k时刻n维状态向量, ${ w}_{k}$、${ d}_{k}$为过程噪声, 其中${w}_{k}\sim {\rm N}(0, Q_{k})$属于零均值高斯白噪声, $Q_{k}$为过程噪声的非负定协方差矩阵, ${ d}_{k}\in D_{k}\subset R^{n}$为UBB噪声, $D_{k}$为边界已知的有界集合.
测量方程
${{y}_{k}}={{h}_{k}}({{x}_{k, }}{{v}_{k}}, {{e}_{k}})$
(2) 式中${ y}_{k}$为k时刻m维测量输出, ${ v}_{k}$、${ e}_{k}$为测量噪声, 其中${ v}_{k}\sim {\rm N}(0, R_{k})$属于零均值高斯白噪声, $R_{k}$为测量噪声的正定协方差矩阵, ${ e}_{k}\in E_{k}\subset R^{m}$为UBB噪声, $E_{k}$为边界已知的有界集合.
这时, 我们得到了包含高斯白噪声和UBB噪声的双重不确定性非线性系统. 在解决非线性模型的滤波问题中, 非线性模型可以通过线性化来近似表示, 为了易于说明, 首先考虑系统是线性的式(1)和式(2)写成如下形式
${{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}({{w}_{k}}+{{d}_{k}})$
(3) ${{y}_{k}}={{H}_{k}}{{x}_{k}}+{{v}_{k}}+{{e}_{k}}$
(4) 假设在模型中UBB噪声${ d}_{k}=0$、${ e}_{k}=0$, 即典型的随机模型, 此时该模型满足经典卡尔曼滤波条件, 其一步状态预测方程为
$\hat{{ x}}_{k, k-1}=A_{k}\hat{{ x}}_{k-1} $
(5) 预测估计协方差矩阵
$P_{k, k-1}=A_{k}P_{k-1}A^{{\rm T}}_{k}+B_{k}Q_{k}B_{k}^{\rm T} $
(6) 滤波增益
$K_{k}=P_{k, k-1}H^{\rm T}_{k}[H_{k}P_{k, k-1}H^{\rm T}_{k}+R_{k}]^{-1} $
(7) 更新状态估计值
${{{\hat{x}}}_{k}}={{{\hat{x}}}_{k, k-1}}+{{K}_{k}}[{{y}_{k}}-{{H}_{k}}{{{\hat{x}}}_{k, k-1}}]=(I-{{K}_{k}}{{H}_{k}}){{{\hat{x}}}_{k, k-1}}+{{K}_{k}}{{y}_{k}}$
(8) 更新估计协方差矩阵
$P_{k}=[I-K_{k}H_{k}]P_{k, k-1} $
(9) 如果考虑UBB噪声项, 由于高斯白噪声一阶矩为常数, 不会引起系统变量均值的变化, 而UBB噪声会引起均值的偏移, 因此均值不再是唯一值, 定义如下集合
${{\delta }_{k}}=\{{{d}_{k}}|{{d}_{k}}\in {{D}_{k}}\}{{\psi }_{k}}=\{{{y}_{k}}-{{e}_{k}}|{{e}_{k}}\in {{E}_{k}}\}$
(10) ${{\psi }_{k}}=\{{{y}_{k}}-{{e}_{k}}|{{e}_{k}}\in {{E}_{k}}\}$
(11) 式中 ${\delta_{k}}$为UBB噪声均值集合, ${\psi_{k}}$为测量输出均值集合. 当系统包含UBB噪声集合后, 可以将点运算变为集合运算. 那么类似于式(5)和式(8), 状态的一步预测和更新分别可写成集合运算的形式
${{\chi }_{k, k-1}}={{A}_{k}}{{\chi }_{k-1}}\oplus {{B}_{k}}{{\delta }_{k}}{{\chi }_{k}}=(I-{{K}_{k}}{{H}_{k}}){{\chi }_{k, k-1}}\oplus {{K}_{k}}{{\psi }_{k}}$
(12) ${{\chi }_{k}}=(I-{{K}_{k}}{{H}_{k}}){{\chi }_{k, k-1}}\oplus {{K}_{k}}{{\psi }_{k}}$
(13) ${\chi_{k}}$称为条件均值集合, 符号$\oplus$表示椭球的闵可夫斯基 (Minkowski)和. 在随后的内容中, 我们通过适当的方法参数化这些集合, 推导出一个能够处理线性系统的估计方法.
如果考虑系统是非线性的, 滤波过程中就需要对非线性系统线性化, 然而在这类混合噪声模型中, 此时估计的并不是一个值, 而是一个状态集合, 因此, EKF中用的线性化方法在处理该问题时并不适用, 如何找到一种合适的近似方法将在第2.2节中讨论.
这里建立的双重不确定性模型, 都是假定合理有效的. 然而由于实际系统中噪声的复杂性, 建立合理有效的模型并不容易. 事实上, 随机和有界很难严格地划分, 并且也可以近似地相互转换. 例如, 高斯白噪声, 如果用有界的形式表示, 可以采用$3\sigma$置信区间, 选取的噪声边界为高斯白噪声方差$\sigma$的3倍, 那么噪声在$3\sigma$区间的概率为$99.73 %$. 对于有些情况下定义的有界噪声, 并非是指边界内噪声的统计特性完全未知, 可能的情况是统计特性已知但属于非高斯分布, 当非高斯程度较小时, 这时完全可以根据有界噪声的期望和方差来转化为近似高斯分布的随机噪声. 因此, 只有对实际噪声的合理划分, 才能建立合理有效的双重不确定性模型, 但这需要对系统进行大量的分析和实践.
1.2 椭球集合及其运算性质
在给出的双重不确定性模型中, 我们已经分离出模型的有界部分, SMF是一种基于集合论的估计方法, 解决了有界集合如何被椭球近似包含的问题, 以及滤波过程中的集合运算问题. 下面介绍集员相关的定义与定理.
假设线性动态模型存在UBB噪声, 在此条件下, 状态估计由点估计变为一个状态可行集合的估计问题, UBB噪声集合和状态可行集均可以用椭球集合来近似描述.
定义 1. 椭球集合$F( {a}, S)$可以表示为
$F(a, S)=\{x\in {{R}^{n}}|{{(x-a)}^{\text{T}}}{{S}^{-1}}(x-a)\le 1\}$
(14) 式中${ a}$为椭球中心, $S\in R^{n\times n}$为对称正定矩阵, 度量椭球的形状大小.
定义 2. 椭球$F( {a}, S)$的支持函数 [5]可表示为
$\eta (F(a, S))={{\eta }_{\beta }}(\alpha )={{\max }_{\beta \in F(a}}, S){{\beta }^{\text{T}}}\alpha ={{\alpha }^{\text{T}}}a+{{({{\alpha }^{\text{T}S}}\alpha )}^{\frac{1}{2}}}, \ \ \alpha \in {{R}^{n}}\ \ \ \ \ \ $
(15) 这里注意$\eta(\cdot)$与$\eta_{{ \beta}}(\cdot)$的区别, $\eta(\cdot)$的自变量为椭球集合, $\eta_{{ \beta}}(\cdot)$的自变量为向量.
去掉上式中max函数, 得到不等式形式
${{\beta }^{\text{T}}}\alpha \le {{\eta }_{\beta }}(\alpha )={{\alpha }^{\text{T}}}a+{{({{\alpha }^{\text{T}S}}\alpha )}^{\frac{1}{2}}}, \{\beta \in F(a, S), \ \ \ \alpha \in {{R}^{n}}$
(16) 引理 1. 支持函数存在如下运算性质: 椭球集合$F({a_1},{S_1}),F({a_2},{S_2}) \subseteq {R^n}$, 其支持函数的Minkowski和存在以下性质
$\eta (F({a_1},{\rm{ }}{S_1}) \oplus F({a_2},{S_2})) = \eta (F({a_1},{\rm{ }}{S_1})) + \eta (F({a_2},{S_2}))$
(17) 根据引理1中支持函数的性质, 假设式(12)中条件均值集合$ \chi_{k-1}$和UBB噪声的边界集合${\delta_{k}}$包含在椭球集合(闭凸集)内, 将式(12)写成如下支持函数计算形式
$\eta ({{\chi }_{k, k-1}})=\text{ }\eta ({{A}_{k}}{{\chi }_{k-1}}\oplus {{B}_{k}}{{\delta }_{k}})=\eta ({{A}_{k}}{{\chi }_{k-1}})+\eta ({{B}_{k}}{{\delta }_{k}})$
(18) 由于集合的线性运算保留集合"凸"的特性, 那么预测条件均值集合$ \chi_{k, k-1}$为凸集.
同样假设测量输出集合$ \psi_{k}$也包含在椭球集合内, 那么式(13)可写成
$\begin{align} &\eta (\chi k)=\eta ((I-{{K}_{k}}{{H}_{k}}){{\chi }_{k, k-1}}\oplus {{K}_{k}}{{\psi }_{k}})= \\ &\eta ((I-{{K}_{k}}{{H}_{k}}){{\chi }_{k, k-1}})+\eta ({{K}_{k}}{{\psi }_{k}}) \\ \end{align}$
(19) 同理, 计算得到的更新条件均值集合$\chi_{k}$也是凸集. 可以看出, 支持函数将椭球集合运算中Minkowski和转换为加法.
根据上面的内容可知, 滤波模型中加入了有界集合, 有界集合可以用椭球集合来包含. 定义1给出了椭球集合的描述, 但该描述下的两个椭球集合不能直接进行运算, 因此, 定义2给出了椭球集合的支持函数, 引理1给出了支持函数的运算性质. 虽然引理1中的$F({{a}_{1}}和{{S}_{1}})F({{a}_{2}}$的Minkowski和求出的是一个凸集, 但并不意味着该凸集是一个确定形状大小的集合. 也就是说, 两个椭球集合的Minkowski和不会产生一个新的椭球, 例如式(18)中的凸集 $\chi_{k, k-1}$形状大小就不能确定. 一般可行的方法就是找到包含两个椭球集合$F({a}_{1}, S_{1})$和 $F( {a}_{2}, S_{2})$ Minkowski和的外定界椭球$F({a}, S)$, 如图 1所示.
下面通过如下定理, 找到某种最优准则下的外定界椭球$F( {a}, S)$.
定理 1. $F( {a}, S)$为椭球集合, A为已知n维方阵, 则
$AF(a, S)=F(Aa, ASA\text{T})$
(20) 证明. $F( {a}, S)$用支持函数形式表示, 那么
$AF(a, S)=A[{{\alpha }^{\text{T}}}a+{{({{\alpha }^{\text{T}S}}\alpha )}^{\frac{1}{2}}}]=A{{\alpha }^{\text{T}}}a+{{({{\alpha }^{\text{T}A}}^{\text{T}}SA\alpha )}^{\frac{1}{2}}}=F(Aa, AS{{A}^{\text{T}}})$
(21) 定理 2. 已知椭球$F( {a}_{1}, S_{1})$、$F( {a}_{2}, S_{2})$, 包含两个椭球集合Minkowski和的外定界椭球$F( {a}, S)$可以表示为
$F(a, S)=F({{a}_{1}}, {{S}_{1}})\oplus F({{a}_{2}}, {{S}_{2}})=F({{a}_{1}}+{{a}_{2}}, S(p))$
(22) 式中
$\left\{ \matrix{ a = {a_1} + {a_2} \hfill \cr S(p) = (1 + {p^{ - 1}}) \hfill \cr {S_1} + (1 + p){S_2},\;p > 0 \hfill \cr} \right.\;$
证明. 考虑两个椭球$F( {a}_{1}, S_{1})$、$F( {a}_{2}, S_{2})$,
$F({{a}_{i}}, {{S}_{i}})=\{x\in {{R}^{n}}|{{(x-{{a}_{i}})}^{\text{T}}}S_{i}^{-1}\times (x-{{a}_{i}})\le 1\}, \ \ \ i=1, 2$
(23) 假定一个椭球
$F(a, S)=\{x\in {{R}^{n}}|{{(x-a)}^{\text{T}}}{{S}^{-1}}(x-a)\le 1\}$
(24) 如图 1所示, 由于两个椭球的Minkowski和并不能直接表示为一个椭球, 为了使椭球$F( {a}, S)$ 能够包含椭球$F( {a}_{1}, S_{1})$和$F( {a}_{2}, S_{2})$的Minkowski和, 则必须满足不等式 [5]
$\eta (F(a, S)) \ge \eta (F({a_1}, {S_1})) + \eta (F({a_2}, {S_2}))$
(25) 式中$\eta(\cdot)$为支持函数, 根据支持函数的定义, 则
${\alpha ^{\rm{T}}}a + {({\alpha ^{\rm{T}}}S\alpha )^{{1 \over 2}}} \ge {\alpha ^{\rm{T}}}{a_1} + {({\alpha ^{\rm{T}}}{S_1}\alpha )^{{1 \over 2}}} + {\alpha ^{\rm{T}}}{a_2} + {({\alpha ^{\rm{T}}}{S_2}\alpha )^{{1 \over 2}}}$
(26) 取外定界椭球中心
${ a}={ a}_{1}+{ a}_{2} $
(27) 取正定矩阵S为$S_{1}$与$S_{2}$的线性组合
$S=\gamma S_{1}+ \rho S_{2} $
(28) 那么式(26)可以表示为
$\gamma {{\alpha }^{\text{T}}}{{S}_{1}}\alpha +\rho {{\alpha }^{\text{T}}}{{S}_{2}}\alpha \ge {{\alpha }^{\text{T}}}{{S}_{1}}\alpha +2{{(\alpha \text{T}{{S}_{1}}\alpha )}^{\frac{1}{2}}}{{({{\alpha }^{\text{T}}}{{S}_{2}}\alpha )}^{\frac{1}{2}}}+{{\alpha }^{\text{T}}}{{S}_{2}}\alpha $
(29) 令$g_{1}^{2}={ \alpha}^{\rm T}S_{1}{ \alpha}$, $g_{2}^{2}={ \alpha}^{\rm T}S_{2}{ \alpha}$, 并乘以因子$\gamma-1$, 则上式变为
$[(\gamma-1)g_{1}-g_{2}]^{2}+[(\gamma-1)(\rho-1)-1]g_{2}^{2}\geq 0 $
(30) 显然, 当满足下式时, 不等式(30)必然成立
$(\gamma-1)(\rho-1)\geq 1 $
(31) 令可行标量p满足$\gamma-1=p^{-1}$, $\rho-1\geq p$, 则
$\gamma=1+p^{-1}\\rho=1+p $
(32) $rho=1+p$
(33) 即$S(p)$可以表示为
$S(p)=(1+{{p}^{-1}}){{S}_{1}}+(1+p){{S}_{2}}, \ \ \ p>0$
(34) 已知椭球集合描述中的S表征椭球的形状大小, 通过定理2可以看出, 包含两个椭球的Minkowski和的外定界椭球是关于形状大小矩阵$S(p)$的函数, p为引入的可行标量, 不同的p对应不同的矩阵S, 不同 的S又对应不同形状大小的外定界椭球, 在所有这些外定界椭球中, 希望找到某种优化意义下的封闭椭球.
定理 3. 外定界椭球$F( {a}, S(p))$中的最优椭球, 可在如下两种不同最优意义下获得 1)容积最小意义下的最小容积椭球可由如下方程计算
$\sum\limits_{i=1}{n}{{\lambda }_{i}}({{S}_{1}}S_{2}^{-1)+p}=np(p+1), \ \ \ p>0$
(35) 其中, ${{\lambda }_{i}}({{S}_{1}}{{S}_{2}}^{-1})$为矩阵 $S_{1}S_{2}^{-1}$ 的特征值, n 为状态维数.
2)半轴平方和最小意义下的最小迹椭球可由如下方程计算
$p=\text{tr}({{S}_{1}})\text{tr}({{S}_{2}})$
(36) 证明. 最小化椭球$F( {a}, S)$的容积等价于求取椭球形状大小矩阵的最小行列式 [18]
$f(p)=\det (S(p))=\det ((1+{{p}^{-1}}){{S}_{1}}+(1+p){{S}_{2}}), \ \ \ p>0$
(37) 即等价于求取方程(35)特征值.
最小化椭球$F( {a}, S)$的最小迹等价于求取如下函数的最小值
$f(p)=\text{tr}(S(p))=\text{tr}((\text{1+}{{\text{p}}^{\text{-1}}}){{\text{S}}_{\text{1}}}\text{+}(\text{1+p})\text{2}), \ \ \ \text{p}>\text{0}$
(38) 即等价于求取式(36). 可以看出, p可以在不同的最优意义下计算得到. 获得最小容积椭球的计算需要求解方程(35), 需要较大的计算量, 而通过式(36)获得最小迹椭球计算简单. 此外, 迹是更加适当的标准, 因为半轴的最大长度等价于一个有界误差的最大边界.
2. 联合滤波算法
基于前面集员的知识, 本文提出一种包含椭球集合运算, 且同时能够处理随机和有界噪声的滤波算法 该算法将噪声的随机部分和有界部分分别处理, 随机部分的处理运用卡尔曼滤波的思想, 而有界部分的处理运用SMF的思想, 由于该算法结合了两种滤波思想, 我们将其称之为联合滤波. 下面将线性系统和非线性系统分别展开讨论.
2.1 线性系统
时间更新
线性系统状态方程见式(3), 式中, 状态向量${{x}_{k-1}}\in {{\chi }_{k-1}}\subseteq F({{a}_{k-1}}, {{S}_{k-1}})$, 其中$F( {a}_{k-1}, S_{k-1})$为椭球集合, 且其协方差矩阵为$P_{k-1}$. 系统被零均值高斯白噪声${ w}_{k}$和UBB噪声${ d}_{k}$同时污染, 其中${ w}_{k}$的协方差矩阵为$Q_{k}$, 且${ d}_{k}\in {\delta_{k}}\subseteq F({0}, D_{k})$. 由于高斯白噪声的均值和UBB噪声的中心都为零, 那么将两种噪声结合在一起, 其均值的集合也包含在椭球集合$F({0}, D_{k})$内, 其协方差矩阵记为$Q_{k}$.
状态集合一步预测
${{\chi }_{k, k-1}}={{A}_{k}}{{\chi }_{k-1}}\oplus {{B}_{k}}{{\delta }_{k}}\subseteq F({{a}_{k, k-1}}, {{S}_{k, k-1}})$
(39) 此时的状态用椭球集合表示. 由于椭球集合${\delta_{k}}$中心为0, 依据定理1及定理2, 包含状态集合$ \chi_{k, k-1}$的外定界椭球$F( {a}_{k, k-1}, S_{k, k-1})$的中心值$ {a}_{k, k-1}$为
${ a}_{k, k-1}=A_{k}{ a}_{k-1} $
(40) 表征椭球形状大小的矩阵$S_{k, k-1}$为
${{S}_{k, k-1}}=(1+{{p}^{-1}}){{A}_{k}}{{S}_{k-1}}A_{k}^{\text{T}}+(1+p){{B}_{k}}{{D}_{k}}B_{k}^{\text{T}}$
(41) 式中p的选择依据定理3的结论.
协方差矩阵一步预测
$P_{k, k-1}=A_{k}P_{k-1}A^{\rm T}_{k}+B_{k}Q_{k}B^{\rm T}_{k} $
(42) 测量更新 线性系统测量方程见式(4), 同样, 式中包含两种测量噪声, ${ v}_{k}$为高斯白噪声, 其均值为0且协方差矩阵为$R_{k}$. ${ e}_{k}$为UBB噪声, 其包含在椭球$F({0}, Y_{k})$内. 可以得出$( {y}_{k}-{ e}_{k})\in\psi_{k}$包含在椭球集合$F( {y}_{k}, Y_{k})$内.
状态估计集合
${{\chi }_{k}}=(I-{{K}_{k}}{{H}_{k}}){{\chi }_{k, k-1}}\oplus {{K}_{k}}{{\psi }_{k}}$
(43) 滤波增益
${{K}_{k}}={{P}_{k, k-1}}H_{k}^{\text{T}}{{({{H}_{k}}{{P}_{k, k-1}}H_{k}^{\text{T}}+{{R}_{k}})}^{-1}}$
(44) 类似预测步骤, 包含状态集合$\chi_{k}$的外定界椭球$F( {a}_{k}, S_{k})$的中心${ a_{k}}$为
${{a}_{k}}=(I-{{K}_{k}}{{H}_{k}}){{a}_{k, k-1}}+{{K}_{k}}{{y}_{k}}$
(45) 表征椭球形状大小的矩阵为
${{S}_{k}}=(1+{{q}^{-1}})(I-{{K}_{k}}{{H}_{k}}){{S}_{k, k-1}}{{(I-{{K}_{k}}{{H}_{k}})}^{\text{T}}}+(1+q){{K}_{k}}{{Y}_{k}}K_{k}^{\text{T}}$
(46) 同样, 式中q的选择依据定理3的结论.
估计误差方差阵
$P_{k}=P_{k, k-1}-K_{k}H_{k}P_{k, k-1} $
(47) 对于受到随机和有界两种噪声影响的线性系统, 为了描述统计特性未知的有界噪声, 引入均值集合的概念: 零均值的高斯白噪声不会引起均值的偏移, 而UBB噪声会引起均值的偏移, 将这些所有可能的偏移用集合的形式表达, 即均值集合. 因此, 相比传统的卡尔曼滤波, 当利用椭球边界来描述有界噪声时, 需要多考虑一个参数. 椭球中心均值的计算(式(40)和式(45))和协方差矩阵的计算(式(43)和式(47))都是基于卡尔曼滤波方程实现, 而描述椭球形状大小的矩阵利用集员滤波的思想实现(式(41)和式(46)), 且时间更新和测量更新 步骤都涉及椭球Minkowski和的计算, 并产生一组条件均值. 本文在计算外定界椭球时采用定理3中的最小迹椭球, 当然, 相比较传统的卡尔曼滤波, 相应地增加了计算量.
2.2 非线性系统
EKF在处理非线性滤波问题时, 通过线性化逼近非线性函数, 其中所述的线性化 是在该点(预测值)的一阶泰勒级数展开获得. 然而, 这类双重不确定性系统预测的已不再是唯一的点, 当然不能再以相同的方式实现系统模型的线性化. 因此, 我们需要找到一种能够线性化均值集合的方法. 为了完成这个目的, 将采用文献[19]中提出的方法.
我们需要选择一组近似点, 近似点的选择依据是: 对状态方程和测量方程仿射映射近似时参数的计算, 以尽量减少非线性函数的函数值和那些选择适当近似点的线性化值的加权平方误差和. 选择近似点的方法依赖于估计椭球集合的形状大小. 在文献[19]中, N维椭球建议选择$4N+1$个近似点, 且这些点等间距的分布在椭球的轴上, 如图 2所示. 在随后讨论的时间更新和测量更新过程中, 将采用该方法来实现非线性函数的线性近似.
非线性系统的近似仿射可写成如下形式:状态方程
${{x}_{k}}={{f}_{k}}({{x}_{k-1}})={{A}_{k}}{{x}_{k-1}}+f_{k}^{0}$
(48) 测量方程
${{y}_{k}}={{h}_{k}}({{x}_{k}})={{H}_{k}}{{x}_{k}}+h_{k}^{0}$
(49) 线性化的实现需要解得$A_{k}, { f}_{k}^{0}$和 $H_{k}, { h}_{k}^{0}$. 令${ x}_{k}^{0}$, ${ x}_{k}^{1}$, $\cdots$, $ { x}_{k}^{N}$为椭球集合${{\chi }_{k}}({{\chi }_{k}}\in F({{a}_{k}}, {{S}_{k}}))$滤波的近似点; 令${ x}_{k, k-1}^{0}$, ${ x}_{k, k-1}^{1}$, $\cdots$, ${ x_{k, k-1}}^{N}$为${{\chi }_{k, k-1}}({{\chi }_{k, k-1}}\in F({{a}_{k, k-1}}, {{S}_{k, k-1}}))$预测的近似点. 令$ {d}_{k}^{N}$为真实状态方程和在${ x}_{k}^{N}$点线性化近似计算的误差
$d_{k}^{i}={{f}_{k}}({{x}_{k}}^{i})-{{A}_{k}}x_{k-1}^{i}-f_{k}^{0}, i=1, \cdots , N$
(50) 同样, 令${ e}_{k}^{N}$为真实测量方程和在${ x}_{k, k-1}^{N}$点线性化近似计算的误差
${ e}_{k}^{i}=h_{k}( {x}_{k, k-1}^{i})-H_{k}{ x}_{k, k-1}^{i}-{ h}_{k}^{0} $
(51) 要求选择的$A_{k}, { f}_{k}^{0}$和$H_{k}, { h}_{k}^{0}$, 使得各自的误差和最小, 我们分别在近似点计算加权动态平方误差与测量平方误差
$[A_{k}, { f}_{k}^{0}]=\arg \min_{A_{k}, { f}_{k}^{0}} \sum_{n=0}^{N-1}l_{k}^{N}[{ d}_{k}^{n}]^{\rm T}[{ d}_{k}^{n}] $
(52) 及
$[H_{k}, { h}_{k}^{0}]=\arg \min_{H_{k}, { h}_{k}^{0}} \sum_{n=0}^{N-1}l_{k}^{N}[{ e}_{k}^{n}]^{\rm T}[{ e}_{k}^{n}] $
(53) 其中$l_{k}^{N}$为第n个近似点的权系数. $l_{k}^{N}$选取可以通过加权最小二乘求取.
定义如下矩阵
$\eqalign{ &{L_k} = {\rm{diag}}\{ l_{k - 1}^0, \cdots ,l_{k - 1}^{N - 1}\} \cr &{F_k} = \left[ {\matrix{ {x_k^0}& \cdots &{{x_k}^N} \cr 1& \cdots &1 \cr } } \right] \cr &{G_k} = \left[ {\matrix{ {x_{k,k - 1}^0}& \cdots &{{x_{k - 1}}^N} \cr 1& \cdots &1 \cr } } \right] \cr &{\beta _k} = \left[ \matrix{ {\left( {f_k^0} \right)^T} \hfill \cr \vdots \hfill \cr {\left( {f_k^{N - 1}} \right)^T} \hfill \cr} \right],{\alpha _k} = \left[ \matrix{ {\left( {h_k^0} \right)^T} \hfill \cr \vdots \hfill \cr {\left( {h_k^{N - 1}} \right)^T} \hfill \cr} \right] \cr} $
通过加权最小二乘, 可解得线性化后的参数
$\left[ \begin{align} & A_{k}^{T} \\ & {{\left( f_{k}^{0} \right)}^{T}} \\ \end{align} \right]={{\left( {{F}_{k}}{{L}_{k}}F_{k}^{T} \right)}^{-1}}{{F}_{K}}{{L}_{k}}\beta _{k}^{T}$
(54) $\left[ \begin{align} & H_{k}^{T} \\ & {{\left( h_{k}^{0} \right)}^{T}} \\ \end{align} \right]={{\left( {{G}_{k}}{{L}_{k}}G_{k}^{T} \right)}^{-1}}{{G}_{K}}{{L}_{k}}\alpha _{k}^{T}$
(55) 至此, 包含椭球集合的非线性系统完成了线性化.
时间更新
椭球集合$F( {a}_{k, k-1}, S_{k, k-1})$的中心通过计算非线性方程获得
${ a}_{k, k-1}= f_{k}( {a}_{k-1}) $
(56) 由于线性化已得到矩阵$A_{k}$, 则其余步骤与第2.1节的时间更新一致.
测量更新
预测得到的状态集合$F( {a}_{k, k-1}, S_{k, k-1})$和测量集合$F( {y}_{k}, Y_{k})$融合, 得到椭球$F( {a}_{k}, S_{k})$的中心
${a_k} = {a_{k,k - 1}} + {K_k}[{y_k} - {h_k}({a_{k,k - 1}})]$
(57) 同样, 由于线性化已得到矩阵$H_{k}$, 其余步骤与第2.1节的测量更新一致. 至此, 实现了非线性系统的联合滤波.
本节中线性化是通过最小二乘拟合的方法实现的, 与EKF计算雅可比矩阵不同, 该方法线性化近似的精度取决于系统的阶数和选取近似点的数量, 对于高阶系统, 这将导致矩阵$L_{k}, F_{k}, G_{k}, \beta_{k}, \alpha_{k}$维数较大和最小平方和的计算变得复杂. 在这种情况下, 可以选择以牺牲精度为代价, 选取较少的近似点来减少运算量.
3. 仿真实验
3.1 实验场景
二维平面内, 观测雷达建立在坐标原点, 设定飞行目标在平面内做匀速直线运动, 给出非线性系统描述如下:
${{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}({{w}_{k}}+{{d}_{k}})$
式中
$\begin{align} & {{\text{A}}_{\text{k}}}\text{=}\left[ \begin{matrix} 1 & T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \\ & {{B}_{K}}=\left[ \begin{matrix} \frac{{{T}^{2}}}{2} & 0 \\ T & 0 \\ 0 & \frac{{{T}^{2}}}{2} \\ 0 & T \\ \end{matrix} \right] \\ \end{align}$
其中状态方程为线性, 状态向量${ x}=[x_{1}\;\;\dot{x}_{1}\;\; x_{2}\;\; \dot{x}_{2}]$, $x_{1}$与$x_{2}$表示由$X_{1}$轴和$X_{2}$轴组成的直角坐标系下的位移分量, $\dot{x}_{1}$与$\dot{x}_{2}$表示速度分量. 系统过程噪声分为两部分, ${ w}_{k}$为高斯白噪声, ${ d}_{k}$为UBB噪声.
${y_k} = \left[ \matrix{ {r_k} \hfill \cr {\theta _k} \hfill \cr} \right] = \left[ \matrix{ \sqrt {\left( {{x_1}} \right)_K^2\left( {{x_2}} \right)_k^2} \hfill \cr {\tan ^{ - 1}}{{\left( {{x_2}} \right)k} \over {\left( {{x_1}k} \right)}} \hfill \cr} \right] + {v_k} + {e_k}$
雷达系统的测量值用极坐标表示, 因此测量方程为非线性. ${ y}_{k}$为测量向量, 分量$r_{k}$表示斜距, 分量$\theta_{k}$表示方位角, 测量噪声${ v}_{k}$和${ e}_{k}$分别表示高斯白噪声和UBB噪声.
仿真设定的采样时间为T = 1 s, 过程噪声${ w}_{k}$为零均值高斯白噪声, 其协方差阵$Q_{k}={\rm diag}\{0.5^2, 0.5^2\}$; ${ d}_{k}$为UBB噪声, 其包含在椭球$F(0, {\rm{diag}}\left\{ {{{0.8}^2}, {{0.8}^2}} \right\}$内; 状态初始值为${ x}_{0}=[200 12 200 10]^{\rm T}$, 设定初始状态椭球${\chi _0} = F({x_0}, {S_0}), {S_0}$为初始椭球形状大小矩阵. 测量噪声$ { v}_{k}$为零均值高斯白噪声, 其协方差阵$R_{k}={\rm diag}\{20^2, 0.01^2\}$; ${ e}_{k}$为UBB噪声, 其包含在椭球$F({y_k}, {\rm{diag}}\{ {30^2}, {0.015^2})$内.
由于系统状态方程为线性, 而测量方程为非线性, 仿真中预测步骤 按照第2.1节中的时间更新, 校正步骤按照第2.2节中的测量更新.
3.2 仿真结果
由于实验场景中系统存在噪声, 目标实际运动只能是近似匀速直线运动. 下面分别采用EKF、扩展集员滤波(Extended set-membership filter, ESMF) [20]及本文提出的联合滤波算法对目标运动轨迹进行跟踪.
单独一次实验中, 三种算法跟踪结果如图 3所示, 与EKF相比, 联合滤波算法和ESMF算法估计得到的是一个集合. 仿真中, 为了显示联合滤波估计集合, 选取若干等间距椭球中心($\ast$号表示)并画出其表示的椭球集合. 同样, ESMF估计出的也是一个边界集合, 这里为了方便显示, 图中只给出了ESMF估计集合的中心. 为了测试滤波算法的性能, 进行100次蒙特卡罗仿真, 图 4和图 5分别给出了位移分量和速度分量的均方根误差(Root mean square error, RMSE).
由图 4和图 5以及表 1可以看出, 对于本仿真设定的混合噪声, 联合滤波算法的均方根误差好于ESMF, 接近EKF算法. 相比EKF, 新算法没有获得更好的估计结果是由于 SMF算法是一种以保守性换取鲁棒性的算法, 而联合滤波中处理有界部分采用了SMF的思想. 虽然联合滤波目前在精度上不存在优势, 但该算法有如下特点: 1) 对有界噪声的区别处理, 避免了卡尔曼滤波在非高斯假设下导致的滤波性能下降, 因此, 同SMF一样, 联合滤波得到的估计结果也是一种边界保证估计, 即边界内的值都是可靠的. 如图 3中, 真实值(实线)保证在椭球区域内. 2) 联合滤波算法不但具有较强的鲁棒性, 而且未来通过进一步建模分析系统未建模误差以及线性化误差边界, 使得选取的噪声边界尽量的"紧", 从而进一步提高系统的估计精度 [21]. 3) 允许我们从更深入的视角分析系统对非随机噪声的敏感度.
表 1 RMSE 均值对比Table 1 Comparison of RMSE means算法 RMSE 均值 位移(m) 速度(m/s) EKF 11.2541 1.9795 ESMF 17.7999 3.3716 New -lter 13.5249 2.1869 4. 结论
本文根据混合噪声中随机部分和有界部分的特性, 提出的联合滤波算法解决了线性系统和非线性系统存在混合噪声 情况下的滤波问题. 由于高斯白噪声一阶矩阵为常数, 而UBB噪声会引起误差均值发生偏移, 因此, 该算法得到估计结果可以理解为是具有一定误差边界的卡尔曼滤波值, 且边界内的值都是有效的. 然而, 联合滤波在处理过程中, 在传统卡尔曼滤波的基础上增加了椭球 集合的相关运算, 非线性系统选取近似点线性化的过程中更进一步增加了计算量, 尤其是在系统维数较高以及近似点的选取数量较多时. 这类非线性系统在线性化过程中, 如何能够提高近似精度和减少计算量是需要关注的问题. 另外, 状态估计椭球中"点"的选取值得考虑, 目前一般都是选取椭球中心点作为估计值. 那么选取中心点与可行椭球集合中其他点的区别以及对滤波性能的影响 也是未来需要研究的重点.
-
图 15 同时绕XW和YW轴旋转后的姿态向量
Fig. 15 The pose vector after rotating along with XW and YW axis, simultaneously
表 1 在坐标系{C}中的微球姿态向量
Table 1 The pose vector of micro-sphere in {C}
编号 1 2 3 4 5 ∆xc 0.0350 0.0362 0.0366 0.0370 0.0375 ∆yc 0.0186 0.0254 0.0320 0.0378 0.0439 ∆zc -0.9992 -0.9990 -0.9988 -0.9985 -0.9983 
下载: 导出CSV
表 2 绕YW轴旋转的实验结果
Table 2 The results of rotating along with YW axis
绕XW, YW旋转角(度) 编号 真实值 测量值 1 0, -3 -0.04, -3.0 2 0, -2 -0.02, -2.0 3 0, -1 -0.04, -1.0 4 0, 1 0.06, 1.1 5 0, 2 -0.00, 2.2 6 0, 3 0.12, 3.1
下载: 导出CSV
表 3 绕XW轴旋转的实验结果
Table 3 The results of rotating along with XW axis
绕XW, YW旋转角(度) 编号 真实值 测量值 1 -5, 0 -4.9, -0.03 2 -4, 0 -3.9, 0.02 3 -3, 0 -2.9, -0.01 4 -2, 0 -2.0, -0.02 5 -1, 0 -1.0, -0.05 6 1, 0 1.0, -0.02 7 2, 0 2.0, -0.04 8 3, 0 3.0, 0.01 9 4, 0 3.9, 0.01 10 5, 0 4.9, -0.03
下载: 导出CSV
表 4 同时绕XW、YW轴旋转的实验结果
Table 4 The results of rotating along with XW, YW axis, simultaneously
绕XW, YW旋转角(度) 编号 真实值 测量值 1 -5, -3 -4.8, -3.1 2 -3, -2 -3.0, -2.1 3 -1, -1 -1.0, -1.1 4 1, 1 1.0, 0.9 5 3, 2 3.0, 1.8 6 5, 3 4.8, 2.8
下载: 导出CSV
-
[1] Liu S, Xu D, Zhang D, Zhang Z. High precision automatic assembly based on microscopic vision and force information. IEEE Transactions on Automation Science and Engineering, 2016, 13(1):382-393 doi: 10.1109/TASE.2014.2332543 [2] Liu S, Xu D, Liu F, Zhang D, Zhang Z. Relative pose estimation for alignment of long cylindrical components based on microscopic vision. IEEE/ASME Transactions on Mechatronics, 2016, 21(3), 1388-1398 doi: 10.1109/TMECH.2015.2506906 [3] Jin P, Matikainen P, Srinivasa S S. Sensor fusion for fiducial tags: highly robust pose estimation from single frame RGBD. In: Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canada: IEEE, 2017. 5770-5776 [4] Zheng W, Yan B, Wang Z. Multi-sensor fusion based pose estimation for unmanned aerial vehicles on ships. In: Proceedings of the 2016 IEEE International Conference on Information and Automation. Ningbo, China: IEEE, 2016. 648-653 [5] Chen Y, Fu M, Yang Y, Song W. A method of head pose estimation based on active shape model and stereo vision. In: Proceedings of the 33rd Chinese Control Conference. Nanjing, China: IEEE, 2014. 8277-8282 [6] Su J, Huang X, Wang M. Pose detection of partly covered target in micro-vision system. In: Proceedings of the 10th World Congress on Intelligent Control and Automation. Beijing, China: IEEE, 2012. 4721-4725 [7] Zhang P, Xu D, Wu B. Pose estimation for plane based on monocular microscope vision system. In: Proceedings of the 32nd Chinese Control Conference. Xi'an, China: IEEE, 2013. 5687-5692 [8] Jiang C, Mills J K. Planar cell orientation control system using a rotating electric field. IEEE/ASME Transactions on Mechatronics, 2015, 20(5):2350-2358 doi: 10.1109/TMECH.2014.2377116 [9] Liu S, Li Y, Xing D, Xu D, Su H. An efficient insertion control method for precision assembly of cylindrical components. IEEE Transactions on Industrial Electronics, 2017, 64(12):9355-9365 doi: 10.1109/TIE.2017.2711551 [10] Park H, Park J, Lee D H, Park J H, Baeg M H, Bae J H. Compliance-based robotic peg-in-hole assembly strategy without force feedback. IEEE Transactions on Industrial Electronics, 2017, 64(8):6299-6309 doi: 10.1109/TIE.2017.2682002 [11] Das A N, Popa D O. Precision evaluation of modular multiscale robots for peg-in-hole microassembly tasks. In: Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. San Francisco, USA: IEEE, 2011. 1699-1704 [12] 史亚莉, 张正涛, 徐德.跨尺度微管微球三维半自动装配点胶系统.光学精密工程, 2015, 23(11):3121-3128 http://d.old.wanfangdata.com.cn/Periodical/gxjmgc201511016Shi Ya-li, Zhang Zheng-tao, Xu De. 3D semi-automatic assembly and dispensing system for trans-scale parts of micro-tube and micro-sphere. Optics and Precision Engineering, 2015, 23(11):3121-3128 http://d.old.wanfangdata.com.cn/Periodical/gxjmgc201511016 [13] Li F, Xu D, Zhang Z, Shi Y, Shen F. Pose measuring and aligning of a micro glass tube and a hole on the micro sphere. International Journal of Precision Engineering and Manufacturing, 2014, 15(12):2483-2491 doi: 10.1007/s12541-014-0618-0 期刊类型引用(5)
1. 兰建军. 基于单目视觉的光电跟踪传感器设计. 仪表技术与传感器. 2022(08): 20-24 . 
百度学术2. 李基顺，兰天，蒋泓杨，黄发千，邹胜友. 高原输变电工程电网设备安装姿态自动测量方法. 微型电脑应用. 2022(10): 97-100 . 百度学术3. 高兴华，董登峰，王博，周维虎. 芯片级原子钟核心器件识别与位姿测量. 半导体光电. 2021(01): 136-143 . 百度学术4. 曲吉旺，徐德，张大朋，许家忠. 基于两台显微相机主动运动的微球孔姿态高精度测量方法. 自动化学报. 2021(06): 1315-1326 . 本站查看5. Ying Li，Xi-Long Liu，De Xu，Da-Peng Zhang. Orientation Measurement for Objects with Planar Surface Based on Monocular Microscopic Vision. International Journal of Automation and Computing. 2020(02): 247-256 . 必应学术其他类型引用(6)
-
下载:
计量
- 文章访问数: 1894
- HTML全文浏览量: 521
- PDF下载量: 426
- 被引次数: 11
