CN114063653B - 空间与时间一致性约束小型无人机高精度航迹控制方法 - Google Patents

Abstract

本发明涉及空间与时间一致性约束小型无人机高精度航迹控制方法，包括无人机机体、速度控制器、航迹处理器、速度修正器和航向角调整器，所述无人机机体内部设有速度控制器，其速度控制器包含有航迹处理模块、速度修正模块和航向角调整模块，其中航迹处理模块对应设有航迹处理器，速度修正模块对应设有速度修正器，航向角调整模块对应设有航向角调整器，并且航迹处理器、速度修正器和航向角调整器均作用于速度控制器用于调整无人机机体的飞行轨迹、速度和角度。该空间与时间一致性约束小型无人机高精度航迹控制方法在目标轨迹切线方向定义一个虚拟的节点，通过控制无人机跟随该节点，从而使得无人机位于规划轨迹上，同时满足时间要求。

Description

空间与时间一致性约束小型无人机高精度航迹控制方法

技术领域

本发明涉及雷达技术领域，具体为空间与时间一致性约束小型无人机高精度航迹控制方法。

背景技术

为了提高航迹控制的精度，无人机需要尽快调整参数，及时到达预期点，调整参数主要包括速度和方向，频繁的调整运动状态会导致航迹不稳，航迹的平稳和跟踪精度是矛盾的，因此需要优化控制参数，同时做到实际航迹与规划航迹在空间上和时间上的一致。

对雷达的航迹生成中，由不确定性误差导致的估计问题主要包含两大类：一是由于先验信息不足，雷达位置误差对航迹欺骗效果产生影响；二是受到动力学约束、硬件系统误差、控制误差和风力影响等原因导致的无人机位置误差。前者可采用复合定位方法提高雷达的定位精度，后者可采取高精度定位系统、航迹控制优化、偏差补偿措施。航迹控制算法主要为间接控制算法，其设计思路是将航迹控制问题转化为航向、航速控制问题，主要实现方式是将其分解为双闭环控制系统，外环使用航向、航速解算算法，计算得到期望航向和期望航速，内环由航向、航速控制算法构成，根据期望值和实际值的差异，控制无人机快速平稳的实现差值纠正。常规的航迹控制算法只考虑空间上的一致性，没有考虑时间上的一致性，无法满足目标模拟的要求。

发明内容

本发明的目的在于提供空间与时间一致性约束小型无人机高精度航迹控制方法，根据上述问题，尤其涉及用无人机搭载雷达回波模拟器进行目标模拟时，解决无人机实际飞行的航迹与规划设计下无人机航迹存在位置和时间不一致的问题。

为实现上述目的，本发明提供如下技术方案：空间与时间一致性约束小型无人机高精度航迹控制方法，包括无人机机体、速度控制器、航迹处理器、速度修正器和航向角调整器，所述无人机机体内部设有速度控制器，其速度控制器包含有航迹处理模块、速度修正模块和航向角调整模块，其中航迹处理模块对应设有航迹处理器，速度修正模块对应设有速度修正器，航向角调整模块对应设有航向角调整器，并且航迹处理器、速度修正器和航向角调整器均作用于速度控制器用于调整无人机机体的飞行轨迹、速度和角度；

空间与时间一致性约束小型无人机高精度航迹控制方法，具体如下：

S1：根据无人机机体预计航行轨迹和实际位置的关系，控制速度的大小，设定速度控制器；

S2：根据无人机机体预计航行轨迹，设定航迹模块中的航迹处理器的位置信息；

所述航迹处理器的位置信息受风力等干扰因素的影响带来周期性误差、固定偏移误差、系统定位测量误差，速度控制器对实时的干扰因素带来误差进行速度校正，并通过RTK高度精度定位进行精确数据测量；

S3：根据无人机机体预计航行轨迹，并结合设定的位置信息后，设计航迹的速度修正器和航向角调整器的参数；

