CN101630413A - 一种多机器人跟踪移动目标算法 - Google Patents

Abstract

本发明公开了一种新的多机器人跟踪移动目标算法，包括以下步骤：1)使用移动目标的当前位置信息和前一时刻的位置信息构造移动目标的运动预测模型；2)使用移动目标的运动预测模型的移动目标预测位置构造动态的机器人运动控制模型；3)使用移动机器人对移动目标的测量信息更新移动目标的位置信息，同时使用协方差插值方法融合各个机器人对移动目标位置的估计，以得到最终的移动目标位置。本发明方法能够保证移动目标始终处于多机器人的可视范围内，可以保证对移动目标位置估计的精确性。

Description

一种多机器人跟踪移动目标算法

技术领域

本发明涉及多机器人对移动目标跟踪领域，具体来说是根据移动目标的预测位置来动态调整移动机器人的运动模型，以使得移动目标始终处于各个机器人的可视范围。

背景技术

目标跟踪技术已经被广泛应用于监控、自动防卫系统以及机器人领域。随着机器人在人类日常生活中的普及，机器人必须实现诸如编队、对环境的感知、避障等任务。移动目标跟踪也是机器人领域的一个重要，而且富有挑战性的研究方向，尤其是当移动目标的运动路径、运动速度等参数未知的情况下。使用多机器人对随机运动目标进行跟踪时必须考虑如下两个问题，一是如何确保移动目标始终处于各个机器人的可视范围内；二是如何使用各个机器人对移动目标的测量信息融合得到最终的移动目标位置。

发明内容

本发明提供了一种新的多机器人跟踪移动目标的算法，使用移动目标的历史位置信息构造移动目标的位置预测模型，使用移动目标的预测位置构造动态机器人运动控制模型，并使用融合算法对移动目标位置进行更新得到更加精确的移动目标位置。

本发明算法首先根据移动目标的历史信息动态构造移动目标的运动预测模型，并使用此模型对移动目标的位置进行预测，然后使用移动目标的预测位置对机器人的运动控制模型进行动态调整，最后使用各个机器人对移动目标的检测信息来更新移动目标的位置。

一种新的多机器人跟踪移动目标的算法，包括以下步骤：

1)使用移动目标的当前位置信息和前一时刻的位置信息构造移动目标的运动预测模型，构造移动目标的运动预测模型时分两种情况进行考虑：一是在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离小于等于之前相邻时刻间移动目标位置间的笛卡尔距离时，预测的移动目标位置和当前时刻移动目标位置之间的欧氏距离差和角度差同当前时刻移动目标位置和前一时刻移动目标位置的差都保持一致的移动目标运动预测模型，简称为等距离等角度的移动目标运动预测模型，二是在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离大于之前相邻时刻间移动目标位置间的笛卡尔距离时，预测的移动目标位置和当前时刻移动目标位置之间的欧氏距离差同当前时刻移动目标位置和前一时刻移动目标位置的差都保持一致的移动目标运动预测模型，简称为等距离的移动目标运动预测模型；

2)使用移动目标运动预测模型的移动目标预测位置构造动态的机器人运动控制模型，从而使得移动目标始终处于移动机器人的可视范围内；

3)使用移动机器人对移动目标的测量信息更新移动目标的位置信息，同时使用协方差插值方法融合各个机器人对移动目标位置的估计，以得到最终的移动目标位置。

所述的等距离等角度的移动目标运动预测模型如式(3)所示。其中object(k+1)_x和object(k+1)_y表示移动目标在k+1时刻的笛卡尔坐标，object(k)_x和object(k)_y表示移动目标在k时刻的笛卡尔坐标。robot(k+1)_x和robot(k+1)_y表示机器人在k+1时刻的笛卡尔坐标。

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=\mathrm{object}(k+1)\_x\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\cos (\mathrm{\θ}\_B)\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=\mathrm{object}(k+1)\_y\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\sin (\mathrm{\θ}\_B)\end{array}\right.---\left(3\right)$

其中θ_B为式(2)所示。

θ_B＝2×π-θ1-θ2-θ3-θ4              (4)

式(2)中的θ1，θ2，θ3和θ4分别为式(5)，(6)，(7)所示。

$\mathrm{\θ}1=\mathrm{\θ}2=\arccos \frac{\sqrt{(\mathrm{robot}(k+1)\_x^2+\mathrm{robot}(k+1)\_y^2)}\×\sin (\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\arctan \frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}---\left(5\right)$

$\mathrm{\θ}3=\arccos \frac{\sqrt{({(\mathrm{object}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_x)}^2)}-\sqrt{({(\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}\left(k\right)\_y)}^2)}*\cos \left(\arctan (\frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})\right)}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}$

(6)

$\mathrm{\θ}4=\mathrm{\π}-\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}---\left(7\right)$

所述的等距离的移动目标运动预测模型如式(8)所示。

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y\end{array}\right.---\left(8\right)$

所述的机器人运动控制模型如式(9)所示，其中robot(k+2)_x和robot(k+2)_y表示机器人在k+2时刻的位置坐标，θ_R(k+2)表示机器人在k+2时刻的方位角，θ_R(k+1)表示机器人在k+1时刻的方位角。object(k+2)_x_predict和object(k+2)_y_predict表示机器人在k+2时刻的移动目标预测位置。

$\left\{\begin{array}{c}\mathrm{robot}(k+2)\_x=\mathrm{robot}(k+1)\_x\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y)}^2}\end{array}\right)\cos (\mathrm{\θ}\_R(k+2))\\ \mathrm{robot}(k+2)\_y=\mathrm{robot}(k+1)\_y\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y)}^2}\end{array}\right)\sin (\mathrm{\θ}\_R(k+2))\\ \mathrm{\θ}\_R(k+2)=\arctan \frac{\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x}\\ -\arctan \frac{\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x}+\arctan \frac{\mathrm{robot}(k+1)\_y}{\mathrm{robot}(k+1)\_x}\end{array}\right.$ (9)

本发明方法使用移动目标当前时刻的位置和前一时刻的位置构造移动目标的运动预测模型，使用移动目标的预测位置对移动机器人的控制模型进行动态调整，同时使用协方差插值融合方法对移动目标位置进行更新。本发明方法能够保证移动目标始终处于多机器人的可视范围内，可以保证对移动目标位置估计的精确性。

附图说明

图1为本发明算法的具体流程图；

图2为移动目标位置预测模型构造描述；

图3为等距离等角度的移动目标运动预测模型；

图4为等距离的移动目标运动预测模型；

图5为机器人的动态控制模型；

图6为两个机器人对移动目标位置预测测试的仿真环境；

图7为机器人R1对移动目标位置跟踪后的机器人路径信息；

图8为机器人R2对移动目标位置跟踪后的机器人路径信息；

图9为两个机器人对移动目标位置跟踪后的移动目标位置信息；

图10为两个机器人对移动目标位置跟踪得到的移动目标位置信息和实际移动目标位置信息的比较。

具体实施方式

一种新的多机器人跟踪移动目标算法，算法流程图如图1所示，首先造移动目标的运动预测模型，构造移动目标的运动预测模型时分两种情况进行考虑：一是在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离小于等于之前相邻时刻间移动目标位置间的笛卡尔距离，二是在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离大于之前相邻时刻间移动目标位置间的笛卡尔距离。然后使用移动目标运动预测模型的移动目标预测位置构造机器人的运动控制模型，并使用运动控制模型对机器人的位姿进行估计，以保证移动目标始终处于移动机器人的可视范围内。最后使用移动机器人对移动目标的测量信息更新移动目标的位置信息，同时使用协方差插值方法融合各个机器人对移动目标位置的估计，得到最终的移动目标位置。

首先构造移动目标的运动预测模型，图2表示了移动目标的运动预测模型构造方法。α表示移动目标在时刻k+1和k的位置坐标之间的角度差，β表示在保证k+2时刻移动目标的位置和k+1时刻移动目标位置之间的欧式距离和k+1时刻与k时刻之间欧式距离相等的条件下，和k+1时刻目标位置之间最大的角度差。对移动目标的运动预测模型构造分为两种情况进行考虑：

