CN110824496B - 运动估计方法、装置、计算机设备和存储介质 - Google Patents
运动估计方法、装置、计算机设备和存储介质 Download PDFInfo
- Publication number
- CN110824496B CN110824496B CN201910880124.6A CN201910880124A CN110824496B CN 110824496 B CN110824496 B CN 110824496B CN 201910880124 A CN201910880124 A CN 201910880124A CN 110824496 B CN110824496 B CN 110824496B
- Authority
- CN
- China
- Prior art keywords
- laser
- moment
- estimated
- loss function
- laser point
- 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 36
- 238000012216 screening Methods 0.000 claims abstract description 16
- 230000006870 function Effects 0.000 claims description 123
- 238000004590 computer program Methods 0.000 claims description 25
- 230000001131 transforming effect Effects 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 7
- 238000010586 diagram Methods 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
- G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4808—Evaluating distance, position or velocity data
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请涉及一种运动估计方法、装置、计算机设备和存储介质。包括:确定获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在第一时刻的速度和位姿;对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点;根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数;根据损失函数,确定待估计参数。从而消除运动畸变对确定待估计参数的值的影响,使估计得到的待估计参数的值更接近于真实的运动参数。
Description
技术领域
本申请涉及雷达定位技术领域,特别涉及一种运动估计方法、装置、计算机设备和存储介质。
背景技术
随着无人驾驶和移动机器人技术的不断发展,利用单线激光雷达估计两时刻之间移动机器人的运动姿态得到了越来越广泛的应用。传统技术中,利用单线激光雷达估计两时刻之间的运动姿态往往不考虑无人车或者移动机器人的运动状态,认为激光数据是在一瞬间采集完成。
然而,激光传感器在采集每一帧数据时,一般都需要耗费一定的时间,当激光雷达处于高速运动时,采集到的测量数据往往包含大量的运动畸变,因此,根据包含大量运动畸变的测量数据估计得到的运动姿态与真实的运动姿态之间必然存在较大的差别。
发明内容
基于此,有必要针对上述技术问题,提供一种运动估计方法、装置、计算机设备和存储介质。
一种运动估计方法,所述方法包括:
确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在所述第一时刻的速度和位姿;
对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述速度和所述位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点;
根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数;
根据所述损失函数,确定待估计参数,其中,所述待估计参数包括所述目标物体在所述第一时刻与所述第二时刻之间的位姿和所述目标物体在所述第二时刻的速度。
在其中一个实施例中,所述对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述速度和所述位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点,包括:
根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下;
从所述第一激光帧中,筛选出与坐标变换后的第二激光点距离满足所述预设条件的两个第一激光点。
在其中一个实施例中,所述第二激光帧中相邻的第二激光点之间的采集时间间隔为预设时间差;
所述根据所述第一时刻、所述第二时刻、所述速度和所述位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下,包括:
根据所述第二时刻和所述预设时间差,确定出所述第二激光帧中每一第二激光点的位姿;
根据所述第一时刻、所述第二时刻、所述速度、所述位姿和所述每一第二激光点的位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下。
在其中一个实施例中,所述根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数,包括:
对于每一所述第二激光点,根据与所述第二激光点距离满足所述预设条件的两个第一激光点,确定出一条直线;
计算每一所述第二激光点到对应的直线的距离;
根据每一所述第二激光点到对应的直线的距离,构建所述损失函数。
在其中一个实施例中,所述根据所述损失函数,确定待估计参数,包括:
为所述待估计参数设定初始值;
根据所述初始值和所述损失函数,确定所述待估计参数。
在其中一个实施例中,所述根据所述初始值和所述损失函数,确定待估计参数,包括:
根据所述初始值对所述损失函数进行优化,得到损失函数值和与所述损失函数值对应的待估计参数的估计值;
将所述初始值替换为所述估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将所述当前迭代得到的估计值确定为所述待估计参数的估计值。
在其中一个实施例中,所述根据所述初始值和所述损失函数,确定待估计参数,包括:
根据所述初始值对所述损失函数进行优化,得到损失函数值和与所述损失函数值对应的待估计参数的估计值;
将所述初始值替换为所述估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为所述待估计参数的估计值。
一种运动估计装置,所述装置包括:
信息获取模块,用于确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在所述第一时刻的速度和位姿;
激光点确定模块,用于对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述速度和所述位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点;
损失函数构建模块,用于根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数;
运动估计模块,用于根据所述损失函数,确定待估计参数,其中,所述待估计参数包括所述目标物体在所述第一时刻与所述第二时刻之间的位姿和所述目标物体在所述第二时刻的速度。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现上述任一项所述方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述任一项所述方法的步骤。
根据本申请实施例的运动估计方法、装置、计算机设备和存储介质,通过从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点,并根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数,根据损失函数,确定待估计参数,可以充分考虑由运动畸变引起的第二激光点与第一激光点之间的距离差值,消除运动畸变对确定待估计参数的值的影响,使估计得到的待估计参数的值更接近于真实的运动参数。
附图说明
图1为一个实施例中运动估计系统的结构框图;
图2为一个实施例中运动估计方法的流程示意图;
图3为一个实施例中步骤S200的一种可实施方式的流程示意图;
图4为一个实施例中步骤S300的一种可实施方式的流程示意图;
图5为一个实施例中步骤S400的一种可实施方式的流程示意图;
图6为一个实施例中运动估计装置的结构框图;
图7为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
可以理解本申请中所使用的术语“第一”、“第二”等可在本文中用于描述各种条件关系,但这些条件关系不受这些术语限制。这些术语仅用于将一个条件关系与另一个条件关系区分开来。
图1是本申请实施例提供的一种运动估计系统的结构框图。本申请实施例提供的运动估计方法,可以应用于如图1所示的运动估计系统中。该运动估计系统包括:终端10、可移动设备20和激光雷达201。
终端10通过无线网络或有线网络与可移动设备20连接。在一实施例中,终端10还可以通过无线网络或有线网络与激光雷达201连接。终端10可以但不限于是各种个人计算机、笔记本电脑、智能手机和平板电脑。本领域技术人员可以知晓,上述终端的数量可以更多或更少。比如上述终端可以仅为一个,或者上述终端为几十个或几百个,或者更多数量,此时上述运动估计系统还包括其他终端。本申请实施例对终端的数量和设备类型不加以限定。终端10可以泛指多个终端中的一个,本实施例仅以终端10来举例说明。
在一个实施例中,激光雷达201设置于可移动设备20上,可将激光雷达201的位置近似地看作可移动设备20的位置。且当激光雷达201与可移动设备20的体积大小相差不大时,还可将激光雷达201的坐标近似地看作可移动设备20的坐标。示例地,可移动设备20例如可以是无人车、移动机器人等设备。
在一个实施例中,终端10通过网络接收激光雷达201获取第一激光帧的第一时刻和激光雷达201获取第二激光帧的第二时刻,以及可移动设备20(目标物体)在第一时刻的速度和位姿,并且终端10对获取到的上述信息进行处理,并构建损失函数,最终确定出可移动设备20在第一时刻与第二时刻之间的位姿和可移动设备20在第二时刻的速度。
在一个实施例中,如图2所示,为本申请一个实施例中运动估计方法的流程示意图,以该运动估计方法应用于图1中的终端为例进行说明,包括以下步骤:
步骤S100,确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在第一时刻的速度和位姿。
其中,第一时刻是指激光雷达201第一次获取激光帧时的时刻;第二时刻是指激光雷达201第二次获取激光帧时的时刻;第一时刻获取到的激光帧为第一激光帧,第二时刻获取到的激光帧为第二激光帧。
由于激光雷达201设置于可移动设备20上,且激光雷达201与可移动设备20的体积相差不大,可将激光雷达201的位姿和速度近似地看作可移动设备20的位姿和速度。目标物体为搭载激光雷达201并根据激光雷达201产生的激光数据确定自身运动状态的可移动设备,例如可以为图1所对应的实施例中的可移动设备20。第一时刻的速度和位姿是指激光雷达201在获取第一激光帧时激光雷达201的位姿和速度(可近似看作可移动设备20的位姿和速度,也即目标物体的位姿和速度)。其中,获取第二激光帧时的位姿和速度为未知的参数,无法直接获取。
步骤S200,对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、目标物体在第一时刻的速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点。
其中,一帧激光一般由多束激光点组成,因此,第一激光帧包括多个第一激光点,相同地,第二激光帧也包括多个第二激光点。预设条件为筛选与每一第二激光点相对应的两个第一激光点的条件,可以为距离第二激光点最近的两个第一激光点,或者其他需要的距离条件,本申请实施例对此不作限定。
具体地,根据第一时刻、第二时刻以及第一时刻对应的目标物体的速度和位姿,可以将第二激光帧中的每一第二激光点与第一激光帧中的第一激光点相对应,并求得第二激光帧中的每一第二激光点与第一激光帧中的第一激光点之间的距离,根据上述距离与预设条件,可以筛选出与每一第二激光点距离满足预设条件的两个第一激光点。
步骤S300,根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数。
具体地,每一第二激光点与两个第一激光点对应,计算得到某一第二激光点与对应的两个第一激光点确定的直线之间的距离,按照同样的方式可以计算得到第二激光帧中的每一第二激光点与由对应于该第二激光点的两个第一激光点确定的直线之间的距离。根据上述每一第二激光点与由对应于该第二激光点的两个第一激光点确定的直线之间的距离,可以构建该损失函数。
步骤S400,根据损失函数,确定待估计参数,其中,待估计参数包括目标物体在第一时刻与第二时刻之间的位姿和目标物体在第二时刻的速度。
具体地,每一损失函数值都对应一组待估计参数的值,根据损失函数与实际需要达到的精度要求,可以确定一个符合条件的损失函数值,进而根据符合条件的损失函数值可以确定出一组待估计参数的值。
根据本申请实施例的运动估计方法、装置、计算机设备和存储介质,通过从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点,并根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数,根据损失函数,确定待估计参数,可以充分考虑由运动畸变引起的第二激光点与第一激光点之间的距离差值,消除运动畸变对确定待估计参数的值的影响,使估计得到的待估计参数的值更接近于真实的运动参数。
在其中一个实施例中,如图3所示,为步骤S200的一种可实施方式的流程示意图,步骤S200,对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点,包括:
步骤S210,根据第一时刻、第二时刻、第一时刻的速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下。
可选地,根据第二时刻和预设时间差,确定出第二激光帧中每一第二激光点的位姿;根据第一时刻、第二时刻、速度、位姿和每一第二激光点的位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下;其中,第二激光帧中相邻的第二激光点之间的采集时间间隔为预设时间差。
其中,预设时间差可以设置为固定的时间间隔。激光雷达201采集到的每一激光帧中均包含多束激光点组成,相邻两激光点之间的发射时间间隔一般均相等,具体时间间隔的长度由具体采用的激光雷达装置而定。
具体地,假设一帧激光由N束激光点组成,相邻激光点之间采集间隔为Δt,假设第一激光帧Li的采集时刻为i时刻,第二激光帧Lj的采集时刻为j时刻,待估计参数为第一激光帧Li的采集时刻(第一时刻)和第二激光帧Lj的采集时刻(第二时刻)之间的位姿Tij=(tx,ty,θ)和第二激光帧Lj的采集时刻(第二时刻)的速度v=(vx,vy,vθ)。
在其中一个实施例中,激光雷达201采集一帧数据需要的时间在几十毫秒以内,示例地,假设第二激光帧Lj的采集时刻(第二时刻)在采集数据时为匀速运动,则根据第二激光帧Lj的采集时刻(第二时刻)的速度和相邻激光点之间采集时间间隔(预设时间差)Δt,可以得到采集第k个激光点的时刻t(t=k*Δt)时激光雷达装置201的相对运动量T(t)。示例地,可以采用平面运动李代数模型计算该相对运动量,可选地,还可以采用其他的运动模型对激光雷达201相对运动量进行计算,此处不进行具体限定。以根据平面运动李代数模型进行运算为例,具体计算方式如公式(1)所示:
其中,t为当前激光点对应的采集时刻(由第二时刻和预设时间差决定),v=(vx,vy,vθ)为第二激光帧Lj的采集时刻(第二时刻)的速度(其中,vx为激光雷达201在第二激光帧对应的坐标系下的X方向的分速度,vy为激光雷达201在第二激光帧对应的坐标系下的Y方向的分速度,vθ为激光雷达201第二激光帧对应的坐标系下的角速度),x=υθt,
根据第一时刻、第二时刻、速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下的具体方式如公式(2)所示:
其中,表示第二激光帧Lj中的第k个激光点的坐标,表示将第二激光帧Lj中第k个激光点变换到第一激光帧Li下的坐标,v=(vx,vy,vθ)为第二激光帧Lj的采集时刻(第二时刻)的速度(其中,vx为激光雷达201在第二激光帧对应的坐标系下的X方向的分速度,vy为激光雷达201在第二激光帧对应的坐标系下的Y方向的分速度,vθ为激光雷达201第二激光帧对应的坐标系下的角速度),tx为第二激光帧对应的坐标系下的X方向的相对运动量,ty为第二激光帧对应的坐标系下的Y方向的相对运动量,公式(2)中的函数式R与公式(1)中的函数式R对应,公式(2)中的函数式V与公式(1)中的函数式V对应。
步骤S220,从第一激光帧中,筛选出与坐标变换后的第二激光点距离满足预设条件的两个第一激光点。
具体地,当根据步骤S210中的方式将每一第二激光点的坐标变换到第一激光帧对应的坐标系下后,可以分别计算第一激光帧中的每一第一激光点分别与每一第二激光点之间的距离。示例地,该第一激光点与第二激光点之间的距离为欧式距离。在一实施例中,根据第一激光点与第二激光点之间的欧式距离,从第一激光帧中筛选出与坐标变换后的第二激光点之间的欧式距离满足预设条件的两个第一激光点。可选地,从第一激光帧中筛选出与坐标变换后的第二激光点之间的欧式距离最小的两个第一激光点。
上述实施例中,根据第一时刻、第二时刻、第一时刻的速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下,并从第一激光帧中,筛选出与坐标变换后的第二激光点距离满足预设条件的两个第一激光点。可以为后续计算消除运动畸变对确定待估计参数的值的影响,使估计得到的待估计参数的值更接近于真实的运动参数。
在其中一个实施例中,如图4所示,为步骤S300的一种可实施方式的流程示意图,步骤S300,根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数,包括:
步骤S310,对于每一第二激光点,根据与该第二激光点距离满足预设条件的两个第一激光点,确定出一条直线。
具体地,根据两点确定一条直线的原则,每一第二激光点对应的两个第一激光点可以确定出一条直线,这一直线与第二激光点相对应。
步骤S320,计算每一第二激光点到对应的直线的距离。
具体地,根据点到直线之间的距离计算公式,可以得到每一第二激光点到与其对应的直线的之间的距离,例如,在第一激光帧Li中找到的两个与第二激光点距离最近的第一激光点分别为和计算点到和确定出的直线的距离为rk,具体rk的计算方式如公式(3)所示:
步骤S330,根据每一第二激光点到对应的直线的距离,构建损失函数。
示例地,可以根据每一第二激光点到对应的直线的距离,利用如下公式(4),构建损失函数F:
其中,rk表示由公式(3)得到的第二激光点到对应的直线的距离,i表示第二激光点到对应的直线的距离的个数,N表示第二激光点到对应的直线的距离的总个数。
上述实施例中,通过对于每一第二激光点,根据与第二激光点距离满足预设条件的两个第一激光点,确定出一条直线,计算每一第二激光点到对应的直线的距离,并根据每一第二激光点到对应的直线的距离,构建损失函数。其中,根据第二激光点和两个第一激光点之间的距离关系构建损失函数,可以充分考虑由运动畸变造成的第二激光点与第一激光点之间的距离差值,消除运动畸变对参数估计的影响,使估计得到的运动姿态更接近于真实的运动姿态。
在其中一个实施例中,如图5所示,为步骤S400的一种可实施方式的流程示意图,步骤S400,根据损失函数,确定待估计参数,包括:
步骤S410,为待估计参数设定初始值。
具体地,为待估计参数设定初始值并没有具体的限定,可以由计算机随机为待估计参数设定初始值。可选地,为了进一步保证最终的估计参数的准确性,可以根据步骤S100中的第一时刻、第二时刻以及确定目标物体在第一时刻的速度和位姿,由用户根据经验在待估计参数可能的数值范围内为待估计参数设定初始值。
步骤S420,根据初始值和损失函数值,确定待估计参数。
可选地,根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为待估计参数的估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将当前迭代得到的待估计参数的估计值确定为待估计参数的值。
示例地,可以采用最优化算法对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值。其中,最优化算法是一种寻找使函数值最小的参数向量的算法,最优化算法的方式有很多,此处不对具体算法进行限定。
在一实施例中,可以采用最优化算法中的Levenberg-Marquardt算法最小化损失函数,并在每轮迭代运算过程中均对损失函数进行优化,确定出每轮迭代得到的损失函数值,进而确定与每轮迭代得到的损失函数值对应的待估计参数的估计值。若当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将当前迭代得到的待估计参数的估计值确定为待估计参数的值。
具体地,将初始值替换为待估计参数的估计值,对初始值进行更新,并返回执行步骤S420进行迭代运算,每轮迭代运算均会得到一组对应的损失函数值和待估计参数的估计值,判断当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值与预设阈值之间的关系。示例地,若当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则说明在当前迭代得到的待估计参数的估计值处趋于收敛,当前迭代得到的待估计参数的估计值已接近运动姿态的真实值,此时,则将当前迭代得到的待估计参数的估计值确定为待估计参数的值。
可选地,根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为待估计参数的估计值。
具体地,将初始值替换为估计值,对初始值进行更新,接着,返回执行步骤S420进行迭代运算,每轮迭代运算均会得到一组对应的损失函数值和估计值,并将执行预设次数迭代得到的估计值确定为待估计参数的估计值。
上述实施例中,通过为待估计参数设定初始值,并根据初始值和损失函数,确定待估计参数。其中,根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数,根据损失函数,确定待估计参数,可以充分考虑由运动畸变引起的第二激光点与第一激光点之间的距离差值,消除运动畸变对确定待估计参数的值的影响,使估计得到的待估计参数的值更接近于真实的运动参数。
在一个实施例中,如图6所示,提供了一种运动估计装置,包括:信息获取模块601、激光点确定模块602、损失函数构建模块603和运动估计模块604,其中:
信息获取模块601,用于确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在第一时刻的速度和位姿;
激光点确定模块602,用于对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点;
损失函数构建模块603,用于根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数;
运动估计模块604,用于根据损失函数,确定待估计参数,其中,待估计参数包括目标物体在第一时刻与第二时刻之间的位姿和目标物体在第二时刻的速度。
在其中一个实施例中,激光点确定模块602还用于根据第一时刻、第二时刻、第一时刻的速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下;从第一激光帧中,筛选出与坐标变换后的第二激光点距离满足预设条件的两个第一激光点。
在其中一个实施例中,激光点确定模块602还用于根据第二时刻和预设时间差,确定出第二激光帧中每一第二激光点的位姿;根据第一时刻、第二时刻、速度、位姿和每一第二激光点的位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下。
在其中一个实施例中,损失函数构建模块603还用于对于每一第二激光点,根据与第二激光点距离满足预设条件的两个第一激光点,确定出一条直线;计算每一第二激光点到对应的直线的距离;根据每一第二激光点到对应的直线的距离,构建损失函数。
在其中一个实施例中,运动估计模块604还用于为待估计参数设定初始值;根据初始值和损失函数,确定待估计参数。
在其中一个实施例中,运动估计模块604还用于根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将当前迭代得到的估计值确定为待估计参数的估计值。
在其中一个实施例中,运动估计模块604还用于根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为待估计参数的估计值。
关于运动估计装置的具体限定可以参见上文中对于运动估计方法的限定,在此不再赘述。上述运动估计装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是终端,其内部结构图可以如图7所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种运动估计方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图7中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现以下步骤:
确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在第一时刻的速度和位姿;
对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点;
根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数;
根据损失函数,确定待估计参数,其中,待估计参数包括目标物体在第一时刻与第二时刻之间的位姿和目标物体在第二时刻的速度。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:根据第一时刻、第二时刻、第一时刻的速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下;从第一激光帧中,筛选出与坐标变换后的第二激光点距离满足预设条件的两个第一激光点。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:根据第二时刻和预设时间差,确定出第二激光帧中每一第二激光点的位姿;根据第一时刻、第二时刻、速度、位姿和每一第二激光点的位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:对于每一第二激光点,根据与第二激光点距离满足预设条件的两个第一激光点,确定出一条直线;计算每一第二激光点到对应的直线的距离;根据每一第二激光点到对应的直线的距离,构建损失函数。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:为待估计参数设定初始值;根据初始值和损失函数,确定待估计参数。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将当前迭代得到的估计值确定为待估计参数的估计值。
在其中一个实施例中,处理器执行计算机程序时还实现以下步骤:根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为待估计参数的估计值。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现以下步骤:
确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在第一时刻的速度和位姿;
对于第二激光帧中的每一第二激光点,根据第一时刻、第二时刻、速度和位姿,从第一激光帧中筛选出与每一第二激光点距离满足预设条件的两个第一激光点;
根据每一第二激光点和与每一第二激光点距离满足预设条件的两个第一激光点之间的距离关系,构建损失函数;
根据损失函数,确定待估计参数,其中,待估计参数包括目标物体在第一时刻与第二时刻之间的位姿和目标物体在第二时刻的速度。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据第一时刻、第二时刻、第一时刻的速度和位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下;从第一激光帧中,筛选出与坐标变换后的第二激光点距离满足预设条件的两个第一激光点。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据第二时刻和预设时间差,确定出第二激光帧中每一第二激光点的位姿;根据第一时刻、第二时刻、速度、位姿和每一第二激光点的位姿,将每一第二激光点的坐标变换到第一激光帧对应的坐标系下。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:对于每一第二激光点,根据与第二激光点距离满足预设条件的两个第一激光点,确定出一条直线;计算每一第二激光点到对应的直线的距离;根据每一第二激光点到对应的直线的距离,构建损失函数。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:为待估计参数设定初始值;根据初始值和损失函数,确定待估计参数。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将当前迭代得到的估计值确定为待估计参数的估计值。
在其中一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据初始值对损失函数进行优化,得到损失函数值和与损失函数值对应的待估计参数的估计值;将初始值替换为估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为待估计参数的估计值。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种运动估计方法,其特征在于,所述方法包括:
确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在所述第一时刻的速度和位姿;
对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述第一时刻的位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点;
根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数;
根据所述损失函数,确定待估计参数,其中,所述待估计参数包括所述目标物体在所述第一时刻与所述第二时刻之间的位姿和所述目标物体在所述第二时刻的速度。
2.根据权利要求1所述的方法,其特征在于,所述对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述第一时刻的位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点,包括:
根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述第一时刻的位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下;
从所述第一激光帧中,筛选出与坐标变换后的第二激光点距离满足所述预设条件的两个第一激光点。
3.根据权利要求2所述的方法,其特征在于,所述第二激光帧中相邻的第二激光点之间的采集时间间隔为预设时间差;
所述根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述第一时刻的位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下,包括:
根据所述第二时刻和所述预设时间差,确定出所述第二时刻所述第二激光帧中每一第二激光点的位姿;
根据所述第一时刻、所述第二时刻、所述第一时刻的速度、所述第一时刻的位姿和所述每一第二激光点的位姿,将每一所述第二激光点的坐标变换到所述第一激光帧对应的坐标系下。
4.根据权利要求1所述的方法,其特征在于,所述根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数,包括:
对于每一所述第二激光点,根据与所述第二激光点距离满足所述预设条件的两个第一激光点,确定出一条直线;
计算每一所述第二激光点到对应的直线的距离;
根据每一所述第二激光点到对应的直线的距离,构建所述损失函数。
5.根据权利要求1所述的方法,其特征在于,所述根据所述损失函数,确定待估计参数,包括:
为所述待估计参数设定初始值;
根据所述初始值和所述损失函数,确定所述待估计参数。
6.根据权利要求5所述的方法,其特征在于,所述根据所述初始值和所述损失函数,确定待估计参数,包括:
根据所述初始值对所述损失函数进行优化,得到损失函数值和与所述损失函数值对应的待估计参数的估计值;
将所述初始值替换为所述估计值,进行迭代运算,直到当前迭代得到的损失函数值与上一轮迭代得到的损失函数值之间的差值小于或等于预设阈值,则将所述当前迭代得到的估计值确定为所述待估计参数的估计值。
7.根据权利要求5所述的方法,其特征在于,所述根据所述初始值和所述损失函数,确定待估计参数,包括:
根据所述初始值对所述损失函数进行优化,得到损失函数值和与所述损失函数值对应的待估计参数的估计值;
将所述初始值替换为所述估计值,进行迭代运算,并将执行预设次数迭代得到的估计值确定为所述待估计参数的估计值。
8.一种运动估计装置,其特征在于,所述装置包括:
信息获取模块,用于确定激光雷达获取第一激光帧的第一时刻和获取第二激光帧的第二时刻,以及确定目标物体在所述第一时刻的速度和位姿;
激光点确定模块,用于对于所述第二激光帧中的每一第二激光点,根据所述第一时刻、所述第二时刻、所述第一时刻的速度和所述第一时刻的位姿,从所述第一激光帧中筛选出与每一所述第二激光点距离满足预设条件的两个第一激光点;
损失函数构建模块,用于根据每一所述第二激光点和与每一所述第二激光点距离满足所述预设条件的两个第一激光点之间的距离关系,构建损失函数;
运动估计模块,用于根据所述损失函数,确定待估计参数,其中,所述待估计参数包括所述目标物体在所述第一时刻与所述第二时刻之间的位姿和所述目标物体在所述第二时刻的速度。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910880124.6A CN110824496B (zh) | 2019-09-18 | 2019-09-18 | 运动估计方法、装置、计算机设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910880124.6A CN110824496B (zh) | 2019-09-18 | 2019-09-18 | 运动估计方法、装置、计算机设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110824496A CN110824496A (zh) | 2020-02-21 |
CN110824496B true CN110824496B (zh) | 2022-01-14 |
Family
ID=69548043
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910880124.6A Active CN110824496B (zh) | 2019-09-18 | 2019-09-18 | 运动估计方法、装置、计算机设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110824496B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112014825A (zh) * | 2020-07-24 | 2020-12-01 | 深圳市优必选科技股份有限公司 | 一种激光数据采样方法、装置、终端及存储介质 |
CN113253293B (zh) * | 2021-06-03 | 2021-09-21 | 中国人民解放军国防科技大学 | 一种激光点云畸变的消除方法和计算机可读存储介质 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6424522B2 (ja) * | 2014-09-04 | 2018-11-21 | 株式会社Soken | 車載装置、車載測距システム |
CN107144292B (zh) * | 2017-06-08 | 2019-10-25 | 杭州南江机器人股份有限公司 | 一种运动设备的里程计方法以及里程计装置 |
CN109188457B (zh) * | 2018-09-07 | 2021-06-11 | 百度在线网络技术(北京)有限公司 | 物体检测框的生成方法、装置、设备、存储介质及车辆 |
CN109493407B (zh) * | 2018-11-19 | 2022-03-25 | 腾讯科技(深圳)有限公司 | 实现激光点云稠密化的方法、装置及计算机设备 |
CN109584183B (zh) * | 2018-12-05 | 2020-05-29 | 吉林大学 | 一种激光雷达点云去畸变方法及系统 |
-
2019
- 2019-09-18 CN CN201910880124.6A patent/CN110824496B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110824496A (zh) | 2020-02-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108805898B (zh) | 视频图像处理方法和装置 | |
US10852139B2 (en) | Positioning method, positioning device, and robot | |
CN108830900B (zh) | 关键点的抖动处理方法和装置 | |
CN111708355A (zh) | 基于强化学习的多无人机动作决策方法和装置 | |
CN110221600B (zh) | 路径规划方法、装置、计算机设备和存储介质 | |
CN110824496B (zh) | 运动估计方法、装置、计算机设备和存储介质 | |
US20210398346A1 (en) | Systems and Methods for Fine Adjustment of Roof Models | |
CN111707294B (zh) | 基于最优区间估计的行人导航零速区间检测方法和装置 | |
CN110263754B (zh) | 屏下指纹去底纹方法、装置、计算机设备和存储介质 | |
CN113759724B (zh) | 基于数据驱动的机器人控制方法、装置和计算机设备 | |
CN114348019A (zh) | 车辆轨迹预测方法、装置、计算机设备和存储介质 | |
CN112629565B (zh) | 像机与惯性测量单元旋转关系校准方法、装置和设备 | |
CN113409583B (zh) | 一种车道线信息确定方法及装置 | |
CN116758146A (zh) | 基于图像的无人机集群目标定位方法、装置、设备及介质 | |
CN111639523A (zh) | 目标检测方法、装置、计算机设备和存储介质 | |
CN110196630B (zh) | 指令处理、模型训练方法、装置、计算机设备及存储介质 | |
CN115509386A (zh) | 一种触摸轨迹的预测方法及其装置 | |
CN113805587B (zh) | 多无人车分布式编队控制方法、装置和设备 | |
CN114852096A (zh) | 域控制器变量的管理方法、装置、电子设备及存储介质 | |
CN114564801A (zh) | 液压系统模型辨识方法及装置 | |
CN116432690B (zh) | 基于马尔科夫的智能决策方法、装置、设备及存储介质 | |
CN111310906A (zh) | 神经形态芯片中计算核的布局的方法、装置及设备 | |
CN111582254A (zh) | 视频预测方法、装置、计算机设备和可读存储介质 | |
CN115792794A (zh) | 一种无线电测向的卡尔曼跟踪方法、装置、设备和介质 | |
CN117475399B (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 | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20211229 Address after: 316-318, block a, Rongke Information Center, No.2, South Road, Academy of Sciences, Haidian District, Beijing, 100190 Applicant after: MEGVII (BEIJING) TECHNOLOGY Co.,Ltd. Applicant after: Kuangshitu (Suzhou) Intelligent Equipment Co., Ltd Address before: 316-318, block a, Rongke Information Center, No.2, South Road, Academy of Sciences, Haidian District, Beijing, 100190 Applicant before: MEGVII (BEIJING) TECHNOLOGY Co.,Ltd. |
|
TA01 | Transfer of patent application right | ||
GR01 | Patent grant | ||
GR01 | Patent grant |