CN102928846B - 小型无人直升机超低空激光雷达数字地形测绘系统及方法 - Google Patents

Abstract

本发明公开了一种小型无人直升机超低空激光雷达数字地形测绘系统及方法，包括负责各传感器数据的正确采集、对传感器数据进行同步处理以及无人直升机的导航控制，并通过无线收发设备实现与地面站系统数据交互的机载系统和用于接收和处理来自机载激光雷达硬件系统的数据，或离线查看和处理数据文件的地面站系统，所述机载系统由协处理器和主处理器的组成，所述协处理器用于采集系统的姿态数据和位置数据，或实现对机载系统的导航控制。本发明采用协处理器配合主处理器的模式，既保证了对小型无人直升机导航控制的实时性和稳定性要求，也使得系统更加灵活,可以方便的扩展机载系统的功能，同时具备低功耗、低成本、高度集成、高效率等优点。

Description

小型无人直升机超低空激光雷达数字地形测绘系统及方法

技术领域

本发明涉及地形测绘的技术领域，特别涉及一种可以实现全自动、无接触和动态实时的小型无人直升机超低空数字地形测绘系统及方法。

背景技术

机载激光雷达技术，是一种集高精度动态DGPS技术、激光测距技术、高精度载体姿态测量技术和计算机技术于一体的新兴测量技术。通过激光测距仪得到的测距信息联合姿态测量模块得到的姿态信息和高精度差分GPS得到的位置信息能够解算获得精确的物体三维坐标和高分辨率数字地形模型。它为人们对地球空间信息的连续自动获取，提供了一种新型遥感技术方法，相比传统的人工单点数据获取方式，其在自动化和智能化方面前进了一大步，使观测精度和速度显著提高。

目前，利用机载激光雷达技术获得地球空间信息给传统的测绘带来了一场新的技术革新，激光雷达设备的研制受到国内外科技企业和科研单位的高度重视，也取得了一定的成果。目前大多数作业的机载激光雷达系统都是大型机载系统，一般工作在高空，设备体积大、质量重，容易受到空域管制和天气影响。这些设备无法实时生成数字高程数据，无法根据采集的数据现场改变飞行航线，作业效率低，易造成人力和物力的浪费。所以开发小型、高度集成的激光雷达设备，基于小型无人直升机平台进行作业，挖掘低空领域的应用很有必要。此小型无人直升机超低空数字地形测绘系统，是一个基于小型无人直升机的低成本、全自动、在线生成高程模型的激光雷达数字地形测绘系统。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种小型无人直升机超低空激光雷达数字地形测绘系统。

本发明的另一目的在于，提供一种小型无人直升机超低空激光雷达数字地形测绘方法。

为了达到上述第一发明目的，本发明采用以下技术方案：

一种小型无人直升机超低空激光雷达数字地形测绘系统，包括负责各传感器数据的正确采集、对传感器数据进行同步处理以及无人直升机的导航控制，并通过无线收发设备实现与地面站系统数据交互的机载系统，和用于接收和处理来自机载激光雷达硬件系统的数据，或离线查看和处理数据文件的地面站系统，所述机载系统由协处理器模块和主处理器模块组成；所述协处理器模块用于采集系统的姿态数据和位置数据，以及实现对机载系统的导航控制，所述的主处理器模块用于采集协处理器模块与激光传感器的信号，并实现通过无线收发设备转发数据；所述的地面站系统实现与机载系统通信，接受来自机载系统的数据，并发送控制指令给机载系统，此外，地面站系统还实现了三维成图显示、数据编辑、数据滤波以及数据格式转换。

优选的，所述协处理器模块包括CPU处理器，与处理器信号连接的电子罗盘、IMU、GPS差分板以及通道舵机。

优选的，所述主处理器模块包括双核CPU处理器，与CPU处理器连接的协处理器模块，与CPU处理器模块连接的无线收发模块，通过USB转RS442与CPU处理器连接的激光扫描仪。

