Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter
-
摘要: 在无GPS信号的受限环境中，基于序列立体影像的运动平台视觉定位精度较高，能够改正航迹推算方法的误差累积，但在纹理贫乏或光照不足的环境下容易定位失败。为提高受限环境下运动平台定位的精度与稳健性，提出一种基于联邦滤波的立体相机、惯性测量单元（inertial measurement unit，IMU）及里程计组合导航方法。该方法在联邦滤波中利用IMU分别同里程计与立体相机构成子滤波器，有效避免立体视觉定位失效而导致的系统定位失败，提高了定位稳健性。地下巷道实验结果证明，所提方法能有效提高运动平台导航定位的精度，并且在立体视觉定位失效的情况下仍能实现连续定位。Abstract: In a GPS-denied environment, visual odometry based on sequential stereo images can be applied to correction of accumulated localization errors from dead-reckoning. Visual odometry however, preforms. poorly or even fails under poor texture or illumination conditions. To achieve high precision and robust localization, a federated filter based localization method is proposed that integrates stereo cameras, the IMU, and wheel odometer. The two sub filters in the federated filter, constructed by a IMU integrated with the wheel odometer and stereo cameras provide robust localization when visual odometry fails. Experimental results in an underground field demonstrate robust performance and improvement in localization accuracy.
-
Keywords:
- federated filter /
- visual odometry /
- dead-reckoning /
- IMU
-
总体最小二乘(total least squares，TLS)是同时顾及观测向量和系数矩阵误差的数据处理方法[1]，作为变量误差(errors-in-variables，EIV)模型的严密处理方法，近年来在大地测量等领域得到了广泛应用[2-11]。大地测量领域中参数估计往往伴随先验信息，此时EIV模型就扩展为附有约束的EIV模型[12]。参数的先验信息为等式形式时，即为附有等式约束的EIV模型(equality-constrained EIV，ECEIV)，文献[13-17]对ECEIV模型进行了详细的研究；若附加不等式约束条件，就构成附有不等式约束的EIV模型(inequality -constrained EIV，ICEIV)。国内外有关附有不等式约束的EIV模型的文献不多，文献[18]首次把ICEIV问题转化为基于组合方法求解的线性互补问题；文献[19]提出基于穷举法的ICTLS算法，将不等式约束转化为等式约束，计算所有可能的附有等式约束组合的总体最小二乘解，将最优解作为ICTLS问题的参数解。上述两种算法存在计算效率较低的问题，穷举算法的迭代次数与不等式方程个数成指数关系，如模型共有N个不等式方程，则需迭代2N次，这种算法理论上可以求得最优解，但显然不适合解算大规模的附不等式约束问题；且两种算法都不能解算观测向量和系数矩阵元素不等精度或相关性的情况。文献[20]提出附有不等式约束的加权总体最小二乘(inequality-constrained weighted TLS，ICWTLS)算法，将ICEIV模型转化为非线性最优化问题，给出了ICWTLS最优化的充分必要条件，并通过常见的有效集法和序列二次规划法(sequential quadratic programming，SQP)等最优化方法[21, 22]求解；ICWTLS算法能够解决观测量间精度不等和观测量相关的问题，且不受约束方程个数限制。文献[20]给出了目标函数的一阶梯度和Hesse矩阵(二阶导数)的计算公式，然而求解Hesse矩阵的公式复杂、计算量大，对于大规模的参数估计问题，需要考虑算法的计算能力，参数个数较多、观测数据量大时，将大大增加SQP算法的计算量；另外，每次迭代所求的Hesse矩阵往往出现非正定的情况，此时不能保证计算所得的方向为对应点处的下降方向，使算法的计算效率降低。文献[12]提出了基于Partial EIV模型[23]的ICWTLS算法，Partial EIV模型顾及了系数矩阵中随机元素和非随机元素同时存在的情况，将系数矩阵中的随机元素和非随机元素分离，把随机元素作为未知参数和模型参数一同求解。文献[24]则提出了Partial EIV模型中未知参数和系数矩阵元素都附有不等式约束时的ICWTLS算法。
本文以ICEIV模型为基础，针对文献[20]中目标函数的Hesse矩阵计算量大、容易出现非正定的问题，利用拟牛顿校正公式得到Hesse的近似矩阵，研究了利用拟牛顿校正法解算ICWTLS的问题。本文算法可以解算观测量和系数矩阵元素的权阵为一般正定对称权阵的情况，同时避免了求解复杂的Hesse矩阵，而且每次迭代过程近似矩阵都是对称正定的，保证了算法的收敛速度，对于涉及大规模的附不等式约束问题时更有效。
1 附有不等式约束的EIV模型
附有不等式约束的总体最小二乘模型[20]：
$$ \left\{ \begin{array}{l} \mathit{\boldsymbol{y}} - {\mathit{\boldsymbol{e}}_\mathit{\boldsymbol{y}}} = \left( {\mathit{\boldsymbol{A}} - {\mathit{\boldsymbol{E}}_A}} \right)\mathit{\boldsymbol{\xi }}\\ \mathit{\boldsymbol{C\xi }} - \mathit{\boldsymbol{w}} \ge 0 \end{array} \right. $$ (1) $$ \mathit{\boldsymbol{e}}: = \left[ \begin{array}{l} {\rm{vec}}\left( {{\mathit{\boldsymbol{E}}_A}} \right)\\ {\mathit{\boldsymbol{e}}_\mathit{\boldsymbol{y}}} \end{array} \right] = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{e}}_A}}\\ {{\mathit{\boldsymbol{e}}_y}} \end{array}} \right] \sim \left( {\left[ {\begin{array}{*{20}{c}} 0\\ 0 \end{array}} \right],\sigma _0^2{\mathit{\boldsymbol{Q}}_{ll}} = \sigma _0^2{\mathit{\boldsymbol{P}}^{ - 1}}} \right) $$ (2) 式中，y ∈Rn×1为观测向量; ey∈Rn×1表示观测向量的随机误差向量; EA∈Rn×u表示系数矩阵A ∈Rn×u的随机误差矩阵；ξ ∈Ru×1为未知参数向量；C ∈Rm×u、w ∈Rm×1分别表示不等式组的系数矩阵和常数向量；符号vec(·)表示矩阵的每一列拉直成列向量的运算符号；其中l =vec(A, Y])∈Rn(u+1)×1;e∈Rn(u+1)×1表示扩展向量l的误差向量；eA=vec(EA)∈Rn(u×1)；σ02为单位权方差；矩阵Qll及其逆矩阵P分别表示l的协因数阵和权阵(Qll的详细形式参见文献[20])。
上述模型中的Qll和P为一般的正定对称矩阵，故可用于解决普通的附不等式约束的加权总体最小二乘问题。
2 ICWTLS问题的拟牛顿修正算法及收敛性分析
2.1 ICWTLS问题的拟牛顿修正算法
根据总体最小二乘准则
$$ \begin{array}{l} \min :{\mathit{\boldsymbol{e}}^{\rm{T}}}\mathit{\boldsymbol{Pe}}\\ {\rm{s}}{\rm{.t}}{\rm{.:}}\;\;\;\mathit{\boldsymbol{y}} - {\mathit{\boldsymbol{e}}_\mathit{\boldsymbol{y}}} = \left( {\mathit{\boldsymbol{A}} - {\mathit{\boldsymbol{E}}_A}} \right)\mathit{\boldsymbol{\xi }} \end{array} $$ (3) 构造拉格朗日目标函数
$$ \begin{array}{l} \mathit{\boldsymbol{ \boldsymbol{\varPhi} }}\left( {\mathit{\boldsymbol{e}},\mathit{\boldsymbol{\mu }},\mathit{\boldsymbol{\xi }}} \right) = {\mathit{\boldsymbol{e}}^{\rm{T}}}\mathit{\boldsymbol{Pe}} + \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;2{\mathit{\boldsymbol{\mu }}^{\rm{T}}}\left( {\mathit{\boldsymbol{y}} - {\mathit{\boldsymbol{e}}_y} - \mathit{\boldsymbol{A\xi }} + {\mathit{\boldsymbol{E}}_A}\mathit{\boldsymbol{\xi }}} \right) \end{array} $$ (4) 式中，μ ∈Rn×1表示拉格朗日乘子。
对式(4)分别求关于e、ξ、μ的偏导数，且令偏导数等于零，最终得到如下的形式[6]：
$$ {{\mathit{\boldsymbol{\hat e}}}^{\rm{T}}}\mathit{\boldsymbol{P\hat e}} = {\left( {\mathit{\boldsymbol{y}} - \mathit{\boldsymbol{A\hat \xi }}} \right)^{\rm{T}}}{\left( {\mathit{\boldsymbol{\hat B}}{\mathit{\boldsymbol{Q}}_{ll}}{{\mathit{\boldsymbol{\hat B}}}^{\rm{T}}}} \right)^{ - 1}}\left( {\mathit{\boldsymbol{y}} - \mathit{\boldsymbol{A\hat \xi }}} \right) $$ (5) 式中，B = [ξT⊗In, －In]∈Rn×n(u+1)，且为行满秩矩阵。
则ICWTLS模型最终转化为普通附有不等式约束的最优化问题[20]
$$ \begin{array}{l} \min :\;\;\;\left. {\mathit{\boldsymbol{f}}\left( {\mathit{\boldsymbol{\hat \xi }}} \right)} \right) = {\left( {\mathit{\boldsymbol{y}} - \mathit{\boldsymbol{A\xi }}} \right)^{\rm{T}}}{\left( {\mathit{\boldsymbol{\hat B}}{\mathit{\boldsymbol{Q}}_{ll}}{{\mathit{\boldsymbol{\hat B}}}^{\rm{T}}}} \right)^{ - 1}} \cdot \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\left( {\mathit{\boldsymbol{y}} - \mathit{\boldsymbol{A\xi }}} \right)\mathit{\boldsymbol{C\hat \xi }} - \mathit{\boldsymbol{w}} \ge 0 \end{array} $$ (6) 将附不等式约束的加权总体最小二乘问题转化为最优化问题，避免了运用类似穷举法[19]需要顾及不等式方程个数对计算效率的影响。对上述最优化问题，本文仍基于SQP法来求解, 基本思想是通过求解二次规划子问题确定搜索的下降方向，以减小选取的罚函数来取得步长，重复解算直到获得最优解。由于二次规划子问题中的Hesse矩阵存在计算量大等不足，本文算法每次迭代时通过拟牛顿修正法得到近似的对称正定矩阵代替原有Hesse矩阵。构造拉格朗日函数：
$$ \mathit{\boldsymbol{L}}\left( {\mathit{\boldsymbol{\hat \xi }},\mathit{\boldsymbol{\lambda }}} \right) = \mathit{\boldsymbol{f}}\left( {\mathit{\boldsymbol{\hat \xi }}} \right) - {\mathit{\boldsymbol{\lambda }}^{\rm{T}}}\left( {\mathit{\boldsymbol{C\hat \xi }} - \mathit{\boldsymbol{w}}} \right) $$ (7) 根据文献[21]可知，存在互补条件或松弛条件，有$ {\mathit{\boldsymbol{\hat \lambda }}_{ki}} \ge 0 $且$ {\mathit{\boldsymbol{\hat \lambda }}_{ki}} $(Ciξ－ wi)=0(i=1, 2, …，m)，其中，Ci为C的第i行；wi为w的第i个元素；λk i表示乘子向量λk的第i个元素。在给定未知参数$ {\mathit{\boldsymbol{\hat \xi }}_k} $及乘子向量λk处，将拉格朗日函数进行二次多项式近似并把不等式约束函数线性化，得到二次规划子问题：
$$ \begin{array}{l} \min :\;\;\;\frac{1}{2}\mathit{\boldsymbol{d}}_k^{\rm{T}}\mathit{\boldsymbol{H}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right){\mathit{\boldsymbol{d}}_k} + \nabla \mathit{\boldsymbol{f}}{\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right)^{\rm{T}}}{\mathit{\boldsymbol{d}}_k}\\ \;\;\;\;\;\;\;\;\;\;\;\;\mathit{\boldsymbol{C\hat \xi }} + \mathit{\boldsymbol{C}}{\mathit{\boldsymbol{d}}_k} \ge 0 \end{array} $$ (8) 式中，$ \nabla \mathit{\boldsymbol{f}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right) $是目标函数$ f\left( {\mathit{\boldsymbol{\hat \xi }}} \right) $的一阶梯度；$ \mathit{\boldsymbol{H}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right) $是$ f\left( {\mathit{\boldsymbol{\hat \xi }}} \right) $的Hesse矩阵。
用Bk近似代替$ \mathit{\boldsymbol{H}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right) $，Bk具备几个特点[22]：
① 一定意义上$ {\mathit{\boldsymbol{B}}_k} \approx \mathit{\boldsymbol{H}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k}} \right) $；使得算法产生的dk方向近似于牛顿方向；
② 所有的Bk对称正定，确保dk方向为目标函数$ f\left( {\mathit{\boldsymbol{\hat \xi }}} \right) $的下降方向；
③ 矩阵的校正规则相对简单。
BFGS[21]校正公式是目前最有效的拟牛顿校正，校正过程为：
令$ {\mathit{\boldsymbol{s}}_k} = {{\mathit{\boldsymbol{\hat \xi }}}_{k + 1}} - {{\mathit{\boldsymbol{\hat \xi }}}_k} $
$$ {\mathit{\boldsymbol{y}}_k} = {\nabla _x}\mathit{\boldsymbol{L}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_{k + 1}},{{\mathit{\boldsymbol{\hat \lambda }}}_{k + 1}}} \right) - {\nabla _x}\mathit{\boldsymbol{L}}\left( {{{\mathit{\boldsymbol{\hat \xi }}}_k},{{\mathit{\boldsymbol{\hat \lambda }}}_k}} \right) $$ (9) 为满足校正矩阵正定性，需要skTyk＞0，但式(9)很难满足此条件，Powell[22]给出了yk的修正公式，令
$$ {\mathit{\boldsymbol{z}}_k} = {\mathit{\boldsymbol{\theta }}_k}{\mathit{\boldsymbol{y}}_k} + \left( {1 - {\mathit{\boldsymbol{\theta }}_k}} \right){\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k} $$ (10) 式中，
$$ {\mathit{\boldsymbol{\theta }}_k} = \left\{ \begin{array}{l} 1,\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{y}}_k} \ge 0.2\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k}\\ \frac{{0.8\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k}}}{{\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k}}},\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{y}}_k} < 0.2\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k} \end{array} \right. $$ (11) 因此，得到Bk的校正公式为：
$$ {\mathit{\boldsymbol{B}}_{k + 1}} = {\mathit{\boldsymbol{B}}_k} - \frac{{{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k}\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}}}{{\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{s}}_k}}} + \frac{{{\mathit{\boldsymbol{z}}_k}\mathit{\boldsymbol{z}}_k^{\rm{T}}}}{{\mathit{\boldsymbol{s}}_k^{\rm{T}}{\mathit{\boldsymbol{z}}_k}}} $$ (12) 可以验证skTzk≥0.2 skTBksk＞0，这样就确保了当Bk为正定矩阵时，Bk+1也为正定矩阵。
为取得下降方向的步长，借助罚函数P (ξ, σ)：
$$ \mathit{\boldsymbol{P}}\left( {\mathit{\boldsymbol{\xi }},\sigma } \right) = \mathit{\boldsymbol{f}}\left( \mathit{\boldsymbol{\xi }} \right) + {\sigma ^{ - 1}}{\left\| {{\mathit{\boldsymbol{g}}_i}\left( \mathit{\boldsymbol{\xi }} \right)} \right\|_1} $$ (13) 式中，σ表示罚参数(σ＞0)；gi(ξ)=max{0, Ciξ － wi}(i=1, 2, …, m)；上述的搜索方向dk是罚函数P(ξ, σ)在(ξk, σk)处的下降方向，这是dk的一个很好的性质。另外，在每次迭代中，需要更新罚参数，更新规则可为：
$$ {\sigma _{k + 1}} = \left\{ \begin{array}{l} {\sigma _k},\sigma _k^{ - 1} \ge \tau + \delta ,\\ \left( {\tau + 2\delta } \right) - 1,\sigma _k^{ - 1} < \tau + \delta \end{array} \right. $$ (14) 式中，τ=‖ λk‖，δ为任意大于零的常数。按某种线性搜索规则确定步长αk，得到优化参数$ {{\mathit{\boldsymbol{\hat \xi }}}_{k + 1}} $及拉格朗日乘子$ {{\mathit{\boldsymbol{\hat \lambda }}}_{k + 1}} $；若满足条件，即为对应的ICWTLS问题的最优解，否则通过(Broyden、Fletcher、Goldfarb、Sharno, BFGS)公式校正矩阵Bk为Bk+1，继续求解二次规划子问题，直到得到最优解。
本文拟牛顿修正法的计算步骤如下。
步骤1 给定初始点ξ0, λ0；ξ0可以取不考虑不等式约束时的TLS参数解。初始正定对称矩阵B0(如B0为单位矩阵)。不等式方程的Jacobi矩阵A0=▽C(ξ0)T。参数η∈(0, 1/2)，ρ∈(0, 1)，容许误差ε，令k=0。
步骤2 求解子问题式(8)，得到最优解dk。
步骤3 对于罚函数P (ξ, σ)，根据式(14)更新罚参数(取常数σ0＞0)，保证dk是P (ξk, σk)处的下降方向。
步骤4 按照Armijo非精确线搜索技术确定步长，即求解满足下列不等式成立的最小正数mk：
$$ \begin{array}{l} \mathit{\boldsymbol{P}}\left( {{\mathit{\boldsymbol{\xi }}_k} + {\rho ^m}{\mathit{\boldsymbol{d}}_k},{\sigma _k}} \right) - \mathit{\boldsymbol{P}}\left( {{\mathit{\boldsymbol{\xi }}_k},{\sigma _k}} \right)\\ \;\;\;\;\;\;\;\;\;\;\; \le \eta {\rho ^m}\mathit{\boldsymbol{P'}}\left( {{\mathit{\boldsymbol{\xi }}_k},{\sigma _k},{\mathit{\boldsymbol{d}}_k}} \right) \end{array} $$ (15) 式中，P (ξk, σk, dk)为P(ξk, σk)的方向导数，令αk=ρmk，得ξk+1=ξk+αk dk；‖ dk‖/‖ξk+1‖≤ε则停止迭代，ξk+1即为参数最优值；否则，转步骤5。
步骤5 计算Ak+1=▽ C(ξk+1)T及最小二乘乘子λk+1= Ak+1 Ak+1T]－1 Ak+1▽ fk+1。
步骤6 校正矩阵Bk为Bk+1；令k=k+1，转至步骤2，直到取得满足条件的最优解。
2.2 拟牛顿修正算法的收敛性分析
本文利用拟牛顿算法的迭代公式来代替Hesse矩阵，是对Hesse矩阵的近似处理，使得替代矩阵Bk满足拟牛顿方程(条件)：Bk+1 sk= yk。对于任意函数f(x)，BFGS迭代公式产生的序列{ xk}全局收敛到f(x)的极小点x*，需满足两个条件[22]：①函数f (x)是二阶连续可微的；②水平集Τ(x)={x∈Rn| f (x)≤f(x0) }是凸集，并且函数f(x)在T(x)上一致凸，即满足dTG(x)d＞0。本文讨论的ICWTLS问题，目标函数f(ξ)在可行区域内二阶连续可微，满足条件①；对于条件②，一般情况下，dTG(x)d＞0在可行方向上也成立，即使G(x)为非正定矩阵时，也有dTG(x)d＞0成立[20]。因此，对于满足上述两个条件的ICWTLS问题，采用Bk代替Hesse矩阵可以收敛到最优极值点。
3 拟牛顿修正算法验证与分析
为了验证本文拟牛顿修正算法及其计算效率，运用文献[25]设计的算例进行了分析计算，并与已有的ICWTLS算法估计结果进行了比较。主要讨论了两种情况，一是验证本文算法的适用性，不考虑系数矩阵和观测量的元素的精度及相关性，即Q(或P)矩阵为单位矩阵；二是考虑系数矩阵和观测量元素的权阵，即Q(或P)矩阵为一般对称正定矩阵。
3.1 系数矩阵与观测向量元素精度相同且不相关的情况
$$ \begin{array}{l} \mathit{\boldsymbol{A}} = \\ \left\{ {\begin{array}{*{20}{c}} {0.9501}&{0.7620}&{0.6153}&{0.4057}\\ {0.2311}&{0.4564}&{0.7919}&{0.9354}\\ {0.6068}&{0.0185}&{0.9218}&{0.9169}\\ {0.4859}&{0.8214}&{0.7382}&{0.4102}\\ {0.8912}&{0.4447}&{0.1762}&{0.8936} \end{array}} \right\},\mathit{\boldsymbol{y}} = \left\{ {\begin{array}{*{20}{c}} {0.0578}\\ {0.3528}\\ {0.8131}\\ {0.0098}\\ {0.1388} \end{array}} \right\} \end{array} $$ $$ \mathit{\boldsymbol{C}} = \left\{ {\begin{array}{*{20}{c}} { - 0.2027}&{ - 0.2721}&{ - 0.7467}&{ - 0.4659}\\ { - 0.1987}&{ - 0.1988}&{ - 0.4450}&{ - 0.4186}\\ { - 0.6037}&{ - 0.0152}&{ - 0.9318}&{ - 0.8462} \end{array}} \right\} $$ $$ \mathit{\boldsymbol{w}} = \left\{ {\begin{array}{*{20}{c}} { - 0.5251}\\ { - 0.2026}\\ { - 0.6721} \end{array}} \right\}, - 0.1 \le {\mathit{\boldsymbol{\xi }}_i} \le 2\left( {i = 1,2,3,4} \right) $$ 系数矩阵A ∈R5×4，模型参数ξ∈R4×1，观测向量y ∈R5×1；A和y所有元素为等精度且不相关；共11个不等式约束方程，参数区间－0.1≤ξi≤2(i=1, 2, 3, 4)表示为(I4⊗[1, -1]T)ξ≥(l4T⊗[-0.1, -2]T)(I4表示4阶单位矩阵；l4T表示4×1阶元素为1的矩阵)。
ICWTLS算法的结果见表 1，本文同时计算了不考虑约束时的总体最小二乘解$ {{\mathit{\boldsymbol{\hat \xi }}}_{{\rm{TLS}}}} $，并作为计算ICWTLS解的初始值。
表 1 单位权阵下的计算结果Table 1. Estimation Results of Unit Weight Condition参数 $ {{\mathit{\boldsymbol{\hat \xi }}}_{{\rm{本文}}}} $ $ \mathit{\boldsymbol{\hat \xi }}_{{\rm{ICWTLS}}}^{\left[{20} \right]} $ $ \mathit{\boldsymbol{\hat \xi }}_{{\rm{ICTLS}}}^{\left[{19} \right]} $ $ \mathit{\boldsymbol{\hat \xi }}_{{\rm{TLS}}}^{} $ $ {{\mathit{\boldsymbol{\hat \xi }}}_1} $ -0.100 000 0 -0.100 000 2 -0.100 0 0.188 8 $ {{\mathit{\boldsymbol{\hat \xi }}}_2} $ -0.100 000 0 -0.100 000 0 -0.100 0 -0.716 7 $ {{\mathit{\boldsymbol{\hat \xi }}}_3} $ 0.168 547 2 0.168 547 2 0.168 5 0.560 5 $ {{\mathit{\boldsymbol{\hat \xi }}}_4} $ 0.399 776 6 0.399 776 8 0.399 8 0.210 6 k 8 4 204 8 - 注：k为迭代次数 (1) 本文的计算结果与文献[20]的结果$ \mathit{\boldsymbol{\hat \xi }}_{{\rm{ICWTLS}}}^{\left[{20} \right]} $及文献[19]采用穷举法的结果$ \mathit{\boldsymbol{\hat \xi }}_{{\rm{ICTLS}}}^{\left[{19} \right]} $几乎一致，说明算法迭代中用近似矩阵代替Hesse矩阵并不影响参数估计结果；不等式$ \mathit{\boldsymbol{C\hat \xi }} - \mathit{\boldsymbol{w}} $=[0.260 5, 0, 0.238 6, 0, 2.100 0, 0, 2.100 0, 0.268 5, 1831 5, 0.499 8, 1.600 2]T≥0；全部满足不等式约束条件方程。本文算法与文献[20]的算法迭代次数明显少于采用穷举法的迭代次数；说明两者在本算例中都是适用的且有很好的运算效率。
(2) 本文算法迭代次数为8次，多于文献[20]的次数(4次)，这是由于本文算法是通过近似矩阵代替目标函数的Hesse矩阵，可以大大减少计算量和存储量，但迭代过程中是近似牛顿型搜索，而直接运用Hesse矩阵的文献[20]算法是牛顿型搜索，因此，当迭代过程中Hesse矩阵是对称正定矩阵时，后者收敛迭代次数会更少；算例一可以验证算法的有效性，但观测数据量有限，因此，本文算法在计算量和存储量方面的优势没有很好体现。
3.2 系数矩阵与观测向量元素精度不相同且相关的情况
当考虑系数矩阵元素和观测向量的元素精度不相同且相关时，则对应的Q矩阵为非单位阵，为一般正定对称矩阵；在文献[20]中，由于矩阵Q的复杂性，将直接导致Hesse的计算量增大；用Toeplitz矩阵来构造Q矩阵，且矩阵元素为qij=1－|i－j|/25(1≤i, j≤25)。计算结果见表 2。
表 2 精度不相同且相关情况下的计算结果Table 2. Estimation Results of Unequal Precision and Relative Condition参数 $ {{\mathit{\boldsymbol{\hat \xi }}}_1} $ $ {{\mathit{\boldsymbol{\hat \xi }}}_2} $ $ {{\mathit{\boldsymbol{\hat \xi }}}_3} $ $ {{\mathit{\boldsymbol{\hat \xi }}}_4} $ k T/s $ {{\mathit{\boldsymbol{\hat \xi }}}_{{\rm{本文}}}} $ -0.014 929 32 -0.100 000 00 -0.081 021 650 0.624 401 911 9 1.0 $ \mathit{\boldsymbol{\hat \xi }}_{{\rm{ICWTLS}}}^{\left[{20} \right]} $ -0.014 929 34 -0.100 000 00 -0.081 021 650 0.624 401 912 127 11.5 注：k为迭代次数，T为迭代时间 由表 2，两种算法都适用解决加权的ICTLS问题，所得参数解几乎完全一致；同样全部满足不等式约束方程；但本文的计算效率明显高于文献[20]的ICWTLS算法，本文算法迭代次数为9次，而后者迭代了127次；算法收敛时间T分别约为1.0 s和11.5 s，避免了直接计算Hesse矩阵，大大减少了计算量。迭代过程发现，目标函数的Hesse矩阵出现非正定的情况，导致文献[20]的算法不能保证每次搜索方向都是下降方向，这将影响其收敛速度；本文算法每次迭代近似矩阵都是正定矩阵，每次的搜索方向都是下降的，因此，本文算法的收敛速度比文献[20]算法快。
4 结语
当模型参数存在先验信息时，需要结合这些先验信息来求解参数，如用大地测量方法反演地震断层模型参数[26]，根据先验信息可以判断断层参数在某个区间范围或边界条件内取得，因此，在估计模型参数时充分考虑先验信息的约束可以得到更合理的结果。
本文通过研究附有约束的EIV模型，利用拟牛顿修正法来解算ICWTLS问题。本文算法可以解决观测向量和系数矩阵元素精度不相同且相关的情况；针对文献[20]的算法对于大规模参数估计存在计算量大且复杂的问题，本文通过拟牛顿法校正得到的近似矩阵来代替Hesse矩阵，使得ICWTLS算法的计算量大大减少，更适合解决参数估计中观测数据量较大，模型参数较多的问题；本文校正矩阵在每次迭代中都确保其为对称正定矩阵，这就使得算法具有较好的收敛速度；本文分析了关于拟牛顿修正矩阵在解决ICWTLS问题的严密性。值得注意的是，当ICEIV模型系数矩阵同时存在随机和非随机元素时，可以通过文献[27]中提出的5条定权准则来确定权矩阵，此时附不等式约束问题加权总体最小二乘问题的拟牛顿修正算法仍然适用；也可以将模型转化为PEIV模型[28]，运用本文算法进行求解。
-
![]()
图 4 实验1三种方法确定的行驶轨迹
Figure 4. Localization Results in Experiment 1 from Dead-reckoning, Visual Odometry and Federated Filter
![]()
图 5 实验2三种方法确定的行驶轨迹
Figure 5. Localization Results in Experiment 2 from Dead-reckoning, Visual Odometry and Federated Filter
![]()
图 6 不同方法定位误差
Figure 6. Localization Errors from Dead-Reckoning, Visual Odometry and Federated Filter
![]()
图 7 不同数量断帧下的定位误差
Figure 7. Localization Errors with Different Numbers of Failed Frames
表 1 全路径7个检查点处定位误差统计/m
Table 1 Statistic of Localization Errors in 7 Check Points of Traverse Path/m
解算方法 检查点号 2 3 4 5 6 7 1 航迹推算 3.13 3.14 3.12 4.43 6.10 10.57 21.70 视觉测程 0.25 1.18 0.60 0.57 3.74 5.13 5.59 联邦滤波(无断帧) 0.47 1.23 0.76 0.97 3.31 4.50 4.96 
下载: 导出CSV
表 2 联邦滤波中不同数量断帧定位误差对比/m
Table 2 Statistic of Localization Errors with Different Numbers of Failed Frames/m
断帧情况 检查点号 2 3 4 5 6 7 1 无断帧 0.47 1.23 0.76 0.97 3.31 4.50 4.96 断36帧 0.47 1.23 1.05 1.07 3.87 4.87 5.55 断136帧 0.47 1.23 2.14 1.59 4.93 5.88 6.67 断236帧 0.47 1.23 1.96 1.84 4.70 5.27 6.41 断336帧 0.47 1.23 5.03 3.87 7.53 8.93 9.32
下载: 导出CSV
-
[1] 万文辉. 基于立体视觉的深空探测车自主定位理论与方法研究[D]. 北京: 中国科学院遥感应用研究所, 2012: 51-76 http://www.irgrid.ac.cn/handle/1471x/823847 Wan Wenhui. Theory and Methods of Stereo Vision Based Autonomous Rover Localization in Deep Space Exploration[D]. Beijing: Institute of Remote Sensing Applications, Chinese Academy of Sciences, 2012: 51-76 http://www.irgrid.ac.cn/handle/1471x/823847
[2] Matthies L. Dynamic Stereo Vision[D]. Pittsburgh, USA: Carnegie Mellon University, 1989
[3] Olson C F, Matthies L H, Schoppers M, et al. Rover Navigation Using Stereo Ego-Motion[J]. Robotics and Autonomous Systems, 2003, 43(4):215-229 doi: 10.1016/S0921-8890(03)00004-6
[4] Li R, Di K, Matthies R E, et al. Rover Localization and Landing Site Mapping Technology for 2003 Mars Exploration Rover Mission[J]. Photogrammetric Engineering and Remote Sensing, 2004, 70(1):77-90 doi: 10.14358/PERS.70.1.77
[5] Sünderhauf N, Konolige K, Lacroix S, et al. Visual Odometry Using Sparse Bundle Adjustment on an Autonomous Outdoor Vehicle[M]. Proceedings of Tagungsb and Autonome Mobile Systeme, Springer Vehicle, 2005
[6] Konolige K, Agrawal M, Solá J. Large Scale Visual Odometry for Rough Terrain[J].Tracts in Advanced Robotics, 2007, 66:201-212 doi: 10.1007/978-3-642-14743-2_18
[7] Di Kaichang, Wan Wenhui, Liu Zhaoqing. New Visual Odometry Method for Planetary Rover Localization[J].Journal of Remote Sensing, 2013, 17(1):46-61 http://www.oalib.com/paper/1470223
[8] Cheng Y, Maimone M W, Matthies L H. Visual Odometry on the Mars Exploration Rovers-A Tool to Ensure Accurate Driving and Science Imaging[J]. IEEE Robotics and Automation Magzine, 2006, 13(2):54-62 doi: 10.1109/MRA.2006.1638016
[9] Grotzinger J, Crisp J, Vasavada A R, et al. Mars Science Laboratory Mission and Science Investigation[J]. Space Science Reviews, 2012, 170(1-4):5-56 doi: 10.1007/s11214-012-9892-2
[10] Liu Z, Di K, Peng M, et al. High Precision Landing Site Mapping and Rover Localization for Change-3 mission[J]. Science China-Physics Mechanics & Astronomy, 2015, 58(1):1-11 https://www.researchgate.net/publication/293184240_High_precision_landing_site_mapping_and_rover_localization_for_Chang'E-3_mission
[11] Kelly J, Sukhatme G S. Fast Relative Pose Calibration for Visual and Inertial Sensors[C]. International Symposium on Experimental Robotics, Berlin, Germany, 2009 http://www.springerlink.com/content/5v467u1790128793
[12] Corke P, Lobo J, Dias J. An Introduction to Inertial and Visual Sensing[J]. International Journal of Robotics Research, 2007, 26(6):519-535 doi: 10.1177/0278364907079279
[13] Liu C, Zhou F, Sun Y, et al. Stereo-Image Matching Using a Speeded Up Robust Feature Algorithm in an Integrated Vision Navigation System[J]. Journal of Navigation, 2012, 65(4):671-692 doi: 10.1017/S0373463312000264
[14] Carlson N A. Federated Filter for Fault-Tolerant Integrated Navigation Systems[C]. IEEE PLANS, Orlando FL, USA, 1988 http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=195473
[15] Zhou Jiangbin, Yua Jianping, Luo Jianjun, et al. Fully Integrated SINS/GPS Navigation System Based on Federated Filters[J].Journal of System Simulation, 2009, 21(6):1562-1569 https://www.researchgate.net/publication/297166724_Fully_integrated_SINSGPS_navigation_system_based_on_federated_filters
[16] Pei Fujun, Ju Hehua, Cui Pingyuan. Autonomous Navigation Method Based on Federated Kalman Filter for Lunar Rover[J]. Journal of Astronautics, 2009, 30(4):1429-1434 http://en.cnki.com.cn/Article_en/CJFDTOTAL-YHXB200904019.htm
-
期刊类型引用(5)
1. 王乐洋，韩澍豪. 不等式约束下加乘性混合误差模型的简单迭代解法. 武汉大学学报(信息科学版). 2024(06): 996-1004 . 
百度学术
2. 陆荣秀，黄学文，杨辉，张智军. 求解时变非线性不等式的新型动态学习网络. 控制工程. 2022(03): 404-412 . 百度学术
3. 王乐洋，邹传义. PEIV模型参数估计理论及其应用研究进展. 武汉大学学报(信息科学版). 2021(09): 1273-1283+1297 . 百度学术
4. 王乐洋，邹传义，吴璐璐. 不等式约束Partial EIV模型的WHP拟牛顿修正解法及其精度评定的SUT法. 大地测量与地球动力学. 2019(06): 648-653 . 百度学术
5. 张波，潘双彦. 含不等式约束的总体最小二乘算法在矿山测量数据处理中的应用研究. 决策探索(中). 2018(05): 18-19 . 百度学术
其他类型引用(6)
计量
- 文章访问数: 4909
- HTML全文浏览量: 241
- PDF下载量: 477
- 被引次数: 11
