CN103868521A - 基于激光雷达的四旋翼无人机自主定位及控制方法 - Google Patents

Abstract

本发明属于四旋翼无人机自主飞行控制研究的技术领域，为提出一套新的基于二维激光雷达的自主定位方法，及基于此定位方法及其它机载MEMS芯片的四旋翼无人机控制系统设计方法，为此，本发明采用的技术方案是，基于激光雷达的四旋翼无人机自主定位及控制方法，首先利用二维激光雷达进行无人机水平方向的初步定位，利用机载气压计得到无人机的高度方向的初步位置值；之后利用互补滤波算法，结合机载加速度计芯片，获得更高频率的无人机位置信息；最后基于此位置信息，应用于无GPS信号下的无人机控制系统。本发明主要应用于无人机自主飞行控制装置的设计制造。

Description

基于激光雷达的四旋翼无人机自主定位及控制方法

技术领域

本发明属于四旋翼无人机自主飞行控制研究的技术领域，具体讲，涉及四旋翼无人机自主定位及控制方法；

背景技术

无人驾驶飞机简称无人机，是一种利用无线电遥控或自身程序控制为主的不载人飞机；与载人飞机相比，无人机具有体积小、造价低、使用方便等优点；根据其结构设计的不同，无人机可分为固定翼和旋翼两种类型；四旋翼无人机属于旋翼无人机中的一种，其具有体积小巧、结构简单、安全性高以及对飞行空间要求低等优点，在军用及民用领域有着广阔的应用前景，包括地形测绘、电网维护、航空运输、灾难搜救以及跟踪监视等实际应用；四旋翼无人机是一种典型的欠驱动、强耦合、静不稳定的非线性系统，对其进行飞行控制研究有着重要的理论价值；国外对于四旋翼无人机研究起步较早，技术也更加成熟；例如美国宾夕法尼亚大学、麻省理工学院，佐治亚理工大学，德国弗莱堡大学，瑞士苏黎世联邦理工及法国、澳大利亚、日本、韩国等国家的科研团队，已经取得了较为深入的科研成果；现在正在向更加复杂的机动动作，更加完备的任务执行方面进行研究；近年来，国内也在逐步开展对四旋翼无人机的研究，如中国科学院、清华大学、国防科技大学、北京航空航天大学、东北大学等知名高校均进行了一定的研究；

四旋翼无人机电子系统可由传感器及飞行控制两个子系统组成；传感器系统需要对无人机状态进行精确测量或估计，其将无人机位置、速度、姿态角、角速度等飞行状态传递给飞行控制子系统，并由其实现良好的飞行控制；传感器系统可细分为姿态传感器及位置传感器两类；常用姿态传感器由三轴加速度计、三轴陀螺仪、磁力计等MEMS元件组成，其在室内外均可使用；位置传感器则多采用GPS、北斗等卫星定位导航系统，然而该种传感器难以在GPS信号微弱的楼群间及室内环境中实现精确定位；国际上为解决无GPS环境下精确定位问题，常用方法可分为基于VICON等运动捕获系统定位，基于机载摄像头定位及基于机载激光雷达定位三种；利用激光雷达作为机载位置传感器，相较于其他方法，具有定位精度高、对环境依赖程度较弱、利于向室外扩展等优势，是国际控制领域研究的热点问题；

然而在我国针对无GPS信号环境，设计一套基于激光雷达的四旋翼无人机自主定位及控制系统尚处于研究起步阶段，因此设计一套定位精确、功能完善、易扩展、控制性能良好的无人机定位及控制系统具有极大的理论价值及意义，并可拓宽四旋翼无人机的应用领域；

发明内容

本发明旨在克服现有技术的不足，提出一套新的基于二维激光雷达的自主定位方法，及基于此定位方法及其它机载MEMS芯片的四旋翼无人机控制系统设计方法，为此本发明采用的技术方案是，基于激光雷达的四旋翼无人机自主定位及控制方法，首先利用二维激光雷达进行无人机水平方向的初步定位，利用机载气压计得到无人机的高度方向的初步位置值；之后利用互补滤波算法，结合机载加速度计芯片，获得更高频率的无人机位置信息；最后基于此位置信息，应用于无GPS信号下的无人机控制系统。

实际上本发明所采用的定位方法由以下两个阶段完成：

第一个阶段为较低频率的三维位置裸数据获取阶段。其中平面二维位置主要使用激光雷达的定位算法，其包含三维投影、换图策略及匹配定位三个部分，产生二维平面定位裸数据；而高度方向裸数据由机载气压计芯片测量数据获取。

第二个阶段为更高频率定位数据获取阶段。其具体基于互补滤波器实现，使用机载加速度计融合第一阶段产生的裸数据，得到更新速率更高的位置数据。

基于激光雷达的水平方向初步定位具体步骤为：