优选的，所述机载平台的处理器系统采用三层结构搭建，底层是协处理器电路板，实现数据采集和向上层传输；中间层是一个连接板，实现上下层数据的通讯桥梁；顶层是一个主处理器电路板，实现数据的同步设计和数据转发。

优选的，所述地面站系统包括便携式计算机服务器和无线通信设备。

优选的，还包括供电装置，所述供电装置为两个锂电池，根据需要分压到提供12V、9V、5V及3.3V的四种电压。

为了达到上述第二目的，本发明采用以下技术方案：

小型无人直升机超低空激光雷达数字地形测绘方法，包括下述步骤：

S1、协处理器模块采集传感器的数据，为了得到机载系统的正确状态，在对多传感器系统进行数据融合处理前，需要保证不同传感器数据的同步性，即要求各个传感器的数据在进行三维点云数据解算时是同一时刻的，协处理器模块不断主动通过SPI口采集IMU的数据，在一次协处理器模块CPU更新周期里面多次采样IMU数据，保证发送给主处理器模块的IMU数据是最新的；

S2、协处理器模块在将采集到的传感器数据打包成一个自定义的字符形式数据包，并将该数据包传给主处理器的模块，协处理器的数据包包括HMR3000航向角、IMU三轴角速度、IMU三轴加速度、GPS模式和GPS星数信息；

S3、主处理器模块采集协处理器模块和激光传感器的数据，分别对两种数据加入时间属性的标记，并通过无线收发模块把数据转发到地面站系统；

S4、地面站系统收到两种数据后保存数据，对于协处理器模块的数据进行扩展卡尔曼数据融合算法实现，选取小型无人直升机的非线性导航模型的系统在导航坐标系中的3个位置信息(x,y,z)，3个速度信息(u，v，w)，用四元数描述的机载系统姿态信息(q₀,q₁,q₂,q₃)和当地重力加速度g作为系统的状态向量；选取3个位置信息(x,y,z)，3个速度信息(u,v,w)，三个用欧拉角表示的机载系统姿态信息

θ,ψ)作为系统的观测向量；

S5、地面站系统利用卡尔曼数据融合后的姿态和位置信息，联合激光测距仪的测距信息进行三维解算，得到三维点云数据。

优选的，步骤S1中，保证数据实时性和同步性的具体方法是：GPS差分板和电子罗盘采用中断方式，有数据更新就产生中断，然后协处理器模块更新数据和各自的数据更新标志位，这样主处理器可以通过判断标志位来判断此时协处理器模块传来的GPS差分板或电子罗盘数据是不是最新的，这样数据到达主处理器模块的时间延迟就近似只有协处理器串口传送数据的时间，在主处理器模块的数据接收时刻减去这个时间就可以实现数据的同步性。

优选的，步骤S4中，在参数的选择中欧拉角表示的姿态角在机载平台大幅度姿态变化的时候会使欧拉角方程出现奇异现象，针对这一情况，用四元数来表示机载系统的姿态角，采用这种方法表示系统的姿态运动能消除这种奇异；四元数本质是一个向量，它的维度是4，其中的三个维度定义了空间中的一个转动轴，第四个维度是表示绕该转动轴的角度，四元素和欧拉角表示的姿态角可以通过下面两式相互转化：

$\stackrel{\→}{q}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}\cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ -\cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}2\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}-\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\end{array}\right].$

优选的，构建的小型无人直升机的非线性导航模型如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=f(X\left(t\right),u\left(t\right),t)\\ Z\left(t\right)=h(X\left(t\right),t)\end{array}\right.$

其中：

X=[x y z u v w q₀ q₁ q₂ q₃ g]^T

为系统的状态向量；

为系统的观测向量；

$f(x,u)=\left[\begin{array}{c}\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2\left(q_1^2{+q}_2^2\right)\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]-\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right]g-\left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\\ 0\end{array}\right]$

为系统的状态方程；这里的[q₀,q₁,q₂,q₃]是四元素表示的姿态角信息，

θ,ψ]为欧拉角表示的姿态角信息；[a_x,a_y,a_z]表示飞机在导航坐标系中的加速度；g为当地的重力加速度；[p,q,r]表示飞机在机体坐标系中分别沿三个轴旋转的角速度；

