CN108562289A - 连续多边几何环境中四旋翼飞行器激光雷达导航方法 - Google Patents

连续多边几何环境中四旋翼飞行器激光雷达导航方法 Download PDF

Info

Publication number
CN108562289A
CN108562289A CN201810579682.4A CN201810579682A CN108562289A CN 108562289 A CN108562289 A CN 108562289A CN 201810579682 A CN201810579682 A CN 201810579682A CN 108562289 A CN108562289 A CN 108562289A
Authority
CN
China
Prior art keywords
moment
point
quadrotor
coordinate
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810579682.4A
Other languages
English (en)
Other versions
CN108562289B (zh
Inventor
邹冰倩
吕品
赖际舟
石鹏
付相可
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201810579682.4A priority Critical patent/CN108562289B/zh
Publication of CN108562289A publication Critical patent/CN108562289A/zh
Application granted granted Critical
Publication of CN108562289B publication Critical patent/CN108562289B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/933—Lidar systems specially adapted for specific applications for anti-collision purposes of aircraft or spacecraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种连续多边几何环境中四旋翼飞行器激光雷达导航方法，属于自主导航与制导领域。本发明可用于四旋翼飞行器在多边几何环境中的导航，将安装于四旋翼飞行器的激光雷达与惯性传感器相融合，生成导航信息，使多旋翼飞行器在多边几何环境重心处飞行。在本发明中，通过激光雷达对多边几何环境进行辨识，从而估计四旋翼飞行器与环境各边的相对距离与方位。通过滤波器将激光雷达的估计信息与惯性传感器进行数据融合，从而得到四旋翼飞行器的位置、速度、姿态信息。接着，通过激光雷达测量的数据，对几何环境重心进行计算，令四旋翼飞行器在几何重心处飞行。本发明可令四旋翼飞行器在多边几何环境中安全、稳定的飞行。

Description

连续多边几何环境中四旋翼飞行器激光雷达导航方法
技术领域
本发明涉及自主导航与领域，尤其涉及连续多边几何环境中四旋翼飞行器激光雷达导航方法。
背景技术
随着四旋翼飞行器相关技术的发展，其被应用于越来越多的飞行任务，对导航性能提出了越来越高的要求。激光雷达是四旋翼飞行器常用的导航设备之一，其不受无线电信号和光线干扰，具有较强的自主性与较高的导航精度。当激光雷达用于导航时，普遍采用同时定位与地图构建方法(Simultaneous Localization And Mapping，简称SLAM)，在载体上通过对各种传感器数据进行采集和计算，实现对其自身位置、姿态的定位以及对场景地图的构建。
多边几何环境，指的是周边呈现规则多边形的环境，例如走廊、铁塔内部等。对于这类多边几何环境，其多边特征明显，而沿多边几何方向其特征不显著。在该类环境中应用SLAM 方法时效果通常不理想。因此，若在该环境中，将激光雷达SLAM方法用于四旋翼飞行器，飞行器的导航性能难以得到保障，容易与多边几何环境发生碰撞，亟需一种解决四旋翼飞行器在多边几何环境中的稳定飞行问题的方法。
发明内容
发明目的：本发明的目的是提供连续多边几何环境中四旋翼飞行器激光雷达导航方法，以解决四旋翼飞行器在多边几何环境中的稳定飞行问题。
技术方案：连续多边几何环境一种四旋翼飞行器激光雷达导航方法，包括以下步骤：
(1)周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达信息S(k)，陀螺仪信息加速度计信息气压高度计信息hb(k)；
(2)通过惯性传感器，预测k时刻四旋翼飞行器的姿态、速度、位置；
(3)辨识k时刻多边几何环境特征；
(4)计算k时刻四旋翼飞行器相对于多边几何环境的相对偏航角及在其导航系下的坐标；
(5)估计k时刻多边几何环境的几何重心；
(6)通过卡尔曼滤波器，对k时刻四旋翼飞行器的姿态、速度、位置进行校正。
其中的相关坐标系定义为：
机体系的X、Y、Z轴分别为飞行器的机头方向、右向、下向，激光雷达与机体系相固连；导航系原点为垂直于机头方向线段中点，此线段所在直线为参考边，Z轴竖直向下，X轴垂直指向参考边，Y轴根据右手定则确定。
步骤(2)中采用如下形式预测k时刻四旋翼飞行器的姿态四元数、速度、位置：
(2.1)姿态四元数预测采用如下公式：
其中，Q(k)＝[qo(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置， Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数；ΔT为离散采样周期，通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的飞行器机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、 Y、Z轴上的分量；
(2.2)速度预测采用如下公式：
其中， 为k时刻加速度计读取的飞行器机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值； 为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量； 为k-1时刻机体系相对于导航系的线速度在导航系X、 Y、Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
(2.3)位置预测采用如下公式：
其中，k时刻的位置 分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；k-1时刻的位置 分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
(2.4)加速度计零偏和陀螺仪零偏预测采用如下公式：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
进一步的，步骤(3)中辨识k时刻多边几何环境特征的方法包括如下步骤：
(3.1)计算S(k)中激光点投影至飞行器所在水平面的二维坐标：
记pi为S(k)的第i个激光点(i＝1,2,…,N0)，N0为S(k)中激光点的数量，和为pi在机体系中的坐标，记θ(k-1)和φ(k-1)分别为飞行器在k-1时刻的俯仰角和横滚角，pi投影至飞行器所在水平面的二维坐标和根据下式计算：
(3.2)检测S(k)中的撕裂点：
计算S(k)相邻序号激光点之间的距离若大于阈值Et，则pi和pi+1为撕裂点。记撕裂点集合为{ptr}，1≤r≤Nt，Nt为撕裂点的数量。
(3.3)检测S(k)中的角点，将点云数据分群：
若dc大于阈值Ec则pi为角点，且撕裂点不是角点，即dc的计算公式如下：
记角点集合为1≤c≤Nc，Nc为角点的数量。将角点和撕裂点作为分群的标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，记为sm，1≤m≤Nm，Nm为点群的数量。
(3.4)筛选直线特征，将点云数据分群：
记直线方程为x＝a1my+a2m，记qim为sm的第i个激光点(i＝1,2,…,N1m)，N1m为sm中激光点的数量，和为qim在机体系中的坐标，根据下式计算直线方程的参数a1m和a2m：
比较相邻两个点群的直线方程参数a1m和a2m，当|a1j-a1(j+1)|＞Ea1或|a2j-a2(j+1)|＞Ea2 (j＝1,2,…,Nm-1)时，其中Ea1、Ea2为设置的阈值，以sj+1最小序号激光点为分群标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，根据激光点在S(k)的序号，计算每个点群中序号最小的点与序号最大的点之间的距离筛选出大于阈值Lt的点群，记为点群Sd，1≤d≤Nd，Nd为点群的数量。
(3.5)拟合多边几何环境：
记直线方程为x＝A1dy+A2d，记qid为sd的第i个激光点(i＝1,2,…,N1)，N1d为sd中激光点的数量，和为qid在机体系中的坐标，根据下式计算直线方程的参数A1d和A2d：
进一步的，步骤(4)中计算k时刻四旋翼飞行器相对于多边几何环境的相对偏航角及其在导航系下坐标的方法为：
(4.1)确定参考坐标系：
参考坐标系原点为参考边中点，Z轴竖直向下，X轴垂直指向参考边，Y轴根据右手定则确定。参考边确定方法如下：
①根据权利要求4定义的导航系，计算预测偏航角ψc，方法如下：
a.若A1dmin＝0，则四旋翼飞行器的初始偏航角ψini为：
b.若则四旋翼飞行器的初始偏航角ψini为：
②根据权利要求6对偏航角的预测，对预测偏航角ψc的计算公式如下：ψc＝ψ+ψini
③根据预测的偏航角ψc，确定k时刻的参考边，计算方法如下：
a.若则参考边为A1d最大值对应的直线，其直线参数为
b.若则预测参考边斜率为：
计算(A1d-Ac)的值，当(A1d-Ac)取最小值时，对应的直线即为参考边，其直线参数为 E1A为设置的阈值，
(4.2)计算偏航角：
①若则四旋翼飞行器的偏航角ψl(k)为：
②若则四旋翼飞行器的偏航角ψl(k)为：
其中，四旋翼飞行器机头方向垂直指向于参考边的偏航角为0，顺时针旋转为正。
(4.3)计算在导航系下的坐标：
①参考边对应点群为分别计算该点群所拟合的直线与点群和点群所拟合直线的交点，分别记为点M、N，其在机体系下的坐标分别为计算线段MN的距离，记为lMN,计算公式如下：点M在导航系下的坐标为
②计算飞行器相对于参考边的距离，记为lref，计算公式如下：飞行器在导航系下x轴坐标为：xn＝-lref。
③计算直线与参考边所拟合直线的交点，记为点F，在机体系下的坐标为线段MF的长度计算公式为：飞行器在导航系下y轴坐标为
进一步的，步骤(5)中计算k时刻多边几何环境的几何重心的方法为：
(5.1)几何重心为多边几何内接圆圆心，记计算机体系原点为初定内接圆圆心O，坐标记为到点群Sd对应的直线的距离Ld，计算公式如下：
(5.2)筛选出其中与圆心距离最短和次短的两条直线对应的点群Sp和Sq，最短距离为LA，与其对应的直线方程参数A1p、A2p以及A1q、A2q。建立直线方程 (若A1p＝0，直线方程为若A1p＝∞，直线方程为)，分别计算其与对应点群直线的交点A、B，坐标为
(5.3)设一坐标点C，坐标为使得其中
在点C与原点延长线上取一点O'，坐标为O'为新的圆心坐标，其中a为步长因子。
计算点O'到点群Sd对应的直线的距离，筛选出其中与点O'距离最短的直线，其距离为LO'。
(5.4)若LO'＞LA，重复2)和3)；若LO'＜LA，舍去O'，回到上一步确定的圆心坐标，将步长因子改为原来的0.618，重复1)和2)，重新计算圆心坐标。直至a小于阈值Δa，记录机体系下O'坐标。
(5.5)O'即为k时刻多边几何环境的几何重心，在导航系下的坐标为计算公式如下：
其中，为气压高度计信息，ψl为偏航角，T2×2为(k-1)时刻飞行器在导航系下的位置。
进一步的，步骤(6)中通过卡尔曼滤波器，对k时刻四旋翼飞行器的姿态、速度、位置进行校正：
(6.1)计算一步预测均方误差Pk|k-1：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
式中，
为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵， A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；G为滤波器k-1时刻的滤波器噪声系数矩阵，W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
(6.2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
式中，Λa＝[1 0 0],Λb＝[0 1 0]，Λc＝[0 0 -1]，Θa＝[q0(k-1)2+q1(k-1)2-q2(k-1)2-q3(k-1)22(q1(k-1)q2(k-1)+q0(k-1)q3(k-1))2(q1(k-1)q3(k-1)-q0(k-1)q2(k-1))]，Θb＝[2(q1(k-1)q2(k-1)-q0(k-1)q3(k-1))q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)22(q2(k-1)q3(k-1)+q0(k-1)q1(k-1))]，
H(k)为k时刻量测矩阵，K(k)为k 时刻的滤波增益，为k时刻的量测噪声，diag表示矩阵对角化，其中分别为ψl、d、hb的噪声；kHx、kHy为模型参数，均为常数，通过离线辨识方法获得；04×1为4×1的零矩阵，03×1为3×1的零矩阵， 06×1为6×1的零矩阵；
(6.3)计算k时扩展卡尔曼滤波器状态估计值
式中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤二的预测公式计算得到，Y(k)＝[ψl(k)xn yn hb(k)]T为k时刻的量测值，通过步骤四和步骤五获得；
(6.4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
工作原理：本发明通过激光雷达对多边几何环境进行辨识，对四旋翼飞行器距周边环境的距离、航向进行估计，并进一步与惯性传感器相融合，计算四旋翼飞行器的姿态、速度、位置信息，进而生成制导指令，解决四旋翼飞行器在多边几何环境中的稳定飞行问题。
有益效果：与现有技术相比，本发明具有以下优点：通过本发明，可以在连续多边几何环境中，完成基于激光雷达的四旋翼飞行器姿态、速度、位置的解算，并生成制导指令保障其在环境中心稳定飞行。
附图说明
图1为本发明流程示意图；
图2为确定多边几何结构重心方法的流程示意图。
具体实施方式
如图1所示，本发明具体步骤如下：
步骤(1)：周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达信息S(k)，陀螺仪信息加速度计信息气压高度计信息hb(k)；
步骤(2)：通过惯性传感器，预测k时刻四旋翼飞行器的姿态、速度、位置，具体方法如下：
(2.1)姿态四元数预测采用如下公式：
其中，Q(k)＝[qo(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置；； Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的飞行器机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、 Y、Z轴上的分量；
(2.2)速度预测采用如下公式：
其中， 为k时刻加速度计读取的飞行器机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值； 为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量； 为k-1时刻机体系相对于导航系的线速度在导航系X、Y、 Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
(2.3)位置预测采用如下公式：
其中，k时刻的位置 分别为飞行器k 时刻在导航系X、Y、Z轴上的位置坐标；k-1时刻的位置 分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
(2.4)加速度计零偏和陀螺仪零偏预测采用如下公式：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
步骤(3)：辨识k时刻多边几何环境特征，具体方法如下：
(3.1)计算S(k)中激光点投影至飞行器所在水平面的二维坐标：
记pi为S(k)的第i个激光点(i＝1,2,…,N0)，N0为S(k)中激光点的数量，和为pi在机体系中的坐标，记θ(k-1)和φ(k-1)分别为飞行器在k-1时刻的俯仰角和横滚角，pi投影至飞行器所在水平面的二维坐标和根据下式计算：
(3.2)检测S(k)中的撕裂点：
计算S(k)相邻序号激光点之间的距离若大于阈值Et，则pi和pi+1为撕裂点。记撕裂点集合为1≤r≤Nt，Nt为撕裂点的数量。
(3.3)检测S(k)中的角点，将点云数据分群：
若dc大于阈值Ec则pi为角点，且撕裂点不是角点，即dc的计算公式如下：
记角点集合为1≤c≤Nc，Nc为角点的数量。将角点和撕裂点作为分群的标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，记为sm，1≤m≤Nm，Nm为点群的数量。
(3.4)筛选直线特征，将点云数据分群：
记直线方程为x＝a1my+a2m，记qim为sm的第i个激光点(i＝1,2,…,N1m)，N1m为sm中激光点的数量，和为qim在机体系中的坐标，根据下式计算直线方程的参数a1m和a2m：
比较相邻两个点群的直线方程参数a1m和a2m，当|a1j-a1(j+1)|＞Ea1或|a2j-a2(j+1)|＞Ea2 (j＝1,2,…,Nm-1)时，其中Ea1、Ea2为设置的阈值，以sj+1最小序号激光点为分群标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，根据激光点在S(k)的序号，计算每个点群中序号最小的点与序号最大的点之间的距离筛选出大于阈值Lt的点群，记为点群Sd，1≤d≤Nd，Nd为点群的数量。
(3.5)拟合多边几何环境：
记直线方程为x＝A1dy+A2d，记qid为sd的第i个激光点(i＝1,2,…,N1)，N1d为sd中激光点的数量，和为qid在机体系中的坐标，根据下式计算直线方程的参数A1d和A2d：
步骤(4)：计算k时刻四旋翼飞行器相对于多边几何环境的相对偏航角及其在导航系下的坐标，具体方法如下：
(4.1)确定参考坐标系：
参考坐标系原点为参考边中点，Z轴竖直向下，X轴垂直指向参考边，Y轴根据右手定则确定。参考边确定方法如下：
①根据权利要求4定义的导航系，计算预测偏航角ψc，方法如下：
a.若则四旋翼飞行器的初始偏航角ψini为：
b.若则四旋翼飞行器的初始偏航角ψini为：
②根据权利要求6对偏航角的预测，对预测偏航角ψc的计算公式如下：ψc＝ψ+ψini
③根据预测的偏航角ψc，确定k时刻的参考边，计算方法如下：
a.若则参考边为A1d最大值对应的直线，其直线参数为
b.若则预测参考边斜率为：
计算(A1d-Ac)的值，当(A1d-Ac)取最小值时，对应的直线即为参考边，其直线参数为 E1A为设置的阈值，
(4.2)计算偏航角：
①若则四旋翼飞行器的偏航角ψl(k)为：
②若则四旋翼飞行器的偏航角ψl(k)为：
其中，四旋翼飞行器机头方向垂直指向于参考边的偏航角为0，顺时针旋转为正。
(4.3)计算在导航系下的坐标：
①参考边对应点群为分别计算该点群所拟合的直线与点群和点群所拟合直线的交点，分别记为点M、N，其在机体系下的坐标分别为计算线段MN的距离，记为lMN,计算公式如下：点M在导航系下的坐标为
②计算飞行器相对于参考边的距离，记为lref，计算公式如下：飞行器在导航系下x轴坐标为：xn＝-lref。
③计算直线与参考边所拟合直线的交点，记为点F，在机体系下的坐标为线段MF的长度计算公式为：飞行器在导航系下y轴坐标为
步骤(5)：如图2所示，估计k时刻多边几何环境的几何重心，具体方法如下：
(5.1)几何重心为多边几何内接圆圆心，记计算机体系原点为初定内接圆圆心O，坐标记为到点群Sd对应的直线的距离Ld，计算公式如下：
(5.2)筛选出其中与圆心距离最短和次短的两条直线对应的点群Sp和Sq，最短距离为LA，与其对应的直线方程参数A1p、A2p以及A1q、A2q。建立直线方程 (若A1p＝0，直线方程为若A1p＝∞，直线方程为)，分别计算其与对应点群直线的交点A、B，坐标为
(5.3)设一坐标点C，坐标为使得其中
在点C与原点延长线上取一点O'，坐标为O'为新的圆心坐标，其中
a为步长因子。
计算点O'到点群Sd对应的直线的距离，筛选出其中与点O'距离最短的直线，其距离为LO'。
(5.4)若LO'＞LA，重复2)和3)；若LO'＜LA，舍去O'，回到上一步确定的圆心坐标，将步长因子改为原来的0.618，重复1)和2)，重新计算圆心坐标。直至a小于阈值Δa，记录机体系下O'坐标。
(5.5)O'即为k时刻多边几何环境的几何重心，在导航系下的坐标为计算公式如下：
其中，为气压高度计信息，ψl为偏航角，T2×2为(k-1)时刻飞行器在导航系下的位置。
步骤(6)：通过卡尔曼滤波器，对k时刻四旋翼飞行器的姿态、速度、位置进行校正，具体方法如下：
(6.1)计算一步预测均方误差Pk|k-1：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
式中，
为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵， A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；G为滤波器k-1时刻的滤波器噪声系数矩阵，W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
(6.2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
式中，Λa＝[1 0 0],Λb＝[0 1 0]，Λc＝[0 0 -1]，Θa＝[q0(k-1)2+q1(k-1)2-q2(k-1)2-q3(k-1)22(q1(k-1)q2(k-1)+q0(k-1)q3(k-1))2(q1(k-1)q3(k-1)-q0(k-1)q2(k-1))]，Θb＝[2(q1(k-1)q2(k-1)-q0(k-1)q3(k-1))q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)22(q2(k-1)q3(k-1)+q0(k-1)q1(k-1))]，
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益，为k时刻的量测噪声，diag表示矩阵对角化，其中分别为ψl、d、hb的噪声；kHx、kHy为模型参数，均为常数，通过离线辨识方法获得；04×1为4×1的零矩阵，03×1为3×1的零矩阵， 06×1为6×1的零矩阵；
(6.3)计算k时扩展卡尔曼滤波器状态估计值
式中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤二的预测公式计算得到，Y(k)＝[ψl(k) xn yn hb(k)]T为k时刻的量测值，通过步骤四和步骤五获得；
(6.4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。

Claims (6)

1.一种连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于，包括以下步骤：
(1)周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达信息S(k)，陀螺仪信息加速度计信息气压高度计信息hb(k)；
(2)通过惯性传感器，预测k时刻四旋翼飞行器的姿态、速度、位置；
(3)辨识k时刻多边几何环境特征；
(4)计算k时刻四旋翼飞行器相对于多边几何环境的相对偏航角及在其导航系下的坐标；
(5)估计k时刻多边几何环境的几何重心；
(6)通过卡尔曼滤波器，对k时刻四旋翼飞行器的姿态、速度、位置进行校正。
2.根据权利要求1所述的连续多边几何环境中的四旋翼飞行器激光雷达导航方法，其特征在于：在步骤(2)中采用如下形式预测k时刻四旋翼飞行器的姿态四元数、速度、位置：
(2.1)姿态四元数预测采用如下公式：
其中，Q(k)＝[qo(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置，Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数，ΔT为离散采样周期，通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的飞行器机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
(2.2)速度预测采用如下公式：
其中， 为k时刻加速度计读取的飞行器机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量； 为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；g＝[0 0 g]T，g为当地重力加速度值； 为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量； 为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
(2.3)位置预测采用如下公式：
其中，k时刻的位置 分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；k-1时刻的位置 分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
(2.4)加速度计零偏和陀螺仪零偏预测采用如下公式：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
3.根据权利要求1所述的连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于：步骤(3)中辨识k时刻多边几何环境特征的方法包括以下步骤：
(3.1)计算S(k)中激光点投影至飞行器所在水平面的二维坐标：
记pi为S(k)的第i个激光点(i＝1,2,…,N0)，N0为S(k)中激光点的数量，和为pi在机体系中的坐标，记θ(k-1)和φ(k-1)分别为飞行器在k-1时刻的俯仰角和横滚角，pi投影至飞行器所在水平面的二维坐标和根据下式计算：
(3.2)检测S(k)中的撕裂点：
计算S(k)相邻序号激光点之间的距离若大于阈值Et，则pi和pi+1为撕裂点。记撕裂点集合为1≤r≤Nt，Nt为撕裂点的数量；
(3.3)检测S(k)中的角点，将点云数据分群：
若dc大于阈值Ec则pi为角点，且撕裂点不是角点，即dc的计算公式如下：
记角点集合为1≤c≤Nc，Nc为角点的数量。将角点和撕裂点作为分群的标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，记为sm，1≤m≤Nm，Nm为点群的数量；
(3.4)筛选直线特征，将点云数据分群：
记直线方程为x＝a1my+a2m，记qim为sm的第i个激光点(i＝1,2,…,N1m)，N1m为sm中激光点的数量，和为qim在机体系中的坐标，根据下式计算直线方程的参数a1m和a2m：
比较相邻两个点群的直线方程参数a1m和a2m，当|a1j-a1(j+1)|＞Ea1或|a2j-a2(j+1)|＞Ea2(j＝1,2,…,Nm-1)时，其中Ea1、Ea2为设置的阈值，以sj+1最小序号激光点为分群标志，根据激光点在S(k)中的序号依次从p1遍历到将点云数据分群，根据激光点在S(k)的序号，计算每个点群中序号最小的点与序号最大的点之间的距离筛选出大于阈值Lt的点群，记为点群Sd，1≤d≤Nd，Nd为点群的数量；
(3.5)拟合多边几何环境：
记直线方程为x＝A1dy+A2d，记qid为sd的第i个激光点(i＝1,2,…,N1)，N1d为Sd中激光点的数量，和为qid在机体系中的坐标，根据下式计算直线方程的参数A1d和A2d：
4.根据权利要求1所述的连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于：步骤(4)中计算k时刻四旋翼飞行器相对于多边几何环境的相对偏航角及其在导航系下的坐标方法为：
(4.1)确定参考坐标系：
参考坐标系原点为参考边中点，Z轴竖直向下，X轴垂直指向参考边，Y轴根据右手定则确定，参考边确定方法如下：
①计算预测偏航角ψc，方法如下：
a.若则四旋翼飞行器的初始偏航角ψini为：
b.若则四旋翼飞行器的初始偏航角ψini为：
②根据权利要求6对偏航角的预测，对预测偏航角ψc的计算公式如下：ψc＝ψ+ψini
③根据预测的偏航角ψc，确定k时刻的参考边，计算方法如下：
a.若则参考边为A1d最大值对应的直线，其直线参数为
b.若则预测参考边斜率为：
计算(A1d-Ac)的值，当(A1d-Ac)取最小值时，对应的直线即为参考边，其直线参数为 E1A为设置的阈值，
(4.2)计算偏航角：
①若则四旋翼飞行器的偏航角ψl(k)为：
②若则四旋翼飞行器的偏航角ψl(k)为：
其中，四旋翼飞行器机头方向垂直指向于参考边的偏航角为0，顺时针旋转为正；
(4.3)计算在导航系下的坐标：
①参考边对应点群为分别计算该点群所拟合的直线与点群和点群所拟合直线的交点，分别记为点M、N，其在机体系下的坐标分别为计算线段MN的距离，记为lMN,计算公式如下：点M在导航系下的坐标为
②计算飞行器相对于参考边的距离，记为lref，计算公式如下：飞行器在导航系下x轴坐标为：xn＝-lref；
③计算直线与参考边所拟合直线的交点，记为点F，在机体系下的坐标为线段MF的长度计算公式为：飞行器在导航系下y轴坐标为
5.根据权利要求1所述的连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于：步骤(5)中计算k时刻多边几何环境的几何重心的方法为：
(5.1)几何重心为多边几何内接圆圆心，记计算机体系原点为初定内接圆圆心O，坐标记为到点群Sd对应的直线的距离Ld，计算公式如下：
(5.2)筛选出其中与圆心距离最短和次短的两条直线对应的点群Sp和Sq，最短距离为LA，与其对应的直线方程参数A1p、A2p以及A1q、A2q；建立直线方程(若A1p＝0，直线方程为若A1p＝∞，直线方程为)，分别计算其与对应点群直线的交点A、B，坐标为
(5.3)设一坐标点C，坐标为使得其中
在点C与原点延长线上取一点O'，坐标为O'为新的圆心坐标，其中
a为步长因子；
计算点O'到点群Sd对应的直线的距离，筛选出其中与点O'距离最短的直线，其距离为LO'；
(5.4)若LO'＞LA，重复(5.2)和(5.3)；若LO'＜LA，舍去O'，回到上一步确定的圆心坐标，将步长因子改为原来的0.618，重复(5.1)和(5.2)，重新计算圆心坐标。直至a小于阈值Δa，记录机体系下O'坐标；
(5.5)O'即为k时刻多边几何环境的几何重心，在导航系下的坐标为计算公式如下：
其中，为气压高度计信息，ψl为偏航角，T2×2为(k-1)时刻飞行器在导航系下的位置。
6.根据权利要求1所述的连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于：步骤(6)中通过卡尔曼滤波器，对k时刻四旋翼飞行器的姿态、速度、位置进行校正：
(6.1)计算一步预测均方误差Pk|k-1：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
式中， I3×3为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；G为滤波器k-1时刻的滤波器噪声系数矩阵，W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
(6.2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
式中，Λa＝[1 0 0],Λb＝[0 1 0]，Λc＝[0 0 -1]，Θa＝[q0(k-1)2+q1(k-1)2-q2(k-1)2-q3(k-1)2 2(q1(k-1)q2(k-1)+q0(k-1)q3(k-1)) 2(q1(k-1)q3(k-1)-q0(k-1)q2(k-1))]，Θb＝[2(q1(k-1)q2(k-1)-q0(k-1)q3(k-1)) q0(k-1)2+q2(k-1)2-q1(k-1)2-q3(k-1)2 2(q2(k-1)q3(k-1)+q0(k-1)q1(k-1))]， H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益，为k时刻的量测噪声，diag表示矩阵对角化，其中分别为ψl、d、hb的噪声；kHx、kHy为模型参数，均为常数，通过离线辨识方法获得；04×1为4×1的零矩阵，03×1为3×1的零矩阵，06×1为6×1的零矩阵；
(6.3)计算k时扩展卡尔曼滤波器状态估计值
式中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤二的预测公式计算得到，Y(k)＝[ψl(k) xn yn hb(k)]T为k时刻的量测值，通过步骤四和步骤五获得；
(6.4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
CN201810579682.4A 2018-06-07 2018-06-07 连续多边几何环境中四旋翼飞行器激光雷达导航方法 Active CN108562289B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810579682.4A CN108562289B (zh) 2018-06-07 2018-06-07 连续多边几何环境中四旋翼飞行器激光雷达导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810579682.4A CN108562289B (zh) 2018-06-07 2018-06-07 连续多边几何环境中四旋翼飞行器激光雷达导航方法

Publications (2)

Publication Number Publication Date
CN108562289A true CN108562289A (zh) 2018-09-21
CN108562289B CN108562289B (zh) 2021-11-26

Family

ID=63553137

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810579682.4A Active CN108562289B (zh) 2018-06-07 2018-06-07 连续多边几何环境中四旋翼飞行器激光雷达导航方法

Country Status (1)

Country Link
CN (1) CN108562289B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737968A (zh) * 2019-03-07 2019-05-10 中山大学 基于二维LiDAR和智能手机的室内融合定位方法
CN109901606A (zh) * 2019-04-11 2019-06-18 大连海事大学 一种用于四旋翼精确轨迹跟踪的混合有限时间控制方法
CN110749327A (zh) * 2019-08-08 2020-02-04 南京航空航天大学 一种合作环境下的车辆导航方法
WO2020220729A1 (zh) * 2019-04-29 2020-11-05 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN112084875A (zh) * 2020-08-11 2020-12-15 南京航空航天大学 一种多激光雷达坐标系统一方法

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CN103528587A (zh) * 2013-10-15 2014-01-22 西北工业大学 自主组合导航系统
CN103941744A (zh) * 2013-01-18 2014-07-23 通用电气航空系统有限责任公司 用于确定飞行路径的方法
CN104503339A (zh) * 2015-01-05 2015-04-08 黑龙江工程学院 基于激光雷达和四轴飞行器的多分辨室内三维场景重构装置及方法
US9007570B1 (en) * 2013-01-11 2015-04-14 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Airborne wind profiling algorithm for Doppler Wind LIDAR
CN105334518A (zh) * 2015-11-30 2016-02-17 南京大学 一种基于室内四旋翼飞行器的激光雷达三维成像方法
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN105460210A (zh) * 2015-12-04 2016-04-06 上海浦江桥隧大桥管理有限公司 用于桥梁检测的便携式六旋翼飞行器
CN105759829A (zh) * 2016-04-12 2016-07-13 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
CN105910602A (zh) * 2016-05-30 2016-08-31 南京航空航天大学 一种组合导航方法
CN106226780A (zh) * 2016-07-26 2016-12-14 南京航空航天大学 基于激光扫描雷达的多旋翼室内定位系统及实现方法
US20170023937A1 (en) * 2015-07-24 2017-01-26 The Trustees Of The University Of Pennsylvania Systems, devices, and methods for on-board sensing and control of micro aerial vehicles
CN106979781A (zh) * 2017-04-12 2017-07-25 南京航空航天大学 基于分布式惯性网络的高精度传递对准方法
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
EP3306344A1 (en) * 2016-10-07 2018-04-11 Leica Geosystems AG Flying sensor

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
US9007570B1 (en) * 2013-01-11 2015-04-14 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Airborne wind profiling algorithm for Doppler Wind LIDAR
CN103941744A (zh) * 2013-01-18 2014-07-23 通用电气航空系统有限责任公司 用于确定飞行路径的方法
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CN103528587A (zh) * 2013-10-15 2014-01-22 西北工业大学 自主组合导航系统
CN104503339A (zh) * 2015-01-05 2015-04-08 黑龙江工程学院 基于激光雷达和四轴飞行器的多分辨室内三维场景重构装置及方法
US20170023937A1 (en) * 2015-07-24 2017-01-26 The Trustees Of The University Of Pennsylvania Systems, devices, and methods for on-board sensing and control of micro aerial vehicles
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN105334518A (zh) * 2015-11-30 2016-02-17 南京大学 一种基于室内四旋翼飞行器的激光雷达三维成像方法
CN105460210A (zh) * 2015-12-04 2016-04-06 上海浦江桥隧大桥管理有限公司 用于桥梁检测的便携式六旋翼飞行器
CN105759829A (zh) * 2016-04-12 2016-07-13 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
CN105910602A (zh) * 2016-05-30 2016-08-31 南京航空航天大学 一种组合导航方法
CN106226780A (zh) * 2016-07-26 2016-12-14 南京航空航天大学 基于激光扫描雷达的多旋翼室内定位系统及实现方法
EP3306344A1 (en) * 2016-10-07 2018-04-11 Leica Geosystems AG Flying sensor
CN106979781A (zh) * 2017-04-12 2017-07-25 南京航空航天大学 基于分布式惯性网络的高精度传递对准方法
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
JIZHOU LAI等: "Research on an Autonomously Tightly Integrated Positioning Method for UAV in Sparse-feature Indoor Environment", 《2018 15TH INTERNATIONAL BHURBAN CONFERENCE ON APPLIED SCIENCES AND TECHNOLOGY (IBCAST)》 *
冯登超等: "面向低空安全三维数字化空中走廊体系的飞行器交通管理平台构建", 《计算机测量与控制》 *
张航等: "基于无人机和LIDAR的三维场景建模研究", 《系统仿真学报》 *
杨景阳: "微惯性辅助的激光雷达室内同步建图与定位技术研究", 《中国优秀博硕士学位论文全文数据库(硕士) 工程科技Ⅱ辑》 *
杭义军等: "基于混合特征匹配的微惯性/激光雷达组合导航方法", 《航空学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737968A (zh) * 2019-03-07 2019-05-10 中山大学 基于二维LiDAR和智能手机的室内融合定位方法
CN109901606A (zh) * 2019-04-11 2019-06-18 大连海事大学 一种用于四旋翼精确轨迹跟踪的混合有限时间控制方法
WO2020220729A1 (zh) * 2019-04-29 2020-11-05 南京航空航天大学 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN110749327A (zh) * 2019-08-08 2020-02-04 南京航空航天大学 一种合作环境下的车辆导航方法
CN112084875A (zh) * 2020-08-11 2020-12-15 南京航空航天大学 一种多激光雷达坐标系统一方法
CN112084875B (zh) * 2020-08-11 2024-06-11 南京航空航天大学 一种多激光雷达坐标系统一方法

Also Published As

Publication number Publication date
CN108562289B (zh) 2021-11-26

Similar Documents

Publication Publication Date Title
CN110631593B (zh) 一种用于自动驾驶场景的多传感器融合定位方法
You et al. Data fusion of UWB and IMU based on unscented Kalman filter for indoor localization of quadrotor UAV
CN108562289A (zh) 连续多边几何环境中四旋翼飞行器激光雷达导航方法
CN106767853B (zh) 一种基于多信息融合的无人驾驶车辆高精度定位方法
Bevly Global positioning system (GPS): A low-cost velocity sensor for correcting inertial sensor errors on ground vehicles
EP3410249B1 (en) System to estimate wind direction and strength using constant bank angle turn
EP3128386B1 (en) Method and device for tracking a moving target from an air vehicle
US10114373B2 (en) Navigation system with trajectory calculation mechanism and method of operation thereof
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
Cappello et al. A low-cost and high performance navigation system for small RPAS applications
CN108536163B (zh) 一种单面结构环境下的动力学模型/激光雷达组合导航方法
Petrich et al. On-board wind speed estimation for uavs
KR20150043392A (ko) 바람 벡터 추산
US11624611B1 (en) Self-locating compass
Sabatini et al. Navigation and guidance system architectures for small unmanned aircraft applications
Suwandi et al. Low-cost IMU and GPS fusion strategy for apron vehicle positioning
WO2021112074A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
CN108592911A (zh) 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
CN107063248A (zh) 基于旋翼转速的动力学模型辅助惯导的导航方法
Xia et al. Integrated emergency self-landing method for autonomous uas in urban aerial mobility
US11965940B2 (en) Self-locating compass
Yun et al. Range/optical flow-aided integrated navigation system in a strapdown sensor configuration

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant