CN113470342A - 一种自运动估计的方法及装置 - Google Patents

Abstract

本申请公开了一种自运动估计的方法及装置，属于传感器领域。该方法包括：获取第一传感器的第一转动速度矢量估计值ω₁；获取第二传感器的运动速度矢量估计值v和相对于参考系静止的目标的数据集，该数据集包括静止目标的位置数据；根据v、静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。该方法通过第一传感器的ω₁和第二传感器的v和相对于参考系的静止目标的数据集可以准确确定自运动的平动速度矢量并有效提高转动速度矢量的估计精度，可以应用于辅助驾驶和自动驾驶系统以及配置上述传感器的机器人、无人机、舰载以及星载等系统。

Description

一种自运动估计的方法及装置

技术领域

本申请涉及传感器领域，具体涉及一种自运动估计的方法及装置。

背景技术

先进自动驾驶辅助系统(advanced driver-assistance system，ADAS)或者自动驾驶(autonomous driving,AD)系统通常配置多种传感器，例如:雷达(radio detectionand ranging，Radar)、声纳、超声波传感器、视觉传感器(如：摄像头)等。这些传感器用于感知周边环境信息。周边环境信息包括运动目标和静止目标，运动目标如车辆、行人，静止目标如障碍物、护栏、路沿、灯杆、周围的树木和建筑物等。

相对于固定位置的传感器，配置于可移动设备上的传感器的运动将造成诸多影响。例如：相对参考系运动的目标和相对参考系静止的目标通常采用不同的方法分析和处理，对相对参考系运动的目标通常需要分类、识别和跟踪。对相对参考系静止的目标通常需要分类和识别，为自动驾驶提供额外信息，如规避障碍物、提供可行驶区域等。传感器的运动将导致无法区分相对参考系运动的目标和相对参考系静止的目标，因此，有必要估计传感器或者其平台的自运动状态，特别是其速度，以补偿上述影响。

在现有技术中，自运动估计通常通过移动设备上的惯性测量单元(inertialmeasurement unit，IMU)来测量传感器的速度矢量。然而IMU测量的速度矢量通常是基于加速度计测量的加速度得到，测量误差会随时间累积，此外，易受电磁干扰的影响。因此，如何准确地得到自运动估计的结果是目前亟待解决的问题。

发明内容

本申请实施例提供一种自运动估计的方法，用于准确确定自运动的平动速度矢量和转动速度矢量。本申请实施例还提供了相应的装置。

本申请第一方面提供一种自运动(ego-motion)估计的方法，该方法包括：获取第一传感器的第一转动速度矢量估计值ω₁；获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，该数据集包括静止目标的位置数据；根据v、静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

该第一方面中，第一传感器可以是视觉传感器或者成像传感器等。第二传感器可以是毫米波雷达、激光雷达或者超声波雷达等。第二传感器可以获取静止目标的至少一个速度分量，如径向速度分量。

自运动是传感器或者传感器所在载体或者平台系统的运动。第一装置可以是传感器所在载体或者平台系统，例如：可以是车载、机载、船/舰载、星载、自动化或者智能体系统等可移动设备平台。

该参考系可以是预定义的参照物坐标系，如大地或者星体或者地图等坐标系，或者相对于大地匀速运动的惯性坐标系；所述静止目标可以是周边环境中的物体。

第一转动速度矢量估计值ω₁的三个分量包括横摆角角速度(yaw rate)、俯仰角速度(pitch rate)和滚转角速度(roll rate)。

运动速度矢量估计值v可以是第二传感器的瞬时速度矢量的估计值。

相对于参考系静止的目标的数据集，可以是从第二传感器或者第一传感器得到的测量数据集，或者是通过通信链路(例如云端)从其它传感器得到的测量数据集。其中，数据集中可以包含一个或者多个静止目标；静止目标的位置数据可以是静止目标的直角坐标位置数据、极坐标位置数据或球坐标位置数据。需要指出的是，对于静止目标，其位置数据可以是一个位置数据，或者多个位置数据。对于一个静止目标的多个位置数据可以对应目标的不同部分，此时目标为扩展目标。

第一装置自运动的平动速度矢量估计值T包括第一装置自运动的平动速度的大小和方向信息，可以包括平动速度矢量在直角坐标系三个坐标轴上的分量的估计值。

由上述第一方面可知，通过第一传感器的第一转动速度矢量估计值ω₁、第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集可以得到第一装置的自运动的平动速度矢量估计值T和转动速度矢量估计值ω。因此，该第一方面可以更为准确的确定第一装置自运动的平动速度矢量和转动速度矢量估计值，从而提高自运动的估计精度。

在第一方面的一种可能的实现方式中，该方法还包括：获取第一传感器的平动速度矢量尺度伸缩的估计值T′；所述根据v、静止目标的位置数据和ω₁确定自运动的平动速度矢量估计值T和转动速度矢量估计值ω，包括：根据v、静止目标的位置数据、T′和ω₁确定自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

该种可能的实现方式中，平动速度矢量尺度伸缩(scaled)的估计值T′可以是归一化的平动速度矢量的估计值或者是平动速度矢量按某一比例系数缩放或者加权的估计值。

该种可能的实现方式中，根据第二传感器的v、静止目标的位置数据、第一传感器的T′和ω₁确定自运动的平动速度矢量估计值T和转动速度矢量估计值ω，可以有效提高第一装置自运动的平动速度矢量和转动速度矢量的估计精度。

在第一方面的一种可能的实现方式中，ω₁、v、T′和静止目标的位置数据是相对于一个公共坐标系的数据。

该种可能的实现方式中，公共坐标系可以是第一传感器和第二传感器所在的载体平台的坐标系，例如车载传感器可以选择所在的车体坐标系为其公共坐标系；无人机载传感器可以选择飞机坐标系为其公共坐标系；或者，公共坐标系也可以是其中一个坐标系为公共坐标系，另一个传感器的数据通过坐标转换到公共坐标系；或者，公共坐标系也可以是其它坐标系，如大地坐标系，或者所使用的地图坐标系，或者，导航系统的坐标如NEU(north-east-up，NEU)坐标系等。此处不做进一步限定。

在第一传感器和第二传感器都有各自的坐标系时，获取到的第一传感器的数据和第二传感器的数据都是在各自坐标系中数据，获取到这些数据后需要先对数据进行坐标系的转换，ω₁、v、T′和静止目标的位置数据是经过坐标系转换后的值。若第一传感器和第二传感器位于同一个坐标系时，则无需进行坐标系转换。由该种可能的实现方式可知，ω₁、v、T′和静止目标的位置数据是相对于公共坐标系的数据，可以确保第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω的准确度。

在第一方面的一种可能的实现方式中，所述T为根据v、静止目标的位置数据和ω₁确定的第一装置的第一平动速度矢量估计值T₁，所述ω为ω₁。

该种可能的实现方式中，可以理解为：根据v、静止目标的位置数据和ω₁确定第一装置的第一平动速度矢量估计值T₁，然后将T₁作为T，将ω₁作为ω。

由该种可能的实现方式可知，经过一轮计算就得到了T和ω，提高了T和ω计算的效率。

在第一方面的一种可能的实现方式中，ω为根据第一装置的第一平动速度矢量估计值T₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，其中，T₁是根据v、静止目标的位置数据和ω₁确定的；T为根据v、静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该种可能的实现方式，也可以理解为：根据v、静止目标的位置数据和ω₁确定T₁；根据T₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂，然后将T₂作为T，将ω₂作为ω。

可以理解的是，该种可能的实现方式中虽然只描述了两次迭代得到平动速度矢量和转动速度矢量的估计值的情况，但本申请中不限于两次迭代，还可以是基于上次迭代的结果进一步迭代，以n次迭代为例，对于第n次迭代，T为T_n，ω为ω_n。由该种可能的实现方式可知，通过多次迭代，可以得到更为精确的平动速度矢量估计值T和转动速度矢量估计值ω。

在第一方面的一种可能的实现方式中，ω为根据ω₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T为根据v、静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该种可能的实现方式中，可以理解为：根据ω₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂，然后将T₂作为T，将ω₂作为ω。

可以理解的是，该种可能的实现方式中虽然只描述了通过两次迭代得到转动速度矢量估计值并基于转动速度矢量估计值得到平动速度矢量估计值的情况，但本申请中不限于两次迭代得到转动速度矢量估计值，还可以是三次或者更多次迭代得到转动速度矢量估计值，以n次迭代为例，第n次迭代，T为T_n，ω为ω_n。由该种可能的实现方式可知，通过多次迭代得到的转动速度矢量的估计值ω，并基于多次迭代得到的转动的精准度更高。

在第一方面的一种可能的实现方式中，数据集包括至少两个子集，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω₁确定的；T为根据v、第一子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该种可能的实现方式，也可以理解为：根据v、第二子集中的静止目标的位置数据和ω₁确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第一子集中的静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该种可能的实现方式中，至少两个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

可以理解的是，该种可能的实现方式中在确定ω₂时也可以使用ω₁，而不需要确定出T₁。

可以理解的是，该种可能的实现方式中虽然只描述了两次迭代得到平动速度矢量和转动速度矢量的估计值的情况，但本申请中不限于两次迭代，还可以是三次或者更多次迭代得到转动速度矢量估计值，以n次迭代为例，第n次迭代，T为T_n，ω为ω_n。由该种可能的实现方式可知，使用不同的子集多次迭代输出的T和ω的精准度更高。

在第一方面的一种可能的实现方式中，数据集包括至少三个子集，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω₁确定的；T为根据v、第三子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该种可能的实现方式中，可以理解为：根据v、第二子集中的静止目标的位置数据和ω₁确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第三子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该种可能的实现方式中，至少三个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

可以理解的是，该种可能的实现方式中在确定ω₂时也可以使用ω₁，而不需要确定出T₁。

