【机器学习】纯方位角目标跟踪及移动平台可观性控制方法

第17卷第5期 智能系统学报 Vol.17 No.5 2022年9月 CAAI Transactions on Intelligent Systems Sep.2022 D0:10.11992/tis.202107066 网络出版地址：https:/ns.cnki.net/kcms/detail/23.1538.TP.20220729.1154.002.html 纯方位角目标跟踪及移动平台可观性控制方法 伍明，李广宇2，魏振华，汪洪桥 (1.西安高技术研究所信息系统室，陕西西安710025：2.陕西广播电视大学教学科，陕西西安710023) 摘要：为了解决未知环境下的单目视觉移动机器人目标跟踪问题，提出了一种将目标状态估计与机器人可观 性控制相结合的机器人同时定位、地图构建与目标跟踪方法。在状态估计方面，以机器人单目视觉同时定位与 地图构建为基础，设计了扩展式卡尔曼滤波框架下的目标跟踪算法：在机器人可观性控制方面，设计了基于目 标协方差阵更新最大化的优化控制方法。该方法能够实现机器人在单目视觉条件下对自身状态、环境状态、 目标状态的同步估计以及目标跟随。仿真和原型样机实验验证了目标状态估计和机器人控制之间的耦合关 系，证明了方法的准确性和有效性，结果表明：机器人将产生螺旋状机动运动轨迹，同时，目标跟踪和机器人定 位精度与机器人机动能力成正比例关系。 关键词：目标跟踪；纯方位角观测：机器人同时定位与地图构建；机器人导航控制：单目视觉导航；可观性控制： 自主移动平台；单目视觉 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2022)05-0919-12 中文引用格式：伍明，李广宇，魏振华，等.纯方位角目标跟踪及移动平台可观性控制方法.智能系统学报，2022,17（⑤）： 919-930 英文引用格式：WU Ming,LI Guangyu,WEI Zhenhua,,et al Bearing only target tracking and observability control of a mobile ro- bot[J].CAAI transactions on intelligent systems,2022,17(5):919-930. Bearing only target tracking and observability control of a mobile robot WU Ming',LI Guangyu',WEI Zhenhua',WANG Hongqiao' (1.Information System Office,Xi'an High Technology Research Institute,Xi'an 710025,China;2.Teaching Department,Shaanxi Radio and TV University,Xi'an 710023,China) Abstract:To address the object tracking issue of a mobile robot based on monocular vision in an unknown environment, a method for robotic simultaneous localization,map building,and object tracking is proposed,combining the object state estimation with the observability control of a mobile robot.Regarding state estimation,based on robot monocular visual simultaneous localization and mapping,a target tracking algorithm under the extended Kalman filtering framework is designed.Considering the observability control,an optimal control method based on updating the maximization of the target covariance matrix is designed.Using the monocular vision,this method can synchronously estimate the robot's own state,environmental state,and target state,as well as the target following.The simulation and prototype experi- ments verify the coupling relationship between the target state estimation and robot control,demonstrating the effective- ness and accuracy of the method.The results indicate that by employing this method,the robot can generate a spiral maneuvering trajectory,and the accuracy of target tracking and robot positioning is directly proportional to its maneuv- ering ability. Keywords:object tracking;bearing only observation;SLAM;robot navigation control;monocular visual navigation; observability control;autonomous mobile platform;monocular vision 收稿日期：2021-07-12.网络出版日期：2022-08-01 智能移动机器人是一类能够通过传感器感知 基金项目：国家自然科学基金项目(61503389)；陕西省自然科 学基金项目(2015JM6313,2020JM358). 环境和自身状态，实现在有障碍物的环境中面向 通信作者：魏振华.E-mail:weizhenhua8303@I63.com 目标的自主运动，从而完成一定作业功能的机器

DOI: 10.11992/tis.202107066 网络出版地址: https://kns.cnki.net/kcms/detail/23.1538.TP.20220729.1154.002.html 纯方位角目标跟踪及移动平台可观性控制方法 伍明1 ，李广宇2 ，魏振华1 ，汪洪桥1 （1. 西安高技术研究所 信息系统室，陕西 西安 710025; 2. 陕西广播电视大学 教学科，陕西 西安 710023） 摘 要：为了解决未知环境下的单目视觉移动机器人目标跟踪问题，提出了一种将目标状态估计与机器人可观 性控制相结合的机器人同时定位、地图构建与目标跟踪方法。在状态估计方面，以机器人单目视觉同时定位与 地图构建为基础，设计了扩展式卡尔曼滤波框架下的目标跟踪算法；在机器人可观性控制方面，设计了基于目 标协方差阵更新最大化的优化控制方法。该方法能够实现机器人在单目视觉条件下对自身状态、环境状态、 目标状态的同步估计以及目标跟随。仿真和原型样机实验验证了目标状态估计和机器人控制之间的耦合关 系，证明了方法的准确性和有效性，结果表明：机器人将产生螺旋状机动运动轨迹，同时，目标跟踪和机器人定 位精度与机器人机动能力成正比例关系。 关键词：目标跟踪；纯方位角观测；机器人同时定位与地图构建；机器人导航控制；单目视觉导航；可观性控制； 自主移动平台；单目视觉 中图分类号：TP242.6 文献标志码：A 文章编号：1673−4785(2022)05−0919−12 中文引用格式：伍明, 李广宇, 魏振华, 等. 纯方位角目标跟踪及移动平台可观性控制方法 [J]. 智能系统学报, 2022, 17(5): 919–930. 英文引用格式：WU Ming, LI Guangyu, WEI Zhenhua, et al. Bearing only target tracking and observability control of a mobile ro￾bot[J]. CAAI transactions on intelligent systems, 2022, 17(5): 919–930. Bearing only target tracking and observability control of a mobile robot WU Ming1 ，LI Guangyu2 ，WEI Zhenhua1 ，WANG Hongqiao1 (1. Information System Office, Xi’an High Technology Research Institute, Xi’an 710025, China; 2. Teaching Department, Shaanxi Radio and TV University, Xi’an 710023, China) Abstract: To address the object tracking issue of a mobile robot based on monocular vision in an unknown environment, a method for robotic simultaneous localization, map building, and object tracking is proposed, combining the object state estimation with the observability control of a mobile robot. Regarding state estimation, based on robot monocular visual simultaneous localization and mapping, a target tracking algorithm under the extended Kalman filtering framework is designed. Considering the observability control, an optimal control method based on updating the maximization of the target covariance matrix is designed. Using the monocular vision, this method can synchronously estimate the robot’s own state, environmental state, and target state, as well as the target following. The simulation and prototype experi￾ments verify the coupling relationship between the target state estimation and robot control, demonstrating the effective￾ness and accuracy of the method. The results indicate that by employing this method, the robot can generate a spiral maneuvering trajectory, and the accuracy of target tracking and robot positioning is directly proportional to its maneuv￾ering ability. Keywords: object tracking; bearing only observation; SLAM; robot navigation control; monocular visual navigation; observability control; autonomous mobile platform; monocular vision 智能移动机器人是一类能够通过传感器感知 环境和自身状态，实现在有障碍物的环境中面向 目标的自主运动，从而完成一定作业功能的机器 收稿日期：2021−07−12. 网络出版日期：2022−08−01. 基金项目：国家自然科学基金项目（61503389）；陕西省自然科 学基金项目（2015JM6313,2020JM358）. 通信作者：魏振华. E-mail: weizhenhua8303@163.com. 第 17 卷第 5 期 智 能 系 统 学 报 Vol.17 No.5 2022 年 9 月 CAAI Transactions on Intelligent Systems Sep. 2022

第17卷 智能系统学报 ·920· 人系统川。从该定义可见，机器人对自身和环境 构建的多线程估计方法。计算机视觉领域的 对象（包括：静态对象和动态对象）的感知在任务 SLAMOT以基于单目视觉的目标跟踪为核心，又 执行中起到关键性和基础性作用。 称为：纯方位角观测目标跟踪(bearing only object 单目视觉传感器作为一种被动式传感器，凭 tracking)或目标运动分析(target motion analysis), 借其体积小、耗能少、细节呈现度高等特点，正在 此类方法希望利用平台机动和目标纯方位角观 机器人导航学中发挥着重要作用。机器人和计 测，实现运动目标的状态估计，通常假设移动平台 算机视觉领域的相关学者已经展开了基于单目视 状态已知。Oh等20提出了一种基于粒子群优化 觉的机器人同时定位、地图构建与目标跟踪(simul- 的解决方法，但未考虑平台的机动优化问题。Zhuk taneous localization,mapping and object tracking, 等)通过对比相邻帧中跟踪点实现运动物体检 SLAMOT)问题研究。机器人领域的SLAMOT 测，但并未实现目标状态估计。Watanabe等提 以SLAM为核心，目标识别与跟踪目的是提供 出了基于单目视觉的移动平台导航方法，该方法 更好的环境特征观测值，通常假设目标为静止的 针对单目视觉传感器特点将导航控制和目标状态 结构性物体且不考虑平台可观性机动问题，Cast- 估计作为耦合问题处理，其假设移动平台状态已 ler等I在PTAM(parallel tracking and mapping)方 知。Kim等21针对纯方位角观测目标跟踪问题 法II基础上提出了独立于摄像机SFM(structure 提出了“双重作用(dual effect)理论”认为由于观测 from motion)的目标识别与跟踪方法，该方法在图 深度信息缺失使得目标状态估计和平台机动运动 像关键帧上通过特征点匹配识别环境目标并利用 之间存在关联作用，目标跟踪准确性极大依赖于 扎集优化方法(bundle adjustment,BA)实现对目标 平台机动方式，同样，其假设平台状态已知。 空间位置的估计，但其假设目标为静止的画像平 综上所述，学界已经开展了基于单目视觉的 面。De等例将实际场景中运动物体融入地图构 SLAMOT问题的相关研究，但通常将SLAM和 建以解决增强现实中虚拟物体投影参考缺失问 OT相互割裂，并未考虑目标的移动性、机器人状 题。Dai等o利用RGBD传感器解决了动态物体 态未知性以及平台的可观性优化控制问题。文章 对SLAM的影响，但并未研究动态物体跟踪问题。 正是针对以上问题展开研究，提出了基于单目视 Lⅰ等山研究了基于运动目标跟踪和静止物体识 觉的机器人同时定位、地图构建与目标跟踪及平 别的语义学SLAM方法，运功目标跟踪目的同样 台控制方法，该方法利用单目视觉SLAM实现对 是为了减少对SLAM的干扰。Civera等I将目标 机器人状态估计，并结合目标观测模型进行基于 识别与跟踪纳入到基于反转深度的全关联卡尔曼 扩展式卡尔曼滤波的目标状态估计。为了克服观 滤波框架中，实现了环境物体的识别和空间位 测模型缺陷，提出了基于目标协方差阵更新最大 置的在线估计。同样，该方法假设目标为静止的 化的机器人优化控制方法，最终实现了未知环境 平面物体。Wangsiripitak等设计了单目SLAM 下基于单目视觉的移动机器人目标跟踪。 与目标跟踪同步估计算法，该算法处理的是空间 1问题描述和模型构建 立体静止物体并未考虑目标运动问题。Migliore 等设计了一种单目SLAM和动态目标跟踪算 1.1 问题描述 法，该算法将传统的SLAM滤波器和纯方位角观 假设k时刻机器人状态为X,环境特征状态为 测目标跟踪器相结合，实现了对摄像机、环境特 X,i=1,2,…,m,目标状态为X,机器人利用单目 征和动态目标状态的同时迭代估计，方法利用的 视觉传感器对目标和环境特征进行观测且观测值 平台为手持摄像机，因此并未考虑平台可观性机 分别为和z,方法处理目的是在机器人端实现 动对目标状态估计的影响。 对X,及X,i=1,2,…,m的在线估计。由于单目 Bescos等I6设计了一种SLAM和多目标跟 摄像机得到的观测量缺少距离信息，存在可观性 踪相结合的方法，验证了动态目标跟踪对SLAM 问题。为了估计目标与机器人的相对距离，机器 的辅助作用。Liu等m提出了SLAM和目标跟踪 人需要根据目标不确定分布实时计算控制量， 强、弱耦合估计切换方法，有效结合了两者的有 以保证在该控制量作用下得到的前后时序目标观 点。Sualehm等)采用多传感器融合方法实现动 测值-，和之间存在足够视差，从而为距离估计 态环境下SLAM任务，能够完成对多个运动目标 提供足够的信息。 的跟踪。Liu等9研究了人造动态环境下的SLAM 相关对象状态演变符合马尔可夫过程，其对 问题，提出了动态目标跟踪、视觉里程计和地图 应的贝叶斯网络模型如图1所示。其中m=

