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

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

Laser radar navigation method for four rotor aircraft in continuous multilateral geometry environment

The invention discloses a laser radar navigation method for a Four-rotor aircraft in a continuous polygonal geometry environment, belonging to the field of autonomous navigation and guidance. The invention can be used for the navigation of a quad-rotor aircraft in a multilateral geometric environment. The laser radar installed on the quad-rotor aircraft is fused with an inertial sensor to generate navigation information so that the multi-rotor aircraft can fly at the center of gravity of the multilateral geometric environment. In the invention, the polygonal geometric environment is identified by a lidar, and the relative distance and the azimuth between the Four-rotor aircraft and each side of the environment are estimated. The estimation information of the lidar is fused with the inertial sensor by the filter, and the position, velocity and attitude information of the quad-rotor aircraft are obtained. Then, the geometric center of gravity of the environment is calculated by the data measured by the lidar, so that the Four-rotor aircraft flies at the geometric center of gravity. The invention can make the four rotor aircraft fly safely and steadily in the multilateral geometry environment.

【技术实现步骤摘要】
连续多边几何环境中四旋翼飞行器激光雷达导航方法
本专利技术涉及自主导航与领域，尤其涉及连续多边几何环境中四旋翼飞行器激光雷达导航方法。
技术介绍
随着四旋翼飞行器相关技术的发展，其被应用于越来越多的飞行任务，对导航性能提出了越来越高的要求。激光雷达是四旋翼飞行器常用的导航设备之一，其不受无线电信号和光线干扰，具有较强的自主性与较高的导航精度。当激光雷达用于导航时，普遍采用同时定位与地图构建方法(SimultaneousLocalizationAndMapping，简称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＝[00g]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，在机体系下本文档来自技高网...

【技术保护点】
1.一种连续多边几何环境中四旋翼飞行器激光雷达导航方法，其特征在于，包括以下步骤：(1)周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达信息S(k)，陀螺仪信息

【技术特征摘要】
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＝[00g]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的计算公式如下...

【专利技术属性】
技术研发人员：邹冰倩，吕品，赖际舟，石鹏，付相可，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32