一种自运动估计方法及相关装置与流程

1.本技术涉及传感器技术领域，尤其涉及一种自运动估计方法及相关装置。


背景技术：

2.先进辅助驾驶系统(advanced driver-assistance system，adas)或者自动驾驶系统(autonomous driving，ad)通常会配置多种传感器，例如相机或者摄像头等传感器，用于感知周边环境信息，包括运动目标和静止目标，例如，运动目标有车辆、行人等；静止目标有障碍物、护栏、灯杆和建筑物等等。对于运动目标和静止目标，通常采用不同的方法分析处理，如对运动目标进行分类、识别以及跟踪处理；对静止目标进行分类和识别处理，从而为无人驾驶提供额外的信息，如规避障碍物、提供可行驶区域等等。传感器通常可以安装于不同的载体平台，例如车辆、舰船、卫星、无人机以及机器人等，传感器将跟随传感器所在的载体平台运动。一方面，传感器的运动导致运动目标和静止目标无法独立分析，因此，需要估计传感器的运动实现运动目标和静止目标的分离；另一方面，运动目标的跟踪通常是基于模型的，且模型通常假定相对地面或者大地坐标系，但是传感器的运动将导致上述模型失效或者跟踪性能下降；此时，需要对传感器或者传感器载体的运动进行补偿。此外，对实际的不同场景，通过传感器运动估计实现对传感器运动平台的定位和跟踪也至关重要。
3.因此如何确定传感器以及传感器所在的载体的运动，是本领域人员正在解决的技术问题。


技术实现要素：

