一种可移动设备的定位方法、装置、系统及可移动设备与流程
本申请涉及自动驾驶技术领域，尤其涉及一种可移动设备的定位方法、装置、系统及可移动设备。
背景技术：
可移动设备是指车辆、无人机、智能机器人等可在预设路径上行驶的设备。在自动驾驶技术领域中，为了保证自动驾驶车辆、无人机、智能机器人等的精确运行，一般需要对这些可移动设备进行精确定位。目前定位的方式多种多样，如可采用可移动设备上搭载的gnss传感器(全球导航卫星系统，globalnavigationsatellitesystem)、机器视觉传感器(摄像头、激光雷达、毫米波雷达、超声波雷达)来进行定位，或者通过与外部基站通信来进行定位。
然而，当前的可移动设备简单依赖于gnss传感器、机器视觉传感器以及与外部基站的通信来进行定位，存在定位结果不准确的问题。
技术实现要素：
本申请的实施例提供一种可移动设备的定位方法、装置、系统及可移动设备，以解决可移动设备简单依赖于gnss传感器、机器视觉传感器以及与外部基站的通信来进行定位，存在的定位结果不准确的问题。
为达到上述目的，本申请采用如下技术方案：
在本申请实施例的第一方面，提供一种可移动设备的定位方法，包括：
获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点；
获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点；
根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边；
根据所述第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边；
根据所述第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。
在本申请实施例的第二方面，提供一种可移动设备的定位装置，包括：
第一位姿信息获得单元，用于获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点；
第二位姿信息获得单元，用于获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点；
第一类边确定单元，用于根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边；
第二类边确定单元，用于根据所述第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边；
位姿优化结果确定单元，用于根据所述第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。
在本申请实施例的第三方面，提供一种可移动设备的定位系统，包括可移动设备的定位装置、可移动设备和用于感知所述可移动设备的感知器，所述可移动设备的定位装置与所述可移动设备和用于感知所述可移动设备的感知器分别通信连接；
所述可移动设备的定位装置，用于：
获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点；
获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点；
根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边；
根据所述第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边；
根据所述第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。
在本申请实施例的第四方面，提供一种可移动设备，所述可移动设备包括可移动设备的定位装置，该可移动设备的定位装置用于实现上述的可移动设备的定位方法。
在本申请实施例的第五方面，提供一种感知器，所述感知器包括可移动设备的定位装置，该可移动设备的定位装置用于实现上述的可移动设备的定位方法。
在本申请实施例的第六方面，提供一种云服务器，所述云服务器包括可移动设备的定位装置，该可移动设备的定位装置用于实现上述的可移动设备的定位方法。
在本申请实施例的第七方面，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的可移动设备的定位方法。
在本申请实施例的第八方面，提供一种计算机设备，包括存储器、处理器及存储在存储上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述的可移动设备的定位方法。
本申请实施例提供的一种可移动设备的定位方法、装置、系统及可移动设备，首先获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点；获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点；之后，根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边；根据所述第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边；然后，根据所述第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。可见，本申请实施例通过图优化的方式，可对可移动设备的位姿进行优化，避免了可移动设备简单依赖于gnss传感器、机器视觉传感器以及与外部基站的通信来进行定位，存在的定位结果不准确的问题。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种可移动设备的定位系统的结构示意图；
图2为本申请实施例中的感知器的位置示意图；
图3为本申请实施例中的感知器的分布示意图；
图4为本申请实施例中的图优化的顶点和边的示意图；
图5为本申请实施例提供的一种可移动设备的定位方法的流程图；
图6为本申请实施例中的确定可移动设备的位姿优化结果的方法流程图；
图7为本申请实施例提供的一种可移动设备的定位装置的结构示意图；
图8为本申请实施例提供的一种可移动设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
值得说明的是，术语“可移动设备”在本申请中广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的可移动设备，例如电车或火车以及其它有轨车辆。本申请中的“可移动设备”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，可移动设备可以包括更多、更少或者不同的系统。
其中，动力系统是为可移动设备提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。
控制系统可以包括控制可移动设备及其组件的装置的组合，例如转向单元、节气门、制动单元。
外围设备可以是允许可移动设备与外部传感器、其它可移动设备、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。
基于上述描述的可移动设备，例如无人驾驶车辆中还配置有传感器系统和无人驾驶控制装置。
传感器系统可以包括用于感测可移动设备所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(radar)单元、相机、激光测距仪、光检测和测距(lidar)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视可移动设备内部系统的传感器(例如o2监视器、燃油表、引擎温度计等)。
无人驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知可移动设备所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为可移动设备规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对可移动设备控制系统的控制命令输出，并通过车载网(例如通过can总线、局域互联网络、多媒体定向系统传输等方式实现的可移动设备内部电子网络系统)将控制命令发送给可移动设备控制系统中的对应部件，实现对可移动设备的自动控制；自动控制模块还可以通过车载网来获取可移动设备中各部件的信息。
为了使本领域的技术人员更好的了解本申请，下面对本申请实施例中所涉及的技术术语进行解释如下：
slam：simultaneouslocalizationandmapping，是指同步定位和地图构建技术，通常是用于自动驾驶和机器人领域的专用技术，通过使用激光雷达、摄像头等传感器感知自身姿态，并对环境构建地图。
图优化：应用计算机中的图理论，把一个常规的优化问题，以图的形式进行表达，并对关注的参数进行优化的问题。
顶点：图的顶点是需要优化的变量，通常以一个向量的形式进行表示。
边：连接两个顶点之间的转换关系，对于向量形式的顶点，边的表达形式是一个矩阵，表明两个变量之间的转移关系。在slam中，通常用两个姿态之间的转移矩阵进行表示。
gps：globalpositioningsystem，全球定位系统。
gnss：globalnavigationsatellitesystem，全球卫星导航系统。
imu：inertialmeasurementunit，惯性测量单元。
uwb：ultrawideband，超宽带，是一种无载波通信技术。
v2x：vehicletox，车对外界的信息交换技术，是未来智能交通运输系统的关键技术。
4g：the4thgenerationmobilecommunicationtechnology，第四代移动通信技术。
5g：the5thgenerationmobilecommunicationtechnology，第五代移动通信技术。
岸吊：岸边集装箱起重机。
场吊：货场集装箱起重机。
位姿：pose，位置和姿态的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。3个朝向自由度通常用俯仰角(pitch)、翻滚角(roll)、偏航角(yaw)来表示。在本申请中，由于可移动设备一般在水平路面上行驶，因此可仅考虑偏航角。
为了使本领域的技术人员更好的了解本申请，下面对本申请所涉及的应用环境进行说明，例如本申请可以应用于自动驾驶车辆在高速公路、城市道路、港口，海关，仓库，物流园区等环境的精准定位，也可以应用于物流机器人在仓库、物流园区等环境的精准定位。以上仅是本申请中的个别应用实例，需要知道的是，在本申请实施例的教示下，本领域技术人员还可以根据需求提供更多的应用实例，本申请不限于这些应用实例。
在实现本申请实施例的过程中，发明人发现一般情况下可移动设备(如自动驾驶车辆)采用自身设置的激光雷达、摄像头等机器视觉传感器或gnss传感器(如gps、北斗卫星导航系统等)来进行定位。然而在一些环境复杂的区域和gnss信号差的区域，例如港口、海关，仓库、物流园区等，由于集装箱码放层数较高，岸吊、场吊的几何尺寸较大，对车载接收的gnss信号形成严重干扰。造成车辆定位飘移较远，对行驶安全带来严重挑战。同时，由于箱区的集装箱被频繁搬运，因此造成周边环境不稳定，因此也不能使用集装箱作为视觉定位的特征。
可见，在上述的环境复杂的区域和gnss信号差的区域，是难以采用自身设置的激光雷达、摄像头等机器视觉传感器或gnss传感器来进行准确定位的。另外，在激光雷达、摄像头等机器视觉传感器发生故障时，同样会影响可移动设备的定位。
为了实现一种可移动设备的可靠的定位方式，在一实施例中，可移动设备以自动驾驶车辆或智能机器人等在预设行驶路径上移动的设备为例，如图1所示，本申请实施例提供一种可移动设备的定位系统10，该可移动设备的定位系统10包括可移动设备的定位装置101、可移动设备102和用于感知可移动设备102的感知器103，该可移动设备的定位装置101与可移动设备102和用于感知可移动设备102的感知器103分别通信连接。此外，在该可移动设备102上可以设置多种传感器，例如该多种传感器包括gnss传感器104、uwb标签105、imu传感器106、激光雷达传感器107、毫米波雷达传感器108、超声波雷达传感器109和摄像头110等中的任意多种的组合，此处为了描述方便，可移动设备102可以设置有上述多种传感器中的每一种。
另外，在一实施例中，为了便于数据存储和通信，如图1所示，该可移动设备的定位系统10还可以包括云服务器111，该云服务器111、可移动设备102(可移动设备102可以带有车载服务器或车载计算机，从而能够实现和云服务器11以及感知器103之间的通信)和感知器103之间两两通信连接(例如云服务器111与感知器103和可移动设备102之间可采用4g或5g网络通信，可移动设备102和感知器103之间可以采用4g、5g、局域网或者v2x进行通信)；该可移动设备的定位装置101可以设置于可移动设备102、感知器103或者云服务器111处，可移动设备的定位装置101在设置于不同位置时，需要与另外两处进行通信(例如可移动设备的定位装置101设置于可移动设备102处，则需要与感知器103和云服务器111通信连接，但不仅局限于此。)。
另外，在一实施例中，该感知器103可以设置于可移动设备102行驶路径的上端或者任意侧端。例如如图2所示，以可移动设备102是车辆为例，其行驶路径为公路201，在公路201的上端202或者任意侧端203可以设置感知器103(例如可以通过架子、灯杆等来架设感知器103)。该感知器103可以为摄像头、激光雷达传感器等可感知车辆的设备。
另外，在一实施例中，如图3所示，为了便于可移动设备102在一段较长行驶路径上的定位，在行驶路径上，每隔一段预设距离(例如50米距离)即可设置一组感知器103。感知器103可朝向可移动设备102的来车方向，从而有效感知一定区域范围内的可移动设备102，以用于后续的精准定位。当然感知器103也可以朝向可移动设备102的来车方向的相反方向，也可以感知逐渐远离感知器103的可移动设备102。
以下介绍在一实施例中，可移动设备的定位装置101的具体定位过程。
在一实施例中，如图4所示，该可移动设备的定位装置101可以：
获得预先标定的感知器103的第一位姿信息，作为图优化的第一顶点301。例如该第一顶点301(p点)即为图4中感知器的位置。
获得当前采样时刻的可移动设备102的第二位姿信息，作为图优化的第二顶点302。例如，在图4中，以可移动设备102从a位置行驶到b位置，再行驶到c位置为例，进行说明(此处仅以3个位置为例，需要知道的是，基于不同的相邻采样时刻的间隔，可以有更多的第二位姿信息，即相当于存在更多的第二顶点)；当可移动设备102到达a位置时，则将a位置的第二位姿信息所表示的点作为当前采样时刻的第二顶点302，以此类推，可以得到各个采样时刻的可移动设备102对应的第二顶点。在一实施例中，相邻采样时刻的间隔可以为1秒或其他预设间隔。
根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边303。例如，在图4中，当可移动设备102从a位置行驶到b位置，在图优化技术中，则从a位置到b位置的边即可以确定，即第一类边303，后续再从b位置到c位置的边也可以确定。
根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边304。例如，在可移动设备102到达a位置时，则从p点到a位置的边即可确定，即该第二类边304(pa)。以此类推，在可移动设备102分别到达b位置和c位置时，则从p点到b位置和c位置的边即可确定，即该第二类边304(pb和pc)。
根据第一顶点301、多个采样时刻的第一类边303和第二类边304对第二顶点302进行图优化，确定可移动设备的位姿优化结果。此处，由于各第二顶点302(例如图4中的可移动设备102在a位置、b位置和c位置)的来源是可移动设备上的传感器，因此其定位结果并不准确，需要通过图优化来进行优化得到可移动设备的位姿优化结果(即将各个第二顶点302进行了优化)。
值得说明的是，如图4所示，在获得第一位姿信息和第二位姿信息的情况下，各位姿信息在坐标系中对应的点即可以获知，从而在图优化技术中，可以确定上述的第一顶点301和第二顶点302。在获得第一顶点301和第二顶点302之后，两个顶点之间的转换关系即可以确定，形成上述的第一类边303和第二类边304。
在一实施例中，该第一位姿信息可以包括感知器103在预设坐标系下的三轴坐标(x0,y0,z0)和偏航角θ0。该预设坐标系可以是预先建立的关于场景的全局坐标系，也可以是世界坐标系，但不仅局限于此。
在一实施例中，对于获得当前采样时刻的可移动设备102的第二位姿信息，可移动设备的定位装置101，具体可以采用如下方式：
根据可移动设备102上设置的多种传感器中的一种或多种(如gnss传感器104、uwb标签105、imu传感器106、激光雷达传感器107、毫米波雷达传感器108、超声波雷达传感器109和摄像头110)获得当前采样时刻的可移动设备102的第二位姿信息；当前采样时刻的可移动设备102的第二位姿信息包括当前采样时刻k的可移动设备102在预设坐标系下的三轴坐标(xk初始,yk初始,zk初始)和偏航角θk初始。此时得到的第二位姿信息即为待优化的位姿，其结果并不一定准确，可能存在位置漂移的情况。
在一实施例中，对于根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备102的位姿信息确定图优化的第一类边303，该上一采样时刻的可移动设备102的位姿信息可采用上一采样时刻的可移动设备102的第二位姿信息(即优化前的位姿)；其中，该上一采样时刻的可移动设备102的第二位姿信息包括上一采样时刻k-1的可移动设备102在预设坐标系下的三轴坐标(xk-1初始,yk-1初始,zk-1初始)和偏航角θk-1初始。这样，该可移动设备的定位装置101，可以采用如下方式来确定图优化的第一类边303：
根据匀速直线运动模型：
此处，为了方便计算，该xk初始可以扩展为五元向量，即xk初始可以表示为五元向量(xk初始,yk初始,zk初始，θk初始，1)；同理，该xk-1初始可以表示为五元向量(xk-1初始,yk-1初始,zk-1初始，θk-1初始，1)。一般情况下，该第一类边tk-1，k可以采用对角线为1的方阵来表示，如根据上述的匀速直线运动模型的内容，可以得到：
另外，在一实施例中，对于根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备102的位姿信息确定图优化的第一类边303，该上一采样时刻的可移动设备102的位姿信息可采用上一采样时刻的可移动设备102的位姿优化结果(即优化后的位姿)，作为上一采样时刻的优化后的第二顶点；其中上一采样时刻的可移动设备102的位姿优化结果包括上一采样时刻k-1的可移动设备102在预设坐标系下的三轴坐标优化值(xk-1优化,yk-1优化,zk-1优化)和偏航角优化值θk-1优化。这样，该可移动设备的定位装置101，可以采用如下方式来确定图优化的第一类边303：
根据匀速直线运动模型：
此处，为了方便计算，该xk初始可以扩展为五元向量，即xk初始可以表示为五元向量(xk初始,yk初始,zk初始，θk初始，1)；同理，该xk-1优化可以表示为五元向量(xk-1优化,yk-1优化,zk-1优化，θk-1优化，1)。一般情况下，该第一类边tk-1，k可以采用对角线为1的方阵来表示，如根据上述的匀速直线运动模型的内容，可以得到：
另外，在一实施例中，在确定图优化的第二类边304之前，该可移动设备的定位装置101，还可以：
获得当前采样时刻的感知器103感知的可移动设备102的第三位姿信息。
该第三位姿信息包括当前采样时刻k的感知器103感知的可移动设备102在预设坐标系下的三轴坐标(xk感知,yk感知,zk感知)和偏航角θk感知。
在一实施例中，对于根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边304，该可移动设备的定位装置101，可以采用如下方式实现：
根据感知器103感知可移动设备的关系模型：
此处，为了方便计算，该xk初始可以扩展为五元向量，即xk初始可以表示为五元向量(xk初始,yk初始,zk初始，θk初始，1)；同理，该xk感知可以表示为五元向量(xk感知,yk感知,zk感知，θk感知，1)；该x0可以表示为五元向量(x0,y0,z0，θ0，1)；一般情况下，该第二类边t0，k可以采用对角线为1的方阵来表示，如根据上述的感知可移动设备的关系模型的内容，可以得到：
另外，在一实施例中，对于根据第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果，该可移动设备的定位装置101，可以采用如下方式实现：
根据第一顶点、多个采样时刻的第二类边建立第一部分残差函数。
根据多个采样时刻的第一类边建立第二部分残差函数。
根据第一部分残差函数和第二部分残差函数，确定待优化残差函数。
根据预先设置的优化算法对待优化残差函数进行优化，确定可移动设备的位姿优化结果。
在一实施例中，对于根据第一顶点、多个采样时刻的第二类边建立第一部分残差函数，可移动设备的定位装置101，可以采用如下方式实现：
根据第一顶点对应的第一位姿信息x0、多个采样时刻的第二类边t0，n建立位姿优化结果xn优化的第一部分残差函数：
在一实施例中，对于根据多个采样时刻的第一类边建立第二部分残差函数，可移动设备的定位装置101，可以采用如下方式实现：
根据多个采样时刻的第一类边tn-1，n建立位姿优化结果xn优化的第二部分残差函数：
在一实施例中，对于根据第一部分残差函数和第二部分残差函数，确定待优化残差函数，可移动设备的定位装置101，可以采用如下方式实现：
将第一部分残差函数
在一实施例中，对于根据预先设置的优化算法对待优化残差函数进行优化，确定可移动设备的位姿优化结果，可移动设备的定位装置101，可以采用如下方式实现：
根据预先设置的优化算法，对待优化残差函数：
另外，在一实施例中，如图5所示，本申请实施例还提供一种可移动设备的定位方法，包括：
步骤401、获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点。
步骤402、获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点。
步骤403、根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边。
步骤404、根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边。
步骤405、根据第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。
在一实施例中，该第一位姿信息包括感知器在预设坐标系下的三轴坐标(x0,y0,z0)和偏航角θ0。
在上述步骤402中，获得当前采样时刻的可移动设备的第二位姿信息，可以采用如下方式实现：
根据可移动设备上设置的多种传感器中的一种或多种获得当前采样时刻的可移动设备的第二位姿信息；当前采样时刻的可移动设备的第二位姿信息包括当前采样时刻k的可移动设备在预设坐标系下的三轴坐标(xk初始,yk初始,zk初始)和偏航角θk初始；多种传感器包括gnss传感器、uwb标签、imu传感器、激光雷达传感器、毫米波雷达传感器、超声波雷达传感器和摄像头中的任意多种的组合。
在一实施例中，上一采样时刻的可移动设备的位姿信息包括上一采样时刻的可移动设备的第二位姿信息；其中上一采样时刻的可移动设备的第二位姿信息包括上一采样时刻k-1的可移动设备在预设坐标系下的三轴坐标(xk-1初始,yk-1初始,zk-1初始)和偏航角θk-1初始。
在上述步骤403中，根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边，可以采用如下方式实现：
根据匀速直线运动模型：
或者，在一实施例中，上一采样时刻的可移动设备的位姿信息包括上一采样时刻的可移动设备的位姿优化结果，作为上一采样时刻的优化后的第二顶点；其中上一采样时刻的可移动设备的位姿优化结果包括上一采样时刻k-1的可移动设备在预设坐标系下的三轴坐标优化值(xk-1优化,yk-1优化,zk-1优化)和偏航角优化值θk-1优化。
在上述步骤403中，根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边，可以采用如下方式实现：
根据匀速直线运动模型：
在一实施例中，在上述步骤404，根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边之前，还可以进行如下过程：
获得当前采样时刻的感知器感知的可移动设备的第三位姿信息；第三位姿信息包括当前采样时刻k的感知器感知的可移动设备在预设坐标系下的三轴坐标(xk感知,yk感知,zk感知)和偏航角θk感知。
在一实施例中，上述步骤404，根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边，可以采用如下方式实现：
根据感知器感知可移动设备的关系模型：
在一实施例中，上述步骤405，根据第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果，可以采用如图6所示的方法实现：
步骤501、根据第一顶点、多个采样时刻的第二类边建立第一部分残差函数。
步骤502、根据多个采样时刻的第一类边建立第二部分残差函数。
步骤503、根据第一部分残差函数和第二部分残差函数，确定待优化残差函数。
步骤504、根据预先设置的优化算法对待优化残差函数进行优化，确定可移动设备的位姿优化结果。
在一实施例中，上述步骤501，根据第一顶点、多个采样时刻的第二类边建立第一部分残差函数，可以采用如下方式实现：
根据第一顶点对应的第一位姿信息x0、多个采样时刻的第二类边t0，n建立位姿优化结果xn优化的第一部分残差函数：
在一实施例中，上述步骤502，根据多个采样时刻的第一类边建立第二部分残差函数，可以采用如下方式实现：
根据多个采样时刻的第一类边tn-1，n建立位姿优化结果xn优化的第二部分残差函数：
在一实施例中，上述步骤503，根据第一部分残差函数和第二部分残差函数，确定待优化残差函数，可以采用如下方式实现：
将第一部分残差函数
在一实施例中，预先设置的优化算法包括牛顿法、拟牛顿法、梯度下降法或共轭梯度法；
上述步骤504，根据预先设置的优化算法对待优化残差函数进行优化，确定可移动设备的位姿优化结果，可以采用如下方式实现：
根据预先设置的优化算法，对待优化残差函数：
在一实施例中，如图7所示，本申请实施例还提供一种可移动设备的定位装置，包括：
第一位姿信息获得单元61，用于获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点。
第二位姿信息获得单元62，用于获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点。
第一类边确定单元63，用于根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边。
第二类边确定单元64，用于根据第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边。
位姿优化结果确定单元65，用于根据第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。
在一实施例中，如图8所示，本申请实施例还提供一种可移动设备102，可移动设备102包括可移动设备的定位装置101，此外，在该可移动设备102上可以设置多种传感器，例如该多种传感器包括gnss传感器104、uwb标签105、imu传感器106、激光雷达传感器107、毫米波雷达传感器108、超声波雷达传感器109和摄像头110等中的任意多种的组合，此处为了描述方便，可移动设备102可以设置有上述多种传感器中的每一种。该可移动设备的定位装置101可以是可移动设备102上的车载服务器、车载计算机等设备，但不仅局限于此。
该可移动设备的定位装置101用于实现上述的可移动设备的定位方法，此处不再赘述。
在一实施例中，本申请实施例还提供一种感知器，感知器包括可移动设备的定位装置，该可移动设备的定位装置用于实现上述的可移动设备的定位方法，此处不再赘述。
在一实施例中，本申请实施例还提供一种云服务器，云服务器包括可移动设备的定位装置，该可移动设备的定位装置用于实现上述的可移动设备的定位方法，此处不再赘述。
在一实施例中，本申请实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的可移动设备的定位方法，此处不再赘述。
在一实施例中，本申请实施例还提供一种计算机设备，包括存储器、处理器及存储在存储上并可在处理器上运行的计算机程序，处理器执行程序时实现上述的可移动设备的定位方法，此处不再赘述。
本申请实施例提供的一种可移动设备的定位方法、装置、系统及可移动设备，首先获得预先标定的感知器的第一位姿信息，作为图优化的第一顶点；获得当前采样时刻的可移动设备的第二位姿信息，作为图优化的第二顶点；之后，根据当前采样时刻的第二位姿信息和上一采样时刻的可移动设备的位姿信息确定图优化的第一类边；根据所述第一位姿信息和当前采样时刻的第二位姿信息确定图优化的第二类边；然后，根据所述第一顶点、多个采样时刻的第一类边和第二类边对第二顶点进行图优化，确定可移动设备的位姿优化结果。可见，本申请实施例通过图优化的方式，可对可移动设备的位姿进行优化，避免了可移动设备简单依赖于gnss传感器、机器视觉传感器以及与外部基站的通信来进行定位，存在的定位结果不准确的问题。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本申请中应用了具体实施例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。
- 还没有人留言评论。精彩留言会获得点赞！