1)三维环境模型向二维平面投影

利用机器人运动学坐标变换方法，并结合机载惯性导航单元得到的欧拉角，将激光雷达扫描得到的参考图像及当前图像投影到水平面上，在这一平面上，再利用二维平面匹配方法进行位移及角度测量，从而得到无人机在水平方向上精确的位置信息，具体步骤如下，设激光雷达在时刻t，按逆时针顺序第i条激光束在三维空间中扫描测到点其在激光雷达坐标系下定义为

其中θ_i代表该条激光束所在角度，r_i代表该条激光束测得的距离，与此同时根据机载惯性导航单元，测得激光雷达所处滚转角为γ、俯仰角为β，则通过（1）式，将

投影至世界坐标系中：

$P_{i3d}=R_{\mathrm{pitch}}\×R_{\mathrm{roll}}\×P_i^l---\left(1\right)$

其中 $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\γ}\right)& -\sin \left(\mathrm{\γ}\right)\\ 0& \sin \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\γ}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\β}\right)& 0& \sin \left(\mathrm{\β}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\β}\right)& 0& \cos \left(\mathrm{\β}\right)\end{array}\right]$

最后将P_i3d的Z轴分量置为0，从而得到采集图像中一个三维点

在二维平面上的投影点P_i2d，这里由所有投影点组成的图像即为三维环境模型在二维平面上的投影图像；

2)迭代式无人机平面位移及转角初步估计方法

利用1)中投影方法将无人机在不同时刻，不同角度下采集到的图像投影到同一平面上；而后利用迭代式方法对无人机平面位移及转角进行估计，因此需要选取两幅处于同一平面的图像进行比较，将其中一幅图像选定为当前时刻采集到的投影图像，另一幅参考图像根据换图策略进行选取；

依据换图策略获得两幅比较图像后，需要利用迭代估计方法对无人机平面位移及转角进行估计；这里将所估计的参考图像到当前图像的变换关系定义为q，其包含由平面偏航角θ决定的旋转变换R(θ)及平移变换t，定义p_i为激光雷达采集到的当前投影图像中一个点，定义

为利用所估计的位移及转角将点p_i投影到参考图像中的点，定义

及为参考图像中距离

最近的相邻点；

设当前迭代次数为k且k＞0，迭代直至收敛或陷入迭代环，其每一迭代步描述如下：

第一步，设置迭代初值：若第一次进入迭代程序，则其变换猜测初值q₀定义为q₀＝(t₀,θ₀)，利用机载惯性导航元件，通过测得参考时刻及当前时刻间偏航角的差值作为θ₀，并设初始时间t₀为0，之后每次迭代中，迭代初值q_k＝(t_k,θ_k)设定为上一步计算出的结果，θ_k、t_k分别为第k次迭代时的偏航角的差值及时刻；

第二步，确定匹配关系：将选取的迭代初值q_k应用于当前图像点p_i上，得到该点在参考图像上的坐标对于每一个

在参考图像所有点中寻找距离最近的两个相邻点

以及其中下角标j₁、j₂代表点

以及点

分别由激光雷达沿逆时针方向的第j₁及j₂条激光束采集得到，而上脚标i代表点

及点

与当前图像中的点p_i相匹配，继而可将该匹配关系简化为<i，j₁ ⁱ，j₂ ⁱ>，即当前图像中顺序标号为i的点p_i，匹配于参考图像中由顺序标号为j₁ ⁱ及j₂ ⁱ的两个相邻点所确定的线段

此方法需要首先确定参考图像中满足（2）式的点

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(2\right)$

这里首先需要根据激光束分辨率设置初始搜索点，如式3所示；

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(3\right)$

其中ψ₀为初始扫描激光束角度，nrays为激光束总数，θmax-θmin为起始及终止激光束角度差值，之后比较起始点前后两点与

的距离差，取距离较小的点所在方向开始搜索；如果当前点与的距离已经超过了之前获得的最短距离，则终止此方向的搜索并转向起始点另一方向继续搜索；此后，当两个方向的搜索均达到终止条件时，得到最短匹配

最后选取

前后两点中，与

距离更近的那个点作为

这样便可得到有效匹配对<i，j₁ ⁱ，j₂ ⁱ>及其匹配距离；

第三步，裁剪部分匹配对：首先裁剪掉重复匹配参考图中一点

的部分匹配对，建立一个数组存储所有与

有关的匹配对，通过遍历这些匹配对应的匹配距离，取出其中最小的距离值，之后裁剪掉大于该距离一定倍数的匹配对<i，j₁ ⁱ，j₂ ⁱ>，其后将剩下的所有匹配对按匹配距离长短排序，再按预设比例裁剪掉其中距离较长的匹配对；

第四步，定义式（4）为误差函数J，

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(4\right)$

其中n_i为垂直于线段