为系统的观测方程。

本发明相对于现有技术具有如下的优点及效果：

1、本发明激光雷达数字地形系统实现在线、实时采集地形数据并快速生成数字高程模型功能，可以在场分析DEM数据，根据需要即时改变系统飞行航线。

2、本发明在激光成图系统中，增加了对飞行器的控制功能，用同一套传感器设备实现高精度数字地形数据采集和无人直升机的导航控制。

3、本发明在处理器系统构建上，采用协处理器配合主处理器的模式，既保证了对小型无人直升机导航控制的实时性和稳定性要求，也使得系统更加灵活，可以方便的扩展机载系统的功能。

4、本发明自主实现的多传感器数据同步机制，使用硬件设计配合软件实现的同步方式，给不同传感器数据增加精确的时间属性，这个时间属性随同传感器数据一起被传送，在地面站系统根据这个时间属性来解算数据。

5、针对小型无人直升机平台，实现了十一阶扩展卡尔曼数据融合算法，有效的融合了各个传感器的数据，克服了传感器的缺点，为高精度点云数据的获得提供了基础。

6、本发明构建了基于小型无人直升机平台的机载激光雷达系统，实现低空贴地扫描飞行，便于进行工程实践作业，具有成本低、效率高、测绘条件简单等优点。

附图说明

图1是本发明的系统结构图；

图2是本实施例采用的协处理器的结构图；

图3是本实施例的主处理器图的结构示意图；

图4是本实施例机载系统供电方案的结构图；

图5是实施例的协处理器传感器数据采集时序流程图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

如图1所示，本实施例的基于小型无人直升机超低空机载激光雷达数字地形测绘系统的总体架构包括地面站系统和机载系统两大部分。为构建实用的轻小型系统，在充分保证系统正常工作时间和精度的基础上，选择重量轻、体积小和功耗低的传感器件。地面站系统由便携式计算机服务器和无线通讯设备两部分组成。机载系统包含激光测距仪及数据转换模块、电池及供电系统、GPS差分板及基准站数据无线接收模块、传感器数据采集系统、主控制器和无线通讯模块。

机载系统主要负责各传感器数据的正确采集、对传感器数据进行同步处理以及无人直升机的导航控制，并通过无线收发设备实现与地面站系统的数据交互。利用高动态的惯性导航单元IMU、高性能电子罗盘HMR3000以及高精度载波相位动态实时GPS差分板联合德国SICK公司的LMS291-S05激光测距仪得到的扫描距离信息。机载系统的设计除了要对这些传感器数据的正确采集，一个实现起来困难而重要的技术是保证这些数据的同步性，这样通过无线设备把这些数据传给地面站系统的时候，地面站系统才能够对这些数据进行正确的解算，从而得到正确的三维点云数据。

地面站系统用于接收和处理来自机载激光雷达硬件系统的数据，也可以离线查看和处理数据文件。地面站系统的构建是利用一台移动服务器，在它上面运行LINUX操作系统，利用C语言、GTK+和Opengl技术设计地面站软件系统。地面站系统主要实现机载系统整机控制操作、各传感器数据的融合算法处理、对机载系统发来数据的解算和海量数据的保存与处理、三维点云数据的实时动态显示、系统误差校正与精度分析、点云数据滤波配准等功能。

如图2所示在本发明的机载系统中，协处理器主要有两个方面作用：一方面用来采集系统的姿态数据和位置数据，即协处理器需要和惯性导航单元IMUADIS16365、电子罗盘HMR3000以及GPS差分板进行数据通讯。另一方面，用来实现对机载系统的导航控制。在这一小节中主要介绍协处理模块如何采集各个传感器的数据，具体的数据采集策略和同步处理机制在下面小节中讨论。

