CN117635708A - 一种位姿估计方法、装置、存储介质及电子设备 - Google Patents
一种位姿估计方法、装置、存储介质及电子设备 Download PDFInfo
- Publication number
- CN117635708A CN117635708A CN202311507930.1A CN202311507930A CN117635708A CN 117635708 A CN117635708 A CN 117635708A CN 202311507930 A CN202311507930 A CN 202311507930A CN 117635708 A CN117635708 A CN 117635708A
- Authority
- CN
- China
- Prior art keywords
- target object
- pose
- information
- model
- sensor
- 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
- 238000000034 method Methods 0.000 title claims abstract description 57
- 239000013598 vector Substances 0.000 claims abstract description 57
- 238000004590 computer program Methods 0.000 claims description 16
- 238000013178 mathematical model Methods 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 abstract description 10
- 230000015556 catabolic process Effects 0.000 abstract description 3
- 238000006731 degradation reaction Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 9
- 230000006872 improvement Effects 0.000 description 8
- 239000011159 matrix material Substances 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 229920001296 polysiloxane Polymers 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000010979 ruby Substances 0.000 description 1
- 229910001750 ruby Inorganic materials 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本说明书公开了一种位姿估计方法、装置、存储介质及电子设备。可以利用扰动向量表示目标物体位姿估计过程中的不确定性，而不依赖于特定的基准参考坐标系，避免了过度参数化的问题，并有效避免奇异解和退化情况的发生，从而可以提升估计出的三维空间中目标物体的位置、姿态等信息的准确率。
Description
技术领域
本说明书涉及计算机技术领域，尤其涉及一种位姿估计方法、装置、存储介质及电子设备。
背景技术
目前，在计算机视觉、机器人等领域中，由于传感器采集到的目标物体的各观测数据中通常仅包含有目标物体表面上局部元素(如平面、边缘、顶点等)在空间中的位置和姿态等几何特征信息，所以，为了保障估计出的目标物体的位置信息以及姿态信息的准确性，通常可以通过融合来自同一个传感器采集到的不同视角下目标物体的观测数据或不同传感器采集到的目标物体的观测数据，以估计三维空间中目标物体的位置、姿态等信息。
而在计算机视觉、机器人等领域的许多应用中对于通过融合各观测数据以估计三维空间中目标物体的位置、姿态等信息的准确率有着较高的要求，因此，如何提升估计出的三维空间中目标物体的位置、姿态等信息的准确率，则是一个亟待解决的问题。
发明内容
本说明书提供一种位姿估计方法、装置、存储介质及电子设备，以部分的解决现有技术存在的上述问题。
本说明书采用下述技术方案：
本说明书提供了一种位姿估计方法，所述方法包括：
获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据；
根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息；
根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态；
将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
可选地，构建所述传感器观测模型，具体包括：
获取所述目标物体的样本观测数据；
根据所述样本观测数据，构建所述目标物体对应于所述样本观测数据的对称扰动模型，作为观测扰动模型，所述观测扰动模型用于表征观测的所述目标物体在所述传感器坐标系下的位姿信息，观测的所述目标物体在所述传感器坐标系下的位姿可约束自由度信息，观测的所述目标物体对应的协方差信息；
构建预先构建的所述目标物体的对称扰动模型与所述观测扰动模型之间的关联关系的数学模型，作为所述目标物体对应的传感器观测模型。
可选地，构建所述目标物体的对称扰动模型，具体包括：
根据所述目标物体的附体坐标系相对于基准坐标系位姿差异值，确定所述目标物体的参考估计位姿以及估计误差，所述估计误差通过六维微分向量表示；
根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，并确定所述附体坐标系下所述扰动向量的协方差信息；
根据所述目标物体的扰动向量和所述估计误差之间的关联联系，对所述目标物体的参考估计位姿进行表示，得到第一表示模型；
根据所述第一表示模型、所述目标物体的扰动向量和所述估计误差之间的关联联系、所述协方差信息，得到所述目标物体的对称扰动模型。
可选地，根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，具体包括：
针对所述附体坐标系下所述目标物体的每个位姿自由度，根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，判断该位姿自由度是否能够被约束；
若否，则确定该位姿自由度在扰动向量中对应的维度的维度值为指定值；
若是，则根据该位姿自由度的值，确定该位姿自由度在扰动向量中对应的维度的维度值；
根据扰动向量中的各维度的维度值，确定所述目标物体的扰动向量。
可选地，根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，具体包括：
针对每个传感器，根据预先构建的该传感器对应的传感器观测模型以及该传感器采集的观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，作为该传感器对应的观测位姿信息；
将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，具体包括：
将所述预测位姿信息与各传感器对应的观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿。
本说明书提供了一种位姿估计装置，包括：
获取模块，用于获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据；
预测模块，用于根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息；
确定模块，用于根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态；
估计模块，用于将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
可选地，所述装置还包括：构建模块；
所述构建模块具体用于，获取所述目标物体的样本观测数据；
根据所述样本观测数据，构建所述目标物体对应于所述样本观测数据的对称扰动模型，作为观测扰动模型，所述观测扰动模型用于表征观测的所述目标物体在所述传感器坐标系下的位姿信息，观测的所述目标物体在所述传感器坐标系下的位姿可约束自由度信息，观测的所述目标物体对应的协方差信息；
构建预先构建的所述目标物体的对称扰动模型与所述观测扰动模型之间的关联关系的数学模型，作为所述目标物体对应的传感器观测模型。
可选地，所述构建模块具体用于，根据所述目标物体的附体坐标系相对于基准坐标系位姿差异值，确定所述目标物体的参考估计位姿以及估计误差，所述估计误差通过六维微分向量表示；
根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，并确定所述附体坐标系下所述扰动向量的协方差信息；
根据所述目标物体的扰动向量和所述估计误差之间的关联联系，对所述目标物体的参考估计位姿进行表示，得到第一表示模型；
根据所述第一表示模型、所述目标物体的扰动向量和所述估计误差之间的关联联系、所述协方差信息，得到所述目标物体的对称扰动模型。
本说明书提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述位姿估计方法。
本说明书提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述位姿估计方法。
本说明书采用的上述至少一个技术方案能够达到以下有益效果：
在本说明书提供的位姿估计方法中，获取指定时刻下目标物体的位姿信息作为基础位姿信息、目标物体的运动状态信息以及各传感器在指定时刻的下一时刻采集到的目标物体的观测数据，根据基础位姿信息以及运动状态信息，预测在指定时刻的下一时刻目标物体的位姿信息，作为预测位姿信息，根据预先构建的传感器观测模型以及观测数据，确定在指定时刻的下一时刻目标物体的观测位姿信息，传感器观测模型用于估计目标物体与传感器之间的相对位姿状态，将预测位姿信息与观测位姿信息进行融合，得到目标物体在指定时刻的下一时刻的估计位姿，并根据估计位姿进行任务执行。
从上述方法可以看出，可以利用扰动向量表示目标物体位姿估计过程中的不确定性，而不依赖于特定的基准参考坐标系，避免了过度参数化的问题，并有效避免奇异解和退化情况的发生，从而可以提升估计出的三维空间中目标物体的位置、姿态等信息的准确率。
附图说明
此处所说明的附图用来提供对本说明书的进一步理解，构成本说明书的一部分，本说明书的示意性实施例及其说明用于解释本说明书，并不构成对本说明书的不当限定。在附图中：
图1为本说明书中提供的一种位姿估计方法的流程示意图；
图2为本说明书中提供的不能被约束的位姿自由度的示意图；
图3为本说明书提供的另一种位姿估计装置的示意图；
图4为本说明书提供的一种对应于图1的电子设备示意图。
具体实施方式
为使本说明书的目的、技术方案和优点更加清楚，下面将结合本说明书具体实施例及相应的附图对本说明书技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本说明书一部分实施例，而不是全部的实施例。基于本说明书中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本说明书保护的范围。
以下结合附图，详细说明本说明书各实施例提供的技术方案。
图1为本说明书中提供的一种位姿估计方法的流程示意图，包括以下步骤：
S101：获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据。
在物体跟踪、机器人导航、视觉定位等领域中，通常需要根据各传感器采集到的目标物体的观测数据，来估计出目标物体的位姿，进而可以根据估计出的目标物体的位姿，进行任务执行，因此，确定出的目标物体的位姿的准确率，就显得尤为重要。
基于此，业务平台可以在任务执行过程中，获取指定时刻下目标物体的位姿信息作为基础位姿信息，获取指定时刻下目标物体的运动状态信息，以及，获取各传感器在指定时刻的下一时刻采集到的目标物体的观测数据。
上述的目标物体可以为刚性物体，例如：墙壁，路障等。
进一步地，业务平台可以通过卡尔曼滤波算法，基于预先构建的目标物体扰动模型以及传感器观测模型，确定指定时刻的下一时刻目标物体的估计位姿。
在本说明书中，用于实现位姿估计方法的执行主体，可以是指服务器等设置于业务平台中指定设备，也可以为笔记本电脑、台式电脑、手机等终端设备，下面仅以服务器是执行主体为例，对本说明书提供的位姿估计方法进行说明。
S102：根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息。
S103：根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态。
在本说明书中，服务器可以根据获取到的目标物体的基础位姿信息以及运动状态信息，预测在指定时刻的下一时刻目标物体的位姿信息，作为预测位姿信息，其中，上述的运行状态信息是指目标物体相对于观测者的运动状态信息。
进一步地，服务器可以根据预先构建的传感器观测模型以及观测数据，确定在指定时刻的下一时刻所述目标物体的观测位姿信息，这里的传感器观测模型用于估计目标物体与传感器之间的相对位姿状态。
其中，上述的传感器观测模型的构建方法可以为获取目标物体的样本观测数据，根据获取到的样本观测数据，构建目标物体对应于样本观测数据的对称扰动模型，作为观测扰动模型，构建预先构建的目标物体的对称扰动模型与观测扰动模型之间的关联关系的数学模型，作为目标物体对应的传感器观测模型，这里的观测扰动模型用于表征观测的目标物体在传感器坐标系下的位姿信息，观测的目标物体在传感器坐标系下的位姿可约束自由度信息，观测的目标物体对应的协方差信息。
上述内容中，目标物体的对称扰动模型的构建方法可以为，根据目标物体的附体坐标系相对于基准坐标系位姿差异值，确定目标物体的参考估计位姿以及估计误差，这里的估计误差ξE可以通过六维微分向量表示，具体可以参考如下公式：
上述公式中，表示ξE向量对应的反对称矩阵，exp(·)表示矩阵的指数映射。
进一步地，服务器可以根据附体坐标系下目标物体的位姿可约束自由度信息，确定目标物体的扰动向量，并确定附体坐标系下扰动向量之间的协方差信息(这里的协方差信息可以为扰动向量包含的各维度特征之间的协方差矩阵)，根据目标物体的扰动向量和估计误差之间的关联联系，对目标物体的参考估计位姿进行表示，得到第一表示模型，这里的目标物体的扰动向量和估计误差之间的关联联系可以参考如下公式：
ζE＝BEξE
上述公式中，BE为自绑定矩阵，BE∈Re*6，ζE为扰动向量。
进一步地，服务器可以根据第一表示模型、目标物体的扰动向量和估计误差之间的关联联系、协方差信息，得到目标物体的对称扰动模型，具体可以参考如下公式：
CE＝Cov(ζE)
上述公式中，为扰动向量的估计值，CE为协方差矩阵。
需要说明的是，服务器构建目标物体对应的观测扰动模型的方法与上述构建目标物体的对称扰动模型的方法相同，上述的目标物体对应的传感器观测模型可以参考如下公式：
上述公式中，log(·)表示矩阵对数映射，(·)∨为(·)∧的逆运算，和/>为子绑定矩阵。
上述的扰动向量的确定方法可以为，针对附体坐标系下目标物体的每个位姿自由度，根据附体坐标系下目标物体的位姿可约束自由度信息，判断该位姿自由度是否能够被约束，若否，则确定该位姿自由度在扰动向量中对应的维度的维度值为指定值，若是，则根据该位姿自由度的值，确定该位姿自由度在扰动向量中对应的维度的维度值，进而可以根据扰动向量中的各维度的维度值，确定目标物体的扰动向量，其中，上述的指定值可以为0。
需要说明的是，上述的位姿自由度可以是指目标物体可以发生位姿变换的维度，例如：向前、向后、向上、向下、俯仰、旋转等。
上述的位姿自由度是否能够被约束的判断方法可以为，判断该位姿自由度的值发生变化时，对目标物体的位姿是否造成较大影响，如图2所示。
图2为本说明书中提供的不能被约束的位姿自由度的示意图。
结合图2可以看出，若目标物体为圆柱体，则以该圆柱体的中轴线为轴进行转动时，对目标物体的位姿无明显影响，因此，上述的以该圆柱体的中轴线为轴进行转动的位姿自由度即为不能被约束的位姿自由度。
从上述内容中可以看出，可以通过扰动向量表达位姿估计过程中的不确定性。
S104：将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
进一步地，服务器可以通过卡尔曼滤波算法，将预测位姿信息与观测位姿信息进行融合得到目标物体在指定时刻的下一时刻的估计位姿，并根据估计位姿进行任务执行。
除此之外，上述的传感器往往为多个，因此，服务器可以针对每个传感器，根据预先构建的该传感器对应的传感器观测模型以及该传感器采集的观测数据，确定在指定时刻的下一时刻目标物体的观测位姿信息，作为该传感器对应的观测位姿信息，将预测位姿信息与各传感器对应的观测位姿信息进行融合，得到目标物体在所述指定时刻的下一时刻的估计位姿，进而可以根据估计位姿进行任务执行。
其中，上述的任务可以根据实际应用场景确定，例如：在机器人导航领域中，服务器可以根据估计位姿，为机器人进行路线规划，以使得机器人对目标物体进行规避。
从上述内容中可以看出，服务器可以利用扰动向量表示目标物体位姿估计过程中的不确定性，而不依赖于特定的基准参考坐标系，避免了过度参数化的问题，并有效避免奇异解和退化情况的发生，从而可以提升估计出的三维空间中目标物体的位置、姿态等信息的准确率。
以上为本说明书的一个或多个实施位姿估计方法，基于同样的思路，本说明书还提供了相应的位姿估计装置，如图3所示。
图3为本说明书提供的一种位姿估计装置的示意图，包括：
获取模块301，用于获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据；
预测模块302，用于根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息；
确定模块303，用于根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态；
估计模块304，用于将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
可选地，所述装置还包括：构建模块305；
所述构建模块305具体用于，获取所述目标物体的样本观测数据；根据所述样本观测数据，构建所述目标物体对应于所述样本观测数据的对称扰动模型，作为观测扰动模型，所述观测扰动模型用于表征观测的所述目标物体在所述传感器坐标系下的位姿信息，观测的所述目标物体在所述传感器坐标系下的位姿可约束自由度信息，观测的所述目标物体对应的协方差信息；构建预先构建的所述目标物体的对称扰动模型与所述观测扰动模型之间的关联关系的数学模型，作为所述目标物体对应的传感器观测模型。
可选地，所述构建模块305具体用于，根据所述目标物体的附体坐标系相对于基准坐标系位姿差异值，确定所述目标物体的参考估计位姿以及估计误差，所述估计误差通过六维微分向量表示；根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，并确定所述附体坐标系下所述扰动向量的协方差信息；根据所述目标物体的扰动向量和所述估计误差之间的关联联系，对所述目标物体的参考估计位姿进行表示，得到第一表示模型；根据所述第一表示模型、所述目标物体的扰动向量和所述估计误差之间的关联联系、所述协方差信息，得到所述目标物体的对称扰动模型。
可选地，所述构建模块305具体用于，针对所述附体坐标系下所述目标物体的每个位姿自由度，根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，判断该位姿自由度是否能够被约束；若否，则确定该位姿自由度在扰动向量中对应的维度的维度值为指定值；若是，则根据该位姿自由度的值，确定该位姿自由度在扰动向量中对应的维度的维度值；根据扰动向量中的各维度的维度值，确定所述目标物体的扰动向量。
可选地，所述估计模块304具体用于，针对每个传感器，根据预先构建的该传感器对应的传感器观测模型以及该传感器采集的观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，作为该传感器对应的观测位姿信息；将所述预测位姿信息与各传感器对应的观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿。
本说明书还提供了一种计算机可读存储介质，该存储介质存储有计算机程序，计算机程序可用于执行上述图1提供的一种位姿估计方法。
本说明书还提供了图4所示的一种对应于图1的电子设备的示意结构图。如图4所述，在硬件层面，该电子设备包括处理器、内部总线、网络接口、内存以及非易失性存储器，当然还可能包括其他业务所需要的硬件。处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行，以实现上述图1所述的位姿估计方法。当然，除了软件实现方式之外，本说明书并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。
对于一个技术的改进可以很明显地区分是硬件上的改进(例如，对二极管、晶体管、开关等电路结构的改进)还是软件上的改进(对于方法流程的改进)。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件(ProgrammableLogic Device,PLD)(例如现场可编程门阵列(Field Programmable Gate Array，FPGA))就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一个数字系统“集成”在一片PLD上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器(logiccompiler)”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言(Hardware DescriptionLanguage，HDL)，而HDL也并非仅有一种，而是有许多种，如ABEL(Advanced BooleanExpression Language)、AHDL(Altera Hardware Description Language)、Confluence、CUPL(Cornell University Programming Language)、HDCal、JHDL(Java HardwareDescription Language)、Lava、Lola、MyHDL、PALASM、RHDL(Ruby Hardware DescriptionLanguage)等，目前最普遍使用的是VHDL(Very-High-Speed Integrated CircuitHardware Description Language)与Verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。
控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该(微)处理器执行的计算机可读程序代码(例如软件或固件)的计算机可读介质、逻辑门、开关、专用集成电路(Application Specific Integrated Circuit，ASIC)、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本说明书时可以把各单元的功能在同一个或多个软件和/或硬件中实现。
本领域内的技术人员应明白，本说明书的实施例可提供为方法、系统、或计算机程序产品。因此，本说明书可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本说明书可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本说明书是参照根据本说明书实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
本领域技术人员应明白，本说明书的实施例可提供为方法、系统或计算机程序产品。因此，本说明书可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本说明书可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本说明书可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本说明书，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本说明书的实施例而已，并不用于限制本说明书。对于本领域技术人员来说，本说明书可以有各种更改和变化。凡在本说明书的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本说明书的权利要求范围之内。
Claims (10)
1.一种位姿估计方法，其特征在于，所述方法包括：
获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据；
根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息；
根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态；
将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
2.如权利要求1所述的方法，其特征在于，构建所述传感器观测模型，具体包括：
获取所述目标物体的样本观测数据；
根据所述样本观测数据，构建所述目标物体对应于所述样本观测数据的对称扰动模型，作为观测扰动模型，所述观测扰动模型用于表征观测的所述目标物体在所述传感器坐标系下的位姿信息，观测的所述目标物体在所述传感器坐标系下的位姿可约束自由度信息，观测的所述目标物体对应的协方差信息；
构建预先构建的所述目标物体的对称扰动模型与所述观测扰动模型之间的关联关系的数学模型，作为所述目标物体对应的传感器观测模型。
3.如权利要求2所述的方法，其特征在于，构建所述目标物体的对称扰动模型，具体包括：
根据所述目标物体的附体坐标系相对于基准坐标系位姿差异值，确定所述目标物体的参考估计位姿以及估计误差，所述估计误差通过六维微分向量表示；
根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，并确定所述附体坐标系下所述扰动向量的协方差信息；
根据所述目标物体的扰动向量和所述估计误差之间的关联联系，对所述目标物体的参考估计位姿进行表示，得到第一表示模型；
根据所述第一表示模型、所述目标物体的扰动向量和所述估计误差之间的关联联系、所述协方差信息，得到所述目标物体的对称扰动模型。
4.如权利要求3所述的方法，其特征在于，根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，具体包括：
针对所述附体坐标系下所述目标物体的每个位姿自由度，根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，判断该位姿自由度是否能够被约束；
若否，则确定该位姿自由度在扰动向量中对应的维度的维度值为指定值；
若是，则根据该位姿自由度的值，确定该位姿自由度在扰动向量中对应的维度的维度值；
根据扰动向量中的各维度的维度值，确定所述目标物体的扰动向量。
5.如权利要求1所述的方法，其特征在于，根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，具体包括：
针对每个传感器，根据预先构建的该传感器对应的传感器观测模型以及该传感器采集的观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，作为该传感器对应的观测位姿信息；
将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，具体包括：
将所述预测位姿信息与各传感器对应的观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿。
6.一种位姿估计装置，其特征在于，包括：
获取模块，用于获取指定时刻下目标物体的位姿信息作为基础位姿信息、所述目标物体的运动状态信息以及各传感器在所述指定时刻的下一时刻采集到的所述目标物体的观测数据；
预测模块，用于根据所述基础位姿信息以及所述运动状态信息，预测在所述指定时刻的下一时刻所述目标物体的位姿信息，作为预测位姿信息；
确定模块，用于根据预先构建的传感器观测模型以及所述观测数据，确定在所述指定时刻的下一时刻所述目标物体的观测位姿信息，所述传感器观测模型用于估计所述目标物体与所述传感器之间的相对位姿状态；
估计模块，用于将所述预测位姿信息与所述观测位姿信息进行融合，得到所述目标物体在所述指定时刻的下一时刻的估计位姿，并根据所述估计位姿进行任务执行。
7.如权利要求6所述的装置，其特征在于，所述装置还包括：构建模块；
所述构建模块具体用于，获取所述目标物体的样本观测数据；
根据所述样本观测数据，构建所述目标物体对应于所述样本观测数据的对称扰动模型，作为观测扰动模型，所述观测扰动模型用于表征观测的所述目标物体在所述传感器坐标系下的位姿信息，观测的所述目标物体在所述传感器坐标系下的位姿可约束自由度信息，观测的所述目标物体对应的协方差信息；
构建预先构建的所述目标物体的对称扰动模型与所述观测扰动模型之间的关联关系的数学模型，作为所述目标物体对应的传感器观测模型。
8.如权利要求7所述的装置，其特征在于，所述构建模块具体用于，根据所述目标物体的附体坐标系相对于基准坐标系位姿差异值，确定所述目标物体的参考估计位姿以及估计误差，所述估计误差通过六维微分向量表示；
根据所述附体坐标系下所述目标物体的位姿可约束自由度信息，确定所述目标物体的扰动向量，并确定所述附体坐标系下所述扰动向量的协方差信息；
根据所述目标物体的扰动向量和所述估计误差之间的关联联系，对所述目标物体的参考估计位姿进行表示，得到第一表示模型；
根据所述第一表示模型、所述目标物体的扰动向量和所述估计误差之间的关联联系、所述协方差信息，得到所述目标物体的对称扰动模型。
9.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述权利要求1～5任一项所述的方法。
10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述权利要求1～5任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311507930.1A CN117635708A (zh) | 2023-11-13 | 2023-11-13 | 一种位姿估计方法、装置、存储介质及电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311507930.1A CN117635708A (zh) | 2023-11-13 | 2023-11-13 | 一种位姿估计方法、装置、存储介质及电子设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117635708A true CN117635708A (zh) | 2024-03-01 |
Family
ID=90031318
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311507930.1A Pending CN117635708A (zh) | 2023-11-13 | 2023-11-13 | 一种位姿估计方法、装置、存储介质及电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117635708A (zh) |
-
2023
- 2023-11-13 CN CN202311507930.1A patent/CN117635708A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111797906B (zh) | 基于视觉和惯性里程定位的方法及装置 | |
| CN116309823A (zh) | 一种位姿的确定方法、装置、设备及存储介质 | |
| CN116740361B (zh) | 一种点云分割方法、装置、存储介质及电子设备 | |
| CN111288971B (zh) | 一种视觉定位方法及装置 | |
| CN117332523B (zh) | 一种基于非局域时空模型的机器人结构件优化方法及装置 | |
| CN116347623B (zh) | 一种任务调度的方法、装置、存储介质及电子设备 | |
| CN117635708A (zh) | 一种位姿估计方法、装置、存储介质及电子设备 | |
| CN112461258A (zh) | 一种参数修正的方法及装置 | |
| CN111798489B (zh) | 一种特征点跟踪方法、设备、介质及无人设备 | |
| CN112734851B (zh) | 一种位姿确定的方法以及装置 | |
| CN113048989B (zh) | 一种无人驾驶设备的定位方法及定位装置 | |
| CN115990883A (zh) | 一种机器人的控制方法及装置 | |
| CN114494381A (zh) | 模型训练及深度估计方法、装置、存储介质及电子设备 | |
| CN112712561A (zh) | 一种建图方法、装置、存储介质及电子设备 | |
| CN117870696B (zh) | 一种基于感知信息融合的路径导航方法、装置及电子设备 | |
| CN117131633B (zh) | 一种基于热传导本构模型的机器人结构件优化方法及装置 | |
| CN112393723B (zh) | 一种定位方法、设备、介质及无人设备 | |
| CN116665032A (zh) | 一种控制无人设备的方法、装置及电子设备 | |
| CN116740114B (zh) | 一种基于凸包检测的目标物边界拟合方法及装置 | |
| CN116558504B (zh) | 一种单目视觉的定位方法及装置 | |
| CN116740197B (zh) | 一种外参的标定方法、装置、存储介质及电子设备 | |
| CN118053153B (zh) | 一种点云数据的识别方法、装置、存储介质及电子设备 | |
| CN117521452B (zh) | 一种基于时空有限元模型的机器人结构件优化方法及装置 | |
| CN116051616A (zh) | 一种深度测量方法、装置、存储介质及电子设备 | |
| Trinh et al. | IMU Calibration Methods and Orientation Estimation Using Extended Kalman Filters |
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 |