人系统[1]。从该定义可见，机器人对自身和环境 对象（包括：静态对象和动态对象）的感知在任务 执行中起到关键性和基础性作用。 单目视觉传感器作为一种被动式传感器，凭 借其体积小、耗能少、细节呈现度高等特点，正在 机器人导航学中发挥着重要作用[2]。机器人和计 算机视觉领域的相关学者已经展开了基于单目视 觉的机器人同时定位、地图构建与目标跟踪 (simul￾taneous localization , mapping and object tracking, SLAMOT) 问题研究[3]。机器人领域的 SLAMOT 以 SLAM 为核心[4-6] ，目标识别与跟踪目的是提供 更好的环境特征观测值，通常假设目标为静止的 结构性物体且不考虑平台可观性机动问题，Cast￾ler 等 [7] 在 PTAM (parallel tracking and mapping) 方 法 [8] 基础上提出了独立于摄像机 SFM (structure from motion) 的目标识别与跟踪方法，该方法在图 像关键帧上通过特征点匹配识别环境目标并利用 扎集优化方法 (bundle adjustment, BA) 实现对目标 空间位置的估计，但其假设目标为静止的画像平 面。De 等 [9] 将实际场景中运动物体融入地图构 建以解决增强现实中虚拟物体投影参考缺失问 题。Dai 等 [10] 利用 RGBD 传感器解决了动态物体 对 SLAM 的影响，但并未研究动态物体跟踪问题。 Li 等 [11] 研究了基于运动目标跟踪和静止物体识 别的语义学 SLAM 方法，运功目标跟踪目的同样 是为了减少对 SLAM 的干扰。Civera 等 [12] 将目标 识别与跟踪纳入到基于反转深度的全关联卡尔曼 滤波框架[13] 中，实现了环境物体的识别和空间位 置的在线估计。同样，该方法假设目标为静止的 平面物体。Wangsiripitak 等 [14] 设计了单目 SLAM 与目标跟踪同步估计算法，该算法处理的是空间 立体静止物体并未考虑目标运动问题。Migliore 等 [15] 设计了一种单目 SLAM 和动态目标跟踪算 法，该算法将传统的 SLAM 滤波器和纯方位角观 测目标跟踪器相结合，实现了对摄像机、环境特 征和动态目标状态的同时迭代估计，方法利用的 平台为手持摄像机，因此并未考虑平台可观性机 动对目标状态估计的影响。 Bescos 等 [16] 设计了一种 SLAM 和多目标跟 踪相结合的方法，验证了动态目标跟踪对 SLAM 的辅助作用。Liu 等 [17] 提出了 SLAM 和目标跟踪 强、弱耦合估计切换方法，有效结合了两者的有 点。Sualehm 等 [18] 采用多传感器融合方法实现动 态环境下 SLAM 任务，能够完成对多个运动目标 的跟踪。Liu 等 [19] 研究了人造动态环境下的 SLAM 问题，提出了动态目标跟踪、视觉里程计和地图 构建的多线程估计方法。计算机视觉领域的 SLAMOT 以基于单目视觉的目标跟踪为核心，又 称为：纯方位角观测目标跟踪 (bearing only object tracking) 或目标运动分析 (target motion analysis)， 此类方法希望利用平台机动和目标纯方位角观 测，实现运动目标的状态估计，通常假设移动平台 状态已知。Oh 等 [20] 提出了一种基于粒子群优化 的解决方法，但未考虑平台的机动优化问题。Zhuk 等 [21] 通过对比相邻帧中跟踪点实现运动物体检 测，但并未实现目标状态估计。Watanabe 等 [22] 提 出了基于单目视觉的移动平台导航方法，该方法 针对单目视觉传感器特点将导航控制和目标状态 估计作为耦合问题处理，其假设移动平台状态已 知。Kim 等 [23] 针对纯方位角观测目标跟踪问题 提出了“双重作用 (dual effect) 理论”认为由于观测 深度信息缺失使得目标状态估计和平台机动运动 之间存在关联作用，目标跟踪准确性极大依赖于 平台机动方式，同样，其假设平台状态已知。 综上所述，学界已经开展了基于单目视觉的 SLAMOT 问题的相关研究，但通常将 SLAM 和 OT 相互割裂，并未考虑目标的移动性、机器人状 态未知性以及平台的可观性优化控制问题。文章 正是针对以上问题展开研究，提出了基于单目视 觉的机器人同时定位、地图构建与目标跟踪及平 台控制方法，该方法利用单目视觉 SLAM 实现对 机器人状态估计，并结合目标观测模型进行基于 扩展式卡尔曼滤波的目标状态估计。为了克服观 测模型缺陷，提出了基于目标协方差阵更新最大 化的机器人优化控制方法，最终实现了未知环境 下基于单目视觉的移动机器人目标跟踪。 1 问题描述和模型构建 1.1 问题描述 k X r k X lmi k ,i = 1,2,··· ,m X t k z t k z lmi k X r k X t k X lmi k ,i = 1,2,··· ,m u r k z t k−1 z t k 假设 时刻机器人状态为 ，环境特征状态为 ，目标状态为 ，机器人利用单目 视觉传感器对目标和环境特征进行观测且观测值 分别为 和 ，方法处理目的是在机器人端实现 对 ， 及 的在线估计。由于单目 摄像机得到的观测量缺少距离信息，存在可观性 问题。为了估计目标与机器人的相对距离，机器 人需要根据目标不确定分布实时计算控制量 ， 以保证在该控制量作用下得到的前后时序目标观 测值 和 之间存在足够视差，从而为距离估计 提供足够的信息。 X slam k = 相关对象状态演变符合马尔可夫过程，其对 应的贝叶斯网络模型如图 1 所示。其中 第 17 卷 智 能 系 统 学 报 ·920·

