·921· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第5期 [为机器人状态和环境特征状态扩展构成 0 -△ -△ -△ 的SLAM系统状态向量。系统的两条主线为 △ 0 △W -△ 2(△p) Xam估计流程和X估计流程，Xm的估计采用基于 △ -△ 0 △ 反转深度的单目视觉SLAM方法。X估计过程 △ △6 -△ 0 基于扩展式卡尔曼滤波，首先，根据目标运动模 3)目标观测模型 型预测目标状态和协方差阵P,第二，利用 目标观测模型实现从机器人状态X:、目标状 SLAM环节得到的机器人状态X和协方差P,结 态X,摄像机在机器人坐标系中的位姿X到图像 合目标观测模型得到目标预测观测值和观测残 观测值=[4的映射，其符合单目小孔成像模 差阵S,最后，对预测状态X和协方差阵P进行 型，可表示为 更新，得到X和P。 z=h(Xi.Xi,Xge,d.s)+r (3) 式中：s=[sSoo为摄像机内参向量，d=[dd] 为摄像机畸变系数向量，s和d由文献[16]介绍的 相机标定方法获得。r是均值为0，标准差为σr的 观测加性白噪声。 2系统总体框架 XXXXi-X 系统总体框架如图2所示。 图像目标 单日视觉 图1贝叶斯网络模型 识别 OT Fig.1 Bayesian network model 系统解决的关键问题有两点：1)设计，X及 XI P X,i=1,2…,m在线估计方法；2)如何生成机器人 机器人控制量 优化控制量，这两点将在第4和第5部分讨论。 生成近 XP 1.2数学模型构建 里程表 1)目标运动模型 目标k时刻状态由各坐标轴分量及其速度构 图像环境特 单日视觉 成，即，X=[:]。假设目标运动模式 征点提取 SLAM 为定速度模型(constant velocity model,.CVM)2 图2系统总体框架 则CVM模型的离散形式可表示成为 Fig.2 Overall framework of the system X=f(X-1Aw-1,9-1)=Aw-1X-1+-1 (1) 机器人外感传感器是前向单目摄像机，每轮 式中：A-1为状态转移阵；q-为高斯白噪声。 观测对拍摄的图片进行环境特征提取以及目标识 2)机器人运动模型 别进而获得目标观测值与环境特征观测值“。 机器人运动模型符合运动方向和线速度解耦的 系统包含3个核心处理模块：I)单目SLAM处理 多旋翼无人机模型，设k时刻状态为X=[xq其中 模块；2)单目视觉OT模块；3)机器人控制量生成 x=[]'为空间位置向量，=[q1kq2q3sq4 模块。其中，单目SLAM处理模块采用基于反转 为朝向4元组向量。 深度参数表示法的全概率卡尔曼滤波方法实现机 假设k时刻机器人的控制量为=[△x△p], 器人状态和协方差阵的估计；单目OT模块利用 其中△r=[△x△yW△z为位移控制分量，△p= 和P结合此时目标观测值z进行基于扩展式卡 [△△△灯为角度控制分量，则机器人的状态 尔曼滤波的目标跟踪得到目标状态！和协方差阵 更新可分解位置和朝向更新，即： P;由于单目视觉传感器缺少深度观测值使目标 x-1+R(q-)小△r- X=fX-,-)= 状态估计出现可观性问题，即，目标状态无法利 1+22△pi小乐-1 2 用机器人状态和观测值直接推导出，因此在机器 其中，R(q)是世界坐标系到机器人坐标系的旋转 人控制量生成环节，需要利用优化控制方法生成 矩阵，其值可根据q计算，2（△p)为朝向3元组到 机器人控制量，以保证机器人对目标的跟随和 4元组的反对称矩阵，表示为 目标状态的有效估计。[ X r ′ k X LM′ k ]′ X slam k X t k X slam k X t k X t − k P t − k X r k P r k z t − k S t − k X t − k P t − k X t k P t k 为机器人状态和环境特征状态扩展构成 的 SLAM 系统状态向量。系统的两条主线为 估计流程和 估计流程， 的估计采用基于 反转深度的单目视觉 SLAM 方法[12]。 估计过程 基于扩展式卡尔曼滤波，首先，根据目标运动模 型预测目标状态 和协方差阵 ，第二，利用 SLAM 环节得到的机器人状态 和协方差 ，结 合目标观测模型得到目标预测观测值 和观测残 差阵 ，最后，对预测状态 和协方差阵 进行 更新，得到 和 。 Xslam k−2 Xk−1 k+1 k+2 slam Xk slam Xslam Xslam Xt k−2 Xt k−1 Xt k Xt k+1 Xt k+2 z t k u r k−1 u r k u r k+1 u r k+2 z LM k−1 zk LM z LM k+1 z LM k+2 z t k−1 z t k z t k+1 z t k+2 z LM k−2 … … 图 1 贝叶斯网络模型 Fig. 1 Bayesian network model X r k X t k X lmi k ,i = 1,2,··· ,m u r k 系统解决的关键问题有两点：1) 设计 ， 及 在线估计方法；2) 如何生成机器人 优化控制量 ，这两点将在第 4 和第 5 部分讨论。 1.2 数学模型构建 1）目标运动模型 k X t k = [x t k y t k z t k x˙ t k y˙ t k z˙ t k ] ′ 目标 时刻状态由各坐标轴分量及其速度构 成，即， 。假设目标运动模式 为定速度模型 (constant velocity model, CVM)[24] ， 则 CVM 模型的离散形式可表示成为 X t k = f t (X t k−1 , A t k|k−1 , q t k|k−1 ) = A t k|k−1 · X t k−1 + q t k|k−1 (1) A t k|k−1 q t 式中： 为状态转移阵； k|k−1为高斯白噪声。 2）机器人运动模型 X r ′ k = [x r ′ k q r ′ k ] x r k = [x r k y r k z r k ] ′ q r k = [q1,k q2,k q3,k q4,k] ′ 机器人运动模型符合运动方向和线速度解耦的 多旋翼无人机模型，设 k 时刻状态为 其中 为空间位置向量， 为朝向 4 元组向量。 u r k = [∆x r k ∆φ r k ] ′ ∆x r k = [∆x r k ∆y r k ∆z r k ] ′ ∆φ r k = [∆ϕ r k ∆θ r k ∆ψ r k ] ′ 假设 k 时刻机器人的控制量为 ， 其 中 为位移控制分量， 为角度控制分量，则机器人的状态 更新可分解位置和朝向更新，即： X r k = f(X r k−1 ,u r k−1 ) =   x r k−1 + R(q r k−1 )·∆x r k−1 q r k−1 + 1 2 ·Ω(∆φ r k−1 )· q r k−1   (2) R(q r ) q r Ω(∆φ r ) 其中， 是世界坐标系到机器人坐标系的旋转 矩阵，其值可根据 计算， 为朝向 3 元组到 4 元组的反对称矩阵，表示为 Ω(∆φ r )   0 −∆ϕ r k −∆θ r k −∆ψ r k ∆ϕ r k 0 ∆ψ r k −∆θ r k ∆θ r k −∆ψ r k 0 ∆ϕ r k ∆ψ r k ∆θ r k −∆ϕ r k 0   3）目标观测模型 X r k X t k X R,c k z t k= [u t k v t k ] ′ 目标观测模型实现从机器人状态 、目标状 态 ，摄像机在机器人坐标系中的位姿 到图像 观测值 的映射，其符合单目小孔成像模 型 [25] ，可表示为 z t k = h t (X t k ,X r k ,X R,c k , d,s)+ r (3) s = [su sv u0 v0] ′ d = [d1 d2] ′ s d r σr 式中： 为摄像机内参向量， 为摄像机畸变系数向量， 和 由文献 [16] 介绍的 相机标定方法获得。 是均值为 0，标准差为 的 观测加性白噪声。 2 系统总体框架 系统总体框架如图 2 所示。 图像环境特 征点提取 图像目标 识别 zk LM z t k 单目视觉 SLAM 单目视觉 OT 机器人控制量 生成 u r k 里程表 X r k P r k Xt k P t k 图 2 系统总体框架 Fig. 2 Overall framework of the system z t k z LM k X r k P r k z t k X t k P t k u r k 机器人外感传感器是前向单目摄像机，每轮 观测对拍摄的图片进行环境特征提取以及目标识 别进而获得目标观测值 与环境特征观测值 。 系统包含 3 个核心处理模块：1) 单目 SLAM 处理 模块；2) 单目视觉 OT 模块；3) 机器人控制量生成 模块。其中，单目 SLAM 处理模块采用基于反转 深度参数表示法的全概率卡尔曼滤波方法实现机 器人状态和协方差阵的估计；单目 OT 模块利用 和 结合此时目标观测值 进行基于扩展式卡 尔曼滤波的目标跟踪得到目标状态 和协方差阵 ；由于单目视觉传感器缺少深度观测值使目标 状态估计出现可观性问题，即，目标状态无法利 用机器人状态和观测值直接推导出，因此在机器 人控制量生成环节，需要利用优化控制方法生成 机器人控制量 ，以保证机器人对目标的跟随和 目标状态的有效估计。 ·921· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第 5 期