CN114170297A - 车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 - Google Patents
车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 Download PDFInfo
- Publication number
- CN114170297A CN114170297A CN202111424961.1A CN202111424961A CN114170297A CN 114170297 A CN114170297 A CN 114170297A CN 202111424961 A CN202111424961 A CN 202111424961A CN 114170297 A CN114170297 A CN 114170297A
- Authority
- CN
- China
- Prior art keywords
- real
- time
- initial
- pose
- residual error
- 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.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
-
- 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
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本公开提供了一种车辆位姿的处理方法、装置、电子设备和自动驾驶车辆,涉及人工智能领域,尤其涉及自动驾驶领域。具体实现方案为:构建车辆在不同时刻的初始位姿信息,其中,初始位姿信息包括:初始位置和初始姿态,初始位置采用经度、纬度和高程表示;采集车辆在不同时刻时的实时位姿,其中,实时位姿基于车辆上多个传感器感测得到;基于不同时刻时采集到的实时位姿对初始位姿信息进行优化,得到目标位姿信息。构建初始位姿信息,利用车辆上的多个传感器感测得到实时位姿对初始位姿信息进行优化的过程中,使用的坐标为经纬度,无需考虑坐标系的切换,有效地保证了全局一致性,解决了现有技术在车辆位姿优化中需要更换坐标原点、不断重置的问题。
Description
技术领域
本公开涉及人工智能技术领域,尤其涉及自动驾驶技术领域,具体涉 及一种车辆位姿的处理方法、装置、电子设备、存储介质以及自动驾驶车 辆。
背景技术
在无人汽车自动驾驶运行过程中,需要定位系统实时输出连续高频率 且精确的定位结果,以确保路径规划、感知等模块的正常工作。
传统的无人驾驶中定位模块在采用图优化进行多个传感器定位信息 融合过程中使用通用横墨卡托网系统(Universal Transverse Mercartor Grid System,UTM)等非全球一致坐标系。在投影带的边沿坐标系发生变化, 导致需要不断的重置优化的图,程序设计复杂且容易出错。因此,在图优 化中存在需要更换坐标原点、不断重置的问题。
针对上述的问题,目前尚未提出有效的解决方案。
发明内容
本公开提供了一种车辆位姿的处理方法、装置、电子设备、存储介质 以及自动驾驶车辆。
根据本公开的第一方面,提供了一种车辆位姿的处理方法,包括:构 建车辆在不同时刻的初始位姿信息,其中,初始位姿信息包括:初始位置 和初始姿态,初始位置采用经度、纬度和高程表示;采集车辆在不同时刻 时的实时位姿,其中,实时位姿基于车辆上的多个传感器感测得到;基于 不同时刻时采集到的实时位姿对初始位姿信息进行优化,得到目标位姿信 息。
根据本公开的第二方面,提供了一种车辆位姿的处理装置,包括:构 建模块,用于构建车辆在不同时刻的初始位姿信息,其中,初始位姿信息 包括:初始位置和初始姿态,初始位置采用经度、纬度和高程表示;第一 采集模块,用于采集车辆在不同时刻时的实时位姿,其中,实时位姿基于 车辆上的多个传感器感测得到;优化模块,用于基于不同时刻时采集到的 实时位姿对初始位姿信息进行优化,得到目标位姿信息。
根据本公开的第三方面,提供了一种电子设备,包括:至少一个处理 器;以及与至少一个处理器通信连接的存储器;其中,存储器存储有可被 至少一个处理器执行的指令,指令被至少一个处理器执行,以使至少一个 处理器能够执行上述实施例中任一项的车辆位姿的处理方法。
根据本公开的第四方面,提供了一种存储有计算机指令的非瞬间计算 机可读存储介质,其中,计算机指令用于使计算机执行根据上述实施例中 任一项的车辆位姿的处理方法。
根据本公开的第五方面,提供了一种计算机程序产品,包括计算机程 序,该计算机程序在被处理器执行时实现根据上述实施例中任一项的车辆 位姿的处理方法。
根据本公开的第六方面,提供了一种自动驾驶车辆,包括上述第三方 面中的电子设备。
在本公开上述的实施例中,构建车辆在不同时刻的初始位姿信息,并 且采集车辆在不同时刻时的实时位姿,基于不同时刻时采集到的实时位姿 对初始位姿信息进行优化,得到目标位姿信息,实现车辆位姿优化的目的。 容易注意到的是,构建初始位姿信息,利用车辆上的多个传感器感测得到 实时位姿对初始位姿信息进行优化的过程中,使用的坐标为经纬度,无需 考虑坐标系的切换,有效地保证了全局一致性,解决了现有技术在车辆位姿优化中需要更换坐标原点、不断重置的问题。
应当理解,本部分所描述的内容并非旨在标识本公开的实施例的关键 或重要特征,也不用于限制本公开的范围。本公开的其它特征将通过以下 的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案,不构成对本公开的限定。其中:
图1是现有技术中的UTM 6度带经度的示意图;
图2是现有技术中的UTM投影坐标示意图;
图3是现有技术中的UTM分带示意图;
图4是根据本公开实施例的一种车辆位姿的处理方法的流程图;
图5是现有技术的一种地球椭球参数的示意图;
图6是根据本公开实施例的一种可选的位姿优化框架的示意图;
图7是根据本公开实施例的一种车辆位姿的处理装置的结构图;
图8是用来实施本公开的实施例的示例电子设备的示意性框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明,其中包括本公开实 施例的各种细节以助于理解,应当将它们认为仅仅是示范性的。因此,本 领域普通技术人员应当认识到,可以对这里描述的实施例做出各种改变和 修改,而不会背离本公开的范围和精神。同样,为了清楚和简明,以下的 描述中省略了对公知功能和结构的描述。
现有技术中,位置用UTM下的坐标表示。UTM投影全称为“通用横 墨卡托投影”,是一种“等角横轴割圆柱投影”,椭圆柱割地球于南纬80 度、北纬84度两条等高圈,投影后两条相割的经线上没有变形,而中央 经线上长度比0.9996。按经差6度将地球椭球面划分成若干投影带,如图 1所示,六度带自0度子午线起每隔经差6度自西向东分带,带号依次编为第1、2…60带。按分带方法各自进行投影,故各带坐标成独立系统, 每带以中央经线(L0)投影为纵轴X,赤道投影为横轴Y,两轴交点即为 各带的坐标原点,如图2所示。每个点的UTM坐标可以直接用X、Y和 高程H表示,单位为米,在位姿图(pose graph)中被优化。
由于现有的位姿图优化方案中位置变量为UTM下的坐标(X,Y,H), 由图3中UTM分带所示,当车辆在带的分界线行使时,zone id(带号) 会切换,导致车辆位置的UTM坐标变化剧烈,导致位置不连续,使得pose graph中需要更换坐标原点,重新构造,还需要计算局部坐标系到全局坐 标系的转换参数。
为了解决现有技术在车辆位姿优化中需要更换坐标原点、不断重置的 问题,本公开提出了一种车辆位姿的处理方法,该方法采用构建初始位姿 信息(经度、纬度和高程),利用车辆上的多个传感器感测得到实时位姿 对初始位姿信息进行优化的过程中,使用的坐标为经纬度,无需考虑坐标 系的切换,有效地保证了全局一致性。
图4是根据本公开实施例的一种车辆位姿的处理方法的流程图,如图 4所示,该方法包括:
步骤S401,构建车辆在不同时刻的初始位姿信息,其中,初始位姿 信息包括:初始位置和初始姿态,初始位置采用经度、纬度和高程表示。
上述步骤中的初始位置和初始姿态信息可以从行驶中的车辆预先规 划好的路线中直接获得,初始位置可以用经度、纬度和高程表示即可以表 示为(l,b,h),姿态可以用四次元数q表示。
上述步骤中的车辆可以是自动驾驶车辆,也可以是普通车辆,其中, 本公开在普通车辆中起到辅助驾驶的功能。
S402,采集车辆在不同时刻时的实时位姿,其中,实时位姿基于车辆 上的多个传感器感测得到。
上述步骤中的实时位姿包括:实时位置和实时姿态,实时位置也可以 用经度、纬度和高程表示,实时姿态也可以用四次元数q表示。
上述步骤中的多个传感器包括全球导航系统(Global Navigation SatelliteSystem,简称GNSS)、激光雷达系统(Light Deteation and Ranging System,简称LiDAR)、智能摄像机(Intelligent Camera/Visual)以及惯 性测量单元(Inertial MeasurementUnit,简称IMU)测量得到,但不限于 此。其中,激光雷达系统是一种集激光,全球定位系统(Global Positioning System,简称GPS)和惯性导航系统(Inertial Navigation System,简称INS) 三种技术于一身的系统,用于获得点云数据并生成精确的数字化三维模型; 惯性测量单元(IMU),用于测量物体三轴姿态角(或角速率)以及加速 的的装置,一般安装在被测物体的重心上。
步骤S403,基于不同时刻时采集到的实时位姿对初始位姿信息进行 优化,得到目标位姿信息。
上述步骤中可以根据获取到不同时刻时采集到的实时位置和姿态,利 用其和初始位置及姿态信息构建优化方程,并且求解方程来优化位姿信息, 以便于得到更准确的位置和姿态信息。
在本公开上述的实施例中,构建车辆在不同时刻的初始位姿信息,并 且采集车辆在不同时刻时的实时位姿,基于不同时刻时采集到的实时位姿 对初始位姿信息进行优化,得到目标位姿信息,实现车辆位姿优化的目的。 容易注意到的是,构建初始位姿信息,利用车辆上的多个传感器感测得到 实时位姿对初始位姿信息进行优化的过程中,使用的坐标为经纬度,无需 考虑坐标系的切换,有效地保证了全局一致性,解决了现有技术在车辆位姿优化中需要更换坐标原点、不断重置的问题。
可选地,基于不同时刻时采集到的实时位姿对初始位姿信息进行优化, 得到目标位姿信息包括:基于不同时刻时采集到的实时位姿和初始位姿信 息构建位姿残差;对位姿残差进行最小二乘处理,得到目标位姿信息。
上述步骤中根据不同时刻采集到的实时位置和姿态信息构建位置和 姿态残差,对构建好的位置和姿态残差进行最小二乘法处理,得到更准确 的位置和姿态信息。进行上述处理后提高了位姿的准确性,无需考虑坐标 系的切换,有效地保证了全局一致性。
可选地,基于不同时刻时采集到的实时位姿和初始位姿信息构建位姿 残差包括:基于初始位置、实时位姿中的实时经度、目标半径和目标偏心 率,构建经度残差;基于初始位姿信息中的初始纬度和初始高程、实时位 姿中的实时纬度、目标半径和目标偏心率,构建纬度残差;基于初始位姿 信息中的初始高程和实时位姿中的实时高程,构建高程残差;基于初始位 姿信息中的初始姿态和实时位姿中的实时姿态,构建姿态残差;基于经度 残差、纬度残差、高程残差和姿态残差,得到位姿残差。
上述步骤中的目标半径和目标偏心率可以是地球椭球的基本参数,其 中,目标半径可以是地球椭球的长半轴,目标偏心率可以是地球椭球的第 一偏心率,如图5所示,其中a表示地球椭圆的长半轴,b’表示地球椭 圆的短半轴,e表示地球椭圆的第一偏心率,
上述步骤中分别构建经度残差、纬度残差、高程残差和姿态残差,组 合得到残差方程,如下所示:
rh=h-hsensor,
上式中,r表示残差项,其中,rlon表示经度残差,rlat表示纬度残差, rh表示高程残差,rq表示姿态残差。其中,(lsensor,bsensor,hsensor)表示 通过传感器sensor(GNSS、LIDAR、Visual)得到的实时经纬度和实时高 程表示的实时位置,qsensor是通过传感器sensor(GNSS、LIDAR、Visual) 得到的实时姿态,用四元数表示。其中,(l,b,h)是初始经纬度和高程表 示的初始位置,q是四元数表示的初始姿态。
上述步骤中构造残差方程,之后使用残差方程优化初始位姿,进行上 述处理后提高了位姿的准确性,无需考虑坐标系的切换,有效地保证了全 局一致性。
可选地,在基于实时位姿对初始位姿信息进行优化,得到目标位姿信 息之前,该方法还包括:采集车辆在不同时刻时的实时增量位姿,其中, 实时增量位姿基于车辆上的至少一个传感器感测得到;基于不同时刻时采 集到的实时位姿和实时增量位姿对初始位姿信息进行优化,得到目标位姿 信息。
在一种可选的实施例中,如图6所示,其中,白色大圆表示优化变量 位姿,白色小圆表示通过全球导航系统(GNSS)得到的位姿、白色三角 形表示通过激光雷达系统(LiDAR)得到的位姿、白色矩形表示通过智能 摄像机(Camera/Visual)得到的位姿,黑色小圆表示根据车辆上的惯性测 量单元(IMU)和轮速计得到增量位姿。整个位姿优化(pose graph)的优 化变量为位姿(pose),利用的已知信息为全球导航系统(GNSS)、激 光雷达系统(LiDAR)和智能摄像机(Camera/Visual)得到的位姿,惯性 测量单元(IMU)和轮速计得到增量位姿,融合多元传感器得到更准确的 位姿,有效地保证了全局一致性。
可选地,基于不同时刻时采集到的实时位姿和实时增量位姿对初始位 姿信息进行优化,得到目标位姿信息包括:基于不同时刻时采集到的实时 位姿和初始位姿信息构建位姿残差;基于相邻两个时刻的初始位姿信息和 相邻两个时刻对应的实时增量位姿,构建增量位姿残差;对位姿残差和增 量位姿残差进行最小二乘处理,得到目标位姿信息。
上述步骤中的根据不同时刻时采集到的实时位姿和初始位姿信息构 建位姿残差方程,根据相邻两个时刻的初始位姿信息和相邻两个时刻对应 的实时增量位姿,构建增量位姿残差方程;对位姿残差和增量位姿残差进 行最小二乘法处理,得到目标位姿信息。通过上述步骤可以得到更准确的 位姿,无需考虑坐标系的切换,有效地保证了全局一致性。
可选地,基于相邻两个时刻的初始位姿信息和相邻两个时刻对应的实 时增量位姿,构建增量位姿残差包括:基于相邻两个时刻的初始位姿信息 中的初始位置、相邻两个时刻对应的实时增量位姿中的实时增量经度、目 标半径和目标偏心率,构建增量经度残差;基于相邻两个时刻的初始位姿 信息中的初始纬度和初始高程、相邻两个时刻对应的实时增量位姿中的实 时增量纬度、目标半径和目标偏心率,构建增量纬度残差;基于相邻两个 时刻的初始位姿信息中的初始高程和相邻两个时刻对应的实时增量位姿 中的实时增量高程,构建增量高程残差;基于相邻两个时刻的初始位姿信 息中的初始姿态和相邻两个时刻对应的实时增量位姿中的实时增量姿态, 构建增量姿态残差;基于增量经度残差、增量纬度残差、增量高程残差和 增量姿态残差,得到增量位姿残差。
上述步骤中的目标半径和目标偏心率可以是地球椭球的基本参数,其 中,目标半径可以是地球椭球的长半轴,目标偏心率可以是地球椭球的第 一偏心率,如图5所示,其中a表示地球椭圆的长半轴,b’表示地球椭 圆的短半轴,e表示地球椭圆的第一偏心率,
上述步骤中分别构建增量经度残差、增量纬度残差、增量高程残差和 增量姿态残差,组合得到增量残差方程,如下所示:
rΔh=htj-hti-Δhsensor,
上式中,b表示当前时刻的初始纬度,h表示当前时刻的初始高程,rΔ表 示两个时刻的增量残差,指ti时刻和tj时刻的增量。其中,rΔlon表示增量 经度残差,rΔlat表示增量纬度残差,rΔh表示增量高程残差,rΔq表示增量姿 态残差。其中,(Δlsensor,Δbsensor,Δhsensor)表示通过传感器sensor(GNSS、 LIDAR、Visual)得到的ti时刻和tj时刻的实时增量经纬度和实时增量高 程表示的实时增量位置,qsensor是通过传感器sensor(GNSS、LIDAR、 Visual)得到的ti时刻和tj时刻的实时增量姿态,用四元数表示。其中, (lth,btj,htj)是tj时刻的初始经纬度和高程表示的tj时刻的初始位置,qtj是四元数表示的tj时刻的初始姿态。其中,(lti,bti,hti)是ti时刻的初始 经纬度和高程表示的ti时刻的初始位置,qti是四元数表示的ti时刻的初始 姿态。
上述步骤中构造增量残差方程,之后使用增量残差方程优化初始位姿, 进行上述处理后提高了位姿的准确性,无需考虑坐标系的切换,有效地保 证了全局一致性。
本公开的技术方案中,所涉及的车辆位姿信息的获取,存储和应用等, 均符合相关法律法规的规定,且不违背公序良俗。
根据本公开的开的实施例,本公开提供了一种车辆位姿的处理装置, 该装置用于实现上述实施例及优选实时方式,已经进行过说明的不再赘述。 如以下使用的,术语“模块”可以实现预定功能的软件和/硬件的组合,尽 管以下实施例所描述的装置较佳地以软件来实现,但是硬件,或软件和硬 件的组合的实现也是可能并被构想的。
图7是根据本公开实施例的一种车辆位姿的处理装置的结构图,如图 7所示,该装置包括:
构建模块72,用于构建车辆在不同时刻的初始位姿信息,其中,初始 位姿信息包括:初始位置和初始姿态,初始位置采用经度、纬度和高程表 示。
第一采集模块74,用于采集车辆在不同时刻时的实时位姿,其中,实 时位姿基于车辆上的多个传感器感测得到。
优化模块76,用于基于不同时刻时采集到的实时位姿对初始位姿信息 进行优化,得到目标位姿信息。
在本公开上述的实施例中,构建车辆在不同时刻的初始位姿信息,并 且采集车辆在不同时刻时的实时位姿,基于不同时刻时采集到的实时位姿 对初始位姿信息进行优化,得到目标位姿信息,实现车辆位姿优化的目的。 容易注意到的是,构建初始位姿信息,利用车辆上的多个传感器感测得到 实时位姿对初始位姿信息进行优化的过程中,使用的坐标为经纬度,无需 考虑坐标系的切换,有效地保证了全局一致性,解决了现有技术在车辆位姿优化中需要更换坐标原点、不断重置的问题。
可选地,优化模块包括:第一残差构建单元,用于基于不同时刻时采 集到的实时位姿和初始位姿信息构建位姿残差;优化单元,用于对位姿残 差进行最小二乘处理,得到目标位姿信息。
可选地,第一残差构建单元还用于基于初始位置、实时位姿中的实时 经度、目标半径和目标偏心率,构建经度残差;基于初始位姿信息中的初 始纬度和初始高程、实时位姿中的实时纬度、目标半径和目标偏心率,构 建纬度残差;基于初始位姿信息中的初始高程和实时位姿中的实时高程, 构建高程残差;基于初始位姿信息中的初始姿态和实时位姿中的实时姿态, 构建姿态残差;基于经度残差、纬度残差、高程残差和姿态残差,得到位 姿残差。
可选地,该装置还包括:第二采集模块,用于采集车辆在不同时刻时 的实时增量位姿,其中,增量位姿基于车辆上的至少一个传感器感测得到; 优化模块还用于基于不同时刻时采集到的实时位姿和实时增量位姿对初 始位姿信息进行优化,得到目标位姿信息。
可选地,优化模块包括:第一残差构建单元,用于基于不同时刻时采 集到的实时位姿和初始位姿信息构建位姿残差;第二残差构建单元,用于 基于相邻两个时刻的初始位姿信息和相邻两个时刻对应的实时增量位姿, 构建增量位姿残差;优化单元,用于对位姿残差和增量位姿残差进行最小 二乘处理,得到目标位姿信息。
可选地,第二残差构建单元还用于基于相邻两个时刻的初始位置、相 邻两个时刻对应的实时增量位姿中的实时增量经度、目标半径和目标偏心 率,构建增量经度残差;基于相邻两个时刻的初始位姿信息中的初始纬度 和初始高程、相邻两个时刻对应的实时增量位姿中的实时增量纬度、目标 半径和目标偏心率,构建增量纬度残差;基于相邻两个时刻的初始位姿信 息中的初始高程和相邻两个时刻对应的实时增量位姿中的实时增量高程, 构建增量高程残差;基于相邻两个时刻的初始位姿信息中的初始姿态和相 邻两个时刻对应的实时增量位姿中的实时增量姿态,构建增量姿态残差; 基于增量经度残差、增量纬度残差、增量高程残差和增量姿态残差,得到 增量位姿残差。
根据本公开的实施例,本公开还提供了一种电子设备、一种可读存储 介质、一种计算机程序产品和一种自动驾驶车辆。
图8示出了可以用来实施本公开的实施例的示例电子设备800的示意 性框图。电子设备旨在表示各种形式的数字计算机,诸如,膝上型计算机、 台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算 机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置,诸 如,个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算 装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示 例,并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图8所示,设备800包括计算单元801,其可以根据存储在只读存 储器(ROM)802中的计算机程序或者从存储单元808加载到随机访问存 储器(RAM)803中的计算机程序,来执行各种适当的动作和处理。在 RAM 803中,还可存储设备800操作所需的各种程序和数据。计算单元 801、ROM 802以及RAM 803通过总线804彼此相连。输入/输出(I/O) 接口805也连接至总线804。
设备800中的多个部件连接至I/O接口805,包括:输入单元806, 例如键盘、鼠标等;输出单元807,例如各种类型的显示器、扬声器等; 存储单元808,例如磁盘、光盘等;以及通信单元809,例如网卡、调制 解调器、无线通信收发机等。通信单元809允许设备800通过诸如因特网 的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元801可以是各种具有处理和计算能力的通用和/或专用处理 组件。计算单元801的一些示例包括但不限于中央处理单元(CPU)、图 形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机 器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的 处理器、控制器、微控制器等。计算单元801执行上文所描述的各个方法和处理,例如车辆位姿的处理方法。例如,在一些实施例中,车辆位姿的 处理方法可被实现为计算机软件程序,其被有形地包含于机器可读介质, 例如存储单元808。在一些实施例中,计算机程序的部分或者全部可以经 由ROM 802和/或通信单元809而被载入和/或安装到设备800上。当计算 机程序加载到RAM 803并由计算单元801执行时,可以执行上文描述的 车辆位姿的处理方法的一个或多个步骤。备选地,在其他实施例中,计算 单元801可以通过其他任何适当的方式(例如,借助于固件)而被配置为 执行车辆位姿的处理方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路 系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、 专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设 备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些 各种实施方式可以包括:实施在一个或者多个计算机程序中,该一个或者 多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/ 或解释,该可编程处理器可以是专用或者通用可编程处理器,可以从存储 系统、至少一个输入装置、和至少一个输出装置接收数据和指令,并且将 数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出 装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的 任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其 他可编程数据处理装置的处理器或控制器,使得程序代码当由处理器或控 制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可 以完全在机器上执行、部分地在机器上执行,作为独立软件包部分地在机 器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中,机器可读介质可以是有形的介质,其可以包含 或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设 备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读 储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电 磁的、红外的、或半导体系统、装置或设备,或者上述内容的任何合适组 合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、 可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑 盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的 任何合适组合。
为了提供与用户的交互,可以在计算机上实施此处描述的系统和技术, 该计算机具有:用于向用户显示信息的显示装置(例如,CRT(阴极射线 管)或者LCD(液晶显示器)监视器);以及键盘和指向装置(例如,鼠 标或者轨迹球),用户可以通过该键盘和该指向装置来将输入提供给计算 机。其它种类的装置还可以用于提供与用户的交互;例如,提供给用户的 反馈可以是任何形式的传感反馈(例如,视觉反馈、听觉反馈、或者触觉 反馈);并且可以用任何形式(包括声输入、语音输入或者、触觉输入) 来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如, 作为数据服务器)、或者包括中间件部件的计算系统(例如,应用服务器)、 或者包括前端部件的计算系统(例如,具有图形用户界面或者网络浏览器 的用户计算机,用户可以通过该图形用户界面或者该网络浏览器来与此处 描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部 件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如,通信网络)来将系统的部件相互连接。通信网络 的示例包括:局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此 并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具 有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器 可以是云服务器,也可以为分布式系统的服务器,或者是结合了区块链的 服务器。
应该理解,可以使用上面所示的各种形式的流程,重新排序、增加或 删除步骤。例如,本发公开中记载的各步骤可以并行地执行也可以顺序地 执行也可以不同的次序执行,只要能够实现本公开公开的技术方案所期望 的结果,本文在此不进行限制。
上述具体实施方式,并不构成对本公开保护范围的限制。本领域技术 人员应该明白的是,根据设计要求和其他因素,可以进行各种修改、组合、 子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和 改进等,均应包含在本公开保护范围之内。
Claims (16)
1.一种车辆位姿的处理方法,包括:
构建车辆在不同时刻的初始位姿信息,其中,所述初始位姿信息包括:初始位置和初始姿态,所述初始位置采用经度、纬度和高程表示;
采集所述车辆在所述不同时刻时的实时位姿,其中,所述实时位姿基于所述车辆上的多个传感器感测得到;
基于所述不同时刻时采集到的实时位姿对所述初始位姿信息进行优化,得到目标位姿信息。
2.根据权利要求1所述的方法,其中,基于所述不同时刻时采集到的所述实时位姿对所述初始位姿信息进行优化,得到所述目标位姿信息包括:
基于所述不同时刻时采集到的实时位姿和所述初始位姿信息构建位姿残差;
对所述位姿残差进行最小二乘处理,得到所述目标位姿信息。
3.根据权利要求2所述的方法,基于所述不同时刻时采集到的实时位姿和所述初始位姿信息构建所述位姿残差包括:
基于所述初始位置、所述实时位姿中的实时经度、目标半径和目标偏心率,构建经度残差;
基于所述初始位姿信息中的初始纬度和初始高程、所述实时位姿中的实时纬度、所述目标半径和所述目标偏心率,构建纬度残差;
基于所述初始位姿信息中的初始高程和所述实时位姿中的实时高程,构建高程残差;
基于所述初始位姿信息中的初始姿态和所述实时位姿中的实时姿态,构建姿态残差;
基于所述经度残差、所述纬度残差、所述高程残差和所述姿态残差,得到所述位姿残差。
4.根据权利要求1至3中任意一项所述的方法,其中,在基于所述实时位姿对所述初始位姿信息进行优化,得到所述目标位姿信息之前,所述方法还包括:
采集所述车辆在所述不同时刻时的实时增量位姿,其中,所述实时增量位姿基于所述车辆上的至少一个传感器感测得到;
基于所述不同时刻时采集到的实时位姿和实时增量位姿对所述初始位姿信息进行优化,得到所述目标位姿信息。
5.根据权利要求4所述的方法,其中,基于所述不同时刻时采集到的实时位姿和实时增量位姿对所述初始位姿信息进行优化,得到所述目标位姿信息包括:
基于所述不同时刻时采集到的实时位姿和所述初始位姿信息构建位姿残差;
基于相邻两个时刻的初始位姿信息和所述相邻两个时刻对应的实时增量位姿,构建增量位姿残差;
对所述位姿残差和所述增量位姿残差进行最小二乘处理,得到所述目标位姿信息。
6.根据权利要求5所述的方法,其中,基于相邻两个时刻的初始位姿信息和所述相邻两个时刻对应的实时增量位姿,构建所述增量位姿残差包括:
基于所述相邻两个时刻的初始位置、所述相邻两个时刻对应的实时增量位姿中的实时增量经度、目标半径和目标偏心率,构建增量经度残差;
基于所述相邻两个时刻的初始位姿信息中的初始纬度和初始高程、所述相邻两个时刻对应的实时增量位姿中的实时增量纬度、所述目标半径和所述目标偏心率,构建增量纬度残差;
基于所述相邻两个时刻的初始位姿信息中的初始高程和所述相邻两个时刻对应的实时增量位姿中的实时增量高程,构建增量高程残差;
基于所述相邻两个时刻的初始位姿信息中的初始姿态和所述相邻两个时刻对应的实时增量位姿中的实时增量姿态,构建增量姿态残差;
基于所述增量经度残差、所述增量纬度残差、所述增量高程残差和所述增量姿态残差,得到所述增量位姿残差。
7.一种车辆位姿的处理装置,包括:
构建模块,用于构建车辆在不同时刻的初始位姿信息,其中,所述初始位姿信息包括:初始位置和初始姿态,所述初始位置采用经度、纬度和高程表示;
第一采集模块,用于采集所述车辆在所述不同时刻时的实时位姿,其中,所述实时位姿基于所述车辆上的多个传感器感测得到;
优化模块,用于基于所述不同时刻时采集到的实时位姿对所述初始位姿信息进行优化,得到目标位姿信息。
8.根据权利要求7所述的装置,其中,所述优化模块包括:
第一残差构建单元,用于基于所述不同时刻时采集到的实时位姿和所述初始位姿信息构建位姿残差;
优化单元,用于对所述位姿残差进行最小二乘处理,得到所述目标位姿信息。
9.根据权利要求8所述的装置,所述第一残差构建单元还用于:
基于所述初始位置、所述实时位姿中的实时经度、目标半径和目标偏心率,构建经度残差;
基于所述初始位姿信息中的初始纬度和初始高程、所述实时位姿中的实时纬度、所述目标半径和所述目标偏心率,构建纬度残差;
基于所述初始位姿信息中的初始高程和所述实时位姿中的实时高程,构建高程残差;
基于所述初始位姿信息中的初始姿态和所述实时位姿中的实时姿态,构建姿态残差;
基于所述经度残差、所述纬度残差、所述高程残差和所述姿态残差,得到所述位姿残差。
10.根据权利要求7至9中任意一项所述的装置,其中,所述装置还包括:
第二采集模块,用于采集所述车辆在所述不同时刻时的实时增量位姿,其中,所述增量位姿基于所述车辆上的至少一个传感器感测得到;
优化模块还用于基于所述不同时刻时采集到的实时位姿和实时增量位姿对所述初始位姿信息进行优化,得到所述目标位姿信息。
11.根据权利要求10所述的装置,其中,所述优化模块包括:
第一残差构建单元,用于基于所述不同时刻时采集到的实时位姿和所述初始位姿信息构建位姿残差;
第二残差构建单元,用于基于相邻两个时刻的初始位姿信息和所述相邻两个时刻对应的实时增量位姿,构建增量位姿残差;
优化单元,用于对所述位姿残差和所述增量位姿残差进行最小二乘处理,得到所述目标位姿信息。
12.根据权利要求11所述的装置,其中,所述第二残差构建单元还用于:
基于所述相邻两个时刻的初始位置、所述相邻两个时刻对应的实时增量位姿中的实时增量经度、目标半径和目标偏心率,构建增量经度残差;
基于所述相邻两个时刻的初始位姿中的初始纬度和初始高程、所述相邻两个时刻对应的实时增量位姿中的实时增量纬度、所述目标半径和所述目标偏心率,构建增量纬度残差;
基于所述相邻两个时刻的初始位姿信息中的初始高程和所述相邻两个时刻对应的实时增量位姿中的实时增量高程,构建增量高程残差;
基于所述相邻两个时刻的初始位姿信息中的初始姿态和所述相邻两个时刻对应的实时增量位姿中的实时增量姿态,构建增量姿态残差;
基于所述增量经度残差、所述增量纬度残差、所述增量高程残差和所述增量姿态残差,得到所述增量位姿残差。
13.一种电子设备,包括:
至少一个处理器;以及
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行权利要求1-6中任一项所述的方法。
14.一种存储有计算机指令的非瞬时计算机可读存储介质,其中,所述计算机指令用于使所述计算机执行根据权利要求1-6中任一项所述的方法。
15.一种计算机程序产品,包括计算机程序,所述计算机程序在被处理器执行时实现根据权利要求1-6中任一项所述的方法。
16.一种自动驾驶车辆,包括如权利要求13所述的电子设备。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111424961.1A CN114170297A (zh) | 2021-11-26 | 2021-11-26 | 车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111424961.1A CN114170297A (zh) | 2021-11-26 | 2021-11-26 | 车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114170297A true CN114170297A (zh) | 2022-03-11 |
Family
ID=80481171
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111424961.1A Pending CN114170297A (zh) | 2021-11-26 | 2021-11-26 | 车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114170297A (zh) |
-
2021
- 2021-11-26 CN CN202111424961.1A patent/CN114170297A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107438752B (zh) | 定位方法、终端和服务器 | |
CN113820735B (zh) | 位置信息的确定方法、位置测量设备、终端及存储介质 | |
Niu et al. | Development and evaluation of GNSS/INS data processing software for position and orientation systems | |
JP7356528B2 (ja) | 地図データ処理方法及び装置 | |
CN114046792B (zh) | 一种无人船水面定位与建图方法、装置及相关组件 | |
KR20220052312A (ko) | 차량 포지셔닝 방법, 장치 및 자율 운전 차량 | |
CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN115164936A (zh) | 高精地图制作中用于点云拼接的全局位姿修正方法及设备 | |
CN115166791A (zh) | 一种智能驾驶车辆双gnss天线航向角标定方法及装置 | |
CN113592951A (zh) | 车路协同中路侧相机外参标定的方法、装置、电子设备 | |
CN113029136A (zh) | 定位信息处理的方法、设备、存储介质及程序产品 | |
CN115792985A (zh) | 一种车辆定位方法、装置、电子设备、存储介质及车辆 | |
CN114170297A (zh) | 车辆位姿的处理方法、装置、电子设备和自动驾驶车辆 | |
US20220236069A1 (en) | Method and apparatus for route navigation, electronic device, computer readable medium | |
CN113218380B (zh) | 一种电子罗盘的校正方法、装置、电子设备及存储介质 | |
US20220299592A1 (en) | Ranging-aided robot navigation using ranging nodes at unknown locations | |
CN109459769A (zh) | 一种自主定位方法与系统 | |
CN114488237A (zh) | 定位方法、装置、电子设备及智能驾驶方法 | |
CN114170300A (zh) | 高精地图点云位姿优化方法、装置、设备及介质 | |
CN114281832A (zh) | 基于定位结果的高精地图数据更新方法、装置和电子设备 | |
CN111811500A (zh) | 目标对象的位姿估计方法、装置、存储介质和电子设备 | |
EP4095542A2 (en) | Positioning method, on-board device,terminal device and positioning system | |
CN111353203A (zh) | 一种地震采集排列布设优化方法、装置和存储介质 | |
CN115127561A (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 |