CN111486841B - 一种基于激光定位系统的无人机导航定位方法 - Google Patents

Abstract

一种基于激光定位系统的无人机导航定位方法，步骤1，无人机传感器健康检查，根据传感器输出的数据剔除坏点数据；步骤2，通过激光加惯性导航系统进行组合导航滤波；步骤3，组合导航算法的一致性检查和组合导航算法健康检查，得到稳定的无人机姿态和位置数据。本发明的Laser(激光)+INS(Inertial navigation system惯性导航系统)进行组合导航滤波，这种方法不仅可以实时输出无人机的位置和姿态信息，还可以得到很高的位置和姿态精度，并且可以得到平滑精准的无人机速度，还可以降低无线数据链配置要求。

Description

一种基于激光定位系统的无人机导航定位方法

技术领域

本发明属于无人机定位技术领域，特别涉及一种基于激光定位系统的无人机导航定位 方法。

背景技术

为了便于研究无人机集群算法，编队协同算法，人工智能算法，智能控制算法，路径规划算法，智能避障算法的研究，搭建了室内定位系统实验室，以无人机作为被控对象进行算法研究。搭建室内定位系统可以快速方便的进行科学研究，节省时间，缩短研究周期，避免了由于GPS信号弱或者接受信号不良导致由此导致的定位不准带来的问题，有 利于科研人员更专注于集群算法，编队协同算法，人工智能算法，智能控制算法，路径规 划算法，智能避障算法等的研究。

由于受限于目前民用无人机数据链硬件以及研究成本的原因，作为运动载体的无人 机无法接受数据刷新如此快且数据量如此大的数据。

发明内容

本发明的目的在于提供一种基于激光定位系统的无人机导航定位方法，以解决上述 问题。

为实现上述目的，本发明采用以下技术方案：

一种基于激光定位系统的无人机导航定位方法，所述激光定位系统包括激光定位塔、 Hub集线器、AP控制器和定位件；若干激光定位塔均连接到Hub集线器，Hub集线器通 过无线路由器连接到AP控制器，AP控制器连接到定位件；无线路由器还连接到服务器； 激光定位塔、Hub集线器和AP控制器作为激光定位系统的发射端，定位件作为激光定位 系统的接收端；包括以下步骤：

步骤1，无人机传感器健康检查，根据传感器输出的数据剔除坏点数据，具体包括激 光定位系统数据健康检查和陀螺仪以及加速度计健康检查；

步骤2，通过激光加惯性导航系统进行组合导航滤波，以激光定位系统定位件输出的 无人机的姿态和位置数据作为量测信息，通过序贯kalman滤波的方式进行组合导航算法， 进而输出高频率、高精度的无人机自身位置和姿态数据；

步骤3，组合导航算法的一致性检查和组合导航算法健康检查，得到稳定的无人机姿 态和位置数据。

进一步的，激光定位塔用来标定过零信号，并实现光塔的编码，进而实现空间内安装 多个光塔分时扫描；Hub集线器通过自己的通信端口与其控制的各个激光定位塔连接，用 来控制各个激光定位塔的工作方式，实现所有光塔的同步；AP控制器用来连接Hub集线器，实现对整个定位空间所有Hub集线器以及激光定位塔的控制，实现配置整个工作组 网的工作模式以及状态；定位件将无人机在激光定位系统空间内的局部坐标系下的位置和姿态信息传递给无人机飞控，飞控结合无人机上的惯导设备进行组合导航。

进一步的，步骤1中，激光定位系统数据健康检查：

a)根据布置的激光定位空间以及激光定位光塔的扫描盲区进行位置超限判断，如果 位置数据超限不进入组合导航算法，只通过机载惯导进行状态递推；

b)激光定位系统位置和姿态数据失联判断，如果100ms内无法接收到激光定位系统 数据，向上位机报错，并通过机载惯导进行状态递推；

c)根据实际无人机的最大飞行速度v_max，以及设置的激光定位系统刷新频率并转换 为刷新时间间隔dt，计算最大的位置pos_max＝v_max*dt，如果激光定位系统输出的相邻两针之间的位置变化超过最大位置pos_max，那么改帧数据超限，该数据不进入组合导航滤波器，只进行一步状态预测。

进一步的，步骤1中，陀螺仪以及加速度计健康检查：

a)陀螺仪和加速度计数据是否校准，如果校准参数为空，那么陀螺和加计未校准，使用默认参数；

b)陀螺和加计数据是否超限，如果超限不使用该数据；

c)陀螺和加计数据是否失联，如果失联，上报上位机。

进一步的，步骤2中，组合导航滤波：

1)坐标系定义

选取激光定位系统自身定义的坐标系为导航坐标系为n系；无人机重心为原点的前右 下坐标系为机体坐标系，为b系；

2)状态方程

