CN117537807A - 可移动机构的定位方法、装置、介质及电子设备 - Google Patents

可移动机构的定位方法、装置、介质及电子设备 Download PDF

Info

Publication number
CN117537807A
CN117537807A CN202210918064.4A CN202210918064A CN117537807A CN 117537807 A CN117537807 A CN 117537807A CN 202210918064 A CN202210918064 A CN 202210918064A CN 117537807 A CN117537807 A CN 117537807A
Authority
CN
China
Prior art keywords
angle
coordinate system
initial
measurement unit
inertial measurement
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
Application number
CN202210918064.4A
Other languages
English (en)
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.)
Tencent Technology Shenzhen Co Ltd
Original Assignee
Tencent Technology Shenzhen Co Ltd
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 Tencent Technology Shenzhen Co Ltd filed Critical Tencent Technology Shenzhen Co Ltd
Priority to CN202210918064.4A priority Critical patent/CN117537807A/zh
Publication of CN117537807A publication Critical patent/CN117537807A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本申请的实施例提供了一种可移动机构的定位方法、装置、介质及电子设备，该方法包括：获取惯性测量单元坐标系相对于机构坐标系的第一初始横滚角、第一初始俯仰角和第一初始航向角；对第一初始横滚角、第一初始俯仰角及第一初始航向角分别进行坐标转换，得到机构坐标系相对于惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；求解误差函数，得到机构坐标系相对于惯性测量单元坐标系的第二目标俯仰角和第二目标航向角；根据第二目标俯仰角和第二目标航向角生成安装角，以基于安装角进行定位。本申请实施例可以精准地对可移动机构定位。本申请实施例可应用于地图、自动驾驶、智慧交通、辅助驾驶等各种场景。

Description

