Grid Map Merging Approach Based on Image Registration
-
摘要: 栅格地图拼接是多移动机器人协同创建环境地图中的一项关键技术. 本文提出一种图像配准意义下的栅格地图拼接方法. 该方法将栅格地图拼接问题视为图像配准问题, 建立相应的目标函数, 并给出局部收敛的迭代最近点算法求解该目标函数. 为获得最优的拼接结果, 该方法从待拼接的地图中提取局部不变特征, 并借助随机抽样一致性算法分析初始拼接参数, 以作为迭代最近点算法的初值. 最后, 提出了拼接参数已知时的栅格地图融合规则. 实验结果表明, 该方法能可靠地实现栅格地图拼接, 且具有精度高和速度快的优点.Abstract: Grid map merging is an important and fundamental issue in cooperative mapping for multi-robot systems. To address this issue, this paper proposes a novel approach to merging grid maps based on image registration. It first turns the map merging problem into the problem of image registration and then presents the corresponding objective function. This function can be solved by the proposed ICP algorithm, which is locally convergent. To obtain the global minimum, SIFT (scale-invariant feature transform) are extracted from the grid maps and are adopted to estimate good initial parameters by the RANSAC algorithm. Furthermore, a rule is also presented to fuse two grid maps with merging parameters. Experimental results demonstrate that the proposed approach can achieve grid map merging with good robustness, accuracy and efficiency.
-
子空间辨识算法由于能对多输入多输出系统采用统一框架建立状态空间模型,在系统辨识和控制工程领域受到广泛关注 [1]. 有一些子空间算法被提出用于开环辨识工业过程,得到一致估计结果 [2]. 但是,由于过程操作安全性和稳定性的需要,许多工业过程限制在闭环条件下进行辨识实验,由于反馈控制作用的影响,使得过程噪声和输入存在相关性,使得传统开环子空间方法产生辨识偏差 [3]. 闭环系统辨识因此在近年来受到很多关注和探讨 [4],一些闭环子空间辨识算法被相继提出 [5]. 这些算法可被归纳为三类,第一类方法 [6-7]采用辅助变量策略消除噪声影响,保证估计结果的一致性;第二类方法 [8]采用最小二乘法对高阶VARX模型(Vector autoregressive with exogenous inputs model)进行计算得到马尔科夫估计参数,由于VARX模型只包括当前时刻的不可测噪声,该噪声和VARX模型的过去时刻输入无关,从而可保证所得参数的一致性;第三类算法 [9]用噪声预估值代替真实值进行计算保证得到一致估计结果.
基于奇偶空间的闭环子空间辨识算法SIMPCAwc [6]采用过去时刻输入输出数据作为辅助变量来消除噪声,以得到无噪声的输入输出数据,然后从无噪声影响的输入输出数据奇偶空间中提取得到扩展可观测矩阵和下三角形Toeplitz矩阵,从而求得系统矩阵,该方法取得较好辨识精度.然而,文献[7]指出当闭环系统设定点输入激励为不相关白噪声序列时,虽然引入辅助变量与噪声不相关,可以有效地消除噪声,但由于该辅助变量和系统设定点输入激励也不相关,导致从无噪声输入输出数据奇偶空间中提取参数可能同时含有过程模型参数信息和控制器参数信息,因而无法对它们进行区分,从而致使过程模型估计出现偏差.针对SIMPCAwc [6]辨识方法存在的问题,本文通过将输入输出数据正交投影到新息数据的正交补空间来消除噪声,以得到新的无噪声数据矩阵,进而从其对应的奇偶空间中提取得到扩展可观测矩阵和下三角形Toeplitz矩阵. 由于新息数据的正交补空间数据和噪声无关,且同时与系统设定输入激励相关,确保本文方法从新的无噪声奇偶空间中提取的参数只包含过程模型参数,有效地保证估计结果的一致无偏性.由于新息数据为不可测量数据,本文通过模型推导,得到和待辨识状态空间模型等价的VARX模型. 在此基础上,采用最小二乘法对VARX模型进行计算以得到新息的一致估计值. 采用新息一致估计值代替真实值,以完成模型参数估计.为了论证说明本文方法的有效性,严格分析和给出了本文算法保证一致估计的条件.
1. 问题描述
本文研究如下线性离散状态空间过程模型:
$S:\left\{ \begin{align} & x(t+1)=Ax(t)+Bu(t)+w(t) \\ & y(t)=Cx(t)+Du(t)+v(t) \\ \end{align} \right.$
(1) 其中,$x(t)\in {{R}^{{{n}_{x}}}}$,$u(t)\in {{R}^{{{n}_{u}}}}$,$y(t)\in {{R}^{{{n}_{y}}}}$分别为系统状态和过程输入和输出. $v(t)\in {{R}^{{{n}_{y}}}}$和$w(t)\in {{R}^{{{n}_{x}}}}$分别为过程测量噪声. A,B,C,D 分别为相应维数的系统矩阵.本文研究系统在闭环工作条件下,利用系统输入和输出观测数据,辨识对象状态空间(亦称子空间)模型.
由于闭环反馈控制作用的影响,使得过程测量噪声和输入存在相关性,若直接通过模型(1)来辨识系统矩阵,很难消除噪声对辨识结果的不利影响.因此,采用卡尔曼滤波原理 [10],将系统模型(1)等价表示为新息形式
${{S}_{I}}:\left\{ \begin{array}{*{35}{l}} \begin{align} & x(t+1)=Ax(t)+Bu(t)+Ke(t) \\ & y(t)=Cx(t)+Du(t)+e(t) \\ \end{align} \\ \end{array} \right.$
(2) 其中,K为卡尔曼滤波增益,新息$e(t)$为零均值白噪声,当$i＜j$时,新息$e(j)$和输入输出$\{u(i),y(i)\}$不相关.
进一步定义$\bar{A}=A-KC$和$\bar{B}=B-KD$,模型(2)可被等价描述为如下预测形式:
${{S}_{P}}:\left\{ \begin{array}{*{35}{l}} \begin{align} & x(t+1)=\bar{A}x(t)+\bar{B}u(t)+Ky(t) \\ & y(t)=Cx(t)+Du(t)+e(t) \\ \end{align} \\ \end{array} \right.$
(3) 其中,假设$\bar{A}$的特征值严格位于单位圆内.
定义过去和将来水平数分别为p和f,过去和将来输入向量分别为$u_p(t)=[u(t-p)^{\textrm T}$ $\cdots$ $u(t-2)^{\textrm T}$ $u(t-1)^{\textrm T}]^{\textrm T}$和$u_f(t)=[u(t)^{\textrm T}$ $\cdots$ $u(t+f-2)^{\textrm T}$ $u(t+f-1)^{\textrm T}]^{\textrm T}$,定义过去和将来输入Hankel 矩阵Up $=$ $[u_p(t)^{\textrm T}$ $\cdots$ $u_p(N)^{\textrm T}]^{\textrm T}$和$U_f=[u_f(t)^{\textrm T}$ $\cdots$ $u_f(N)^{\textrm T}]^{\textrm T}$,输出和新息数据做类似定义.
对式(3)进行迭代可得:
$x(t)={{\bar{A}}^{p}}x(t-p)+{{\bar{L}}_{1}}{{u}_{p}}(t)+{{\bar{L}}_{2}}{{y}_{p}}(t)$
(4) 其中,扩展可观性矩阵分别表示为 $\bar{L}_1=[\bar{A}^{p-1}\bar{B}$ $\cdots$ $\bar{A}\bar{B}$ $\bar{B}]$,$\bar{L}_2=[\bar{A}^{p-1}K$ $\cdots$ $\bar{A}K$ $K]$.初始状态为$x(t$ $-$ $p)$.当p充分大时,$x(t-p)$可被忽略,将式(4)带入式(3})得到等价VARX模型
${{S}_{V}}:y(t)=C{{\bar{L}}_{1}}{{u}_{p}}(t)+C{{\bar{L}}_{2}}{{y}_{p}}(t)+e(t)$
(5) 本文将采用模型(2)和(5)对系统矩阵进行辨识.定义$X_p=[x(t-p)$ $\cdots$ $x(t-p+N-1)]$ 和Xf $=$ $[x(t)$ $\cdots$ $x(t+N-1)]$.通过对式(2)进行迭代可得:
$Y(t)={{\Gamma }_{f}}{{X}_{f}}+{{H}_{f}}{{U}_{f}}+{{G}_{f}}{{E}_{f}}$
(6) 其中,扩展可观测矩阵为$\Gamma_f=[C^{\textrm T}$ $\cdots$ $(CA^{f-1})^{\textrm T}]^{\textrm T}$.下三角形Toeplitz矩阵分别为
$\begin{align} & {{H}_{f}}=\left[ \begin{matrix} D & \cdots & \cdots & 0 \\ CB & \cdots & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ C{{A}^{f-2}} & \cdots & CB & D \\ \end{matrix} \right] \\ & {{G}_{f}}=\left[ \begin{matrix} 0 & \cdots & \cdots & 0 \\ C & \cdots & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ C{{A}^{f-2}} & \cdots & C & 0 \\ \end{matrix} \right] \\ \end{align}$
2. 闭环子空间辨识算法和一致性分析
2.1 闭环子空间辨识算法
在式(6)的基础上,通过同时计算扩展可观测矩阵$\Gamma_f$和下三角形Toeplitz矩阵Hf实现对系统矩阵的辨识.首先将输入数据移至式(6)的左侧,得到:
$[I-{{H}_{f}}]{{W}_{f}}={{\Gamma }_{f}}{{X}_{f}}+{{G}_{f}}{{E}_{f}}$
(7) 其中,$W_f=[Y_f^{\textrm T}U_f^{\textrm T}]^{\textrm T}$,对Wp做同样定义.
为求解式(7)得到$\Gamma_f$和Hf的估计值,需要消除未知状态和新息的影响.通过在式(7)的左右侧同时引入$\Gamma_f$的正交补向量$\Gamma_f^{\bot}$,由于$(\Gamma_f^{\bot})^{\textrm T}\Gamma_f=0$
$\left[ {{(\Gamma _{f}^{\bot })}^{\text{T}}}-{{(\Gamma _{f}^{\bot })}^{\text{T}}}{{H}_{f}} \right]{{W}_{f}}={{(\Gamma _{f}^{\bot })}^{\text{T}}}{{G}_{f}}{{E}_{f}}$
(8) 通过将式(8)正交投影到Ef的正交补空间来消除新息噪声,
$\left[ {{(\Gamma _{f}^{\bot })}^{\text{T}}}-{{(\Gamma _{f}^{\bot })}^{\text{T}}}{{H}_{f}} \right]{{W}_{f}}\Pi _{{{E}_{f}}}^{\bot }=0$
(9) 其中,$\Pi_{E_f}^{\bot}=I_N-E_f^{\textrm T}(E_fE_f^{\textrm T})^{-1}E_f$是Ef的正交补.由于$\lim_{N \to \infty }E_f\Pi_{{E}_f}^{\bot}=0$和$\lim_{N \to \infty }R_f\Pi_{{E}_f}^{\bot}=R_f$ $\neq$ $0$.根据文献[7]结论可知,无噪声数据块$W_f\Pi_{E_f}^{\bot}$的奇偶空间只包含过程模型参数信息,不会包括控制器模型参数信息.因此,通过$W_f\Pi_{E_f}^{\bot}$的奇偶空间可得到过程模型参数的一致估计值.
由于新息Ef未知,不能进行模型参数估计.这里利用等价辅助模型(5)计算Ef的估计值$\hat{E}_f$,再采用估计值代替真实值进行后续计算.定义新的过去输入输出Hankel 矩阵$U_p(t,N+f)=[u_p(t)$ $\cdots$ $u_p(N+f)]$,$Y_p(t,N+f)=[y_p(t)$ $\cdots$ $y_p(N+f)]$,新息数据做同样定义.定义$W_p(t,N+f)=$ $[U_p^{\textrm T}(t,$ $N+f)$ $Y_p^{\textrm T}(t,N+f)]^{\textrm T}$ 和$Y(t,N+f)=$ $[y(t)$ $\cdots$ $y(N+f)]$.同时定义 $\theta= [C\bar{L}_1$ $C\bar{L}_2]$.采用最小二乘法对式(5)进行计算,可得:
$\hat{\theta }=Y(t,N+f)W_{p}^{\dagger }(t,N+f)$
(10) 其中,
$\begin{align} & W_{p}^{\dagger }(t,N+f)= \\ & W_{p}^{\text{T}}(t,N+f){{[{{W}_{p}}(t,N+f)W_{p}^{\text{T}}(t,N+f)]}^{-1}} \\ \end{align}$
由于估计值$\hat{\theta}$和真实值的误差为
$\Delta \theta =\hat{\theta }-\theta =E(t,N+f)W_{p}^{\dagger }(t,N+f)$
(11) 其中,
$E(t,N+f)=[e(t)\cdots (N+f)]$
由于$\lim_{N \to \infty }E(t,N+f)W_p^{\textrm T}(t,N+f)=0$,若$\lim_{N \to \infty }W_p(t,N+f)W_p^{\textrm T}(t,N+f)>0$ (可作为默认成立条件),则$\hat{\theta}$是一致估计值.
将一致估计值$\hat{\theta}$带入估计值$\hat{Y}(t,N+f)$,可以得到一致估计值$\hat{E}(t,N+f)$ (证明见 第2.2节).
$\begin{align} & \hat{E}(t,N+f)=Y(t,N+f)-\hat{Y}(t,N+f)= \\ & Y(t,N+f)-\hat{\theta }{{W}_{p}}(t,N+f)= \\ & Y(t,N+f)[{{I}_{N+f}}-W_{p}^{\dagger }(t,N+f){{W}_{p}}(t,N+f)]= \\ & Y(t,N+f)\Pi _{{{W}_{p}}(t,N+f)}^{\bot } \\ \end{align}$
(12) 其中,
$\begin{align} & \Pi _{{{W}_{p}}(t,N+f)}^{\bot }={{I}_{N+f}}-W_{p}^{\text{T}}(t,N+f)\times \\ & {{\left[ {{W}_{p}}(t,N+f)W_{p}^{\text{T}}(t,N+f) \right]}^{-1}}{{W}_{p}}(t,N+f) \\ \end{align}$
定义
$\Pi _{{{W}_{p}}(t,N+f)}^{\bot }=[{{\beta }_{1}}\cdots {{\beta }_{N+f}}]$
(13) 则未来噪声Hankel矩阵的一致估计值可通过以下方式重构得到:
${{{\hat{E}}}_{f}}=Y(t,N+f)\left[ \begin{matrix} {{\beta }_{1}} & \cdots & {{\beta }_{N}} \\ {{\beta }_{2}} & \cdots & {{\beta }_{N+1}} \\ \vdots & \ddots & \vdots \\ {{\beta }_{f}} & \cdots & {{\beta }_{f+N}} \\ \end{matrix} \right]$
(14) 进一步将Ef用其一致估计值代替,可得一致估计值$W_f\Pi_{\hat{E}_f}^{\bot}$.通过SVD分解得到:
$\begin{align} & {{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot }= \\ & \left[ {{{\hat{U}}}_{1}}\hat{U}_{1}^{\bot } \right]\left[ \begin{matrix} {{{\hat{\Sigma }}}_{1}} & 0 \\ {{{\hat{\Sigma }}}_{2}} & 0 \\ \end{matrix} \right]\left[ \begin{matrix} \begin{matrix} \hat{V}_{1}^{\text{T}} \\ {{(\hat{V}_{1}^{\bot })}^{\text{T}}} \\ \end{matrix} \\ \end{matrix} \right]={{{\hat{U}}}_{1}}{{{\hat{\Sigma }}}_{1}}\hat{V}_{1}^{\text{T}} \\ \end{align}$
(15) 其中,$\hat{U}_1$是$W_f\Pi_{\hat{E}_f}^{\bot}$的前$n_x+fn_u$个特征向量,则$[(\Gamma_f^{\bot})^{\textrm T}-(\Gamma_f^{\bot})^{\textrm T}H_f]$的一致估计值如下(证明见第2.2节):
$\left[ {{(\hat{\Gamma }_{f}^{\bot })}^{\text{T}}}-{{(\hat{\Gamma }_{f}^{\bot })}^{\text{T}}}{{{\hat{H}}}_{f}} \right]={{(\hat{U}_{1}^{\bot })}^{\text{T}}}$
(16) 定义$\hat{U}_1^{\bot}=[P_1^{\textrm T}P_2^{\textrm T}]^{\textrm T}$,其中,$P_1=\hat{U}_1^{\bot}(1:fn_y$,$:)$,$P_2=\hat{U}_1^{\bot}(1+fn_y:end,:)$ (Matlab表示).可得:
$\hat{\Gamma }_{f}^{\bot }={{P}_{1}}$
(17) $-{{(\hat{\Gamma }_{f}^{\bot })}^{\text{T}}}{{{\hat{H}}}_{f}}=P_{2}^{\text{T}}$
(18) 由于$(\Gamma_f^{\bot})^{\bot}=\Gamma_f$,根据式(17),可得:
${{{\hat{\Gamma }}}_{f}}={{(\hat{\Gamma }_{f}^{\bot })}^{\bot }}={{I}_{f{{n}_{y}}}}-{{P}_{1}}{{(P_{1}^{\text{T}}{{P}_{1}})}^{-1}}P_{1}^{\text{T}}$
(19) 为了得到Hf的估计值,做如下定义:
$-P_{1}^{\text{T}}=[{{\phi }_{1}}\cdots {{\phi }_{f}}]$
(20) $P_{2}^{\text{T}}=[{{\varphi }_{1}}\cdots {{\varphi }_{f}}]$
(21) 其中,${{\phi }_{i}}\in {{R}^{(i{{n}_{y}}-{{n}_{x}})\times {{n}_{y}}}},{{\varphi }_{i}}\in {{R}^{(i{{n}_{y}}-{{n}_{x}})\times {{n}_{u}}}}$.
将式(20)和式(21)带入式(18),可得:
$\left[ \begin{matrix} {{\phi }_{1}} & \cdots & {{\phi }_{f-1}} & {{\phi }_{f}} \\ {{\phi }_{2}} & \cdots & {{\phi }_{f}} & 0 \\ \vdots & \vdots & \ddots & \vdots \\ {{\phi }_{f}} & 0 & \cdots & 0 \\ \end{matrix} \right]{{H}_{f1}}=\left[ \begin{matrix} {{\varphi }_{1}} \\ \phi {{i}_{2}} \\ \vdots \\ {{\phi }_{f}} \\ \end{matrix} \right]$
(22) 其中,$H_{f1}=[D^{\textrm T}(CB)^{\textrm T}\cdots(CA^{f-2}B)^{\textrm T}]$.
采用最小二乘法可得$H_{f1}$的一致估计如下:
${{{\hat{H}}}_{f1}}={{\left[ \begin{matrix} {{\phi }_{1}} & \cdots & {{\phi }_{f-1}} & {{\phi }_{f}} \\ {{\phi }_{2}} & \cdots & {{\phi }_{f}} & 0 \\ \vdots & \vdots & \ddots & \vdots \\ {{\phi }_{f}} & 0 & \cdots & 0 \\ \end{matrix} \right]}^{\dagger }}\left[ \begin{matrix} {{\varphi }_{1}} \\ \phi {{i}_{2}} \\ \vdots \\ {{\phi }_{f}} \\ \end{matrix} \right]$
(23) 系统矩阵估计值$\hat{A}$和$\hat{C}$可直接从$\hat{\Gamma}$中提取,即
$\hat{C}=\hat{\Gamma }(1:{{n}_{y}},1:{{n}_{x}})$
(24) $\hat{A}={{{\hat{\Gamma }}}^{\dagger }}(1+{{n}_{y}}:f{{n}_{y}},1:{{n}_{x}})\hat{\Gamma }(1:(f-1){{n}_{y}},1:{{n}_{x}})$
(25) 由于
${{H}_{f1}}=\left[ \begin{matrix} {{I}_{{{n}_{y}}}} & 0 \\ 0 & \Gamma (1:(f-1){{n}_{y}},1:{{n}_{x}}) \\ \end{matrix} \right]\left[ \begin{matrix} \begin{matrix} D \\ B \\ \end{matrix} \\ \end{matrix} \right]$
(26) 系统矩阵估计值$\hat{B}$和$\hat{D}$可采用最小二乘法从$\hat{H}_{f1}$中计算得到:
$\left[ \begin{matrix} \begin{matrix} {\hat{D}} \\ {\hat{B}} \\ \end{matrix} \\ \end{matrix} \right]={{\left[ \begin{matrix} {{I}_{{{n}_{y}}}} & 0 \\ 0 & \hat{\Gamma }(1:(f-1){{n}_{y}},1:{{n}_{x}}) \\ \end{matrix} \right]}^{\dagger }}{{{\hat{H}}}_{f1}}$
(27) 本文基于新息估计和正交投影的闭环子空间辨识方法(Closed-loop subspace identification method using innovation estimation and orthogonal projection,CSIMIEOP)可总结如下:
步骤 1. 由式(10)求解$\hat{\theta}$,再由式(12)计算$\hat{E}(t,N+f)$,采用式(14)构造$\hat{E}_f$.
步骤 2. 通过SVD分解对式(15)进行计算.
步骤 3. 通过式(19})和式(23)计算估计值$\hat{\Gamma}$和$\hat{H}_{f1}$.
步骤 4. 通过式(24),式(25)和式(27)求解系统矩阵$\hat{A}$,$\hat{B}$,$\hat{C}$ 和 $\hat{D}$.
2.2 闭环一致条件分析
由第2.1节闭环子空间辨识算法可知,本文采用新息估计和正交投影消除噪声,得到一致估计结果. 辨识结果是否一致取决于$\hat{E}(t,N+f)$ 和$[(\hat{\Gamma}_f^{\bot})^{\textrm T}$ $-$ $(\hat{\Gamma}_f^{\bot})^{\textrm T}\hat{H}_f]$是否一致,本文对它们的一致估计条件进行分析和说明,给出如下定理.
定理 1. 若$$\lim_{N \to \infty }W_p(t,N+f)W_p^{\textrm T}(t,N+f)>0$$ 则$\hat{E}(t,N+f)$ 为一致估计值.
证明. 由式(12)可知,估计值$\hat{E}(t,N+f)$和真实值的误差为
$\begin{align} & \Delta E(t,N+f)=\hat{E}(t,N+f)-E(t,N+f)= \\ & Y(t,N+f)\Pi _{{{W}_{p}}(t,N+f)}^{\bot }-E(t,N+f)= \\ & [Y(t,N+f)-\hat{Y}(t,N+f)]-E(t,N+f)= \\ & [\theta -\hat{\theta }]{{W}_{p}}(t,N+f)= \\ & \Delta \theta {{W}_{p}}(t,N+f)= \\ & E(t,N+f){{W}_{p}}{{(t,N+f)}^{\dagger }}{{W}_{p}}(t,N+f) \\ \end{align}$
(28) 由于
$\underset{N\to \infty }{\mathop{\lim }}\,E(t,N+f)W_{p}^{\text{T}}(t,N+f)=0$
若
$\underset{N\to \infty }{\mathop{\lim }}\,{{W}_{p}}(t,N+f)W_{p}^{\text{T}}(t,N+f)＞0$
则
$\underset{N\to \infty }{\mathop{\lim }}\,\Delta E(t,N+f)=0$
从而可知$\hat{E}(t,N+f)$为一致估计值.
定理 2. 若系统可控可观,且$E_fE_f^{\textrm T}>0$ 和
$\bar{E}\left\{ \left[ \begin{matrix} {{u}_{p}}(t) \\ {{u}_{f}}(t) \\ {{e}_{p}}(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{p}}(t) \\ {{u}_{f}}(t) \\ {{e}_{p}}(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\}＞0$
则$[(\hat{\Gamma}_f^{\bot})^{\textrm T} -(\hat{\Gamma}_f^{\bot})^{\textrm T}\hat{H}_f]$为一致估计值.
证明. 令$\Omega =\left[ \begin{matrix} {{\Gamma }_{f}} & {{H}_{f}} \\ 0 & I \\ \end{matrix} \right]$,由式(9)可知:
$\Omega \left[ \begin{matrix} \begin{matrix} {{U}_{f}} \\ {{X}_{f}} \\ \end{matrix} \\ \end{matrix} \right]\left[ \begin{matrix} {{I}_{N}}-E_{f}^{\text{T}}{{({{E}_{f}}E_{f}^{\text{T}})}^{-1}}{{E}_{f}} \\ \end{matrix} \right]$
(29) 由式(29)可得:
$\begin{array}{l} \mathop {\lim }\limits_{N \to 0} {W_f}\Pi _{{{\hat E}_f}}^ \bot W_f^{\rm{T}} = \\ \Omega \left[ {\begin{array}{*{20}{c}} \begin{array}{l} {U_f}\\ {X_f} \end{array} \end{array}} \right]\left[ {{I_N} - E_f^{\rm{T}}{{({E_f}E_f^{\rm{T}})}^{ - 1}}{E_f}} \right]{\left[ {\begin{array}{*{20}{c}} \begin{array}{l} {U_f}\\ {X_f} \end{array} \end{array}} \right]^{\rm{T}}}{\Omega ^{\rm{T}}} = \\ \Omega \left\{ {{R_1} - {R_2}{{({E_f}E_f^{\rm{T}})}^{ - 1}}R_2^{\rm{T}}} \right\}{\Omega ^{\rm{T}}} \end{array}$
(30) 其中,
$\begin{array}{l} {R_1} = \left[ {\begin{array}{*{20}{c}} {{U_f}{X_f}} \end{array}} \right]{\left[ {\begin{array}{*{20}{c}} {{U_f}{X_f}} \end{array}} \right]^{\rm{T}}}\\ = \bar E\left\{ {\left[ {\begin{array}{*{20}{c}} {{u_f}(t)}\\ {x(t)} \end{array}} \right]{{\left[ {\begin{array}{*{20}{c}} {{u_f}(t)}\\ {x(t)} \end{array}} \right]}^{\rm{T}}}} \right\}\\ {R_2} = \left[ {\begin{array}{*{20}{c}} \begin{array}{l} {U_f}\\ {X_f} \end{array} \end{array}} \right]E_f^{\rm{T}} = \bar E\left\{ {\left[ {\begin{array}{*{20}{c}} {{u_f}(t)}\\ {x(t)} \end{array}} \right]e_f^{\rm{T}}(t)} \right\} \end{array}$
若系统为可控可观系统,则$\Omega$为满秩矩阵
${\rm{rank}}(\Omega ) = {n_x} + f{n_u}$
(31) 由于新息为平稳零均值白噪声系列,可知$E_fE_f^{\rm T}$ $>$ $0$,据文献[11]定理2可知,式(31)中第2项的秩可通过如下计算得到:
$\text{rank}\left\{ \bar{E}\left\{ \left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\} \right\}-f{{n}_{y}}$
(32) 进一步将式(4)带入式(32),可得:
$\begin{align} & \bar{E}\left\{ \left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\}= \\ & \Upsilon \bar{E}\left\{ \left[ \begin{matrix} {{u}_{p}}(t) \\ {{u}_{f}}(t) \\ {{e}_{p}}(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{p}}(t) \\ {{u}_{f}}(t) \\ {{e}_{p}}(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\}{{\Upsilon }^{\text{T}}} \\ \end{align}$
(33) 其中,$\Upsilon =\left[ \begin{matrix} 0 & {{I}_{p{{n}_{u}}}} & 0 & 0 \\ {{L}_{1}} & 0 & {{L}_{1}} & 0 \\ 0 & 0 & 0 & {{I}_{p{{n}_{y}}}} \\ \end{matrix} \right]$.
由于系统为可控可观测系统,采用文献[11]定理2可知$\Upsilon$为满秩矩阵.同时如果 式(33)第2项为正定满秩矩阵,根据文献[12]给定的秩条件可得:
$\begin{align} & \text{rank}\left\{ \overline{\text{E}}\left\{ \left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{f}}(t) \\ x(t) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\} \right\}= \\ & f({{n}_{u}}+{{n}_{y}})+{{n}_{x}} \\ \end{align}$
(34) 因此,矩阵(32)为满秩矩阵.
$\text{rank}\left( \left[ {{R}_{1}}-{{R}_{2}}{{({{E}_{f}}E_{f}^{\text{T}})}^{-1}}R_{2}^{\text{T}} \right] \right)=f{{n}_{u}}+{{n}_{x}}$
(35) 由式(30)、式(32)和式(35)可知\vskip0.1mm\noindent
$\text{rank}\left( \underset{N\to 0}{\mathop{\lim }}\,{{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot }W_{f}^{\text{T}} \right)=f{{n}_{u}}+{{n}_{x}}$
(36) 则
$\begin{align} & \text{rank}\left( \underset{N\to 0}{\mathop{\lim }}\,{{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot } \right)= \\ & \text{rank}\left( \underset{N\to 0}{\mathop{\lim }}\,{{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot }W_{f}^{\text{T}} \right)=f{{n}_{u}}+{{n}_{x}} \\ \end{align}$
(37) 由以上秩条件可知,$\lim_{N \to \infty }W_f\Pi_{\hat{E}_f}^{\bot}$的非零特征向量个数为$f(n_u+n_y)-n_x-fn_u=fn_y-n_x$.因此$\lim_{N \to \infty }\hat{U}_1^{\bot}$ 是$\lim_{N \to \infty }W_f\Pi_{\hat{E}_f}^{\bot}$的零特征向量.则
$\underset{N\to 0}{\mathop{\lim }}\,\hat{U}_{1}^{\bot }{{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot }=0$
(38) 由于${\rm rank}(\Gamma_f^{\bot})=fn_y-n_x$ [13],因此
$\text{rank}\left( [{{(\Gamma _{f}^{\bot })}^{\text{T}}}-{{(\Gamma _{f}^{\bot })}^{\text{T}}}{{H}_{f}}] \right)=f{{n}_{y}}-{{n}_{x}}$
(39) 同时,由于
$\underset{N\to 0}{\mathop{\lim }}\,\left[ {{(\Gamma _{f}^{\bot })}^{\text{T}}}-{{(\Gamma _{f}^{\bot })}^{\text{T}}}{{H}_{f}} \right]{{W}_{f}}\Pi _{{{{\hat{E}}}_{f}}}^{\bot }=0$
(40) 所以$[(\Gamma_f^{\bot})^{\textrm T}-(\Gamma_f^{\bot})^{\textrm T}H_f]$和$\lim_{N \to \infty }\hat{U}_1^{\bot}$满足
$\begin{align} & rowspace\left( [{{(\Gamma _{f}^{\bot })}^{\text{T}}}-{{(\Gamma _{f}^{\bot })}^{\text{T}}}{{H}_{f}}] \right)= \\ & rowspace\left( \underset{N\to 0}{\mathop{\lim }}\,\hat{U}_{1}^{\bot } \right) \\ \end{align}$
(41) 因此,$[(\hat{\Gamma}_f^{\bot})^{\textrm T} -(\hat{\Gamma}_f^{\bot})^{\textrm T}\hat{H}_f]=(\hat{U}_1^{\bot})^{\textrm T}$,由于$\hat{E}_f$是一致估计值,可知$(\hat{U}_1^{\bot})^{\textrm T}$为一致估计值,则$[(\hat{\Gamma}_f^{\bot})^{\textrm T}$ $-$ $(\hat{\Gamma}_f^{\bot})^{\textrm T}\hat{H}_f]$为一致估计值.
由于$[(\hat{\Gamma}_f^{\bot})^{\textrm T} -(\hat{\Gamma}_f^{\bot})^{\textrm T}\hat{H}_f]$为一致估计值,根据平移变换法求取系统矩阵的一致不变性,可知系统矩阵估计值也为一致估计值.
2.3 闭环一致条件合理性分析
第2.2节定理2的两个假设条件涉及未知新息信息,以上假设是否合理,将直接决定本文方法能否取得一致估计结果.
由于模型(1)可表示为模型(2),则新息的协方差可表示为$\bar{E}[e(t)e^{\textrm T}(t)]=CP^{\textrm T}C+R_3$ (具体表述可参考文献[10]第5章),其中P为半正定矩阵,对于系统噪声有$R_3=\bar{E}[v(t)v^{\textrm T}(t)]>0$,则$\bar{E}[e(t)e^{\textrm T}(t)]$ $>$ $0$,可知$E_fE_f^{\textrm T}>0$.
对于假设2,由于
$\begin{align} & \underset{N\to 0}{\mathop{\lim }}\,{{W}_{p}}(t,N+f)W_{p}^{\text{T}}(t,N+f)= \\ & \bar{E}\left\{ \left[ \begin{matrix} {{u}_{p}}(t) \\ {{y}_{p}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{p}}(t) \\ {{y}_{p}}(t) \\ \end{matrix} \right]}^{\text{T}}} \right\} \\ \end{align}$
(42) 进一步,由式(2)可知
$\begin{align} & {{y}_{p}}(t)={{\Gamma }_{p}}[{{L}_{1}}u_{p}^{\text{T}}(t-p)+{{L}_{2}}e_{p}^{\text{T}}(t-p)]+ \\ & {{H}_{p}}{{u}_{p}}(t)+{{{\bar{G}}}_{p}}{{e}_{p}}(t) \\ \end{align}$
(43) 其中,扩展可观性矩阵分别表示为 ${L}_1=[{A}^{p-1}{B}$ $\cdots$ ${A}{B}$ ${B}]$,${L}_2=[{A}^{p-1}K$ $\cdots$ ${A}K$ $K]$,下三角形Toeplitz矩阵为
${{H}_{f}}=\left[ \begin{matrix} I & \cdots & \cdots & 0 \\ C & \cdots & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ C{{A}^{f-2}} & \cdots & C & I \\ \end{matrix} \right]$
将式(43)带入式(42),可得:
$\begin{align} & \bar{E}\left[ \begin{matrix} {{u}_{p}}(t) \\ {{y}_{p}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{p}}(t) \\ {{y}_{p}}(t) \\ \end{matrix} \right]}^{\text{T}}}= \\ & \Xi \bar{E}\left[ \begin{matrix} {{u}_{p}}(t-p) \\ {{u}_{p}}(t) \\ {{e}_{p}}(t-p) \\ {{e}_{f}}(t) \\ \end{matrix} \right]{{\left[ \begin{matrix} {{u}_{p}}(t-p) \\ {{u}_{p}}(t) \\ {{e}_{p}}(t-p) \\ {{e}_{f}}(t) \\ \end{matrix} \right]}^{\text{T}}}{{\Xi }^{\text{T}}} \\ \end{align}$
(44) 其中,$\Xi=\left[\begin{array}{cccc} 0&I_{pn_u}&0&0\\Gamma_{P}L_1&H_p&\Gamma_{P}L_2&\bar{G}_p \end{array}\right]$.
若系统可控可观,由文献[11]定理2可知,${\rm rank}\left(\Xi\right) =p(n_u+n_y)$,$\Xi$是满秩矩阵,则式(42)正定的充分必要条件为式(44)中第2项正定,由于变量p和f可取任何值,则假设2成立的充分必要条件为$\lim_{N \to \infty }W_{p+f}W_{p+f}^{\textrm T}>0$.可通过验证$\lim_{N \to \infty }W_{p+f}W_{p+f}^{\textrm T}>0$来确定假设2是否成立.即若$\lim_{N \to \infty }W_{p+f}W_{p+f}^{\textrm T}>0$,则假设2成立.
3. 仿真研究
考虑文献[6]中研究的闭环系统
$y(t)-0.9y(t-1)=u(t-1)+e(t)+0.9e(t-1)$
(45) 其中,反馈控制结构为$u(t)=- 0.6y(t)+r(t)$.过程噪声设置为方差为0.2的白噪声序列.
对系统设定输入激励$r(k)$为单位方差白噪声序列和相关序列两种情况进行研究. 其中相关序列设为$r(t)=(1+0.8q^{-1}+0.6q^{-2})r_0(t)$,$r_0(t)$为单位方差白噪声序列. 过去和未来水平数均设置为10.在不同数据长度$N\in[200,8 000]$的情况下进行1 000次Monte Carlo仿真. 本文方法的辨识结果与SIMPCAwc算法 [6]进行比较,当$r(t)$为单位方差白噪声系列时,系统极点(真实值为0.9)的估计平均值如图 1所示,系统极点的估计标准方差如图 2 所示.当系统设定输入$r(t)$为相关系列时,系统极点的平均值如图 3所示,系统极点的估计标准方差如图 4所示.
图 1 系统设定输入激励为白噪声的系统极点估计平均值Fig. 1 Mean value of estimated poles for white noise setpoint excitation
图 2 系统设定输入激励为白噪声的系统极点估计标准方差Fig. 2 Standard deviation of estimated poles for white noise setpoint excitation
图 3 系统设定输入激励为相关序列的系统极点估计平均值Fig. 3 Mean value of estimated poles for correlated quasi-stationary setpoint excitation
图 4 系统设定输入激励为相关序列的系统极点估计标准方差Fig. 4 Standard deviation of estimated poles for correlated quasi-stationary setpoint excitation从以上结果可以看出,当系统设定输入为单位方差白噪声系列时,SIMPCAwc算法得出有偏估计结果;只有当系统设定输入激励为相关序列时,SIMPCAwc算法才能保证无偏估计结果.本文CSIMIEOP算法对系统设定输入激励为无关序列和相关序列时均可得到一致无偏结果,并且估计精度优于SIMPCAwc算法.
4. 结论
本文提出一种基于新息估计和正交投影的闭环子空间辨识算法,对系统设定点输入激励为白噪声无关序列和相关序列的情况均可得到一致无偏估计结果,并且相对于近期有关文献给出的方法如SIMPCAwc算法 [6],能进一步提高辨识精度.同时,严格分析和证明了本文算法保证一致估计的条件.最后通过仿真实例验证了本文方法的有效性和优越性.
-
[1] Durrant-Whyte H, Bailey T. Simultaneous localization and mapping (SLAM): Part I. IEEE Robotics and Automation Magazine, 2006, 13(2): 99-110 [2] [2] Bailey T, Durrant-Whyte H. Simultaneous localization and mapping (SLAM): Part II. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117 [3] [3] Thrun S, Burgard W, Fox D. Probabilistic Robotics. Cambridge: MIT Press, 2005. [4] [4] Strasdat H, Montiel J M M, Davison A J. Visual slam: why filter? Image and Vision Computing, 2012, 30(2): 65-77 [5] Zhu Ji-Hua, Zheng Nan-Ning, Yuan Ze-Jian, Zhang Qiang. A SLAM algorithm based on central difference particle filter. Acta Automatica Sinica, 2010, 6(3): 249-257(祝继华, 郑南宁, 袁泽剑, 张强. 基于中心差分粒子滤波的SLAM算法. 自动化学报, 2010, 6(3): 249-257) [6] Song Yu, Li Qing-Ling, Kang Yi-Fei, Yan De-Li. SLAM with square-root cubature Rao-Blackwillised particle filter. Acta Automatica Sinica, 2014, 40(2): 357-367 (宋宇, 李庆玲, 康轶非, 闫德立. 平方根容积Rao-Blackwillised粒子滤波SLAM算法. 自动化学报, 2014, 40(2): 357-367) [7] [7] Williams S B, Dissanayake G, Durrant-Whyte H. Towards multi-vehicle simultaneous localisation and mapping. In: Proceedings of the 2002 IEEE International Conference on Robotics and Automation. Washington, D.C., USA: IEEE, 2002, 3: 2743-2748 [8] [8] Thrun S, Liu Y. Multi-robot SLAM with sparse extended information filers. Robotics Research. Berlin: Springer-Verlag, 2005. 254-266 [9] [9] Carpin S, Birk A, Jucikas V. On map merging. Robotics and Autonomous Systems, 2005, 53(1): 1-14 [10] Birk A, Carpin S. Merging occupancy grid maps from multiple robots. Proceedings of the IEEE, 2006, 94(7): 1384- 1397 [11] Howard A, Parker L E, Sukhatme G S. Experiments with a large heterogeneous mobile robot team: exploration, mapping, deployment and detection. The International Journal of Robotics Research, 2006, 25(5-6): 431-447 [12] Fox D, Ko J, Konolige K, Limketkai B, Schulz D, Stewart B. Distributed multirobot exploration and mapping. Proceedings of the IEEE, 2006, 94(7): 1325-1339 [13] Censi A, Iocchi L, Grisetti G. Scan matching in the Hough domain. In: Proceedings of the 2005 IEEE International Conference on Robotics and Automation. Barcelona, Spain: IEEE, 2005. 2739-2744 [14] Carpin S. Fast and accurate map merging for multi-robot systems. Autonomous Robots, 2008, 25(3): 305-316 [15] Saeedi S, Paull L, Trentini M, Seto M, Li H. Map merging using Hough peak matching. In: Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura, Portugal: IEEE, 2012. 4683-4688 [16] Ma Xin, Song Rui, Guo Rui, Li Yi-Bin. Immune adaptive genetic algorithm for occupancy grid maps merging. Control Theory and Applications, 2009, 26(9): 1004-1008(马昕, 宋锐, 郭睿, 李贻斌. 基于免疫自适应遗传算法的机器人栅格地图融合. 控制理论与应用, 2009, 26(9): 1004-1008) [17] Pan Wei, Cai Zi-Xing, Chen Bai-Fan. An approach to cooperative multi-robot map building in complex environments. Journal of Sichuan University (Engineering Science Edition), 2010, 42(1): 144-148(潘薇, 蔡自兴, 陈白帆. 复杂环境下多机器人协作构建地图的方法. 四川大学学报(工程科学版), 2010, 42(1): 144-148) [18] Liu Li-Mei, Cai Zi-Xing. Study on map merging for multi-robots. Journal of Chinese Computer Systems, 2012, 33(9): 1934-1937(刘利枚, 蔡自兴. 多机器人地图融合方法研究. 小型微型计算系统, 2012, 33(9): 1934-1937) [19] Sun Rong-Chuan, Ma Shu-Gen, Li Bin, Wang Ming-Hui, Wang Yue-Chao. Simultaneous localization and sampled environment mapping based on a divide-and-conquer ideology. Acta Automatica Sinica, 2010, 36(12): 1697-1705(孙荣川, 马书根, 李斌, 王明辉, 王越超. 基于分治法的同步定位与环境采样地图创建. 自动化学报, 2010, 36(12): 1697-1705) [20] Besl P J, McKay N D. A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256 [21] Chetverikov D, Stepanov D, Krsek P. Robust Euclidean alignment of 3D point sets: the trimmed iterative closest point algorithm. Image and Vision Computing, 2005, 23(3): 299-309 [22] Nuchter A, Lingemann K, Hertzberg J. Cached k-d tree search for ICP algorithms. In: Proceedings of the 6th International Conference on 3-D Digital Imaging and Modeling. Quebec, Canada: IEEE, 2007. 419-426 [23] Hwang Y, Han B, Ahn H K. A fast nearest neighbor search algorithm by nonlinear embedding. In: Proceedings of the 2012 IEEE International Computer Vision and Pattern Recognition. Providence, USA: IEEE, 2012. 3053-3060 [24] Nchter A, Elseberg J, Schneider P, Paulus D. Study of parameterizations for the rigid body transformations of the scan registration problem. Computer Vision and Image Understanding, 2010, 114(8): 963-980 [25] Lowe D G. Distinctive image features from scale-invariant keypoints. International Journal of Computer Vision, 2004, 60(2): 91-110 [26] Lebeda K, Matas J, Chum O. Fixing the locally optimized RANSAC. In: Proceedings of the 23rd British Machine Vision Conference. Guildford, UK: BMVA Press, 2012. 95.1- 95.11 [27] Brown M, Lowe D G. Automatic panoramic image stitching using invariant features. International Journal of Computer Vision, 2007, 74(1): 59-73 [28] Zhu Ji-Hua, Zheng Nan-Ning, Yuan Ze-Jian, He Yong-Jian. A SLAM approach by combining ICP algorithm and particle filter. Acta Automatica Sinica, 2009, 35(8): 1107-1113(祝继华, 郑南宁, 袁泽剑, 何永健. 基于ICP算法和粒子滤波的未知环境地图创建. 自动化学报, 2009, 35(8): 1107-1113) [29] Ying S H, Peng J G, Du S Y, Qiao H. A scale stretch method based on ICP for 3D data registration. IEEE Transactions on Automation Science and Engineering, 2009, 6(3): 559- 565 [30] Lu F, Milios E. Robot pose estimation in unknown environments by matching 2D range scans. Journal of Intelligent and Robotic Systems, 1994, 18(3): 249-275 期刊类型引用(12)
1. 王亚军，白翱，张博，郭超. 基于TOGAF的工艺知识全生命周期管理架构研究. 知识管理论坛. 2024(06): 519-532 . 
百度学术2. 刘拥民，罗皓懿，谢铁强. 基于XGBoost-ARIMA方法的PM_(2.5)质量浓度预测模型的研究及应用. 安全与环境学报. 2023(01): 211-221 . 百度学术3. 张旭，张亮，金博，张红哲. 基于不确定性的多元时间序列分类算法研究. 自动化学报. 2023(04): 790-804 . 本站查看4. 缪燕子，王志铭，李守军，代伟. 基于背景值和结构相容性改进的多维灰色预测模型. 自动化学报. 2022(04): 1079-1090 . 本站查看5. 徐任超，阎威武，王国良，杨健程，张曦. 基于周期性建模的时间序列预测方法及电价预测研究. 自动化学报. 2020(06): 1136-1144 . 本站查看6. 李晓理，张博，杨旭. 基于图像混合核的列生成PM_(2.5)预测. 工程科学学报. 2020(07): 922-929 . 百度学术7. 杨博帆，张琳，张搏，宋亚飞，丁尔启. 动态多模型指数平滑法融合的在线预测方法. 系统工程与电子技术. 2020(09): 2013-2021 . 百度学术8. 乔俊飞，丁海旭，李文静. 基于WTFMC算法的递归模糊神经网络结构设计. 自动化学报. 2020(11): 2367-2378 . 本站查看9. 郝广涛，林清华，李晓梅. 超短期负荷预测中指数平滑法平滑系数的确定方法. 莆田学院学报. 2020(05): 80-86 . 百度学术10. 杨亚莉，李智伟，钟卫军. 基于二向注意力循环神经网络的PM_(2.5)浓度预测. 空军工程大学学报(自然科学版). 2020(06): 101-106 . 百度学术11. 慕凯，张祥，余士龙，李俊，李宇，戴笑俊. 滑动加权马尔科夫模型在降水量预测中的应用. 气象水文海洋仪器. 2019(04): 34-36 . 百度学术12. 周强，肖强宏，王浩然，高乐乐. 基于BEADS-ESMC组合算法的三相光伏并网逆变柜触点红外温度预测方法. 变频器世界. 2018(11): 72-78 . 百度学术其他类型引用(16)
-
下载:
计量
- 文章访问数: 2504
- HTML全文浏览量: 122
- PDF下载量: 1436
- 被引次数: 28
下载:
