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

摘要

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

主权项

一种多机器人跟踪移动目标方法，包括以下步骤：1)使用移动目标的当前位置信息和前一时刻的位置信息构造移动目标的运动预测模型；2)使用移动目标的运动预测模型的移动目标预测位置构造动态的机器人运动控制模型；3)使用移动机器人对移动目标的测量信息更新移动目标的位置信息，同时使用协方差插值方法融合各个机器人对移动目标位置的估计，以得到最终的移动目标位置；其中，在保证移动目标相邻时刻间位置角度差保持一致的条件下，当预测到的移动目标位置和当前时刻移动目标位置之间的笛卡尔距离小于等于之前相邻时刻间移动目标位置间的笛卡尔距离时，移动目标的运动预测模型为式(3)所示，否则，移动目标的运动预测模型为式(4)所示： object ( k + 2 ) _ x _ predict = object ( k + 1 ) _ x + ( object ( k + 1 ) _ x - object ( k ) _ x ) 2 + ( object ( k + 1 ) _ y - object ( k ) _ y ) 2 cos ( θ _ B ) object ( k + 1 ) _ y _ predict = object ( k + 1 ) _ y + ( object ( k + 1 ) _ x - object ( k ) _ x ) 2 + ( object ( k + 1 ) _ y - object ( k ) _ y ) 2 sin ( θ _ B ) - - - ( 3 ) object ( k + 2 ) _ x _ predict = 2 × object ( k + 1 ) _ x - object ( k ) _ x object ( k + 2 ) _ y _ predict = 2 × object ( k + 1 ) _ y - object ( k ) _ y - - - ( 4 ) 其中θ_B表示移动目标k+2时刻的预测位置和x坐标系之间的角度差，如式(5)所示：θ_B＝2×π‑θ1‑θ2‑θ3‑θ4    (5)θ1，θ2，θ3和θ4分别为式(6)，(7)，(8)所示： θ 1 = θ 2 = arccos ( robot ( k + 1 ) _ x 2 + robot ( k + 1 ) _ y 2 ) × sin ( arctan object ( k + 1 ) _ y object ( k + 1 ) _ x - arctan object ( k ) _ y object ( k ) _ x ) ( object ( k + 1 ) _ y - object ( k ) _ y ) 2 + ( object ( k + 1 ) _ x - object ( k ) _ x ) 2 - - - ( 6 ) θ 3 = arccos ( ( object ( k + 1 ) _ x ) 2 + ( object ( k + 1 ) _ x ) 2 ) - ( ( object ( k ) _ x ) 2 + ( object ( k ) _ y ) 2 ) * cos ( arctan ( object ( k + 1 ) _ y object ( k + 1 ) _ x - object ( k ) _ y object ( k ) _ x ) ) ( object ( k + 1 ) _ y - object ( k ) _ y ) 2 + ( object ( k + 1 ) _ x - object ( k ) _ x ) 2 - - - ( 7 ) θ 4 = π - arctan object ( k + 1 ) _ y object ( k + 1 ) _ x - - - ( 8 ) 其中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时刻的笛卡尔坐标；object(k+2)_x_predict和object(k+2)_y_predict表示机器人在k+2时刻的移动目标预测位置；根据移动目标的运动预测模型式(3)或式(4)对移动目标位置进行预测后，机器人的运动控制模型如式(9)所示： robot ( k + 2 ) _ x = robot ( k + 1 ) _ x + ( object ( k + 1 ) _ x _ predict - rob ot ( k + 1 ) _ x ) 2 + ( object ( k + 2 ) _ y _ predict - robot ( k + 1 ) _ y ) 2 - ( object ( k + 1 ) _ x - robot ( k + 1 ) _ x ) 2 + ( object ( k + 1 ) _ y - r obot ( k + 1 ) _ y ) 2 cos ( θ _ R ( k + 2 ) ) robot ( k + 1 ) _ y = robot ( k + 1 _ y + ( object ( k + 2 ) _ x _ predict - robot ( k + 1 ) _ x ) 2 + ( object ( k + 2 ) _ y _ predict ( k + 1 ) _ y ) 2 - ( ibject ( k + 1 ) _ x - robot ( k + 1 ) _ x ) 2 + ( object ( k + 1 ) _ y - robot ( k + 1 ) _ y ) 2 sin ( θ _ R ( k + 2 ) ) θ _ R ( k + 1 ) = arctan object ( k + 1 ) _ y _ predict - robot ( k + 1 ) _ y object ( k + 1 ) _ y _ predict - robot ( k + 1 ) _ x - arctan object ( k + 1 ) _ y - robot ( k + 1 ) _ y object ( k + 1 ) _ x - robot ( k + 1 ) _ x + arctan robot ( k + 1 ) _ y robot ( k + 1 ) _ x (9)，其中θ_R(k+2)表示机器人在k+2时刻的方位角，θ_R(k+1)表示机器人在k+1时刻的方位角。