可以理解的是，该种可能的实现方式中虽然只描述了两次迭代得到平动速度矢量和转动速度矢量的估计值的情况，但本申请中不限于两次迭代，还可以是三次或者更多次迭代得到转动速度矢量估计值，以n次迭代为例，第n次迭代，T为T_n，ω为ω_n。由该种可能的实现方式可知，使用不同的子集多轮迭代输出的T和ω的精准度更高。

在第一方面的一种可能的实现方式中，T、v和ω之间满足关系式v-ω×r＝T，其中，r为数据集中静止目标的位置数据。也可以描述为：所述T基于关系式v-ω×r＝T，从v和ω以及所述数据集中静止目标的位置数据r得到。

该种可能的实现方式中，r可以是一个位置数据，也可以是两个或多个位置数据，可以是数据集中的全部位置数据，也可以是部分位置数据。v-ω×r＝T是可以变形的，例如：在该关系式中的一个、两个或多个参数前面添加系数。结合前面的可能实现方式，可以理解的是：v-ω₁×r＝T₁，v-ω₂×r＝T₂，也可以表述为v-ω_n×r＝T_n。当然，n取值不同,关系式中的r可以取的值可以相同也可以不相同。由该种可能的实现方式可知，通过该关系式可以快速确定出T。

在第一方面的一种可能的实现方式中，T、T′、v和ω之间满足关系式v-ω×r＝|T|·T′＝T，其中，r为数据集中静止目标的位置数据。也可以描述为：T基于关系式v-ω×r＝|T|·T′＝T，从v和ω以及所述数据集中静止目标的位置数据r得到。

该种可能的实现方式中，r可以是一个位置数据，也可以是两个或多个位置数据，可以是数据集中的全部位置数据，也可以是部分位置数据。v-ω×r＝|T|·T′＝T是可以变形的，例如：在该关系式中的一个、两个或多个参数前面添加系数。结合前面的可能实现方式，可以理解的是：v-ω₁×r＝|T1|·T′＝T₁，v-ω₂×r＝|T₂|·T′＝T₂，也可以表述为v-ω_n×r＝|T_n|·T′＝T_n。当然，n取值不同，关系式中的r可以取的值可以相同也可以不相同。由该种可能的实现方式可知，通过该关系式可以快速确定出T。

在第一方面的一种可能的实现方式中，T₁、v和ω₂之间满足关系式

其中，r为数据集中静止目标的位置数据。也可以描述为：ω₂基于关系式

从v和T₁以及所述数据集中静止目标的位置数据r得到。

该种可能的实现方式中，r可以是一个位置数据，也可以是两个或多个位置数据，可以是数据集中的全部位置数据，也可以是部分位置数据。关系式

是可以变形的，例如：在该关系式中的一个、两个或多个参数前面添加系数。结合前面的可能实现方式，可以理解的是：

n为大于2的整数。当然，n取值不同,关系式中的r可以取的值可以相同也可以不相同。由该种可能的实现方式可知，通过该关系式可以快速确定出ω。

在第一方面的一种可能的实现方式中，T₁、v和ω₂之间满足关系式

其中，r为第一子集中静止目标的位置数据。也可以描述为：ω₂基于关系式

从v和T₁以及所述第一子集中静止目标的位置数据r得到。

该种可能的实现方式中，r可以是第一子集中的一个位置数据，也可以是两个或多个位置数据，可以第一子集中的全部位置数据，也可以是部分位置数据。关系式

是可以变形的，例如：在该关系式中的一个、两个或多个参数前面添加系数。结合前面的可能实现方式，可以理解的是：

当然，n取值不同,关系式中的r可以取的值可以相同也可以不相同。由该种可能的实现方式可知，通过该关系式可以快速确定出ω。

在第一方面的一种可能的实现方式中，上述步骤：获取第二传感器的运动速度矢量估计值v，包括：根据静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，确定第二传感器的运动速度矢量估计值v；或者，根据静止目标相对第二传感器的三维位置矢量和径向速度v′，确定第二传感器的运动速度矢量估计值v。

可以理解的是，该种可能的实现方式可以表示为：根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v，该Λ是根据静止目标相对第二传感器的方位角θ和俯仰角

确定的，或者该Λ是根据静止目标相对第二传感器的三维位置矢量和径向速度v′确定的。

可以理解的是，该种可能的实现方式还可以表示为：获取静止目标相对第二传感器的方位角θ、俯仰角

和第二传感器相对静止目标的径向速度v′；根据θ和

确定静止目标相对第二传感器的方向余弦矢量Λ；根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v。

该种可能的实现方式中，通过静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，可以确定出三维的第二传感器的运动速度矢量估计值v。

可以理解的是，该种可能的实现方式还可以表示为：获取以第二传感器为原点的直角坐标系中第二传感器到静止目标的三个轴向的轴向距离x，y，z和第二传感器相对静止目标的径向速度v′；根据三个轴向的距离x，y，z确定静止目标相对第二传感器的方向余弦矢量Λ；根据Λ和v′，确定第二传感器的运动速度矢量估计值v。

在第一方面的一种可能的实现方式中，该方法还包括：根据第二传感器相对目标物的运动速度矢量估计值、该目标物相对述第二传感器的方向余弦矢量Λ、该目标物相对第二传感器的径向速度v′，以及速度门限值V_Thresh，确定目标物为静止目标。确定目标物为静止目标后，还可以获取该静止目标的位置数据，将该静止目标的位置数据划分到数据集中。

本申请第二方面提供一种获取速度矢量估计值的方法，该方法包括：获取静止目标相对第二传感器的方位角θ、俯仰角

和第二传感器相对静止目标的径向速度v′；根据静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，确定第二传感器的运动速度矢量估计值v。

该第二方面，通过静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，可以确定出三维的第二传感器的运动速度矢量估计值v。

在第二方面的一种可能的实现方式中，上述步骤：根据静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，确定第二传感器的运动速度矢量估计值v，包括：根据θ和

确定静止目标相对第二传感器的方向余弦矢量Λ；根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v。

本申请第三方面提供一种获取速度矢量估计值的方法，该方法包括：获取据静止目标相对第二传感器的三维位置矢量、径向距离s和径向速度v′，根据静止目标相对第二传感器的三维位置矢量、径向距离s和径向速度v′，确定第二传感器的运动速度矢量估计值v。

该第三方面，通过静止目标相对第二传感器的三维位置矢量、径向距离s和径向速度v′，可以确定出三维的第二传感器的运动速度矢量估计值v。

在第三方面的一种可能的实现方式中，上述步骤：根据静止目标相对第二传感器的三维位置矢量、径向距离s和径向速度v′，确定第二传感器的运动速度矢量估计值v，包括：根据静止目标相对第二传感器的三维位置矢量和径向距离s确定静止目标相对第二传感器的方向余弦矢量Λ；根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v。

需要说明的是，本申请实施例中的同一字符有的用了“斜体”，有的不是“斜体”，例如：ω和ω，实际上表达的都是同一个含义。

本申请第四方面提供一种自运动估计的装置，用于执行上述第一方面或第一方面的任意可能的实现方式中的方法。具体地，该自运动估计的装置包括用于执行上述第一方面或第一方面的任意可能的实现方式中的方法的模块或单元。

本申请第五方面提供一种获取速度矢量估计值的装置，用于执行上述第二方面或第二方面的任意可能的实现方式中的方法，或者用于执行上述第三方面或第三方面的任意可能的实现方式中的方法。具体地，该获取速度矢量估计值的装置包括用于执行上述第二方面或第二方面、或者第三方面或第三方面的任意可能的实现方式中的方法的模块或单元。

本申请第六方面提供一种自运动估计的装置，包括：至少一个处理器和至少一个存储器以及存储在存储器中并可在处理器上运行的计算机执行指令，当计算机执行指令被处理器执行时，处理器执行如上述第一方面或第一方面任意一种可能的实现方式的方法。

本申请第七方面提供一种获取速度矢量估计值的装置，包括：至少一个处理器和至少一个存储器以及存储在存储器中并可在处理器上运行的计算机执行指令，当计算机执行指令被处理器执行时，处理器执行如上述第二方面或第二方面的任意可能的实现方式中的方法，或者用于执行上述第三方面或第三方面的任意可能的实现方式中的方法。

本申请第八方面提供一种传感器系统，该传感器系统包括第一传感器、第二传感器和用于执行前述第一方面或第一方面任一种可能的实施方式的自运动估计的装置。

本申请第九方面提供一种传感器系统，该传感器系统包括第二传感器和用于执行前述第二方面或第二方面、或者第三方面或第三方面任一种可能的实施方式的获取速度矢量估计值的装置。

本申请第十方面提供一种承载第九方面的传感器系统的载体，该载体包括第一传感器、第二传感器和用于执行前述第一方面或第一方面任一种可能的实施方式的自运动估计的装置。该载体可以为上述第一方面的第一装置，例如：汽车、摩托车、自行车、无人机、直升机、喷气式飞机、轮船、汽艇、卫星和机器人等。

本申请第十一方面提供一种承载第十方面的传感器系统的载体，该载体包括第二传感器和用于执行前述第二方面或第二方面、或者第三方面或第三方面任一种可能的实施方式的获取速度矢量估计值的装置。

本申请第十二方面提供一种存储一个或多个计算机执行指令的计算机可读存储介质，当计算机执行指令被至少一个处理器执行时，所述至少一个处理器执行如上述第一方面或第一方面任意一种可能的实现方式的方法。

本申请第十三方面提供一种存储一个或多个计算机执行指令的计算机可读存储介质，当计算机执行指令被至少一个处理器执行时，所述至少一个处理器执行如上述第二方面或第二方面的任意可能的实现方式中的方法，或者用于执行上述第三方面或第三方面的任意可能的实现方式中的方法。