可移动机构的定位方法、装置、介质及电子设备
技术领域
本申请涉及车载导航技术领域，具体而言，涉及一种可移动机构的定位方法、装置、计算机可读介质及电子设备。
背景技术
目前，车辆导航技术领域越来越多地使用IMU(Inertial Measurement Unit，惯性测量单元)采集的数据来实现导航和定位。因此，求解IMU与车辆载体坐标系间的安装角已经成为一个亟需解决的问题。
然而，现有的估计安装角的方法只能在特定的场景下取得一定效果，通用性差，而且这些估计安装角的方法的效率很低，安装角估计的准确率也不高，进而导致车辆无法准确实现导航和定位。
发明内容
本申请的实施例提供了一种可移动机构的定位方法、装置、计算机可读介质及电子设备，进而至少在一定程度上可以提高估计安装角的效率和准确性，并进一步提高估计安装角的通用性，并提高定位的准确性。
本申请的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本申请的实践而习得。
根据本申请实施例的一个方面，提供了一种可移动机构的定位方法，所述方法包括：获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，所述惯性测量单元位于所述可移动机构上；确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，并对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；根据将所述机构坐标系的速度向所述惯性测量单元坐标系的转换结果与所述惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解所述误差函数的最小值，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二目标俯仰角和第二目标航向角，其中，所述第二初始俯仰角和所述第二初始航向角用于首次将所述机构坐标系的速度向所述惯性测量单元坐标系转换；根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，以基于所述安装角对所述可移动机构进行定位。
根据本申请实施例的一个方面，提供了一种可移动机构的定位装置，所述装置包括：获取单元，用于获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，所述惯性测量单元位于所述可移动机构上；确定和转换单元，用于确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，并对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；建立和求解单元，用于根据将所述机构坐标系的速度向所述惯性测量单元坐标系的转换结果与所述惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解所述误差函数的最小值，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二目标俯仰角和第二目标航向角，其中，所述第二初始俯仰角和所述第二初始航向角用于首次将所述机构坐标系的速度向所述惯性测量单元坐标系转换；生成单元，用于根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，以基于所述安装角对所述可移动机构进行定位。
在本申请的一些实施例中，基于前述方案，所述确定和转换单元配置为：以预设角度步长分别设置多个第一初始航向角，将每个第一初始航向角分别与所述第一初始横滚角和所述第一初始俯仰角组成初始安装角；针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系的速度向所述机构坐标系进行转换，并确定转换后结果与所述机构坐标系的速度的差值；根据每个初始安装角对应的所述差值，得到每个初始安装角对应的误差，并将对应的误差最小的初始安装角中的第一初始航向角作为所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
在本申请的一些实施例中，基于前述方案，所述确定和转换单元配置为：如果获取到与目标数目个时刻分别对应的所述惯性测量单元坐标系的速度和所述机构坐标系的速度，则确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
在本申请的一些实施例中，基于前述方案，所述确定和转换单元配置为：针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系在每一个时刻的速度向所述机构坐标系进行转换，并确定每一个时刻的转换后结果与同一时刻下所述机构坐标系的速度的差值；根据每个初始安装角与目标数目个时刻分别对应的差值，得到每个初始安装角对应的误差。
在本申请的一些实施例中，基于前述方案，所述建立和求解单元配置为：将所述第二初始俯仰角作为第二当前俯仰角，并将所述第二初始航向角作为第二当前航向角；迭代执行误差函数求解步骤，直至满足预定收敛条件，所述误差函数求解步骤包括：根据所述第二当前俯仰角和所述第二当前航向角分别确定雅可比矩阵和所述误差函数在各个轴的误差值；根据所述雅可比矩阵和所述误差函数在各个轴的误差值求解得到所述第二当前俯仰角和所述第二当前航向角的增量；基于所述增量对所述第二当前俯仰角和所述第二当前航向角进行更新。
在本申请的一些实施例中，基于前述方案，所述确定和转换单元配置为：根据所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角确定所述惯性测量单元坐标系相对于所述机构坐标系的第一旋转矩阵；确定所述第一旋转矩阵的转置矩阵，得到第二旋转矩阵；基于所述第二旋转矩阵确定出所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角。
在本申请的一些实施例中，基于前述方案，所述获取单元配置为：根据惯性测量单元中的加速度计在各个轴的输出值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
在本申请的一些实施例中，基于前述方案，所述获取单元配置为：获取惯性测量单元中的加速度计在预定时间段内在每个轴的多个输出值；确定所述加速度计在每个轴的多个输出值的平均值；根据在每个轴的多个输出值的平均值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
在本申请的一些实施例中，基于前述方案，所述生成单元配置为：将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角；或者将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角转换为所述惯性测量单元坐标系相对于所述机构坐标系的第一目标横滚角、第一目标俯仰角及第一目标航向角，并将所述第一目标横滚角、所述第一目标俯仰角和所述第一目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角。
根据本申请实施例的一个方面，提供了一种计算机可读介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述实施例中所述的可移动机构的定位方法。
根据本申请实施例的一个方面，提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上述实施例中所述的可移动机构的定位方法。
根据本申请实施例的一个方面，提供了一种计算机程序产品，所述计算机程序产品包括计算机指令，所述计算机指令存储在计算机可读存储介质中，计算机设备的处理器从所述计算机可读存储介质读取所述计算机指令，所述处理器执行所述计算机指令，使得所述计算机设备执行如上述实施例中所述的可移动机构的定位方法。
在本申请的一些实施例所提供的技术方案中，通过先分别获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角、第一初始俯仰角及第一初始航向角，并将第一初始横滚角、第一初始俯仰角及第一初始航向角转换为机构坐标系相对于惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角，在此基础上，通过从第二初始俯仰角和第二初始航向角开始，以迭代方式求解误差函数的最小值，直至找到第二目标俯仰角和第二目标航向角，最终，根据第二初始横滚角、第二目标俯仰角和第二目标航向角便可以生成安装角。因而，整个方案适用于各种在可移动机构上按照安装惯性测量单元的情形，通用性强；由于误差函数是根据将机构坐标系的速度向惯性测量单元坐标系的转换结果与惯性测量单元坐标系的速度之间的差值建立而成的，而第二目标俯仰角和第二目标航向角是通过以迭代方式求解误差函数的最小值得到的，因此，可以准确地实现安装角的估计；此外，由于对误差函数进行迭代求解是从第二初始俯仰角和第二初始航向角开始的，无需从0开始求解，提高了估计安装角的效率，可以快速完成对安装角的估计；在准确估计出安装角的基础上，可以更精准地实现对可移动机构的定位。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。在附图中：
图1示出了可以用来实现本申请实施例的技术方案的示例性系统架构的示意图；
图2示出了根据本申请的一个实施例的可移动机构的定位方法的流程图；
图3示出了根据本申请的一个实施例的IMU坐标系和车体坐标系的关系示意图；
图4示出了根据本申请的一个实施例的图2实施例中步骤210的细节的流程图；
图5示出了根据本申请的一个实施例的根据惯性测量单元中的加速度计在各个轴的输出值确定第一初始横滚角和第一初始俯仰角的流程图；
图6示出了根据本申请的一个实施例的确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角的流程图；
图7示出了根据本申请的一个实施例的对第一初始横滚角、第一初始俯仰角及第一初始航向角分别进行坐标转换的流程图；
图8示出了根据本申请的一个实施例的以迭代方式求解误差函数的最小值的流程图；
图9示出了根据本申请的一个实施例的整体流程示意图；
图10示出了根据本申请的一个实施例的可移动机构的定位装置的框图；
图11示出了适于用来实现本申请实施例的电子设备的计算机系统的结构示意图。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本申请将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。
此外，所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施例中。在下面的描述中，提供许多具体细节从而给出对本申请的实施例的充分理解。然而，本领域技术人员将意识到，可以实践本申请的技术方案而没有特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知方法、装置、实现或者操作以避免模糊本申请的各方面。
附图中所示的方框图仅仅是功能实体，不一定必须与物理上独立的实体相对应。即，可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。
附图中所示的流程图仅是示例性说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作/步骤还可以分解，而有的操作/步骤可以合并或部分合并，因此实际执行的顺序有可能根据实际情况改变。
IMU(Inertial Measurement Unit，惯性测量单元)，又称惯性传感器，包含三轴加速度计和三轴陀螺仪，分别测量载体的加速度和角速度信息，积分后可以获取载体的位置速度姿态信息。
NHC(nonholonomic，非完整性约束)是指在汽车正常行驶过程中，车体坐标系下横向和纵向速度为0。
车载导航技术领域常用IMU与GNSS(全球导航卫星系统，Global NavigationSatellite System)、视觉传感器、激光雷达等传感器信息融合解算，以期获取高精连续的定位结果，但是当后面三种传感器失效时，IMU进行自我积分推算定位，由于其观测存在误差，随着时间的推移，定位误差越来越大。为了解决这一问题，NHC和车(轮)速信息的有效利用便成了重中之重。NHC和轮速观测信息的应用是基于车辆载体坐标系的，但是导航中的坐标信息通常是基于IMU坐标系和导航坐标系的，因此求解IMU与车辆载体坐标系间的旋转关系，即安装角就变成了一个亟需解决的问题。对于安装角估计这一问题，学术界和工业界都进行了广泛的研究，并提出了大量的方法。
在相关技术中，存在以下三种方法，这些方法在一些特定的场景下能取得一定的效果。
方法一、在设计组合导航系统时，将安装角作为待估参数，利用NHC和轮速的速度信息更新，与位置速度姿态一起进行估计。
方法二、不通过速度观测，而是通过轨迹的相关性来估计安装角。
方法三、通过IMU角速度与车体角速度间的相关性来估计安装角。
然而，这些方法均存在一定的缺陷：
对于方法一，通常认为安装角是一个小角度，这样的假设对于IMU固定安装在车上且初始安装时安装角较小的情况下，收敛是比较快的，但是对于手机IMU这种可以自由安装且安装角较大时，算法收敛时间过久，效率低下。
对于方法二，由于需要IMU进行一段自我推导，其算法对IMU精度要求较高，因此，对于消费级的IMU恐难以适用。
对于方法三，其需要更为苛刻的估计条件，需要车辆进行一系列的拐弯操作，这在实际应用的场景，是比较不合理的。
为此，本申请首先提供了一种可移动机构的定位方法。基于本申请实施例提供的可移动机构的定位方法可以克服上述缺陷，可以适用于手机IMU场景下的安装角估计，不需要对IMU具有高精度，也不需要单独设置苛刻的估计条件，即使对于手机IMU这种可以自由安装且安装角较大的情形，也能快速准确地估计出安装角，因此，提高了安装角估计的通用性和便捷性，进而可以更精准地对可移动机构进行定位。
图1示出了可以用来实现本申请实施例的技术方案的示例性系统架构的示意图。如图1所示，该系统架构100包括车辆110和云端120，车辆110和云端120能够进行通信。其中，车辆110具体包括激光雷达111、智能手机112以及GNSS(全球导航卫星系统，GlobalNavigation Satellite System)传感器113，智能手机112内置有IMU(InertialMeasurement Unit，惯性测量单元)，车辆110可以将激光雷达111、智能手机112中的IMU及GNSS传感器113采集的信号发送至云端120，以获得云端120返回的高精度的定位结果。当激光雷达111和GNSS传感器113失效时，车辆110会通过如下过程估计智能手机112中IMU的IMU坐标系相对于车辆110的车体坐标系的安装角：首先，车辆110会分别确定出IMU坐标系相对于车体坐标系的第一初始横滚角、第一初始俯仰角及第一初始航向角，并将第一初始横滚角、第一初始俯仰角及第一初始航向角转换为车体坐标系相对于IMU坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；然后，车辆110会通过基于第二初始俯仰角及第二初始航向角求解建立的误差函数的最小值来获得车体坐标系相对于IMU坐标系的第二目标俯仰角和第二目标航向角；最后，车辆110根据第二初始横滚角、第二目标俯仰角和第二目标航向角生成车体坐标系相对于IMU坐标系的安装角。车辆110在获得安装角之后，会将智能手机112中的IMU采集的信号、非完整性约束、车辆110的车速观测信息以及车体坐标系相对于IMU坐标系的安装角发送至云端120，从而得到云端120返回的定位结果。
在本申请的一些实施例中，智能手机112中的IMU包括加速度计和陀螺仪，车辆110是根据IMU中的加速度计在各个轴的输出值确定出IMU坐标系相对于车体坐标系的第一初始横滚角和第一初始俯仰角的。
在本申请的一些实施例中，在确定出IMU坐标系相对于车体坐标系的第一初始横滚角和第一初始俯仰角之后，车辆110是通过固定第一初始横滚角和第一初始俯仰角不变，每次以预设角度步长进行搜索，得到第一初始航向角的。
在本申请的一些实施例中，车辆110是根据车体坐标系的速度和IMU坐标系的速度来估计出第一初始航向角、第二目标俯仰角和第二目标航向角的。
应该理解，图1中的激光雷达、GNSS传感器、IMU及智能手机的数目仅仅是示意性的。根据实现需要，可以具有任意数目的激光雷达、GNSS传感器、IMU及智能手机，例如，激光雷达、GNSS传感器、IMU及智能手机均可以为多个。
需要说明的是，图1示出的仅为本申请的一个实施例。虽然图1实施例的方案用于车载导航领域，但在本申请的其他实施例中，该方案还可以应用于各种其他领域中，例如可以应用于机器人巡航领域等；虽然在图1实施例的方案中，IMU位于智能手机上，但在本申请的其他实施例中，IMU可以位于各种设备上，也可以将IMU作为一个单独的装置安装在汽车上，IMU甚至可以直接内嵌在车辆上；虽然在图1实施例的方案中，安装角估计是在车辆上进行的，而车辆定位是在云端实现的，但在本申请的其他实施例中，安装角估计和车辆定位可以均在车辆上实现，也可以均在云端实现；虽然在图1实施例的方案中，车辆是基于激光雷达、智能手机以及GNSS传感器来定位的，但在本申请的其他实施例中，车辆还可以基于视觉传感器、超声波传感器等其他各种类型的传感器进行定位。本申请实施例对此不作任何限定，本申请的保护范围也不应因此而受到任何限制。
易于理解，本申请实施例所提供的可移动机构的定位方法一般由车载终端执行，相应地，可移动机构的定位装置一般设置于车载终端中。但是，在本申请的其它实施例中，服务器也可以与车载终端具有相似的功能，从而执行本申请实施例所提供的可移动机构的定位方案。
因此，本申请实施例可以应用于终端或服务器中。服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN、以及大数据和人工智能平台等基础云计算服务的云服务器。终端可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音箱、智能手表等，但并不局限于此。终端以及服务器可以通过有线或无线通信方式进行直接或间接地连接，本申请在此不做限制。
以下对本申请实施例的技术方案的实现细节进行详细阐述：
图2示出了根据本申请的一个实施例的可移动机构的定位方法的流程图，该可移动机构的定位方法可以由各种能够计算和处理的设备来执行，比如可以是车载终端或云服务器，车载终端包括但不限于手机、电脑、智能语音交互设备、便携式设备等。本申请实施例可应用于各种场景，包括但不限于云技术、人工智能、智慧交通、辅助驾驶等。请参照图2所示，该可移动机构的定位方法至少包括以下步骤：
在步骤210中，获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
其中，惯性测量单元位于可移动机构上。
惯性测量单元即IMU，惯性测量单元可以是一个单独的装置，也可以以一个模块的形式内嵌在智能手机等设备的内部。可移动机构可以是车辆、无人车、机器人等各种类型的能够安装惯性测量单元的载体。当可移动机构为车辆时，可移动机构的机构坐标系即为车辆的车体坐标系。下面，将以车辆为例，介绍本申请实施例的方案。
图3示出了根据本申请的一个实施例的IMU坐标系和车体坐标系的关系示意图。请参见图3所示，其示出了两个坐标系的俯视图，这两个坐标系分别为b系和v系，其中，b系为IMU坐标系(Body Frame)的简称，v系为车体坐标系(Vehicle Frame)的简称。两个坐标系之间的关系在图3中示出，可以看到两者之间存在一定的旋转角度。b系和v系均包括X轴、Y轴和Z轴。在v系中，以车辆的前进方向作为X轴，Z轴垂直车体向下，Y轴向右与X轴和Z轴构成右手坐标系。v系下的速度即车辆的速度，根据非完整性约束可知，v系下的速度很特殊，可表示为vv＝[vvx 0 0]T，其中vvx为v系在x轴方向上的速度；b系下的速度是根据IMU的输出值确定出来的，b系下的速度可以表示为vb＝[vbx vby vbz]T，其中，vbx为b系在x轴方向上的速度，vby为b系在y轴方向上的速度，vbz为b系在z轴方向上的速度。
两个坐标系间的旋转关系用两套欧拉角表示：从b系到v系的旋转欧拉角分别为(横滚角)、/>(俯仰角)、/>(航向角)，对应旋转矩阵/>从v系到b系的旋转欧拉角分别为/>(横滚角)、/>(俯仰角)、/>(航向角)，对应旋转矩阵/>
欧拉角和旋转矩阵的转化关系如下面的两个公式所示：
其中，C为旋转矩阵，φ为横滚角，θ为俯仰角，ψ为航向角，C32表示旋转矩阵的第三行第二列，cθ＝cosθ，sθ＝sinθ，其他以此类推。
图4示出了根据本申请的一个实施例的图2实施例中步骤210的细节的流程图。请参见图4所示，图2实施例中的步骤210具体可以包括以下步骤：
在步骤210′中，根据惯性测量单元中的加速度计在各个轴的输出值确定惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
IMU包括三轴加速度计，因此，三轴加速度计可以输出与每个轴对应的输出值。
本申请的发明人发现，由于重力存在，加速度计所感知的重力方向在各个轴的投影可以反映出当前IMU相对于地理坐标系的横滚角和俯仰角，而车辆正常在道路上行驶时，可以认为在横滚和俯仰这两个维度上，车体坐标系与地理坐标系大概一致，因而，可以根据加速度计在各个轴的输出值确定IMU坐标系相对于车体坐标系的第一初始横滚角和第一初始俯仰角。
图5示出了根据本申请的一个实施例的根据惯性测量单元中的加速度计在各个轴的输出值确定第一初始横滚角和第一初始俯仰角的流程图。请参见图5所示，可以包括以下步骤：
在步骤510中，获取惯性测量单元中的加速度计在预定时间段内在每个轴的多个输出值。
加速度计可以以一定频率产生与每个轴对应的输出值，从而可以得到预定时间段内与每个轴对应的多个输出值。例如，预定时间段为10秒，每一秒可以得到与各轴分别对应的一个输出值，那么，在10秒内可以得到与各轴分别对应的10个输出值。
在步骤520中，确定加速度计在每个轴的多个输出值的平均值。
由于每个轴对应多个输出值，因此可以确定出与每个轴对应的多个输出值的平均值。
在步骤530中，根据在每个轴的多个输出值的平均值确定惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
具体地，可以根据在每个轴的多个输出值的平均值并利用如下公式来确定出第一初始横滚角和第一初始俯仰角：
其中，Fx为加速度计在x轴的输出值的平均值，Fy为加速度计在y轴的输出值的平均值，Fz为加速度计在z轴的输出值的平均值，为b系相对于v系的第一初始横滚角，/>为为b系相对于v系的第一初始俯仰角。
在本申请实施例中，通过根据各个轴的多个输出值的平均值来确定第一初始横滚角和第一初始俯仰角，可以消除偶然因素的影响，提高确定出的第一初始横滚角和第一初始俯仰角和准确性，从而可以提高安装角估计的效率。
在本申请的一个实施例中，在预定时间段内，惯性测量单元中的加速度计在每个轴的多个输出值的方差小于预定方差阈值。
在本申请的一个实施例中，在预定时间段内，惯性测量单元中的加速度计在每个轴的多个输出值的最大差值小于预定差值阈值。
多个输出值的方差或者多个输出值的最大差值反映了每个轴的多个输出值的稳定情况。因而，通过在这种情况下才确定第一初始横滚角和第一初始俯仰角，提高了确定第一初始横滚角和第一初始俯仰角的准确性。
在步骤220中，确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角，并对第一初始横滚角、第一初始俯仰角及第一初始航向角分别进行坐标转换，得到机构坐标系相对于惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角。
在得到b系相对于v系的第一初始横滚角和第一初始俯仰角/>之后，还需要进一步确定b系相对于v系的第一初始航向角/>
在本申请的一个实施例中，确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角，包括：随机生成惯性测量单元坐标系相对于机构坐标系的第一初始航向角。
图6示出了根据本申请的一个实施例的确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角的流程图。在图6所示实施例中，固定第一初始横滚角和第一初始俯仰角/>不变，搜索计算第一初始航向角/>请参见图6所示，可以包括以下步骤：
在步骤610中，以预设角度步长分别设置多个第一初始航向角，将每个第一初始航向角分别与第一初始横滚角和第一初始俯仰角组成初始安装角。
具体地，预设角度步长可以自由设置，例如可以设置为10度，从0度到360度以10度为预设角度步长进行设置，可以得到36个第一初始航向角将每个第一初始航向角/>与第一初始横滚角/>和第一初始俯仰角/>进行组合，得到一个初始安装角。
在步骤620中，针对每个初始安装角，根据初始安装角将惯性测量单元坐标系的速度向机构坐标系进行转换，并确定转换后结果与机构坐标系的速度的差值。
惯性测量单元坐标系的速度vb与机构坐标系的速度vv可以是同一时刻采集到的速度。
具体地，可以将每个初始安装角中的第一初始航向角第一初始横滚角/>以及第一初始俯仰角/>分别代入至上述的旋转矩阵C的计算公式中，得到/>然后根据/>将b系的速度vb向v系转换，并确定转换后结果与b系的速度vb之间的差值。
在步骤630中，根据每个初始安装角对应的差值，得到每个初始安装角对应的误差，并将对应的误差最小的初始安装角中的第一初始航向角作为惯性测量单元坐标系相对于机构坐标系的第一初始航向角。
本申请实施例中，通过先根据误差确定第一初始航向角，再进行后续的安装角估计步骤，可以提高安装角的估计效率。
在本申请的一个实施例中，确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角，包括：如果获取到与目标数目个时刻分别对应的惯性测量单元坐标系的速度和机构坐标系的速度，则确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角。
在每个时刻分别采集得到惯性测量单元坐标系的速度和机构坐标系的速度，当采集到与目标数目个时刻分别对应的惯性测量单元坐标系的速度和机构坐标系的速度的情况下，再确定惯性测量单元坐标系相对于机构坐标系的第一初始航向角。
在本申请实施例中，在采集到与目标数目个时刻对应的速度之后，可以基于这些速度确定第一初始航向角。
在本申请的一个实施例中，针对每个初始安装角，根据初始安装角将惯性测量单元坐标系的速度向机构坐标系进行转换，并确定转换后结果与机构坐标系的速度的差值，包括：
针对每个初始安装角，根据初始安装角将惯性测量单元坐标系在每一个时刻的速度向机构坐标系进行转换，并确定每一个时刻的转换后结果与同一时刻下机构坐标系的速度的差值；
根据每个初始安装角对应的差值，得到每个初始安装角对应的误差，包括：
根据每个初始安装角与目标数目个时刻分别对应的差值，得到每个初始安装角对应的误差。
在本申请的一个实施例中，根据每个初始安装角与目标数目个时刻分别对应的差值，得到每个初始安装角对应的误差，包括：确定每个初始安装角与目标数目个时刻分别对应的差值的绝对值之和，作为每个初始安装角对应的误差。
具体地，可以利用如下公式计算出与每个初始安装角对应的误差：
其中，为通过将每个初始安装角代入至上述的旋转矩阵C的计算公式中得到的旋转矩阵，/>为第i个时刻的b系的速度，/>为第i个时刻的v系的速度。
在本申请实施例中，通过根据两个坐标系在多个时刻的速度来确定误差，进而确定第一初始航向角，可以消除偶然因素的影响，提高确定第一初始航向角的准确性。
图7示出了根据本申请的一个实施例的对第一初始横滚角、第一初始俯仰角及第一初始航向角分别进行坐标转换的流程图。请参见图7所示，具体可以包括以下步骤：
在步骤710中，根据第一初始横滚角、第一初始俯仰角及第一初始航向角确定惯性测量单元坐标系相对于机构坐标系的第一旋转矩阵。
具体地，将确定出的第一初始航向角第一初始横滚角/>以及第一初始俯仰角/>代入上述的旋转矩阵C的计算公式中，可以得到b系相对于v系的第一旋转矩阵/>
在步骤720中，确定第一旋转矩阵的转置矩阵，得到第二旋转矩阵。
通过这一公式计算出v系相对于b系的第二旋转矩阵/>
在步骤730中，基于第二旋转矩阵确定出机构坐标系相对于惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角。
基于第二旋转矩阵根据上述的计算φ、θ和ψ的公式可以计算出v系相对于b系的第二初始横滚角/>第二初始俯仰角/>第二初始航向角/>
请继续参见图2所示，在步骤230中，根据将机构坐标系的速度向惯性测量单元坐标系的转换结果与惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解误差函数的最小值，得到机构坐标系相对于惯性测量单元坐标系的第二目标俯仰角和第二目标航向角。
其中，第二初始俯仰角和第二初始航向角用于首次将机构坐标系的速度向所述惯性测量单元坐标系转换。
可以根据多个时刻分别对应的b系的速度和v系的速度来建立误差函数。具体地，误差函数可以为如下公式：
其中，为用于将v系的速度向b系进行转换的第二旋转矩阵，/>与每个第二俯仰角和第二航向角对应，/>可以通过上述的旋转矩阵C的计算公式计算得到，/>为第i个时刻的b系的速度，/>为第i个时刻的v系的速度。
针对某一个时刻下的b系的速度和v系的速度，可以根据上述的旋转矩阵C的计算公式将上述误差函数展开为如下形式：
其中，ex为误差函数在x轴方向的分量，ey为误差函数在y轴方向的分量，ez为误差函数在z轴方向的分量，vvx为v系的速度在x轴的分量，vbx为b系的速度在x轴的分量，vby为b系的速度在y轴的分量，vbz为b系的速度在z轴的分量。
通过上述误差函数的展开形式可知，误差函数与第二初始横滚角无关。这是因为由于vv只在X轴有值，因此安装角估计过程中/>是不可观的。
以迭代方式求解误差函数的过程实际上是一个一步一步最小化误差的过程，待优化变量
图8示出了根据本申请的一个实施例的以迭代方式求解误差函数的最小值的流程图。请参见图8所示，具体可以包括以下步骤：
在步骤810中，将第二初始俯仰角作为第二当前俯仰角，并将第二初始航向角作为第二当前航向角。
将第二初始俯仰角和第二初始航向角赋值给待优化变量
在步骤820中，根据第二当前俯仰角和第二当前航向角分别确定雅可比矩阵和误差函数在各个轴的误差值。
具体地，误差函数在各个轴的误差值可以通过如上的展开形式表示，可以通过如下方式求解得到雅可比矩阵：
其中，J为雅可比矩阵，vvx为v系的速度在x轴的分量，为第二当前俯仰角，/>为第二当前航向角，fx、fy、fz的取值只有1或-1两种情况，其计算方式如下：
其中，J为雅可比矩阵，vvx为v系的速度在x轴的分量，vbx为b系的速度在x轴的分量，vby为b系的速度在y轴的分量，vbz为b系的速度在z轴的分量。
在步骤830中，根据雅可比矩阵和误差函数在各个轴的误差值求解得到第二当前俯仰角和第二当前航向角的增量。
上述的雅可比矩阵是某一个时刻对应的雅可比矩阵，将所有时刻的雅可比矩阵按行拼接构成一个大的雅可比矩阵，在每一次迭代的时候，按照下式求解得到的增量：
其中，J为雅可比矩阵，ex为误差函数在x轴方向的误差值，ey为误差函数在y轴方向的误差值，ez为误差函数在z轴方向的误差值，δx为增量。
在步骤840中，基于增量对第二当前俯仰角和第二当前航向角进行更新。
可以通过如下公式对进行更新：
x＝x+δx，
其中，δx为增量，等号右侧的x为更新前的第二当前俯仰角和第二当前航向角，等号左侧的x为更新后的第二当前俯仰角和第二当前航向角。
在步骤850中，是否满足预定收敛条件。
如果不满足预定收敛条件，则基于更新后的第二当前俯仰角和第二当前航向角重新迭代执行步骤820及之后的误差函数求解步骤。
在本申请的一个实施例中，预定收敛条件为误差函数求解步骤的迭代执行次数达到预定数目次或者增量小于预定增量阈值。
当δx小于某个阈值或者迭代次数超过某个阈值时，退出迭代，将此时的x作为第二目标俯仰角和第二目标航向角。
虽然上述实施例中是基于雅可比矩阵求解误差函数的最小值的，但在本申请的其他实施例中，还可以基于Levenberg-Marquardt算法、Dogleg算法、梯度下降法等迭代优化算法来求解误差函数的最小值。
在步骤240中，根据第二初始横滚角、第二目标俯仰角和第二目标航向角生成惯性测量单元坐标系和机构坐标系之间的安装角，以基于安装角对可移动机构进行定位。
安装角即惯性测量单元坐标系相对于机构坐标系或者机构坐标系相对于惯性测量单元坐标系的差异角。
在本申请的一个实施例中，根据第二初始横滚角、第二目标俯仰角和第二目标航向角生成惯性测量单元坐标系和机构坐标系之间的安装角，包括：
将第二初始横滚角、第二目标俯仰角和第二目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角；或者
将第二初始横滚角、第二目标俯仰角和第二目标航向角转换为惯性测量单元坐标系相对于机构坐标系的第一目标横滚角、第一目标俯仰角及第一目标航向角，并将第一目标横滚角、第一目标俯仰角和第一目标航向角组合成惯性测量单元坐标系和机构坐标系之间的安装角。
将第二初始横滚角、第二目标俯仰角和第二目标航向角组合，可以得到v系相对于b系的安装角；可以根据上述的旋转矩阵C的计算公式和计算φ、θ和ψ的公式得到b系相对于v系的安装角。
在本申请的一个实施例中，可移动机构为车辆，基于安装角对可移动机构进行定位，包括：根据安装角、非完整性约束、车辆的车速信息以及惯性测量单元的输出值对车辆进行导航定位。
在获得安装角之后，可以基于安装角对车辆进行更高精度的导航定位。
图9示出了根据本申请的一个实施例的整体流程示意图。下面，结合图9进一步介绍本申请实施例的方案。请参见图9所示，首先，获取加速度计在一段时间的均值，并基于均值计算出初始和/>然后，基于b系下的三轴速度和v系下的前向速度搜索计算出初始接下来，通过以坐标系表示的方式进行欧拉角的转换，得到第二初始横滚角/>第二初始俯仰角/>第二初始航向角/>然后，固定第二初始横滚角/>迭代计算/>和/>得到第二目标俯仰角和第二目标航向角，在迭代过程中需要使用b系下的三轴速度和v系下的前向速度；最后，根据第二目标俯仰角和第二目标航向角生成安装角，并输出安装角。
综上所述，根据本申请实施例提供的可移动机构的定位方法至少能够取得以下技术效果：能够解决现有方法的不足，适用于任意安装角度，通用性强，对IMU的精度几乎没有什么要求，可以快速准确、方便地进行安装角的估计，进而可以有效提高车辆的导航定位性能。
以下介绍本申请的装置实施例，可以用于执行本申请上述实施例中的可移动机构的定位方法。对于本申请装置实施例中未披露的细节，请参照本申请上述的可移动机构的定位方法的实施例。
图10示出了根据本申请的一个实施例的可移动机构的定位装置的框图。
参照图10所示，根据本申请的一个实施例的可移动机构的定位装置1000，包括：获取单元1010、确定和转换单元1020、建立和求解单元1030以及生成单元1040。其中，获取单元1010用于获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，所述惯性测量单元位于所述可移动机构上；确定和转换单元1020，用于确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，并对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；建立和求解单元1030，用于根据将所述机构坐标系的速度向所述惯性测量单元坐标系的转换结果与所述惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解所述误差函数的最小值，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二目标俯仰角和第二目标航向角，其中，所述第二初始俯仰角和所述第二初始航向角用于首次将所述机构坐标系的速度向所述惯性测量单元坐标系转换；生成单元1040，用于根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，以基于所述安装角对所述可移动机构进行定位。
在本申请的一些实施例中，基于前述方案，确定和转换单元1020配置为：以预设角度步长分别设置多个第一初始航向角，将每个第一初始航向角分别与所述第一初始横滚角和所述第一初始俯仰角组成初始安装角；针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系的速度向所述机构坐标系进行转换，并确定转换后结果与所述机构坐标系的速度的差值；根据每个初始安装角对应的所述差值，得到每个初始安装角对应的误差，并将对应的误差最小的初始安装角中的第一初始航向角作为所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
在本申请的一些实施例中，基于前述方案，确定和转换单元1020配置为：如果获取到与目标数目个时刻分别对应的所述惯性测量单元坐标系的速度和所述机构坐标系的速度，则确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
在本申请的一些实施例中，基于前述方案，确定和转换单元1020配置为：针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系在每一个时刻的速度向所述机构坐标系进行转换，并确定每一个时刻的转换后结果与同一时刻下所述机构坐标系的速度的差值；根据每个初始安装角与目标数目个时刻分别对应的差值，得到每个初始安装角对应的误差。
在本申请的一些实施例中，基于前述方案，建立和求解单元1030配置为：将所述第二初始俯仰角作为第二当前俯仰角，并将所述第二初始航向角作为第二当前航向角；迭代执行误差函数求解步骤，直至满足预定收敛条件，所述误差函数求解步骤包括：根据所述第二当前俯仰角和所述第二当前航向角分别确定雅可比矩阵和所述误差函数在各个轴的误差值；根据所述雅可比矩阵和所述误差函数在各个轴的误差值求解得到所述第二当前俯仰角和所述第二当前航向角的增量；基于所述增量对所述第二当前俯仰角和所述第二当前航向角进行更新。
在本申请的一些实施例中，基于前述方案，确定和转换单元1020配置为：根据所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角确定所述惯性测量单元坐标系相对于所述机构坐标系的第一旋转矩阵；确定所述第一旋转矩阵的转置矩阵，得到第二旋转矩阵；基于所述第二旋转矩阵确定出所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角。
在本申请的一些实施例中，基于前述方案，获取单元1010配置为：根据惯性测量单元中的加速度计在各个轴的输出值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
在本申请的一些实施例中，基于前述方案，获取单元1010配置为：获取惯性测量单元中的加速度计在预定时间段内在每个轴的多个输出值；确定所述加速度计在每个轴的多个输出值的平均值；根据在每个轴的多个输出值的平均值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
在本申请的一些实施例中，基于前述方案，生成单元1040配置为：将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角；或者将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角转换为所述惯性测量单元坐标系相对于所述机构坐标系的第一目标横滚角、第一目标俯仰角及第一目标航向角，并将所述第一目标横滚角、所述第一目标俯仰角和所述第一目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角。
图11示出了适于用来实现本申请实施例的电子设备的计算机系统的结构示意图。
需要说明的是，图11示出的电子设备的计算机系统1100仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。
如图11所示，计算机系统1100包括中央处理单元(Central Processing Unit，CPU)1101，其可以根据存储在只读存储器(Read-Only Memory，ROM)1102中的程序或者从存储部分1108加载到随机访问存储器(Random Access Memory，RAM)1103中的程序而执行各种适当的动作和处理，例如执行上述实施例中所述的方法。在RAM 1103中，还存储有系统操作所需的各种程序和数据。CPU 1101、ROM 1102以及RAM 1103通过总线1104彼此相连。输入/输出(Input/Output，I/O)接口1105也连接至总线1104。
以下部件连接至I/O接口1105：包括键盘、鼠标等的输入部分1106；包括诸如阴极射线管(Cathode Ray Tube，CRT)、液晶显示器(Liquid Crystal Display，LCD)等以及扬声器等的输出部分1107；包括硬盘等的存储部分1108；以及包括诸如LAN(Local AreaNetwork，局域网)卡、调制解调器等的网络接口卡的通信部分1109。通信部分1109经由诸如因特网的网络执行通信处理。驱动器1110也根据需要连接至I/O接口1105。可拆卸介质1111，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器1110上，以便于从其上读出的计算机程序根据需要被安装入存储部分1108。
特别地，根据本申请的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本申请的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分1109从网络上被下载和安装，和/或从可拆卸介质1111被安装。在该计算机程序被中央处理单元(CPU)1101执行时，执行本申请的系统中限定的各种功能。
需要说明的是，本申请实施例所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、闪存、光纤、便携式紧凑磁盘只读存储器(Compact Disc Read-Only Memory，CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、有线等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本申请各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。其中，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现，所描述的单元也可以设置在处理器中。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定。
作为一方面，本申请还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该电子设备执行时，使得该电子设备实现上述实施例中所述的方法。
应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。
通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本申请实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、触控终端、或者网络设备等)执行根据本申请实施方式的方法。
可以理解的是，在本申请的具体实施方式中，涉及到与车辆相关的数据，当本申请以上实施例运用到具体产品或技术中时，需要获得用户许可或者同意，且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准。
本领域技术人员在考虑说明书及实践这里公开的实施方式后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。
应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (13)