选取的n系下的姿态，速度，位置，加速度计零偏，陀螺仪零偏作为组合导航算法的状态变量向量，共16维状态方程，即：

X＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_z gyro_bias_x gyro_bias_y gyro_bias_z acc_bias_x acc_bias_y acc_bias_z]

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴无人机速度；

[p_x p_y p_z]为n系下的三轴无人机位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏。

3)序贯滤波进行状态估计。

进一步的，序贯滤波进行状态估计包括：

1)状态一步预测：

使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态变量的一步预 测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化， Δθ是陀螺在时间段[t_m-1,t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪一般采用 角增量输出采样的方式，只需要简单地将其乘以采样间隔时间T_s即可变换为角增量；

速度更新

对于低中速行驶的运载体，例如地速v<100m/s，忽略地球自转以及地球曲率的影响， 速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在 时间段[t_m-1,t_m]时间内输出的比力增量，实际中可以采用比例乘以采样间隔进行近似，式 中的g为重力加速度矢量；

位置更新

陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值；

加速度计bias更新

根据状态更新方程，求取状态转移矩阵记为F；

2)误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵；

3)计算kalman滤波增益

4)状态更新

激光定位系统获取得到的无人机的姿态和位置信息作为量测信息，根据序贯滤波第三 步计算的卡拉曼滤波增益对状态状态变量进行更新；

5)误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/k-1

完成一次滤波状态和协方差矩阵更新后，滤波器可以得到陀螺和加计的bias估计值， 在进行下一轮的状态更新之前可以对采样得到角增量和速度增量进行状态修正，从陀螺仪， 加速度计采用得到的角增量和速度增量减去陀螺仪和加速度计的零偏，以此来对IMU数 据进行修正。

进一步的，一致性检查：

定义k时刻的测量向量为Z_k，通过Z_k减去量测矩阵H与序贯滤波组合导航算法一步预测得到的状态向量

二者的乘积

即

Res称为信息，信息的期望

H为量测矩阵；

P_k-1为一步协方差预测矩阵；

Q_k为过程噪声；

R_k为量测噪声。

正常情况下，下式成立

如果在进行组合导航算法过程中，出现Ratio>1的情况，那么该时刻的测量向量不应 该进入组合导航算法，组合导航算法只进行一步预测。

进一步的，协方差负定检查：

根据kalman滤波原理，协方差矩阵P_k/k-1是对称正定矩阵，对称正定矩阵表示如下

设A∈R^n×n，若A＝A^T，对任意0≠X∈Rⁿ，都有X^TAX>0，则称A为对称正定矩 阵；

如果协方差矩阵P_k/k-1出现负定，那么表示组合导航算法计算错误，需要重置协方差 矩阵。

与现有技术相比，本发明有以下技术效果：

本发明的Laser(激光)+INS(Inertial navigation system惯性导航系统)进行组合导 航滤波，这种方法不仅可以实时输出无人机的位置和姿态信息，还可以得到很高的位置和 姿态精度，并且可以得到平滑精准的无人机速度，还可以降低无线数据链配置要求。

本发明可以实现在室内情况下对无人机运动速度的估计；

本发明可以降低对数据链路的要求；

本发明可以使研究人员更专注于上层算法科学以及工程研究。

附图说明

图1是激光定位系统信息流图

图2是序贯kalman滤波流程图。

具体实施方式

以下结合附图对本发明进一步说明：

请参阅图1至图2，本发明分为三部分：第一部分，传感器健康检查；第二部分，laser+INS组合导航算法详细说明；第三部分，组合导航算法的健康检查。

传感器健康主要是根据传感器输出的数据剔除坏点数据，以便后续的组合导航算法 能稳定健康的运行。在整个无人机系统中，包含如下传感器：

1)激光定位系统传感器；

2)陀螺仪；

3)加速度计；

4)气压计。

步骤1，无人机传感器健康检查，根据传感器输出的数据剔除坏点数据，具体包括激 光定位系统数据健康检查和陀螺仪以及加速度计健康检查；

步骤2，通过激光加惯性导航系统进行组合导航滤波，以激光定位系统定位件输出的 无人机的姿态和位置数据作为量测信息，通过序贯kalman滤波的方式进行组合导航算法， 进而输出高频率、高精度的无人机自身位置和姿态数据；

步骤3，组合导航算法的一致性检查和组合导航算法健康检查，得到稳定的无人机姿 态和位置数据。

步骤1中，激光定位系统数据健康检查：

a)根据布置的激光定位空间以及激光定位光塔的扫描盲区进行位置超限判断，如果 位置数据超限不进入组合导航算法，只通过机载惯导进行状态递推；

b)激光定位系统位置和姿态数据失联判断，如果100ms内无法接收到激光定位系统 数据，向上位机报错，并通过机载惯导进行状态递推；