所述无人机机体预计航行轨迹上分解多个目标点，在每个目标点上计算预期航向和预期速度，其中平滑航行轨迹进行均匀分割，转弯航行轨迹需要提高分割密度；

所述航向角调整器在预计航行轨迹计算目标点时刻的航向角，其中航向角需要结合无人机机体实时的时间和实际的目标点，并且需要根据航迹垂点与前视距离计算得到前视目标点坐标；

S4：根据无人机机体调整航行速度和角度后，再对航向角进行优化设定；

所述航向角优化需要根据前视目标点和期望航向角通过航向角调整器加入控制参数，对航向角进一步优化；

所述航向角进一步优化步骤：

第一步：根据前视距离确定前视目标点；

第二步：加入控制项和控制参数；

第三步：计算得到优化后的航向角；

第四步：通过优化后的航向角控制无人机机体更加快速稳定的跟踪到规划航迹。

优选的，所述无人机机体内部设有信号发射器，其信号发射器采用无线连接方式与控制器连接。

优选的，所述无人机机体的航迹处理器中包含有多个虚拟轨迹节点，其中至少包含有虚拟轨迹节点A、虚拟轨迹节点B、虚拟轨迹节点C和虚拟轨迹节点D。

与现有技术相比，本发明的有益效果是：该空间与时间一致性约束小型无人机高精度航迹控制方法在目标轨迹切线方向定义一个虚拟的节点，通过控制无人机跟随该节点，从而使得无人机位于规划轨迹上，同时满足时间要求。根据当前时刻无人机的实际位置及预期航迹，利用航迹修正算法，计算得到无人机当前时刻预期航向和预期速度，然后将预期速度和实际速度的差值作为速度控制器的输入，利用速度控制器计算速度控制参数，从而实时调整速度，同时调整无人机的航向，从而实现航迹的位置、时间一致性。

附图说明

图1为本发明空间与时间一致性约束小型无人机高精度航迹控制方法控制流程图；

图2为本发明空间与时间一致性约束小型无人机高精度航迹控制方法速度控制器连接关系示意图；

图3为本发明空间与时间一致性约束小型无人机高精度航迹控制方法速度控制示意图；

图4为本发明空间与时间一致性约束小型无人机高精度航迹控制方法无人机控制系统仿真结构示意图；

图5为本发明空间与时间一致性约束小型无人机高精度航迹控制方法无速度控制时距离误差示意图；

图6为本发明空间与时间一致性约束小型无人机高精度航迹控制方法改进后速度控制时距离误差示意图；

图7为本发明空间与时间一致性约束小型无人机高精度航迹控制方法航向角求解示意图。

图中：1、无人机机体，2、速度控制器，3、航迹处理器，4、速度修正器，5、航向角调整器。

具体实施方式

下面将结合本发明以下实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-7，本发明提供一种技术方案：空间与时间一致性约束小型无人机高精度航迹控制方法，包括无人机机体1、速度控制器2、航迹处理器3、速度修正器4和航向角调整器5，无人机机体1内部设有速度控制器2，其速度控制器2包含有航迹处理模块、速度修正模块和航向角调整模块，其中航迹处理模块对应设有航迹处理器3，速度修正模块对应设有速度修正器4，航向角调整模块对应设有航向角调整器5，并且航迹处理器3、速度修正器4和航向角调整器5均作用于速度控制器2用于调整无人机机体1的飞行轨迹、速度和角度，无人机机体1内部设有信号发射器，其信号发射器采用无线连接方式与控制器连接，无人机机体1的航迹处理器3中包含有多个虚拟轨迹节点，其中至少包含有虚拟轨迹节点A、虚拟轨迹节点B、虚拟轨迹节点C和虚拟轨迹节点D，需要额外说明的是，其中虚拟轨迹节点设计越多，其修正调节越精确。

空间与时间一致性约束小型无人机高精度航迹控制方法，具体如下：