的法向量，C_k为当前第k步所有有效匹配关系，该式可描述为求取变换关系q_k+1＝(t_k+1,θ_k+1)，使得未被裁剪的所有匹配对中，点

到与其匹配的线段

的距离总和最短，对（4）式这一非线性方程求取极值，计算出无人机水平方向位置及转角的变化估计q_k+1＝(t_k+1,θ_k+1)，若未满足迭代终止条件，则将此值设置为迭代初值，至第一步继续迭代运算，直至满足预设终止迭代条件。

换图策略具体为：首先等角度选取1/4的激光束进行采样，其次选定悬停点附近，水平方向正负30厘米×30厘米且转角不超过30°的区域为不换图区域，在该区域内固定悬停点采集到的第一副投影图像为参考图像，其余时刻采样得到的当前投影图像均与第一副投影图像进行比较；当飞行器飞出这一区域后，则需要替换参考图像并记录飞行器已有位移，之后再次以新的参考图像采集点为中心生成不换图区域，继续进行匹配定位。

利用720MHz ARM处理芯片执行水平方向定位方法，其输出数据更新频率介于10-20Hz之间。

步骤2：基于机载气压计芯片的的高度方向初步定位

本发明采用无人机机载气压计芯片对无人机高度进行测量，由于无人机旋翼的运转对气压计芯片附近气流产生影响，致使在没有保护措施的情况下，气压计芯片测量数据难以使用；因此本发明将装载气压计芯片的飞行控制系统安置于封闭性能较好的塑料盒中，并利用海绵等材料阻塞塑料盒与气压计芯片间的缝隙，从而有效排除了无人机旋翼运转对气压计芯片附近气流的影响；之后采集机载MEMS气压计测得数据，其更新频率为10Hz；

步骤3：基于互补滤波的无人机三维位置估计

采用一种三维互补滤波方法，利用机载加速度计获得的无人机三维加速度信息，对步骤1、2中的位置数据进行插值，从而提高位置数据更新频率，具体为：将无人机加速度传感器更新得到的机载坐标系下的测量值，通过坐标变换投影至世界坐标系，并通过两次积分计算，得到由加速度计估计出的100Hz位置量传递给机载控制器进行位置控制；在水平方向上，当激光雷达匹配方法计算完毕时，计算激光雷达定位数据与加速度计估计值的差值，再通过反馈系统调整加速度积分偏置量，从而实现加速度计与激光雷达这两种异类传感器间的数据融合，互补滤波器增益k₁,k₂,k₃可由时间常数τ定义如下：

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

同理，高度方向上也采用互补滤波器，实现加速度计与气压计的数据融合。

本发明的技术特点及效果：

1.系统稳定可靠；

本发明提出的系统搭建方法，将激光雷达定位传感器及底层飞行控制分布在不同机载单元上，确保了整个系统运行稳定、高效、可靠；

2.匹配定位方法实时性高；传统的ICP匹配方法往往需要进行多次迭代、逐一查找才能找到匹配对，因此这些方法普遍存在搜寻效率低的缺点；而本发明使用的改进型ICP方法，采用了提前建立跳表，裁剪匹配对等策略，其搜寻效率明显高于传统的搜寻方法，因此可在机载控制器上实现；

3.数据融合方法简洁高效；由于所采用互补滤波方法编程实现并不复杂，计算量比常见卡尔曼滤波算法更小，因此便于在低速机载单片机芯片中使用；

4.无GPS信号环境无人机应用价值高；本发明提出的基于激光雷达的定位及控制系统搭建方案，扩展了无人机系统的应用领域，尤其是在无GPS信号情况下，精确定位及控制的需求；

附图说明

图1改进型定位方法流程图；

图2一维互补滤波器框图；

图3基于激光雷达的四旋翼无人机硬件结构图；

图4基于激光雷达四旋翼无人机软件结构图；

图5四旋翼无人机室内悬停实验结果图；

具体实施方式

本发明采用的技术方案是四旋翼无人机自主定位及控制方法，平面位移及转角测量：采用激光雷达进行迭代式相对定位，因此需要选取两幅图像进行比较，这里定义p_i为当前图像中的一个点，定义

为参考图像中的点，参考图像到当前图像的变换关系定义为旋转R(θ)以及平移t；

设当前迭代次数为k且k＞0，迭代直至收敛或陷入迭代环，其每一迭代步描述如下：

第一步，设置迭代初值：若第一次进入迭代程序，则其变换猜测初值q₀＝(t₀,θ₀)，利用机载惯性导航元件，通过测得两个时刻间偏航角差值作为θ₀，t₀可设置为0，之后每次迭代中，迭代初值q_k＝(t_k,θ_k)设定为为上一步计算出的结果；

第二步，确定匹配关系：将选取的迭代初值q_k应用于当前图像点p_i上，得到该点在参考图像上的坐标对于每一个