c)根据实际无人机的最大飞行速度v_max，以及设置的激光定位系统刷新频率并转换 为刷新时间间隔dt，计算最大的位置pos_max＝v_max*dt，如果激光定位系统输出的相邻两针之间的位置变化超过最大位置pos_max，那么改帧数据超限，该数据不进入组合导航滤波器，只进行一步状态预测。

步骤1中，陀螺仪以及加速度计健康检查：

a)陀螺仪和加速度计数据是否校准，如果校准参数为空，那么陀螺和加计未校准，使用默认参数；

b)陀螺和加计数据是否超限，如果超限不使用该数据；

c)陀螺和加计数据是否失联，如果失联，上报上位机。

步骤2中，组合导航滤波：

1)坐标系定义

选取激光定位系统自身定义的坐标系为导航坐标系为n系；无人机重心为原点的前右 下坐标系为机体坐标系，为b系；

2)状态方程

选取的n系下的姿态，速度，位置，加速度计零偏，陀螺仪零偏作为组合导航算法的状态变量向量，共16维状态方程，即：

X＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_z gyro_bias_x gyro_bias_y gyro_bias_z acc_bias_x acc_bias_y acc_bias_z]

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴无人机速度；

[p_x p_y p_z]为n系下的三轴无人机位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏。

3)序贯滤波进行状态估计。

序贯滤波进行状态估计包括：

1)状态一步预测：

使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态变量的一步预 测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化， Δθ是陀螺在时间段[t_m-1,t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪一般采用 角增量输出采样的方式，只需要简单地将其乘以采样间隔时间T_s即可变换为角增量；

速度更新

对于低中速行驶的运载体，例如地速v<100m/s，忽略地球自转以及地球曲率的影响， 速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在 时间段[t_m-1,t_m]时间内输出的比力增量，实际中可以采用比例乘以采样间隔进行近似，式 中的g为重力加速度矢量；

位置更新

陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值；

加速度计bias更新

根据状态更新方程，求取状态转移矩阵记为F；

2)误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵；

3)计算kalman滤波增益

4)状态更新

激光定位系统获取得到的无人机的姿态和位置信息作为量测信息，根据序贯滤波第三 步计算的卡拉曼滤波增益对状态状态变量进行更新；

5)误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/k-1

完成一次滤波状态和协方差矩阵更新后，滤波器可以得到陀螺和加计的bias估计值， 在进行下一轮的状态更新之前可以对采样得到角增量和速度增量进行状态修正，从陀螺仪， 加速度计采用得到的角增量和速度增量减去陀螺仪和加速度计的零偏，以此来对IMU数 据进行修正。

一致性检查：

定义k时刻的测量向量为Z_k，通过Z_k减去量测矩阵H与序贯滤波组合导航算法一步预测得到的状态向量

二者的乘积

即

Res称为信息，信息的期望

H为量测矩阵；

P_k-1为一步协方差预测矩阵；

Q_k为过程噪声；

R_k为量测噪声。

正常情况下，下式成立

如果在进行组合导航算法过程中，出现Ratio>1的情况，那么该时刻的测量向量不应 该进入组合导航算法，组合导航算法只进行一步预测。

协方差负定检查：

根据kalman滤波原理，协方差矩阵P_k/k-1是对称正定矩阵，对称正定矩阵表示如下

设A∈R^n×n，若A＝A^T，对任意0≠X∈Rⁿ，都有X^TAX>0，则称A为对称正定矩 阵；

如果协方差矩阵P_k/k-1出现负定，那么表示组合导航算法计算错误，需要重置协方差 矩阵。

Claims (6)

1.一种基于激光定位系统的无人机导航定位方法，其特征在于，所述激光定位系统包括激光定位塔、Hub集线器、AP控制器和定位件；若干激光定位塔均连接到Hub集线器，Hub集线器通过无线路由器连接到AP控制器，AP控制器连接到定位件；无线路由器还连接到服务器；激光定位塔、Hub集线器和AP控制器作为激光定位系统的发射端，定位件作为激光定位系统的接收端；包括以下步骤：

步骤1，无人机传感器健康检查，根据传感器输出的数据剔除坏点数据，具体包括激光定位系统数据健康检查和陀螺仪以及加速度计健康检查；

步骤2，通过激光加惯性导航系统进行组合导航滤波，以激光定位系统定位件输出的无人机的姿态和位置数据作为量测信息，通过序贯kalman滤波的方式进行组合导航算法，进而输出高频率、高精度的无人机自身位置和姿态数据；

步骤3，组合导航算法的一致性检查和组合导航算法健康检查，得到稳定的无人机姿态和位置数据；