协处理器模块通过同步串行外设接口SPI与ADIS16365进行数据通讯，由于ARM7的外设接口电压为3.3V而ADIS16365为5V供电，故在协处理器模块上使用双向总线电平转换器SN74ALVC164245实现不同电平的正常通讯。SPI工作在主从模式下，协处理器的ARM7是主控制器，ADIS16365作为从控制器。主控制器产生串行时钟，串行时钟的频率不高于2MHz。主控制器选中并使能ADIS16365的SPI口，在16个串行时钟脉冲的驱动下发送16位序列，这16位序列包含读写命令和目标寄存器的地址。在紧接着的16个时钟脉冲里，ADIS16365把16位寄存器的内容发送到DOUT线上。由于SPI以全双工的模式工作，所以主控制器能够在接收DOUT线上ADIS16365发来16位数据的同时，利用同一串行时钟脉冲在DIN线上发送下一个目标寄存器地址。如此循环发送对应三轴加速度计和三轴角速度计的六个地址，读取相应六个寄存器的内容，即可以得到IMU的信息。

协处理器模块通过ARM7的通用同步/异步收发器USART1经过双通道RS232芯片ADM3202ARN与HMR3000的RS232异步串行口进行数据通讯。只要HMR3000一上电，就会在其串口输出字符形式的传感器信息报文，其频率为10Hz，波特率为19200bps。ARM7的USART1使用中断的方式接收HMR3000的数据报文并在中断服务子程序中解析报文信息，获得航向角的信息。中断处理程序首先检测数据包的字头，接着一次读取每个姿态角的数据，最后把字符数据转换为数值。

协处理模块采集OEM4-g2GPS差分板的数据方式和HMR3000类似，使用ARM7的DBUG口复用为通用同步/异步收发器的方式通过双通道RS232芯片ADM3202ARN进行数据通讯。ARM7同样通过中断的方式接收OEM4-g2GPS差分板的数据报文并在中断服务程序中接卸报文信息，得到机载系统的位置和速度信息。中断服务程序在这两个报文中得到相应的GPS位置和速度信息后，还需要经过坐标转换处理才能得到当地水平坐标系END下的位置和速度信息，这一部分数据处理的功能也在协处理器模块程序中实现。

最后协处理器模块在程序中将采集到的各传感器模块数据打包成一个自定义的字符形式数据包，通过ARM7的USART0口作为整个协处理器模块的输出，传给上位机主处理器模块，协处理器的数据包包括HMR3000航向角、IMU三轴角速度、IMU三轴加速度、GPS模式、GPS星数等信息。USART0使用RS232协议与主处理器模块进行数据通讯，波特率为115200bps。为使机载系统能够正常工作，协处理器模块采集各个模块信息的频率为75Hz，由于需要在每个工作周期内传输较大数据包，这里需要注意异步串口的带宽限制，这里以每个字节数据在异步串行传输时带一位起始位和一位停止位计算，则这个串口在每个工作周期内发送的数据包最大字节数SIZE为：SIZE＝115200÷75÷(8+1+1)＝153.6

所以协处理器系统一个周期里面最多能够传输153个字节数据，这对于目前构建的机载系统来说是够用的。

如图3所示，本发明的主处理器模块运行一个多线程的程序，它的主要功能有四个方面：（1）实现通过USB口与LMS291-S05激光测距仪进行通讯，采集激光测距仪信息；（2）接收来自协处理器的位置和姿态数据并进行解析；（3）实现不同传感器的数据同步处理，给不同数据打上时间标记；（4）通过无线收发模块，实现机载系统和地面站系统间的通讯。

为满足激光雷达系统地形测绘的需要，SICK LMS291-S05激光测距仪选择工作于RS422 500Kbps的高速模式，扫描角范围选取100°，扫描角分辨率0.25°，扫描频率为75Hz。在这种工作模式下，由于系统的通讯方式为500Kbps的高速非标准波特率RS422工作模式，直接与带标准RS232/422串口的处理器无法正常通讯。

