基于视觉的机动目标定位跟踪的滤波算法概要.docx

1、基于视觉的机动目标定位跟踪的滤波算法概要第 27卷第 2期 2006年 2月 仪 器 仪 表 学 报Chinese Jo ur nal o f Scientific I nstr umentVo l. 27N o. 2 F eb. 2006基于视觉的机动目标定位跟踪的滤波算法朱枫 1, 2侯菲莉 1, 2 1(中国科学院沈阳自动化研究所 沈阳 110016 2(中国科学院研究生院 北京 100080摘要 在视觉定位系统中 , 由于各种噪声的影响 , 运动目标的三维位置和姿态的计算精度受到 一定限制。为了提高运 动物体 的定位跟踪精度 , 提出了一种有效的滤波算法。和已有的方法相比 , 这种算法

2、具有以下两个特 点 :第一 , 不再局限于平 缓运动 的物体 , 它对于未知运动规律的机动目标同样有效 ; 第二 , 由于避免了扩展卡尔曼滤 波器的使用 , 滤波复杂度有所 下降。通过 分析噪声对位姿计算误差的影响 , 建立了一组描述位姿测量值和真实值关系的线性 测量方程。然后 , 分别给出了两种 滤波算 法 :基于有限记忆的检测自适应滤波和基于数值微分模型的卡尔曼滤波。在检测自适 应滤波算法中 , 给出了分别适用 于快机 动和慢机动的最优机动检测函 数。 一旦检测出机动发生 , 系统 采用有限记忆滤波进行矫正。 在第二种滤波算法中 , 系统采用数 值微分技术构造出了描述机动 目标运动行为的鲁

3、棒估计模型。 并且 , 引入了衰减因子 , 以防止滤波器的发散现象。 该衰减因子 可以根据位姿计算值自适应估计。 最后 , 通过伪贝叶斯估计算法 , 将两种滤波器进行数据融合 , 有效的降低了机动时刻位姿估 值的误差抖动 , 进一步提高了定位跟踪精度。仿真结果验证了本算法的有效性。关键词 滤波算法位姿计算机动检 测数值微分伪贝叶斯估计中图分类号 T P 391. 9 文献标识 码 A 国家标准学 科分类代码 520. 6040Filter Algorithm for Visual Positioning and Tracking of Maneuvering TargetZhu Feng 1,

4、 2 Hou Feili 1, 2 1(Robotics L abor atory , S heny ang I nstitute of A utomation , Chinese A cadem y of Sciences , S heny ang 110016, China 2(Gr aduate School , Chinese A cademy of S ciences , Beij ing 100080, China Abstract It present s a filter algorithm f or accurat ely visual t racking of a mane

5、uvering t arget . Emphasis is given to f ind a solution for the 3D pose (position and orient at ion est imation degradation caused by noise in the images. Superior t o previous approaches t hat were limit ed to t he assumpt ion t hat t he t arget mot ion is slow and smooth , this algorithm is applic

6、able for a maneuvering t arget t hat acts in an unknown manner. M oreover, t he comput at ion load is reduced by avoiding t he need of extended Kalman filtering. First , by analyzing the eff ect of noise in 2D images on 3D pose errors , linear measure equations relating the nominal pose paramet ers

7、(obt ained by 3D reconstruction and t he t rue ones are derived. T hen, tw o filter schemes are int roduced respect ively. T he first filt er uses maneuver det ection t echnique, in which t he criterion of an optimal det ector is deduced, and tw o sub -optimal det ect ors suit able for f ast and slo

8、w maneuvers are given respectively . When a maneuver is declared , limit ed memory filt ering is adopt ed for updat e. In t he second filt er, a robust dynamic model is const ruct ed by t he numerical dif ferent iation technique . T o rest rain t he divergence caused by truncation errors of estimat

9、e model of numerical diff erent iation, a f ading f actor is import ed, w hich can be controlled adapt ively based on t he measurement s. F inally, generalized pseudo Bayes algorit hm is employed t o combine t he tw o f ilt ers for more accurate t racking . Experiment result s illust rate the capaci

10、t y of t his algorit hm .Key words Filt er algorit hm Posit ion and orientation est imation M aneuver det ect ion N umerical differentiat ion Pseudo Bayes algorit hm1引言在基于视觉的运动目标定位跟踪系统中 , 一个重要的问题是对物体的三维位置和姿态的计算。由于各种噪声的影响 (如传感器噪声、 背景噪声和量化噪声等 , 物体的位姿估计值存在很大的误差。由于长图像序列能够提供更多的信息来抑制噪声 , 因此扩展卡尔曼滤波器一直以来被广泛

11、的采用 , 并取得了很好的效果 1-4。 然而 , 在这些算法中 , 物体的运动模型都近似为常 (加 速度的平移和旋转模型 , 从而限制了这些算法 , 使其仅仅适用于目标运动比较平缓的情况。当物体的 运动规律比较复杂 , 运动状态突然改变时 , 这些滤波算 法则不能达到预期的效果。文中在利用单目视觉进行物体位姿计算的基础 上 , 提出了一种有效的滤波算法 , 用于准确估计机动目 标的位姿。 同时 , 根据滤波器提供的位姿预测值和位姿 变化率 , 可以预测下一时刻物体各特征点的像点坐标 的范围 , 从而大大减少了图像处理的时间。中文所提到的算法有以下特点 :(1 和现有算法不同的是 , 将位姿参

12、数的计算值代 替特征点的像点坐标作为测量值 , 建立了线性测量模 型。 一则避免了使用扩展卡尔曼滤波 , 降低了计算复杂 度 ; 二则可以对位姿并行滤波 , 加快了计算速度 , 更好 满足实时性要求。(2 在机动检测自适应滤波算法中 , 针对不同的情 况 , 推导出了分别适用于快机动和慢机动的检测方法 , 并采用有限记忆滤波进行机动矫正。和基于加速度输 入估值的矫正方法 5, 7-8相比 , 有限记忆滤波矫正不需 要估计机动起始时刻 , 滤波精度有所提高。(3 在基于数值微分模型的滤波算法中 , 利用数值 微分技术构造出的运动模型具有较强的鲁棒性。 同时 , 衰减因子的引入防止了数值微分模型的

13、截断误差可能 引起的滤波发散的现象。该衰减因子可以根据位姿的 测量值进行自适应估计。(4 两种滤波器交互输出 , 有效的降低了机动时刻 位姿估值的误差抖动 , 进一步提高了定位跟踪精度。2位姿估计2. 1位姿估计模型 计的方法 , 并给出了位姿的测量模型。如图 1所示 , 分 别在摄像机上和物体上设定摄像机坐标系和物体坐标 系 , 并在物体上固定一组特征点。 摄像机实时摄取物体 的图像 , 通过图像处理 , 得到各特征点的像点。由于特 征点在物体上的位置已知 , 根据基于模型的单目视觉 计算方法 , 即可求出它们在摄像机坐标系下的坐标。 最 后 , 通过几何变换 , 即可得到两坐标系间的旋转矩

14、阵 R (t 和平移矢量 T (t , 进而求得相对位置参数 (相对方 向 x (t , 水平方向 y (t , 垂直方向 z (t 和旋转参数 (横滚角 (t , 俯仰角 (t , 偏航角 (t 。图 1位姿估计模型2. 2测量模型文献 1-4建立的测量模型都是以特征点的像点 坐标为观测值。考虑到位姿参数和像点坐标的非线性 关系 , 直接采用位姿参数为观测值 , 建立观测模型为 : (t (t (t x (t y (t z (t =(t (t (t x (t y (t z (t +n (t n (t n (t n x (t n y (t n z (t (1其中 :n (t , n (t n

15、z (t T 为观测噪声。下面将分析观测噪声的方差特性。 假定观测噪声 服从零均值高斯分布 , 根据特征点像点和目标位姿的 对应关系 , 由各个像点的误差范围 , 可以得到位姿观测 值的最大误差。 根据 3! 原理 , 若某个位姿参数的误差 不超过 L (t , 则可近似认为其方差为 :(L (t /3 2。 根据以上线性测量方程 , 可以采用相同的滤波算 法对各位姿参数同时滤波 , 以更好的满足实时性要求。 下面 , 以 x 为代表 , 叙述整个滤波过程。166仪器仪表学报 第 27卷3机动检测自适应滤波3. 1运动模型建立离散化常加速模型 :X k +1= X k +#k , BU k +

16、W k(2其中 , X k =x (k x (k x (k T , =1T T 2/ 01T 01,B =0, #k , =1发生机动 0没有机动3. 2机动检测3. 2. 1最优检测函数假定系统在 m 时刻发生机动 , 其加速度输入为 U 。 取有偏新息 #i 的加权和作为检测统计量 , 令机动检测 时间为 d , 则检测判别函数为 :L k = di =1b i#k -d +i di =1b 2i !2k -d +i(3其中 :! 2k为新息的方差。 如果 L k T H(T H 为阈值 , 认 为机动发生。下面 , 需要求取加权系数 b i , 使得 L k 检测概率最 大。 令 e k

17、 为无偏新息 , 则 #k 和 e k 的关系为 :#k =Hk -mU +e k 其中 :H k -m =k m C B k =m +1C k -1l =m +1(I -K l C Bk m +1(4由于 e k 为零均值白噪声 , 得到 #k 服从正态分布 :#k N (H k -m U , ! 2k (5根据式 (3 、 (5 , 可以得到 L k 服从正态分布 :L k N (0, 1k mN db iHk -m -d +iU di =1b 2i !2k -d +i, 1k m(6所以 , 要使 k m 后式 (3 的检测概率最大 , 即等价 于 di =1b i H k -m -d

18、+i U di = 1b 2i!2k -d +i最大。令 J k = di =1b iHk -m -d +iU di =1b 2i !2k -d +i(7对 J k 求偏导 , 得到当 b i , 并满足 :b op t i=Hk -m -d +i! -2k -d +i d j =1k -2-d (8此时 , 式 (3 为具有最大检测概率的最优检测函数。3. 2. 2快慢机动的检测函数在实际情况中 , 机动起始时刻 m 是未知的 , 所以 需要针对不同的情况 , 求取合适的次优检测函数。 分析 文献 5, 7-8中提出的基于加速度输入估值的检测函 数 :L k =U kV ar (U k (9

19、其中 :U k 是对 k 时刻的加速度输入估计值 , 且 :U k = di =1b i#k -d +i =di =1H i ! -2k -d +i dj =1Hj!-2k -d +j#k -d +i(10机动检测延时的时间越短 , 式 (9 越接近文中给出 的最优检测函数 , 所以 , 式 (9 是适合快机动的检测函 数。慢机动时 , 机动检测的滞后时间较长。此时 , 可以 近似认为 H k -m -d +1 H k -m 相等 , 又由于 d 不大时 , 亦可近似认为 ! -2k -d +i (i =1 d 也相等 , 因此式 (8 中的加权系数可近似认为 b op ti 1/d , 即有

20、函数 :L k = di =1#k -d +i di =1!2k -d +i(11式 (11 为适合于慢机动的检测函数。 3. 3有限记忆滤波矫正考虑到发生机动后 , 离当前时刻较远的历史数据 意义不大 , 所以在此采用有限记忆滤波法进行机动矫 正。 和文献 5, 7-8中基于加速度输入估值的矫正方法比较 , 该算法不需要估计机动起始时刻 , 避免了由于起 始时刻估计不准确造成的滤波精度下降。滤波矫正如图 2所示 , 如果在 k 时刻检测到机动发生 , 则认为 k -N 时刻以前的量测值不再作为估计 现在状态的依据 (N 为有限记忆长度 , 根据 k -N +1 k 时刻的测量值 , 将状态变

21、量 X k 和误差协方差矩阵P k更新为 :X k 和 P k :图 2有限记忆滤波矫正X k =(P -1k -P -1k k -N -1(P -1k X k -P -1k k -N X k k -N P k =(P -1k -P -1k k -N -1(12167第 2期 基于视觉的机动目标定位跟踪的滤波算法4基于数值微分模型的滤波4. 1运动模型利用数值微分技术13-14, 构造速度的三点微分模型 :v t +T =2T 3v t +T -4v t +v t -T +23v %1v t =2T v t +T -v t -T -26v%2v t -T =2T -v t +T +4v t -

22、3v t -T +23v %3(13其中 :T 为采样周期 , v %1、 v %2、 v %3为截断误差 , %1、 %2、 %3 (t -T , t +T 。 由此 , 构造出运动模型 :Xt = X t +W t(14其中 :X t =x tx t x t -T x t -2, =010003/2T -2/T 1/2T 01/2T0-1/2T 0-1/2T 2/T-3/2T离散化 , 得到 :X k +1= X k +W k(15其中 :X k =x kx kx k -1x k -, =123T /12-4T /35T /03-3110010该模型不需要目标运动行为的先验信息 , 是一种

23、鲁棒 性估计模型。 4. 2滤波发散抑制文献 13-14将微分模型中的截断误差并入系统 噪声 W k 中处理 , 这使得 W k 的方差难以估计。若 ! 2w 取 得过大 , 滤波精度降低 , 反之 , 则容易引起滤波发散。 因 此 , 文中引入衰减因子 S k 。当滤波器发散的时候 , 该衰 , 波器发散的效果。发散抑制的方法如下 :若 #T k #k tr(E (#k #Tk , 认为滤波器发散。将预测误差协方差阵更新如下 :P k k -1=S k P k -1 +Q k -1(16其中 :S k 根据当前的新息 , 由闭环回路自动选择 :S k =tr#k #Tk -CQ k -1C

24、T-R k C P k -1 TC T-1(175数据融合为了进一步提高滤波精度 , 将以上两个模型的滤 波器同时工作 , 并根据贝叶斯推理计算出各模型的权 值 , 系统的输出为两滤波器输出值的加权和。假定模型之间的转移服从状态有限、 时间离散的 时齐马尔可夫链。 令 M i (k 表示在 k -1, k , 滤波周期内 i 模型与系统运动模式匹配 , X i (k k 表示 k 时刻第 i 个滤波器的输出 , Z k为到 k 时刻为止累积的测量值 , &i (k 为 k 时刻模型 i 的概率权值 , i (k 为第 i 个模 型的似然函数。 融合步骤为 :(1 并行滤波 :将 k -1时刻的

25、 X i (k -1 k -1 和 P i (k -1 k -1 输入第 i 个滤波器中 , 得到预测值 X i (k k -1 和 P i (k k -1 。 再根据测量值 , 得到各个 滤波器的输出 X i (k k 和 P i (k k 。(2 计算各模型的似然函数 : i (k =p (z k M i (k Zk -1=2( P i (k k -1 -1/2exp -2#i (k P -1i (k k -1 #i (k (18(3 , 更新概率权值 : &i (k =p (z k M i (k Z k -1p (M i (k Z k -1 jp (z k M j (k Z k -1 p

26、 (M j (k Z k -1= i (k j j (k (kH j k &i (k -1 k H ik &k -1i (19 (4 由全概率公式 , 得到交互输出值 :X (k k =&1(k X 1(k k +&2(k X 2(k k (206实验结果整个滤波算法流程如图 3所示 , 通过计算机仿真 , 来验证本算法的有效性。先设定一组理想的运动目标 相对于摄像机的位姿变化曲线 , 得到各个时刻特征点 的像点坐标。 将各像点加上高斯白噪声 , 然后通过基于 模型的单目视觉计算方法 , 得到带噪声的位姿参数的 , 168仪器仪表学报 第 27卷数的误差均方根曲线。选取 x 方向位移和横滚角

27、的 滤波结果为代表 , 如以下各图所示。图 3滤波算法流程图图 4和图 5表示设定的 x 和 的理想运动曲线。 x 在 t =060s 加速度为 a =0. 0461m /s 2; t =60s 时 , 加 速度发生变化 , 变为 a =-0. 0395m/s 2; 当 t 90s 时 , x的加速度为零 , 横滚角 则以加速度 a =0. 873mrad/s 2作 匀 加 速 运 动 ; 当 t 50s 时 , 加 速 度 变 为 a =0. 140mrad/s 2 。图 4 x 运动规律图 6是 x 误差均方根曲线。 L M 为基于有限记忆 的机动检测自适应滤波的结果 , ND 为基于数值

28、微分 模型的卡尔曼滤波结果 , GPB 为融合以上两种滤波算 法的最终滤波结果。 虚线为未滤波以前的误差均方根。 从图中可以看出 , 分别采用文中给出的两种滤波器 , 都 能实现很好的位置估计。而通过数据融合将两滤波器 , 性能进一步提高。图 5 运动规律图 6 x 误差均方根图 7 x 速度跟踪曲线图 7为 x 的速度跟踪曲线 (实线为真实值 , 虚线为 滤波值 , 图 8表示速度的误差均方根值。 可以看出 , 该 滤波器对速度也实现了很好的估计。图 8 x 速度误差均方根图 9 误差均方根图 9、 图 10和图 11给出了对横滚角 进行滤波的 结果。图 9为 的误差均方根曲线 , 图 10

29、为 的速度 跟踪曲线 , 图 11为对应的速度误差均方根。 可见 , 算法 对姿态角的定位跟踪也取得了良好的效果。图 10 速度跟踪曲线图 11 速度误差均方根169第 2期 基于视觉的机动目标定位跟踪的滤波算法170 仪器仪表学报 第 2 7 卷 4 BRO IDA T J , fro m a CHA N DRA SHEK HA R imag e sequence. S, IEEE 通过机器人跟踪实验, 也验证了算法的有效性。 图 12 为 1 号机器人跟踪 2 号机器人的实现过程。1 号机 器人上 的摄像机实 时拍摄 2 号 机器人 的图像, 再 由图 像处理器检测出安装在 2 号机器人上

30、的各特征点的像 点。通过视觉计算, 得到两机器人的相对位姿测量值, 并将其输入滤波器中。滤波器的输出反馈给 1 号机器 人的 控制器, 实现 对 2 号机 器人的实 时跟踪 控制。同 时, 通过 位姿预测值 得到的特征 点坐标 的预测值 反馈 给图像处理器, 简化了特征点的检测过程, 加快了图像 处理的速度。 CHEL L A PPA R . R ecur sive 3- mot ion estimatio n D mono cular T r ansactions o n A ero space and Electr onic Systems , 1990, 26( 4 : 639-656. 5