激光定位塔用来标定过零信号，并实现光塔的编码，进而实现空间内安装多个光塔分时扫描；Hub集线器通过自己的通信端口与其控制的各个激光定位塔连接，用来控制各个激光定位塔的工作方式，实现所有光塔的同步；AP控制器用来连接Hub集线器，实现对整个定位空间所有Hub集线器以及激光定位塔的控制，实现配置整个工作组网的工作模式以及状态；定位件将无人机在激光定位系统空间内的局部坐标系下的位置和姿态信息传递给无人机飞控，飞控结合无人机上的惯导设备进行组合导航；

步骤1中，激光定位系统数据健康检查：

a)根据布置的激光定位空间以及激光定位光塔的扫描盲区进行位置超限判断，如果位置数据超限不进入组合导航算法，只通过机载惯导进行状态递推；

b)激光定位系统位置和姿态数据失联判断，如果100ms内无法接收到激光定位系统数据，向上位机报错，并通过机载惯导进行状态递推；

c)根据实际无人机的最大飞行速度v_max，以及设置的激光定位系统刷新频率并转换为刷新时间间隔dt，计算最大的位置pos_max＝v_max*dt，如果激光定位系统输出的相邻两帧之间的位置变化超过最大位置pos_max，那么该帧数据超限，该数据不进入组合导航滤波器，只进行一步状态预测。

2.根据权利要求1所述的一种基于激光定位系统的无人机导航定位方法，其特征在于，步骤1中，陀螺仪以及加速度计健康检查：

a)陀螺仪和加速度计数据是否校准，如果校准参数为空，那么陀螺和加速度计未校准，使用默认参数；

b)陀螺和加速度计数据是否超限，如果超限不使用该数据；

c)陀螺和加速度计数据是否失联，如果失联，上报上位机。

3.根据权利要求1所述的一种基于激光定位系统的无人机导航定位方法，其特征在于，步骤2中，组合导航滤波：

1)坐标系定义

选取激光定位系统自身定义的坐标系为导航坐标系为n系；无人机重心为原点的前右下坐标系为机体坐标系，为b系；

2)状态方程

选取的n系下的姿态，速度，位置，加速度计零偏，陀螺仪零偏作为组合导航算法的状态变量向量，共16维状态方程，即：

X＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_z

gyro_bias_x gyro_bias_y gyro_bias_z acc_bias_x acc_bias_y acc_bias_z]

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴无人机速度；

[p_x p_y p_z]为n系下的三轴无人机位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏；

3)序贯滤波进行状态估计。

4.根据权利要求3所述的一种基于激光定位系统的无人机导航定位方法，其特征在于，序贯滤波进行状态估计包括：

1)状态一步预测：

使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态变量的一步预测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化，Δθ是陀螺在时间段[t_m-1，t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪采用角增量输出采样的方式，将其乘以采样间隔时间T_s即可变换为角增量；

速度更新

对于低中速行驶的运载体，低速v＜100m/s，忽略地球自转以及地球曲率的影响，速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在时间段[t_m-1，t_m]时间内输出的比力增量，实际中采用比例乘以采样间隔进行近似，式中的g为重力加速度矢量；

位置更新

陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值；

加速度计bias更新

根据状态更新方程，求取状态转移矩阵记为F；

2)误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵；

3)计算kalman滤波增益

4)状态更新

激光定位系统获取得到的无人机的姿态和位置信息作为量测信息，根据序贯滤波第三步计算的卡拉曼滤波增益对状态变量进行更新；

5)误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/k-1

完成一次滤波状态和协方差矩阵更新后，滤波器得到陀螺和加计的bias估计值，在进行下一轮的状态更新之前对采样得到角增量和速度增量进行状态修正，从陀螺仪，加速度计采用得到的角增量和速度增量减去陀螺仪和加速度计的零偏，以此来对IMU数据进行修正。

5.根据权利要求1所述的一种基于激光定位系统的无人机导航定位方法，其特征在于，一致性检查：

定义k时刻的测量向量为Z_k，通过Z_k减去量测矩阵H与序贯滤波组合导航算法一步预测得到的状态向量

二者的乘积

即

Res称为信息，信息的期望

H为量测矩阵；

P_k-1为一步协方差预测矩阵；

Q_k为过程噪声；

R_k为量测噪声；

正常情况下，下式成立

如果在进行组合导航算法过程中，出现Ratio＞1的情况，那么该时刻的测量向量不应该进入组合导航算法，组合导航算法只进行一步预测。

6.根据权利要求1所述的一种基于激光定位系统的无人机导航定位方法，其特征在于，协方差负定检查：

根据kalman滤波原理，协方差矩阵P_k/k-1是对称正定矩阵，对称正定矩阵表示如下

设A∈R^n×n，若A＝A^T，对任意0≠X∈Rⁿ，都有X^TAX＞0，则称A为对称正定矩阵；

如果协方差矩阵P_k/k-1出现负定，那么表示组合导航算法计算错误，需要重置协方差矩阵。

Images