一是当β大于α时的移动目标运动预测模型构造，此时构造方法如图3所示，此移动目标运动预测模型称为等距离等角度的移动目标运动预测模型。

二是当β小于α时的移动目标运动预测模型构造，此时构造方法如图4所示，此移动目标运动预测模型称为等距离的移动目标运动预测模型。

由图2可知，α的计算方法为式(1)，β的计算方法为式(2)。其中object(k+1)_x和object(k+1)_y表示移动目标在k+1时刻的笛卡尔坐标，object(k)_x和object(k)_y表示移动目标在k时刻的笛卡尔坐标。

$\mathrm{\α}=\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\arctan \frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x}---\left(1\right)$

$\mathrm{\β}=\arcsin \frac{\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}}{\sqrt{{(\mathrm{object}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y)}^2}}---\left(2\right)$

由图3可知，当β大于α时，等距离等角度的移动目标运动预测模型为式(3)所示。

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=\mathrm{object}(k+1)\_x\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\cos (\mathrm{\θ}\_B)\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=\mathrm{object}(k+1)\_y\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\sin (\mathrm{\θ}\_B)\end{array}\right.---\left(3\right)$

式(3)中θ_B表示图3中移动目标k+2时刻的预测位置和x坐标系之间的角度差，如式(4)所示。

θ_B＝2×π-θ1-θ2-θ3-θ4               (4)

式(4)中的θ1，θ2，θ3和θ4的值如式(5)，(6)和(7)所示。

$\mathrm{\θ}1=\mathrm{\θ}2=\arccos \frac{\sqrt{(\mathrm{robot}(k+1)\_x^2+\mathrm{robot}(k+1)\_y^2)}\×\sin (\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\arctan \frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}---\left(5\right)$

$\mathrm{\θ}3=\arccos \frac{\sqrt{({(\mathrm{object}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_x)}^2)}-\sqrt{({(\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}\left(k\right)\_y)}^2)}*\cos \left(\arctan (\frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})\right)}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}$

(6)

$\mathrm{\θ}4=\mathrm{\π}-\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}---\left(7\right)$

由图4可知，当β小于α时，等距离的移动目标运动预测模型为式(8)所示。

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y\end{array}\right.---\left(8\right)$

其次是根据图5获得机器人的动态控制模型，为了使得移动目标处于机器人的可视范围内，机器人在k+2时刻的方位角必须满足式(9)所示的关系。其中robot(k+1)_x和robot(k+1)_y表示机器人在k+1时刻的笛卡尔坐标位置，θ_R(k+1)表示机器人在k+1时刻的方位角，robot(k+2)_x和robot(k+2)_y表示机器人在k+2时刻的笛卡尔坐标位置，θ_R(k+2)表示机器人在k+2时刻的方位角。

$\arctan \frac{\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x}-\mathrm{\θ}\_R(k+1)$ (9)

$=\arctan \frac{\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x}-\mathrm{\θ}\_R(k+2)$

由图5和式(9)可得，时刻k+2时机器人的动态控制模型如为式(10)所示。其中robot(k+2)_x和robot(k+2)_y表示机器人在k+2时刻的位置坐标，θ_R(k+2)表示机器人在k+2时刻的方位角，θ_R(k+1)表示机器人在k+1时刻的方位角。object(k+2)_x_predict和object(k+2)_y_predict表示机器人在k+2时刻的移动目标预测位置。

$\left\{\begin{array}{c}\mathrm{robot}(k+2)\_x=\mathrm{robot}(k+1)\_x\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y)}^2}\end{array}\right)\cos (\mathrm{\θ}\_R(k+2))\\ \mathrm{robot}(k+2)\_y=\mathrm{robot}(k+1)\_y\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y)}^2}\end{array}\right)\sin (\mathrm{\θ}\_R(k+2))\\ \mathrm{\θ}\_R(k+2)=\arctan \frac{\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x}\\ -\arctan \frac{\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x}+\arctan \frac{\mathrm{robot}(k+1)\_y}{\mathrm{robot}(k+1)\_x}\end{array}\right.$ (10)

