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

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


背景技术：

2.先进辅助驾驶系统(advanced driver assistant system，adas)或者自动驾驶(autonomous driving，ad)系统会配置多种传感器，例如毫米波雷达、激光雷达、超声波传感器如声呐、视觉传感器如相机或者摄像头等传感器，用于感知周边环境信息，周边环境信息包括运动目标和静止目标。对于运动目标和静止目标，通常采用不同的方法分析处理，示例性的，对运动目标(比如车辆、行人)进行分类、识别和跟踪，对静止目标(比如障碍物、护栏、路沿)分类和识别。通过上述方式，可以为自动驾驶提供额外信息，如规避障碍物、提供可行驶区域等。
3.传感器通常可以安装于载体，传感器跟随传感器所在载体运动。一方面，传感器所在载体的运动导致运动目标和静止目标无法独立分析，因此，需要估计传感器所在载体的运动，从而实现运动目标和静止目标的分离。另一方面，运动目标的跟踪通常基于运动模型，如常速度(constant velocity，cv)/常加速度(constant acceleration，ca)/匀速圆周运动(coordinated turn，ct)等模型，且模型通常假定相对地面或者大地坐标系，传感器所在载体的运动将导致上述模型失效或者跟踪性能下降，因此，需要对传感器所在载体的运动进行补偿。
4.综上，准确估计传感器所在载体的运动，是本领域人员正在解决的技术问题。


技术实现要素：

