CN103760908A - 一种巡视器闭环跟踪控制方法 - Google Patents

Abstract

一种巡视器闭环跟踪控制方法，巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；本发明通过一次性给定闭环控制目标点并定周期执行位置控制，巡视器即可实现持续朝向目标点运动；通过一次性给定闭环控制目标点及期望偏航姿态并定周期执行位置与航向闭环控制，巡视器即可实现持续朝向目标点运动，且最终以期望的偏航方向接近目标点；通过定周期执行闭环跟踪目标点选取和位置与航向闭环控制，巡视器即可实现持续跟踪期望的规划路径。

Description

一种巡视器闭环跟踪控制方法

技术领域

本发明提出了一种闭环跟踪控制方法，对路径跟踪控制、点对点控制、方位角受控的点对点控制进行了整合，使用同一组控制方程，通过对控制输入项及控制参数进行调节，可以分别实现位置闭环控制、位置及航向闭环控制、路径跟踪控制等不同的控制目标。本方法可以推广应用于一般遥控式漫游车辆的闭环跟踪控制。

背景技术

巡视器具有四轮转向、六轮驱动的摇臂悬架式底盘结构，可以被动适应自然起伏地形，因此属于具有非完整约束的轮式移动机器人。根据控制目标的不同，非完整系统的反馈控制问题大致可分为三类：点镇定、路径跟踪、轨迹跟踪。

点镇定即控制机器人到达运动平面上的任意给定的某个目标点，并且能够稳定在该目标点；路径跟踪，即控制机器人能够到达并最终以给定的速度跟踪运动平面上给定的某条路径；轨迹跟踪，即控制机器人能够到达并最终以给定的速度跟踪运动平面上给定的某条轨迹。简单来说，后两者的区别是路径跟踪只要求机器人能够跟踪已经规划好的路径，对何时到达何点不作要求，而轨迹跟踪要求机器人实时跟踪按时间变化的参考轨迹。

对于巡视器而言，由于滑移的影响，巡视器的速度并不是精确可控的，而且巡视器的任务需求也并不要求实现轨迹跟踪，因此，仅需考虑巡视器的点镇定控制和路径跟踪控制即可。一般认为，对点镇定问题和路径跟踪问题，需要设计不同的控制器来予以解决，因为点镇定属于镇定控制，路径跟踪属于跟踪控制，是不能通过一种控制律来解决的。但在工程实践中，这样会增加控制算法设计的复杂性，占用系统资源，不利于提高移动机器人系统的效率。事实上，只需经过必要的整合，是可以采用统一的控制律来同时满足巡视器目标点控制和路径跟踪控制的需求的。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种轮式机器人（例如巡视器）的闭环跟踪控制方法。

本发明的技术解决方案是：一种巡视器闭环跟踪控制方法，步骤如下：巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；其中，位置及航向闭环控制步骤如下：

（1）从解析的指令内容中获取闭环控制目标点的位置及偏航方向(x_t,y_t,ψ_t)；

（2）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（3）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（4）；

（4）根据步骤（2）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行；

位置闭环控制步骤如下：

（1）从解析的指令内容中获取闭环控制目标点的位置(x_t,y_t)；

（2）根据(x_t,y_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t；

（3）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（4）；

（4）根据步骤（2）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行；

路径跟踪控制步骤如下：

（1）从解析的指令内容中获取期望的运动曲率

及期望的运动里程l_ep；

（2）判断巡视器从起始时刻至当前位置的运动总里程l_c是否大于或等于l_ep，若是，则本次控制结束；否则根据上述运动曲率确定当前控制周期的跟踪目标点作为当前闭环控制目标点，其位置及偏航方向记为(x_t,y_t,ψ_t)；

（3）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（4）根据步骤（3）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行。

所述的航向角修正项γ_r计算公式如下：

${\mathrm{\γ}}_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)(\arcsin \frac{R_t}{L_m}-\arcsin \frac{R_t\cos {\mathrm{\γ}}_e}{L_m})$

其中，L_m=max(L,R_t)， $L=\sqrt{R_t^2+e_t^2-{2R}_t{e}_t\left|\sin {\mathrm{\γ}}_e\right|}$

γ_e为期望位置水平投影方向与期望航向角的夹角，R_t为闭环控制运动中最小转弯半径，sgn(γ_e)函数用于获取γ_e的正负符号。

所述的距离修正项e_r的计算公式如下：

$e_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)\min (\arctan \frac{\min (d_{L\min },k_L{L}_m)\sin {\mathrm{\γ}}_e}{L_m},{\mathrm{\θ}}_r)$