再次是当各个机器人检测到移动路标时，使用扩展卡尔曼滤波器对移动目标位置进行更新。假设机器人R1对移动目标的位置均值和协方差估计依次为object_position_R1，object_position_uncertainty1，机器人R2对移动目标的位置均值和协方差估计object_position_R2和object_position_uncertainty2。使用协方差插值方法对机器人R1和机器人R2的信息进行融合后的移动目标位置估计协方差为式(11)所示。

(object_position_uncertainty)^-1＝w×(object_position_uncertainty1)^-1

                                                                            (11)

+(1-w)×(object_position_uncertainty2)^-1

使用协方差插值方法对机器人R1和机器人R2的信息进行融合后的移动目标位置估计均值为式(12)所示。

$\mathrm{object}\_\mathrm{position}=\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}$

$\×\left(\begin{array}{c}w\×{(\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}\_1)}^{-1}\×\mathrm{object}\_\mathrm{position}\_1\\ +(1-w)\×{(\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}\_2)}^{-1}\×\mathrm{object}\_\mathrm{position}\_2\end{array}\right)---\left(12\right)$

w的取值如式(13)所示。式(13)中t1表示object_position_uncertainty1矩阵的迹，t2表示object_position_uncertainty1矩阵的迹。

$w=\frac{t1}{t1+t2}---\left(13\right)$

图6描述的是机器人R1和机器人R2对移动目标进行跟踪的测试环境。图7、图8、图9以及图10为使用本发明算法得到的测试结果。图7为机器人R1对移动目标进行跟踪后机器人的运动轨迹，图8为机器人R2对移动目标进行跟踪后机器人的运动轨迹，图9为机器人R1和R2估计得到的移动目标的运动轨迹，图10为移动目标的估计运动轨迹和实际运动轨迹之间的误差。可以看出，使用本发明得到的移动目标估计轨迹和实际轨迹的误差不大，因此，本发明可以很好的实现多机器人对移动目标的跟踪。

Claims (4)

1.一种新的多机器人跟踪移动目标算法，包括以下步骤：

1)使用移动目标的当前位置信息和前一时刻的位置信息构造移动目标的运动预测模型；

2)使用移动目标的运动预测模型的移动目标预测位置构造动态的机器人运动控制模型；

3)使用移动机器人对移动目标的测量信息更新移动目标的位置信息，同时使用协方差插值方法融合各个机器人对移动目标位置的估计，以得到最终的移动目标位置。

2.如权利要求1所述的方法，其特征在于：在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离小于等于之前相邻时刻间移动目标位置间的笛卡尔距离时，移动目标的运动预测模型为式(3)所示，否则，移动目标的运动预测模型为式(4)所示：

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=\mathrm{object}(k+1)\_x\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\cos (\mathrm{\θ}\_B)\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=\mathrm{object}(k+1)\_y\\ +\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2}\sin (\mathrm{\θ}\_B)\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}\mathrm{object}(k+2)\_x\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x\\ \mathrm{object}(k+2)\_y\_\mathrm{predict}=2\×\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y\end{array}\right.---\left(4\right)$

其中θ_B如式(5)所示：

θ_B＝2×π-θ1-θ2-θ3-θ4                        (5)

θ1，θ2，θ3和θ4分别为式(6)，(7)，(8)所示：

