设为首页 加入收藏 期刊导航 网站地图

Dynamical Systems and Control
Vol.06 No.04(2017), Article ID:22551,8 pages
10.12677/DSC.2017.64024

Position Estimation Algorithm for Rotorcraft Based on Laser Range Finder

*通讯作者。

Guifang Tan, Chenxiao Cai*

School of Automation, Nanjing University of Science and Technology, Nanjing Jiangsu

Received: Oct. 2nd, 2017; accepted: Oct. 23rd, 2017; published: Oct. 31st, 2017

ABSTRACT

In this paper, the environment information is captured and the position of rotorcraft is calculated by using laser scanner and using iterative closest point (ICP) algorithm. It could estimate the position more rapid and accurate by improving the extended Kalman filter (EKF) algorithm which reduces the amount of computation. The trajectory model of rotorcraft will be constructed by combining and fusing the ICP and improved EKF algorithms to update the prediction. In the process of updating, the observed environment model will be sketched through extracting line features for surrounding of the rotorcraft. Then the model expression is simplified according to the accurate attitude angles of rotorcraft which come from Inertial Measurement Unit (IMU) sensor. Finally, location information is estimated accurately for the rotorcraft via simulation and experiment.

Keywords:Rotorcraft, Laser Scanner, ICP Algorithm, EKF Algorithm, Position

基于激光测距仪的旋翼飞行器位置估计算法

谭贵芳，蔡晨晓*

南京理工大学自动化学院，江苏 南京

收稿日期：2017年10月2日；录用日期：2017年10月23日；发布日期：2017年10月31日

摘 要

本文通过采用激光测距仪获取周围环境的信息，运用迭代最近点(Iterative Closest Point, ICP)算法计算出位置，并通过对扩展卡尔曼滤波(Extended Kalman Filter, EKF)算法的改进，减少EKF算法的计算量，使旋翼飞行器能更加快速的实现对位置的估计。该算法通过在预测更新的过程中引进ICP算法计算的结果构建过程模型，并在测量更新过程中通过对环境中线段特征的提取，构建观测模型，结合飞控能够提供较为精确的角度信息的特点，简化观测模型的表达式，最终得到准确的位置信息。

关键词 :旋翼飞行器，激光测距仪，ICP算法，EKF算法，位置

Copyright © 2017 by authors and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY).

http://creativecommons.org/licenses/by/4.0/

1. 引言

旋翼飞行器作为无人飞行器的一种，由于其具有简单的机械结构，灵活的机动能力，操控方便，能实现小范围内升降，能够很好的在复杂多变且空间较小的环境中飞行，因此在军事、民事方面的应用逐渐增多 [1] [2] 。精确的定位信息是旋翼飞行器在复杂环境中执行导航、路径规划等任务的基础 [3] ，大多数室外情况是依靠GPS来提供准确的位置，但是因为室内GPS信号不好，很难接收到GPS的信息，因此需要用其它传感器来提供位置信息 [4] 。目前，在室内用来作为提供位置信息的传感器主要有视觉传感器、红外传感器和激光测距仪 [5] 。视觉传感器模拟人体的双目，一般比较小，易于安装，但是它的动态性能差，处理时间较长，不利于实时处理，不利于旋翼飞行器对于位置的实时获取 [6] 。红外传感器利用红外线来感知周围环境，物体的颜色、周围的光线等可能引起红外传感器的测量误差，因此受环境的影响较大，从而使计算得到的位置的误差变大 [7] 。激光测距仪通过激光来感知周围环境，环境对于激光的影响较小，因此激光测距仪测距的精度很高，激光测距仪获取周围环境的信息非常快，有助于快速估计旋翼飞行器的位置 [8] ，因此本文选择使用激光测距仪作为提供位置信息的传感器。

本文通过使用激光测距仪来获取周围环境信息，并通过ICP算法解算出两个时刻的位移，从而迭代出当前时刻的位置，但是由于ICP算法每次解算出来的位置变化量都有误差，随着迭代次数的增多，会造成位置的误差逐渐变大，使得计算出的旋翼飞行器的位置与实际位置有很大的偏差，因此需要提高定位精度 [9] 。目前提高定位精度的算法主要有EKF [10] [11] 、粒子滤波(PF) [12] [13] 和图优化(Graph-SLAM) [14] 。EKF算法结合卡尔曼滤波的思想，对非线性系统线性化，然后对线性化后的系统用卡尔曼滤波进行滤波 [15] 。PF是对事件的后验概率用粒子集表示，粒子越集中的地方概率越大，越有可能代表旋翼飞行器的位置 [16] 。Graph-SLAM是通过旋翼飞行器的位置和周围环境的特征作为节点，它们之间通过一系列的关系式连接，表示它们之间的关系，通过当前时刻位置与节点之间的关系，使得当前时刻的位置达到最优 [17] 。但是PF、Graph-SLAM计算量太大 [18] ，不利于实时实现，而EKF算法的计算量相对较小，能够比较快速的估计旋翼飞行器的位置，因此本文选择EKF算法。