本实施例选用USB-COMi-M USB转串口模块只需要在主处理器QMAP3530的linux操作系统内核中增加ftdi、ftdi_elan和support non-standard三个标准模块直接编译进内核就可以驱动USB转串口适配器。这给系统的灵活使用和移植带来了很大的便利。为使扫描仪能够正常工作，需要在主处理器模块上安装扫描仪的驱动程序，这样在程序中通过调用相关功能函数就能对扫描仪进行工作模式设置和采集数据操作。主处理器模块的程序中设计一个独立线程来接收并解析从协处理器模块传来的信息，当激光扫描仪或者协处理器模块传来的数据有更新时，就会调用无线发送函数通过无线收发模块往地面站系统传输数据。

地面站系统收到两种数据后保存数据，对于协处理器模块的数据进行扩展卡尔曼数据融合算法实现，选取系统的在导航坐标系中的3个位置信息(x,y,z)，3个速度信息(u,v,w)，用四元数描述的机载系统姿态信息(q₀,q₁,q₂,q₃)和当地重力加速度g作为系统的状态向量。选取3个位置信息(x,y,z)，3个速度信息(u,v,w)，三个用欧拉角表示的机载系统姿态信息

θ,ψ)作为系统的观测向量。这里参数的选择中欧拉角表示的姿态角在机载平台大幅度姿态变化的时候会使欧拉角方程出现奇异现象。针对这一情况，用四元数来表示机载系统的姿态角，采用这种方法表示系统的姿态运动能消除这种奇异。四元数本质是一个向量，它的维度是4，其中的三个维度定义了空间中的一个转动轴，第四个维度是表示绕该转动轴的角度。四元素和欧拉角表示的姿态角可以通过下面两式相互转化：

$\stackrel{\→}{q}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}\cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ -\cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}-\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\end{array}\right]$

本文构建的小型无人直升机的非线性导航模型如下

$\left\{\begin{array}{c}\stackrel{\·}{X}=f(X\left(t\right),u\left(t\right),t)\\ Z\left(t\right)=h(X\left(t\right),t)\end{array}\right.$

其中：

X=[x y z u v w q₀ q₁ q₂ q₃ g]^T

为系统的状态向量。

为系统的观测向量。

$f(x,u)=\left[\begin{array}{c}\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2\left(q_1^2{+q}_2^2\right)\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]-\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right]g-\left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\\ 0\end{array}\right]$

为系统的状态方程，这里的[q₀,q₁,q₂,q₃]是四元素表示的姿态角信息，

θ,ψ]为欧拉角表示的姿态角信息；[a_x,a_y,a_z]表示飞机在导航坐标系中的加速度；g为当地的重力加速度；[p,q,r]表示飞机在机体坐标系中分别沿三个轴旋转的角速度。

为系统的观测方程。

如图2所示，协处理器模块主要设计用于采集DGPS、IMU和电子罗盘的数据，然后将数据打包发送给主处理器模块。协处理器模块不断主动通过SPI口采集IMU的数据，在一次协处理器更新周期里面多次采样IMU数据，保证发送给上位机的IMU数据是最新的。如图5所示，为了保证数据的实时性和同步性，DGPS和电子罗盘采用中断方式，有数据更新就产生中断，然后协处理器更新数据和各自的数据更新标志位，这样主处理器可以通过判断标志位来判断此时协处理器传来的DGPS或电子罗盘数据是不是最新的。这样数据到达主处理器的时间延迟就近似只有协处理器串口传送数据的时间，这段时间很容易计算得到，在主处理器的接受时刻减去这各时间就可以了。

主处理器上运行linux操作系统，使用操作系统的系统时间作为整个机载系统的基准时间，其上设计一个多线程的程序，用来采集激光测距仪和协处理器的数据，一旦主处理器接收到激光测距仪或协处理器的数据就第一时间为这个数据标记上这个时间属性。然后主处理器把包含有这个时间标识属性的激光数据包或协处理器数据包分别发送到地面站系统。地面站系统通过识别数据包的类型属性分别存储包含时间属性的激光数据包和协处理器数据包数据。