在参考图像所有点中寻找距离

最近的两个点

以及

定义匹配关系为<i，j₁ ⁱ，j₂ ⁱ>,即当前图像中点i匹配于参考图像中线段j₁ ⁱ_j₂ ⁱ；此方法需要首先确定满足（1）式的点

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(1\right)$

首先根据激光束分辨率设置初始搜索点，如式2所示。

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(2\right)$

其中ψ₀为初始扫描激光束角度，nrays为激光束总数，θmax-θmin为最大最小激光束差值，之后比较起始点上下两点与

的距离差，取距离较小的点所在方向开始搜索；如果当前点与

的距离已经超过了之前获得最短距离，则终止搜索并转向起始点另一方向继续搜索；此后，当两个方向的搜索均达到终止条件时，得到最短匹配

最后选取

前后两点中，与

距离更近的那个点作为

这样便可得到有效匹配对<i，j₁ ⁱ，j₂ ⁱ>及其匹配距离；

第三步，裁剪部分匹配对：首先裁剪掉重复匹配参考图中一点

的部分匹配对，建立一个数组存储所有与

有关的匹配，通过遍历这些匹配对应的匹配距离，取出其中最小的距离值，之后裁剪掉大于该距离一定倍数的匹配对<i，j₁ ⁱ，j₂ ⁱ>，其后将剩下的所有匹配对按匹配距离长短排序，再按预设比例裁剪掉其中距离较短的匹配对；

第四步，定义式（3）为误差函数，其中n_i为垂直于线段j₁ ⁱ_j₂ ⁱ的法向量，该式可描述为求取变换关系q_k+1＝(t_k+1,θ_k+1)，使得未被裁剪的所有匹配对中，点到与其匹配的线段

的距离总和；

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(3\right)$

对（3）式这一非线性方程求取极值，计算出位置姿态的变化估计q_k+1＝(t_k+1,θ_k+1)，若未满足迭代终止条件，则将此值最为迭代初值，至第一步继续迭代运算，直至满足预设终止迭代条件。

采用换图策略对采用激光雷达进行迭代式相对定位进行改进：

首先等角度选取1/4的激光束进行采样，其次选定悬停点附近，水平方向正负60cm*60cm，转角不超过30°的区域为不换图区域，在该区域内，固定悬停点采集到的第一副图像为参考图像，其余时刻采样得到的当前图像均与第一副图像进行比较；当飞行器飞出这一区域后，则需要替换参考图像并记录飞行器已有位移，之后再次以新的参考图像采集点为中心生成不换图区域，继续进行匹配定位。

在三维环境下定位：利用机器人运动学坐标变换方法，结合惯性导航单元得到的欧拉角，将激光雷达扫描得到的参考图像及当前图像投影到水平面上，在这一平面上，再利用二维平面匹配方法进行位移及角度测量，从而得到无人机在水平方向上精确的位置信息，具体步骤如下，设时刻t激光束i在三维空间中扫描测到点

其在激光雷达坐标系下定义为

其中θ_i代表该条激光束所在角度，r_i代表该条激光束测得的距离，与此同时根据机载惯性导航单元，测得激光雷达所处滚转角为γ、俯仰角为β，则通过（4）式，将

投影至世界坐标系中：

$P_i=R_{\mathrm{pitch}}\&times;R_{\mathrm{roll}}\&times;P_i^l---\left(4\right)$

其中 $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\&gamma;}\right)& -\sin \left(\mathrm{\&gamma;}\right)\\ 0& \sin \left(\mathrm{\&gamma;}\right)& \cos \left(\mathrm{\&gamma;}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\&beta;}\right)& 0& \sin \left(\mathrm{\&beta;}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\&beta;}\right)& 0& \cos \left(\mathrm{\&beta;}\right)\end{array}\right]$

最后将P_i的Z轴分量置为0，从而得到三维点在二维平面上的投影，继而应用1中平面测量方法计算无人机在水平方向的位移。

采用三维互补滤波方法，将采集到的加速度信息，对定位方法及气压计得到的位置数据进行插值，提高位置数据更新频率，具体为：将无人机加速度传感器更新得到的数据，通过坐标变换投影到世界坐标系，通过两次积分计算，得到由加速度计估计出的100Hz位置量传递给机载控制器进行位置控制，当激光雷达匹配方法计算完毕时，计算激光雷达定位数据与加速度计估计值的差值，再通过反馈系统调整加速度积分偏置量，从而实现加速度计与激光雷达这两种异类传感器间的数据融合，互补滤波器增益k₁,k₂,k₃由时间常数τ定义如下：

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

同理，高度方向采用互补滤波器，实现加速度计与气压计的数据融合。