其中，L_m=max(L,R_t)， $L=\sqrt{R_t^2+e_t^2-{2R}_t{e}_t\left|\sin {\mathrm{\γ}}_e\right|}$

γ_e为期望位置水平投影方向与期望航向角的夹角，R_t为闭环控制运动中最小转弯半径，sgn(γ_e)函数用于获取γ_e的正负符号；d_Lmin、k_L、θ_r为非负常数。

所述的期望偏航角速度ω_ez计算公式如下：

${\mathrm{\ω}}_{\mathrm{ez}}=\min [k,\frac{v_{\mathrm{ex}}}{\left|{\mathrm{\γ}}_t\right|}(\frac{1}{R_t}-\frac{\left|{\mathrm{\γ}}_t\right|}{e_t})]\·{\mathrm{\γ}}_t+\frac{v_{\mathrm{ex}}{\mathrm{\γ}}_t}{e_t}$

其中，R_t为闭环控制运动中最小转弯半径，k为非负常数，用于平滑行走轨迹，k值越小，则行走的轨迹越平滑。

本发明与现有技术相比有益效果为：

（1）本方法将巡视器闭环跟踪控制问题分解为跟踪目标点选取、位置与航向闭环控制两部分工作，使巡视器的目标点控制和路径跟踪控制需求得到了形式上的统一，在此基础上设计了闭环跟踪控制律，从而使用同一组控制方程，通过对控制输入项及控制参数进行调节，可以分别实现位置控制、位置及航向闭环控制、轨迹跟踪控制等不同的控制目标，在工程应用中取得了良好的效果。

（2）本方法以修正项的形式，将不同的控制需求统一转换为角度形式的被控量，以叠加的方式作为控制系统唯一的状态变量，通过对该状态变量进行控制，实现期望的控制目标，简化了控制律的设计，整合了不同的控制目标。算法简洁高效，灵活性强，适用于各种漫游车辆的闭环控制，具有较好的应用前景。

附图说明

图1为本发明位置及航向闭环控制示意图。

具体实施方式

巡视器的位置闭环控制的目标是控制巡视器自运动起始位置移动到指定目标点，即，使巡视器从(x₀,y₀)运动至(x_t,y_t)，其中的位置坐标描述+X和+Y轴分别指向正北方向和正东方向；巡视器的位置及航向闭环控制的目标是控制巡视器自运动起始位置移动到指定目标点，且到达该目标点时的偏航姿态为指定值，即，使巡视器从(x₀,y₀,ψ₀)运动至(x_t,y_t,ψ_t)，其中的偏航方向ψ为巡视器的前进方向与正北方向的夹角；巡视器的路径跟踪控制的目标是控制巡视器自当前位置沿指定的直线或圆弧型路径移动指定的距离。

为达到同一组控制方程可以实现巡视器的位置闭环控制、位置及航向闭环控制、路径跟踪控制等不同的控制目标，首先对巡视器的闭环控制进行分解，将其分为两个步骤：

1）闭环跟踪目标点选取

对于位置控制和位置及航向闭环控制，巡视器的运动目标点是确定不变的，不需进行闭环跟踪目标点选取工作，而对于路径跟踪控制，需在每个控制周期内实时选取当前周期的控制目标点，以达到与位置控制或位置及航向闭环控制形式上的一致。

记巡视器运动起始时刻的位置及偏航方向为(x₀,y₀,ψ₀)，期望的运动曲率为

巡视器自起始时刻至当前时刻的运动总里程（即弧长）为l_c，则：

若跟踪路径为直线，即

则选取当前周期的跟踪目标点(x_t,y_t,ψ_t)为：

x_t=x₀+(l_c+l_t)cosψ₀

y_t=y₀+(l_c+l_t)sinψ₀

ψ_t=ψ₀

若跟踪路径为弧形路径，则选取当前周期的跟踪目标点(x_t,y_t,ψ_t)为：

$x_t=x_0-\frac{\sin {\mathrm{\ψ}}_0-\sin [{\mathrm{\ψ}}_0+(l_c+l_t){\tilde{C}}_{\mathrm{ep}}]}{{\tilde{C}}_{\mathrm{ep}}}$

$y_t=y_0+\frac{{\cos \mathrm{\ψ}}_0-\cos [{\mathrm{\ψ}}_0+(l_c+l_t){\tilde{C}}_{\mathrm{ep}}]}{{\tilde{C}}_{\mathrm{ep}}}$

其中，l_t为路径跟踪的预瞄距离，可结合巡视器的实际运动能力调节。