如图4所示，本发明的供电方案考虑到系统搭载于小型无人直升机上，故需要提供独立的电源，综合考虑机载系统的载重和电池的电容量，本系统选用两个重量0.4kg，电容量6600mAh，标称电压11.1V的锂电池作为系统的电源。结合每个器件的对电源的具体要求，需要分压到提供12V/9V/5V/3.3V四种电压，其中IMU在设计的时候集成在协处理器模块中，经稳压芯片后的5V电压对其供电。

机载激光雷达系统需要集成DGPS、IMU、电子罗盘和激光测距仪等传感器件，是一个典型的多传感器系统。为了得到机载系统的正确状态，在对多传感器系统进行数据融合处理前，需要保证不同传感器数据的同步性，即要求各个传感器的数据在进行三维点云数据解算时是同一时刻的。本文构建的机载激光雷达系统在实际应用中，电子罗盘的数据更新频率为10Hz，DGPS的数据更新频率为20Hz，IMU的数据更新频率为约300Hz，而激光测距仪的数据更新频率为75Hz。显然，这四个传感器数据更新是不同步的，而且这是不可避免的。为了消除传感器的这种差异对机载激光雷达系统精度的影响，得到精确的三维点云数据，必须要解决多传感器间实时数据的同步问题。

本实施例中，卡尔曼滤波算法受系统的过程噪声阵Q和观测噪声阵R影响较大，这两个矩阵的选取会对滤波算法的鲁棒性和实用性产生直接的影响。过程噪声矩阵Q、观测噪声矩阵R分别为n×n维、m×m的对角矩阵，并且它们对角线上的元素都大于零。在工程实践中一般根据大量的实际机载飞行试验确定这两个矩阵的参数。卡尔曼滤波方程表明，观测噪声阵R将直接影响修正作用的强弱。其对角线上的元素数值越大，修正作用越微弱。在前期试验的一个重要工作之一就是确定过程噪声阵Q和观测噪声阵R，需要大量的试验。在现实情况中，难以得到准确的过程噪声阵Q。在实际的工程试验中，系统的硬件条件和外部条件是变化的，也会导致这两个矩阵产生变化，本文提出一种适合工程应用的确定这两个矩阵参数的方法并进行验证。

由根据卡尔曼滤波原理可知：

$Q=E\left[W_k{W}_k^T\right]$

假设无人直升机系统处于悬停状态是机载系统的平衡状态，并先调整机头使偏航角向正北。在理想的情况下所有的状态变量为

X₀=[0 0 hight 0 0 0 1 0 0 0 g₀]^T

Z₀=[0 0 hight 0 0 0 0 0 0 ]^T

其中hight为悬停点的高度参考值，g₀为当地的重力加速度参考值。若假设随机噪声X列

W=[w₁ w₂ w₃ w₄ w₅ w₆ w₇ w₈ w₉ w₁₀ w₁₁]^T

则

从悬停时刻开始采集n组飞机状态的数据X₁,X₂,…,X_n以及Z₁,Z₂,…,Z_n。用实时采集得到的状态的值与理想状态下变量值X₀、Z₀的差值作为各个时刻随机噪声的标量值来计算系统计算出系统的对过程噪声阵Q和观测噪声阵R：

本实施例的机载系统的搭建考虑到小型无人直升机的载重和重心配置，本文的小型无人直升机重量35kg，最大载重10kg，机身长约2米，最大飞行速度90km/h。整个机载激光雷达系统由激光测距仪、电子舱和电池三大部分组成，其中电子舱集成了IMU、HMR3000、GPS差分板、处理器系统等核心部件。这三部分的重量分别为4.5kg，1.4kg和0.8kg。为保持激光测距仪和电子舱的一致性，设计时必须把电子舱和激光测距仪通过硬连接固连在一起。考虑到无人直升机的重心偏后，将电子舱和激光测距仪放在机头位置，然后把电池放在尾杆，可通过调节电池位置平衡小型无人直升机。由于小型无人直升机发动机振动频率较大，直接将机载激光雷达系统与机体硬连接会导致系统振动剧烈，影响测量结果，为此设计了减震机构。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.小型无人直升机超低空激光雷达数字地形测绘方法，其特征在于，包括下述步骤：