三维互补滤波器中的“三维”指的是两个平动自由度加一个高度方向自由度。无人机体上可安装一个加速度传感器，用以测得无人机当前的加速度，这一传感器更新频率较高，为100Hz。而只依靠激光雷达匹配算法估计的无人机二维平面位移更新频率只有10-20Hz，而气压计芯片测得的高度数据更新频率只有10Hz。并且只使用裸数据定位，难以取得较好的无人机控制效果。因此本发明采用了互补滤波器融合加速度及激光雷达两个传感器，使传感器输出定位数据频率提升至100Hz。

“互补滤波增益”是滤波器反馈回路上的参数，用来决定激光雷达估计或气压计测量数据对正向通路上加速度积分的影响强弱，即是决定滤波器输出结果更接近激光雷达或气压计裸数据，还是更接近于加速度积分估计的位置数据。

“互补滤波增益”之间具有联系，其值的选取可依靠“时间常数”确定。

下面结合附图和具体实施方式，进一步详细说明本发明。

四旋翼无人机在无GPS信号情况下进行自主定位及控制，要求无人机能够在短时间内处理异类多传感器的测量信息，如激光雷达、加速度、陀螺仪、磁力计等传感设备；由于传统单片机控制器的计算能力及存储资源难以满足复杂方法实时处理需求，尤其难以解决激光雷达定位方法实时性的问题，本发明设计了一种基于ARM Cortex-A8微处理器并结合Atmega2560机载控制器的四旋翼无人机系统搭建方案，将激光雷达定位及底层控制分立到不同机载单元上；此外本发明利用互补滤波方法融合激光雷达数据及加速度数据，确保了实际飞行系统的高稳定性、高精度及高可靠性；

1.四旋翼无人机系统硬件组成

该无GPS信号下四旋翼无人机系统硬件组成如说明书附图3所示，该图展示了基于激光雷达的四旋翼无人机系统硬件组成及连接关系，下面详细介绍各部分特性及其功能；

1)四旋翼无人飞行器

本发明机身本体采用小型电动航模四旋翼飞行器机体，其直径为450mm，使用尼龙加纤材料制成，搭载4组无刷电机、电调及旋翼；可使用Futaba遥控器及2.4GHz接收机实现四旋翼飞行器的手动飞行；

2)Hokuyo UTM-30LX激光雷达

本发明使用日本Hokuyo公司UTM-30LX激光雷达作为机载定位传感器，其供电电压为DC12V±10%，激光束扫描角度为270°，分辨率约为0.25°，测距范围为0.1-30m，扫描时间为25ms；

3)机载微处理器

本发明选用基于Cortex-A8芯片的720MHz嵌入式处理器作为机载计算单元；该微处理器内嵌终端Linux操作系统，运行激光雷达定位方法，并将解算出的位置数据发送给机载微控制器进行飞行控制；

4)机载微控制器

本发明选用Atmega2560芯片设计机载控制器；该飞控系统集成了三轴加速度计、三轴陀螺仪、三轴磁力计及高度气压计，其可实现增稳飞行、定点悬停、航迹跟踪、路径规划等多种飞行模式；本发明重新编写APM开源代码，改善了其位置数据传输接口，实现兼容激光雷达定位数据的目的；

5)系统配电

系统采用2300mAh3S锂聚合物电池为四旋翼飞行器供电；该电池使用之前需良好充电至12.6V，可采用大功率开关电源及充电器完成，充电电流不应大于4A；飞行过程中需连接电压报警器，防止锂聚合物电池电压低于10.8V；输入电压为12V的UTM-30LX激光雷达及电子调速器可直接由锂聚合物电池供电，输入电压为5V的机载处理器、机载控制器、RC接收机等电子设备可由电调输出端进行供电，输入电压为3.3V的气压计等芯片可通过微控制器电路板上5V至3.3V电压转换芯片进行供电；

本发明所搭建的基于激光雷达的四旋翼无人机定位及控制系统，总重1600g，全负载飞行时间可达7分钟；

2.四旋翼无人机系统软件组成及控制信号流程

说明书附图4展示了基于激光雷达的四旋翼无人机系统软件组成及控制信号流程，具体如下：

1)机载微处理器部分

机载微处理器内嵌Linux操作系统，采用C语言编写定位软件；该软件由多个线程进行组织，包含激光雷达数据读取线程，投影及匹配算法线程、串口通讯线程、日志线程等；其中以串口堵塞方式采集激光雷达裸数据，其更新频率达40Hz；本发明提出的包含三维投影的改进型扫描匹配方法，在此处理器中更新频率可达20Hz；通过改进型GPS通讯接口，可将计算出的虚拟GPS位置数据传递给机载微控制器，这里考虑到微控制器处理能力有限，因此将更新频率设为10Hz；

2)机载微控制器部分

