第17卷 智能系统学报 ·922· 3机器人单目视觉SLAM 3)目标状态、协方差更新 根据扩展式卡尔曼滤波原理，更新后的目标 采用文献[9]提出的反转深度单目视觉 状态X和协方差阵P分别为 SLAM方法，基本处理流程图3所示。 X=X+K(-) (8) EKF预测观测值数 EKF更新 m环境特 P=P -K.Sk.K' (9) 据关联 征管理 K=P%·H2.(S) (10) ® 其中，K为卡尔曼增益阵，一为式(6)对X的雅 可比阵。 图3 VSLAM处理流程 Fig.3 Processing flow of VSLAM 5基于目标协方差阵更新最大化的 系统状态为m=[],和M为机器 机器人优化控制方法 人和环境特征状态向量，初始阶段利用反转深度 参数法表示环境特征状态，当特征状态线性化程 机器人控制要达到两个目的：首先，要确保机 度达到预设门限时，在环境特征管理环节将特征 器人对目标的追踪，使目标始终在机器人视野内 的反转深度状态转换为笛卡尔状态。在每轮估计 并保持一定距离；第二，保证机器人能够在纯方 结束时，提取Xm和Xm中的机器人状态和协方差 位角观测条件下对目标状态的准确估计。优化控 分量得到X和X用于后续目标状态估计。 制量生成过程如图4示。 4单目视觉移动机器人目标跟踪方法 机器人估计 机器人控制量生成 状态及方差 X、PH ! 机器人角度控制量生成△p: 目标跟踪采用扩展式卡尔曼滤波框架，假设 目标估计状 k时刻机器人的状态X和协方差P由第3节介绍 机器人运动 态及方羞 跟随控制分量 状态更新 方法获得。目标跟踪的处理过程如下。 X、P% △x 机器人 控制量 1)目标状态、协方差预测 目标观测值 可观性控制分量 生成 目标状态预测公式如下： △x X=A-1X-1 (4) 图4优化控制量生成过程 P%=A-1X-A{-1+0 (5) Fig.4 Generation process of optimal control value 式中：A“-1为目标定速模型状态转移矩阵；Q为状 机器人控制量包括角度控制3元组△P:和位 态误差阵。 置控制3元组△x,首先，产生角度控制量△p,该 2)目标观测值及观测残差阵预测 控制分量要保证机器人始终朝向目标。之后，产 将机器人状态和预测目标状态代入式 生位移控制量△xk,△x由跟随控制分量△x{和可观 (3)得到目标预测观测值： 性控制分量△r组成。 z或=h'(X,,X,d,s) (6) 5.1角度控制△p计算 式中：X为式(4)确定的预测目标状态；为利用 假设k时刻，目标观测值为z=[y,根据摄 单目视觉SLAM环节得到的机器人位姿状态。 像机小孔成像模型可知： 利用误差传播公式链式产生目标观测残差阵 xCt ('-o)/s S如下： -Vo)/s (11) S%=H2.P·H-2'+H-.P·H-'+H-2.P (7) f-'+-2.P,-'+H-.PH-r+R f+1 RC→R.xC,- 其中， (12) zRr-t =oh 2= oh 式中：x为摄像机坐标系中，成像焦点指向目标 0XR -x 的向量；o、o、Sw、S为摄像机内参；RCR为机器人 Ha-2=h ,H2= oh 坐标系中摄像机相对于机器人的旋转矩阵。 as s-s 假设k时刻，机器人角度控制量为△p:=[中0， 分别为式(3)对、X、、d、s的雅可比 分量表示机器人坐标系中绕X轴（旋转roll),Y 阵。P、PE、PE、P、P,R分别为此刻X、X、X、 轴（俯仰pitch)和Z轴（偏航yaw)的角度增量。△p d、s、r对应的协方差阵。 的计算公式为3 机器人单目视觉 SLAM 采用文 献 [ 9 ] 提出的反转深度单目视 觉 SLAM 方法，基本处理流程图 3 所示。 EKF 预测 观测值数 EKF 更新 据关联 zk LM Xk slam− Pk slam− Xk slam Pk slam 环境特 征管理 图 3 VSLAM 处理流程 Fig. 3 Processing flow of VSLAM X slam′ k = [X r ′ k X LM′ k ] X r k X LM k X slam k X slam k X r k X r k 系统状态为 ， 和 为机器 人和环境特征状态向量，初始阶段利用反转深度 参数法表示环境特征状态，当特征状态线性化程 度达到预设门限时，在环境特征管理环节将特征 的反转深度状态转换为笛卡尔状态。在每轮估计 结束时，提取 和 中的机器人状态和协方差 分量得到 和 用于后续目标状态估计。 4 单目视觉移动机器人目标跟踪方法 k X r k P r k 目标跟踪采用扩展式卡尔曼滤波框架，假设 时刻机器人的状态 和协方差 由第 3 节介绍 方法获得。目标跟踪的处理过程如下。 1）目标状态、协方差预测 目标状态预测公式如下： X t − k = A t k|k−1 · X t k−1 (4) P t − k = A t k|k−1 · X t k−1 · A t ′ k|k−1 +Q (5) A t 式中： k|k−1为目标定速模型状态转移矩阵； Q 为状 态误差阵。 2）目标观测值及观测残差阵预测 X r k X t − 将机器人状态 和预测目标状态 k 代入式 （3）得到目标预测观测值： z t − k = h t (X t − k ,X r k ,X R,c k , d,s) (6) X t − k X r 式中： 为式（4）确定的预测目标状态； k为利用 单目视觉 SLAM 环节得到的机器人位姿状态。 S t − k 利用误差传播公式链式产生目标观测残差阵 如下： S t − k = H t→z · P t − k · H t→z′ + H r→z · P r k · H r→z′ + H c→z · P c k · H c→z′ + H s→z · P S k · H s→z′ + H d→z · P d k · H d→z ′ + R (7) 其中， H r→z = ∂h t ∂X r k       X r k =X r ∗ k , H t→z = ∂h t ∂X t k       X t k =X t− k , H c→z = ∂h t ∂X R,c k       X R,c k =X R,c ∗ k H d→z = ∂h t ∂d       d=d * , H s→z = ∂h t ∂S       S=S ∗ X r k X t k X R,c k d s P t − k P r k P c k P S k P d k R X r k X t k X R,c k d s r 分别为式（ 3）对 、 、 、 、 的雅可比 阵。 、 、 、 、 ， 分别为此刻 、 、 、 、 、 对应的协方差阵。 3）目标状态、协方差更新 X t + k P t + k 根据扩展式卡尔曼滤波原理，更新后的目标 状态 和协方差阵 分别为 X t + k = X t − k + K ·(z t k − z t − k ) (8) P t + k = P t − k − K ·S t k · K ′ (9) K = P t − k · H t→z k ·(S t − k ) (10) K Ht→z k X t 其中， 为卡尔曼增益阵， 为式（6）对 k的雅 可比阵。 5 基于目标协方差阵更新最大化的 机器人优化控制方法 机器人控制要达到两个目的：首先，要确保机 器人对目标的追踪，使目标始终在机器人视野内 并保持一定距离；第二，保证机器人能够在纯方 位角观测条件下对目标状态的准确估计。优化控 制量生成过程如图 4 示。 可观性控制分量 ∆x a k 机器人角度控制量生成 ∆φk 跟随控制分量 ∆x f k 机器人 控制量 生成 ∆xk 机器人运动 状态更新 机器人估计 状态及方差 目标估计状 态及方差 目标观测值 X r k、P r k Xt k、P t k 机器人控制量生成 z t k 图 4 优化控制量生成过程 Fig. 4 Generation process of optimal control value ∆φk ∆xk ∆φk ∆xk ∆xk ∆x f k ∆x a k 机器人控制量包括角度控制 3 元组 和位 置控制 3 元组 ，首先，产生角度控制量 ，该 控制分量要保证机器人始终朝向目标。之后，产 生位移控制量 ， 由跟随控制分量 和可观 性控制分量 组成。 5.1 角度控制 ∆φk计算 z t k = [u t v t ] 假设 ′ k 时刻，目标观测值为 ，根据摄 像机小孔成像模型可知： x C,r→t =   x C,r→t y C,r→t z C,r→t   =   (u t −u0)/su (v t −v0)/sv 1   (11) u R,r→t =   x R,r→t y R,r→t z R,r→t   = R C→R · x C,r→t (12) x C,r→t u0 v0 su sv R C→R 式中： 为摄像机坐标系中，成像焦点指向目标 的向量； 、 、 、 为摄像机内参； 为机器人 坐标系中摄像机相对于机器人的旋转矩阵。 ∆φk = [ϕ θ ψ] ′ ∆φk 假设 k 时刻，机器人角度控制量为 ， 分量表示机器人坐标系中绕 X 轴（旋转 roll），Y 轴（俯仰 pitch）和 Z 轴（偏航 yaw）的角度增量。 的计算公式为 第 17 卷 智 能 系 统 学 报 ·922·