$\mathrm{\θ}1=\mathrm{\θ}2=\arccos \frac{\sqrt{(\mathrm{robot}(k+1)\_x^2+\mathrm{robot}(k+1)\_y^2)}\×\sin (\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\arctan \frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}---\left(6\right)$

$\mathrm{\θ\θ}3=\arccos \frac{\sqrt{({(\mathrm{object}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_x)}^2)}-\sqrt{({(\mathrm{object}\left(k\right)\_x)}^2+{(\mathrm{object}\left(k\right)\_y)}^2)}*\cos \×\left(\arctan (\frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}-\frac{\mathrm{object}\left(k\right)\_y}{\mathrm{object}\left(k\right)\_x})\right)}{\sqrt{{(\mathrm{object}(k+1)\_y-\mathrm{object}\left(k\right)\_y)}^2+{(\mathrm{object}(k+1)\_x-\mathrm{object}\left(k\right)\_x)}^2}}---\left(7\right)$

$\mathrm{\θ}4=\mathrm{\π}-\arctan \frac{\mathrm{object}(k+1)\_y}{\mathrm{object}(k+1)\_x}---\left(8\right)$

其中object(k+1)_x和object(k+1)_y表示移动目标在k+1时刻的笛卡尔坐标；object(k)_x和object(k)_y表示移动目标在k时刻的笛卡尔坐标；robot(k+1)_x和robot(k+1)_y表示机器人在k+1时刻的笛卡尔坐标；robot(k+2)_x和robot(k+2)_y表示机器人在k+2时刻的笛卡尔坐标。θR(k+2)表示机器人在k+2时刻的方位角，θ_R(k+1)表示机器人在k+1时刻的方位角；object(k+2)_x_predict和object(k+2)_y_predict表示机器人在k+2时刻的移动目标预测位置。

3.如权利要求1所述的方法，其特征在于：根据移动目标的运动预测模型式(3)或式(4)对移动目标位置进行预测后，机器人的运动控制模型如式(9)所示：

$\left\{\begin{array}{c}\mathrm{robot}(k+2)\_x=\mathrm{robot}(k+1)\_x\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y)}^2}\end{array}\right)\cos (\mathrm{\θ}\_R(k+2))\\ \mathrm{robot}(k+2)\_y=\mathrm{robot}(k+1)\_y\left(\begin{array}{}\end{array}\right)\\ +\left(\begin{array}{c}\sqrt{{(\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y)}^2}\\ -\sqrt{{(\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x)}^2+{(\mathrm{object}(k+1)\_y-\mathrm{robbot}(k+1)\_y)}^2}\end{array}\right)\sin (\mathrm{\θ}\_R(k+2))\\ \mathrm{\θ}\_R(k+2)=\arctan \frac{\mathrm{object}(k+2)\_y\_\mathrm{predict}-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+2)\_x\_\mathrm{predict}-\mathrm{robot}(k+1)\_x}\\ -\arctan \frac{\mathrm{object}(k+1)\_y-\mathrm{robot}(k+1)\_y}{\mathrm{object}(k+1)\_x-\mathrm{robot}(k+1)\_x}+\arctan \frac{\mathrm{robot}(k+1)\_y}{\mathrm{robot}(k+1)\_x}\end{array}\right.$                 (9)

4.如权利要求1所述的方法，其特征在于：使用协方差插值方法进行目标位置协方差信息融合的方法如式(10)所示，目标位置均值信息融合的方法如式(11)所示：

(object_position_uncertainty)^-1＝w×(objeect_position_uncertainty1)^-1

                                                                        (10)

+(1-w)×(object_position_uncertainty2)^-1

$\mathrm{object}\_\mathrm{position}=\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}$

$\×\left(\begin{array}{c}w\×{(\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}\_1)}^{-1}\×\mathrm{object}\_\mathrm{position}\_1\\ +(1-w)\×{(\mathrm{object}\_\mathrm{position}\_\mathrm{uncertainty}\_2)}^{-1}\×\mathrm{object}\_\mathrm{position}\_2\end{array}\right)---\left(11\right)$

其中object_position_R1和object_position_uncertainty1为机器人R1对移动目标的位置均值和协方差估计，object_position_R2和object_position_uncertainty2为机器人R2对移动目标的位置均值和协方差估计，w的值如式(12)所示。

$w=\frac{t1}{t1+t2}---\left(12\right)$

其中t1表示object_position_uncertainty1矩阵的迹，t2表示object_position_uncertainty1矩阵的迹。

Images