机载微控制器通过I2C接口采集外围加速度计、陀螺仪、磁力计等芯片数据，利用非线性互补滤波方法解算无人机姿态数据，其更新频率为100Hz；此外机载微控制器还可采集外围芯片中气压计信息，其更新频率为10Hz；由于微控制器中采用了互补滤波方法，通过加速度插值位置传感器数据，将包含激光雷达数据在内的三维位置数据更新频率提升至100Hz，从而达到了较为精确的位置控制效果；

3)系统各部分软件配置及控制信号流程

系统使用Futaba遥控器及八通道RC接收机进行远程操控及模式切换；飞行器起飞前需进行遥控器及接收机信号的有效对频，确保通信信号稳定顺畅；RC遥控接收机信号线及4个电机对应的电子调速器信号线均需接到机载微控制器上；为达到最佳飞行效果，系统可采用外置磁力计方法，从而减轻电磁干扰对偏航角的影响；

机载微处理器通过串口1连接机载激光雷达，测取当前环境图像，并进行图像迭代匹配，估算无人机水平位置；通过串口2与机载微控制器上GPS信号接口连接，从而将定位数据传输给微控制器，进行无人机飞行控制；

为达到最佳系统状态观测及操控需求，可使用地面站远程操控软件；该软件使用一组数传电台与机载微控制器连接，实时获取无人机当前飞行姿态、位置、速度、加速度及各类方法参数；此外还可通过地面站系统实现无人机轨迹规划、飞行任务切换等功能；

3.无GPS信号环境下无人机悬停实验例

1.最佳效果控制方法选取

本发明中改进型迭代匹配方法可进行水平方向定位，通过互补滤波方法与加速度传感器融合得到高精度、高更新率位置数据；作为定位传感器可与多种飞行控制方法配合使用，为达到最佳飞行效果，本次实验应用如下内外环控制方法，外环为位置环，内环为姿态环，位置环设定姿态给定；

该方法中各位置及姿态自由度均单独控制；首先考虑位置环，该环设计为位置-速度双环结构，设无人机机体坐标系下，给定参考位置为p^ref，由互补滤波器计算当前位置为p^actual，位置环PI控制率设计如下：