5.本技术提供一种运动估计方法及装置，用于准确估计传感器所在载体的运动。
6.第一方面，本技术提供一种运动估计方法，该方法包括：
7.获取m个第一传感器的转动角速度矢量估计值和n个第二传感器的瞬时速度矢量估计值；其中，m≥1，n≥1；
8.根据所述n个第二传感器的瞬时速度矢量估计值和所述n个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述m个第一传感器的转动角速度矢量估计值确定的。
9.上述技术方案中，由于第一传感器的转动角速度矢量估计值和第二传感器的瞬时速度矢量估计值较为准确，根据至少一个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值，再结合第二传感器的瞬时速度矢量估计值，确定载体的第一平动速度矢量估计值，有助于得到较为准确的传感器所在载体的运动。
10.在一种可能的实现方式中，所述第一平动速度矢量估计值基于如下关系式确定：
[0011][0012]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系
的位置平移矢量。
[0013]
上述关系式基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，且上述关系式可以存在多种变形。根据上述关系式可以较准确确定出第一平动速度矢量估计值。
[0014]
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
[0015][0016]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w
2,j
为第j个第二传感器的加权系数，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0017]
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
[0018][0019]
其中，ω为第一转动角速度矢量估计值，w
1,i
为第i个第一传感器的加权系数，ω
1,i
为第i个第一传感器的转动角速度矢量估计值。
[0020]
在一种可能的实现方式中，还包括：
[0021]
获取所述m个第一传感器中m
′
个第一传感器的归一化的平动速度矢量估计值，其中，1≤m
′
≤m；
[0022]
根据所述第一平动速度矢量估计值、所述m
′
个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
[0023]
上述技术方案中，第一传感器评估自身运动得到的归一化的平动速度矢量估计值较准确，该将第一传感器的归一化的平动速度矢量估计值和第一平动速度矢量估计值进行融合，得到载体的第二平动速度矢量估计值，可进一步提高载体的平动速度矢量估计值的精确度。
[0024]
在一种可能的实现方式中，所述第二平动速度矢量估计值基于如下关系式确定：
[0025][0026]
其中，为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，si由第一平动速度矢量估计值确定。
[0027]
上述关系式基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，且上述关系式可以存在多种变形。根据上述关系式可以较准确确定出第二平动速度矢量估计值。
[0028]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0029][0030]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计
值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0031]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0032][0033]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计值，w
′
1,i,k
为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0034]
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0035]si,k
＝‖t
k-1
ω
×r1,i
‖
[0036]
其中，t
k-1
为第k-1次迭代中载体的平动速度矢量估计值，t0为第一平动速度矢量估计值。
[0037]
上述技术方案中，每次迭代中，根据多个第一传感器的归一化的平动速度矢量估计值，确定载体的平动速度矢量估计值。将多个第一传感器的参数作为一次迭代的输入，有助于提高第二平动速度矢量的估计精度。
[0038]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0039][0040]
其中，为第二平动速度矢量估计值为第二平动速度矢量估计值为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i，l
为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,l
由第一平动速度矢量估计值或或确定。
[0041]
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0042][0043]
其中，为第一平动速度矢量估计值；
[0044]
上述技术方案中，针对每个第一传感器的归一化的平动速度矢量估计值，确定每个第一传感器对应的载体的平动速度矢量估计值，相当于，针对每个第一传感器进行一次迭代，将一个第一传感器的参数作为一次迭代的输入，可以快速得到较为准确的第二平动速度矢量估计值。
[0045]
在一种可能的实现方式中，还包括：
[0046]
基于如下关系式确定所述载体的第二转动角速度矢量估计值：
[0047][0048]
其中，ω
′
为第二转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0049]
上述技术方案中，根据较准确的第二平动速度矢量估计值进一步确定较准确的第二转动角速度矢量估计值。
[0050]
第二方面，本技术提供一种运动估计装置，该装置包括：
[0051]
获取单元和处理单元；
[0052]
所述获取单元用于获取m个第一传感器的转动角速度矢量估计值和n个第二传感器的瞬时速度矢量估计值；其中，m≥1，n≥1；
[0053]
所述处理单元用于根据所述n个第二传感器的瞬时速度矢量估计值和所述n个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述m个第一传感器的转动角速度矢量估计值确定的。
[0054]
在一种可能的实现方式中，所述处理单元具体用于基于如下关系式确定所述第一平动速度矢量估计值：
[0055][0056]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0057]
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
[0058][0059]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w
2,j
为第j个第二传感器的加权系数，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0060]
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
[0061][0062]
其中，ω为第一转动角速度矢量估计值，w
1,i
为第i个第一传感器的加权系数，ω
1,i
为第i个第一传感器的转动角速度矢量估计值。
[0063]
在一种可能的实现方式中，所述获取单元还用于获取所述m个第一传感器中m
′
个第一传感器的归一化的平动速度矢量估计值，其中，1≤m
′
≤m；所述处理单元还用于根据所述第一平动速度矢量估计值、所述m
′
个第一传感器的归一化的平动速度矢量估计值，确定
所述载体的第二平动速度矢量估计值。
[0064]
在一种可能的实现方式中，所述处理单元具体用于基于如下关系式确定所述第二平动速度矢量估计值：
[0065][0066]
其中，为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，si由第一平动速度矢量估计值确定。
[0067]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0068][0069]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0070]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0071][0072]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计值，w
′
1，i，k
为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0073]
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0074]si,k
＝‖t
k-1
ω
×r1,i
‖
[0075]
其中，t
k-1
为第k-1次迭代中载体的平动速度矢量估计值，t0为第一平动速度矢量估计值。
[0076]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0077][0078]
其中，为第二平动速度矢量估计值为第二平动速度矢量估计值为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i，l
为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,l
由第一平动速度矢量估计值或或确定。
[0079]
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0080][0081]
其中，为第一平动速度矢量估计值；
[0082]
在一种可能的实现方式中，所述处理单元还用于基于如下关系式确定所述载体的第二转动角速度矢量估计值：
[0083][0084]
其中，ω
′
为第二转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0085]
第三方面，本技术提供一种通信装置，包括至少一个处理器和通信接口，所述通信接口用于接收来自所述通信装置之外的其它通信装置的信号并传输至所述至少一个处理器或将来自所述至少一个处理器的信号发送给所述通信装置之外的其它通信装置，所述至少一个处理器通过逻辑电路或执行代码指令用于实现上述第一方面或第一方面的任意可能的实现方式中的方法。
[0086]
第四方面，本技术提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述第一方面或第一方面的任意可能的实现方式中的方法。
[0087]
第五方面，本技术提供一种计算机程序产品，所述计算机程序产品包括计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述第一方面或第一方面的任意可能的实现方式中的方法。
[0088]
第六方面，本技术提供一种芯片，包括至少一个处理器和接口；
[0089]
所述接口，用于为所述至少一个处理器提供程序指令或者数据；
[0090]
所述至少一个处理器用于执行所述程序行指令，以实现上述第一方面或第一方面的任意可能的实现方式中的方法。
[0091]
第七方面，本技术提供一种终端，所述终端包含上述第二方面提供的任意运动估计装置、上述第三方面提供的任意通信装置、或者上述第四方面提供的任意计算机可读存储介质。进一步可选的，所述终端可以为车辆、无人机、机器人、智能家居设备或者卫星等。
[0092]
上述第二方面至第七方面中任一方面可以达到的技术效果可以参照上述第一方面中有益效果的描述，此处不再重复赘述。
附图说明
[0093]
图1为本技术提供的一种自运动估计系统的结构示意图；
[0094]
图2为本技术提供的一种运动估计方法的流程示意图；
[0095]
图3为本技术提供的一种车载系统中多传感器配置示意图；
[0096]
图4为本技术提供的一种转动角速度矢量的示意图；
[0097]
图5为本技术提供的一种位置平移矢量的示意图；
[0098]
图6为本技术提供的一种载体坐标系与传感器坐标系之间的变换关系的示意图；
[0099]
图7为本技术提供的一种运动估计装置的结构示意图；
[0100]
图8为本技术提供的一种芯片的结构示意图。
具体实施方式
[0101]
下面将结合附图，对本技术实施例进行详细描述。
[0102]
请参见图1，图1是本技术实施例提供的一种自运动估计系统的结构示意图，该系统包括第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030。
[0103]
第一传感器1010可以为视觉传感器，例如，相机或者摄像头、红外热成像传感器等。第一传感器1010可以提供视觉测量数据，例如图像或者视频。第一运动感知模块1011用于根据第一传感器1010提供的测量数据确定运动测量数据，如传感器运动的转动角速度矢量和/或归一化或尺度伸缩(scaled)的平动速度矢量等或者带有完整尺度信息的平动速度矢量估计值。第二传感器1020可以为雷达传感器、超声波传感器、惯性测量传感器或者定位传感器等，例如，毫米波雷达、声呐、激光雷达、惯性测量单元(inertial measurement unit，imu)或者全球导航卫星系统(global navigation satellite system，gnss)等。第二传感器1020用于提供位置测量数据和/或速度测量数据，例如位置和/或径向速度或者速度投影分量的测量数据。第二运动感知模块1021用于根据第二传感器1020提供的位置测量数据和/或速度测量数据，确定运动测量数据，如传感器运动的瞬时平动速度矢量等。数据处理模块1030用于处理第一运动感知模块1011和第二运动感知模块1021提供的运动测量数据。本技术中，运动测量数据也可以称为是运动感知数据。
[0104]
第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030可以通过有线或者无线方式等连接在一起；第一传感器1010和第二传感器1020可以分布于载体的相同或者不同的位置；第一运动感知模块1011和第二运动感知模块1021可以分别与第一传感器1010和第二传感器1020集成在一起；也可以与数据处理模块1030集成在一起；也可以分别独立于其它模块存在，本技术不做限定。
[0105]
在一种示例中，第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030部署于一个处理器系统上。在又一种示例中，第一传感器1010、第二传感器1020分别部署于一个处理器系统上；第一运动感知模块1011、第二运动感知模块1021和数据处理模块1030部署于一个处理器系统上。
[0106]
本技术实施例可适用各种载体中的多传感器系统，其中载体如车载(如汽车、摩托车或自行车等)、机载(如无人机、直升机或喷气式飞机或者气球)、船载(轮船、汽艇或者舰艇等)、星载(如卫星)或者智能体(如机器人等)等。
[0107]
示例性的，载体为车载，车载上可以承载有至少一个第一传感器和至少一个第二传感器。比如，车载安装有1个第一传感器和1个第二传感器，或者，车载安装有1个第一传感器和5个第二传感器，或者，车载安装有6个第一传感器和5个第二传感器等。
[0108]
目前，获得传感器运动速度或者载体运动速度估计的方法有多种。以车载平台为例，车载的运动估计中，可以是采用如下三种传感器。
[0109]
一、imu
[0110]
imu是测量物体三轴姿态角(或角速度)以及加速度的装置。一般地，一个imu内会装有三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此可以解算出物体运动速度和姿态。
[0111]
二、雷达传感器
[0112]
雷达传感器，通常可以提供距离、方位角和径向速度测量数据。基于静止目标物体的方位角和径向速度分量测量数据，利用最小二乘方法或者其他的方式，可以获得传感器相对于大地的瞬时速度，特别的，雷达传感器可以获取到较为精确的纵向速度估计值。此外，基于上述速度估计值，可以进一步得到传感器的横摆角速度(yaw rate)估计值。
[0113]
三、视觉传感器
[0114]
视觉传感器通常可以提供连续的两帧或者多帧图像。基于上述两帧或者多帧图像，利用光流法或者特征点对应的方法或者直接优化光亮度(intensity)的目标物体函数的方法，可以获得传感器尺度伸缩的平动速度估计值和转动速度估计值。
[0115]
但上述三种传感器均存在各自缺陷：
[0116]
(1)imu的运动速度估计是基于加速度计积累得到，测量误差会随时间累积，因而存在误差累积问题，需要利用其它传感器进行额外的校准，而且，一般用于车辆中的imu的精度太低，若选用高精度的imu，成本高昂。
[0117]
(2)雷达传感器获取到的传感器的横向速度估计值的精确度较低，且不能获取传感器的俯仰角速度估计值(pitch rate)和滚转角速度(roll rate)估计值。
[0118]
(3)视觉传感器存在尺度伸缩问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确的深度估计，无法精确获得平动速度估计值，或者仅能获得平动速度的一个尺度伸缩的估计值。
[0119]
为解决上述问题，本技术实施例提供一种运动估计的方法，用于较精确的确定载体的平动速度矢量估计值和转动角速度矢量估计值，从而对载体实现精确的运动估计。
[0120]
需要指出的是，本技术中，平动速度矢量估计值也可以是平动位移矢量估计值，平动位移矢量估计值可以是两帧间的位置偏移矢量的估计值，也可以是两帧间的时间差和平动速度矢量估计值的乘积。
[0121]
请参见图2，图2是本技术实施例提供的一种运动估计方法的流程示意图，该方法的执行主体可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该方法的执行主体也可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。
[0122]
以下不同的执行步骤可以集中式实现，或者，以下不同的执行步骤也可以分布式实现。方法包括但不限于如下步骤：
[0123]
步骤201，获取m个第一传感器的转动角速度矢量估计值和n个第二传感器的瞬时速度矢量估计值。
[0124]
其中，m个第一传感器和n个第二传感器承载于一个载体上，第一传感器的数量m≥1，第二传感器的数量n≥1。
[0125]
具体的，第一传感器可以为视觉传感器如相机、摄像头、红外传感器或其它成像传感器等，或惯性测量传感器如imu，第二传感器可以为雷达传感器如毫米波雷达、激光雷达等，或超声波传感器如声呐等。
[0126]
一种实现中，m(m＞1)个第一传感器可以是相同类型或不同类型，示例性的，m个第一传感器中包括m1个视觉传感器如相机或者摄像头和m2个惯性测量传感器如imu，其中，m1 m2＝m，且m1≥0，m2≥0。和/或，n(n＞1)个第二传感器可以是相同类型或不同类型，示例性的，n个第二传感器中包括n1个雷达传感器如毫米波雷达、激光雷达和n2个超声波传感器如声呐，其中，n1 n2＝n，且n1≥0，n 2≥0。
[0127]
m个第一传感器和n个第二传感器可以安装于载体的相同位置或者不同位置。下面以车载为例说明：
[0128]
例子1，车载前端安装1个相机和1个毫米波雷达；
[0129]
例子2，车载前端安装1个相机和1个毫米波雷达，车载4个角位置另外安装4个毫米波雷达；
[0130]
例子3，车载前端安装1个毫米波雷达，4个角位置另外安装4个毫米波雷达，另外在车载上均匀安装6个相机。
[0131]
此外，车载上还可以进一步安装imu或者gnss等。
[0132]
示例性的，一种车载系统中多传感器配置示意图如图3所示，车载系统的多传感器可以包括1个相机、5个毫米波雷达和1个imu，其中imu的安装位置可以接近车载坐标系(也可以称为是车体坐标系、载体坐标系)的原点位置，车载坐标系的原点可以位于车体后轴中心。
[0133]
上述例子1至例子3中，毫米波雷达也可以替换成激光雷达或超声波传感器等，相机也可以替换成摄像头或红外传感器等，也可以在原基础上增加至少一个激光雷达。
[0134]
比如上述例子3中，可以将5个毫米波雷达的其中2个毫米波雷达替换为激光雷达，又或者将5个毫米波雷达全部替换为激光雷达，此外，还可以在原5个毫米波雷达的基础上增加1至3个激光雷达。
[0135]
应理解，此处只是本技术示例性提供的车载上安装第一传感器和第二传感器的实现方式，其并不对本技术做任何限定。
[0136]
具体的，所述获取m个第一传感器的转动角速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到，其中转动角速度矢量估计值可以基于传感器的测量数据通过运动或者估计算法得到或者是由传感器直接测量得到；
[0137]
或者，所述获取m个第一传感器的转动角速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到传感器的测量数据，转动角速度矢量估计值根据传感器的测量数据通过运动或者估计算法得到或者是由传感器测量数据直接得到。
[0138]
作为一种实现方式，转动角速度矢量估计值可以根据第一传感器的测量数据通过估计得到。此时，第一传感器的测量数据并不直接包含第一传感器的运动测量的测量值。
[0139]
示例性的，第一传感器为视觉传感器如相机，相机的原始测量数据为视觉测量数据，比如图像或者视频，可以根据图像或者视频中的特征点、线、平面或者区域，基于数据的光学特性或者几何特性确定相机的转动角速度矢量估计值。例如，可以基于8点法或者5点法或者单应性(homography)或者光流法等，得到相机的转动角速度矢量估计值。基于图像或者视频，得到传感器的转动角速度为现有技术，此处不赘述。
[0140]
作为另一种实现方式，转动角速度矢量估计值可以从第一传感器的测量数据中直接得到。此时，第一传感器可以直接测量包含转动角速度矢量。
[0141]
示例性的，第一传感器为惯性测量传感器如imu，imu可以直接测量转动角速度矢量。
[0142]
需要指出的是，上述m个第一传感器可以包含相同类型或不同类型的第一传感器，示例性的，m个第一传感器可以包括m1个视觉传感器例如相机或者摄像头和m2个惯性测量传感器如imu，其中，m1 m2＝m，且m1≥0，m2≥0，相应的，对m1个视觉传感器的测量数据处理得到m1个视觉传感器的转动角速度矢量估计值，以及从m2个惯性测量传感器的测量数据中直接得到读取m2个惯性测量传感器的转动角速度矢量估计值。
[0143]
可选地，作为一种实现方式，转动角速度矢量可以为三维矢量，ω＝[ω
x ω
y ωz]
t
，如图4所示。
[0144]
可选地，作为另一种实现方式，第一传感器或者载体在平面内运动，平面如地面或者平面轨道。转动角速度矢量可以表示为ω＝[0 0 ωz]
t
，此时，转动角速度矢量可以简化为用ωz表示。
[0145]
需要指出的是，上述从传感器的测量数据直接得到或者基于传感器的测量数据通过运动估计得到转动角速度矢量估计值，可以根据传感器坐标系相对于载体坐标的变换关系，得到传感器在载体坐标系的转动角速度矢量估计值。
[0146]
具体的，所述获取n个第二传感器的瞬时速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到，其中瞬时速度矢量估计值可以基于传感器的测量数据通过运动或者估计算法得到；
[0147]
或者，所述获取n个第二传感器的瞬时速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到传感器的测量数据，瞬时速度矢量估计值根据传感器的测量数据通过运动或者估计算法得到。
[0148]
作为一种实现方式，瞬时速度矢量估计值可以根据第二传感器的测量数据通过估计得到。此时，第二传感器的测量数据并不直接包含第二传感器的运动测量的测量值。
[0149]
示例性的，第二传感器为毫米波雷达或者激光雷达或者超声波传感器如声呐，第二传感器的测量数据可以包括位置和径向速度，或者包括角度和径向速度。可以根据其中静止目标的测量数据，基于最小二乘法或者正交距离回归法或者最小均方误差准则等估计方法，确定瞬时速度矢量估计值。此外，也可以根据第二传感器的多次位置测量数据，根据其中静止目标的测量数据，确定瞬时速度矢量估计值。本技术实施例不做限定。
[0150]
需要指出的是，载体获取第一传感器的转动角速度矢量估计值的数量可以小于或等于实际上载体中第一传感器的数量，载体获取第二传感器的瞬时速度矢量估计值的数量可以小于或等于实际上载体中第二传感器的数量。
[0151]
第一个示例性中，载体上承载有6个第一传感器和3个第二传感器，载体可以获取6个第一传感器的转动角速度矢量估计值和3个第二传感器的瞬时速度矢量估计值。
[0152]
第二个示例性中，载体上承载有6个第一传感器和3个第二传感器，载体可以获取4个第一传感器的转动角速度矢量估计值和2个第二传感器的瞬时速度矢量估计值。
[0153]
步骤202，根据n个第二传感器的瞬时速度矢量估计值、n个第二传感器的外部参数和n个第二传感器所在的载体的第一转动角速度矢量估计值，确定载体的第一平动速度矢量估计值。
[0154]
其中，n个第二传感器的外部参数可以包括n个第二传感器相对于载体坐标系的位
置平移矢量，或者，包括n个第二传感器的坐标系原点相对于载体坐标系原点的位置平移矢量。示例性的，对于任一个第二传感器来说，第二传感器相对于载体坐标系的位置平移矢量用于将该第二传感器的坐标系原点平移至与载体坐标系原点一致。
[0155]
以车载上安装5个第二传感器为例，该5个第二传感器可以包括毫米波雷达或者激光雷达或者超声波传感器，参照图5所示，5个第二传感器分别位于车载的不同位置，该5个第二传感器相对于车载坐标系原点的位置平移矢量分别为r
21
,r
22
,
…
,r
25
，该5个第二传感器的瞬时速度矢量估计值分别为v
21
,v
22
,
…
,v
25
。通常r
21
，r
22
，
…
，r
25
互不相同时，v
21
，v
22
，
…
，v
25
也互不相同。
[0156]
具体的，根据n个第二传感器的瞬时速度矢量估计值和n个第二传感器的外部参数以及载体的第一转动角速度矢量估计值，确定载体的第一平动速度矢量估计值，可以是，载体的第一平动速度矢量估计值基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，其中瞬时速度矢量以及位置平移矢量从n个第二传感器的瞬时速度矢量估计值和n个第二传感器的外部参数确定，转动角速度从载体的第一转动角速度矢量估计值确定。
[0157]
具体的，刚体的平动速度矢量、瞬时速度矢量、转动角速度矢量以及位置平移矢量的关系式为t＝v-ω
×
r，或者t＝v r
×
ω，其中，t为刚体的平动速度矢量，v为刚体的瞬时速度矢量，ω为刚体的转动角速度矢量，r为位置平移矢量，
×
表示矢量的叉积。
[0158]
基于刚体的平动速度矢量、瞬时速度矢量、转动角速度矢量以及位置平移矢量的关系式，可以存在多种变形关系式，该多种变形关系式均可以得到载体的第一平动速度矢量估计值。
[0159]
示例性的，确定第一平动速度矢量估计值可以基于如下关系式得到：
[0160][0161]
或者
[0162][0163]
其中，为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v2为第二传感器的瞬时速度矢量估计值，r2为第二传感器相对于载体坐标系原点的位置平移矢量，
×
表示矢量的叉积。
[0164]
具体的，上述关系式可以为
[0165]
其中，ω＝[ω
x ω
y ωz]
t
。
[0166]
具体的，上述关系式也可以为
[0167]
其中，r2＝[r
x,2 r
y,2 r
z,2
]
t
。
[0168]
当然也可以根据其他的方式，本技术实施例不做限定。
[0169]
具体的，r2可以为第二传感器的外部参数，可以为第二传感器的坐标系原点相对于载体坐标系原点的位置平移矢量。
[0170]
作为一种实现方式，第一平动速度矢量估计值可以是基于如下关系式确定：
[0171][0172]
其中，为载体的第一平动速度矢量估计值，t
2,j
为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值。
[0173]
作为一种实现方式，基于最小均方误差(minimum mean sqaured error，mmse)或者最小二乘(least square，ls)法等确定第一平动速度矢量估计值。
[0174]
第一种实现方式中，第一平动速度矢量估计值是载体的n个平动速度矢量估计值的加权和，其中，载体的n个平动速度矢量估计值分别根据n个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定。
[0175]
一种具体实现中，载体的第一平动速度矢量估计值满足如下关系式：
[0176][0177]
其中，为载体的第一平动速度矢量估计值，t
2,j
为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值，w
2,j
为与第j个第二传感器对应的加权系数或者加权系数矩阵。
[0178]
具体的，加权系数w
2,j
可以根据t
2,j
的估计误差或测量误差的概率密度函数或统计特性或协方差矩阵确定。例如，w
2,j
可以根据t
2,j
的估计误差或测量误差的协方差矩阵确定，其中，p
2,j
为t
2,j
的估计误差或测量误差的协方差。
[0179]
示例性的，根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定载体的平动速度矢量估计值，可以是基于如下关系式确定：
[0180]
t
2,j
＝v
2,j-ω
×r2,j
[0181]
相应的，作为第一种实现方式的一种具体示例，可以根据如下关系式确定第一平动速度矢量估计值：
[0182][0183]
其中，为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量，w
2,j
为与第j个第二传感器对应的加权系数或者加权系数矩阵。
[0184]
具体的，加权系数w
2,j
可以根据v
2,j
和/或r
2,j
测量误差或者估计误差确定。
[0185]
例如，w
2,j
根据v
2,j
和r
2,j
测量误差或者估计误差确定，例如，w
2,j
满足如下关系式：
[0186]
其中，
[0187]
其中，为v
2,j
的测量误差或者估计误差的协方差矩阵，为r
2,j
的测量误差或者估计误差的协方差矩阵。ω根据载体的转动角速度矢量得到，具体为，其中，ω＝[ω
x ω
y ωz]
t
。
[0188]
第二种实现方式中，第一平动速度矢量估计值是载体的n个平动速度矢量估计值的均值，其中，载体的n个平动速度矢量估计值分别根据n个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定。
[0189]
一种具体实现中，载体的第一平动速度矢量估计值满足如下关系式：
[0190][0191]
其中，为载体的第一平动速度矢量估计值，t
2,j
为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值。
[0192]
示例性的，根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定载体的平动速度矢量估计值，可以是基于t
2,j
＝v
2,j-ω
×r2,j
。相应的，作为第二种实现方式的一种具体示例，第一平动速度矢量估计值满足如下关系式：
[0193][0194]
其中，为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0195]
此处，需要说明的是，第一种实现方式中，第一平动速度矢量估计值是载体的n个平动速度矢量估计值的加权和，而第二种实现方式中，第一平动速度矢量估计值是载体的n个平动速度矢量估计值的均值，也可以理解，第二种实现方式是第一种实现方式的一种特殊形式，若第一种实现方式中各第二传感器的加权系数相同，则可以为第二种实现方式。进一步的，该说明同样适用于其它实现方式中多个传感器估计值的加权和与多个传感器估计值的均值之间的关系。
[0196]
作为另一种实现方式，n等于1，根据第一转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第一平动速度矢量估计值，具体可参照上述关系式得到。
[0197]
此外，还可以根据载体的第一转动角速度矢量估计值、第j个第二传感器的瞬时速度矢量估计值和第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量，通过正交距离回归(orthogonal distance regression，odr)方法，确定载体的平动速度矢量估计值t2
,j
。
[0198]
本技术实施例中，第一转动角速度矢量估计值是根据m个第一传感器的转动角速
度矢量估计值确定的，其中m≥1。
[0199]
作为一种实现方式，第一转动角速度矢量估计值，可以基于如下关系式确定：
[0200]
ω＝ω
1,i
,i＝1,
…
,m,m≥1
[0201]
其中，ω为第一转动角速度矢量估计值，ω
1,i
为第i个第一传感器的转动角速度矢量估计值。
[0202]
作为一种实现方式，基于最小均方误差或者最小二乘法等确定第一转动角速度矢量估计值。
[0203]
第1种实现方式中，第一转动角速度矢量估计值是载体的m个转动角速度矢量估计值的加权和，其中，载体的m个转动角速度矢量估计值分别根据m个第一传感器的转动角速度矢量估计值确定。
[0204]
一种具体实现中，载体的第一转动角速度矢量估计值满足如下关系式：
[0205][0206]
其中，ω为载体的第一转动角速度矢量估计值，ω
1,i
为根据第i个第一传感器的转动角速度矢量估计值得到载体的转动角速度矢量估计值，w
1,i
为与第i个第一传感器对应的加权系数或者加权系数矩阵。
[0207]
具体的，加权系数w
1,i
可以根据ω
1,i
的估计误差或测量误差的概率密度函数或统计特性或协方差矩阵确定。例如，w
1,i
可以根据ω
1,i
的估计误差或测量误差的协方差矩阵确定，具体为其中，p
1,i
为ω
1,i
的估计误差或测量误差的协方差。
[0208]
第2种实现方式中，第一转动角速度矢量估计值是载体的m个转动角速度矢量估计值的均值，其中，载体的m个转动角速度矢量估计值分别根据m个第一传感器的转动角速度矢量估计值确定。
[0209]
一种具体实现中，载体的第一转动角速度矢量估计值满足如下关系式：
[0210][0211]
其中，ω为载体的第一转动角速度矢量估计值，ω
1,i
为根据第i个第一传感器的转动角速度矢量估计值得到的载体的转动角速度矢量估计值。
[0212]
作为另一种实现方式，m等于1，根据第一传感器的转动角速度矢量估计值，确定第一转动角速度矢量估计值，具体可参照上述关系式得到。
[0213]
需要指出的是，以上，第一传感器的转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、载体的第一转动角速度矢量估计值、载体的第一平动速度矢量估计值等均为相对于载体坐标系定义的。而实际应用中，传感器的测量数据往往相对于传感器坐标系定义，因此，从第一传感器或者第二传感器得到运动速度矢量，包括转动角速度矢量和平动速度矢量或者瞬时速度矢量，往往相对于所在传感器定义比较方便。此时，需要基于传感器相对于载体坐标系的外部参数得到相对于载体坐标系的转动角速度矢量和平动速度矢量或者瞬时速度矢量。
[0214]
不失一般性的，如图6所示，载体坐标系与传感器坐标系之间的变换关系通常可以用传感器的外部参数确定，外部参数可以包括旋转参数和传感器坐标系相对于载体坐标系的位置平移矢量。基于旋转参数，可将传感器坐标系的方向旋转至与载体坐标系的方向一致。基于位置平移矢量，可将传感器坐标系的原点平移至与载体坐标系的原点一致。
[0215]
传感器坐标系相对于载体坐标系的位置平移矢量可以是如图6中矢量r，或者可以是如图5中的r
21
,r
22
,
…
,r
25
；再如前述r
2,j
,j＝1,
…
,n,n≥1。
[0216]
旋转参数用于表示载体坐标系和传感器坐标系之间的旋转；具体的，可以是用四元数、旋转矩阵、欧拉角等表示。其中，四元数、旋转矩阵、欧拉角等可以互相转化。示例性的，旋转矩阵可以根据四元数得到，或旋转矩阵可以根据欧拉角得到。示例性的，传感器坐标系与载体坐标系的方向可以一致，此时，旋转参数为单位矩阵。
[0217]
本技术实施例，进一步基于传感器的外部参数和相对于传感器坐标系的运动速度矢量，得到相对于载体坐标系的运动速度矢量，其中，运动速度矢量可以包括转动角速度、平动速度矢量、瞬时运动速度矢量中的一个或多个。
[0218]
具体的，可以基于第二传感器的外部参数和相对于第二传感器坐标系的瞬时速度矢量，得到相对于载体坐标系的瞬时速度矢量，其中外部参数包括旋转参数。
[0219]
例如，旋转参数可以是旋转矩阵，相对于载体坐标系的瞬时平动速度矢量可以由以下关系式确定：
[0220]v2,j
＝r
2,jv′
2,j
[0221]
其中，v
2,j
为第j个第二传感器相对于载体坐标系的瞬时速度矢量估计值，v
′
2,j
第j个第二传感器相对于传感器坐标系的瞬时速度矢量估计值，r
2,j
为第二传感器的传感器坐标系到载体坐标系的旋转参数。
[0222]
具体的，可以基于第一传感器的外部参数和相对于第一传感器坐标系的转动角速度矢量，得到相对于载体坐标系的转动角速度矢量，其中外部参数包括旋转参数。
[0223]
例如，旋转参数可以是旋转矩阵，相对于载体坐标系的转动角速度矢量可以由以下关系式确定：
[0224]
ω
1,i
＝r
1,i
ω
′
1,i
[0225]
其中，ω
1,i
为第i个第一传感器相对于载体坐标系的转动角速度矢量估计值，ω
′
1,i
为第i个第一传感器相对于该第一传感器的坐标系的转动角速度矢量估计值，r
1,i
为第一传感器的传感器坐标系到载体坐标系的旋转矩阵。
[0226]
需要指出的是，旋转参数可以是固定值，也可以是运动估计中根据在线算法估计得到；位置平移矢量可以是固定值，也可以是运动估计中根据在线算法估计得到。本技术实施例对此不限定。
[0227]
基于上述变换关系，可以根据传感器的外部参数、m个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值。其中传感器的外部参数可以包括传感器的旋转参数。
[0228]
具体的，根据m个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值。
[0229]
作为一种实现方式，可以基于如下关系式，确定第一转动角速度矢量估计值。
[0230]
ω＝r
1,i
ω
′
1,i
[0231]
其中，ω为第一转动角速度矢量估计值，ω
′
1,i
为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
[0232]
作为一种实现方式，根据m个第一传感器中各第一传感器相对于传感器坐标系的转动角速度矢量估计值、各第一传感器的旋转参数，确定载体的第一转动角速度矢量估计值。
[0233]
在一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
[0234][0235]
其中，ω为载体的第一转动角速度矢量估计值，ω
′
1,i
为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，w
1,i
为与第i个第一传感器对应的加权系数或者加权系数矩阵，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
[0236]
在又一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
[0237][0238]
其中，ω为载体的第一转动角速度矢量估计值，ω
′
1,i
为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
[0239]
本技术实施例中，根据至少一个第一传感器的转动角速度矢量估计值，可以准确确定载体的第一转动角速度矢量估计值。结合第二传感器的瞬时速度矢量估计值，可以准确得到载体的第一平动速度矢量估计值。因此，采用本技术实施例的方法，可以实现载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
[0240]
可选地，为了进一步提升载体的平动速度矢量估计值的准确性，本技术实施例可以进一步包括如下步骤203和步骤204。
[0241]
步骤203，获取m个第一传感器中m
′
个第一传感器的归一化的平动速度矢量估计值。
[0242]
具体的，m个第一传感器中包括m
′
个第一传感器，其中m
′
≤m，该m
′
个第一传感器可以是视觉传感器例如相机或者摄像头。
[0243]
一个具体示例中，m个第一传感器包括m
′
个视觉传感器例如相机或者摄像头和m-m
′
个惯性测量传感器例如imu。
[0244]
作为一种实现方式，m
′
个第一传感器可以是视觉传感器例如相机或者摄像头，获取m
′
个第一传感器的归一化的平动速度矢量估计值，可以是根据相机或者摄像头得到的图像或者视频，根据其中的特征点、线、或者平面或者区域，基于数据的光学特性或者几何特性确定，例如基于8点法或者5点法或者单应性(homography)或者光流法等方法得到，本技术实施例不做限定。
[0245]
第一传感器的归一化的平动速度矢量估计值，理解为，根据第一传感器确定的尺度伸缩的平动速度矢量估计值，也即，第一传感器的归一化的平动速度矢量估计值与第一
传感器的平动速度矢量估计值成比例。应理解，第一传感器的平动速度矢量估计值为第一传感器实际运动的平动速度矢量估计值。
[0246]
示例性的，在第一传感器的坐标系中，第一传感器的归一化的平动速度矢量估计值可以表示为第一传感器的平动速度矢量估计值可以表示为v1′
，二者之间符合关系式：
[0247][0248]
其中，v1′
＝[v
′
1x
,v
′
1y
,v
′
1z
]
t
，s为第一传感器的平动速度矢量估计值的归一化参数或者尺度伸缩因子。
[0249]
具体的，归一化参数或者尺度伸缩因子可以是平动速度矢量估计值的幅度或者范数或者模，或平动速度矢量估计值的某一分量，如z轴分量。
[0250]
以光流法为例，可以基于以下关系式确定归一化的平动速度矢量估计值
[0251][0252][0253]
其中u,v为图像平面上的光流分量，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]；其中(p
x
,py)为中心位置，w
x
和wy为非负整数，w
x
＝0,1,2,3,4
…
；wy＝0,1,2,3,4
…
。z
′
为像素对应的目标点的相对深度，和ω为相对于传感器坐标系的归一化平动速度矢量和转动角速度矢量。
[0254]
其中归一化的平动速度矢量估计值和相对深度z
′
，满足如下关系
[0255][0256]
或者，等价地
[0257]
z＝sz
′
[0258]
其中t
′
为相对于传感器坐标系的绝对平动速度矢量，z为像素对应的目标点的绝对深度，t
′z为t
′
的z轴分量，s＝t
′z为尺度伸缩因子。需要指出的是，尺度伸缩因子s不限于t
′z，可以根据需要选择其它值，例如尺度伸缩因子s为t
′
的范数或者幅度等，如s＝‖t
′
‖。
[0259]
需要指出的是，获取m
′
个第一传感器的归一化的平动速度矢量估计值，可以进一步包括，根据第一传感器的外部参数得到相对于载体坐标系的归一化的平动速度矢量估计值，其中第一传感器的外部参数包括第一传感器坐标系相对于载体坐标系的旋转参数。
[0260]
例如，相对于载体坐标系的归一化的平动速度矢量估计值为其中，r1为第一传感器的坐标系相对于载体坐标系的旋转矩阵。
[0261]
需要指出的是，上述旋转变换为正交变换矩阵，并不改变归一化参数。下面，为方便描述，可以以归一化参数为例说明。
[0262]
步骤204，根据第一平动速度矢量估计值、m
′
个第一传感器的归一化的平动速度矢量估计值，确定载体的第二平动速度矢量估计值。
[0263]
本技术中，在确定出第一平动速度矢量估计值的基础上，可以基于m
′
个第一传感
器的归一化的平动速度矢量估计值，进一步提高载体的平动速度矢量的估计精度，此处将进一步更新得到的载体的平动速度矢量估计值称为第二平动速度矢量估计值。
[0264]
具体的，可以根据如下归一化的第一传感器的平动速度矢量、第一传感器的位置平移矢量、载体的平动速度矢量和载体的转动角速度矢量的关系，确定载体的第二平动速度矢量估计值：
[0265][0266]
其中，t为载体的平动速度矢量，ω为载体的转动角速度矢量估计值，为第一传感器的归一化的平动速度矢量估计值，r1为第一传感器相对于载体坐标系的位置平移矢量，s为第一传感器的平动速度矢量的归一化参数。
[0267]
作为一种实现方式，可以根据载体的第一平动速度矢量、载体的第一转动角速度矢量、第一传感器的归一化的平动速度矢量以及第一传感器相对于载体坐标系的位置平移矢量的关系，确定载体的第二平动速度矢量估计值：
[0268]
具体的，载体的第二平动速度矢量估计值根据以下关系式确定：
[0269][0270]
其中，为载体的第二平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数。
[0271]
具体的，载体的第二平动速度矢量估计值根据以下关系式确定：
[0272][0273]
其中，为载体的第二平动速度矢量估计值，w
′
1,i
为与第i个第一传感器对应的加权系数矩阵，ω为载体的第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数。加权系数矩阵w
′
1，i
可以根据的估计误差的协方差矩阵确定，与之前方法类似，此处不详述。
[0274]
具体的，载体的第二平动速度矢量估计值也根据以下关系式确定：
[0275][0276]
其中ω为载体的第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数。
[0277]
可选的，si具体可以根据下述关系式确定：
[0278][0279]
其中为载体的第一平动速度矢量估计值。
[0280]
作为另一种实现方式，可以根据上述载体的平动速度矢量、载体的第一转动角速
度矢量和第一传感器的平动速度矢量以及第一传感器相对于载体坐标系的位置平移矢量的关系，确定载体的第二平动速度矢量估计值,符合以下关系式：
[0281][0282]
其中，载体的第二平动速度矢量估计值由上述确定。ω为载体的第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数，si具体可以根据下述关系式确定：
[0283][0284]
其中其中为载体的第一平动速度矢量估计值。
[0285]
进一步的，基于上述两种实现方式中任一种实现方式，可以通过迭代方式确定第二平动速度矢量估计值。具体的，每次迭代可以得到一次载体的平动速度矢量估计值，最后一次迭代得到的载体的平动速度矢量估计值作为载体的第二平动速度矢量估计值。迭代实现方式可以进一步利用传感器的平动速度矢量估计值和位置平移矢量，从而提高载体的平动速度矢量的估计精度。
[0286]
具体的，在第一种迭代方式中，可以是根据如下关系式得到载体的第二平动速度矢量估计值：
[0287][0288]
其中，tk为第k次迭代中得到的载体的平动速度矢量估计值，也可以理解，第k次迭代(最后一次迭代)得到t作为载体的第二平动速度矢量估计值ω为载体的第一转动角速度矢量估计值，为第一传感器的归一化的平动速度矢量估计值，r1为第一传感器相对于载体坐标系的位置平移矢量，sk为第k次迭代中第一传感器的平动速度矢量的归一化参数。具体的，sk可以是根据如下关系式得到:
[0289]
sk＝‖t
k-1
ω
×
r1‖
[0290]
其中，t
k-1
为第k-1次迭代中得到的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r1为第一传感器相对于载体坐标系的位置平移矢量。
[0291]
作为一种具体实现，每次迭代可以是，根据m
′
个第一传感器的归一化的平动速度矢量估计值、第k-1次迭代中m
′
个第一传感器的平动速度矢量的归一化参数、m
′
个第一传感器相对于载体坐标系的位置平移矢量和第一转动角速度矢量估计值，确定第k次迭代载体的平动速度矢量估计值。其中，第k次迭代中m
′
个第一传感器的平动速度矢量的归一化参数可以根据第k-1次迭代中得到的载体的平动速度矢量估计值确定。通过迭代过程，将第k次迭代(最后一轮迭代)中得到符合预设条件的载体的平动速度矢量估计值作为载体的第二平动速度矢量估计值。
[0292]
作为一种实现方式，可以根据上述第一传感器的平动速度矢量、第一传感器的位置平移矢量与载体的平动速度矢量和载体的转动角速度矢量的关系，确定载体的第二平动速度矢量估计值，具体包括：
[0293]
载体的第二平动速度矢量估计值根据以下关系式确定：
[0294][0295]
其中，tk为第k次迭代中载体的平动速度矢量估计值，tk为第二平动速度矢量估计值ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定，w
′
1,i,k
为第k次迭代中第i个第一传感器的加权系数。
[0296]w′
1,i,k
在每次迭代中可以取固定值或者根据预设算法确定。示例性的，w
′
1,i,k
可以根据si·v1，i-ω
×r1，i
的估计误差的协方差矩阵确定，与之前方法类似，此处不详述。
[0297]
或者，载体的第二平动速度矢量估计值根据以下关系式确定：
[0298][0299]
其中，tk为第k次迭代中载体的平动速度矢量估计值，tk为第二平动速度矢量估计值ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0300]si,k
是根据第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定，可以有如下两种情况：
[0301]
k等于1，也即第一次迭代中，s
i,1
可以由第一平动速度矢量估计值根据如下关系式确定：
[0302][0303]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0304]
k大于1，也即之后的几次迭代中，s
i,k
可以由第k-1次迭代中载体的平动速度矢量估计值根据如下关系式确定：
[0305]si,k
＝‖t
k-1
ω
×r1,i
‖
[0306]
其中，t
k-1
为第k-1次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0307]
可选的，作为一种具体的实现方式，m
′
等于1，可以根据第一传感器的归一化的平动速度矢量估计值、第k-1次迭代中第一传感器的平动速度矢量的归一化参数、第一传感器的坐标系相对于载体坐标系的位置平移矢量和第一转动角速度矢量估计值，确定第k次迭代中载体的平动速度矢量估计值，具体可参照上述关系式得到。其中，第k次迭代中第一传感器的平动速度矢量的归一化参数是根据第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0308]
需要说明的是，可以将第一种迭代方式的迭代终止条件设置为，第二平动速度矢
量估计值和第一平动速度矢量估计值之间的矢量距离不大于第一预设的阈值或者门限。相当于，确定第k次迭代得到的载体的平动速度矢量估计值与第一平动速度矢量估计值之间的矢量距离，若该矢量距离大于第一预设的阈值或者门限，则进一步执行第k 1次迭代，若该矢量距离不大于第一预设的阈值或者门限，则确定迭代终止，此时，第k次迭代(也即第k次迭代，或最后一次迭代)得到的载体的平动速度矢量估计值可以称为是第二平动速度矢量估计值。
[0309]
示例性的，第二平动速度矢量估计值和第一平动速度矢量估计值满足如下关系式：
[0310][0311]
其中，threshold1为第一预设阈值或者门限。
[0312]
另外，还可以将第一种迭代方式的迭代终止条件设置为，达到最大迭代次数。相当于，设置最大迭代次数为k，也即，共计执行k次迭代，将第k次迭代(也即最后一次迭代)得到的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。示例性的，可以设置最大迭代次数k的取值为20。
[0313]
上述第一种迭代方式中，每次迭代中，根据多个第一传感器的归一化的平动速度矢量估计值，确定载体的平动速度矢量估计值。将多个第一传感器的参数作为一次迭代的输入，有助于提高第二平动速度矢量的估计精度。
[0314]
具体的，第二种迭代方式中，根据第i个第一传感器对应的载体的平动速度矢量估计值、第i个第一传感器的平动速度矢量的归一化参数、第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，确定第i个第一传感器对应的载体的平动速度矢量估计值。其中，第i个第一传感器的平动速度矢量的归一化参数是根据第i-1个第一传感器对应的载体的平动速度矢量估计值确定的。
[0315]
等价地，第二种迭代方式为根据前一个第一传感器对应的载体的平动速度矢量估计值，确定后一个第一传感器的平动速度矢量的归一化参数，进而确定后一个第一传感器对应的载体的平动速度矢量估计值，其主要适用于m
′
大于1的情况。
[0316]
可以是根据如下关系式得到的载体的平动速度矢量估计值：
[0317][0318]
其中，为第i个第一传感器对应的载体的平动速度矢量估计值，为载体的第二平动速度矢量估计值ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数，si由第一平动速度矢量估计值或确定。
[0319]
具体的，si由可以是根据如下关系式得到。
[0320][0321]
其中，为第i-1个第一传感器对应的载体的平动速度矢量估计值，为第一平动速度矢量估计值；ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0322]
此外，本技术实施例可以执行多轮迭代，每轮迭代包括m
′
个第一传感器之间的迭代。可以是根据如下关系式得到的载体的平动速度矢量估计值：
[0323][0324]
其中，为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，也可以理解，第l轮迭代(最后一轮迭代)中，t
i,l
′
为第二平动速度矢量估计值ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,l
为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数，s
i，l
由第一平动速度矢量估计值或或确定。
[0325]
具体的，s
i,l
由可以是根据如下关系式得到。
[0326][0327]
其中，为第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，为第一平动速度矢量估计值；ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0328]
需要说明的是，前一轮迭代中最后一次迭代是后一轮迭代中第一次迭代的前一次迭代，或者说，第l-1轮迭代中第m
′
次迭代是第l轮迭代中第1次迭代的前一次迭代。当然，也可以理解，第k次迭代可以是第l-1轮迭代中第m
′
次迭代，第k 1次迭代可以是第l轮迭代中第1次迭代。进一步的，后一轮迭代的参数可以由前一轮迭代的参数确定，具体的，第l轮迭代中第1个第一传感器的平动速度矢量的归一化参数由第l-1轮迭代中第m
′
个第一传感器对应的载体的平动速度矢量估计值确定。
[0329]si,l
是根据第一平动速度矢量估计值，或第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，或第l-1轮迭代中第m
′
个第一传感器对应的载体的平动速度矢量估计值确定，可以有如下三种情况：
[0330]
i等于1，l等于1，s
i,l
是根据第一平动速度矢量估计值确定，可以参照如下关系式。
[0331][0332]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，r
1,1
为第1个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0333]
i大于1，s
i,l
是根据第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值确定，可以参照如下关系式。
[0334][0335]
其中，为第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0336]
i等于1，l大于1，s
1,l
是根据第l-1轮迭代中第m
′
个第一传感器对应的载体的平动速度矢量估计值确定，可以参照如下关系式。
[0337][0338]
其中，为第l-1轮迭代中第m
′
个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
[0339]
需要说明的是，可以将第二种迭代方式的迭代终止条件设置为，第二平动速度矢量估计值和第一平动速度矢量估计值之间的矢量距离不大于第二预设阈值或门限。相当于，确定第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值与第一平动速度矢量估计值之间的矢量距离，若该矢量距离大于第二预设阈值或门限，则进一步确定第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，若该矢量距离不大于第二预设阈值或门限，则确定迭代终止，此时，可以将第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。
[0340]
示例性的，第二平动速度矢量估计值和第一平动速度矢量估计值满足如下关系式：
[0341][0342]
其中，threshold2为第二预设阈值或门限。
[0343]
另外，还可以将第二种迭代方式的迭代终止条件设置为，达到最大迭代轮数。相当于，设置最大迭代轮数为l，也即，共计执行l轮迭代，将第l轮迭代(也即最后一轮迭代)中某个第一传感器对应的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。示例性的，设置最大迭代轮数l的取值为5，设置第5轮中第m
′
个第一传感器对应的载体的平动速度矢量估计值为第二平动速度矢量估计值。
[0344]
上述第二种迭代方式中，针对每个第一传感器的归一化的平动速度矢量估计值，确定每个第一传感器对应的载体的平动速度矢量估计值，相当于，针对每个第一传感器进行一次迭代，将一个第一传感器的参数作为一次迭代的输入，可以快速得到较为准确的第二平动速度矢量估计值。
[0345]
本技术实施例中，第一传感器评估自身运动得到的归一化的平动速度矢量估计值较准确，该将第一传感器的归一化的平动速度矢量估计值和第一平动速度矢量估计值进行融合，得到载体的第二平动速度矢量估计值，相比于第一平动速度矢量估计值，可进一步提高载体的平动速度矢量估计值的精确度。采用载体的第一转动角速度矢量估计值和载体的第二平动速度矢量估计值对载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
[0346]
本技术实施例中，m'个第一传感器中的第一传感器可以理解是可以获得归一化的平动速度矢量估计值的第一传感器，m'个第一传感器可以是视觉传感器，例如相机、摄像头、红外传感器等。一个示例中，m'个第一传感器可以是m1'个相机和m2'个红外传感器，其中，m1' m2'＝m'，且m1'≥0，m2'≥0。
[0347]
此外，本技术实施例还可以进一步结合m-m'个可以获取平动速度矢量估计值的第一传感器，其中m-m
′
个第一传感器获取的平动速度矢量估计值包含速度矢量的尺度信息，例如各个速度分量的完整信息，而不仅仅是归一化值或者方向信息，从而提高载体的第二平动速度矢量估计值的准确性。
[0348]
也即，本技术提供再一种确定第二平动速度矢量估计值的方式如下。
[0349]
在步骤203中可以是获取m
′
个第一传感器如视觉传感器的归一化的平动速度矢量估计值和m-m
′
个第一传感器如惯性传感器(如imu)的平动速度矢量估计值。
[0350]
在步骤204中，先根据m-m
′
个第一传感器如惯性传感器的平动速度矢量估计值，确定m-m
′
个第一传感器如惯性传感器对应的载体的平动速度矢量估计值，
[0351]
示例性的，可基于关系式或确定，关系式的具体描述可参照上述实现方式。
[0352]
作为一种直接实现方式，可以基于关系式或或确定视觉传感器对应的载体的平动速度矢量估计值，将视觉传感器对应的载体的平动速度矢量估计值、惯性传感器对应的载体的平动速度矢量估计值进行加权，得到第二平动速度矢量估计值。
[0353]
作为一种迭代的实现方式，可以将惯性传感器对应的载体的平动速度矢量估计值作为第一次迭代中的载体的平动速度矢量估计值，也即，k等于1中对应的中，替换为惯性传感器对应的载体的平动速度矢量估计值。基于步骤204中的迭代方式进行迭代，直至符合迭代条件，将符合迭代条件的第k次迭代中的载体的平动速度矢量估计值作为第二平动速度矢量估计值。
[0354]
进一步的，在本技术中，还可能存在m
′
＝0，也即，在步骤203中获取m个惯性传感器的平动速度矢量估计值。
[0355]
相应的，在步骤204中，可以根据m个惯性传感器的平动速度矢量估计值，确定惯性传感器对应的载体的平动速度矢量估计值，示例性的，可基于关系式传感器对应的载体的平动速度矢量估计值，示例性的，可基于关系式或确定惯性传感器对应的载体的平动速度矢量估计值，关系式的具体描述可参照上述实现方式。将惯性传感器对应的载体的平动速度矢量估计值和第一平动速度矢量估计值进行加权得到第二平动速度矢量估计值。
[0356]
可选地，根据上述第二平动速度矢量估计值，可以进一步更新载体的转动角速度矢量估计值。此处将更新的该转动角速度矢量估计值称为是载体的第二转动角速度矢量估计值。
[0357]
可选的，步骤205，根据载体的第二平动速度矢量估计值，确定载体的第二转动角速度矢量估计值。
[0358]
可以基于如下关系式，确定第二转动角速度矢量估计值：
[0359][0360]
或者，等价地
[0361][0362]
或者，等价地
[0363][0364]
其中，ω
′
为第二转动角速度矢量估计值，v2为第二传感器的瞬时速度矢量估计
值，为第二平动速度矢量估计值，r2为第二传感器的坐标系相对于载体坐标系的位置平移矢量，
×
表示矢量的叉积。其中[r2]
×
为与r2对应的反对称矩阵。
[0365]
基于上述关系式，第二转动角速度矢量估计值的表达式可以为
[0366]
作为一种实现方式，可以基于如下关系式，确定第二转动角速度矢量估计值：
[0367][0368]
其中，ω
′
为第二转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2，j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0369]
其中，r
2,j
＝[r
x,2,j r
y,2,j r
z,2,j
]
t
，
[0370]
作为一种实现方式，根据第二转动角速度矢量估计值、n个第二传感器的瞬时速度矢量估计值、n个第二传感器中各第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第二转动角速度矢量估计值，其中n≥1。
[0371]
在一个示例中，基于如下关系式确定第二转动角速度矢量估计值：
[0372][0373]
其中，ω
′
为第二转动角速度矢量估计值，w
2,j
为第j个第二传感器的加权系数，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0374]
在又一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
[0375][0376]
其中，ω
′
为第二转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0377]
作为另一种实现方式中，n等于1，根据第二转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第二转动角速度矢量估计值，具体可参照上述关系式得到。
[0378]
本技术中，可以将载体的第二平动速度矢量估计值和载体的第二转动角速度矢量估计值，分别作为最终确定出的载体的平动速度矢量估计值和载体的转动角速度矢量估计值。
[0379]
此外，还可以将第一转动角速度矢量估计值和第二转动角速度矢量估计值进一步
融合，将融合后的转动角速度矢量估计值作为最终确定的载体的转动角速度矢量估计值。具体融合中，可以是确定载体的第一转动角速度矢量估计值和载体的第二转动角速度矢量估计值的平均值。或者，基于最小均方误差进行加权组合，将载体的第一转动角速度矢量估计值和载体的第二转动角速度矢量估计值进行融合。
[0380]
本技术实施例中，根据载体的第二平动速度矢量估计值，确定载体的第二转动角速度矢量估计值，相比于第一转动角速度矢量估计值，可进一步提高载体的转动角速度矢量估计值的精确度。采用载体的第二转动角速度矢量估计值和载体的第二平动速度矢量估计值对载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
[0381]
本文中描述的各个实施例可以为独立的方案，也可以根据内在逻辑进行组合，这些方案都落入本技术的保护范围中。
[0382]
本技术实施例中对模块的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本技术各个实施例中的各功能模块可以集成在一个处理器中，也可以是单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
[0383]
与上述构思相同，本技术实施例还提供一种运动估计装置用于实现上述方法。
[0384]
示例性的，该运动估计装置可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该运动估计装置还可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。
[0385]
该运动估计装置可为具有运动估计功能的车辆，或者为具有运动估计功能的其他部件。该运动估计装置包括但不限于：车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或车载摄像头等其他传感器，车辆可通过该车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或摄像头，实施本技术提供的方法。
[0386]
该运动估计装置还可以为除了车辆之外的其他具有运动估计功能的智能终端，或设置在除了车辆之外的其他具有运动估计功能的智能终端中，或设置于该智能终端的部件中。该智能终端可以为智能运输设备、智能家居设备、机器人等其他终端设备。该运动估计装置包括但不限于智能终端或智能终端内的控制器、芯片、雷达或摄像头等其他传感器、以及其他部件等。
[0387]
该运动估计装置可以是一个通用设备或者是一个专用设备。在具体实现中，该运动估计装置还可以台式机、便携式电脑、网络服务器、掌上电脑(personal digital assistant，pda)、移动手机、平板电脑、无线终端设备、嵌入式设备或其他具有处理功能的设备。本技术实施例不限定该运动估计装置的类型。
[0388]
该运动估计装置还可以是具有处理功能的芯片或处理器，该运动估计装置可以包括多个处理器。处理器可以是一个单核(single-cpu)处理器，也可以是一个多核(multi-cpu)处理器。该具有处理功能的芯片或处理器可以设置在传感器中，也可以不设置在传感器中，而设置在传感器输出信号的接收端。
[0389]
如图7为本技术示例性提供的一种运动估计装置700。该运动估计装置700可以包括：获取单元701和处理单元702。应理解，装置实施例的描述与方法实施例的描述相互对
应，因此，未详细描述的内容可以参见上文方法实施例，为了简洁，这里不再赘述。
[0390]
示例性的，所述获取单元701用于获取m个第一传感器的转动角速度矢量估计值和n个第二传感器的瞬时速度矢量估计值；其中，m≥1，n≥1；
[0391]
所述处理单元702用于根据所述n个第二传感器的瞬时速度矢量估计值和所述n个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述m个第一传感器的转动角速度矢量估计值确定的。
[0392]
在一种可能的实现方式中，所述处理单元702具体用于基于如下关系式确定所述第一平动速度矢量估计值：
[0393][0394]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0395]
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
[0396][0397]
其中，为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w
2,j
为第j个第二传感器的加权系数，w
2,j
为第j个第二传感器的瞬时速度矢量估计值，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0398]
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
[0399][0400]
其中，ω为第一转动角速度矢量估计值，w
1,i
为第i个第一传感器的加权系数，ω
1,i
为第i个第一传感器的转动角速度矢量估计值。
[0401]
在一种可能的实现方式中，所述获取单元701还用于获取所述m个第一传感器中m
′
个第一传感器的归一化的平动速度矢量估计值，其中，1≤m
′
≤m；所述处理单元702还用于根据所述第一平动速度矢量估计值、所述m
′
个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
[0402]
在一种可能的实现方式中，所述处理单元702具体用于基于如下关系式确定所述第二平动速度矢量估计值：
[0403][0404]
其中，为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，si为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，si由第一平动速度矢量估计值确定。
[0405]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0406][0407]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0408]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0409][0410]
其中，tk为第二平动速度矢量估计值tk为第k次迭代中载体的平动速度矢量估计值，w
′
1,i,k
为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,k
为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,k
由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
[0411]
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0412]si,k
＝‖t
k-1
ω
×r1,i
‖
[0413]
其中，t
k-1
为第k-1次迭代中载体的平动速度矢量估计值，t0为第一平动速度矢量估计值。
[0414]
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
[0415][0416]
其中，为第二平动速度矢量估计值为第二平动速度矢量估计值为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，为第i个第一传感器的归一化的平动速度矢量估计值，r
1,i
第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s
i,l
为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s
i,l
由第一平动速度矢量估计值或或确定。
[0417]
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
[0418][0419]
其中，为第一平动速度矢量估计值；
[0420]
在一种可能的实现方式中，所述处理单元702还用于基于如下关系式确定所述载体的第二转动角速度矢量估计值：
[0421][0422]
其中，ω
′
为第二转动角速度矢量估计值，v
2,j
为第j个第二传感器的瞬时速度矢量
估计值，为第二平动速度矢量估计值，为[r
2,j
]
×
的逆矩阵，[r
2,j
]
×
为与r
2,j
对应的反对称矩阵，r
2,j
为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
[0423]
与上述构思相同，如图8所示，为本技术实施例提供的一种芯片的结构示意图。
[0424]
芯片800包括一个或多个处理器801以及接口电路802。可选的，所述芯片800还可以包含总线803。
[0425]
其中，处理器801可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器801中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器801可以是通用处理器、数字通信器(dsp)、专用集成电路(asic)、现场可编程门阵列(fpga)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本技术实施例中的公开的各方法、步骤。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0426]
接口电路802可以用于数据、指令或者信息的发送或者接收，处理器801可以利用接口电路802接收的数据、指令或者其它信息，进行加工，可以将加工完成信息通过接口电路802发送出去。
[0427]
可选的，芯片还包括存储器，存储器可以包括只读存储器和随机存取存储器，并向处理器提供操作指令和数据。存储器的一部分还可以包括非易失性随机存取存储器(nvram)。
[0428]
可选的，存储器存储了可执行软件模块或者数据结构，处理器可以通过调用存储器存储的操作指令(该操作指令可存储在操作系统中)，执行相应的操作。
[0429]
可选的，芯片可以使用在本技术实施例涉及的通信装置(包括主节点和从节点)中。可选的，接口电路802可用于输出处理器801的执行结果。关于本技术的一个或多个实施例提供的数据传输方法可参考前述各个实施例，这里不再赘述。
[0430]
需要说明的，处理器801、接口电路802各自对应的功能既可以通过硬件设计实现，也可以通过软件设计来实现，还可以通过软硬件结合的方式来实现，这里不作限制。
[0431]
本技术实施例还提供一种雷达系统，用于为车辆提供运动估计功能。其包含至少一个本技术上述实施例提到的运动估计装置，该系统内的至少一个运动估计装置可以集成为一个整机或设备，或者该系统内的至少一个运动估计装置也可以独立设置为元件或装置。
[0432]
本技术实施例还提供一种传感器系统，用于为车辆提供运动估计功能。其包含至少一个本技术上述实施例提到的运动估计装置，以及，摄像头或雷达等传感器中的至少一个，该系统内的至少一个传感器装置可以集成为一个整机或设备，或者该系统内的至少一个传感器装置也可以独立设置为元件或装置。
[0433]
本技术实施例还提供一种系统，应用于无人驾驶或智能驾驶中，其包含至少一个本技术上述实施例提到的运动估计装置、摄像头、雷达等传感器中的至少一个，该系统内的至少一个装置可以集成为一个整机或设备，或者该系统内的至少一个装置也可以独立设置为元件或装置。
[0434]
进一步，上述任一系统可以与车辆的中央控制器进行交互，为所述车辆驾驶的决策或控制提供探测和/或融合信息。
[0435]
本技术实施例还提供一种车辆，所述车辆包括至少一个本技术上述实施例提到的运动估计装置或上述任一系统。
[0436]
本技术实施例还提供一种通信装置，包括处理器和通信接口，所述通信接口用于接收来自所述通信装置之外的其它通信装置的信号并传输至所述处理器或将来自所述处理器的信号发送给所述通信装置之外的其它通信装置，所述处理器通过逻辑电路或执行代码指令用于实现上述如图2中的方法。
[0437]
本技术实施例还一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述如图2中的方法。
[0438]
本技术实施例还提供一种计算机程序产品，所述计算机程序产品包括计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述如图2中的方法。
[0439]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、光学存储器等)上实施的计算机程序产品的形式。
[0440]
本技术是参照根据本技术的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0441]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0442]
显然，本领域的技术人员可以对本技术进行各种改动和变型而不脱离本技术的范围。这样，倘若本技术的这些修改和变型属于本技术权利要求及其等同技术的范围之内，则本技术也意图包含这些改动和变型在内。