本申请第十四方面提供一种存储一个或多个计算机执行指令的计算机程序产品，当计算机执行指令被至少一个处理器执行时，所述至少一个处理器执行上述第一方面或第一方面任意一种可能实现方式的方法。

本申请第十五方面提供一种存储一个或多个计算机执行指令的计算机程序产品，当计算机执行指令被至少一个处理器执行时，所述至少一个处理器执行上述第二方面或第二方面的任意可能的实现方式中的方法，或者用于执行上述第三方面或第三方面的任意可能的实现方式中的方法。

上述第四方面和第六方面所描述的自运动估计的装置也可以是芯片，或者其他具有上述自运动估计的装置功能的组合器件、部件等。

自运动估计的装置中的可以包括通信接口，例如：输入/输出(input/output，I/O)接口，处理单元可以是处理器，例如：中央处理单元(central processing unit，CPU)。

上述第五方面和第七方面所描述的获取速度矢量估计值的装置也可以是芯片，或者其他具有上述获取速度矢量估计值的装置功能的组合器件、部件等。

其中，第四、第六、第八、第十、第十二和第十四方面或者其中任一种可能实现方式所带来的技术效果可参见第一方面或第一方面不同可能实现方式所带来的技术效果，此处不再赘述。

其中，第五、第七、第九、第十一、第十三和第十五方面或者其中任一种可能实现方式所带来的技术效果可参见第二方面、第三方面或第四方面不同可能实现方式所带来的技术效果，此处不再赘述。

本申请实施例提供的方案通过第一传感器的第一转动速度矢量估计值ω₁、第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集可以得到第一装置的自运动的平动速度矢量估计值T和转动速度矢量估计值ω。因此，该第一方面可以更为准确的确定第一装置自运动的平动速度矢量和转动速度矢量估计值，从而提高自运动的估计精度。

附图说明

图1是本申请实施例提供的一种系统架构的示意图；

图2是本申请实施例提供的自运动估计的方法的一实施例示意图；

图3是本申请实施例提供的应用场景的一示例示意图；

图4是本申请实施例提供的自运动估计的方法的另一实施例示意图；

图5是本申请实施例提供的一场景示例示意图；

图6是本申请实施例提供的一场景示例示意图；

图7是本申请实施例提供的自运动估计的装置的一实施例示意图；

图8是本申请实施例提供的自运动估计的装置的另一实施例示意图；

图9是本申请实施例提供的传感器系统的一实施例示意图。

具体实施方式

下面结合附图，对本申请的实施例进行描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。本领域普通技术人员可知，随着技术的发展和新场景的出现，本申请实施例提供的技术方案对于类似的技术问题，同样适用。

本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本申请实施例提供一种自运动(ego-motion)估计的方法，用于准确确定自运动的平动速度矢量和转动速度矢量。本申请实施例还提供了相应的装置。以下分别进行详细说明。

本申请实施例提供的自运动估计的方法，该方法可应用于传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如自动驾驶或者智能驾驶领域等，尤其涉及高级辅助驾驶系统(advanced driver assistance systems，ADAS)。该方法的执行主体可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的装置)、融合感知系统以及各种第一装置等。该自运动可以是传感器或者传感器所在载体或者平台系统的运动。以下不同的执行步骤可以集中式也可以分布式实现。

为了能够更好地理解本申请实施例，下面对本申请实施例可应用的系统架构进行说明。

图1是本申请实施例提供的一种系统架构的示意图。如图1所示，该系统架构中包括传感器平台。传感器平台中配置有第一传感器和第二传感器。该系统架构中还包括自运动估计的装置。其中，该自运动估计的装置可以部署于传感器平台中，即该自运动估计的装置可以与传感器平台集成于一体。或者，该自运动估计的装置可以部署于传感器平台之外，该自运动估计的装置与传感器平台通过无线网络进行通信。图1以自运动估计的装置部署于传感器平台中为例。

其中，传感器的载体或者平台可以为可移动的设备。例如，传感器平台可以为车载系统，如汽车、摩托车或者自行车等。或者，传感器平台可以为船载或者舰载平台，如船只、轮船、汽艇等。或者，传感器平台可以是机载平台，如无人机、直升机或者喷气式飞机、气球等。或者，传感器平台可以为星载平台，如卫星等，传感器平台可以为自动化或者智能体系统如机器人系统等。

传感器或者传感器所在载体或者平台相对于参考系运动，该传感器或者传感器所在载体或者平台周边环境中存在静止目标。以车载或者无人机机载传感器为例，该参考系可以是大地坐标系，或者相对于大地匀速运动的惯性坐标系。该静止目标可以是周边环境中的物体，例如护栏、道路边沿、建筑物、灯杆等。以舰载传感器为例，该静止目标也可以是水面浮标、灯塔、岸边或者岛屿建筑物等。以星载传感器为例，该静止目标可以是相对于恒星或者卫星静止或者匀速运动的参照物如飞船等。同样，智能体系统如机器人系统周边存在的静止目标可以是厂房、建筑、环境中的树木、矿石等。

第一传感器可以是视觉传感器，例如相机或者摄像头,所述第一传感器也可以成像传感器，例如红外成像传感器或者合成孔径雷达等。

第二传感器可以是毫米波雷达或者激光雷达(light detection and ranging，Lidar)，或者超声波雷达，例如声呐。第二传感器可以获取目标的至少一个速度分量。例如采用调频连续波(frequency modulated continuous wave，FMCW)信号的毫米波雷达或者激光雷达或者声呐可以得到目标相对于传感器的径向速度。

上述传感器可以对周围的目标(如：相对参考系的静止目标或相对参考系的运动目标、障碍物、建筑物等)进行测量，得到周围目标的测量数据。例如，以雷达为例，测量数据可以包括目标相对于传感器的距离，方位角和/或俯仰角以及径向速度等。

需要进一步指出的是，此处传感器的物理构成可以是一个或者多个物理传感器。例如，该一个或者多个物理传感器的中各个物理传感器可以分别测量方位角、俯仰角以及径向速度，也可以是从该一个或者多个物理传感器的测量数据导出所述方位角、俯仰角以及径向速度，此处不做限定。

自运动通常可以分解为平动(translation)和转动(rotation)。自运动估计就是要确定自运动的平动速度矢量和转动速度矢量的估计值。其中，平动速度矢量可以用2维或者3维直角坐标系坐标轴上的各个分量表示。转动速度矢量可以用转动角速度的各个分量表示，可以包括横摆角角速度(yaw rate)、俯仰角速度(pitch rate)和滚转角速度(rollrate)中的一个或者多个。

下面进一步对本申请所提供的自运动估计的方法及装置进行介绍。

请参见图2，图2是本申请实施例提供的一种自运动估计方法的流程示意图。如图2所示，本申请实施例提供的自运动估计方法的可以包括：

101、获取第一传感器的第一转动速度矢量估计值ω₁。

102、获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，该数据集包括静止目标的位置数据。

103、根据所述v、静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

具体的，第一传感器可以为视觉传感器如相机或者摄像头，也可以是成像传感器如红外传感器或者合成孔径雷达等。

以摄像头为例，第一传感器可以获取周边的目标和环境的图像或者视频信息。利用图像或者视频信息，可以得到转动角速度和/或尺度伸缩的平动速度矢量的估计值，例如，基于光流法或者结合相机的数学模型以及多视图几何方法得到，此处不做过多赘述。

具体的，第二传感器可以为毫米波雷达、激光雷达或者超声波雷达等，第二传感器可以获取目标位置以及至少一个速度分量如径向速度测量数据。利用上述测量数据可以得到第二传感器的运动速度矢量估计值v和相对于大地参考系的静止目标的数据集。例如，可以基于随机抽样一致性(Random sample consensus，RANSAC)算法得到第二传感器的运动速度矢量估计值v并确定相对于参考系静止的目标的数据。

作为另一种可选的实施例，利用第二传感器得到物体的位置数据和径向速度测量数据，可以确定来自于静止目标的测量数据集，并利用来自静止目标的数据可以确定第二传感器的瞬时速度矢量的估计值，也就是第二传感器的运动速度矢量估计值v，该方法实现步骤见其后详细描述。

相对于参考系静止的目标的数据集，可以是从第二传感器或者第一传感器得到的测量数据集，或者是通过通信链路(例如云端)从其它传感器得到的测量数据集。其中，数据集中可以包含一个或者多个静止目标；静止目标的位置数据可以是直角坐标、极坐标或球坐标的位置数据。需要指出的是，所述测量数据集可以包括来自点目标的位置数据，也可以包括来自扩展目标的位置数据。其中，一个点目标得到一个位置数据，一个扩展目标可以得到多个位置数据。扩展目标的多个位置数据中，每个位置数据可以对应扩展目标的不同部分。本申请此处不做限定。

由上述实施例可知，通过第一传感器的第一转动速度矢量估计值ω₁、第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集可以得到第一装置的自运动的平动速度矢量估计值T和转动速度矢量估计值ω。可以更为准确的确定第一装置自运动的平动速度矢量和转动速度矢量估计值，从而提高自运动的估计精度。

可选地，上述方法还可以包括：获取第一传感器的平动速度矢量尺度伸缩的估计值T′。

此时，步骤103包括：根据v、所述静止目标的位置数据、T′和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

所述平动速度矢量尺度伸缩(scaled)的估计值可以是归一化(normalized)的平动速度矢量的估计值或者是平动速度矢量按某一比例系数缩放或者加权的估计值。例如，平动速度矢量的各个分量为T_x，T_y，T_z，归一化的平动速度矢量的估计值可以为αT_x,αT_y,αT_z，其中α为归一化系数或者加权系数，α为小于等于1的正数,α可以满足关系式：例如

或者，