$v^{\mathrm{ref}}=K_{\mathrm{pp}}\&CenterDot;(p^{\mathrm{ref}}-p^{\mathrm{actual}})+K_{\mathrm{ip}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(p^{\mathrm{ref}}-p^{\mathrm{actual}})\mathrm{dt}---\left(6\right)$

其中K_pp，K_ip分别为比例、积分增益，v^ref为速度环给定参考值；根据互补滤波器计算出的当前速度数据v^actual，设计PID控制率如下：

$a^{\mathrm{ref}}=K_{\mathrm{pv}}\&CenterDot;(v^{\mathrm{ref}}-v^{\mathrm{actual}})+K_{\mathrm{dv}}\&CenterDot;({\stackrel{.}{v}}^{\mathrm{ref}}-{\stackrel{.}{v}}^{\mathrm{actual}})+k_{\mathrm{iv}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(v^{\mathrm{ref}}-v^{\mathrm{actual}})\mathrm{dt}---\left(7\right)$

其中K_pv，K_dv，K_iv分别为比例、微分、积分增益，a^ref为姿态环中，该位置自由度对应姿态角的给定参考值；

之后考虑姿态环控制器设计，该环设计为角度-角速度双环控制结构，由微控制器中非线性互补滤波方法计算出飞行器姿态角a^actual，这里设计PI控制器如下：

${\mathrm{\&Omega;}}^{\mathrm{ref}}=K_{\mathrm{pa}}\&CenterDot;(a^{\mathrm{ref}}-a^{\mathrm{actual}})+K_{\mathrm{ia}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(a^{\mathrm{ref}}-a^{\mathrm{actual}})\mathrm{dt}---\left(8\right)$

其中K_pa，K_ia分别为比例、积分增益，Ω^ref为角速度环给定参考值；根据陀螺仪得到当前角速度数据Ω^actual，设计PID控制率如下：

$u=K_{\mathrm{p\&Omega;}}\&CenterDot;({\mathrm{\&Omega;}}^{\mathrm{ref}}-{\mathrm{\&Omega;}}^{\mathrm{actual}})+K_{\mathrm{d\&Omega;}}\&CenterDot;({\stackrel{.}{\mathrm{\&Omega;}}}^{\mathrm{ref}}-{\stackrel{.}{\mathrm{\&Omega;}}}^{\mathrm{actual}})+k_{\mathrm{i\&Omega;}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}({\mathrm{\&Omega;}}^{\mathrm{ref}}-{\mathrm{\&Omega;}}^{\mathrm{actual}})\mathrm{dt}---\left(9\right)$

其中K_pΩ，K_dΩ，K_iΩ分别为比例、微分、积分增益，u为控制器输出；

为确保实际飞行安全，在位置环及姿态环中均设定了积分上限I_{max_p}及I_{max_a}；最后根据各旋翼在飞行器上固接位置及转动方向，由u计算出各电机对应的转速输出给电子调速器进行电机控制；

2.悬停实验例

本次实验是在如下参数下进行的：

1）定位方法中最小迭代距离设定为0.00001厘米、最小迭代转角设定为0.000001弧度，最小不换图区域设定为60厘米×60厘米，最小不换图角度设定为30°，互补滤波器滤波增益τ设定为1；

2）控制方法中参数设置如下：

k_pp＝0.432  k_iv＝0.052    k_pa＝4

k_ip＝0      k_pΩ＝0.2530  k_ia＝0.1

k_pv＝20     k_dΩ＝0.0060  I_{max_a}＝5

k_dv＝0.09   k_iΩ＝0.1     I_{max_p}＝30

依据此参数，利用本发明中提出的飞行器平台进行了室内悬停飞行，实验结果由说明书图5所示；该图记录了一次起飞-悬停-降落完整的飞行过程；第一条竖直虚线记录了手动-自主悬停切换时刻，第二条竖直虚线记录了自主悬停-手动切换时刻；其中分图(a)表示飞行器姿态，虚线代表悬停状态下，位置环计算出的姿态角给定，其中实线为测得实际姿态角；从图中可以看出姿态角跟踪快速准确，且自主悬停飞行姿态平稳，振幅不超过正负1°；分图(b)表示飞行器位置状态，由图中可以看出该飞行器在悬停过程中，悬停定位精度不超过正负10厘米，取得了很好的飞行控制效果。

Claims (5)

1.一种基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，首先利用二维激光雷达进行无人机水平方向的初步定位，利用机载气压计得到无人机的高度方向的初步位置值；之后利用互补滤波算法，结合机载加速度计芯片，获得更高频率的无人机位置信息；最后基于此位置信息，应用于无GPS信号下的无人机控制系统。

2.如权利要求1所述的基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，基于激光雷达的水平方向初步定位具体步骤为：

1)三维环境模型向二维平面投影

利用机器人运动学坐标变换方法，并结合机载惯性导航单元得到的欧拉角，将激光雷达扫描得到的参考图像及当前图像投影到水平面上，在这一平面上，再利用二维平面匹配方法进行位移及角度测量，从而得到无人机在水平方向上精确的位置信息，具体步骤如下，设激光雷达在时刻t，按逆时针顺序第i条激光束在三维空间中扫描测到点

其在激光雷达坐标系下定义为

其中θ_i代表该条激光束所在角度，r_i代表该条激光束测得的距离，与此同时根据机载惯性导航单元，测得激光雷达所处滚转角为γ、俯仰角为β，则通过（1）式，将

投影至世界坐标系中：

$P_{i3d}=R_{\mathrm{pitch}}\&times;R_{\mathrm{roll}}\&times;P_i^l---\left(1\right)$

其中 $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\&gamma;}\right)& -\sin \left(\mathrm{\&gamma;}\right)\\ 0& \sin \left(\mathrm{\&gamma;}\right)& \cos \left(\mathrm{\&gamma;}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\&beta;}\right)& 0& \sin \left(\mathrm{\&beta;}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\&beta;}\right)& 0& \cos \left(\mathrm{\&beta;}\right)\end{array}\right]$

最后将P_i3d的Z轴分量置为0，从而得到采集图像中一个三维点在二维平面上的投影点P_i2d，这里由所有投影点组成的图像即为三维环境模型在二维平面上的投影图像；

2)迭代式无人机平面位移及转角初步估计方法

利用1)中投影方法将无人机在不同时刻，不同角度下采集到的图像投影到同一平面上；而后利用迭代式方法对无人机平面位移及转角进行估计，因此需要选取两幅处于同一平面的图像进行比较，将其中一幅图像选定为当前时刻采集到的投影图像，另一幅参考图像根据换图策略进行选取；

依据换图策略获得两幅比较图像后，需要利用迭代估计方法对无人机平面位移及转角进行估计；这里将所估计的参考图像到当前图像的变换关系定义为q，其包含由平面偏航角θ决定的旋转变换R(θ)及平移变换t，定义p_i为激光雷达采集到的当前投影图像中一个点，定义为利用所估计的位移及转角将点p_i投影到参考图像中的点，定义

及

为参考图像中距离

最近的相邻点；

设当前迭代次数为k且k＞0，迭代直至收敛或陷入迭代环，其每一迭代步描述如下：

第一步，设置迭代初值：若第一次进入迭代程序，则其变换猜测初值q₀定义为q₀＝(t₀,θ₀)，利用机载惯性导航元件，通过测得参考时刻及当前时刻间偏航角的差值作为θ₀，并设初始时间t₀为0，之后每次迭代中，迭代初值q_k＝(t_k,θ_k)设定为上一步计算出的结果，θ_k、t_k分别为第k次迭代时的偏航角的差值及时刻；