经过闭环跟踪目标点选取后，路径跟踪的控制目标或者说控制输入与位置与航向闭环控制在形式上取得了统一，即每一个控制周期内的控制目标均为控制巡视器自当前位置（及航向）朝向目标位置（及航向）运动，即，使巡视器从(x_c,y_c,ψ_c)朝向(x_t,y_t,ψ_t)运动。

2）位置与航向闭环控制

如图1所示，位置与航向闭环控制以巡视器到达期望位置且同时航向角达到或接近期望航向为最终控制目标，巡视器不以抵达目标点为唯一控制目标，而是需要最终使巡视器逐步运动至过目标点并沿目标偏航角与目标点相切的弧形轨迹上，然后沿该弧形轨迹向目标点运动。

为此设计一种状态反馈控制律，选取的状态向量为(e_t,γ_t)，其中e_t为巡视器当前位置与期望位置的水平投影距离，

而γ_t由三部分叠加组成：

γ_t=α_t-γ_r-e_r

其中α_t为航向偏差，即期望位置水平投影方向（即期望位置与当前位置的连线方向）与巡视器当前航向角的夹角，

用于控制实现巡视器朝向目标点行走，为保证闭环控制系统的收敛性，需满足

即目标点不能位于巡视器的后方；

γ_r为偏航角修正项，通过对γ_r施加控制可以对巡视器的运动方向进行修正，使巡视器最终沿期望偏航方向接近目标点，其计算公式为：

${\mathrm{\γ}}_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)(\arcsin \frac{R_t}{L_m}-\arcsin \frac{R_t\cos {\mathrm{\γ}}_e}{L_m})$

其中

L_m=max(L,R_t)

$L=\sqrt{R_t^2+e_t^2-{2R}_t{e}_t\left|\sin {\mathrm{\γ}}_e\right|}$

γ_e为期望位置水平投影方向与期望航向角的夹角，R_t为闭环控制运动中最小转弯半径设定值，可结合跟踪路径的期望曲率确定，一般小于期望的运动曲率

对应的半径，sgn(γ_e)函数用于获取γ_e的正负符号；

e_r为距离修正项，通过对e_r施加控制可以在距目标点较远时提前对巡视器的运动方向进行调整，从而降低临近目标点时的γ_r值，使到达目标点时的运动轨迹尽可能平滑，其计算公式为：

$e_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)\min (\arctan \frac{\min (d_{L\min },k_L{L}_m)\sin {\mathrm{\γ}}_e}{L_m},{\mathrm{\θ}}_r)$

其中，d_Lmin、k_L、θ_r为非负常数，用于调节距离修正幅度，一般来讲d_Lmin不大于R_t，k_L为（0，1）之间的小数，θ_r不大于0.17（即10°）。

闭环控制输出项为巡视器的期望线速度v_ex和期望偏航角速度ω_ez，线速度方向指向巡视器的前进方向，偏航角速度的极性定义为右转为正，左转为负，根据当前控制周期内计算得到的状态量(e_t,γ_t)，令巡视器的期望线速度和期望偏航角速度为：

v_ex=v_x0

${\mathrm{\ω}}_{\mathrm{ez}}=\min [k,\frac{v_{\mathrm{ex}}}{\left|{\mathrm{\γ}}_t\right|}(\frac{1}{R_t}-\frac{\left|{\mathrm{\γ}}_t\right|}{e_t})]\·{\mathrm{\γ}}_t+\frac{v_{\mathrm{ex}}{\mathrm{\γ}}_t}{e_t}$

其中，k为非负常数，用于平滑行走轨迹，k值越小，则行走的轨迹越平滑，一般取（0，1）之间的小数；v_x0为默认线速度，为保证闭环控制系统的收敛性，需满足 $\frac{\left|e_t\right|}{2\sin \left|{\mathrm{\γ}}_t\right|}-R_t\≥0.$

对于位置控制，只需相应地将状态变量γ_t的后两项置零，即取γ_t=α_t即可。

结合上述分析，本发明一种巡视器闭环跟踪控制方法，具体步骤如下：巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；其中，位置及航向闭环控制步骤如下：

（1）从解析的指令内容中获取闭环控制目标点的位置及偏航方向(x_t,y_t,ψ_t)；

（2）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（3）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（4）；

（4）根据步骤（2）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行；

位置闭环控制步骤如下：

（a）从解析的指令内容中获取闭环控制目标点的位置(x_t,y_t)；

