1Χ1]
[0075] 该点的位矢数据为r，切矢数据为r'，曲率数据为κ和挠率数据为τ。
[0076] 在本发明实施例中，姿态信息通过加速度计和电子罗盘的信息融合获得；位置信 息包括经炜度信息和高度信息。
[0077] 其中，飞行器的位置信息（经炜度信息和高度（海拔）信息）可以通过GPS获得， 并且使用气压计辅助修正飞行器的高度信息Z_。
[0078] 姿态信息可以通过加速度计和电子罗盘信息的信息融合获得。利用加速度计和电 子罗盘的输出作为卡尔曼滤波的输入，卡尔曼滤波的输出即为飞行器的姿态信息at_
[0079] 下面结合图2至图4对S3进行详细说明。
[0080] 第一步，在起点时，在原始位置调整飞行器姿态，让飞行器起始的前进方向为第一 个路径点的切矢方向，并且提醒下一个路径点的切矢方向、曲率大小，挠率大小与方向。如 果飞行器不是在路径的起始点，应先让飞行器导航到路径的起始点，再调整飞行器的前进 方向。
[0081] 第二步，根据飞行器当前的位置确定其是否在路径当中。通过求取下一个目标路 径点与飞行器现在位置的距离判断。如果之间距离小于等于阈值，就认定飞行器还在路径 中。如果之间的距离大于给定阈值，则认定飞行器已经偏离路径。如果不在路径中，则查找 最近的路径点，并导航到最近的路径点。这里的导航方法采用直线导航到最近点的方法。
[0082] 第三步，如果飞行器在路径中，获取飞行器的前进方向，根据下一个目标路径点的 切矢与当前飞行器的前进方向之差调整飞行器的速度方向。
[0083] GPS能够获取飞行器的速度方向，即可以获得飞行器的前进方向。
[0084] 第四步，在飞行过程中，飞行器没有偏离路径情况下，导航利用飞行器的位置和姿 态信息，显示接下来的一定数目的导航点的路径，并且显示下一个路径点的切矢方向、曲率 大小和挠率大小。也就是说，能够在导航上直观地看到接下来的一部分路径，而且还可以在 导航上看到接下来的路径的变化数值。
[0085] 对于路径的三维显示，可以利用旋转和平移变换，得到路径的更加直观的显示。
[0086] 图3是坐标变换的流程图。根据流程图，得到的方法如下：从上面步骤得到的飞行 器的位置和姿态信息，也就可以求出飞行器的坐标系状态。将路径点，经过坐标变换，获得 在飞行器坐标系下路径点的坐标。
[0087] 图4是导航仪的坐标系确立示意图。导航仪的前方为X轴的正方向，导航仪的左 方为Y轴的正方向，导航仪的上方为Z轴的正方向。
[0088] 优选地，对于路径形状的显示，可以采用简单的直线将需要显示的路径点连接，也 可以使用插值函数往路径中插值。
[0089] 由于曲率和挠率是曲线几何不变量，与坐标系的选取无关，所以并不需要对曲率 和挠率进行坐标变换，这是使用曲率和挠率的一大好处。
[0090] 第五步，检测接下来的几个路径点的曲率、挠率大小。如果在这几个点内曲率、挠 率大于一定阈值，则导航会发出提醒，提醒飞行器不能超过一定速度，或者提醒飞行器控制 员提尚注意力。
[0091] 相应地，本发明实施例还提供一种飞行器三维导航装置，如图5所示，该装置包 括：
[0092] 数据获取模块1，用于获取路径的位矢、切矢、曲率、挠率数据；并获取飞行器实时 的位置信息和姿态信息；
[0093] 导航模块2,用于根据飞行器实时的位置信息和姿态信息提供三维导航。
[0094] 该装置还包括：计算模块，用于计算路径的位矢、切矢、曲率、挠率数据。
[0095] 进一步地，计算模块包括：
[0096] 路径生成单元，用于根据起始点位置、目标点位置、障碍物状况，生成目标路径；
[0097] 路径分割单元，用于根据目标路径的特性将目标路径分成若干个段路径；
[0098] 计算各段路径的切矢、曲率、挠率的参数方程；
[0099] 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据
[0100] 数据获取模块1通过无线或者有线方式从地面站得到各个路径点的位矢、切矢、 曲率、挠率数据。
[0101] 姿态信息通过加速度计和电子罗盘的信息融合获得；位置信息包括经炜度信息和 高度信息。
[0102] 本发明的装置实施例中各功能模块的功能可参见本发明方法实施例中的流程处 理，这里不再赘述。
[0103] 在本发明实施例中，通过地面站进行飞行路径的规划，传输各个路径点的位矢、切 矢、曲率、挠率这几个几何不变量到飞行器，飞行器在飞行过程中利用自身位置和姿态信息 对路径的导航做出适当的处理，为三维路径导航和显示提供保障。能够更好地提供路径的 信息，让飞行器的控制人员更加容易、更加方便地控制飞行器；不但更加直观，使飞行器的 控制更加简单，而且提高飞行器控制的安全性。
[0104] 本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可 以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存 储介质可以包括：只读存储器（ROM，ReadOnlyMemory)、随机存取存储器（RAM，Random AccessMemory)、磁盘或光盘等。
[0105] 另外，以上对本发明实施例所提供的飞行器三维导航方法及其装置进行了详细介 绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只 是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发 明的思想，在【具体实施方式】及应用范围上均会有改变之处，综上所述，本说明书内容不应理 解为对本发明的限制。
【主权项】
1. 一种飞行器三维导航方法，其特征在于，所述方法包括： 获取路径的位矢、切矢、曲率、挠率数据； 获取飞行器实时的位置信息和姿态信息； 根据飞行器实时的位置信息和姿态信息提供三维导航。2. 如权利要求1所述的飞行器三维导航方法，其特征在于，在所述获取路径的位矢、切 矢、曲率、挠率数据的步骤之前，还包括： 计算路径的位矢、切矢、曲率、挠率数据。3. 如权利要求2所述的飞行器三维导航方法，其特征在于，所述计算路径的位矢、切 矢、曲率、挠率数据的步骤包括： 根据起始点位置、目标点位置、障碍物状况，生成目标路径； 根据目标路径的特性将目标路径分成若干个段路径； 计算各段路径的切矢、曲率、挠率的参数方程； 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据。4. 如权利要求1所述的飞行器三维导航方法，其特征在于，所述获取路径的位矢、切 矢、曲率、挠率数据的步骤，包括： 通过无线或者有线方式从地面站得到各个路径点的位矢、切矢、曲率、挠率数据。5. 如权利要求1所述的飞行器三维导航方法，其特征在于，所述姿态信息通过加速度 计和电子罗盘的信息融合获得；所述位置信息包括经炜度信息和高度信息。6. -种飞行器三维导航装置，其特征在于，所述装置包括： 数据获取模块，用于获取路径的位矢、切矢、曲率、挠率数据；并获取飞行器实时的位置 信息和姿态信息； 导航模块，用于根据飞行器实时的位置信息和姿态信息提供三维导航。7. 如权利要求6所述的飞行器三维导航装置，其特征在于，所述装置还包括：计算模 块，用于计算路径的位矢、切矢、曲率、挠率数据。8. 如权利要求7所述的飞行器三维导航装置，其特征在于，所述计算模块包括： 路径生成单元，用于根据起始点位置、目标点位置、障碍物状况，生成目标路径； 路径分割单元，用于根据目标路径的特性将目标路径分成若干个段路径； 计算各段路径的切矢、曲率、挠率的参数方程； 确定各段参数的分辨率，得到各个路径点的位矢、切矢、曲率、挠率数据。9. 如权利要求6所述的飞行器三维导航装置，其特征在于，所述数据获取模块通过无 线或者有线方式从地面站得到各个路径点的位矢、切矢、曲率、挠率数据。10. 如权利要求6所述的飞行器三维导航装置，其特征在于，所述姿态信息通过加速度 计和电子罗盘的信息融合获得；所述位置信息包括经炜度信息和高度信息。
【专利摘要】本发明实施例公开了一种飞行器三维导航方法及其装置，其中，该方法包括：获取路径的位矢、切矢、曲率、挠率数据；获取飞行器实时的位置信息和姿态信息；根据飞行器实时的位置信息和姿态信息提供三维导航。在本发明实施例中，通过地面站进行飞行路径的规划，传输各个路径点的位矢、切矢、曲率、挠率这几个几何不变量到飞行器，飞行器在飞行过程中利用自身位置和姿态信息对路径的导航做出适当的处理，为三维路径导航和显示提供保障。能够更好地提供路径的信息，让飞行器的控制人员更加容易、更加方便地控制飞行器；不但更加直观，使飞行器的控制更加简单，而且提高飞行器控制的安全性。
【IPC分类】G01C21/32, G01C21/24, G01C21/20
【公开号】CN105241466
【申请号】CN201510741684
【发明人】罗笑南, 江泽宇, 林淑金
【申请人】中山大学
【公开日】2016年1月13日
【申请日】2015年11月2日