第二步，确定匹配关系：将选取的迭代初值q_k应用于当前图像点p_i上，得到该点在参考图像上的坐标对于每一个

在参考图像所有点中寻找距离

最近的两个相邻点

以及

其中下角标j₁、j₂代表点

以及点

分别由激光雷达沿逆时针方向的第j₁及j₂条激光束采集得到，而上脚标i代表点

及点

与当前图像中的点p_i相匹配，继而可将该匹配关系简化为<i，j₁ ⁱ，j₂ ⁱ>，定义该匹配关系为<i，j₁ ⁱ，j₂ ⁱ>,即当前图像中点i匹配于参考图像中线段

此方法需要首先确定参考图像中满足（2）式的点

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(2\right)$

这里首先需要根据激光束分辨率设置初始搜索点，如式3所示；

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(3\right)$

其中ψ₀为初始扫描激光束角度，nrays为激光束总数，θmax-θmin为起始及终止激光束角度差值，之后比较起始点前后两点与

的距离差，取距离较小的点所在方向开始搜索；如果当前点与的距离已经超过了之前获得的最短距离，则终止此方向的搜索并转向起始点另一方向继续搜索；此后，当两个方向的搜索均达到终止条件时，得到最短匹配

最后选取

前后两点中，与

距离更近的那个点作为

这样便可得到有效匹配对<i，j₁ ⁱ，j₂ ⁱ>及其匹配距离；

第三步，裁剪部分匹配对：首先裁剪掉重复匹配参考图中一点的部分匹配对，建立一个数组存储所有与

有关的匹配对，通过遍历这些匹配对应的匹配距离，取出其中最小的距离值，之后裁剪掉大于该距离一定倍数的匹配对<i，j₁ ⁱ，j₂ ⁱ>，其后将剩下的所有匹配对按匹配距离长短排序，再按预设比例裁剪掉其中距离较长的匹配对；

第四步，定义式（4）为误差函数J，

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(4\right)$

其中n_i为垂直于线段

的法向量，C_k为当前第k步所有有效匹配关系，该式可描述为求取变换关系q_k+1＝(t_k+1,θ_k+1)，使得未被裁剪的所有匹配对中，点到与其匹配的线段

的距离总和最短，对（4）式这一非线性方程求取极值，计算出无人机水平方向位置及转角的变化估计q_k+1＝(t_k+1,θ_k+1)，若未满足迭代终止条件，则将此值设置为迭代初值，至第一步继续迭代运算，直至满足预设终止迭代条件。

如权利要求1所述的基于激光雷达的四旋翼无人机自主定位方法及控制方法，其特征是，换图策略具体为：首先等角度选取1/4的激光束进行采样，其次选定悬停点附近，水平方向正负30厘米×30厘米且转角不超过30°的区域为不换图区域，在该区域内固定悬停点采集到的第一副投影图像为参考图像，其余时刻采样得到的当前投影图像均与第一副投影图像进行比较；当飞行器飞出这一区域后，则需要替换参考图像并记录飞行器已有位移，之后再次以新的参考图像采集点为中心生成不换图区域，继续进行匹配定位。

3.如权利要求1所述的基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，利用720MHz ARM处理芯片执行水平方向定位方法，其输出数据更新频率介于10-20Hz之间。

4.如权利要求1所述的基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，基于机载气压计芯片的的高度方向初步定位具体为：采用无人机机载气压计芯片对无人机高度进行测量，具体为：首先有效排除无人机旋翼运转对气压计芯片附近气流的影响，即将装载气压计芯片的飞行控制系统安置于封闭性能较好的塑料盒中，并利用海绵等材料阻塞塑料盒与气压计芯片间的缝隙，从而有效排除了无人机旋翼运转对气压计芯片附近气流的影响；之后采集机载MEMS气压计测得数据，其更新频率为10Hz。

5.如权利要求1所述的基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，基于互补滤波的无人机三维位置估计具体为：采用一种三维互补滤波方法，利用机载加速度计获得的无人机三维加速度信息，对步骤1、2中的位置数据进行插值，从而提高位置数据更新频率，具体为：将无人机加速度传感器更新得到的机载坐标系下的测量值，通过坐标变换投影至世界坐标系，并通过两次积分计算，得到由加速度计估计出的100Hz位置量传递给机载控制器进行位置控制；在水平方向上，当激光雷达匹配方法计算完毕时，计算激光雷达定位数据与加速度计估计值的差值，再通过反馈系统调整加速度积分偏置量，从而实现加速度计与激光雷达这两种异类传感器间的数据融合，互补滤波器增益k₁,k₂,k₃可由时间常数τ定义如下：

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

同理，高度方向上也采用互补滤波器，实现加速度计与气压计的数据融合。

Images