S1：根据无人机机体1预计航行轨迹和实际位置的关系，控制速度的大小，设定速度控制器2；

为了保证航迹在时间上也能对齐，需求引入速度控制器2，根据期望位置和实际位置的关系，控制速度的大小，速度控制器2速度控制参数的计算式如下：

离散化后可表示为：

式中，e_v(t)为实际值与期望值的差值，e_v(t-1)为上一次计算时刻的差值，P_v(t)为速度控制的输出值，用于控制无人机机体1调整速度，K_p、K_T、K_d为控制器系数。

S2：根据无人机机体1预计航行轨迹，设定航迹模块中的航迹处理器3的位置信息；

在无人机机体1的运动模型中，速度不能突变，因此运动模型中的控制参数设置为加速度。

约束条件为：

航迹处理器3的位置信息会受到外界干扰，主要包括风力等因素带来的周期性误差、固定偏移误差，数据测量采集主要是系统定位引入位置的测量误差，速度控制器2能够在干扰存在时通过校正速度，保证时间上的一致性，无法消除数据采集测量带来的误差，采用RTK高精度定位后，位置测量误差为0.1m，数据测量采集误差可以忽略。

S3：根据无人机机体1预计航行轨迹，并结合位置信息后，设计航迹的速度修正器4和航向角调整器5的参数；

通过在预计航行轨迹的切线方向上设定前向目标点，引导无人机机体1的飞行，通过前向目标点可以求解无人机机体1在每一个时刻的无人机机体1的预期航向和预期速度，规划航迹起点为(x_d(t₀)，y_d(t₀))，终点为(x_d(t₁)，y_d(t₁))，将规划航迹进行离散化，可以得到每个时刻规划位置和航向，将点(x_d(t₀)，y_d(t₀))到点(x_d(t₁)，y_d(t₁))之间的规划航迹分割为N个点，表示为(x_d(k)，y_d(k))，k＝1，2，…，N，需要考虑航迹特性进行合理分割，平滑部分可以均匀分割采样，转弯部分需要提高分割采样密度。

除了分割规划航迹得到时间和位置，还需要计算航向角，无人机机体1在t时刻的期望航向角α可用公式来求解：

(航向角求解参看附图7)

其中，P₀为无人机机体1的t时刻所在位置，坐标表示为P₀(x(t)，y(t))，A、B为规划轨迹上的采样点，l为前视距离，根据航迹垂点与前视距离l得到前视目标点P，前视目标点P为无人机机体1期望到达的目标点，坐标表示为P(x_P，y_P)，期望到达时刻为t_P，ψ为无人机机体1的t时刻实际航向角。

t时刻的期望速度v可表示为：

S4：根据无人机机体1调整航行速度和角度后，再对航向角进行优化设定；

为了获得更加稳定的航迹跟踪性能，利用前视目标点和期望航向角，加入控制参数，对航向角进行优化，首先根据前视距离确定前视目标点，然后加入控制项和控制参数，得到航向角导引公式如下：

其中，β为规划航迹方向，l为前视距离，y_e为无人机机体1到规划路径的垂直距离，y_eold为上一个控制时刻无人机机体1与规划航迹的偏差，K_d、K_i为控制参数，通过调整控制参数，可以控制无人机机体1能够更加快速稳定的跟踪到规划航迹。

下面结合附图和具体实施对本发明进一步说明如下：

图3为速度控制示意图，根据航迹修正算法计算得到当前时刻的期望速度和实际速度，进而计算两者之间的误差，对误差、误差积分、误差微分项进行参数控制并相加，得到速度控制量，从而控制无人机机体1调整速度，使其能够快速平稳的向预期速度调整。

图4为无人机机体1速度控制系统仿真结构，当存在风力等因素带来的周期性误差、固定偏移误差时，实际位置与预期位置存在偏差，在速度控制器的作用下，无人机机体1实时调整速度，使得无人机机体1朝向降低误差的方向运动。