4.本技术实施例公开了一种自运动估计方法及相关装置，能够确定传感器和传感器所在的载体的速度，提高传感器或者传感器所在的载体自运动估计的精度。
5.本技术实施例第一方面公开了一种自运动估计方法，包括：
6.获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
7.根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
8.在上述方法中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量通常缺少一个自由度的信息，而本技术实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精
度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
9.可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵r和平移矢量r。
10.可选地，在又一种实现方式中，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：根据所述旋转矩阵r和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
11.在上述方法中，根据传感器坐标系相对于载体坐标器的外部参数，即旋转矩阵r或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
12.可选地，在又一种实现方式中，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及所述传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵r和所述第一平动速度矢量确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
13.可选地，在又一种实现方式中，所述根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te，包括：根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
14.可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，所述s为所述传感器坐标系的平动速度矢量的幅度，所述为所述归一化瞬时速度矢量，所述tr为所述瞬时速度成分。
15.可选地，在又一种实现方式中，所述方法还包括：根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量确定所述传感器自运动的第三平动速度矢量ts。
16.在上述方法中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量与现有技术相比，本技术实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
17.可选地，在又一种实现方式中，所述传感器为视觉传感器，所述方法还包括：获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
18.在上述方法中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度
矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本技术实施例能够通过利用流矢量，获得更加精确的深度z。
19.本技术实施例第二方面公开了一种自运动估计装置，包括：
20.第一获取单元，用于获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
21.第一确定单元，用于根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
22.在上述装置中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量通常缺少一个自由度的信息，因此，本技术实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
23.可选的，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵r和平移矢量r。
24.可选的，在又一种实现方式中，所述第一确定单元，用于根据所述旋转矩阵r和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
25.在上述装置中，根据传感器坐标系相对于载体坐标系的外部参数，即旋转矩阵r或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
26.可选的，在又一种实现方式中，所述第一确定单元，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵r和所述第一平动速度矢量确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
27.可选的，在又一种实现方式中，所述第一确定单元，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
28.可选的，在又一种实现方式中，所述载体的第二平动速度矢量te，所述s为所述传感器坐标系的平动速度矢量的幅度，所述为所述归一化瞬时速度矢量，所述tr为所述瞬时速度成分。
29.可选的，在又一种实现方式中，所述装置还包括：第二确定单元，用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量确定所述传感器自运动的第三平动速度矢量ts。
30.在上述装置中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量与现有技术相比，本技术实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
31.可选的，在又一种实现方式中，所述传感器为视觉传感器，所述装置还包括：第二获取单元，用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；第三确定单元，用于根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
32.在上述装置中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本技术实施例能够通过利用光流矢量，获得更加精确的深度z。
33.本技术实施例第三方面公开了一种自运动估计装置，所述装置包括至少一个处理器和通信接口，可选的，还包括存储器。所述存储器用于存储计算机程序，所述至少一个处理器调用所述计算机程序，用于执行以下操作：
34.获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
35.在上述装置中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量通常缺少一个自由度的信息，而本技术实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了自动驾驶的安全性。
36.可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵r和平移矢量r。
37.可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述旋转矩阵r和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
38.在上述装置中，根据传感器坐标系相对于载体坐标系的外部参数，即旋转矩阵r或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效
利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
39.可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵r和所述第一平动速度矢量确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
40.可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
41.可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，所述s为所述传感器坐标系的平动速度矢量的幅度，所述为所述归一化瞬时速度矢量，所述tr为所述瞬时速度成分。
42.可选地，在又一种实现方式中，所述至少一个处理器，还用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量确定所述传感器自运动的第三平动速度矢量ts。
43.可选地，在上述装置中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量与现有技术相比，本技术实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
44.可选地，在又一种实现方式中，所述传感器为视觉传感器，所述至少一个处理器，还用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
45.在上述装置中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本技术实施例能够通过利用光流矢量，获得更加精确的深度z。
46.本技术实施例第四方面公开了一种计算机产品，当所述计算机程序产品在处理器上运行时，实现任意一方面或者任意一方面的可选的方案所描述的方法。
47.本技术实施例第五方面公开了一种芯片系统，所述芯片系统包括至少一个处理器和通信接口，可选的，所述芯片系统还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述芯片系统所在的装置实现任意一方面或者任意一方面的可选的方案所描述的方法。
48.本技术实施例第六方面公开了一种计算机可读存储介质，所述计算机存储介质存储有计算机程序，所述计算机程序当被处理器执行时实现任意一方面或者任意一方面的可选的方案所描述的方法。
49.本技术实施例第七方面公开了一种终端，所述终端包含至少一个处理器和通信接口，可选的，还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述终端实现任意一方面或者任意一方面的可选的方案所描述的方法。
50.本技术实施例第八方面公开了一种传感器，所述传感器包括至少一个处理器和通信接口，可选的，所述传感器还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述传感器所在的装置实现任意一方面或者任意一方面的可选的方案所描述的方法。
附图说明
51.以下对本技术实施例用到的附图进行介绍。
52.图1是本技术实施例提供的一种自运动估计系统的结构示意图；
53.图2是本技术实施例提供的一种惯性测量单元的示意图；
54.图3是本技术实施例提供的一种自运动估计方法的流程图；
55.图4是本技术实施例提供的一种传感器坐标系的表示形式的示意图；
56.图5是本技术实施例提供的一种载体坐标系的表示形式的示意图；
57.图6是本技术实施例提供的一种自运动估计装置的结构示意图；
58.图7是本技术实施例提供的又一种自运动估计装置的结构示意图。
具体实施方式
59.下面结合本技术实施例中的附图对本技术实施例进行描述。
60.请参见图1，图1是本技术实施例提供的一种自运动估计系统的结构示意图，该系统包括传感器1001、运动测量模块1002和数据处理模块1003，其中，传感器1001可以为视觉传感器，例如，红外热成像传感器、相机或者摄像头等等。传感器1001用于提供视觉测量数据，例如图像或者视频；运动测量模块1002用于根据所述传感器数据获取运动测量数据，如传感器自运动的第一转动速度矢量ωs和归一化或尺度伸缩的第一平动速度矢量该数据处理模块1003用于处理传感器1002提供的测量数据，在一般情况下，运动测量模块1002和数据处理模块1003可以在同一个处理器中。
61.在一种示例中，传感器1001、运动测量模块1002和数据处理模块1003可以通过有线或者无线方式等完全或者不同集成在一起，例如传感器1001、运动测量模块1002和数据处理模块1003部署于一个处理器系统上；此时该自运动估计系统可以在同一个车载、同一个机载或者同一个星载或者智能体上。在又一种示例中，传感器1001在车载、机载或者星载或者智能体上，而运动测量模块1002和/或数据处理模块1003在云端，相应的，传感器1001将提供的视频数据或者参数化数据、或者运动测量模块1002提供的数据发送给在云端的数据处理模块1003，数据处理模块1003将处理后的结果发送给车载、机载或者星载或者智能体。其中，车载可以为如汽车、摩托车或者自行车等，机载可以为无人机、直升机或者喷气式飞机等等，星载可以为卫星等，智能体如机器人。
62.目前，获得运动速度的方法有多种，在一种方法中，车辆中通常安装有惯性测量单元(inertial measurement unit，imu)，如图2所示，图2表示一种惯性测量单元，imu是测量物体三轴姿态角(或角速度)以及加速度的装置。一般地，一个imu内会装有三个单轴的陀螺
仪和三个单轴的加速度计，陀螺仪用于检测载体相对于导航坐标系的角速度信号，而加速度计检测物体在载体坐标系统独立三轴的加速度信号，测量物体在三维空间中的角速度和加速度，并以此可以解算出物体运动速度和姿态。然而，在上面的方法中，物体的运动速度通常基于加速度积累得到的，但是通过加速度计测量的加速度的测量误差会随着时间累积，所以存在误差累积问题，需要利用其它传感器进行额外的校准，而且，一般用于车辆中的imu的精度太低，若选用高精度的imu，成本高昂。
63.在又一种方法中，车辆中通常安装有毫米波雷达传感器，雷达传感器通常能提供距离、方位角和径向速度等测量数据。基于静止目标的方位角和径向速度分量，可以根据最小二乘法或者其他的方式获得传感器相对于大地的瞬时速度。然而，在上述方法中，通常只能获得相对雷达传感器坐标系的两个速度分量，即横向速度和径向速度，但是无法获取第三个速度分量，此外，虽然径向速度误差比较低，但是横向速度误差比较高，难以满足系统的性能需求，而且，该方法只能获得一个转动角速度分量，即横摆角速度(yaw rate)，而无法获得纵向角速度(pitch rate)和滚动角速度(roll rate)。
64.在又一种方法中，车辆中安装相机或者摄像头，用于提供连续的图像。基于上述两帧或者多帧图像，利用光流法、特征点法或者直接优化光亮度的目标函数的方法，从而可以获得传感器相对于大地的(归一化或者尺度伸缩的)平移运动速度矢量和转动速度矢量。但是该方法存在尺度问题，且深度和平动速度的各个分量耦合在一起，通常无法获得更加准确的深度，从而导致无法获得精确的平移运动速度，或者只能获得归一化或者尺度伸缩的平移运动速度。
65.请参见图3，图3是本技术实施例提供的一种自运动估计方法，该方法的执行主体可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该方法的执行主体也可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。以下不同的执行步骤可以集中式实现，或者，以下不同的执行步骤也可以分布式实现。所述方法包括但不限于如下步骤：
66.步骤s301：获取来自传感器的传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
67.具体地，第一平动速度矢量也可以称为第一平移运动速度矢量传感器可以为视觉传感器如相机、摄像头、红外传感器或者其它成像传感器等等，本技术实施例不做限定。
68.具体地，获取传感器自运动的第一转动速度矢量ωs和第一平动速度矢量的方式可以是通过有线或者无线方式获取，例如在同一处理器模块中可以直接读取；同一硬件系统中可以通过总线如外设部件互连标准(peripheral component interconnect，pci)总线获取，或者通过网络如车内的各种类型控制器局域网络(controller area network，can)获取；或者通过无线方式如通过云端通信方式获取，当然也可以是其他的形式获取的，本技术实施例不做限定。
69.具体地，传感器自运动的第一转动速度矢量ωs和第一平动速度矢量可以根据传感器获取的数据中的特征点、线或者、平面或者区域，基于数据的光学特性或者几何特性确定，例如基于8点法或者5点法或者单应性(homography)或者光流法等方法得到，本技术
实施例不做限定。
70.例如，第一转动速度矢量ωs可以表示为ωs＝[ω
s，x
ω
s，y
ω
s，z
]
t
，其中ω
s，x
，ω
s，y
和ω
s，z
为传感器自运动的第一转动速度在三个坐标轴的分量；归一化或尺度伸缩的第一平动速度矢量可以表示为具体的，可以是传感器自运动的第一平动速度矢量t
′
的归一化矢量值或者尺度伸缩的矢量值；例如归一化或尺度伸缩的第一平动速度矢量为传感器自运动的第一平动速度矢量t
′
的尺度伸缩或者归一化，
[0071][0072]
其中t
′
为传感器自运动的第一平动速度矢量，可以表示为：
[0073]
t
′
＝[t
′
x t
′
y t
′z]
t
，
[0074]
其中归一化值s
′
或者尺度伸缩值s
′
，可以是矢量t
′
的幅度或者范数值，即s
′
＝||t
′
||。
[0075]
或者，
[0076][0077]
其中归一化值s
′
或者尺度伸缩值s
′
，也可以是矢量t
′
某一分量，例如可以是矢量t
′
的第三分量t
′z，即：
[0078]s′
＝t
′z，
[0079]
其中归一化值s
′
或者尺度伸缩值s
′
也可以是其它未知的标量，例如深度，此处不做限定。
[0080]
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，所述平面如地面或者平面轨道。第一转动速度矢量ωs可以表示为ωs＝[0 0ω
s，z
]
t
，其中传感器自运动的第一转动速度在x轴方向的分量ω
s，x
＝0，传感器自运动的第一转动速度在y轴方向的分量ω
s，y
＝0；归一化或尺度伸缩的第一平动速度矢量可以表示为此时，第一转动速度矢量ωs可以简化为用ω
s，z
表示，可以简化为用表示。
[0081]
步骤s302：根据第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量以及传感器的外部参数确定载体的第二转动速度矢量ωe和/或第二平动速度矢量te。
[0082]
具体地，载体的第二平动速度矢量te为完整的平动速度矢量，该传感器的外部参数包括传感器坐标系与载体坐标系的转换关系，该外部参数可以包括传感器坐标系相对于载体坐标系平移和/或传感器坐标系相对于载体坐标系的旋转参数；或者，包括载体坐标系相对于传感器坐标系的平移和/或载体坐标系相对于传感器坐标系的旋转参数。
[0083]
具体地，传感器坐标系相对于载体坐标系平移可以是传感器坐标系原点相对于载体坐标系原点的平移矢量；或者，传感器坐标系原点在载体坐标系原点中的位置矢量，此处不做限定。传感器坐标系相对于载体坐标系的旋转可以用传感器坐标系相对于载体坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，此处不做限定。
[0084]
具体地，载体坐标系相对于传感器坐标系平移可以是载体坐标系原点相对于传感器坐标系原点的平移矢量；或者，载体坐标系原点在传感器坐标系原点中的位置矢量，此处
不做限定。载体坐标系相对于传感器坐标系的旋转可以用载体坐标系相对于传感器坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，此处不做限定。
[0085]
具体地，传感器坐标系相对于载体坐标系的外部参数与传感器在载体上的安装位置相关，如当传感器安装在车体(载体)的保险杠的中心位置时，则传感器坐标系如图4所示，通常x轴方向向前，采用右手坐标系。图4为一种传感器坐标系的表示形式，如传感器坐标系用o1xyz表示。载体坐标系与载体固连，载体坐标系的原点为载体中心，如车载坐标系的原点位于车后轴中心位置，如图5所示，通常x轴方向向前，采用右手坐标系。图5为一种载体坐标系的表示形式，如载体坐标系用o2xbybzb表示，其中o2xb沿载体横轴向右，o2yb沿载体纵轴向前，o2zb沿载体立轴向上。
[0086]
可选地，作为一种实现方式，转换关系可以包括传感器坐标系相对于载体坐标系的旋转矩阵r和平移矢量r。传感器测量的目标位置矢量为pb，在载体坐标系下的位置矢量为pe，可以表示为：
[0087]
pe＝rpb r。
[0088]
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，上述坐标旋转可以用围绕z轴方向的旋转表示。
[0089]
在一种实现方式中，根据旋转矩阵r和第一转动速度矢量ωs确定载体的第二转动速度矢量ωe。
[0090]
具体地，所述载体的第二转动速度矢量ωe为旋转矩阵r和所述第一转动速度矢量ωs的乘积。例如，旋转矩阵为r，第一转动速度矢量ωs，确定载体的第二转动速度矢量为：
[0091]
ωe＝rωs。
[0092]
由于传感器坐标系相对于载体坐标系的旋转可以用传感器坐标系相对于载体坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，那么此时，也可以根据四元数或者欧拉角确定载体的第二转动速度矢量，此处不进一步赘述。
[0093]
在一种示例中，传感器坐标系跟载体坐标系各个对应的轴方向相同，此时所述第二转动速度矢量与所述第一转动速度矢量相等，ωe＝ωs，此时r为单位矩阵i。
[0094]
在一种实现方式中，根据第二转动速度矢量ωe和归一化或尺度伸缩的第一平动速度矢量以及传感器的外部参数，确定载体的第二平动速度矢量te。
[0095]
具体地，传感器的外部参数可以包括传感器坐标系相对于载体坐标系的平移矢量r和/或旋转参数，例如旋转参数可以是旋转矩阵、四元数或者欧拉角等。
[0096]
传感器的外部参数包括平移矢量r，在一种实现方式中，根据第二转动速度矢量ωe和平移矢量r确定载体转动引起的瞬时速度成分；根据旋转矩阵r和归一化或尺度伸缩的第一平动速度矢量确定传感器相对于载体坐标系的归一化瞬时速度矢量；根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te。
[0097]
具体地，根据第二转动速度矢量ωe和平移矢量r确定载体转动引起的瞬时速度成分tr可以是指载体转动引起的瞬时速度成分tr为第二转动速度矢量ωe和平移矢量r的叉积；tr＝ωe×
r。
[0098]
其中
×
表示矢量的叉积。具体地，可以为：
[0099][0100]
其中，ωe＝[ω
e，x ω
e，y ω
e，z
]
t
。
[0101]
或者载体转动引起的瞬时速度成分tr为平移矢量r和第二转动速度矢量ωe的叉积的负向量，
[0102]
tr＝-r
×
ωe，
[0103]
其中
×
表示矢量的叉积。具体地，可以为
[0104]
其中，r＝[r
x r
y rz]
t
。当然也可以根据其他的方式，本技术实施例不做限定。
[0105]
其中，上述载体转动引起的瞬时速度成分tr可以是三维矢量，如tr＝[t
r，x t
r，y t
r，z
]
t
或者tr也可以是二维矢量。
[0106]
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，所述平面如地面或者平面轨道。此时，第一转动速度矢量ωs可以简化为用ω
s，z
表示，可以简化为用表示，平移矢量r＝[r
x r
y rz]
t
可以简化为用r
x
，ry表示，rz＝0。瞬时速度成分tr可以简化为：
[0107]
t
r，x
＝-ω
e，z
·ry
，
[0108]
t
r，y
＝ω
e，z
·rx
，
[0109]
t
r，z
＝0。
[0110]
tr可以简化为二维矢量，包括t
r，x
，t
r，y
两个分量。
[0111]
在一种示例中，可以根据旋转参数和归一化或尺度伸缩的第一平动速度矢量确定传感器相对于载体坐标系的归一化瞬时速度矢量。
[0112]
具体地，传感器相对于载体的旋转参数，例如旋转参数可以是旋转矩阵、四元数或者欧拉角等。
[0113]
以旋转矩阵r为例，根据旋转矩阵r和归一化或尺度伸缩的第一平动速度矢量确定传感器相对于载体坐标系的归一化瞬时速度矢量可以是指传感器相对于载体坐标系的归一化瞬时速度矢量为所述旋转矩阵r和归一化或尺度伸缩的第一平动速度矢量的乘积，即
[0114]
在一种实现方式中，r为单位矩阵，传感器相对于载体坐标系的归一化瞬时速度矢量与归一化或尺度伸缩的第一平动速度矢量相等，
[0115]
可以是三维矢量，或者，也可以是二维矢量，包括两个分量。
[0116]
在一种示例中，根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te可以是指载体的第二平动速度矢量te为瞬时速度矢量与瞬时速度成分的差，所述瞬时速度矢量根据所述归一化瞬时速度矢量与瞬时速度成分确定。
[0117]
具体地，所述载体的第二平动速度矢量te为：
[0118][0119]
其中，t
r，y
为tr的第二分量，即沿y轴方向的分量，为的第二分量，即沿y轴方向的分量。
[0120]
具体地，所述瞬时速度矢量根据所述归一化瞬时速度矢量与瞬时速度成分确定，包括
[0121]
所述瞬时速度矢量根据所述归一化瞬时速度矢量及传感器坐标系的平动速度矢量的幅度确定，其中传感器坐标系的平动速度矢量的幅度根据所述瞬时速度成分tr的第二分量以及所述归一化瞬时速度矢量的第二分量确定，所述传感器坐标系的平动速度矢量的幅度可以为：
[0122][0123]
在又一种示例中，根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te包括：根据瞬间速度成分和归一化瞬时速度矢量确定传感器坐标系的平动速度矢量的幅度；根据传感器坐标系的平动速度矢量的幅度、归一化瞬时速度矢量以及瞬时速度成分确定载体的第二平动速度矢量te。
[0124]
具体地，根据瞬间速度成分和归一化瞬时速度矢量确定传感器坐标系的平动速度矢量的幅度，包括：
[0125][0126]
其中，s表示传感器坐标系的平动速度矢量的幅度。
[0127]
具体地，根据传感器坐标系的平动速度矢量的幅度、归一化瞬时速度矢量以及瞬时速度成分确定载体的第二平动速度矢量te，包括：
[0128][0129]
其中，s为传感器坐标系的平动速度矢量的幅度，为归一化瞬时速度矢量，tr为瞬时速度成分。其中，t
e，y
为0，由于车辆在被设计的时候，车辆如果转动，那么车后轴的位置的中心，即在载体坐标系的y轴方向的速度需要为0，即t
e，y
为0，若t
e，y
不为0，车辆会打滑，因此，t
e，y
为0。
[0130]
te可以是三维矢量，te＝[t
e，x t
e，y t
e，z
]
t
或者te也可以是二维矢量，包括t
e，x
，t
e，y
两个速度分量。
[0131]
可选的，本技术实施例可以进一步包括步骤s303：
[0132]
步骤s303：根据传感器坐标系的平动速度矢量的幅度s和归一化或尺度伸缩的第一平动速度矢量确定传感器自运动的第三平动速度矢量ts。
[0133]
具体地，传感器坐标系的平动速度矢量的幅度s的计算方式如步骤s302所述，本步骤不再赘述。传感器自运动的第三平动速度矢量ts为完整的平动速度矢量。根据传感器坐标系的平动速度矢量的幅度s和归一化或尺度伸缩的第一平动速度矢量确定传感器自运动的第三平动速度矢量ts，包括：
[0134][0135]
需要指出的是，传感器自运动的第三平动速度矢量为传感器瞬时速度矢量；或者传感器自运动的第三平动速度矢量为传感器坐标系原点的瞬时速度矢量。
[0136]
进一步地，传感器自运动的第三平动速度矢量可以转换到传感器坐标系表示为t
′s，如，
[0137]
t
′s＝r-1
ts，
[0138]
可选的，本技术实施例可以进一步包括步骤s304：
[0139]
步骤s304：确定静止障碍物的深度z。
[0140]
具体地，在步骤s304中，传感器为视觉传感器。
[0141]
其中，步骤s304包括步骤s3041和步骤s3042。具体如下：
[0142]
步骤s3041：获取静止障碍物的流矢量。
[0143]
具体地，流矢量包括静止障碍物在视觉传感器的图像平面上的投影的运动矢量。获取静止障碍物的流矢量的方法有多种，流矢量可以通过光流获取，例如可以通过lukas-kanade光流算法或者horn-schunck光流算法等算法确定。本技术实施例不做限定。
[0144]
为了理解方便，本技术实施例提供了一种利用lukas-kanade光流法获取静止障碍物的流矢量的方式，如下：
[0145]
静止障碍物在图像上的位置(p
x
，py)对应的流矢量可以用位置(p
x
，py)对应的光流(u，v)计算，具体地，可以通过以下关系式获取，
[0146][0147]
其中g为梯度矩阵，b为失配矢量，具体地，
[0148][0149][0150]
其中，i
x
(x，y)和iy(x，y)为图像点(x，y)的亮度在该点(x，y)处的x和y方向的梯度；i
t
为图像亮度对时间的变化量，[p
x-w
x
，p
x
w
x
]和[p
y-wy，py wy]为点(p
x
，py)对应的窗口，其大小为(2w
x
1)(2wy 1)，w
x
和wy为预先设定的参数，例如w
x
＝wy＝1，2，3......。
[0151]
需要指出的是，即使对于lukas-kanade光流算法也有多种实现形式，例如基于金字塔结构分层估计，此处不赘述。
[0152]
步骤s3042：根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
[0153]
具体地，传感器自运动的第三平动速度矢量ts可以转换到传感器坐标系表示为t
′s，ωs为传感器自运动的第一转动速度矢量。静止障碍物的流矢量可以表示为(u，v)。
[0154]
根据所述静止障碍物的流矢量、所述第三平动速度矢量t
′s和所述第一转动速度矢量ωs确定所述静止障碍物的深度z，可以是基于以下关系式确定，
[0155]
[0156][0157]
其中，s1＝[-f 0 x]
t
，s2＝[0
ꢀ-
f y]
t
，
×
表示矢量的叉积，f为相机焦距，x，y为图像平面的像素位置，x∈[p
x-w
x
，p
x
w
x
]，y∈[p
y-wy，py wy]；其中w
x
和wy为非负整数，w
x
＝0，1，2，3，4...；wy＝0，1，2，3，4...。
[0158]
具体地，静止障碍物的深度z，可以根据以下关系式得到：
[0159][0160]
其中w1(x，y)和w2(x，y)为加权系数，可以根据上述参数u，v和t
′s和ωs的估计精度或者误差确定。
[0161]
具体地，作为一种实现，
[0162][0163]
或者
[0164][0165]
或者
[0166][0167]
或者
[0168][0169]
可选的，本技术实施例可以进一步包括步骤s305。
[0170]
步骤s305：根据载体的第二转动速度矢量ωe和第二平动速度矢量te，以及通过其他传感器获得的载体的转动速度矢量ω
e2
和平动速度矢量t
e2
确定载体的转动速度矢量和平动速度矢量
[0171]
具体地，载体的第二转动速度矢量ωe和第二平动速度矢量te可以是根据视觉传感器提供的数据获得的，载体的转动速度矢量ω
e2
和平动速度矢量t
e2
可以是根据雷达传感器等其他传感器提供的数据获得的，其中，通过其他传感器可以获得一组或者多组载体的转动速度矢量和平动速度矢量。
[0172]
例如，为了理解方便，本技术实施例中，除了通过视觉传感器获得载体的第二转动速度矢量ωe和第二平动速度矢量te之外，还通过雷达传感器获得载体的转动速度矢量ω
e2
和平动速度矢量t
e2
。那么，载体的加权转动速度矢量可以根据以下公式得到：
[0173][0174]
其中加权系数w1，w2可以为或者根据ωe和ω
e2
的误差协方差或者标准差确定，例如正比于协方差或者标准差的逆矩阵或者倒数确定。
[0175]
加权平动速度矢量可以根据以下公式得到：
[0176][0177]
其中加权系数w1，w2可以为或者根据ωe和ω
e2
的误差协方差或者标准差确定，例如正比于协方差或者标准差的逆矩阵或者倒数确定。
[0178]
除了上述加权形式，还包括其他的加权形式，本技术实施例不做限定。
[0179]
在图3所描述的方法中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量通常缺少一个自由度的信息，而本技术实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
[0180]
上述详细阐述了本技术实施例的方法，下面提供了本技术实施例的装置。
[0181]
请参见图6，图6是本技术实施例提供的一种自运动估计装置的结构示意图，该自运动估计装置可以包括第一获取单元601和第一确定单元602，其中，各个单元的详细描述如下。
[0182]
第一获取单元601，用于获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
[0183]
第一确定单元602，用于根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
[0184]
在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵r和平移矢量r。
[0185]
在又一种实现方式中，所述第一确定单元602，用于根据所述旋转矩阵r和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
[0186]
在又一种实现方式中，所述第一确定单元602，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵r和所述第一平动速度矢量确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
[0187]
可选地，在又一种实现方式中，所述第一确定单元602，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
[0188]
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
[0189][0190]
所述s为所述传感器坐标系的平动速度矢量的幅度，所述为所述归一化瞬时速度矢量，所述tr为所述瞬时速度成分。
[0191]
可选地，在又一种实现方式中，所述装置还包括：第二确定单元，用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量确定所述传感器自运动的第三平动速度矢量ts。
[0192]
可选地，在又一种实现方式中，所述传感器为视觉传感器，所述装置还包括：第二获取单元，用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；第三确定单元，用于根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
[0193]
需要说明的是，各个单元的实现及有益效果还可以对应参照图3所示的方法实施例的相应描述。
[0194]
请参见图7，图7是本技术实施例提供的一种自运动估计装置700，该装置700包括至少一个处理器701和通信接口702，可选的，还包括存储器703。所述存储器703用于存储计算机程序，所述至少一个处理器701、存储器703和通信接口702通过总线704相互连接。
[0195]
存储器703包括但不限于是随机存储记忆体(random access memory，ram)、只读存储器(read-on1y memory，rom)、可擦除可编程只读存储器(erasable programmable read only memory，eprom)、或便携式只读存储器(compact disc read-only memory，cd-rom)，该存储器703用于相关计算机程序及数据。通信接口702用于接收和发送数据。
[0196]
处理器701可以是一个或多个中央处理器(central processing unit，cpu)，在处理器701是一个cpu的情况下，该cpu可以是单核cpu，也可以是多核cpu。
[0197]
该装置700中的处理器701读取所述存储器703中存储的计算机程序，用于执行以下操作：
[0198]
获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
[0199]
根据所述第一转动速度矢量ωs、所述第一平动速度矢量以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
[0200]
可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵r和平移矢量r。
[0201]
可选地，在又一种实现方式中，所述处理器，用于根据所述旋转矩阵r和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
[0202]
可选地，在又一种实现方式中，所述处理器，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵r和所述第一平动速度矢量确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
[0203]
可选地，在又一种实现方式中，所述处理器，用于根据所述瞬间速度成分和所述归
一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
[0204]
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
[0205][0206]
所述s为所述传感器坐标系的平动速度矢量的幅度，所述为所述归一化瞬时速度矢量，所述tr为所述瞬时速度成分。
[0207]
在又一种实现方式中，所述处理器，还用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量确定所述传感器自运动的第三平动速度矢量ts。
[0208]
在又一种实现方式中，所述传感器为视觉传感器，所述处理器，还用于获取静止障碍物的流矢量，所述流失量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
[0209]
需要说明的是，各个操作的实现及有益效果还可以对应参照图3所示的方法实施例的相应描述。
[0210]
本技术实施例还提供一种芯片系统，所述芯片系统包括至少一个处理器，至少一个处理器和通信接口，可选的，所述芯片系统还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。
[0211]
本技术实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，当其运行时，图3所示的方法流程得以实现。
[0212]
本技术实施例还提供一种计算机程序产品，当所述计算机程序产品运行时，图3所示的方法流程得以实现。
[0213]
本技术实施例还提供了一种终端，所述终端包含至少一个处理器和通信接口，可选的，还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。可选的，所述终端可以为车辆、舰船、卫星、无人机、机器人、智能家居设备或者智能制造设备等。
[0214]
本技术实施例还提供了一种传感器，所述传感器包括至少一个处理器和通信接口，可选的，所述传感器还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。
[0215]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程可以由计算机程序来计算机程序相关的硬件完成，该计算机程序可存储于计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质包括：rom或随机存储记忆体ram、磁碟或者光盘等各种可存储计算机程序代码的介质。