S1、协处理器模块采集传感器的数据，为了得到机载系统的正确状态，在对多传感器系统进行数据融合处理前，需要保证不同传感器数据的同步性，即要求各个传感器的数据在进行三维点云数据解算时是同一时刻的，协处理器模块不断主动通过SPI口采集IMU的数据，在一次协处理器模块CPU更新周期里面多次采样IMU数据，保证发送给主处理器模块的IMU数据是最新的；

S2、协处理器模块将采集到的传感器数据打包成一个自定义的字符形式数据包，并将该数据包传给主处理器模块，协处理器模块的数据包包括电子罗盘HMR3000航向角、IMU三轴角速度、IMU三轴加速度、GPS模式和GPS星数信息；

S3、主处理器模块采集协处理器模块和激光传感器的数据，分别对两种数据加入时间属性的标记，并通过无线收发模块把数据转发到地面站系统；

S4、地面站系统收到两种数据后保存数据，对于协处理器模块的数据进行扩展卡尔曼数据融合算法实现，选取小型无人直升机的非线性导航模型的系统在导航坐标系中的3个位置信息(x,y,z)，3个速度信息(u,v,w)，用四元数描述的机载系统姿态信息(q₀,q₁,q₂,q₃)和当地重力加速度g作为系统的状态向量；选取3个位置信息(x,y,z)，3个速度信息(u,v,w)，三个用欧拉角表示的机载系统姿态信息作为系统的观测向量；

S5、地面站系统利用卡尔曼数据融合后的姿态和位置信息，联合激光测距仪的测距信息进行三维解算，得到三维点云数据。

2.根据权利要求1所述的小型无人直升机超低空激光雷达数字地形测绘方法，其特征在于，步骤S1中，保证数据实时性和同步性的具体方法是：GPS差分板和电子罗盘HMR3000采用中断方式，有数据更新就产生中断，然后协处理器模块更新数据和各自的数据更新标志位，这样主处理器可以通过判断标志位来判断此时协处理器模块传来的GPS差分板或电子罗盘HMR3000数据是不是最新的，这样数据到达主处理器模块的时间延迟就近似只有协处理器串口传送数据的时间，在主处理器模块的数据接收时刻减去这个时间就可以实现数据的同步性。

3.根据权利要求1所述的小型无人直升机超低空激光雷达数字地形测绘方法，其特征在于，步骤S4中，在参数的选择中欧拉角表示的姿态角在机载平台大幅度姿态变化的时候会使欧拉角方程出现奇异现象，针对这一情况，用四元数来表示机载系统的姿态角，采用这种方法表示系统的姿态运动能消除这种奇异；四元数本质是一个向量，它的维度是4，其中的三个维度定义了空间中的一个转动轴，第四个维度是表示绕该转动轴的角度，四元素和欧拉角表示的姿态角可以通过下面两式相互转化：

$\stackrel{\→}{q}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}\cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ -\cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}+\sin \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}\\ \cos \frac{\mathrm{\φ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\ψ}}{2}-\sin \frac{\mathrm{\φ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\ψ}}{2}\end{array}\right]$

4.根据权利要求1所述的小型无人直升机超低空激光雷达数字地形测绘方法，其特征在于，构建的小型无人直升机的非线性导航模型如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=f(X\left(t\right),u\left(t\right),t)\\ Z\left(t\right)=h(X\left(t\right),t)\end{array}\right.$

其中：

X=[x y z u v w q₀ q₁ q₂ q₃ g]^T

为系统的状态向量；

为系统的观测向量；

$f(x,u)=\left[\begin{array}{c}\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2(q_1^2+q_2^2)\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]-\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right]g-\left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\\ 0\end{array}\right]$

为系统的状态方程；这里的[q₀,q₁,q₂,q₃]是四元素表示的姿态角信息，

为欧拉角表示的姿态角信息；[a_x,a_y,a_z]表示飞机在导航坐标系中的加速度；g为当地的重力加速度；[p,q,r]表示飞机在机体坐标系中分别沿三个轴旋转的角速度；

为系统的观测方程。

Images