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