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

文档序号：15489275发布日期：2018-09-21 20:18阅读：233来源：国知局

本发明涉及自主导航与领域，尤其涉及连续多边几何环境中四旋翼飞行器激光雷达导航方法。

背景技术：

随着四旋翼飞行器相关技术的发展，其被应用于越来越多的飞行任务，对导航性能提出了越来越高的要求。激光雷达是四旋翼飞行器常用的导航设备之一，其不受无线电信号和光线干扰，具有较强的自主性与较高的导航精度。当激光雷达用于导航时，普遍采用同时定位与地图构建方法(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轴坐标为：xⁿ＝-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＝[100],λb＝[010]，λc＝[00-1]，θa＝[q0(k-1)²+q1(k-1)²-q2(k-1)²-q3(k-1)²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)²+q2(k-1)²-q1(k-1)²-q3(k-1)²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)xⁿyⁿ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时刻四旋翼飞行器的姿态、速度、位置，具体方法如下：