1.一种可移动机构的定位方法，其特征在于，所述方法包括：
获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，所述惯性测量单元位于所述可移动机构上；
确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，并对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；
根据将所述机构坐标系的速度向所述惯性测量单元坐标系的转换结果与所述惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解所述误差函数的最小值，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二目标俯仰角和第二目标航向角，其中，所述第二初始俯仰角和所述第二初始航向角用于首次将所述机构坐标系的速度向所述惯性测量单元坐标系转换；
根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，以基于所述安装角对所述可移动机构进行定位。
2.根据权利要求1所述的可移动机构的定位方法，其特征在于，所述确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，包括：
以预设角度步长分别设置多个第一初始航向角，将每个第一初始航向角分别与所述第一初始横滚角和所述第一初始俯仰角组成初始安装角；
针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系的速度向所述机构坐标系进行转换，并确定转换后结果与所述机构坐标系的速度的差值；
根据每个初始安装角对应的所述差值，得到每个初始安装角对应的误差，并将对应的误差最小的初始安装角中的第一初始航向角作为所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
3.根据权利要求2所述的可移动机构的定位方法，其特征在于，所述确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，包括：
如果获取到与目标数目个时刻分别对应的所述惯性测量单元坐标系的速度和所述机构坐标系的速度，则确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角。
4.根据权利要求3所述的可移动机构的定位方法，其特征在于，所述针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系的速度向所述机构坐标系进行转换，并确定转换后结果与所述机构坐标系的速度的差值，包括：
针对每个初始安装角，根据所述初始安装角将所述惯性测量单元坐标系在每一个时刻的速度向所述机构坐标系进行转换，并确定每一个时刻的转换后结果与同一时刻下所述机构坐标系的速度的差值；
所述根据每个初始安装角对应的所述差值，得到每个初始安装角对应的误差，包括：
根据每个初始安装角与目标数目个时刻分别对应的差值，得到每个初始安装角对应的误差。
5.根据权利要求1所述的可移动机构的定位方法，其特征在于，所述以迭代方式求解所述误差函数的最小值，包括：
将所述第二初始俯仰角作为第二当前俯仰角，并将所述第二初始航向角作为第二当前航向角；
迭代执行误差函数求解步骤，直至满足预定收敛条件，所述误差函数求解步骤包括：
根据所述第二当前俯仰角和所述第二当前航向角分别确定雅可比矩阵和所述误差函数在各个轴的误差值；
根据所述雅可比矩阵和所述误差函数在各个轴的误差值求解得到所述第二当前俯仰角和所述第二当前航向角的增量；
基于所述增量对所述第二当前俯仰角和所述第二当前航向角进行更新。
6.根据权利要求1所述的可移动机构的定位方法，其特征在于，所述对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角，包括：
根据所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角确定所述惯性测量单元坐标系相对于所述机构坐标系的第一旋转矩阵；
确定所述第一旋转矩阵的转置矩阵，得到第二旋转矩阵；
基于所述第二旋转矩阵确定出所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角。
7.根据权利要求1所述的可移动机构的定位方法，其特征在于，所述获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，包括：
根据惯性测量单元中的加速度计在各个轴的输出值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
8.根据权利要求7所述的可移动机构的定位方法，其特征在于，所述根据惯性测量单元中的加速度计在各个轴的输出值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，包括：
获取惯性测量单元中的加速度计在预定时间段内在每个轴的多个输出值；
确定所述加速度计在每个轴的多个输出值的平均值；
根据在每个轴的多个输出值的平均值确定所述惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角。
9.根据权利要求1所述的可移动机构的定位方法，其特征在于，所述根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，包括：
将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角；或者
将所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角转换为所述惯性测量单元坐标系相对于所述机构坐标系的第一目标横滚角、第一目标俯仰角及第一目标航向角，并将所述第一目标横滚角、所述第一目标俯仰角和所述第一目标航向角组合成所述惯性测量单元坐标系和所述机构坐标系之间的安装角。
10.一种可移动机构的定位装置，其特征在于，所述装置包括：
获取单元，用于获取惯性测量单元的惯性测量单元坐标系相对于可移动机构的机构坐标系的第一初始横滚角和第一初始俯仰角，所述惯性测量单元位于所述可移动机构上；
确定和转换单元，用于确定所述惯性测量单元坐标系相对于所述机构坐标系的第一初始航向角，并对所述第一初始横滚角、所述第一初始俯仰角及所述第一初始航向角分别进行坐标转换，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二初始横滚角、第二初始俯仰角及第二初始航向角；
建立和求解单元，用于根据将所述机构坐标系的速度向所述惯性测量单元坐标系的转换结果与所述惯性测量单元坐标系的速度之间的差值建立误差函数，并以迭代方式求解所述误差函数的最小值，得到所述机构坐标系相对于所述惯性测量单元坐标系的第二目标俯仰角和第二目标航向角，其中，所述第二初始俯仰角和所述第二初始航向角用于首次将所述机构坐标系的速度向所述惯性测量单元坐标系转换；
生成单元，用于根据所述第二初始横滚角、所述第二目标俯仰角和所述第二目标航向角生成所述惯性测量单元坐标系和所述机构坐标系之间的安装角，以基于所述安装角对所述可移动机构进行定位。
11.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至9中任一项所述的可移动机构的定位方法。
12.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1至9中任一项所述的可移动机构的定位方法。
13.一种计算机程序产品，其特征在于，所述计算机程序产品包括计算机指令，所述计算机指令存储在计算机可读存储介质中，计算机设备的处理器从所述计算机可读存储介质读取所述计算机指令，所述处理器执行所述计算机指令，使得所述计算机设备执行如权利要求1至9中任一项所述的可移动机构的定位方法。
CN202210918064.4A 2022-08-01 2022-08-01 可移动机构的定位方法、装置、介质及电子设备 Pending CN117537807A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210918064.4A CN117537807A (zh) 2022-08-01 2022-08-01 可移动机构的定位方法、装置、介质及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210918064.4A CN117537807A (zh) 2022-08-01 2022-08-01 可移动机构的定位方法、装置、介质及电子设备