2. 坐标系的建立

2.1. 机体坐标系

激光测距仪(UTM30-LX)获取的数据是周围环境距离测距仪中心的距离，它的测量范围为270˚，角分辨率为0.25˚，因此周围环境在激光测距仪记录数据中可以表示成一系列点，记为point 0~point 1080，选择激光测距仪的中心为坐标原点，测量区域的角平分线作为测距仪坐标系y轴方向，建立二维右手坐标系，如图1所示。激光测距仪测得的数据可以写成( d i , φ i )，为同一次扫描中point i与point 0之间的夹角，因此需要对初始数据进行转换，转换公式如下：

{ x i b = d i cos ( φ i − π / 4 ) y i b = d i sin ( φ i − π / 4 ) (1)

x b , y b 是机体坐标系下x, y的坐标值。

2.2. 大地坐标系

当旋翼飞行器起飞时，以此时的机体坐标系对大地坐标系进行定义。因此，两个坐标系之间的转换关系可表示为：

{ x i w = x i b cos θ − y i b sin θ y i w = x i b sin θ + y i b cos θ (2)

x w , y w 表示大地坐标系下的x, y的坐标值(图2)。

Figure 1. Curve: sketch map of body coordinate axis

图1. 机体坐标轴示意图

Figure 2. Curve: The relation between body coordinate and ground coordinate

图2. 机体坐标与地面坐标关系图

3. 环境中特征线段的提取

在线段提取之前，需要对激光测距仪的数据进行处理，根据公式(1)，将每个数据点转换为机体坐标系下的坐标值，并对扫描仪的数据进行分割，即当两个扫描点之间的距离大于一个阈值(根据实际的经验选择合适的阈值)时，一个新的点集S产生。这些点集可以被写成：

P = { S 1 , S 2 , S 3 , ⋯ , S M }

因为本文EKF需要用到线段特征，所以采用split-merge [19] 算法对点集 S i 进行线段的提取如图3所示，具体过程如下：

(1) 根据实际需要，确定一个点和线段关系的阈值。

(2) 点集 S i 作为待处理数据点，连接待处理数据点的起始点和终止点，建立一个线段的模型。

(3) 找到待处理数据点中其它数据点距离线段的距离，记录其最大值，判断这个最大值与阈值的关系，如果最大值小于阈值，则不进行分割，否则，把线段分为两部分。

(4) 判断点集 S i 中的线段是否处理完，如果没有，跳到第(2)步。

4. 改进的EKF算法

4.1. 特征表示与选择

提取出的线段可以用(r, α, l)表示(图4)，r表示坐标原点到直线的距离，α表示线段的垂线与x轴的夹角，l表示线段的长度。当线段长度太小，拟合线段方程时，会造成r, α与实际有较大的误差，因此线段的长度不能太小，本文选择lmin = 0.8 m。

4.2. 模型的建立

旋翼飞行器水平面和垂直平面之间的运动模型分析是可以解耦的。本文基于水平方向的避障功能，只考虑水平面内的运动特性，因此，对于EKF算法模型的建立只考虑水平面，假设旋翼飞行器的飞行高度保持不变。

4.2.1. 观测模型的建立

本文通过对扫描仪离散数据进行处理，提取出周围环境的线段信息，并以此建立观测模型如下 [20] ：

Figure 3. Curve: Sketch map of line segment extraction

图3. 线段提取示意图

Figure 4. Curve: the definition of line in body and ground coordinates, respectively

图4. 直线分别在机体坐标系和大地坐标系下的定义

h i ( X , M ) = ( r i − x 2 + y 2 cos ( β − α i ) α i − θ ) (3)

式中， h i ( X , M ) 表示观测模型， X = ( x , y ) 表示大地坐标系下的坐标值，M是线段的总条数，i表示第i条线段， θ 表示机体坐标系偏离大地坐标系的角度，即偏航角， β 表示当前时刻的位置与大地坐标系x轴之间的夹角。

旋翼飞行器的飞控有提供偏航角的信息，且偏航角的精度较高，因此可以用飞控提供的偏航角信息，因此，观测模型可以简化为：

h i ( X , M ) = ( r i − x 2 + y 2 cos ( β − α i ) ) (4)

由公式(4)可得观测矩阵：

H i = [ H 11 H 12 ] (5)

其中

H 11 = − x ρ cos ( β − α i ) − y ρ sin ( β − α i )

H 12 = − y ρ cos ( β − α i ) + x ρ sin ( β − α i )

ρ = x 2 + y 2

使 x k | k − 1 , 0 = x k | k − 1 ， P k | k − 1 , 0 = P k | k − 1 ，EKF测量更新过程为

{ K k , i = P k | k , i − 1 H i T ( H i P k | k , i − 1 H i T + R i ) x k | k − 1 , i = x k | k − 1 , i + K k , i ( Y i − h i ( X , M ) ) P k | k , i = ( I − K k , i H i ) P k | k , i − 1 (6)

式中，i = 1, 2, 3, ···, vmax, vmax为当前能匹配到的直线的最大数量。迭代结束后令 x k | k = x k | k − 1 , v max 作为下一次预测输入。

4.2.2. 过程模型的建立

为了进行迭代更新，需要对旋翼无人飞行器的位置进行估计。本文采用ICP算法计算上一个时刻和当前时刻的位移变化，从而迭代出旋翼的位置，因此，过程模型如下：

{ x k | k − 1 = x k − 1 | k − 1 + cos θ k Δ x − sin θ k Δ y y k | k − 1 = y k − 1 | k − 1 + sin θ k Δ x + cos θ k Δ y (7)

式中，x(k − 1), x(k)分别表示第k − 1, k时刻旋翼飞行器在大地坐标系下的x轴坐标，y(k − 1), y(k)分别表示第k − 1, k时刻旋翼飞行器在大地坐标系下的y轴坐标， θ k 为旋翼飞行器的偏航角， Δ x , Δ y 分别表示旋翼从k − 1时刻到k时刻的位移变化量。

5. 实验结果分析

在旋翼飞行器上安装好激光测距仪，控制旋翼飞行一个简单的轨迹，同时通过VICON记录旋翼的位置(VICON对位置测量的精度可以达到1 mm)。实验结果如下：

从图5可以看出，在运动过程中，ICP算法计算出来的位置与VICON计算出来的位置运动趋势基本一致，导致出现差异的原因是激光是运动的，相邻的两个时刻扫描到的环境不是完全相同，这会造成ICP算法对位置估计的不准确，随着时间的增长，迭代次数增多，最终造成与实际的位置有比较大的差距。

Figure 5. Curve: Trajectory map

图5. 运动轨迹图

Figure 6. Curve: The position of the x direction and the position of the y direction versus time

图6. x方向位置和y方向位置随时间变化图

从图5和图6可以看出，EKF算法计算的位置基本上与VICON的位置基本保持一致，计算出来的位置都能跟的上实际位置的变化，且不会出现太大的误差，同时从图6的最后一段可以看出，随着旋翼飞行器静止之后，EKF计算出来的位置基本与VICON的一致，说明当旋翼飞行器不移动时，随着EKF对位置的校正，误差会越来越小。

6. 结论

本文首先通过ICP算法对每一个时刻的位移进行估计，然后通过上位机对扫描仪的数据进行处理，提取出线段特征，并对线段特征进行匹配，并设计EKF滤波器对旋翼飞行器的位置进行校正，最终和VICON的数据进行比较。从实验结果可知，本文通过使用EKF算法对ICP计算出来的位置进行校正，提高了位置的精度，能够比较快速的跟踪上旋翼飞行器位置的变化。

基金项目

国家自然科学基金面上项目(No. 61573186)。

文章引用

谭贵芳,蔡晨晓. 基于激光测距仪的旋翼飞行器位置估计算法
Position Estimation Algorithm for Rotorcraft Based on Laser Range Finder[J]. 动力系统与控制, 2017, 06(04): 187-194. http://dx.doi.org/10.12677/DSC.2017.64024

参考文献 (References)

  1. 1. 段世华. 四旋翼飞行器控制系统的设计和实现[D]: [硕士学位论文]. 北京: 电子科技大学, 2012.

  2. 2. 吴中杰. 四旋翼飞行器设计及其姿态控制[D]: [硕士学位论文]. 南京:东南大学, 2012.

  3. 3. Mao, G., Drake, S. and Anderson, B.D.O. (2007) Design of an Extended Kalman Filter for UAV Localization. Information, Decision and Control, 224-229. https://doi.org/10.1109/IDC.2007.374554

  4. 4. Lange, S., Sunderhauf, N. and Protzel, P. (2009) A Vision Based Onboard Approach for Landing and Position Control of an Autonomous Multirotor UAV in GPS-Denied Environments. International Conference on Advanced Robotics, 1-6.

  5. 5. 隋婧, 金伟其. 双目立体视觉技术的实现及其进展[J]. 电子技术应用, 2004, 30(10): 4-6.

  6. 6. Borenstein, J. and Koren, Y. (2002) The Vector Field Histogram-Fast Obstacle Avoidance for Mobile Robots. IEEE Transactions on Robotics & Automation, 7, 278-288. https://doi.org/10.1109/70.88137

  7. 7. 任立敏. 基于激光测距仪的移动机器人障碍感知与避障策略研究[D]: [硕士学位论文]. 哈尔滨: 哈尔滨工业大学, 2007.

  8. 8. 彭晟远. 基于激光测距仪的室内机器人SLAM研究[D]: [硕士学位论文]. 武汉: 武汉科技大学, 2012.

  9. 9. 徐晓东. 移动机器人几何–拓扑混合地图构建及定位研究[D]: [硕士学位论文]. 大连: 大连理工大学, 2005.

  10. 10. Luo, C., Mcclean, S.I., Parr, G., et al. (2013) UAV Position Es-timation and Collision Avoidance Using the Extended Kalman Filter. IEEE Transactions on Vehicular Technology, 62, 2749-2762. https://doi.org/10.1109/TVT.2013.2243480

  11. 11. Li, R., Liu, J. and Zhang, L. (2014) LIDAR/MEMS IMU Inte-grated Navigation (SLAM) Method for a Small UAV in Indoor Environments. Inertial Sensors and Systems Symposium, Karlsruhe, 16-17 September 2014, 1-15. https://doi.org/10.1109/InertialSensors.2014.7049479

  12. 12. 祝继华, 郑南宁, 袁泽剑, 等. 基于ICP算法和粒子滤波的未知环境地图创建[J]. 自动化学报, 2009, 35(8): 1107-1113.

  13. 13. Hiremath, S.A., van der Heijden, G.W.A.M., Evert, F.K.V., et al. (2014) Laser Range Finder Model for Autonomous Navigation of a Robot in a Maize Field Using a Particle Filter. Computers & Electronics in Agriculture, 100, 41-50. https://doi.org/10.1016/j.compag.2013.10.005

  14. 14. Grisetti, G., Kummerle, R., Stachniss, C., et al. (2011) A Tu-torial on Graph-Based SLAM. IEEE Intelligent Transportation Systems Magazine, 2, 31-43. https://doi.org/10.1109/MITS.2010.939925

  15. 15. Daum, F. (2005) Nonlinear Filters: Beyond the Kalman Filter. IEEE Aerospace & Electronic Systems Magazine, 20, 57-69. https://doi.org/10.1109/MAES.2005.1499276

  16. 16. 曲仕茹, 马志强. 改进的粒子滤波在四旋翼姿态估计中的应用[J]. 飞行力学, 2013, 31(5): 458-461.

  17. 17. Cui, J. (2014) Navigation of Unmanned Aerial Vehicles in GPS-Denied Environments. Ph.D. Thesis, National University of Singapore, Singapore, 92-104.

  18. 18. 王龙, 章政, 王立. 改进扩展卡尔曼滤波的四旋翼姿态估计算法[J]. 计算机应用, 2017, 37(4): 1122-1128.

  19. 19. Perera, A.G.A., Srinivas, C., Hoogs, A., et al. (2006) Multi-Object Tracking through Simultaneous Long Occlusions and Split-Merge Conditions. 2006 IEEE Computer Society Conference, New York, 17-22 June 2006, 666-673. https://doi.org/10.1109/CVPR.2006.195

  20. 20. Jensfelt, P. and Christensen, H.I. (2001) Pose Tracking Using Laser Scanning and Minimalistic Environmental Models. IEEE Transactions on Robotics & Automation, 17, 138-147. https://doi.org/10.1109/70.928559

期刊菜单