CN113156450A - 无人机上的主动旋转激光雷达系统及其控制方法 - Google Patents

无人机上的主动旋转激光雷达系统及其控制方法 Download PDF

Info

Publication number
CN113156450A
CN113156450A CN202110287557.8A CN202110287557A CN113156450A CN 113156450 A CN113156450 A CN 113156450A CN 202110287557 A CN202110287557 A CN 202110287557A CN 113156450 A CN113156450 A CN 113156450A
Authority
CN
China
Prior art keywords
point
laser radar
unmanned aerial
aerial vehicle
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110287557.8A
Other languages
English (en)
Other versions
CN113156450B (zh
Inventor
林德福
郑章雄
吕军宁
金忍
王江
莫雳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110287557.8A priority Critical patent/CN113156450B/zh
Publication of CN113156450A publication Critical patent/CN113156450A/zh
Application granted granted Critical
Publication of CN113156450B publication Critical patent/CN113156450B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENTS OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D43/00—Arrangements or adaptations of instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/933—Lidar systems specially adapted for specific applications for anti-collision purposes of aircraft or spacecraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D3/00—Control of position or direction
    • G05D3/12—Control of position or direction using feedback

Abstract

本发明公开了一种无人机上的主动旋转激光雷达系统及其控制方法，该系统及方法中设置能够旋转的激光雷达，在激光雷达主动旋转的过程中，因为IMU短期的高精准性，可以用IMU的姿态数据暂时代替激光雷达的里程计结果，在激光雷达采集到约束足够的数据时，选择使用激光雷达的里程计结果，具体来说，实时解算并判断激光雷达获得的数据图像是否可靠，在不可靠的情况下，主动旋转该激光雷达，以使得下一帧数据图像的可靠性提高，另外，通过设置特异性的非线性方程来处理激光雷达获得的数据图像，从而得到无人机的位姿，进而据此对无人机进行控制。

Description

无人机上的主动旋转激光雷达系统及其控制方法
技术领域
本发明涉及激光雷达导航技术，具体涉及一种无人机上的主动旋转激光雷达系统及其控制方法。
背景技术
无人机(UAV)是指无人驾驶的由无线电遥控设备控制的或预先编译的程序控制的飞行器。与载人飞机相比，它具有体积小、造价低、使用方便、对作战环境要求低和战场生存能力较强等优点。随着近几年无人机的研究有很大进展，小型自主旋翼无人机能够在受限空间内进行高度敏捷的操作，并且不受地面障碍物的阻碍，在危险区域或人类无法抵达空间开展巡检、探测领域有着广阔的应用需求，无人机的应用领域也在不断扩大中。为了无人机能够适应各种不同场景的任务，无人机对周围环境感知的要求也不断提高。因此无人机的导航技术是无人机领域的重要技术之一。
现阶段大多数无人机的定位方法还是基于惯性传感器和全球导航卫星系统(GNSS)。而在应用场景不再是室外GNSS信号无遮挡的区域，而是室内、城市环境和树林等GNSS信号不稳定或无GNSS信号的环境的时候，无人机在这些场景的应用受到了严重的限制。
同步定位与建图(Simultaneous Localization and Mapping,SLAM)技术是是机器人视觉的一个重要研究内容，采用SLAM技术可以通过无人机搭载的激光雷达传感器对环境进行感知。结合IMU惯性测量元件，无人机可以估计自身的位姿。并且无人机通过对激光雷达采集的数据进行处理后，还可以建立起点云地图，通过对障碍物的检测，可以实现避障功能，从而提高无人机的避障飞行能力。
多数SLAM技术采用的是视觉摄像头导航，但是相机对环境的敏感性较高，对硬件的处理能力要求较高，应用场景限制在了无强光照的室内环境下。而在室外环境下，由于激光雷达具有精度高，对计算量要求小和不受光照干扰的特点，更多的方案则采用激光雷达导航。
在反制低慢小无人机时，激光SLAM可以为反制无人机提供良好的定位能力，但同时由于低慢小无人机的空间位置不确定性，要求反低慢小无人机在导航方面具有更强的环境适应性。无人机搭载固定放置的激光雷达传感器，在遇到狭长环境或只有高楼环境等这类俯仰方向约束极小的情况下，会因为几何特征不明显，容易引起退化现象，导致定位可靠性降低。虽然激光雷达的水平方向的视野可以达到360°，但垂直只有30°左右，俯仰方向的约束少，几何特征不明显的缺陷会被无限放大。而如果采用多摄像头或者多激光雷达的方式会造成负重增大的结果，必须提升无人机的载重，同时也会提高大量的经济成本。因此如何在几何特征少，约束不够的环境下进行定位和导航的问题仍需解决。
由于上述原因，本发明人对现有的激光雷达及其制导方法做了深入研究，以期待设计出一种能够解决上述问题的新的系统及方法。
发明内容
为了克服上述问题，本发明人进行了锐意研究，设计出一种无人机上的主动旋转激光雷达系统及其控制方法，该系统及方法中设置能够旋转的激光雷达，在激光雷达主动旋转的过程中，因为IMU短期的高精准性，可以用IMU的姿态数据暂时代替激光雷达的里程计结果，在激光雷达采集到约束足够的数据时，选择使用激光雷达的里程计结果，具体来说，实时解算并判断激光雷达获得的数据图像是否可靠，在不可靠的情况下，主动旋转该激光雷达，以使得下一帧数据图像的可靠性提高，另外，通过设置特异性的非线性方程来处理激光雷达获得的数据图像，从而得到无人机的位姿，进而据此对无人机进行控制，从而完成本发明。
具体来说，本发明的目的在于提供以一种无人机上的主动旋转激光雷达系统，其特征在于，该系统包括安装在无人机上的转台，在所述转台上安装有能够随着转台旋转的激光雷达，在所述激光雷达上还固定安装有IMU模块。
其中，该系统还包括转台控制模块和位姿解算模块；
通过所述转台控制模块实时判断是否需要旋转转台和转台的旋转角度；
通过所述位姿解算模块实时解算无人机的位姿。
其中，所述转台控制模块实时调取激光雷达实扫描获得的数据图像；
在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
其中，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；
优选地，将每帧图像分成6个部分。
本发明还提供一种装载有主动旋转激光雷达的无人机的控制方法，
该方法中，在无人机上安装转台1，在转台1上设置能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21；
通过转台控制模块实时解算安装在无人机上的转台的控制指令，
该方法中，通过位姿解算模块实时解算无人机的位姿。
其中，在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
其中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：
Figure BDA0002981115830000041
其中，当f(λy,λz)结果为0时，认为数据图像不可靠，
当f(λy,λz)结果为1时，认为数据图像可靠；
优选地，旋转角度Δθ通过下式(五)获得：
Figure BDA0002981115830000051
其中，
Figure BDA0002981115830000052
umax表示最大的特征值λmax对应的特征向量；
逆时针方向为正方向，顺时针方向为负方向。
其中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；
在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：
f(Tk+1)＝de+dH＝d (六)
其中，f(Tk+1)的雅克比矩阵为
Figure BDA0002981115830000053
式(六)的迭代过程为：
将Tk+1的初始值
Figure BDA0002981115830000054
代入到(de，dH)中，得到f(Tk+1)，其中
Figure BDA0002981115830000055
是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换，
将得到的f(Tk+1)代入到
Figure BDA0002981115830000056
中，再次得到f(Tk+1)的雅克比矩阵J，
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中，再次得到姿态的增量ΔTk+1，将得到的姿态增量ΔTk+1与原值Tk+1相加，得到新的姿态参数Tk+1；
重复上述迭代过程，直至：
Figure BDA0002981115830000057
且同时满足：
Figure BDA0002981115830000061
de表示点到直线的距离，dH表示点到面的距离。
其中，所述de和dH通过下式(七)、(八)解算获得：
Figure BDA0002981115830000062
Figure BDA0002981115830000063
其中，
Figure BDA0002981115830000064
表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标，
Figure BDA0002981115830000065
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000066
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000067
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值；
其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，
点l是点j所在扫描线中距离j最近的点，
点m是第k帧图像特征点在世界坐标系中离j最近的点。
其中，
Figure BDA0002981115830000068
通过下式(九)获得：
Figure BDA0002981115830000069
其中，R表示旋转矩阵，通过下式(十)实时获得：
R＝I+ωsinθ+ω2(1-cosθ) (十)
I表示单位矩阵，
ω＝T(k+1,i)(4:6)/||T(k+1,i)(4:6)||
θ＝||T(k+1,i)(4:6)||。
本发明所具有的有益效果包括：
(1)根据本发明提供的无人机上的主动旋转激光雷达系统及其控制方法决了无人机在低成本下，一个激光雷达无法完全感知周围的环境，导致数据不足和定位不准的问题，降低了成本，减少了对负重的要求；
(2)根据本发明提供的无人机上的主动旋转激光雷达系统及其控制方法针对激光雷达数据在低空多变的环境下，由于缺少俯仰方向的约束，会产生定位可靠性降低的情况，通过采用可主动旋转激光雷达的方法，有效地提高了定位准确性。
附图说明
图1示出根据本发明一种优选实施方式的无人机上的主动旋转激光雷达系统整体结构逻辑图；
图2示出本发明实施例中飞行轨迹对比图；
图3示出本发明对比例中飞行轨迹对比图；
图4示出本发明实施例中x/y/z方向的里程对比图；
图5示出本发明对比例中x/y/z方向的里程对比图；
图6示出本发明实施例中x/y/z方向的误差对比图；
图7示出本发明对比例中x/y/z方向的误差对比图。
附图标号说明：
1-转台
2-激光雷达
21-IMU模块
3-转台控制模块
4-位姿解算模块
具体实施方式
下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
根据本发明提供的无人机上的主动旋转激光雷达系统，如图1中所示，该系统包括安装在无人机上的转台1，在所述转台上安装有能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21。
其中，所述转台能够带动激光雷达绕着滚转方向旋转。
所述激光雷达扫描的频率是低频的，IMU模块是高频率的，可以达到400HZ，同时具有短期的高精准性，在成功获取激光雷达里程计结果前，使用IMU的姿态变换数据来代替里程计结果，提升激光雷达里程计的输出频率，从而提高可靠性。
在一个优选的实施方式中，该系统还包括转台控制模块3和位姿解算模块4；
通过所述转台控制模块3实时判断是否需要旋转转台1和转台1的旋转角度；
通过所述位姿解算模块4实时解算无人机的位姿，所述位姿包括无人机的位置和速度信息，在获得位姿的情况下，即可集合目标位置信息对无人机进行制导控制。
在一个优选的实施方式中，所述转台控制模块实时调取激光雷达实扫描获得的数据图像；所述激光雷达的工作频率为10HZ，激光雷达每工作一次，会有多条扫描线，由多条扫描线的扫描结果组合成一帧数据图像，也称之为一帧点云数据。
在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
在一个优选的实施方式中，在步骤S1中，通过下式解算每个点的曲率：
Figure BDA0002981115830000091
c(k,i)表示第k帧图像中第i个点的曲率，X(k,i)第k帧图像中第i个点的数据，即该点的坐标。本申请中，在步骤S1中计算除了每条扫描线中最初5个点和最后5个点以外的所有点的曲率。最终得到第k帧图像中，雷达坐标系下所有特征点的集合
Figure BDA0002981115830000092
其中m是特征点个数。
优选地，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；
为使得特征点分布均匀，优选地，将每帧图像分成6个部分。具体来说，每一次扫描线，扫描的角度是360°，从0°开始，每60°为一个部分，从而将扫描线平均分为6个部分。
在一个优选的实施方式中，在步骤S2中，协方差通过下式(一)获得：
Figure BDA0002981115830000101
其中，C表示协方差，m表示数据集中点的数量，
Figure BDA0002981115830000102
表示雷达坐标系下特征点去中心化后的特征点集合，通过下式(二)获得：
Figure BDA0002981115830000103
Figure BDA0002981115830000104
表示
Figure BDA0002981115830000105
去中心化后特征点集合中第i个数据，
Figure BDA0002981115830000106
表示雷达坐标系下，第k帧图像的第i个特征点；
优选地，通过下式(三)进行特征值分解：
C＝UΛUT (三)
Figure BDA0002981115830000107
U＝[ux uy uz]
λx、λy和λz分别表示x轴方向、y轴方向和z轴方向的特征值；
ux、uy和uz分别表示x轴方向、y轴方向和z轴方向的特征向量。
在一个优选的实施方式中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：
Figure BDA0002981115830000111
其中，当f(λy,λz)结果为0时，认为数据图像不可靠，
当f(λy,λz)结果为1时，认为数据图像可靠；
优选地，旋转角度Δθ通过下式(五)获得：
Figure BDA0002981115830000112
其中，
Figure BDA0002981115830000113
umax表示最大的特征值λmax对应的特征向量；
其获得方式为：通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量；
通过比较特征值大小，获得最大的特征值λmax＝max(|λy|,|λz|)，则是最大特征值相应的特征向量umax。
优选地，逆时针方向为正方向，顺时针方向为负方向。
在一个优选的实施方式中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；
在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：
f(Tk+1)＝de+dH＝d (六)
其中，f(Tk+1)的雅克比矩阵为
Figure BDA0002981115830000114
式(六)的迭代过程为：
将Tk+1的初始值
Figure BDA0002981115830000115
代入到(de，dH)中，得到f(Tk+1)，其中
Figure BDA0002981115830000116
是由IMU模块得到的第k帧图像位姿到第k+1帧图像位姿的变换，
将得到的f(Tk+1)代入到
Figure BDA0002981115830000121
中，再次得到f(Tk+1)的雅克比矩阵J，
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中，再次得到姿态的增量ΔTk+1，
将得到的姿态增量ΔTk+1与原值Tk+1相加，得到新的姿态参数Tk+1；
重复上述迭代过程，直至：
Figure BDA0002981115830000122
且同时满足：
Figure BDA0002981115830000123
时迭代终止；
de表示点到直线的距离，dH表示点到面的距离。
本申请中，通过迭代得到Tk+1，其表达式为：
Tk+1＝[tx,ty,tz,θx,θy,θz]T，
其中，tx,ty,tz表示执行第k+1次扫描时无人机在世界坐标系中的坐标值，
θx,θy,θz表示执行第k+1次扫描时无人机的滚转角、俯仰角和偏航角；
所述世界坐标系是指程序初始运行时刻激光雷达所在的坐标系，并设激光雷达正前方为x轴，正左方为y轴，通过右手定则确定正上方为z轴。
在一个优选的实施方式中，所述de和dH通过下式(七)、(八)解算获得：
Figure BDA0002981115830000131
Figure BDA0002981115830000132
其中，
Figure BDA0002981115830000133
表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标，
Figure BDA0002981115830000134
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000135
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000136
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值，
其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，即将特征点全部转换到世界坐标系下进行比较；
点l是点j所在扫描线中距离j最近的点，
点m是第k帧图像特征点在世界坐标系中与j不同扫描线离j最近的点。
所述点j、l和m都是步骤S1中的数据集中的点；
点j、l和m构成平面，该平面即为dH中的平面。
在一个优选的实施方式中，
Figure BDA0002981115830000137
通过下式(九)获得：
Figure BDA0002981115830000138
其中，R表示旋转矩阵，通过下式(十)实时获得：
R＝I+ωsinθ+ω2(1-cosθ) (十)
I表示单位矩阵，
Figure BDA0002981115830000141
ω＝T(k+1,i)(4:6)/||T(k+1,i)(4:6)||
θ＝||T(k+1,i)(4:6)||
在计算过程中T(k+1,i)的值通过IMU模块获得；
本申请中，T(k+1,i)(a:b)代表T(k+1,i)的第a个数据到第b个数据之间的全部数据。
本发明还提供一种装载有主动旋转激光雷达的无人机的控制方法，
该方法中，在无人机上安装转台1，在转台1上设置能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21；
通过转台控制模块3实时解算安装在无人机上的转台的控制指令，
所述转台控制指令包括是否需要旋转转台和转台的旋转角度；
该方法中，通过位姿解算模块4实时解算无人机的位姿。
具体来说，在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
在一个优选的实施方式中，在步骤S1中，通过下式解算每个点的曲率：
Figure BDA0002981115830000151
c(k,i)表示第k帧图像中第i个点的曲率，X(k,i)第k帧图像中第i个点的数据，即该点的坐标。本申请中，在步骤S1中计算除了每条扫描线中最初5个点和最后5个点以外的所有点的曲率。
优选地，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；
优选地，将每帧图像分成6个部分。每一次扫描线，扫描的角度是360°，从0°开始，每60°为一个部分，从而可以将扫描线平均分为6个部分。
在一个优选的实施方式中，在步骤S2中，协方差通过下式(一)获得：
Figure BDA0002981115830000152
其中，C表示协方差，m表示数据集中点的数量，
Figure BDA0002981115830000153
表示雷达坐标系下特征点去中心化后的特征点集合，通过下式(二)获得：
Figure BDA0002981115830000161
Figure BDA0002981115830000162
表示
Figure BDA0002981115830000163
去中心化后特征点集合中第i个数据，
Figure BDA0002981115830000164
表示雷达坐标系下，第k帧图像的第i个特征点
优选地，通过下式(三)进行特征值分解：
C＝UΛUT (三)
Figure BDA0002981115830000165
U＝[ux uy uz]
λx、λy和λz分别表示x轴方向、y轴方向和z轴方向的特征值；
ux、uy和uz分别表示x轴方向、y轴方向和z轴方向的特征向量。
在一个优选的实施方式中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：
Figure BDA0002981115830000166
其中，当f(λy,λz)结果为0时，认为数据图像不可靠，
当f(λy,λz)结果为1时，认为数据图像可靠；
优选地，旋转角度Δθ通过下式(五)获得：
Figure BDA0002981115830000167
其中，
Figure BDA0002981115830000171
umax表示最大的特征值λmax对应的特征向量，其获得方式为：通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量，
通过比较特征值大小，获得最大的特征值λmax＝max(|λy|,|λz|)，则是最大特征值相应的特征向量umax。
优选地，逆时针方向为正方向，顺时针方向为负方向。
在一个优选的实施方式中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；
在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：
f(Tk+1)＝de+dH＝d (六)
其中，f(Tk+1)的雅克比矩阵为
Figure BDA0002981115830000172
式(六)的迭代过程为：
将Tk+1的初始值
Figure BDA0002981115830000173
代入到(de，dH)中，得到f(Tk+1)，其中
Figure BDA0002981115830000174
是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换；，
将得到的f(Tk+1)代入到
Figure BDA0002981115830000175
中，再次得到f(Tk+1)的雅克比矩阵J，
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中，再次得到姿态的增量ΔTk+1；
将得到的姿态增量ΔTk+1与原值Tk+1相加，得到新的姿态参数Tk+1；
重复上述迭代过程，直至：
Figure BDA0002981115830000181
且同时满足：
Figure BDA0002981115830000182
de表示点到直线的距离，dH表示点到面的距离。
本申请中，通过迭代得到Tk+1，其表达式为：
Tk+1＝[tx,ty,tz,θx,θy,θz]T，
其中，tx,ty,tz表示执行第k+1次扫描时无人机在世界坐标系中的坐标值，
θx,θy,θz表示执行第k+1次扫描时无人机的滚转角、俯仰角和偏航角；
所述世界坐标系是指程序初始运行时刻激光雷达所在的坐标系，并设激光雷达正前方为x轴，正左方为y轴，通过右手定则确定正上方为z轴。
在一个优选的实施方式中，所述de和dH通过下式(七)、(八)解算获得：
Figure BDA0002981115830000183
Figure BDA0002981115830000184
其中，
Figure BDA0002981115830000185
表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标，
Figure BDA0002981115830000186
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000187
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值，
Figure BDA0002981115830000191
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值，
其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，
点l是点j所在扫描线中距离j最近的点，
点m是第k帧图像特征点在世界坐标系中和j不同扫描线离j最近的点。
所述点j、l和m都是步骤S1中的数据集中的点；
点j、l和m构成平面，该平面即为dH中的平面。
在一个优选的实施方式中，
Figure BDA0002981115830000192
通过下式(九)获得：
Figure BDA0002981115830000193
其中，R表示旋转矩阵，通过下式(十)实时获得：
R＝I+ωsinθ+ω2(1-cosθ) (十)
I表示单位矩阵，
ω＝T(k+1,i)(4:6)/||T(k+1,i)(4:6)||
θ＝||T(k+1,i)(4:6)||
在计算过程中T(k+1,i)的值通过IMU模块获得；
本申请中，T(k+1,i)(a:b)代表T(k+1,i)的第a个数据到第b个数据之间的全部数据，包括第a个数据和第b个数据。
实施例：
选用以大疆M600无人机，在其上安装转台，在转台上安装能够随转台绕滚转轴旋转的线三维激光雷达VLP-16，在激光雷达上安装、MTI-300惯性传感器；该激光雷达每秒能够有30万个点数据输出，±15°的垂直视场，360°水平视场扫描。
将无人机放置在开阔的土地上，周围只具有一些不超过20m的楼房。设置轨迹为一个半矩形，控制无人机按照该矩形轨迹飞行，在无人机上实时通过转台控制模块控制转台旋转，实时通过位姿解算模块解算无人机自身的位姿，并且将解算得到的位姿存储在无人机中，
在无人机按照轨迹飞行的过程中，还实时记录无人机真实的位姿。
无人机在飞行过程中，转台控制模块的工作过程为：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
其中，是否需要旋转转台通过述方法判断：
Figure BDA0002981115830000201
其中，当f(λy,λz)结果为0时，认为数据图像不可靠，
当f(λy,λz)结果为1时，认为数据图像可靠；
在数据不可靠时，控制转台旋转角度Δθ。
Δθ通过下式获得：
Figure BDA0002981115830000202
其中，
Figure BDA0002981115830000211
umax表示最大的特征值λmax对应的特征向量，通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量。
位姿解算模块的工作过程为：
调取激光雷达实扫描获得的数据图像，
通过下述迭代方法来解算无人机的位姿：
f(Tk+1)＝de+dH＝d (六)
其中，f(Tk+1)的雅克比矩阵为
Figure BDA0002981115830000212
式(六)的迭代过程为：
将Tk+1的初始值
Figure BDA0002981115830000213
代入到(de，dH)中，得到f(Tk+1)，其中
Figure BDA0002981115830000214
是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换，
将得到的f(Tk+1)代入到
Figure BDA0002981115830000215
中，再次得到f(Tk+1)的雅克比矩阵J，
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中，再次得到姿态的增量ΔTk+1，
将得到的姿态增量ΔTk+1与原值Tk+1相加，得到新的姿态参数Tk+1；
重复上述迭代过程，直至：
Figure BDA0002981115830000216
且同时满足：
Figure BDA0002981115830000217
时停止迭代；
通过迭代得到的Tk+1即为执行第k+1次扫描时无人机的位姿。
记录得到的飞行轨迹如图2中的红色线条所示，对应真实的飞行轨迹如图2中的蓝色线条所示；
记录得到的飞行器在x/y/z方向的里程如图4中的红色线条所示，对应真实的飞行器在x/y/z方向的里程如图4中的绿色线条所示；
记录得到的飞行器在x/y/z方向的误差如图6中的红/绿/蓝色线条所示。
对比例：
选用以大疆M600无人机，将线三维激光雷达VLP-16固定安装在该无人机上。
将无人机放置在开阔的土地上，周围只具有一些不超过20m的楼房。设置轨迹为一个半矩形，控制无人机按照该矩形轨迹飞行，在无人机上实时通过读取解算无人机自身的位姿，并且将解算得到的位姿存储在无人机中，
在无人机按照轨迹飞行的过程中，还实时记录无人机真实的位姿。
记录得到的飞行轨迹如图3中的红色线条所示，对应真实的飞行轨迹如图3中的蓝蓝色线条所示；
记录得到的飞行器在x/y/z方向的里程如图5中的红色线条所示，对应真实的飞行器在x/y/z方向的里程如图5中的绿色线条所示；
记录得到的飞行器在x/y/z方向的误差如图7中的红/绿/蓝色线条所示；
根据实施例和对比例的结果，比较图2和图3，图4和图5，图6和图7可知，实施例中得到的轨迹与真实值之间的平均误差为0.190m，对比例中得到的轨迹与真实值之间的平均误差为0.501m，本申请提供的无人机上的主动旋转激光雷达系统及其控制方法在相同条件下能够显著提高自身位姿的获取精度。
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

Claims (10)

1.一种无人机上的主动旋转激光雷达系统，其特征在于，该系统包括安装在无人机上的转台(1)，在所述转台上安装有能够随着转台旋转的激光雷达(2)，在所述激光雷达上还固定安装有IMU模块(21)。
2.根据权利要求1所述的主动旋转激光雷达系统，其特征在于，
该系统还包括转台控制模块(3)和位姿解算模块(4)；
通过所述转台控制模块实时判断是否需要旋转转台和转台的旋转角度；
通过所述位姿解算模块实时解算无人机的位姿。
3.根据权利要求2所述的主动旋转激光雷达系统，其特征在于，
所述转台控制模块实时调取激光雷达实扫描获得的数据图像；
在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
4.根据权利要求3所述的主动旋转激光雷达系统，其特征在于，
在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；
优选地，将每帧图像分成6个部分。
5.一种装载有主动旋转激光雷达的无人机的控制方法，其特征在于：
该方法中，在无人机上安装转台(1)，在转台(1)上设置能够随着转台旋转的激光雷达(2)，在所述激光雷达上固定安装IMU模块(21)；
通过转台控制模块(3)实时解算安装在无人机上的转台的控制指令，
该方法中，还通过位姿解算模块(4)实时解算无人机的位姿。
6.根据权利要求5所述的装载有主动旋转激光雷达的无人机的控制方法，其特征在于：
在所述转台控制模块中执行如下步骤：
S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；
S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；
S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。
7.根据权利要求6所述的装载有主动旋转激光雷达的无人机的控制方法，其特征在于：
在步骤S3中，通过下式(四)实时判断数据图像是否可靠：
Figure FDA0002981115820000021
其中，当f(λy,λz)结果为0时，认为数据图像不可靠，
当f(λy,λz)结果为1时，认为数据图像可靠；
优选地，旋转角度Δθ通过下式(五)获得：
Figure FDA0002981115820000031
其中，
Figure FDA0002981115820000032
umax表示最大的特征值λmax对应的特征向量；
逆时针方向为正方向，顺时针方向为负方向。
8.根据权利要求5所述的装载有主动旋转激光雷达的无人机的控制方法，其特征在于：
所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；
在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：
f(Tk+1)＝de+dH＝d (六)
其中，f(Tk+1)的雅克比矩阵为
Figure FDA0002981115820000033
式(六)的迭代过程为：
将Tk+1的初始值
Figure FDA0002981115820000034
代入到(de，dH)中，得到f(Tk+1)，其中
Figure FDA0002981115820000035
是由IMU模块得到的第k帧图像位姿到第k+1帧图像位姿的变换；
将得到的f(Tk+1)代入到
Figure FDA0002981115820000036
中，再次得到f(Tk+1)的雅克比矩阵J，
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中，再次得到姿态的增量ΔTk+1，
将得到的姿态增量ΔTk+1与原值Tk+1相加，得到新的姿态参数Tk+1；
重复上述迭代过程，直至：
Figure FDA0002981115820000041
且同时满足：
Figure FDA0002981115820000042
de表示点到直线的距离，dH表示点到面的距离。
9.根据权利要求8所述的装载有主动旋转激光雷达的无人机的控制方法，其特征在于：
所述de和dH通过下式(七)、(八)解算获得：
Figure FDA0002981115820000043
Figure FDA0002981115820000044
其中，
Figure FDA0002981115820000045
表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标，
Figure FDA0002981115820000046
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值，
Figure FDA0002981115820000047
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值，
Figure FDA0002981115820000048
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值；
其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，
点l是点j所在扫描线中距离j最近的点，
点m是第k帧图像中离j最近的点。
10.根据权利要求9所述的主动旋转激光雷达系统，其特征在于，
Figure FDA0002981115820000051
通过下式(九)获得：
Figure FDA0002981115820000052
其中，R表示旋转矩阵，通过下式(十)实时获得：
R＝I+ωsinθ+ω2(1-cosθ) (十)
I表示单位矩阵，
ω＝T(k+1,i)(4:6)/||T(k+1,i)(4:6)||
θ＝||T(k+1,i)(4:6)||。
CN202110287557.8A 2021-03-17 2021-03-17 无人机上的主动旋转激光雷达系统及其控制方法 Active CN113156450B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110287557.8A CN113156450B (zh) 2021-03-17 2021-03-17 无人机上的主动旋转激光雷达系统及其控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110287557.8A CN113156450B (zh) 2021-03-17 2021-03-17 无人机上的主动旋转激光雷达系统及其控制方法

Publications (2)

Publication Number Publication Date
CN113156450A true CN113156450A (zh) 2021-07-23
CN113156450B CN113156450B (zh) 2024-02-27

Family

ID=76887507

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110287557.8A Active CN113156450B (zh) 2021-03-17 2021-03-17 无人机上的主动旋转激光雷达系统及其控制方法

Country Status (1)

Country Link
CN (1) CN113156450B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199233A (zh) * 2021-11-08 2022-03-18 北京旷视科技有限公司 位姿确定方法及可移动设备

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017177533A1 (zh) * 2016-04-12 2017-10-19 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
CN207258000U (zh) * 2017-08-28 2018-04-20 国网甘肃省电力公司电力科学研究院 一种惯导数据共享无人机巡检多平台吊舱
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN111948598A (zh) * 2020-07-30 2020-11-17 西南电子技术研究所（中国电子科技集团公司第十研究所） 空域干扰信号检测方法与装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017177533A1 (zh) * 2016-04-12 2017-10-19 深圳市龙云创新航空科技有限公司 基于激光雷达的微型无人机操控方法及系统
CN207258000U (zh) * 2017-08-28 2018-04-20 国网甘肃省电力公司电力科学研究院 一种惯导数据共享无人机巡检多平台吊舱
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN111948598A (zh) * 2020-07-30 2020-11-17 西南电子技术研究所（中国电子科技集团公司第十研究所） 空域干扰信号检测方法与装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
吴毅: "基于激光点云的边坡地形提取及可视化研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, no. 02 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199233A (zh) * 2021-11-08 2022-03-18 北京旷视科技有限公司 位姿确定方法及可移动设备
CN114199233B (zh) * 2021-11-08 2024-04-05 北京旷视科技有限公司 位姿确定方法及可移动设备

Also Published As

Publication number Publication date
CN113156450B (zh) 2024-02-27

Similar Documents

Publication Publication Date Title
EP3903164B1 (en) Collision avoidance system, depth imaging system, vehicle, map generator, amd methods thereof
US10914590B2 (en) Methods and systems for determining a state of an unmanned aerial vehicle
CN108253958B (zh) 一种稀疏环境下的机器人实时定位方法
CN102298070B (zh) 估算无人机,尤其是能够在自动驾驶下执行悬停飞行的无人机的水平速度的方法
CN110221625B (zh) 无人机精确位置的自主降落导引方法
EP3850456B1 (en) Control and navigation systems, pose optimisation, mapping, and localisation techniques
WO2020103049A1 (zh) 旋转微波雷达的地形预测方法、装置、系统和无人机
CN113066120B (zh) 一种基于机器视觉的智能杆塔倾斜检测方法
CN112184812B (zh) 提升无人机相机对AprilTag识别定位精度方法及定位方法和系统
CN106643664A (zh) 一种对无人机进行定位的方法及装置
CN109031312A (zh) 适用于烟囱内部作业的飞行平台定位装置和定位方法
US20210101747A1 (en) Positioning apparatus capable of measuring position of moving body using image capturing apparatus
CN113156450B (zh) 无人机上的主动旋转激光雷达系统及其控制方法
CN114638419A (zh) 一种塔吊物体轨迹预测的方法及设备
CN112947569B (zh) 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法
WO2021166845A1 (ja) 情報処理装置、情報処理方法、及びプログラム
GB2581403A (en) Pose optimisation, mapping, and localisation techniques
CN113465596A (zh) 一种基于多传感器融合的四旋翼无人机定位方法
CN116188470B (zh) 一种基于无人机航拍识别的故障定位方法和系统
CN109827595B (zh) 室内惯性导航仪方向校准方法、室内导航装置及电子设备
CN109764864A (zh) 一种基于颜色识别的室内无人机位姿获取方法及系统
Lecking et al. Localization in a wide range of industrial environments using relative 3D ceiling features
Xin et al. An on-board pan-tilt controller for ground target tracking systems
Ishii et al. Autonomous UAV flight using the Total Station Navigation System in Non-GNSS Environments
CN117870652B (zh) 一种基于雷达、惯性、视觉融合的陆空在线协同定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant