CN109992009A - 一种基于距离量测的移动目标环绕跟踪方法 - Google Patents

Abstract

本发明提出一种基于距离量测的移动目标环绕跟踪方法，属于机器人目标跟踪领域。首先在平面坐标系下建立移动机器人的动力学模型，然后以待跟踪目标为圆心建立极坐标系，将移动机器人的动力学模型从平面坐标转换到极坐标系下，最后基于距离量测设计控制量，控制移动机器人实现对移动目标的环绕跟踪。本发明的方法仅需要距离量测作为反馈变量，设计简单，可以在不限制移动机器人初始状态的情况下保证控制方法的全局稳定性，并且控制参数的设计不依赖移动机器人的初始位置。本发明的方法可以对移动目标进行有效跟踪，可靠性高，适合工程应用。

Description

一种基于距离量测的移动目标环绕跟踪方法

技术领域

本发明涉及一种基于距离量测的移动目标环绕跟踪方法，属于机器人目标跟踪领域。

背景技术

伴随着移动机器人技术的快速发展，通过移动机器人进行目标跟踪在军用和民用领域获得了广泛的应用，比如边境巡防、运输护航和救灾防护等。

其中环绕跟踪是一种特殊的跟踪方式：移动机器人在跟踪过程中需要保持与目标的距离不变。对于固定目标而言，机器人的期望轨迹是以目标为圆心以指定距离为半径的圆形。

当利用移动机器人进行目标跟踪时，在理想情况下，移动机器人和待跟踪目标的状态均可获得。这时可以通过李雅普诺夫制导向量方法或者几何方法设计制导指令，从而实现对目标的环绕跟踪。

在通常情况下，移动移动机器人的状态可以通过自身携带的传感器直接获得，但是待跟踪目标的状态并不能直接获得，比如待跟踪目标是入侵的敌人。这时，可以通过移动机器人携带传感器(相机、雷达等)的量测信息设计估计方法(例如扩展卡尔曼滤波、粒子滤波等)对待跟踪目标状态进行估计，然后，再采用李雅普诺夫制导方法引导移动机器人到达期望的轨道。

在恶劣情况下，例如水下或者隧道等环境，移动机器人无法获得自身的状态信息。这种情况下移动机器人不能对待跟踪目标的状态进行估计，因此先估计目标状态再设计控制算法的方法行不通。这时，研究基于距离量测的控制方法显得至关重要。

然而，基于距离量测的目标跟踪方法面临诸多挑战：1)反馈状态有限，只有距离量测可以使用；2)移动机器人系统存在比较强的非线性，移动机器人与待跟踪目标之间的距离状态需要通过移动机器人的航向进行间接控制；3)移动机器人系统存在很多的不确定性，比如噪声、风扰等。

目前针对目标环绕跟踪的控制方法大多存在缺陷。文献《Range-onlymeasurements based target following for wheeled mobile robots》设计了一种基于滑模控制的移动目标环绕跟踪方法，该方法的控制输出在最大值与最小值之间切换，这会在实际应用中带来“抖振”问题，并且该方法在环绕跟踪静态目标时存在稳态误差。此外，该方法还需要限制移动机器人的起始状态。

发明内容

本发明的目的是克服现有技术的不足之处，提出一种基于距离量测的移动目标环绕跟踪方法。本发明的方法只采用距离量测作为反馈信息就可以保证控制方法的全局稳定性，不需要限制机器人的初始状态，此外控制参数的设计也不依赖移动机器人的初始位置，可实现在移动机器人自身状态未知导致无法估计目标位置的情况下，实现对目标环绕跟踪。

本发明提出的一种基于距离量测的移动目标环绕跟踪方法，其特征在于，包括以下步骤：

(1)在平面坐标系下建立移动机器人的动力学模型：

其中，p(t)＝[x(t)；y(t)]表示移动机器人的在t时刻的位置，表示t时刻p(t)的变化率，θ(t)表示移动机器人的在t时刻的航向角，u(t)表示移动机器人的在t时刻的控制输入，v_c表示移动机器人的线速度，x(t),y(t)分别表示移动机器人t时刻在x轴和y轴的坐标；

(2)以待跟踪目标为圆心建立极坐标系，则移动机器人和待跟踪目标之间的动力学模型表示如下：

其中，是机器人t时刻的角速度，d(t)是t时刻移动机器人到待跟踪目标的距离，是t时刻d(t)的变化率，φ(t)是t时刻待跟踪目标指向移动机器人的方向与机器人航向之间的夹角，是t时刻φ(t)的变化率；

d(t)计算表达式如下：

d(t)＝||p(t)-p_o(t)||₂，

式中，p_o(t)＝[x_o(t)；y_o(t)]表示t时刻待跟踪目标的位置，其中x_o(t),y_o(t)分别表示t时刻待跟踪目标在x轴和y轴的坐标；

(3)令移动机器人开始对待跟踪目标进行跟踪，记当前时刻为t；

(4)在t时刻，通过如下控制方法获得控制变量u(t)：

其中，u_c＝v_c/r_c是系统误差为零时的偏置信号，r_c表示期望的环绕半径，k_i＞0,i＝1,2,3是控制参数；表示t时刻移动机器人和目标之间距离的变化率；

sat(·)表示饱和函数，定义如下：

(5)将步骤(4)获得的控制变量u(t)施加到步骤(1)的移动机器人模型上，移动机器人进行相应运动至t+1时刻；

(6)令t＝t+1,然后重新返回步骤(4)，实现移动机器人对目标的环绕跟踪。

本发明的特点及有益效果在于：

(1)本发明仅需要距离量测作为反馈状态就可以实现对移动目标的环绕跟踪，不需要移动机器人本身的位置、速度等信息，该方法适用范围广，特别是水下或者隧道等无法采用GPS定位的环境；

(2)本发明无需限制移动机器人的初始状态，可以保证控制算法的全局稳定性，无论机器人起始状态如何，都可以快速到达期望的轨道上，该方法设计简单，稳定性高；

(3)本发明涉及的控制参数不依赖移动机器人的初始状态，不需要根据移动机器人的初始位置调整控制参数，非常适合实际工程应用。

附图说明

图1为本发明实施例中移动机器人在极坐标系下运动的示意图。

图2为本发明实施例中移动机器人不同起始状态时的运动示意图。

图3为本发明实施例中移动机器人跟踪固定目标时距离的变化和角度的变化示意图。

图4为本发明实施例中移动机器人跟踪移动目标时的轨迹示意图。

图5为本发明实施例中移动机器人跟踪移动移动目标时的跟踪误差示意图。

具体实施方式

本发明提出的一种基于距离量测的移动目标环绕跟踪方法，下面结合附图和具体实施例进一步详细说明。

本发明提出的一种基于距离量测的移动目标环绕跟踪方法，首先在平面坐标系下建立移动机器人的动力学模型，然后以待跟踪目标为圆心建立极坐标系，将移动机器人的动力学模型从平面坐标转换到极坐标系下，最后基于距离量测设计控制量，控制移动机器人实现对移动目标的环绕跟踪。本发明提出的方法仅采用距离量测作为反馈变量就可以实现对移动目标的环绕跟踪，不需要限制移动机器人的初始状态，也不需要根据移动机器人初始位置调整控制参数。

本发明提出的一种基于距离量测的移动目标环绕跟踪方法，包括以下步骤：

(1)在平面坐标系下建立移动机器人的动力学模型：

其中，p(t)＝[x(t)；y(t)]表示移动机器人的在t时刻的位置，表示t时刻p(t)的变化率，θ(t)表示移动机器人的在t时刻的航向角，u(t)表示移动机器人的在t时刻的控制输入，v_c表示移动机器人的线速度；x(t),y(t)分别表示移动机器人t时刻在x轴和y轴的坐标。本模型是通用的移动机器人模型，不限制移动机器人的体积、重量。

(2)以待跟踪目标(待跟踪目标的线速度应小于移动机器人的线速度，待跟踪目标也可以是静止的)为圆心建立极坐标系如图1所示，则移动机器人和待跟踪目标之间的动力学模型可以如下表示：

其中，是机器人t时刻的角速度，d(t)是t时刻移动机器人到待跟踪目标的距离，是t时刻距离d(t)的变化率，φ(t)是t时刻待跟踪目标指向移动机器人的方向与机器人航向之间的夹角(参考图1)，定义逆时针方向为正方向，是t时刻角度φ(t)的变化率。d(t)＝0意味着移动机器人与待跟踪目标完全重合。此外，t时刻的距离d(t)可以通过移动机器人自身携带的传感器测量得到，其计算方式为：

d(t)＝||p(t)-p_o(t)||₂，

式中，p_o(t)＝[x_o(t)；y_o(t)]表示t时刻待跟踪目标的位置，其中x_o(t),y_o(t)分别表示t时刻待跟踪目标在x轴和y轴的坐标。图1中坐标原点O表示待跟踪目标，r_c表示期望的环绕半径，角度η(t)＝φ(t)-θ(t)。

(3)令移动机器人开始对待跟踪目标进行跟踪，记当前时刻为t；本发明对机器人和目标的运动状态均无特殊设定。

(4)在t时刻，通过如下控制方法获得控制变量u(t)：

其中，u_c＝v_c/r_c是系统误差为零时的偏置信号，r_c为期望的环绕半径，k_i＞0,i＝1,2,3是需要设计的控制参数，可以根据实际应用中的具体机器人型号进行调整(本实施例中k₁＝18，k₂＝0.25，k₃＝1.5)。d(t)和分别表示t时刻移动机器人和目标之间距离和距离的变化率，均可以由移动机器人携带的传感器测量得到。此外sat(·)表示饱和函数，用来解决控制参数需要根据机器人初始状态设计的问题，其定义如下：