图5为无速度控制时距离误差，对干扰条件下无人机机体1的飞行航迹进行了仿真，无人机机体1规划航迹为直线飞行，规划飞行时间60s，规划速度为8m/s，无人机机体1实际飞行中，无人机机体1初始速度存在误差，实际位置存在干扰，其中定位误差0.1m，固定偏移误差2m，存在周期性误差最大值3m，变化周期为12.5s，当无速度控制算法时，距离误差随时间的变化如图5所示，从图中可以看出，由于速度不能及时调整，随着时间的积累，无人机机体1预期航点与实际航点之间的距离差越来越大，不能满足实际需求。

图为使用速度控制算法后，距离误差随时间的变化，从图6中可以看出，除去刚开始的调整阶段，在稳定阶段，无人机机体1预期航点与实际航点之间的误差较小，约为0.2m左右，能够满足系统的设计和使用需求。

尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.空间与时间一致性约束小型无人机高精度航迹控制方法，包括无人机机体(1)、速度控制器(2)、航迹处理器(3)、速度修正器(4)和航向角调整器(5)，其特征在于：所述无人机机体(1)内部设有速度控制器(2)，其速度控制器(2)包含有航迹处理模块、速度修正模块和航向角调整模块，其中航迹处理模块对应设有航迹处理器(3)，速度修正模块对应设有速度修正器(4)，航向角调整模块对应设有航向角调整器(5)，并且航迹处理器(3)、速度修正器(4)和航向角调整器(5)均作用于速度控制器(2)用于调整无人机机体(1)的飞行轨迹、速度和角度；

所述空间与时间一致性约束小型无人机高精度航迹控制方法，具体如下：

S1：根据无人机机体(1)预计航行轨迹和实际位置的关系，控制速度的大小，设定速度控制器(2)；

S2：根据无人机机体(1)预计航行轨迹，设定航迹模块中的航迹处理器(3)的位置信息；

所述航迹处理器(3)的位置信息受风力等干扰因素的影响带来周期性误差、固定偏移误差、系统定位测量误差，速度控制器(2)对实时的干扰因素带来误差进行速度校正，并通过RTK高度精度定位进行精确数据测量；

S3：根据无人机机体(1)预计航行轨迹，并结合设定的位置信息后，设计航迹的速度修正器(4)和航向角调整器(5)的参数；

所述无人机机体(1)预计航行轨迹上分解多个目标点，在每个目标点上计算预期航向和预期速度，其中平滑航行轨迹进行均匀分割，转弯航行轨迹需要提高分割密度；

所述航向角调整器(5)在预计航行轨迹计算目标点时刻的航向角，其中航向角需要结合无人机机体(1)实时的时间和实际的目标点，并且需要根据航迹垂点与前视距离计算得到前视目标点坐标；

S4：根据无人机机体(1)调整航行速度和角度后，再对航向角进行优化设定；

所述航向角优化需要根据前视目标点和期望航向角通过航向角调整器(5)加入控制参数，对航向角进一步优化；

所述航向角进一步优化步骤：

第一步：根据前视距离确定前视目标点；

第二步：加入控制项和控制参数；

第三步：计算得到优化后的航向角；

第四步：通过优化后的航向角控制无人机机体(1)更加快速稳定的跟踪到规划航迹。

2.根据权利要求1所述的空间与时间一致性约束小型无人机高精度航迹控制方法，其特征在于：所述无人机机体(1)内部设有信号发射器，其信号发射器采用无线连接方式与控制器连接。

3.根据权利要求1所述的空间与时间一致性约束小型无人机高精度航迹控制方法，其特征在于：所述无人机机体(1)的航迹处理器(3)中包含有多个虚拟轨迹节点，其中至少包含有虚拟轨迹节点A、虚拟轨迹节点B、虚拟轨迹节点C和虚拟轨迹节点D。