一种飞行器三维导航方法及其装置的制造方法
【技术领域】
[0001] 本发明涉及飞行器导航技术领域，尤其涉及一种飞行器三维导航方法及其装置。
【背景技术】
[0002] 随着科学技术的发展，由于人类不断地"摆脱"地心引力，天空不再是少数人的探 索领域，慢慢会成为日常的人们生活的领域的一部分。就像现在的在地面上行驶的车辆，由 于地面空间有限和人口的膨胀，地方将不再够用，以后的汽车将会在天空中翱翔。就现在而 言，飞行器（FlightVehicle)逐渐出现在人们的视野，各种各样的飞行器用途也越来越广。 在军事方面，飞行器能完成组队搬运、目标跟踪、载人航天等任务；在民用方面，飞行器也能 够运送快递，进行高空航拍、三维地形扫描等任务。现在，大部分飞行器还是需要人为控制， 例如需要控制者使用遥控器或者终端去控制飞行。如何让控制者按照给定路线去控制飞行 器飞行成为了问题。
[0003] 给飞行器添加三维导航仪可以很好地解决飞行器跟随三维路径和指示路径的问 题。目前，大部分的三维导航技术均是对周围的场景进行三维化，让人感觉导航的世界与现 实世界相仿，从而提高导航的效果。这样确实能够使人更加容易地跟随导航路径的行走，但 是，这并不是使用路径的三维导航。在路径的实时显示、数据存储和提醒方面，现在的导航 仪还是使用二维的路径，导航仪没有就对路径的三维数据或者变化作出有效的显示或者提 示。这样，在以后的三维导航发展当中具有很大的局限性。例如：当前飞行器在不同高度飞 行时，导航仪的路径还是指示这左转右转，没有考虑飞行器的高度进行上下高低的导航，那 么，显然，导航仪指示是不足够的。
【发明内容】
[0004] 本发明的目的在于克服现有技术的不足，本发明提供了一种飞行器三维导航方法 及其装置，能够更好地得到全面的三维路径信息，让飞行器的控制人员更加容易、更加方便 地控制飞行器。
[0005] 为了解决上述问题，本发明提出了一种飞行器三维导航方法，所述方法包括：
[0006] 获取路径的位矢、切矢、曲率、挠率数据；
[0007] 获取飞行器实时的位置信息和姿态信息；
[0008] 根据飞行器实时的位置信息和姿态信息提供三维导航。
[0009] 优选地，在所述获取路径的位矢、切矢、曲率、挠率数据的步骤之前，还包括：
[0010] 计算路径的位矢、切矢、曲率、烧率数据。
[0011] 优选地，所述计算路径的位矢、切矢、曲率、挠率数据的步骤包括：
[0012] 根据起始点位置、目标点位置、障碍物状况，生成目标路径；
[0013] 根据目标路径的特性将目标路径分成若干个段路径；
[0014] 计算各段路径的切矢、曲率、挠率的参数方程；
[0015] 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据。
[0016] 优选地，所述获取路径的位矢、切矢、曲率、挠率数据的步骤，包括：
[0017] 通过无线或者有线方式从地面站得到各个路径点的位矢、切矢、曲率、挠率数据。
[0018] 优选地，所述姿态信息通过加速度计和电子罗盘的信息融合获得；所述位置信息 包括经炜度信息和高度信息。
[0019] 相应地，本发明还提供一种飞行器三维导航装置，所述装置包括：
[0020] 数据获取模块，用于获取路径的位矢、切矢、曲率、挠率数据；并获取飞行器实时的 位置信息和姿态信息；
[0021] 导航模块，用于根据飞行器实时的位置信息和姿态信息提供三维导航。
[0022] 优选地，所述装置还包括：计算模块，用于计算路径的位矢、切矢、曲率、挠率数据。
[0023] 优选地，所述计算模块包括：
[0024] 路径生成单元，用于根据起始点位置、目标点位置、障碍物状况，生成目标路径；
[0025] 路径分割单元，用于根据目标路径的特性将目标路径分成若干个段路径；
[0026] 计算各段路径的切矢、曲率、挠率的参数方程；
[0027] 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据
[0028] 优选地，所述数据获取模块通过无线或者有线方式从地面站得到各个路径点的位 矢、切矢、曲率、挠率数据。
[0029] 优选地，所述姿态信息通过加速度计和电子罗盘的信息融合获得；所述位置信息 包括经炜度信息和高度信息。
[0030] 在本发明实施例中，通过地面站进行飞行路径的规划，传输各个路径点的位矢、切 矢、曲率、挠率这几个几何不变量到飞行器，飞行器在飞行过程中利用自身位置和姿态信息 对路径的导航做出适当的处理，为三维路径导航和显示提供保障。能够更好地提供路径的 信息，让飞行器的控制人员更加容易、更加方便地控制飞行器；不但更加直观，使飞行器的 控制更加简单，而且提高飞行器控制的安全性。
【附图说明】
[0031] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本 发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以 根据这些附图获得其它的附图。
[0032] 图1是本发明实施例的飞行器三维导航方法的流程示意图；
[0033] 图2是本发明实施例中根据飞行器实时的位置信息和姿态信息提供三维导航的 流程示意图；
[0034] 图3是本发明实施例中坐标变换的流程示意图；
[0035] 图4是本发明实施例中导航仪的坐标系确立示意图；
[0036] 图5是本发明实施例的飞行器三维导航装置的结构组成示意图。
【具体实施方式】
[0037] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0038] 图1是本发明实施例的飞行器三维导航方法的流程示意图，如图1所示，该方法包 括：
[0039] S1，获取路径的位矢、切矢、曲率、挠率数据；
[0040] S2,获取飞行器实时的位置信息和姿态信息；
[0041] S3,根据飞行器实时的位置信息和姿态信息提供三维导航。
[0042] 进一步地，在S1之前，还包括：
[0043] 计算路径的位矢、切矢、曲率、挠率数据。
[0044] 计算路径的位矢、切矢、曲率、挠率数据的步骤包括：
[0045] 根据起始点位置、目标点位置、障碍物状况，生成目标路径；
[0046] 根据目标路径的特性将目标路径分成若干个段路径；
[0047] 计算各段路径的切矢、曲率、挠率的参数方程；
[0048] 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据。
[0049] 具体实施中，地面站根据起始点位置（经炜坐标和高度），目标点位置（经炜坐 标和高度）和障碍物状况，生成目标路径。接着再将目标路径分成若干段，这个可以根据 计算机处理器的运算速度、问题对路径要求的计算精度等决定分的路径的段数。这里如果 路径是连续的，则路径可以直接进入下一步参数方程的转化；如果路径是离散的，则每一 段进行插值以后在进行参数化。将各段路径分别转换为一般参数（以下将t设置为参数， te[0，1])下的参数方程r(t)。每段参数方程的r(t)的格式如下：
[0050] r(t) = [x(t)y(t)z(t)]
[0051] 每一列x(t)、y(t)、z(t)分别代表这段中各个点的炜度，经度，高度的参数方程。 对r⑴求一次导r'⑴，二次导r"⑴，三次导r" '⑴。
[0067] 然后再利用曲率、挠率公式，代入计算，即可得到各个点的曲率和挠率的数值。
[0068] 上述得到的参数方程均为连续方程，为了更好地将路径传输给飞行器，接下来将 路径离散化。首先需要确定参数t的分辨率。
[0069] 优选地，分辨率可以根据曲率决定，曲率大的地方，t的分辨率大，控制点较多；曲 率小的地方，t的分辨率可以小一点，控制点可以稍微少一点。
[0070] 这里说明采用参数方程的好处就是，更改路径的分辨率，只需要更改t的分辨率 即可，并不需要重新计算曲率，挠率的公式。
[0071] 将各路径段的参数t代入各路径段的位矢参数方程r(t)、切矢参数方程r'（t)、曲 率参数方程κ(t)和挠率参数方程τ(t)，即可得到各个路径点的位矢、切矢、曲率和挠率。
[0072] 在S1中，飞行器通过无线或者有线方式从地面站得到各个路径点的位矢、切矢、 曲率、挠率数据，获得获得数据后，存储在ROM或者SD卡中。
[0073] 每个路径点的数据存储方式为：
[0074] p= [r1X3r1Χ3κ1Χ1τ