其中的sgn(·)表示标准符号函数，变量z无实际意义。

(5)将步骤(4)获得的控制变量u(t)施加到步骤(1)的移动机器人模型上，移动机器人进行相应运动至t+1时刻；在每个时刻，移动机器人会根据当前的u(t)相应改变自身的运动状态。

(6)令t＝t+1,然后重新返回步骤(4)，实现移动机器人对目标的环绕跟踪。

本发明方法所采用的移动机器人和待跟踪目标均为常规型号设备；本发明方法本领域技术人员通过编程即可实现。

下面结合一个具体实施例对本发明进一步说明如下。

仿真实验

(1)仿真设置

设定移动机器人的线速度为v_c＝0.5m/s，期望的环绕半径为r_c＝2m，采样间隔为0.01s，控制参数分别为：k₁＝18，k₂＝0.25，k₃＝1.5。为了方便表示，我们定义s(t)＝[p(t)；θ(t)]代表移动机器人的状态变量。

(2)仿真结果

首先选定固定目标以测试控制方法的全局稳定性。不失一般性，将待跟踪目标位置设定为p_o(t)＝[2；2]。我们选定八种不同的起始状态(s(0)分别取[7；2；-0.6π]，[2；7；0.5π]，[-3；2；π]，[2；-3；-0.5π]，[2；2；0]，[2；2；0.5π]，[2；2；π]，[2；2；-0.5π])来测试控制方法的可靠性，测试结果如图2所示。图2中的小正方形表示移动机器人的起始位置，箭头表示移动机器人的起始航向。从结果可以看出，无论移动机器人初始状态如何，最终都会到达以待跟踪目标为圆心以r_c为半径的圆形轨道上。

以移动机器人初始状态s(0)＝[7；2；-0.6π]为例进行跟踪效果的分析，移动机器人与待跟踪目标之间距离的变化情况如图3(a)所示，从图3(a)可以发现实际距离d(t)随时间收敛到期望距离r_c。从图3(b)可以看出，角度φ(t)最终会收敛于π/2(图中虚线表示的参考角度即为π/2)。

其次，我们设定待跟踪目标为线速度0.15m/s、角速度0.01rad/s的移动目标，实验结果如图4所示。图中菱形和正方形分别表示待跟踪目标和移动机器人的起始位置，箭头表示移动机器人的起始航向。移动机器人的跟踪误差如图5所示，从图中可以看出，跟踪误差小于0.02m。

本发明可以仅用距离量测作为反馈变量就可以实现对移动目标的环绕跟踪，并且可以在不限制移动机器人初始状态的情况下保证控制方法的全局稳定性。此外，控制参数的设定不依赖于移动机器人的初始位置，设计方法简单，比较适合工程应用。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于距离量测的移动目标环绕跟踪方法，其特征在于，包括以下步骤：

(1)在平面坐标系下建立移动机器人的动力学模型：

其中，p(t)＝[x(t)；y(t)]表示移动机器人的在t时刻的位置，表示t时刻p(t)的变化率，θ(t)表示移动机器人的在t时刻的航向角，u(t)表示移动机器人的在t时刻的控制输入，v_c表示移动机器人的线速度，x(t),y(t)分别表示移动机器人t时刻在x轴和y轴的坐标；

(2)以待跟踪目标为圆心建立极坐标系，则移动机器人和待跟踪目标之间的动力学模型表示如下：

其中，是机器人t时刻的角速度，d(t)是t时刻移动机器人到待跟踪目标的距离，是t时刻d(t)的变化率，φ(t)是t时刻待跟踪目标指向移动机器人的方向与机器人航向之间的夹角，是t时刻φ(t)的变化率；

d(t)计算表达式如下：

d(t)＝||p(t)-p_o(t)||₂，

式中，p_o(t)＝[x_o(t)；y_o(t)]表示t时刻待跟踪目标的位置，其中x_o(t),y_o(t)分别表示t时刻待跟踪目标在x轴和y轴的坐标；

(3)令移动机器人开始对待跟踪目标进行跟踪，记当前时刻为t；

(4)在t时刻，通过如下控制方法获得控制变量u(t)：

其中，u_c＝v_c/r_c是系统误差为零时的偏置信号，r_c表示期望的环绕半径，k_i＞0,i＝1,2,3是控制参数；表示t时刻移动机器人和目标之间距离的变化率；

sat(·)表示饱和函数，定义如下：

(5)将步骤(4)获得的控制变量u(t)施加到步骤(1)的移动机器人模型上，移动机器人进行相应运动至t+1时刻；

(6)令t＝t+1,然后重新返回步骤(4)，实现移动机器人对目标的环绕跟踪。