或者α为其它小于1的正数。需要说明的是，本申请中用于表示关系的各种公式可以涵盖公式的各种可能变形，而不仅仅限定是公式本身。

需要指出的是，该处ω₁、v、T′和静止目标的位置数据可以是相对于一个公共坐标系的数据。公共坐标系可以是第一传感器和第二传感器所在的载体平台的坐标系，以车载传感器为例，该公共坐标系可以是车体坐标系；以无人机载传感器为例，公共坐标系可以是无人机坐标系；或者，该公共坐标系也可以是其中一个传感器的坐标系；或者，该公共坐标系也可以是其它坐标系，如大地坐标系，或者所使用的地图坐标系，或者，导航系统的坐标如(north-east-up，NEU)坐标系等，此处对此不做具体限定。

需要说明的是，通常第一传感器和第二传感器都有各自的坐标系，第一传感器的平动速度矢量和/或转动速度矢量，以及第二传感器的速度矢量或者静止目标的测量数据，如果不是相对于所述公共坐标系定义，可以通过坐标变换，得到相对于公共坐标系定义的数据。本申请实施例对矢量在各个坐标系之间的转换过程不做限定。

为了便于理解上述自运动估计的方法，下面以辅助驾驶或者自动驾驶的应用场景为例，对上述过程进行介绍。

如图3所示，该辅助驾驶或者自动驾驶场景中包括本车201，及其周边的移动目标和静止目标，移动目标如目标车202，静止目标如路灯203、树木204和建筑物205,静止目标还可以包括静止障碍物如停止的车辆、道路边界如护栏等。当然，该图3只是示例，实际上本车周围可以有很多移动目标和静止物车辆，很多路灯、树木和建筑物。本车201配置第一传感器2011、第二传感器2012和自运动估计的装置2013。第一传感器2011、第二传感器2012和自运动估计的装置2013通过无线连接方式或有线连接方式连接或者集成在一起。例如，可以通过无线保真(wireless fidelity，Wi-Fi)、紫蜂协议(ZigBee)以及蓝牙或者短距离通信(NFC)等等无线方式连接或者集成，或者通过CAN(Control Area Network)总线等有线方式连接或者集成，本申请实施例对此不做限定。第一传感器2011和第二传感器2012可以分别是安装于本车前端的摄像头和毫米波雷达或者激光雷达，第一传感器2011和第二传感器2012也可以安装于本车的侧端或者后端。安装方式可以集中式安装，可以分布式安装，此处不做限定。

以摄像头为例，第一传感器2011可以得到本车201周围的目标和环境的图像或者视频信息，利用图像或者视频信息可以得到转动速度(也可以称为转动角速度)和/或尺度伸缩的平动速度矢量估计值。

以毫米波雷达或者激光雷达为例，第二传感器2012通过发射和接收毫米波或者激光信号，通过信号处理等方法获取周围环境中目标的位置数据和移动速度信息。利用上述测量数据可以得到第二传感器的运动速度矢量估计值v和相对于大地参考系的静止目标的数据集。

作为另一种可选的实施例，自运动估计的装置2013可以根据第二传感器2012得到物体的位置数据和径向速度测量数据，确定测量数据集合中的数据来自于静止目标，例如：路灯203、树木204和建筑205，或者运动目标，例如：目标车202。利用来自静止目标的数据可以确定第二传感器的瞬时速度矢量的估计值，也就是第二传感器的运动速度矢量估计值v，该方法实现步骤见其后详细描述。

自运动估计的装置2013可以根据v、静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

进一步地，基于第一传感器的平动速度矢量尺度伸缩的估计值T′，自运动估计装置2013进一步确定转动速度矢量的估计值ω。

与之类似，本申请实施例可以应用于无人机、机器人、星载或者舰载系统，此处不一一列举。

可选的，作为一种实施例，T为根据v、静止目标的位置数据和ω₁确定的第一装置的第一平动速度矢量估计值T₁；ω为ω₁。

也可以理解为，上述步骤103可以包括：根据v、静止目标的位置数据和ω₁确定第一装置的第一平动速度矢量估计值T₁，将T₁作为T；将ω₁作为ω。

可选的，作为一种实施例，步骤：根据所述v、所述静止目标的位置数据和ω₁确定所述第一装置的第一平动速度矢量估计值T₁，可以包括：基于关系式T＝v-ω×r或者T＝v+r×ω得到第一平动速度矢量估计值T₁，其中ω×r表示ω与r的矢量叉积(crossproduct)，r×ω表示r与ω的矢量叉积(cross product),r表示静止目标的位置矢量。

具体地，T₁可以根据数据集中的一个静止目标的位置数据得到，如：T₁＝v-ω₁×r_i,或者T₁＝v+r_i×ω₁其中，r_i＝[x_i y_i z_i]^T是第i个静止目标的位置矢量。

具体地，T₁也可以根据数据集中的M个静止目标的位置数据得到，如T₁＝v-ω₁×r,或者T₁＝v+r×ω₁，其中

是第i_k个静止目标的位置矢量,M≥1。

具体地，T₁基于M个静止目标的位置数据得到，也可以是根据最小二乘法或者最小均方误差(minimum mean-squared error,MMSE)准则得到,例如:T₁满足关系式T₁＝v-ω₁×r或者T₁＝v+r×ω₁,其中

是第i_k个静止目标的位置矢量,M≥1。

其中，

为第i_k个静止目标的位置矢量测量误差的协方差。从传感器的测量精度得到位置误差协方差可以采用现有技术，此处不进一步赘述。

上述实施例描述的方案中，T、v和ω之间满足关系式v-ω×r＝T，或者满足关系式v+r×ω＝T,其中，r为数据集中静止目标的位置数据。也可以描述为：T基于关系式v-ω×r＝T，或者基于关系式v+r×ω＝T，从v和ω以及所述数据集中静止目标的位置数据r得到。

可选的，作为另一种实施例，ω为根据第一装置的第一平动速度矢量估计值T₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，其中，T₁是根据v、静止目标的位置数据和ω₁确定的；T为根据v、静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据v、静止目标的位置数据和ω₁确定T₁；根据T₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂；将T₂作为T，将ω₂作为ω。

其中，关于根据v、静止目标的位置数据和ω₁确定T₁的过程可以参阅前述描述进行理解，此处不再重复。

根据T₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂，可以根据关系式v-ω×r＝T或者v+r×ω＝T，或者等价地,

来确定。

通过关系式

确定ω₂可以包括通过一个静止目标的位置数据来确定或者通过多个静止目标的位置数据来确定，下面分别进行描述。

具体地，第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的一个静止目标的位置数据，通过ω₂满足的关系式确定出：

其中

r_i＝[x_i y_i z_i]^T是第i个静止目标的位置矢量，其中，[x_i y_i z_i]^T表示第i个静止目标的位置矢量的转置。

或者第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的(以N个为例，N≥1，且N为整数)静止目标的位置数据，通过ω₂满足的关系式确定出

其中

是第i_k个静止目标的位置矢量，其中，

表示第i_k个静止目标的位置矢量的转置。

步骤：根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂,与前面所描述的根据v、静止目标的位置数据和ω₁确定第一装置的第一平动速度矢量估计值T₁步骤类似，通过关系式T₂＝v-ω₂×r或者T₂＝v+r×ω₂来确定，此处不进一步赘述。

当然，不限于第二次就输出T和ω。可以是继续迭代第三次、第四次或者更多，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和静止目标的位置数据确定的第一装置的第n转动速度矢量估计值ω_k得到，其中，T_k-1是根据v、静止目标的位置数据和ω_k-1确定的,其中2≤k≤n；确定T为第n平动速度矢量估计值T_n，T_n通过v、静止目标的位置数据和ω_k确定的第一装置的第k平动速度矢量估计值T_k得到。

该过程也可以理解为：根据v、静止目标的位置数据和ω_k-1确定T_k-1,其中2≤k≤n；根据T_k-1、v和静止目标的位置数据确定第一装置的第n转动速度矢量估计值ω_k；根据v、静止目标的位置数据和ω_k确定第一装置的第n平动速度矢量估计值T_k将T_n作为T，将ω_n作为ω。

具体地，与前述类似，可以基于关系式T_k＝v-ω_k×r或者T_k＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据最终得到T_n。

具体地，与前述类似，可以基于关系式

利用数据集中部分或者全部静止目标的测量数据最终得到ω_n。

本申请实施例根据关系式v-ω×r＝T或者v+r×ω＝T，利用静止目标的位置数据，通过ω_k-1得到ω_k，从而可以充分利用上述关系和静止目标的数据，在ω_k-1估计精度不断提高的基础上，最终得到更为准确的ω_n；此外在更为准确的ω_n基础上利用上述关系得到更为精确的估计T_n，因此，上述多次迭代过程使得最终得到的T和ω的估计精度大大提高。

需要说明的是，上述ω₂的迭代过程不限于使用T₁这一种，还可以是直接从使用ω₁迭代得到。具体地，ω为根据ω₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T为根据v、静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据ω₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂，然后将T₂作为T，将ω₂作为ω。

其中，根据ω₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂，可以是根据关系式

来确定。

具体地，第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的一个静止目标的位置数据，根据以下关系式给出：

i＝j或者i≠j，其中[r_i]_×和r_j的含义都可以参阅前述相关描述进行理解，此处不再重复赘述。

或者第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的(以N个为例，N≥1，且N为整数)静止目标的位置数据，根据以下关系式给出：

其中

和

的含义都可以参阅前述相关描述进行理解，其中i_k，k＝1，…,N≥1与j_l,l＝1,…,M≥1,相同或者不同或者部分相同，此处不再重复赘述。

步骤根据v、静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂,与前面所描述的根据v、静止目标的位置数据和ω₁确定第一装置的第一平动速度矢量估计值T₁步骤类似，满足关系式T₂＝v-ω₂×r或者T₂＝v+r×ω₂，也就是通过这两个关系式可以来确定T₂，此处不进一步赘述。

当然，不限于两次迭代得到T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1转动速度矢量估计值ω_k-1、v和静止目标的位置数据确定的第一装置的第k转动速度矢量估计值ω_k得到，其中，ω_k-1是根据v、静止目标的位置数据和ω_k-2确定的,其中2≤k≤n；T为第n平动速度矢量估计值T_n，T_n通过v、静止目标的位置数据和ω_k确定的第一装置的第k平动速度矢量估计值T_k得到。

该过程也可以理解为：根据ω_k-1、v和静止目标的位置数据确定第一装置的第k转动速度矢量估计值ω_k,其中2≤k≤n；根据v、静止目标的位置数据和ω_k确定第一装置的第k平动速度矢量估计值T_k,2≤k≤n；将T_n作为T，将ω_n作为ω。

具体地，与前述类似，满足关系式T_k＝v-ω_k×r或者T_k＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据最终得到T_n。

同样，与前述类似，满足关系式

利用数据集中部分或者全部静止目标的测量数据最终得到ω_n。将T_n作为T，将ω_n作为ω。

通过上述迭代方案可知，通过多次迭代输出的T和ω的精准度更高。

本申请实施例根据关系式v-ω×r＝T或者v+r×ω＝T，利用静止目标的位置数据，通过ω_k-1得到ω_k，从而可以充分利用上述关系和静止目标的数据，在ω_k-1估计精度不断提高的基础上，最终得到更为准确的ω_n；此外在更为准确的ω_n基础上利用上述关系得到更为精确的估计T_n，因此，上述多次迭代过程使得最终得到的T和ω的估计精度大大提高。

以上所描述方案，在迭代过程中使用的静止目标的位置数据是数据集的全集或者子集，实际上，可以将数据集的全集划分为至少两个子集，在不同的迭代步骤，例如第k次迭代和第l次迭代，k≠l，使用不同的数据集的子集计算，从而最终得到T_n和ω_n。至少两个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

以迭代两次的情况为例，数据集中包括第一子集和第二子集，可以使用第二子集中的静止目标的位置数据确定T₁，使用第一子集中的静止目标的位置数据确定ω₂和T₂。

可选的，作为一种实施例，当数据集包括至少两个子集时，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω₁确定的；T为根据v、第一子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据v、第二子集中的静止目标的位置数据和ω₁确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第一子集中的静止目标的位置数据和ω₂确定第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该实施例中确定T₁可以满足关系式T₁＝v-ω₁×r_i或者T₁＝v+r_i×ω₁,其中，r_i是第二数据集中的静止目标的位置数据。

确定T₂可以满足关系式T₂＝v-ω₂×r_j或者T₂＝v+r_i×ω₂,其，r_j是第一数据集中的静止目标的位置数据。

确定ω₂的过程与前述过程相同，ω₂可以满足关系式

只是其中使用的r_j是第一数据集中的静止目标的位置数据。

该种可能的实施例中，确定T₂和ω₂时使用的是同一个子集中静止目标的位置数据。

当然，该种使用不同子集迭代的场景中，在确定ω₂时也可以使用ω₁，而不需要确定出T₁具体使用ω₁确定ω₂的过程可以参阅前述使用关系式

确定ω_n的过程进行理解，此处不再重复赘述。

当然，不限于两次迭代得到T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，当数据集包括n个子集时，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和第k子集中的静止目标的位置数据确定的第一装置的第k转动速度矢量估计值ω_k得到，其中，T_k-1是根据v、第k-1子集中静止目标的位置数据和ω_k-1确定的,其中2≤k≤n；T为第n平动速度矢量估计值T_n，T_n通过v、第k子集中的静止目标的位置数据和ω_k确定的第一装置的第n平动速度矢量估计值T_k得到。

该过程也可以理解为：根据v、第k-1子集中的静止目标的位置数据和ω_k-1确定T_k-1,其中2≤k≤n；根据T_k-1、v和第k子集中的静止目标的位置数据确定第一装置的第k转动速度矢量估计值ω_k；根据v、第k子集中的静止目标的位置数据和ω_k确定第一装置的第k平动速度矢量估计值T_k；将T_n作为T，将ω_n作为ω。

具体地，与前述类似，可以满足关系式T_k＝v-ω_k×r或者T_k＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据，最终得到T_n。

同样，与前述类似，可以满足关系式

利用数据集中部分或者全部静止目标的测量数据,最终得到ω_n。将T_n作为T，将ω_n作为ω。

与前述实施例的不同，本实施例在不同的迭代步骤，确定T_k和ω_k使用不同的数据子集中静止目标的位置数据，通常子集中静止目标的位置数据相互独立，使用不同的子集多次迭代可以充分利用不同静止目标的位置数据包含的信息，从而使得最终得到的T和ω的估计精度大大提高。

上述所描述的使用不同子集迭代不同次的T_n和ω_n，还可以在迭代T_n和ω_n时也使用不同的子集。该种情况下，数据集包括至少三个子集，至少三个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

以迭代两次的情况为例，数据集中包括第一子集、第二子集和第三子集，可以使用第二子集中的静止目标的位置数据确定T₁，使用第一子集中的静止目标的位置数据确定ω₂，使用第三子集中的静止目标的位置数据确定T₂。

可选的，作为一种实施例，当数据集包括至少三个子集时，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω1确定的；T为根据v、第三子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据v、第二子集中的静止目标的位置数据和ω1确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第三子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该实施例中，T₁可以满足关系式T₁＝v-ω₁×r_i或者T₁＝v+r_i×ω₁,其中，r_i是第二数据集中的静止目标的位置数据。

确定ω₂的过程与前述过程相同，ω₂可以满足关系式

只是其中使用的r_j是第一数据集中的静止目标的位置数据。

T₂可以满足关系式T₂＝v-ω₂×r_s或者T₂＝v+r_s×ω₂,其，r_s是第三数据集中的静止目标的位置数据。

该种可能的实施例中，确定T₁、T₂和ω₂时使用的都不是同一个子集中静止目标的位置数据。

当然，该种使用不同子集迭代的场景中，在确定ω₂时也可以使用ω₁，而不需要确定出T₁具体使用ω₁确定ω₂的过程可以参阅前述使用关系式

确定ω_n的过程进行理解，此处不再重复赘述。

当然，不限于两次迭代得到T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，当数据集包括2n个子集时，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和第2k-1子集中的静止目标的位置数据确定的第一装置的第n转动速度矢量估计值ω_k，其中，T_k-1是根据v、第2k-2子集中静止目标的位置数据和ω_k-1确定的,其中2≤k≤n；确定T为第n平动速度矢量估计值T_n，T_n通过v、第2k子集中的静止目标的位置数据和ω_k确定的第一装置的第k平动速度矢量估计值T_k得到。

该过程也可以理解为：根据v、第2k-2子集中的静止目标的位置数据和ω_k-1确定T_k-1,其中2≤k≤n；根据T_k-1、v和第2k-1子集中的静止目标的位置数据确定第一装置的第k转动速度矢量估计值ω_k；根据v、第2k子集中的静止目标的位置数据和ω_k确定第一装置的第k平动速度矢量估计值T_k；将T_n作为T，将ω_n作为ω。

具体地，与前述类似，T_n可以满足关系式T_n＝v-ω_n×r或者T_n＝v+r×ω_n，利用数据集中部分或者全部静止目标的测量数据得到T_n。

同样，与前述类似，ω_n可以满足关系式

利用数据集中部分或者全部静止目标的测量数据得到ω_n。

与前述实施例的不同，本实施例在确定T_k和ω_k时使用的都是不同的数据子集中静止目标的位置数据，通常子集中静止目标的位置数据相互独立，使用不同的子集多次迭代可以充分利用不同静止目标的位置数据包含的信息，从而使得最终得到的T和ω的估计精度大大提高。

请参见图4，图4是本申请实施例提供的一种自运动估计方法的另一流程示意图。如图4所示，本申请实施例还提供的自运动估计的方法的另一实施例可以包括：

301、获取第一传感器的第一转动速度矢量估计值ω₁和第一传感器的平动速度矢量尺度伸缩的估计值T′。

302、获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，该数据集包括静止目标的位置数据。

303、根据v、静止目标的位置数据、T′和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

本申请实施例中涉及到特征在前述实施例中都有介绍，可以参阅前述的内容进行理解，此处不再重复赘述。

本申请实施例提供的方案根据第二传感器的v、静止目标的位置数据、第一传感器的T′和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω，可以有效的提高第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω的准确度。

可选的，作为一种实施例，T为根据v、静止目标的位置数据、ω1和T′确定的第一装置的第一平动速度矢量估计值T₁，ω为ω₁。

也可以理解为，上述步骤303可以包括：根据v、静止目标的位置数据、ω₁和T′确定的第一装置的第一平动速度矢量估计值T₁，然后将T₁作为T，将ω₁作为ω。

可选的，作为一种实施例，根据v、静止目标的位置数据、ω₁和T′确定的第一装置的第一平动速度矢量估计值T₁，可以包括：基于关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω得到第一平动速度矢量估计值T1。ω×r表示ω与r的矢量叉积(crossproduct)，r×ω表示r与ω的矢量叉积(cross product),r表示静止目标的位置矢量。

具体地，T₁可以根据所述数据集中的一个静止目标的位置数据得到，如T₁满足关系式T₁＝|T₁|·T′＝v-ω₁×r_i,或者T₁＝|T₁|·T′＝v+r_i×ω₁其中，r_i＝[x_i y_i z_i]^T是第i个静止目标的位置矢量。

具体地，T₁也可以根据所述数据集中的M个静止目标的位置数据得到，如T₁满足关系式T₁＝|T₁|·T′＝v-ω₁×r,或者T₁＝|T₁|·T′＝v+r×ω₁，其中

是第i_k个静止目标的位置矢量,M≥1。

具体地，T₁基于M个静止目标的位置数据得到，也可以是根据最小二乘法或者最小均方误差(minimum mean-squared error,MMSE)准则得到,例如:T₁满足关系式T₁＝|T₁|·T′＝v-ω₁×r或者T₁＝|T₁|·T′＝v+r×ω₁,其中

是第i_k个静止目标的位置矢量,M≥1。

其中

为第i_k个静止目标的位置矢量测量误差的协方差。从传感器的测量精度得到位置误差协方差可以采用现有技术，此处不进一步赘述。

当然，关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω是可以变形的，例如：在该关系式中的一个、两个或多个参数前面添加系数。r可以是一个位置数据，也可以是从两个或多个位置数据(数据集中的全部位置数据，或者部分位置数据)得到的位置矢量，如前述。

T满足关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω，基于T′、v、ω和r，可以得到|T|；通过|T|·T′＝T可以得到T。

上述图4所对应实施例描述的方案中，T、T′、v和ω之间满足关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω，其中，r为数据集中静止目标的位置数据。也可以描述为：所述T基于关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω，从v、T′和ω以及所述数据集中静止目标的位置数据r得到。

上述计算T的过程可以是一次计算得到的，也可以是通过两次或多次迭代计算得到的。

可选的，作为一种实施例，ω为根据第一装置的第一平动速度矢量估计值T₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、静止目标的位置数据、ω₁和T′确定的；T为根据v、静止目标的位置数据ω₂和T′确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为，上述步骤303可以包括：根据v、静止目标的位置数据、ω₁和T′确定T₁；根据T₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、静止目标的位置数据、ω₂和T′确定第一装置的第二平动速度矢量估计值T₂，然后将T₂作为T，将ω₂作为ω。

其中，关于根据v、静止目标的位置数据和ω₁确定T₁的过程可以参阅前述描述进行理解，此处不再重复。

T₁、v和静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂，可以满足关系式

则可以通过该ω₂关系式来确定ω₂。

通过关系式

来确定ω₂可以包括，通过一个静止目标的位置数据来确定ω₂或者通过多个静止目标的位置数据来确定ω₂，下面分别进行描述。

具体地，第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的一个静止目标的位置数据，根据以下关系式给出：

其中

r_i＝[x_i y_i z_i]^T是第i个静止目标的位置矢量，其中，[x_i y_i z_i]^T表示第i个静止目标的位置矢量的转置。

或者第二转动速度矢量估计值ω₂可以由T₁、v和数据集中的多个(以N个为例，N≥1，且N为整数)静止目标的位置数据，通过以下关系式确定出：

其中

是第i_k个静止目标的位置矢量，其中，

表示第i_k个静止目标的位置矢量的转置。

所述根据v、静止目标的位置数据、ω₂和T′确定第一装置的第二平动速度矢量估计值T₂与如前所述的根据v、静止目标的位置数据、ω₁和T′确定T₁步骤类似，T₂满足关系式T₂＝|T₂|·T′＝v-ω₂×r,或者T₂＝|T₂|·T′＝v+r×ω₂，可以根据前述的过程进行理解，此处不重复赘述。

当然，不限于两次迭代得到T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和静止目标的位置数据确定的第一装置的第k转动速度矢量估计值ω_k，其中，T_k-1是根据v、静止目标的位置数据和ω_k-1和T′确定的,其中2≤k≤n；T为根据v、静止目标的位置数据和ω_k和T′确定的第一装置的第k平动速度矢量估计值T_k。

该过程也可以理解为：根据v、静止目标的位置数据和ω_k-1和T′确定T_k-1,其中2≤k≤n；根据T_k-1、v和静止目标的位置数据确定第一装置的第k转动速度矢量估计值ω_k；根据v、静止目标的位置数据和ω_k和T′确定第一装置的第k平动速度矢量估计值T_k,将T_n作为T，将ω_n作为ω。

具体地，与前述类似，T_k可以满足关系式T_k＝T_kT′＝v-ω_k×r或者T_k＝|T_k|·T′＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据最终得到T_n。

同样，与前述类似，ω_k可以满足关系式

利用数据集中部分或者全部静止目标的测量数据最终得到ω_n。

本申请实施例中，T满足关系式T＝|T|·T′＝v-ω×r，或者，T＝|T|·T′＝v+r×ω，利用静止目标的位置数据，通过ω_n-1得到T_n-1，从而可以ω_n-1估计精度提高的基础上得到更为准确的T_n-1；同时在T_n-1精度提高的基础上得到更为精确地ω_n，因此，上述多次迭代过程使得最终得到的T和ω的估计精度大大提高。

需要说明的是，上述ω_k的迭代过程不限于使用T_k-1这一种，还可以是直接从使用

迭代得到。具体过程可以参阅前述实施例中的相应描述进行理解，此处不再重复赘述。

以上所描述方案，在迭代过程中使用的静止目标的位置数据是数据集的全集或者子集，实际上，可以将数据集的全集划分为至少两个子集，在不同的迭代步骤，例如第k次迭代和第l次迭代，k≠l，使用不同的数据集的子集计算，从而最终得到T_n和ω_n。至少两个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

以迭代两次的情况为例，数据集中包括第一子集和第二子集，可以使用第二子集中的静止目标的位置数据确定T₁，使用第一子集中的静止目标的位置数据确定ω₂和T₂。

可选的，作为一种实施例，当数据集包括至少两个子集时，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据、ω₁和T′确定的；T为根据v、第一子集中的静止目标的位置数据、ω₂和T′确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据v、第二子集中的静止目标的位置数据、ω₁和T′确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第一子集中的静止目标的位置数据、ω₂和T′确定第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该实施例中，T₁可以满足关系式T₁＝|T₁|·T′＝v-ω₁×r_i或者T₁＝|T₁|·T′＝v+r_i×ω₁,其中，r_i是第二数据集中的静止目标的位置数据。

T₂可以满足关系式T₂＝|T₂|·T′＝v-ω₂×r_j或者T₂＝|T₂|·T′＝v+r_j×ω₂,其,r_j是第一数据集中的静止目标的位置数据。

确定ω₂的过程与前述过程相同，ω₂可以满足关系式

只是其中使用的r_j是第一数据集中的静止目标的位置数据。

该种可能的实施例中，确定T₂和ω₂时使用的是同一个子集中静止目标的位置数据。

当然，该种使用不同子集迭代的场景中，在确定ω₂时也可以使用ω₁，而不需要确定出T₁具体使用ω₁确定ω₂的过程可以参阅前述使用关系式

确定ω_n的过程进行理解，此处不再重复赘述。

当然，不限于第二次就输出T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，当数据集包括n个子集时，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和第k子集中的静止目标的位置数据确定的第一装置的第k转动速度矢量估计值ω_k得到，其中，T_k-1是根据v、第k-1子集中静止目标的位置数据、ω_k-1和T′确定的,其中2≤k≤n；T为第n平动速度矢量估计值T_n，T_n通过v、第k子集中的静止目标的位置数据、ω_k和T′确定的第一装置的第k平动速度矢量估计值T_k得到。

该过程也可以理解为：根据v、第k-1子集中的静止目标的位置数据、ω_k-1和T′确定T_k-1,其中2≤k≤n；根据T_k-1、v和第k子集中的静止目标的位置数据确定第一装置的第k转动速度矢量估计值ω_k；根据v、第k子集中的静止目标的位置数据、ω_k和T′确定第一装置的第k平动速度矢量估计值T_k；将T_n作为T，将ω_n作为ω。

具体地，与前述类似，T_k可以满足关系式T_k＝|T_k|·T′＝v-ω_k×r或者T_k＝|T_k|·T′＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据最终得到T_n。

同样，与前述类似，ω_k可以满足关系式

利用数据集中部分或者全部静止目标的测量数据最终得到ω_n。

与前述实施例的不同，本实施例在不同的迭代步骤，确定T_k和ω_k使用不同的数据子集中静止目标的位置数据，通常子集中静止目标的位置数据相互独立，使用不同的子集多次迭代可以充分利用不同静止目标的位置数据包含的信息，从而使得最终得到的T和ω的估计精度大大提高。

上述所描述的使用不同子集迭代不同次的T_n和ω_n，还可以在迭代T_n和ω_n时也使用不同的子集。该种情况下，数据集包括至少三个子集，至少三个子集中各子集包含的静止目标的位置数据可以没有交集，也可以有部分重叠，但不完全重叠。

以迭代两次的情况为例，数据集中包括第一子集、第二子集和第三子集，可以使用第二子集中的静止目标的位置数据确定T₁，使用第一子集中的静止目标的位置数据确定ω₂，使用第三子集中的静止目标的位置数据确定T₂。

可选的，作为一种实施例，当数据集包括至少三个子集时，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据、ω₁和T′确定的；T为根据v、第三子集中的静止目标的位置数据、ω₂和T′确定的第一装置的第二平动速度矢量估计值T₂。

该过程也可以理解为：根据v、第二子集中的静止目标的位置数据、ω₁和T′确定第一平动速度矢量估计值T₁；根据T₁、v和第一子集中的静止目标的位置数据确定第一装置的第二转动速度矢量估计值ω₂；根据v、第三子集中的静止目标的位置数据、ω₂和T′确定的第一装置的第二平动速度矢量估计值T₂；然后将T₂作为T，将ω₂作为ω。

该实施例中，T₁可以满足关系式T₁＝|T₁|·T′＝v-ω₁×r_i或者T₁＝|T₁|·T′＝v+r_i×ω₁,其中，r_i是第二数据集中的静止目标的位置数据。

确定ω₂的过程与前述过程相同，ω₂可以满足关系式

只是其中使用的r_j是第一数据集中的静止目标的位置数据。

确定T₂的过程与前述过程相同，T₂可以满足关系式T₂＝|T₂|·T′＝v-ω₂×r_s或者T₂＝|T₂|·T′＝v+r_s×ω₂,其中，r_s是第三数据集中的静止目标的位置数据。

该种可能的实施例中，确定T₁、T₂和ω₂时使用的都不是同一个子集中静止目标的位置数据。

当然，该种使用不同子集迭代的场景中，在确定ω₂时也可以使用ω₁，而不需要确定出T₁具体使用ω₁确定ω₂的过程可以参阅前述使用关系式

确定ω_n的过程进行理解，此处不再重复赘述。

当然，不限于两次迭代得到T和ω。不失去一般性，也可以通过n次迭代得到T和ω，下面用n表示迭代的次数，该示例中n≥2，且n为整数。

可选的，作为另一种实施例，当数据集包括2n个子集时，ω为第n转动速度矢量估计值ω_n,ω_n通过第一装置的第k-1平动速度矢量估计值T_k-1、v和第2k-1子集中的静止目标的位置数据确定的第一装置的第k转动速度矢量估计值ω_k得到，其中，T_k-1是根据v、第2k-2子集中静止目标的位置数据、ω_k-1和T′确定的,其中2≤k≤n；确定T为第n平动速度矢量估计值T_n，T_n通过v、第2k子集中的静止目标的位置数据、和ω_k和T′确定的第一装置的第k平动速度矢量估计值T_k得到。

该过程也可以理解为：根据v、第2k-2子集中的静止目标的位置数据、ω_k-1和T′确定T_k-1,其中2≤k≤n；

根据T_k-1、v和第2k-1子集中的静止目标的位置数据确定第一装置的第n转动速度矢量估计值ω_k,其中2≤k≤n；根据v、第2k子集中的静止目标的位置数据、ω_k和T′确定第一装置的第k平动速度矢量估计值T_k；将T_n作为T，将ω_n作为ω。

具体地，与前述类似，T_k可以满足关系式T_k＝|T_k|·T′＝v-ω_k×r或者T_k＝|T_k|·T′＝v+r×ω_k，利用数据集中部分或者全部静止目标的测量数据最终得到T_n。

同样，与前述类似，ω_k可以满足关系式

利用数据集中部分或者全部静止目标的测量数据最终得到ω_n。

与前述实施例的不同，本实施例在确定T_k和ω_k时使用的都是不同的数据子集中静止目标的位置数据，通常子集中静止目标的位置数据相互独立，使用不同的子集多次迭代可以充分利用不同静止目标的位置数据包含的信息，从而使得最终得到的T和ω的估计精度大大提高。

上述图2对应实施例中的步骤202和图4对应实施例中的步骤302可以利用例如基于随机采样一致性(RANdom SAmple Consensus，RANSAC)方法实现；也可以通过以下两种方案实现，这两种方案都可以获取速度矢量估计值。下面分别对这两种方案进行介绍。

1、第一种方案：通过方位角θ、俯仰角

和径向速度v′确定第二传感器的运动速度矢量估计值v。

该第一种方案中，如图5所示，以第二传感器为原点的三维直角坐标系，第二传感器在图5中用点O表示，静止目标在图5中用点P表示。

该第一种方案包括如下步骤：

S11：获取静止目标相对第二传感器的方位角θ、俯仰角

和第二传感器相对静止目标的径向速度v′。

S12：根据静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，确定第二传感器的运动速度矢量估计值v。

其中，步骤S12包括：根据θ和

确定静止目标相对第二传感器的方向余弦矢量Λ；根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v。

方向余弦矢量Λ包括三个维度的分量，可以表示为Λ＝[Λ_x Λ_y Λ_z]，其中，

利用静止目标对应的测量数据包括角度和径向速度,通过关系式v′＝Λv，可以利用最小二乘法或者最小均方误差估计准则确定运动速度矢量估计值v。

该第一种方案，通过静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，可以确定出三维的第二传感器的运动速度矢量估计值v。

2、第二种方案：通过三维位置矢量(x，y，z)、径向距离s和径向速度v′确定第二传感器的运动速度矢量估计值v。

该第一种方案中，如图6所示，以第二传感器为原点的三维直角坐标系，第二传感器在图6中用点O表示，静止目标在图6中用点P表示。

该第二种方案包括如下步骤：

S21、获取静止目标相对第二传感器的三维位置矢量和径向速度v′。

S22、根据静止目标相对第二传感器的三维位置矢量和径向速度v′，确定第二传感器的运动速度矢量估计值v。

其中，步骤S22包括：根据静止目标相对第二传感器的三维位置矢量(x，y，z)确定静止目标相对第二传感器的方向余弦矢量Λ；根据方向余弦矢量Λ和径向速度v′，确定第二传感器的运动速度矢量估计值v。

方向余弦矢量Λ包括三个维度的分量，可以表示为Λ＝[Λ_x Λ_y Λ_z]，其中，Λ_x＝x/s，Λ_y＝y/s，Λ_z＝z/s，其中，

利用位置分量x、y、z，可以确定Λ＝[Λ_x Λ_y Λ_z]，通过关系式v′＝Λv，基于最小二乘法或者最小均方误差准则可以确定速度矢量v。

该第二种方案，通过静止目标相对第二传感器的三维位置矢量和径向速度v′，可以确定出三维的第二传感器的运动速度矢量估计值。

需要说明的是，本申请实施例所描述的获取速度矢量估计值的方法的两种方案，可以是包含于上述自运动估计的方法，也可以是独立于上述自运动估计的方法。

另外，本申请实施例提供的自运动估计的方法还可以确定静止目标，以及生成数据集。该过程可以包括：根据第二传感器相对目标物的运动速度矢量估计值、该目标物相对述第二传感器的方向余弦矢量Λ、该目标物相对第二传感器的径向速度v′，以及速度门限值V_Thresh，确定目标物为静止目标。确定目标物为静止目标后，还可以获取该静止目标的位置数据，将该静止目标的位置数据划分到数据集中。

其中，第二传感器相对目标物的运动速度矢量估计值的分量包括[v_x v_y v_z]。

该方案可以参与如下关系式进行理解，|v_x·Λ_x+v_y·Λ_x+v_z·Λ_z+v′≤V_Thresh。

以上描述了本申请实施例提供的自运动估计的方法以及场景，下面结合附图介绍本申请实施例提供的自运动估计的装置。

如图7所示，本申请实施例提供的自运动估计的装置40的一实施例包括：

第一获取单元401，用于获取第一传感器的第一转动速度矢量估计值ω₁。

第二获取单元402，用于获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，数据集包括静止目标的位置数据。

处理单元403，用于根据第二获取单元402获取的v、静止目标的位置数据和第一获取单元401获取的ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

本申请实施例提供的方案，通过第一传感器的第一转动速度矢量估计值ω₁、第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集可以计算出第一装置的自运动的平动速度矢量估计值T转动速度矢量估计值ω。相比于通过惯性测量单元(inertial measurement unit，IMU)测量出加速度，再进行加速度累加的方式可以有效的提高第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω的准确度。

一种可能的实施例中，第一获取单元401，还用于获取第一传感器的平动速度矢量尺度伸缩的估计值T′。

处理单元403，用于根据v、静止目标的位置数据、T′和ω₁确定第一装置自运动的平动速度矢量的估计值T和转动速度矢量估计值ω。

一种可能的实施例中，ω₁、v、T′和静止目标的位置数据是相对于公共坐标系的数据。

一种可能的实施例中，T为根据v、静止目标的位置数据和ω₁确定的第一装置的第一平动速度矢量估计值T₁，ω为ω₁。

一种可能的实施例中，ω为根据第一装置的第一平动速度矢量估计值T₁、v和静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、静止目标的位置数据和ω₁确定的；T为根据v、静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

一种可能的实施例中，数据集包括至少两个子集，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω₁确定的；T为根据v、第一子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

一种可能的实施例中，数据集包括至少三个子集，ω为根据第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的第一装置的第二转动速度矢量估计值ω₂，T₁是根据v、第二子集中的静止目标的位置数据和ω₁确定的；T为根据v、第三子集中的静止目标的位置数据和ω₂确定的第一装置的第二平动速度矢量估计值T₂。

一种可能的实施例中，T、v和ω之间满足关系式v-ω×r＝T，其中，r为数据集中静止目标的位置数据。

一种可能的实施例中，T、T′、v和ω之间满足关系式v-ω×r＝|T|·T′＝T，其中，r为数据集中静止目标的位置数据。

一种可能的实施例中，T₁、v和ω₂之间满足关系式

其中，r为数据集中静止目标的位置数据。

一种可能的实施例中，T₁、v和ω₂之间满足关系式

其中，r为第一子集中静止目标的位置数据。

一种可能的实施例中，第二获取单元402，用于根据静止目标相对第二传感器的方位角θ、俯仰角

和径向速度v′，确定第二传感器的运动速度矢量估计值v；或者，根据静止目标相对第二传感器的三维位置矢量、径向距离和径向速度v′，确定第二传感器的运动速度矢量估计值v。

一种可能的实施例中，第二获取单元402，用于根据第二传感器相对地面的高度H和第二传感器到静止目标的径向距离s，以及径向速度v′，确定第二传感器的运动速度矢量估计值v。

需要说明的是，上述所描述的自运动估计的装置由于与本申请方法实施例基于同一构思，其带来的技术效果与本申请方法实施例相同，具体内容可参见本申请前述所示的方法实施例的叙述，此处不再赘述。

本申请实施例还提供一种计算机存储介质，其中，该计算机存储介质存储有程序，该程序执行包括上述方法实施例中记载的部分或全部步骤。

如图8所示，为本申请实施例的又一种自运动估计的装置的结构示意图，该自运动估计的装置可以是芯片，也可以是其他可以实现本申请功能终端设备、以及车辆、次船、飞机、卫星、机器人等设备。该自运动估计的装置可以包括：至少一个处理器(以包括两个处理器为例，可以包括处理器501和处理器502)，通信线路503、收发器504以及存储器505。

处理器501和处理器502可以是一个通用中央处理器(central processing unit，CPU)，微处理器，特定应用集成电路(application-specific integrated circuit，服务器IC)，或一个或多个用于控制本申请方案程序执行的集成电路。这些处理器中的每一个可以是一个单核(single-CPU)处理器，也可以是一个多核(multi-CPU)处理器。这里的处理器可以指一个或多个设备、电路、和/或用于处理数据(例如计算机执行指令)的处理核。

通信线路503可包括一通路，在上述组件之间传送信息。

收发器504，使用任何收发器一类的装置，用于与其他设备或通信网络通信，如以太网，无线接入网(radio access network，RAN)，无线局域网(wireless local areanetworks，WLAN)等。该收发器504也可以是收发电路或者收发信机，可以包括接收器和发送器。

存储器505可以是只读存储器(read-only memory，ROM)或可存储静态信息和指令的其他类型的静态存储设备，随机存取存储器(random access memory，RAM)或者可存储信息和指令的其他类型的动态存储设备，也可以是电可擦可编程只读存储器(electricallyerable programmable read-only memory，EEPROM)、只读光盘(compact disc read-onlymemory，CD-ROM)或其他光盘存储、光碟存储(包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等)、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。存储器可以是独立存在，通过通信线路503与处理器501和处理器502相连接。存储器505也可以和处理器501和处理器502中的至少一个集成在一起。

该自运动估计的装置也可以包括通信接口506。图8中所描述的各器件可以是通过通信线路503连接，也可以是通过其他连接方式连接，对此，本申请实施例中不做限定。

其中，存储器505用于存储执行本申请方案的计算机执行指令，并由处理器501和处理器502中的至少一个来控制执行。处理器501和处理器502用于执行存储器505中存储的计算机执行指令，从而实现本申请上述方法实施例提供的自运动估计的方法。在一些实施例中，上述存储器502用于存储计算机可执行程序代码，程序代码包括指令；当处理器501和处理器502中的至少一个执行指令时，自运动估计的装置中的处理器501和处理器502中的至少一个可以执行图7中处理单元403执行的动作，自运动估计的装置中的收发器504或通信接口506可以执行图7中第一获取单元401和第二获取单元402执行的动作，其实现原理和技术效果类似，在此不再赘述。

在具体实现中，作为一种实施例，处理器501和处理器502可以包括一个或多个CPU，例如图8中的CPU0和CPU1。

另外，如图9所示，本申请实施例还提供一种传感器系统60，该传感器系统60包括第一传感器601、第二传感器602和用于执行前述方法实施例的自运动估计的装置603。

本申请还提供了一种芯片系统，该芯片系统包括处理器，用于支持上述自运动估计的装置实现其所涉及的功能，例如，例如接收或处理上述方法实施例中所涉及的数据和/或信息。在一种可能的设计中，芯片系统还包括存储器，存储器，用于保存计算机设备必要的程序指令和数据。该芯片系统，可以由芯片构成，也可以包含芯片和其他分立器件。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。

计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本申请实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(Digital Subscriber Line，DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (26)

1.一种自运动估计的方法，其特征在于，包括：

获取第一传感器的第一转动速度矢量估计值ω₁；

获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，所述数据集包括所述静止目标的位置数据；

根据所述v、所述静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

获取第一传感器的平动速度矢量尺度伸缩的估计值T′；

所述根据所述v、所述静止目标的位置数据和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω，包括：

根据v、所述静止目标的位置数据、T′和ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

3.根据权利要求2所述的方法，其特征在于，所述ω₁、v、T′和所述静止目标的位置数据是相对于公共坐标系的数据。

4.根据权利要求1-3任一项所述的方法，其特征在于，

所述T为根据所述v、所述静止目标的位置数据和ω₁确定的所述第一装置的第一平动速度矢量估计值T₁，所述ω为所述ω₁。

5.根据权利要求1-3任一项所述的方法，其特征在于，

所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和所述静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，其中所述T₁是根据所述v、所述静止目标的位置数据和ω₁确定的；

所述T为根据所述v、所述静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

6.根据权利要求1-3任一项所述的方法，其特征在于，所述数据集包括至少两个子集，所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，所述T₁是根据所述v、第二子集中的静止目标的位置数据和ω₁确定的；

所述T为根据所述v、所述第一子集中的静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

7.根据权利要求1-3任一项所述的方法，其特征在于，所述数据集包括至少三个子集，所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，所述T₁是根据所述v、第二子集中的静止目标的位置数据和ω₁确定的；

所述T为根据所述v、第三子集中的静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

8.根据权利要求4-7任一项所述的方法，其特征在于，所述T、v和ω之间满足关系式v-ω×r＝T，其中，所述r为所述数据集中静止目标的位置数据。

9.根据权利要求2所述的方法，其特征在于，所述T、T′、v和ω之间满足关系式v-ω×r＝|T|·T′＝T，其中，所述r为所述数据集中静止目标的位置数据。

10.根据权利要求5-7任一项所述的方法，其特征在于，所述T₁、v和ω₂之间满足关系式

其中，所述r为所述数据集中静止目标的位置数据。

11.根据权利要求6或7所述的方法，其特征在于，所述T₁、v和ω₂之间满足关系式

其中，所述r为所述第一子集中静止目标的位置数据。

12.根据权利要求1-11任一项所述的方法，其特征在于，所述获取第二传感器的运动速度矢量估计值v，包括：

根据静止目标相对所述第二传感器的方位角θ、俯仰角

和径向速度v′，确定所述第二传感器的运动速度矢量估计值v；或者，根据静止目标相对所述第二传感器的三维位置矢量和径向速度v′，确定所述第二传感器的运动速度矢量估计值v。

13.一种自运动估计的装置，其特征在于，包括：

第一获取单元，用于获取第一传感器的第一转动速度矢量估计值ω₁；

第二获取单元，用于获取第二传感器的运动速度矢量估计值v和相对于参考系的静止目标的数据集，所述数据集包括所述静止目标的位置数据；

处理单元，用于根据所述第二获取单元获取的v、所述静止目标的位置数据和所述第一获取单元获取的ω₁确定第一装置自运动的平动速度矢量估计值T和转动速度矢量估计值ω。

14.根据权利要求13所述的装置，其特征在于，

所述第一获取单元，还用于获取第一传感器的平动速度矢量尺度伸缩的估计值T′；

所述处理单元，用于根据v、所述静止目标的位置数据、T′和ω₁确定第一装置自运动的平动速度矢量的估计值T和转动速度矢量估计值ω。

15.根据权利要求14所述的装置，其特征在于，所述ω₁、v、T′和所述静止目标的位置数据是相对于公共坐标系的数据。

16.根据权利要求13-15任一项所述的装置，其特征在于，

所述T为根据所述v、所述静止目标的位置数据和ω₁确定的所述第一装置的第一平动速度矢量估计值T₁，所述ω为所述ω₁。

17.根据权利要求13-15任一项所述的装置，其特征在于，

所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和所述静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，所述T₁是根据所述v、所述静止目标的位置数据和ω₁确定的；

所述T为根据所述v、所述静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

18.根据权利要求13-15任一项所述的装置，其特征在于，所述数据集包括至少两个子集，

所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，所述T₁是根据所述v、第二子集中的静止目标的位置数据和ω₁确定的；

所述T为根据所述v、所述第一子集中的静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

19.根据权利要求13-15任一项所述的装置，其特征在于，所述数据集包括至少三个子集，

所述ω为根据所述第一装置的第一平动速度矢量估计值T₁、v和第一子集中的静止目标的位置数据确定的所述第一装置的第二转动速度矢量估计值ω₂，所述T₁是根据所述v、第二子集中的静止目标的位置数据和ω₁确定的；

所述T为根据所述v、第三子集中的静止目标的位置数据和所述ω₂确定的所述第一装置的第二平动速度矢量估计值T₂。

20.根据权利要求13-19任一项所述的装置，其特征在于，所述T、v和ω之间满足关系式v-ω×r＝T，其中，所述r为所述数据集中静止目标的位置数据。

21.根据权利要求14所述的装置，其特征在于，所述T、T′、v和ω之间满足关系式v-ω×r＝|T|·T′＝T，其中，所述r为所述数据集中静止目标的位置数据。

22.根据权利要求17-19任一项所述的装置，其特征在于，所述T₁、v和ω₂之间满足关系式

其中，所述r为所述数据集中静止目标的位置数据。

23.根据权利要求18或19所述的装置，其特征在于，所述T₁、v和ω₂之间满足关系式

其中，所述r为所述第一子集中静止目标的位置数据。

24.根据权利要求13-23任一项所述的装置，其特征在于，

所述第二获取单元，用于根据静止目标相对所述第二传感器的方位角θ、俯仰角

和径向速度v′，确定所述第二传感器的运动速度矢量估计值v；或者，根据静止目标相对所述第二传感器的三维位置矢量和径向速度v′，确定所述第二传感器的运动速度矢量估计值v。

25.一种自运动估计的装置，其特征在于，包括：至少一个处理器和至少一个存储器，所述至少一个存储器用于存储程序或者数据；

所述至少一个处理器调用所述程序或者数据，以使得所述装置实现上述权利要求1-12中任一项所述的方法。

26.一种计算机可读存储介质，其特征在于，所述计算机存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被至少一个处理器执行时使所述处理器执行如权利要求1至12中任一项所述的方法。

Images