CN113156450B - 无人机上的主动旋转激光雷达***及其控制方法 - Google Patents
无人机上的主动旋转激光雷达***及其控制方法 Download PDFInfo
- Publication number
- CN113156450B CN113156450B CN202110287557.8A CN202110287557A CN113156450B CN 113156450 B CN113156450 B CN 113156450B CN 202110287557 A CN202110287557 A CN 202110287557A CN 113156450 B CN113156450 B CN 113156450B
- Authority
- CN
- China
- Prior art keywords
- laser radar
- turntable
- point
- unmanned aerial
- aerial vehicle
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 230000008569 process Effects 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 22
- 239000013598 vector Substances 0.000 claims description 13
- 238000012804 iterative process Methods 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000001131 transforming effect Effects 0.000 claims 1
- 230000000052 comparative effect Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 6
- 238000000354 decomposition reaction Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000005286 illumination Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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; ARRANGEMENT 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
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中,通过下式(四)实时判断数据图像是否可靠:
其中,当f(λy,λz)结果为0时,认为数据图像不可靠,
当f(λy,λz)结果为1时,认为数据图像可靠;
优选地,旋转角度Δθ通过下式(五)获得:
其中,
umax表示最大的特征值λmax对应的特征向量;
逆时针方向为正方向,顺时针方向为负方向。
其中,所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值;
在所述位姿解算模块中,通过下式(六)迭代获得无人机的位姿:
f(Tk+1)=de+dH=d (六)
其中,f(Tk+1)的雅克比矩阵为
式(六)的迭代过程为:
将Tk+1的初始值代入到(de,dH)中,得到f(Tk+1),其中/>是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换,
将得到的f(Tk+1)代入到中,再次得到f(Tk+1)的雅克比矩阵J,
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中,再次得到姿态的增量ΔTk+1,将得到的姿态增量ΔTk+1与原值Tk+1相加,得到新的姿态参数Tk+1;
重复上述迭代过程,直至:
且同时满足:
de表示点到直线的距离,dH表示点到面的距离。
其中,所述de和dH通过下式(七)、(八)解算获得:
其中,表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标,
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值;
其中,点j是第k帧图像特征点在世界坐标系中离点i最近的点,
点l是点j所在扫描线中距离j最近的点,
点m是第k帧图像特征点在世界坐标系中离j最近的点。
其中,通过下式(九)获得:
其中,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中,通过下式解算每个点的曲率:
c(k,i)表示第k帧图像中第i个点的曲率,X(k,i)第k帧图像中第i个点的数据,即该点的坐标。本申请中,在步骤S1中计算除了每条扫描线中最初5个点和最后5个点以外的所有点的曲率。最终得到第k帧图像中,雷达坐标系下所有特征点的集合其中m是特征点个数。
优选地,在步骤S1中,所述每帧图像都包括多条扫描线,将每条扫描线分成多个部分,从每个部分中选取曲率最大的两个点作为边缘点,再从每个部分中选取曲率最小的四个点作为平面点;
为使得特征点分布均匀,优选地,将每帧图像分成6个部分。具体来说,每一次扫描线,扫描的角度是360°,从0°开始,每60°为一个部分,从而将扫描线平均分为6个部分。
在一个优选的实施方式中,在步骤S2中,协方差通过下式(一)获得:
其中,C表示协方差,m表示数据集中点的数量,
表示雷达坐标系下特征点去中心化后的特征点集合,通过下式(二)获得:
表示/>去中心化后特征点集合中第i个数据,/>表示雷达坐标系下,第k帧图像的第i个特征点;
优选地,通过下式(三)进行特征值分解:
C=UΛUT (三)
U=[ux uy uz]
λx、λy和λz分别表示x轴方向、y轴方向和z轴方向的特征值;
ux、uy和uz分别表示x轴方向、y轴方向和z轴方向的特征向量。
在一个优选的实施方式中,在步骤S3中,通过下式(四)实时判断数据图像是否可靠:
其中,当f(λy,λz)结果为0时,认为数据图像不可靠,
当f(λy,λz)结果为1时,认为数据图像可靠;
优选地,旋转角度Δθ通过下式(五)获得:
其中,
umax表示最大的特征值λmax对应的特征向量;
其获得方式为:通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量;
通过比较特征值大小,获得最大的特征值λmax=max(|λy|,|λz|),则是最大特征值相应的特征向量umax。
优选地,逆时针方向为正方向,顺时针方向为负方向。
在一个优选的实施方式中,所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值;
在所述位姿解算模块中,通过下式(六)迭代获得无人机的位姿:
f(Tk+1)=de+dH=d (六)
其中,f(Tk+1)的雅克比矩阵为
式(六)的迭代过程为:
将Tk+1的初始值代入到(de,dH)中,得到f(Tk+1),其中/>是由IMU模块得到的第k帧图像位姿到第k+1帧图像位姿的变换,
将得到的f(Tk+1)代入到中,再次得到f(Tk+1)的雅克比矩阵J,
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中,再次得到姿态的增量ΔTk+1,
将得到的姿态增量ΔTk+1与原值Tk+1相加,得到新的姿态参数Tk+1;
重复上述迭代过程,直至:
且同时满足:
时迭代终止;
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通过下式(七)、(八)解算获得:
其中,表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标,
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值,
其中,点j是第k帧图像特征点在世界坐标系中离点i最近的点,即将特征点全部转换到世界坐标系下进行比较;
点l是点j所在扫描线中距离j最近的点,
点m是第k帧图像特征点在世界坐标系中与j不同扫描线离j最近的点。
所述点j、l和m都是步骤S1中的数据集中的点;
点j、l和m构成平面,该平面即为dH中的平面。
在一个优选的实施方式中,通过下式(九)获得:
其中,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个数据之间的全部数据。
本发明还提供一种装载有主动旋转激光雷达的无人机的控制方法,
该方法中,在无人机上安装转台1,在转台1上设置能够随着转台旋转的激光雷达2,在所述激光雷达上还固定安装有IMU模块21;
通过转台控制模块3实时解算安装在无人机上的转台的控制指令,
所述转台控制指令包括是否需要旋转转台和转台的旋转角度;
该方法中,通过位姿解算模块4实时解算无人机的位姿。
具体来说,在所述转台控制模块中执行如下步骤:
S1,解算数据图像中每个点的曲率,从中挑取出一定数量的边缘点和平面点,共同组成数据集;
S2,对数据集做去中心化处理,解算协方差,并通过特征值分解得到特征值和特征向量;
S3,比较特征值大小,并据此判断此次激光雷达扫描获得的数据图像是否可靠,如数据不可靠,解算出转台旋转角度Δθ,并据此控制转台旋转。
在一个优选的实施方式中,在步骤S1中,通过下式解算每个点的曲率:
c(k,i)表示第k帧图像中第i个点的曲率,X(k,i)第k帧图像中第i个点的数据,即该点的坐标。本申请中,在步骤S1中计算除了每条扫描线中最初5个点和最后5个点以外的所有点的曲率。
优选地,在步骤S1中,所述每帧图像都包括多条扫描线,将每条扫描线分成多个部分,从每个部分中选取曲率最大的两个点作为边缘点,再从每个部分中选取曲率最小的四个点作为平面点;
优选地,将每帧图像分成6个部分。每一次扫描线,扫描的角度是360°,从0°开始,每60°为一个部分,从而可以将扫描线平均分为6个部分。
在一个优选的实施方式中,在步骤S2中,协方差通过下式(一)获得:
其中,C表示协方差,m表示数据集中点的数量,
表示雷达坐标系下特征点去中心化后的特征点集合,通过下式(二)获得:
表示/>去中心化后特征点集合中第i个数据,/>表示雷达坐标系下,第k帧图像的第i个特征点
优选地,通过下式(三)进行特征值分解:
C=UΛUT (三)
U=[ux uy uz]
λx、λy和λz分别表示x轴方向、y轴方向和z轴方向的特征值;
ux、uy和uz分别表示x轴方向、y轴方向和z轴方向的特征向量。
在一个优选的实施方式中,在步骤S3中,通过下式(四)实时判断数据图像是否可靠:
其中,当f(λy,λz)结果为0时,认为数据图像不可靠,
当f(λy,λz)结果为1时,认为数据图像可靠;
优选地,旋转角度Δθ通过下式(五)获得:
其中,
umax表示最大的特征值λmax对应的特征向量,其获得方式为:通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量,
通过比较特征值大小,获得最大的特征值λmax=max(|λy|,|λz|),则是最大特征值相应的特征向量umax。
优选地,逆时针方向为正方向,顺时针方向为负方向。
在一个优选的实施方式中,所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值;
在所述位姿解算模块中,通过下式(六)迭代获得无人机的位姿:
f(Tk+1)=de+dH=d (六)
其中,f(Tk+1)的雅克比矩阵为
式(六)的迭代过程为:
将Tk+1的初始值代入到(de,dH)中,得到f(Tk+1),其中/>是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换;,
将得到的f(Tk+1)代入到中,再次得到f(Tk+1)的雅克比矩阵J,
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中,再次得到姿态的增量ΔTk+1;
将得到的姿态增量ΔTk+1与原值Tk+1相加,得到新的姿态参数Tk+1;
重复上述迭代过程,直至:
且同时满足:
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通过下式(七)、(八)解算获得:
其中,表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标,
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值,/>
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值,
其中,点j是第k帧图像特征点在世界坐标系中离点i最近的点,
点l是点j所在扫描线中距离j最近的点,
点m是第k帧图像特征点在世界坐标系中和j不同扫描线离j最近的点。
所述点j、l和m都是步骤S1中的数据集中的点;
点j、l和m构成平面,该平面即为dH中的平面。
在一个优选的实施方式中,通过下式(九)获得:
其中,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,比较特征值大小,并据此判断此次激光雷达扫描获得的数据图像是否可靠,如数据不可靠,解算出转台旋转角度Δθ,并据此控制转台旋转。
其中,是否需要旋转转台通过述方法判断:
其中,当f(λy,λz)结果为0时,认为数据图像不可靠,
当f(λy,λz)结果为1时,认为数据图像可靠;
在数据不可靠时,控制转台旋转角度Δθ。
Δθ通过下式获得:
其中,
umax表示最大的特征值λmax对应的特征向量,通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量。
位姿解算模块的工作过程为:
调取激光雷达实扫描获得的数据图像,
通过下述迭代方法来解算无人机的位姿:
f(Tk+1)=de+dH=d (六)
其中,f(Tk+1)的雅克比矩阵为
式(六)的迭代过程为:
将Tk+1的初始值代入到(de,dH)中,得到f(Tk+1),其中/>是由IMU数据得到的第k帧图像位姿到第k+1帧图像位姿的变换,
将得到的f(Tk+1)代入到中,再次得到f(Tk+1)的雅克比矩阵J,
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中,再次得到姿态的增量ΔTk+1,
将得到的姿态增量ΔTk+1与原值Tk+1相加,得到新的姿态参数Tk+1;
重复上述迭代过程,直至:
且同时满足:
时停止迭代;
通过迭代得到的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 (8)
1.一种无人机上的主动旋转激光雷达***,其特征在于,该***包括安装在无人机上的转台(1),在所述转台上安装有能够随着转台旋转的激光雷达(2),在所述激光雷达(2)上还固定安装有IMU模块(21);
该***还包括转台控制模块(3)和位姿解算模块(4);
通过所述转台控制模块(3)实时判断是否需要旋转转台(1)和转台的旋转角度;
通过所述位姿解算模块(4)实时解算无人机的位姿;
所述转台控制模块(3)实时调取激光雷达(2)实扫描获得的数据图像;
在所述转台控制模块(3)中执行如下步骤:
S1,解算数据图像中每个点的曲率,从中挑取出一定数量的边缘点和平面点,共同组成数据集;
S2,对数据集做去中心化处理,解算协方差,并通过特征值分解得到特征值和特征向量;
S3,比较特征值大小,并据此判断此次激光雷达(2)扫描获得的数据图像是否可靠,如数据不可靠,解算出转台旋转角度Δθ,并据此控制转台旋转。
2.根据权利要求1所述的主动旋转激光雷达***,其特征在于,
在步骤S1中,每帧图像都包括多条扫描线,将每条扫描线分成多个部分,从每个部分中选取曲率最大的两个点作为边缘点,再从每个部分中选取曲率最小的四个点作为平面点。
3.根据权利要求2所述的主动旋转激光雷达***,其特征在于,将每帧图像分成6个部分。
4.一种装载有主动旋转激光雷达的无人机的控制方法,其特征在于:
该方法中,在无人机上安装转台(1),在转台(1)上设置能够随着转台旋转的激光雷达(2),在所述激光雷达(2)上固定安装IMU模块(21);
通过转台控制模块(3)实时解算安装在无人机上的转台(1)的控制指令,
该方法中,还通过位姿解算模块(4)实时解算无人机的位姿;
在所述转台控制模块(3)中执行如下步骤:
S1,解算数据图像中每个点的曲率,从中挑取出一定数量的边缘点和平面点,共同组成数据集;
S2,对数据集做去中心化处理,解算协方差,并通过特征值分解得到特征值和特征向量;
S3,比较特征值大小,并据此判断此次激光雷达(2)扫描获得的数据图像是否可靠,如数据不可靠,解算出转台旋转角度Δθ,并据此控制转台旋转。
5.根据权利要求4所述的装载有主动旋转激光雷达的无人机的控制方法,其特征在于:
在步骤S3中,通过下式(四)实时判断数据图像是否可靠:
其中,当f(λy,λz)结果为0时,认为数据图像不可靠,
当f(λy,λz)结果为1时,认为数据图像可靠,
λy和λz分别表示y轴方向和z轴方向的特征值。
6.根据权利要求5所述的装载有主动旋转激光雷达的无人机的控制方法,其特征在于:旋转角度Δθ通过下式(五)获得:
其中,
umax表示最大的特征值λmax对应的特征向量;
逆时针方向为正方向,顺时针方向为负方向;
uy和uz分别表示y轴方向和z轴方向的特征向量。
7.根据权利要求4所述的装载有主动旋转激光雷达的无人机的控制方法,其特征在于:
所述位姿解算模块(4)实时调取激光雷达(2)实扫描获得的数据图像和IMU模块的测量值;
在所述位姿解算模块(4)中,通过下式(六)迭代获得无人机的位姿:
f(Tk+1)=de+dH=d (六)
其中,f(Tk+1)的雅克比矩阵为
式(六)的迭代过程为:
将Tk+1的初始值代入到(de,dH)中,得到f(Tk+1),其中/>是由IMU模块得到的第k帧图像位姿到第k+1帧图像位姿的变换;
将得到的f(Tk+1)代入到中,再次得到f(Tk+1)的雅克比矩阵J,
将再次得到的J代入到-(JTJ)-1JTf(Tk+1)中,再次得到姿态的增量ΔTk+1,
将得到的姿态增量ΔTk+1与原值Tk+1相加,得到新的姿态参数Tk+1;
重复上述迭代过程,直至:
且同时满足:
时迭代终止;
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通过下式(七)、(八)解算获得:
其中,表示第k+1帧图像中第i个特征点转换到世界坐标系后对应特征点集合中的第i个点的坐标,
表示第k帧图像中第j个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第l个特征点转换到世界坐标系下的坐标值,
表示第k帧图像中第m个特征点转换到世界坐标系下的坐标值;
其中,点j是第k帧图像特征点在世界坐标系中离点i最近的点,
点l是点j所在扫描线中距离j最近的点,
点m是第k帧图像中离j最近的点,
所述点j、l和m都是步骤S1中的数据集中的点;
点j、l和m构成平面,该平面即为dH中的平面。
8.根据权利要求7所述的装载有主动旋转激光雷达的无人机的控制方法,其特征在于,
通过下式(九)获得:
其中,XL (k+1,i)表示雷达坐标系下,第k+1帧图像的第i个特征点;
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)表示雷达坐标系转换到世界坐标系的转换参数;
在计算过程中T(k+1,i)的值通过IMU模块获得;
T(k+1,i)(a:b)代表T(k+1,i)的第a个数据到第b个数据之间的全部数据。
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 CN113156450A (zh) | 2021-07-23 |
CN113156450B true 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) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114199233B (zh) * | 2021-11-08 | 2024-04-05 | 北京旷视科技有限公司 | 位姿确定方法及可移动设备 |
Citations (4)
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 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 空域干扰信号检测方法与装置 |
-
2021
- 2021-03-17 CN CN202110287557.8A patent/CN113156450B/zh active Active
Patent Citations (4)
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)
Title |
---|
基于激光点云的边坡地形提取及可视化研究;吴毅;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》(第02期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113156450A (zh) | 2021-07-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US20200354079A1 (en) | Systems and methods for dynamic planning and operation of autonomous systems using image observation and information theory | |
EP3903164B1 (en) | Collision avoidance system, depth imaging system, vehicle, map generator, amd methods thereof | |
CN111932588B (zh) | 一种基于深度学习的机载无人机多目标跟踪***的跟踪方法 | |
US11218689B2 (en) | Methods and systems for selective sensor fusion | |
Roelofsen et al. | Reciprocal collision avoidance for quadrotors using on-board visual detection | |
JP6029446B2 (ja) | 自律飛行ロボット | |
CN102298070B (zh) | 估算无人机,尤其是能够在自动驾驶下执行悬停飞行的无人机的水平速度的方法 | |
WO2019152149A1 (en) | Actively complementing exposure settings for autonomous navigation | |
JP6195450B2 (ja) | 自律飛行ロボット | |
CN110221625B (zh) | 无人机精确位置的自主降落导引方法 | |
JP6140458B2 (ja) | 自律移動ロボット | |
CN112363528B (zh) | 基于机载视觉的无人机抗干扰集群编队控制方法 | |
CN112789672A (zh) | 控制和导航***、姿态优化、映射和定位技术 | |
CN112947569B (zh) | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 | |
JP6014484B2 (ja) | 自律移動ロボット | |
Mejias et al. | Omnidirectional bearing-only see-and-avoid for small aerial robots | |
Strydom et al. | Uav guidance: a stereo-based technique for interception of stationary or moving targets | |
CN113156450B (zh) | 无人机上的主动旋转激光雷达***及其控制方法 | |
CN114689030A (zh) | 一种基于机载视觉的无人机辅助定位方法及*** | |
Dufek et al. | Visual pose stabilization of tethered small unmanned aerial system to assist drowning victim recovery | |
Yang et al. | Autonomous landing of a rotor unmanned aerial vehicle on a boat using image-based visual servoing | |
CN114003041A (zh) | 一种多无人车协同探测*** | |
Xin et al. | An on-board pan-tilt controller for ground target tracking systems | |
Xiaodong et al. | Obstacle avoidance for outdoor flight of a quadrotor based on computer vision | |
Shen et al. | Vision-based Autonomous Tracking and Landing Method for UAV on a Dynamic Platform |
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 |