第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 robot[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 experiments verify the coupling relationship between the target state estimation and robot control, demonstrating the effectiveness 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 maneuvering 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]。机器人和计 算机视觉领域的相关学者已经展开了基于单目视 觉的机器人同时定位、地图构建与目标跟踪 (simultaneous localization , mapping and object tracking, SLAMOT) 问题研究[3]。机器人领域的 SLAMOT 以 SLAM 为核心[4-6] ，目标识别与跟踪目的是提供 更好的环境特征观测值，通常假设目标为静止的 结构性物体且不考虑平台可观性机动问题，Castler 等 [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 different 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 期
第17卷 智能系统学报 ·924· r(K.S·K)= R1=(-)fP.R (33) r(P·H2.(S))S·(PE·H2(S))= 将式(30)表示的可观性控制量△写成分量 tr(P·H2.(S)1.2.P) 形式并利用矩阵迹求导性质有： 其中S=H2.P·2+Rk,则有： tr(K·S4·K)= mr-度：(H片+R-)2 A=号 将式(21)代入式(20)可得目标函数最终形式为 g2.Hsa元儿 (34) Object: m2r(.P·H+R'. 1 asH 川形 (22) 其中△号，i=1,2,3是控制向量△的3个分量。计 BAi 算式(29)对分量△吃的偏导有： 对式(22)取△x的偏导并令结果为0，整理可 aH[00-1H△x)e: 00f (35) 得△的优化值为 a△xu Ad-Be瓜 同样，计算式(31)分量△的偏导有： (23) 1 (H.P.+R) axdet(Se ar. (36) 下面给出式(23)的具体求解方法，在不产生 adet() 歧义的情况下，将H一2写成H,P写成P。 假设k时刻机器人位置状态为x,目标位置 将式(35)、(36)代入式(34)即可得到最优控 状态为x,机器人可观性控制量为△W,则世界 制分量。 坐标系中从机器人到目标的向量x一为 实现未知环境下基于单目视觉的机器人目标跟 踪可观性控制需要完成以下3方面计算：首先，完 xW-=xW-(xw+△xW) (24) 成机器人同时定位于地图构建，第二，完成目标状 此刻世界坐标系到机器人坐标系的旋转矩阵 态跟踪，第三，根据机器人和目标状态完成机器 为RW-R,则机器人坐标系下指向目标的向量x为 人控制量计算。计算复杂度主要集中在机器人 R.r- SLAM过程中，由于机器人SLAM、目标跟踪和控 RW-k.xW- (25) 制量生成在计算上是相互解耦的，因此可以选用 根据单目摄像机小孔成像模型有： 运算效率较高的SLAM方法(FAST-SLAM等)。 z=h()= f (26) 6实验设计与验证分析 由偏导链式原则可知： 6.1实验设计 H(Axi)= ah(x-y_ah(x-y）axax- 1)仿真实验设计 AxWa x0Ax 下面通过仿真实验验证方法的有效性、准确 (27) 性并分析影响目标跟踪准确性的因素。实验仿真 根据式(24)~(26)分别计算式(27)中各项整 环境为MATLAB2017,场景如图7所示。 理可得： HA9=a2-太H 28) AxWa 目标 其中 10 环境特 机器人 征点 a&gae-）] (29) Z 0 将式(28)代入式(23)整理可得： 05 X d-号RR5a） (30) 00 -10 -15 5 -20 其中， 025 -25 S1=H1P1·H1'+R1 (31) 图7仿真实验场景 P=RW-R.P.RW-K (32) Fig.7 Experiment scene of simulation
tr(K ·S t k · K ′ ) = tr((P t − k · H t→z k ·(S t − k ) −1 )·S t − k ·(P t − k · H t→z k ·(S t − k ) −1 ) ′ ) = tr(P t − k · H t→z ′ k ·(S t k ) −1 · H t→z k · P t − k ) S t k = Ht→z k · P t − k · Ht→z ′ 其中 k + Rk，则有： tr(K ·S t k · K ′ ) = tr(P t − k · P t − k · H t→z ′ k ·(H t→z k · P t − k · H t→z ′ k + Rk) −1 · H t→z k ) (21) 将式（21）代入式（20）可得目标函数最终形式为 Object: max ∆x a k [ 1 2 ·tr((P t − k ) 2 · H t→z ′ k ·(H t→z k · P t − k · H t→z ′ k + Rk) −1 · H t→z k )− 1 2 ·∆x a ′ k · B·∆x a k ] (22) ∆x a k ∆x a k 对式（22）取 的偏导并令结果为 0，整理可 得 的优化值为 ∆x a k = 1 2 · B −1 · ∂ ∂∆x a k ( tr((P t − k ) 2 · H t→z ′ k · (H t→z k · P t − k · H t→z ′ k + Rk) −1 · H t→z k ) ) (23) Ht→z k H P t − k P 下面给出式（23）的具体求解方法，在不产生 歧义的情况下，将 写成 ， 写成 。 x W,r k x W,t k ∆x W,a k x W,r→t k 假设 k 时刻机器人位置状态为 ，目标位置 状态为 ，机器人可观性控制量为 ，则世界 坐标系中从机器人到目标的向量 为 x W,r→t k = x W,t k −(x W,r k + ∆x W,a k ) (24) R W→R k x R,r→t k 此刻世界坐标系到机器人坐标系的旋转矩阵 为 ，则机器人坐标系下指向目标的向量 为 x R,r→t k = x R,r→t k y R,r→t k z R,r→t k = R W→R k · x W,r→t k (25) 根据单目摄像机小孔成像模型有： z = h(x R,r→t k ) = f z R,r→t [ su · x R,r→t sv · y R,r→t ] + [ u0 v0 ] (26) 由偏导链式原则可知： H(∆x a k ) = ∂h(x R,r→t k ) ∂∆x W,a k = ∂h(x R,r→t k ) ∂x R,r→t k · ∂x R,r→t k ∂x W,r→t k · ∂x W,r→t k ∂∆x W,a k (27) 根据式（24）~（26）分别计算式（27）中各项整 理可得： H(∆x W,a k ) = ∂h(x R,r→t k ) ∂∆x W,a k = − f z R,r→t · H1 · R W→R k (28) 其中， H1 = [ su 0 0 sv − 1 f · ( h(x R,r→t k )− [ u0 v0 ]) ] (29) 将式（28）代入式（23）整理可得： ∆x a k = 1 2 · B −1 · ∂ ∂∆x a k ( tr(P1 · P1 · H1 ′ ·S1 −1 · H1) ) (30) 其中， S1 = H1 · P1 · H1 ′ + R1 (31) P1 = R W→R k · P· R W→R ′ k (32) R1 = (z R,r→t ) 2 / f 2 · R (33) ∆x a 将式（30）表示的可观性控制量 k写成分量 形式并利用矩阵迹求导性质有： ∆x a k,i = 1 2 · B −1 · ∂ ∂∆x a k,i ( tr(P1 · P1 · H ′ 1 ·S1 −1 · H1) ) = B −1 ·tr( P1 2 · H ′ 1 ·S1 −1 · ∂H1 ∂∆x a k,i ) x W,r k ,x W,t k + 1 2 · B −1 ·tr( P1 2 · H ′ 1 · ∂S1 −1 ∂∆x a k,i · H1 ) x W,r k ,x W,t k (34) ∆x a k,i i = 1,2,3 ∆x a k ∆x a k,i 其中 ， 是控制向量 的 3 个分量。计 算式（29）对分量 的偏导有： ∂H1 ∂∆x a k,i = [ 0 0 0 0 −1 f · H(∆x W,a k )· ei ] (35) ∆x a 同样，计算式（31）分量 k,i的偏导有： ∂S1 −1 ∂∆x a k,i = 1 det(S1) det( ∂S1 ∂∆x a k,i ) · ( ∂S1 ∂∆x a k,i )−1 − ( ∂det(S1) ∂∆x a k,i ) ·S1 −1 ) (36) 将式（35）、（36）代入式（34）即可得到最优控 制分量。 实现未知环境下基于单目视觉的机器人目标跟 踪可观性控制需要完成以下 3 方面计算：首先，完 成机器人同时定位于地图构建，第二，完成目标状 态跟踪，第三，根据机器人和目标状态完成机器 人控制量计算。计算复杂度主要集中在机器人 SLAM 过程中，由于机器人 SLAM、目标跟踪和控 制量生成在计算上是相互解耦的，因此可以选用 运算效率较高的 SLAM 方法（FAST-SLAM 等）。 6 实验设计与验证分析 6.1 实验设计 1）仿真实验设计 下面通过仿真实验验证方法的有效性、准确 性并分析影响目标跟踪准确性的因素。实验仿真 环境为 MATLAB2017，场景如图 7 所示。 机器人 目标 环境特 征点 10 0 25201510 5 0 −5 −10 −15 −20 −25 −25 −20 −15 −10 −5 0 X 5 10 15 20 25 −10 Z Y 图 7 仿真实验场景 Fig. 7 Experiment scene of simulation 第 17 卷 智 能 系 统 学 报 ·924·
·925· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第5期 实验场景为三维立体空间，其范围为：X-25~ 均匀地分布着环境特征点。相关实验参数配置如 25m,Y:-25m-25m,Z:-10m~10m,在空间四周 表1所示。 表1实验参数配置表 Table 1 Parameter configuration 机器人控制标准差 摄像机内参/像素 图像像素尺寸/像素 观测标准差/像素 σ△元=[0.01m0.01m0.01m0.01m 4=320,%=240 u=648 r=[33] rA=[0.02°0.02°0.02°] 5=320,5,=320 v=480 2)实体机器人实验设计 机器人，环境中4个UWB基站作为定位标准点 实体机器人实验采用Turtlebot2型差动驱动 且位置已知。 平台，如图8所示。 实验基于ROS(robot operating system,ROS) 框架，节点及消息关系如图10所示。 UWB室内定 位系统天线、 激光 扫描仪 运算服务器 Turtlebot机器人 天线 环境 SLAM 特征点 机器人 环境特 运算 状态 征检测 节点 节点 机器人 控制量 目标 状态 目标 计算 目标检 观测值 状态 节点 RGB-D 测节点 摄像机 目标跟 机器人 踪节点 运动执 控制量 行节点 差动驱 动底盘 图10ROS框架节点及消息关系实验场景 图8 Turtlebot2实体机器人平台 Fig.10 ROS framework node and message diagram Fig.8 Turtlebot2 physical robot platform 考虑到图像数据量巨大不利于实时传输，以 该平台配备单目摄像头，WFi无线通信系统 及Turtlebot上位机运算能力有限不利于计算等因 以及UWB(ultra wide band,UWB)室内定位系 素，系统将感知与执行，对象状态估计与控制量 统。单目摄像头完成RGB图像获取，无线通信系 生成分别在机器人端和运算服务器端完成。Tur- 统完成机器人端和服务器端的数据传输，UWB定 tlebot机器人端包含感知节点和执行节点分别完 位系统提供机器人和目标的定位真值。实验场景 成环境特征检测、目标检测和平台运动控制任 如图9所示。 务，其中环境特征为S特征点，目标检测利用 Camshift方法实现。运算服务器端包含SLAM运 UWB UWB 基站 UWB 算节点、目标跟踪节点和控制量计算节点分别利 基站 用第3、4、5节介绍方法实现机器人状态估计、目 标状态估计和平台控制量生成任务。另外，由于 目标 UWB 色块 该机器人运动学受非完整性约束限制，采用文献 基站 [26]所介绍的控制方法实现完整性约束到非完整 性约束的转换控制。 6.2仿真实验验证及分析 机器人 6.2.1有效性实验分析 为了验证方法有效性，假设目标运动轨迹为直 图9实验场景 线，初始位置坐标为(15,15,1)，速度恒定为2m/s。 Fig.9 Experimental scene 机器人初始位置坐标为(15，-15,1)。机器人与目 实验环境为室内环境，载有色块的机器人作 标的平衡距离设为D=10m,机器人最大机动速 为目标进行匀速直线运动，后方机器人作为跟踪 度设为vx=2.5ms,仿真共进行了300次迭代，时
实验场景为三维立体空间，其范围为：X: −25 m~ 25 m，Y: −25 m~25 m，Z: −10 m~10 m，在空间四周 均匀地分布着环境特征点。相关实验参数配置如 表 1 所示。 表 1 实验参数配置表 Table 1 Parameter configuration 机器人控制标准差 摄像机内参/像素 图像像素尺寸/像素 观测标准差/像素 σ∆x˜ r k = [0.01 m 0.01 m 0.01 m 0.01 m] σ∆e˜ r k = [0.02° 0.02° 0.02° ] u0 = 320，v0 = 240 su = 320，sv = 320 u = 648 v = 480 σr = [3 3] 2）实体机器人实验设计 实体机器人实验采用 Turtlebot2 型差动驱动 平台，如图 8 所示。 UWB 室内定 位系统天线 Wifi 天线 激光 扫描仪 RGB-D 摄像机 差动驱 动底盘 图 8 Turtlebot2 实体机器人平台 Fig. 8 Turtlebot2 physical robot platform 该平台配备单目摄像头，WiFi 无线通信系统 以及 UWB（ultra wide band, UWB）室内定位系 统。单目摄像头完成 RGB 图像获取，无线通信系 统完成机器人端和服务器端的数据传输，UWB 定 位系统提供机器人和目标的定位真值。实验场景 如图 9 所示。 UWB 基站 UWB 基站 UWB 基站 UWB 基站 目标 色块 机器人 图 9 实验场景 Fig. 9 Experimental scene 实验环境为室内环境，载有色块的机器人作 为目标进行匀速直线运动，后方机器人作为跟踪 机器人，环境中 4 个 UWB 基站作为定位标准点 且位置已知。 实验基于 ROS（robot operating system, ROS） 框架，节点及消息关系如图 10 所示。 环境特 征检测 节点 Turtlebot 机器人 环境 特征点 目标 观测值 机器人 控制量 运算服务器 机器人 状态 机器人 状态 SLAM 运算 节点 目标 状态 目标跟 踪节点 控制量 计算 目标检 节点 测节点 运动执 行节点 图 10 ROS 框架节点及消息关系实验场景 Fig. 10 ROS framework node and message diagram 考虑到图像数据量巨大不利于实时传输，以 及 Turtlebot 上位机运算能力有限不利于计算等因 素，系统将感知与执行，对象状态估计与控制量 生成分别在机器人端和运算服务器端完成。Turtlebot 机器人端包含感知节点和执行节点分别完 成环境特征检测、目标检测和平台运动控制任 务，其中环境特征为 Sift 特征点，目标检测利用 Camshift 方法实现。运算服务器端包含 SLAM 运 算节点、目标跟踪节点和控制量计算节点分别利 用第 3、4、5 节介绍方法实现机器人状态估计、目 标状态估计和平台控制量生成任务。另外，由于 该机器人运动学受非完整性约束限制，采用文献 [26] 所介绍的控制方法实现完整性约束到非完整 性约束的转换控制。 6.2 仿真实验验证及分析 6.2.1 有效性实验分析 (15,15,1) (15,−15,1) D eq = 10 m vmax = 2.5 m/s 为了验证方法有效性，假设目标运动轨迹为直 线，初始位置坐标为 ，速度恒定为 2 m/s。 机器人初始位置坐标为 。机器人与目 标的平衡距离设为 ，机器人最大机动速 度设为 ，仿真共进行了 300 次迭代，时 ·925· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第 5 期
第17卷 智能系统学报 ·926· 间步进为0.05s。 实验总体跟踪结果俯视图如图11所示。图 中目标和机器人实际轨迹为实线，目标和机器人 估计轨迹为虚线，十字点为环境特征实际位置， 椭圆为环境特征状态估计分布椭圆。从图可见， 实验开始时机器人距离目标较远，控制以追随为 主，当机器人距离目标较近时可观性控制分量开 12 91012314516178 始发挥作用，机器人做机动运动。从目标估计轨 图12三维空间机器人轨迹局部放大图 迹可见，刚开始由于机器人并未产生可观性机动， Fig.12 A local large robot trajectory in three dimensional 因此目标估计轨迹逐渐偏离实际轨迹，当机器人 space 开始机动时，目标估计轨迹快速收敛到实际轨迹 图13为实验中不同时刻机器人控制量成分 附近。 值变化曲线。图中实线为不同时刻机器人速度控 制值，细虚线为其中追随控制分量值，粗虚线为 -25 目标和机器 其中可观性控制分量值，由图可见，在跟踪初期 -20 人实际轨迹 目标估计轨迹一 0~85s期间，机器人控制量的主要成分为追随控 -15 目标 目标和机器 起点 人估计轨迹 制分量。随着机器人靠近目标可观性控制分量开 -10 目标实 环境特征实 -5 际轨迹 际位置 始发挥作用，在时段85~300s期间，可观性控制 环境特征估 成分大于追随控制成分，因此，在此期间机器人 0 机器人估 计位置 计轨迹 机器人实 开始螺旋式机动运动，从而克服单目视觉深度观 际轨迹 10 测值缺失的问题，快速实现目标位置状态估计。 15 ++ 机器人起点 0.25 分020 △可观性控制分量值子 -25-20-15-10-50510152025 m 追随控制分量值 图11实验总体跟踪结果 0 总体控制量值 Fig.11 Overall tracking results 0 50 100150200250300 s 图12为三维空间中机器人实际和估计轨迹 图13不同时刻机器人控制分量变化曲线 的局部放大图。由图可见，机器人的机动曲线为 Fig.13 The curve of control component of robot at differ- 螺旋式曲线。机器人以此种轨迹对目标进行跟随 ent time 有利于产生足够的单目观测视差，从而快速完成 图14为不同时刻机器人和目标各分量及位 相对目标距离的估计。 置估计误差曲线。 一误差曲线 容差曲线 6[X分量 50 100 150 200 250 300 20 了分量 号10 50 100 150 200 250 300 1.0 Z分量 号0.5 0 A 50 100 150 200 250 300 20位置估计 10 50 100 150 200 250 300 (a)目标各分量及位置估计误差曲线
间步进为 0.05 s。 实验总体跟踪结果俯视图如图 11 所示。图 中目标和机器人实际轨迹为实线，目标和机器人 估计轨迹为虚线，十字点为环境特征实际位置， 椭圆为环境特征状态估计分布椭圆。从图可见， 实验开始时机器人距离目标较远，控制以追随为 主，当机器人距离目标较近时可观性控制分量开 始发挥作用，机器人做机动运动。从目标估计轨 迹可见，刚开始由于机器人并未产生可观性机动， 因此目标估计轨迹逐渐偏离实际轨迹，当机器人 开始机动时，目标估计轨迹快速收敛到实际轨迹 附近。 目标和机器 人实际轨迹 目标和机器 人估计轨迹 环境特征实 际位置 环境特征估 计位置 机器人起点 目标 起点 目标实 际轨迹 目标估计轨迹 机器人实 际轨迹 机器人估 计轨迹 −25 −20 −15 −10 −5 0 5 10 15 20 Y/m X/m 25 −25 −20 −15 −10 −5 0 5 10 15 20 25 图 11 实验总体跟踪结果 Fig. 11 Overall tracking results 图 12 为三维空间中机器人实际和估计轨迹 的局部放大图。由图可见，机器人的机动曲线为 螺旋式曲线。机器人以此种轨迹对目标进行跟随 有利于产生足够的单目观测视差，从而快速完成 相对目标距离的估计。 3.5 3.0 2.5 2.0 1.5 1.0 0.5 Z 0 −4−3−2−1 X Y 0 1 2 3 4 9 10 11 12 13 14 15161718 图 12 三维空间机器人轨迹局部放大图 Fig. 12 A local large robot trajectory in three dimensional space 图 13 为实验中不同时刻机器人控制量成分 值变化曲线。图中实线为不同时刻机器人速度控 制值，细虚线为其中追随控制分量值，粗虚线为 其中可观性控制分量值，由图可见，在跟踪初期 0~85 s 期间，机器人控制量的主要成分为追随控 制分量。随着机器人靠近目标可观性控制分量开 始发挥作用，在时段 85~300 s 期间，可观性控制 成分大于追随控制成分，因此，在此期间机器人 开始螺旋式机动运动，从而克服单目视觉深度观 测值缺失的问题，快速实现目标位置状态估计。 0 0.25 0.20 0.15 0.10 0.05 0 50 100 150 Δx a k Δx f k t/s Δuk/(m·s−1 ) 200 250 300 可观性控制分量值子 追随控制分量值 总体控制量值 图 13 不同时刻机器人控制分量变化曲线 Fig. 13 The curve of control component of robot at different time 图 14 为不同时刻机器人和目标各分量及位 置估计误差曲线。 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 50 100 150 t/s t/s 200 250 300 0 0.5 1.0 10 20 10 20 6 X 分量 误差曲线 容差曲线 X 分量 Y 分量 Y 分量 Z 分量 Z 分量 位置估计 位置估计 4 e/m e/m e/m e/m e/m e/m e/m e/m 2 (a) 目标各分量及位置估计误差曲线 (b) 机器人各分量及位置估计误差曲线 误差曲线 容差曲线 第 17 卷 智 能 系 统 学 报 ·926·
·927· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第5期 误差曲线 -··容差曲线 0.3 5 「X分量 0 50 100 150 200 250 300 0.4r Y分量 号02 50 100 150 200 250 0.2Z分量 ..300 0.1 50 100 150 200 250 300 1.0 位置估计 号0.5 50 100 150 200 250 300 s (b)机器人各分量及位置估计误差曲线 图14目标和机器人各分量及位置估计误差曲线 Fig.14 Estimation error curve of each components and position 从图14(a)可见，在0-85s期间，由于机器人 6,2.2准确性实验及分析 并未进行可观性机动，因此，目标各分量及位置 为了验证方法的准确性，假设目标起点与终 误差持续增加。在85~300s期间，机器人开始机 点均为0,15,1)，目标实际运行轨迹为圆形，线速 动运动，目标各分量及位置误差快速减小并始终 度为2m/s。机器人起点为(10,15,1)，朝向为(0,0，π)， 保持在较低水平。从图14(b)可见，机器人各分量 机器人与目标的平衡距离设为D=10m,机器人 及位置估计始终较为准确，从图11可见，环境特 最大机动速度设为vmas=2.5ms,仿真共进行了 征分布均匀且机器人在运动过程中有足够多的、 500次迭代，时间步进为0.05s。实验总图跟踪结 准确性高的特征保持在其视野中，这些保证了较 果如图16所示。 高的机器人定位精度。 25 目标和机器 图15为不同时刻，目标观测残差阵S对应的 20 88 人实际轨迹 不确定椭圆面积变化曲线。 15 M 目标和机器 人估计轨迹 10 120 目标起点 +环境特征实 100 与终点机器人 际位置 终点 80 0 环境特征估 60 -5 ?机器人实机器人估 计位置 际轨迹 计轨迹 目标估 40 -10 计轨迹 20 50 100150200250300 -15 目标实 -20 际轨迹 -25 图15 不同时刻目标观测值不确定椭圆面积变化曲线 -20 -10 0 10 20 25 Fig.15 Target observation uncertain elliptical area change X/m curve (a)实验俯视图 由图可见，在0~85s期间，观测不确定椭圆面 积变化很平稳，说明机器人对目标的前后时序观 测视差变化较小，用来进行深度估计的信息量较 少，因此，目标状态估计误差持续增大。在85s, 出现了一个较大跳变，此后机器人开始可观性机 动运动，在此期间，观测不确定椭圆面积变化较 为剧烈，这说明机器人在前后观测中存在较大视 差，能够为目标深度估计提供足够多的信息量， 图16实验总体结果 从而保证了目标跟踪的准确性。 Fig.16 Overall tracking results
t/s t/s 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 50 100 150 200 250 300 0 1.0 0.5 0.1 0.2 0.4 0.1 0.2 0.3 X 分量 误差曲线 容差曲线 X 分量 Y 分量 Y 分量 Z 分量 Z 分量 位置估计 位置估计 e/m e/m e/m e/m e/m e/m e/m e/m 0.2 50 100 150 200 250 300 (a) 目标各分量及位置估计误差曲线 (b) 机器人各分量及位置估计误差曲线 误差曲线 容差曲线 图 14 目标和机器人各分量及位置估计误差曲线 Fig. 14 Estimation error curve of each components and position 从图 14(a) 可见，在 0~85 s期间，由于机器人 并未进行可观性机动，因此，目标各分量及位置 误差持续增加。在 85~300 s 期间，机器人开始机 动运动，目标各分量及位置误差快速减小并始终 保持在较低水平。从图 14(b)可见，机器人各分量 及位置估计始终较为准确，从图 11 可见，环境特 征分布均匀且机器人在运动过程中有足够多的、 准确性高的特征保持在其视野中，这些保证了较 高的机器人定位精度。 S t 图 15 为不同时刻，目标观测残差阵 k对应的 不确定椭圆面积变化曲线。 0 50 100 150 200 250 300 t/s 20 40 60 80 100 120 S 图 15 不同时刻目标观测值不确定椭圆面积变化曲线 Fig. 15 Target observation uncertain elliptical area change curve 由图可见，在 0~85 s 期间，观测不确定椭圆面 积变化很平稳，说明机器人对目标的前后时序观 测视差变化较小，用来进行深度估计的信息量较 少，因此，目标状态估计误差持续增大。在 85 s， 出现了一个较大跳变，此后机器人开始可观性机 动运动，在此期间，观测不确定椭圆面积变化较 为剧烈，这说明机器人在前后观测中存在较大视 差，能够为目标深度估计提供足够多的信息量， 从而保证了目标跟踪的准确性。 6.2.2 准确性实验及分析 (0,15,1) 2 m/s (10,15,1) (0,0,π) D eq = 10 m vmax = 2.5 m/s 为了验证方法的准确性，假设目标起点与终 点均为 ，目标实际运行轨迹为圆形，线速 度为 。机器人起点为 ，朝向为 ， 机器人与目标的平衡距离设为 ，机器人 最大机动速度设为 ，仿真共进行了 500 次迭代，时间步进为 0.05 s。实验总图跟踪结 果如图 16 所示。 目标和机器 人实际轨迹 目标和机器 人估计轨迹 环境特征估 计位置 环境特征实 机器人 际位置 终点 机器人 起点 目标起点 与终点 机器人估 计轨迹 机器人实 际轨迹 目标估 计轨迹 目标实 际轨迹 25 20 15 10 5 0 −5 −10 −15 −20 −20 −10 0 (a) 实验俯视图 X/m Y/m 10 20 25 −25 图 16 实验总体结果 Fig. 16 Overall tracking results ·927· 伍明，等：纯方位角目标跟踪及移动平台可观性控制方法 第 5 期
第17卷 智能系统学报 ·928· 从结果可知，机器人和目标的估计轨迹和实 速度v取10个不同值时对应的目标状态估计误 际轨迹保持较高的一致性。 差均值和方差曲线。 图17为机器人和目标轨迹及误差曲线。 25 一实际轨迹 一实际轨迹 20 20 估计轨迹 20 估计轨迹 15 15 10 0 5 5 0 -5 5 00.1020.30.40.50.60.70.80.91.0 -10 -1 o/m -15 -15 50为0505 89为0505 图18机器人定位误差对目标定位精度影响曲线 X/m X/m Fig.18 Influence between robot and target localization ac- (a)机器人轨迹 (b)目标轨迹 curacy 一实际轨迹 60 一估计轨迹 50 0.6 日0.4 0 0.2 0 0 05010150200250300350400450500 s 1.01.52.02.53.03.54.04.55.05.56.0 (©)机器人位置估计误差曲线 Va/(m's-) 一误差曲线 图19机动能力对目标定位精度影响曲线 容差曲线 Fig.19 Influence between robot maneuver ability and tar- 10 get localization accuracy 0enA4e心Aa的 由图可见，随着机器人最大运动速度vmx的增 050100150200250300350400450500 大，目标状态估计准确性逐步提高并最终趋近于 Us (d)目标位置估计误差曲线 恒定值。说明目标定位精度与机器人机动能力成 图17机器人和目标轨迹及误差曲线 正相关性。 Fig.17 Robot,target trajectory and error curve 63实体机器人实验验证及分析 上方两个子图为机器人和目标的实际与估计 实体机器人实验中，目标初始位置坐标为(0，-2)， 轨迹对比图，下方两个子图为不同时刻机器人和 速度恒定为0.07m/s。机器人初始位置坐标为 目标位置估计误差曲线图，从轨迹图可见，机器 (2,-4)。机器人与目标的平衡距离设为D-1m, 人和目标估计轨迹始终贴合实际轨迹。从误差曲 机器人最大机动速度为vmx=02m/s,实验共进行了 线图可见，机器人和目标估计误差均保持在容差 60s。需要说明的是，由于所选用的机器人平台 范围内。另外，在整个估计过程中机器人平均估 较为简易，其单目摄像头视野狭窄，同时，易产生 计误差为0202m,目标平均估计误差为1.227m, 图像晃动问题。因此，在实验中有意将目标和机器 目标和机器人估计准确性相差一个数量级。 人运动速度调慢，意在弱化条件，重点检验方法对 623影响因素实验及分析 目标状态，机器人状态估计的有效性和准确性。 下面分别分析机器人定位精度、机器人机动 实体机器人跟踪结果如图20所示。 程度两种因素对目标跟踪精度的影响。 假设机器人最大速度为vmx=3.5m/s,平衡距 离D=10m条件下，机器人位置估计在10种噪声 干扰下得到的目标状态估计误差均值和方差曲线 2 环境特 目标 征估计 终点 如图18所示。 目标和机器 由图可见，随着人为引入机器人定位噪声的 人实际轨迹 机器人 目标和机器 增加，当加性机器人定位高斯白噪声的标准差为 -2 目标 终点 人估计轨迹 1时，目标定位误差均值为16.36，误差标准差为 终点 机器人 环境特征 ”终点 估计 5.83。由此可见，目标定位精度与机器人定位精 -2 0 2 4 6 度呈现正相关性。 m 图19为假设平衡距离D=10m,机器人最大 (a)总体跟踪结果
从结果可知，机器人和目标的估计轨迹和实 际轨迹保持较高的一致性。 图 17 为机器人和目标轨迹及误差曲线。 (a) 机器人轨迹 实际轨迹 估计轨迹 (b) 目标轨迹 15 20 20 10 5 0 −5 −10 −15 −20 −15 −10 −5 0 Y/m 15 10 5 0 −5 −10 −15 Y/m X/m 5 10 15 −20 −15 −10 −5 0 X/m 5 10 15 0 50 10 150 200 250 (c) 机器人位置估计误差曲线 (d) 目标位置估计误差曲线 t/s 300 350 400 450 500 0 0 5 10 50 100 150 200 250 t/s e/m 0 0.2 0.6 0.4 e/m 300 350 400 450 500 实际轨迹 估计轨迹 实际轨迹 估计轨迹 误差曲线 容差曲线 图 17 机器人和目标轨迹及误差曲线 Fig. 17 Robot , target trajectory and error curve 上方两个子图为机器人和目标的实际与估计 轨迹对比图，下方两个子图为不同时刻机器人和 目标位置估计误差曲线图，从轨迹图可见，机器 人和目标估计轨迹始终贴合实际轨迹。从误差曲 线图可见，机器人和目标估计误差均保持在容差 范围内。另外，在整个估计过程中机器人平均估 计误差为 0.202 m，目标平均估计误差为 1.227 m， 目标和机器人估计准确性相差一个数量级。 6.2.3 影响因素实验及分析 下面分别分析机器人定位精度、机器人机动 程度两种因素对目标跟踪精度的影响。 vmax = 3.5 m/s D eq = 10 m 假设机器人最大速度为 ，平衡距 离 条件下，机器人位置估计在 10 种噪声 干扰下得到的目标状态估计误差均值和方差曲线 如图 18 所示。 由图可见，随着人为引入机器人定位噪声的 增加，当加性机器人定位高斯白噪声的标准差为 1 时，目标定位误差均值为 16.36，误差标准差为 5.83。由此可见，目标定位精度与机器人定位精 度呈现正相关性。 D eq 图 19 为假设平衡距离 = 10 m ，机器人最大 速度 vmax取 10 个不同值时对应的目标状态估计误 差均值和方差曲线。 25 20 15 10 5 0 0.1 0.2 0.3 0.4 0.5 0.6 σ/m e/m 0.7 0.8 0.9 1.0 图 18 机器人定位误差对目标定位精度影响曲线 Fig. 18 Influence between robot and target localization accuracy 60 e/m vmax/(m·s−1) 50 40 30 20 10 0 1.0 1.5 2.0 2.5 3.0 3.5 4.0 4.5 5.0 5.5 6.0 图 19 机动能力对目标定位精度影响曲线 Fig. 19 Influence between robot maneuver ability and target localization accuracy 由图可见，随着机器人最大运动速度 vmax的增 大，目标状态估计准确性逐步提高并最终趋近于 恒定值。说明目标定位精度与机器人机动能力成 正相关性。 6.3 实体机器人实验验证及分析 (0,−2) (2,−4) D eq = 1 m vmax = 0.2 m/s 实体机器人实验中，目标初始位置坐标为 ， 速度恒定为 0.07 m/s。机器人初始位置坐标为 。机器人与目标的平衡距离设为 ， 机器人最大机动速度为 ，实验共进行了 60 s。需要说明的是，由于所选用的机器人平台 较为简易，其单目摄像头视野狭窄，同时，易产生 图像晃动问题。因此，在实验中有意将目标和机器 人运动速度调慢，意在弱化条件，重点检验方法对 目标状态，机器人状态估计的有效性和准确性。 实体机器人跟踪结果如图 20 所示。 (a) 总体跟踪结果 机器人 终点 机器人 终点 目标 终点 目标 终点 环境特 征估计 目标和机器 人实际轨迹 目标和机器 人估计轨迹 环境特征 估计 6 5 4 3 Y/m X/m 2 1 0 −1 −2 −3 −4 −6 −4 −2 0 2 4 6 (b) 机器人跟踪结果 (c) 目标跟踪结果 −2.0 −2.5 1.5 1.0 0.5 −0.5 −1.0 −1.5 −2.5 −3.0 −3.5 −0.5 −2.0 −1.0 0.5 1.0 2.0 Y/m Y/m X/m X/m 1.0 1.5 2.0 2.5 第 17 卷 智 能 系 统 学 报 ·928·