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