Publications (1)

Publication Number Publication Date
CN117537807A true CN117537807A (zh) 2024-02-09

Family

ID=89788607

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210918064.4A Pending CN117537807A (zh) 2022-08-01 2022-08-01 可移动机构的定位方法、装置、介质及电子设备

Country Status (1)

Country Link
CN (1) CN117537807A (zh)

Similar Documents

Publication Publication Date Title
EP3940421A1 (en) Positioning method and device based on multi-sensor fusion
JP7179110B2 (ja) 測位方法、装置、計算装置、コンピュータ可読記憶媒体及びコンピュータプログラム
CN112113574B (zh) 用于定位的方法、装置、计算设备和计算机可读存储介质
EP3920003A1 (en) Positioning method and apparatus, autonomous driving vehicle, electronic device and storage medium
CN113405545B (zh) 定位方法、装置、电子设备及计算机存储介质
CN110058288B (zh) 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN109781117B (zh) 一种组合定位方法和系统
JP2022166248A (ja) 車両の測位方法、装置及び自動運転車両
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
KR102331312B1 (ko) 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템
CN114459471B (zh) 定位信息确定方法、装置、电子设备及存储介质
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN110068323B (zh) 网络时延定位误差补偿方法、装置及电子设备
CN115617051A (zh) 车辆控制方法、装置、设备和计算机可读介质
CN115727843A (zh) 用于航位推算的轮速确定方法、装置及设备
CN111121755A (zh) 一种多传感器的融合定位方法、装置、设备及存储介质
WO2024148908A1 (zh) 定位信息处理方法、装置、设备和介质
CN106646569A (zh) 一种导航定位方法及设备
CN117537807A (zh) 可移动机构的定位方法、装置、介质及电子设备
CN115096321B (zh) 一种车载捷联惯导系统鲁棒无迹信息滤波对准方法及系统
CN115792985A (zh) 一种车辆定位方法、装置、电子设备、存储介质及车辆
CN108240807B (zh) 估计空间占据的方法
CN115032672A (zh) 一种基于定位子系统的融合定位方法及系统
CN113804194B (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN113900132B (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