专利名称：一种基于曲率补偿的横侧向曲线航迹跟踪方法
技术领域：
本发明涉及一种飞行器航迹跟踪控制方法，属于航迹跟踪控制技术领域，具体地说，是指一种基于曲率补偿的横侧向曲线航迹跟踪方法。
背景技术：
无人机也称无人航空器或遥控驾驶航空器，是一种由无线电遥控设备控制，或由预编程序操纵的非载人飞行器，拥有众多有人驾驶飞机所不具有的优点，可执行各种作战和训练保障等任务，具有广阔的军事应用前景。随着无人机的应用领域不断扩大，对无人机的飞行要求也越来越苛刻，对预定航迹进行准确、快速的跟踪便是其中之一。无人机的航迹跟踪控制，主要通过无人机的飞行控制系统来实现。这样，如何通过调节无人机的飞行控制系统来使无人机能够更好更快的跟踪预定航迹，对无人机整体性能的提高以及航空事业的发展都有很重要的意义。航迹规划阶段，所规划航迹由一组离散航路点组成，相邻点之间距离一个规划步长。传统的侧偏距控制原理如图1所示。由航迹规划算法所得航迹点相邻点两两连线得到所规划航迹(tl-t2-t3_t4)，通过实时计算无人机当前位置与所规划航迹之间最近距离得到无人机侧偏距S，在横侧向控制回路的作用下，无人机逐渐向t3航迹点靠近，无人机到达t3航迹点后，切换跟踪下一个航迹段t3_t4。由于无人机跟踪航迹为折线，在跟踪过程中会存在较大的跟踪误差；尤其当航迹点较密时，会造成无人机无法跟踪规划航迹。
发明内容
本发明的目的是为了解决上述问题，提出一种基于曲率补偿的横侧向曲线航迹跟踪方法，通过引入拉格朗日多项式插值算法，对航迹规划所得离散航迹点进行曲线拟合处理，得到曲线航迹，并引入曲线曲率作为补偿量，加入横侧向航迹跟踪控制器设计中，明显改善了对参考航迹的跟踪效果，提高了跟踪精度。本发明的一种基于曲率补偿的横侧向曲线航迹跟踪方法，具体包括如下步骤:步骤一:通过航迹规划模块得到参考航迹的离散航路点数据。步骤二:通过拉格朗日分段多项式插值方法，对步骤一所得离散航路点数据进行分段插值拟合处理，得到曲线航迹。步骤三:计算无人机当前时刻侧偏距，以及参考航迹上距飞机最近点坐标。步骤四:通过曲率计算公式，计算步骤三所得参考航迹上距飞机最近点处曲率值。步骤五:重复步骤二 步骤四，并将步骤三所得侧偏距和步骤四所得曲率值作为无人机横侧向控制器的输入量，完成整个曲线航迹跟踪过程。本发明的优点在于:(I)本发明提供的方法通过对参考航迹进行曲线插值拟合处理，使跟踪过程中航路点的过渡段更加平滑，跟踪误差减小。(2)本发明提供的方法将曲率值作为补偿量加入横侧向控制器中，而曲线曲率代表了曲线的弯曲程度以及弯曲方向，可以为横侧向控制器提供航迹方向走势信息，从而更好地跟踪航迹。
图1是折线航迹侧偏距控制原理图；图2是本发明中航迹规划模块所得离散航路点；图3是本发明中离散航路点曲线拟合处理过程示意图；图4是本发明中无人机航迹跟踪控制器结构示意图。
具体实施例方式下面将结合附图和实施例对本发明作进一步的详细说明。本发明提出了一种基于曲率补偿的横侧向曲线航迹跟踪方法，首先通过航迹规划模块得到参考航迹的离散航路点数据，然后通过拉格朗日分段多项式插值方法，对离散航路点进行分段插值拟合处理，从而得到曲线航迹，实时计算无人机当前位置距离参考曲线航迹段的最近距离从而得到无人机侧偏距，同时可以得到参考航迹上距飞机最近点处坐标，通过曲率计算公式可以得到该对应点处曲率值，将上述所得无人机侧偏距和最近点处曲率作为无人机横侧向控制器的输入量，完成曲线航迹跟踪控制。本发明的一种基于曲率补偿的横侧向曲线航迹跟踪方法，包括以下几个步骤:步骤一:通过航迹规划模块得到参考航迹的离散航路点数据。根据无人机起始点与目标点,通过航迹规划模块,得到无人机从起始点到达目标
点的离散航路点数据，如图2中P⑴，P⑵，......，P(n)所示航迹,P⑴为起始点，p (n)为
目标点，n为航迹上航路点数量。步骤二:通过拉格朗日分段多项式插值方法，对步骤一所得离散航路点数据进行分段插值拟合处理，得到曲线航迹。本发明中采用分段二次多项式插值方法，所谓分段二次多项式插值，即每次选取三个相邻航路点数据作为插值节点，进而求得Lagrange插值多项式。设三个相邻航路点坐标为(Xh, Yi^1)，(Xi, Yi)，(xi+1, yi+1)，则 Lagrange 插值多项式为
权利要求
1.一种基于曲率补偿的横侧向曲线航迹跟踪方法，其特征在于，包括如下步骤: 步骤一:通过航迹规划模块得到参考航迹的离散航路点数据； 步骤二:通过拉格朗日分段多项式插值方法，对步骤一所得离散航路点数据进行分段插值拟合处理，得到曲线航迹； 步骤三:计算无人机当前时刻侧偏距，以及参考航迹上距飞机最近点坐标； 步骤四:通过曲率计算公式，计算步骤三所得参考航迹上距飞机最近点处曲率值； 步骤五:重复步骤二 步骤四，并将步骤三所得侧偏距和步骤四所得曲率值作为无人机横侧向控制器的输入量，完成整个曲线航迹跟踪过程。
2.根据权利要求1所述的一种基于曲率补偿的横侧向曲线航迹跟踪方法，其特征在于:所述的分段多项式插值方法，处理过程具体为: (1)每当无人机到达一个航路点，设该航路点为P。—; (2)如果count( n-2，则利用Pe_t，Pcount+1, Pcount+2三个航路点数据进行二次多项式插值；否则 结束，完成整条航迹的跟踪；(3)如果是C0Unt=n_2,则绘制P_t_Pc；_t+2段拟合航路；否则绘制Pcount-Pcount+l航路段曲线航 迹，并将该曲线航迹作为参考航迹； (4)令count=count+l,返回步骤(2)。
3.根据权利要求1所述的一种基于曲率补偿的横侧向曲线航迹跟踪方法，其特征在于:所述的步骤三所得参考航路段上距离飞机最近点处曲率值K表达式:
全文摘要
本发明公开了一种基于曲率补偿的横侧向曲线航迹跟踪方法，属于航迹跟踪控制技术领域。本发明首先通过航迹规划模块得到参考航迹的离散航路点数据；然后通过拉格朗日分段多项式插值方法，对所得离散航路点数据进行分段插值拟合处理，得到曲线航迹；计算无人机当前时刻侧偏距，以及参考航迹上距飞机最近点坐标，计算所得参考航迹上距飞机最近点处曲率值。将所得侧偏距和曲率值作为无人机横侧向控制器的输入量，完成整个曲线航迹跟踪过程。本发明通过对参考航迹进行曲线插值拟合处理，使跟踪过程中航路点的过渡段更加平滑，跟踪误差减小，从而更好地跟踪航迹。
文档编号G05D1/10GK103149937SQ20131005953
公开日2013年6月12日 申请日期2013年2月26日 优先权日2013年2月26日
发明者王宏伦, 许敬刚, 吕文涛, 盖文东 申请人:北京航空航天大学