·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 期

第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·

·923· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第5期 0 积。为了克服该问题，机器人运动还需包含机动 △P -arc(sin()) (13) 运动因素以产生足够的目标观测视差，从而实现 arc(tan(y.)) 深度估计，这种机动控制量称为可观性控制分量 其中rt、yR-t、R由式(12)确定。 △，下面介绍其生成方法。 5.2位移控制量△计算 3个不同时刻机 3个不同时刻 假设k时刻，机器人位置控制量为△k,其计 器人位置 目标状态椭圆 算公式为 机器人视线方向 △x -明 k+1k+2 =V.△uk/△u k+1 k+2 △ye (14) △z4 图6纯追随模式下不同时刻，目标不确定椭圆变化图 其中， Fig.6 Target uncertain elliptic change diagram at differ- ent times in pure follow-up mode △=RW-R +- △优化有两点原则：首先，希望由式(9)更新 V为机器人速度常量，RWR为世界坐标系到 后的目标协方差阵对应的分布椭圆体积最小；第 机器人坐标系的旋转矩阵，△x为跟随控制分量， 二，希望△x最小以减少机动负担，因此，优化目 △为产生目标观测视差的可观性控制分量。 标函数为 由于机器人通过角度控制始终朝向目标，因 Object: 此，△x仅包含前向运动分量，即 2.V 3a+分-BA (16) 1+eo-- 其中，V为式(9)中P对应的位置不确定椭圆体 (15) 0 积，B>0为加权常量阵。 0 因为： 其中λ∈(01]为转换速度参量，其值越大转换速度 越快，为k时刻机器人与目标之间的距离， (min( 1 (17) D为机器人和目标间的平衡距离。 min tr(P-K.SK) 图5为V=5ms,Dm=15m,A分别取0.3、0.5、 1,d-T为0~30连续变换时形成的△x曲线。 又有： min t(P-K.S.K)] mr(P)-r(K.S·K) (18) -1 其中由式(5)确定，其值与机器人控制量无关， 1a吸-7 因此，式(18)可写成： -5 0 51015202530 min(PKK)]=-min [tr(K.5)= (19) 图5△r随d-T变化曲线 maxr(KsSK刀 Fig.5 Curve graph of Ax with 重写式(16)为 由图5可见，当机器人和目标距离小于平衡 Object: 距离时，△x{为负值，以保证远离目标；当机器人 △xB-△x 和目标距离大于平衡距离时，△为正值，以保证 1 追随目标。在该值的控制下，能够实现机器人在 max [tr(KSK]+mn2A·BA 恒定距离对目标的追随。 若控制量只包含△x机器人将始终追随目标 运动，由于单目视觉无法观测到距离信息，在机 则最终可得目标函数为 器人和目标的视线方向的目标不确定性将逐步增 Object 大，最终造成跟踪失败，如图6所示。图中显示了 纯追随运动模式下3个时刻目标的不确定椭圆变 gKK-2A-Ba (20) 化，目标在机器人视线方向上的不确定性逐步累 根据式(10)有：

∆φk =   ϕ θ ψ   =   0 −arc(sin(z R,r→t )) arc(tan(y R,r→t /x R,r→t ))   (13) x R,r→t y R,r→t z 其中 R,r→t 、 、 由式（12）确定。 5.2 位移控制量 ∆xk计算 假设 k 时刻，机器人位置控制量为 ∆xk，其计 算公式为 ∆xk =   ∆xk ∆yk ∆zk   = V ·∆uk/∆uk (14) 其中， ∆uk = R W→R ′ k ·   ∆x f k +· ( V −   ∆x f k    ) · ∆x a k   ∆x a k      V R W→R k ∆x f k ∆x a k 为机器人速度常量， 为世界坐标系到 机器人坐标系的旋转矩阵， 为跟随控制分量， 为产生目标观测视差的可观性控制分量。 ∆x f k 由于机器人通过角度控制始终朝向目标，因 此， 仅包含前向运动分量，即 ∆x f k =   ∆x f k 0 0   =   −V + 2 ·V 1+e λ·(Deq−|d R→T k | ) 0 0   (15) λ ∈ (0 1]   d R→T k    D eq 其中 为转换速度参量，其值越大转换速度 越快， 为 k 时刻机器人与目标之间的距离， 为机器人和目标间的平衡距离。 V = 5 m/s D eq=15 m λ   d R→T k    ∆x f k 图 5 为 ， ， 分别取 0.3、0.5、 1， 为 0~30 连续变换时形成的 曲线。 5 3 1 −3 −1 −5 0 5 10 λ=0.3 λ=0.5 λ=1.0 15 m m 20 25 30 |dk R T| ∆x f k   d R→T k   图  5 随 变化曲线 ∆x f k   d R→T k   Fig. 5 Curve graph of with  ∆x f k ∆x f k 由图 5 可见，当机器人和目标距离小于平衡 距离时， 为负值，以保证远离目标；当机器人 和目标距离大于平衡距离时， 为正值，以保证 追随目标。在该值的控制下，能够实现机器人在 恒定距离对目标的追随。 ∆x f 若控制量只包含 k机器人将始终追随目标 运动，由于单目视觉无法观测到距离信息，在机 器人和目标的视线方向的目标不确定性将逐步增 大，最终造成跟踪失败，如图 6 所示。图中显示了 纯追随运动模式下 3 个时刻目标的不确定椭圆变 化，目标在机器人视线方向上的不确定性逐步累 ∆x a k 积。为了克服该问题，机器人运动还需包含机动 运动因素以产生足够的目标观测视差，从而实现 深度估计，这种机动控制量称为可观性控制分量 ，下面介绍其生成方法。 k k+1 k+2 k k+1 k+2 机器人视线方向 3 个不同时刻 目标状态椭圆 3 个不同时刻机 器人位置 图 6 纯追随模式下不同时刻，目标不确定椭圆变化图 Fig. 6 Target uncertain elliptic change diagram at differ￾ent times in pure follow-up mode ∆x a k ∆x a k 优化有两点原则：首先，希望由式（9）更新 后的目标协方差阵对应的分布椭圆体积最小；第 二，希望 最小以减少机动负担，因此，优化目 标函数为 Object: min ∆x a k [ 1 2 ·V t (∆x a k )+ 1 2 ·∆x a ′ k · B·∆x a k ] (16) V t P t + k B > 0 其中， 为式（9）中 对应的位置不确定椭圆体 积， 为加权常量阵。 因为： min ∆x a k [ 1 2 ·V t (∆x a k ) ]  min ∆x a k [ tr(P t + k ) ] = min ∆x a k [ tr(P t − k − K ·S t k · K ′ ) ] (17) 又有： min ∆x a k [ tr( P t − k − K ·S t k · K ′ )] = min ∆x a k [ tr( P t − k ) −tr( K ·S t k · K ′ ) ] (18) P t − 其中 k 由式（5）确定，其值与机器人控制量无关， 因此，式（18）可写成： min ∆x a k [ tr(P t − k − K ·S t k · K ′ ) ]  −min ∆x a k [ tr( K ·S t k · K ′ )] = max ∆x a k [ tr( K ·S t k · K ′ )] (19) 重写式（16）为 Object: min ∆x a k [ 1 2 ·V t (∆x a k ) ] +min ∆x a k [ 1 2 ·∆x a ′ k · B·∆x a k ] ⇒ max ∆x a k [ tr(K ·S t k · K ′ ) ] +min ∆x a k [ 1 2 ·∆x a ′ k · B·∆x a k ] ⇒ max ∆x a k [ tr(K ·S t k · K ′ ) ] −max ∆x a k [ 1 2 ·∆x a ′ k · B·∆x a k ] 则最终可得目标函数为 Object: max ∆x a k [ tr(K ·S t k · K ′ ) ] −max ∆x a k [ 1 2 ·∆x a ′ k · B·∆x a k ] (20) 根据式（10）有： ·923· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第 5 期