（b）根据(x_t,y_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t；

（c）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（d）；

（d）根据步骤（b）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（b）循环执行；

路径跟踪控制步骤如下：

（A）从解析的指令内容中获取期望的运动曲率

及期望的运动里程l_ep；

（B）判断巡视器从起始时刻至当前位置的运动总里程l_c是否大于或等于l_ep，若是，则本次控制结束；否则根据上述运动曲率确定当前控制周期的跟踪目标点作为当前闭环控制目标点，其位置及偏航方向记为(x_t,y_t,ψ_t)；

（C）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（D）根据步骤（C）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（B）循环执行。

综上，通过一次性给定闭环控制目标点并定周期执行位置控制，巡视器即可实现持续朝向目标点运动；通过一次性给定闭环控制目标点及期望偏航姿态并定周期执行位置与航向闭环控制，巡视器即可实现持续朝向目标点运动，且最终以期望的偏航方向接近目标点；通过定周期执行闭环跟踪目标点选取和位置与航向闭环控制，巡视器即可实现持续跟踪期望的规划路径。

本发明未详细说明部分属于本领域技术人员公知常识。

Claims (4)

1.一种巡视器闭环跟踪控制方法，其特征在于步骤如下：巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；其中，位置及航向闭环控制步骤如下：

（1）从解析的指令内容中获取闭环控制目标点的位置及偏航方向(x_t,y_t,ψ_t)；

（2）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（3）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（4）；

（4）根据步骤（2）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行；

位置闭环控制步骤如下：

（1）从解析的指令内容中获取闭环控制目标点的位置(x_t,y_t)；

（2）根据(x_t,y_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t；

（3）判断e_t是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤（4）；

（4）根据步骤（2）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行；

路径跟踪控制步骤如下：

（1）从解析的指令内容中获取期望的运动曲率及期望的运动里程l_ep；

（2）判断巡视器从起始时刻至当前位置的运动总里程l_c是否大于或等于l_ep，若是，则本次控制结束；否则根据上述运动曲率确定当前控制周期的跟踪目标点作为当前闭环控制目标点，其位置及偏航方向记为(x_t,y_t,ψ_t)；

（3）根据(x_t,y_t,ψ_t)以及巡视器当前位置及航向(x_c,y_c,ψ_c)，计算巡视器当前位置与期望位置的水平投影距离e_t、航向偏差α_t以及航向角修正项γ_r、距离修正项e_r；将e_t,γ_t作为闭环控制中的状态向量，其中γ_t=α_t-γ_r-e_r；

（4）根据步骤（3）中确定的状态向量e_t,γ_t，计算巡视器当前控制周期的期望线速度v_ex和期望偏航角速度ω_ez；控制巡视器按照计算的期望线速度v_ex和期望偏航角速度ω_ez运动；进入下一控制周期时转步骤（2）循环执行。

2.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于：所述的航向角修正项γ_r计算公式如下：

${\mathrm{\γ}}_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)(\arcsin \frac{R_t}{L_m}-\arcsin \frac{R_t\cos {\mathrm{\γ}}_e}{L_m})$

其中，L_m=max(L,R_t)， $L=\sqrt{R_t^2+e_t^2-{2R}_t{e}_t\left|\sin {\mathrm{\γ}}_e\right|}$

γ_e为期望位置水平投影方向与期望航向角的夹角，R_t为闭环控制运动中最小转弯半径，sgn(γ_e)函数用于获取γ_e的正负符号。

3.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于：所述的距离修正项e_r的计算公式如下：

$e_r=\mathrm{sgn}\left({\mathrm{\γ}}_e\right)\min (\arctan \frac{\min (d_{L\min },k_L{L}_m)\sin {\mathrm{\γ}}_e}{L_m},{\mathrm{\θ}}_r)$

其中，L_m=max(L,R_t)， $L=\sqrt{R_t^2+e_t^2-{2R}_t{e}_t\left|\sin {\mathrm{\γ}}_e\right|}$

γ_e为期望位置水平投影方向与期望航向角的夹角，R_t为闭环控制运动中最小转弯半径，sgn(γ_e)函数用于获取γ_e的正负符号；d_Lmin、k_L、θ_r为非负常数。

4.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于：所述的期望偏航角速度ω_ez计算公式如下：

${\mathrm{\ω}}_{\mathrm{ez}}=\min [k,\frac{v_{\mathrm{ex}}}{\left|{\mathrm{\γ}}_t\right|}(\frac{1}{R_t}-\frac{\left|{\mathrm{\γ}}_t\right|}{e_t})]\·{\mathrm{\γ}}_t+\frac{v_{\mathrm{ex}}{\mathrm{\γ}}_t}{e_t}$

其中，R_t为闭环控制运动中最小转弯半径，k为非负常数，用于平滑行走轨迹，k值越小，则行走的轨迹越平滑。