一种基于曲率补偿的横侧向曲线航迹跟踪方法

摘要

本发明公开了一种基于曲率补偿的横侧向曲线航迹跟踪方法，属于航迹跟踪控制技术领域。本发明首先通过航迹规划模块得到参考航迹的离散航路点数据；然后通过拉格朗日分段多项式插值方法，对所得离散航路点数据进行分段插值拟合处理，得到曲线航迹；计算无人机当前时刻侧偏距，以及参考航迹上距飞机最近点坐标，计算所得参考航迹上距飞机最近点处曲率值。将所得侧偏距和曲率值作为无人机横侧向控制器的输入量，完成整个曲线航迹跟踪过程。本发明通过对参考航迹进行曲线插值拟合处理，使跟踪过程中航路点的过渡段更加平滑，跟踪误差减小，从而更好地跟踪航迹。

主权项

一种基于曲率补偿的横侧向曲线航迹跟踪方法，其特征在于，包括如下步骤：步骤一：通过航迹规划模块得到参考航迹的离散航路点数据；步骤二：通过拉格朗日分段多项式插值方法，对步骤一所得离散航路点数据进行分段插值拟合处理，得到曲线航迹；设航迹规划模块所得航路点信息为P₁，P₂,……P_n，航路点个数为n，则对航路点数据进行分段二次多项式插值的处理过程，具体为：(1)每当无人机到达一个航路点，设该航路点为P_count；(2)如果count≤n‑2，则利用P_count,P_count+1,P_count+2三个航路点数据进行二次多项式插值；否则结束，完成整条航迹的跟踪；(3)如果是count＝n‑2，则绘制P_count‑P_count+2段拟合航路；否则绘制P_count‑P_count+1航路段曲线航迹，并将该曲线航迹作为参考航迹；(4)令count＝count+1，返回步骤(2)；步骤三：通过推导点到曲线最短距离的计算公式，计算无人机当前时刻侧偏距，以及参考航迹上距飞机最近点坐标；步骤四：通过曲率计算公式，计算步骤三所得参考航迹上距飞机最近点处曲率值；将步骤二中所得拉格朗日二次多项式方程表示为L₂(x)＝p₁x²+p₂x+p₃，p₁，p₂，p₃分别是多项式的各项系数，对其求导得到L₂'(x)＝2p₁x+p₂，L₂”(x)＝2p₁，则得到曲线曲率表达式：$K=\frac{{L_2}^{\prime \prime }\left(x\right)}{{(1+{L_2}^{\prime }{\left(x\right)}^2)}^{\frac{3}{2}}}=\frac{{2p}_1}{{[1+{({2p}_{1}x+p_2)}^2]}^{\frac{3}{2}}}---\left(3\right);$步骤五：重复步骤二～步骤四，并将步骤三所得侧偏距和步骤四所得曲率值作为无人机横侧向控制器的输入量，完成整个曲线航迹跟踪过程。