一种状态信息估计方法及装置制造方法及图纸

本发明专利技术实施例公开一种状态信息估计方法及装置，该方法包括：获得在当前时刻目标对象的当前图像及当前传感器数据；利用前一时刻对应的IMU数据及前一状态信息，确定目标对象的初始状态信息；利用各当前图像的特征点，确定各当前图像的第一匹配点对；利用各当前图像中的特征点及前一图像中的特征点，确定各当前图像与前一图像的第二匹配点对；基于当前图像的第一匹配点对、第二匹配点对及前N时刻图像的第一匹配点对、第二匹配点对，确定各待利用特征点对应的三维位置信息；基于三维位置信息、各待利用特征点的图像位置信息、初始状态信息及当前传感器数据，确定目标对象的当前状态信息，以实现对包含任意数量图像采集设备的对象的状态信息的估计。的状态信息的估计。的状态信息的估计。

【技术实现步骤摘要】
一种状态信息估计方法及装置


[0001]本专利技术涉及自动化
，具体而言，涉及一种状态信息估计方法及装置。

技术介绍

[0002]在车辆自动驾驶及机器人运动的建图方案和定位方案中，如何通过安装在车辆或者机器人上的各种传感器，估计得到其精准可靠的运动状态信息是一个非常重要的问题。估计目标即车辆或者机器人的运动状态信息的系统可以称为机器人的状态估计器。
[0003]相关技术中，用于估计目标的运动状态信息的方法一般分为滤波与优化两种方案。其中，滤波方案实时性好、精度较高，更易于轻量地部署在车辆自动驾驶及机器人运动的解决方案中。
[0004]目前，为了保证自动驾驶车辆的安全行驶，自动驾驶车辆常通常安装有多个不同朝向的相机，以针对车辆的四周环境进行拍摄，获取四周环境信息，进而结合其他多传感器所采集的传感器数据，进行定位，以保证车辆的准确定位进而保证车辆的安全行驶。
[0005]目前的滤波方案并没有针对包含任意数量相机的多传感器系统的车辆状态信息估计能力。那么，如何提供一种针对包含任意数量相机的多传感器系统的车辆状态信息进行估计的方法成为亟待解决的问题。

技术实现思路

[0006]本专利技术提供了一种状态信息估计方法及装置，以实现对包含任意数量图像采集设备的多传感器系统的对象的状态信息的估计。具体的技术方案如下：
[0007]第一方面，本专利技术实施例提供了一种状态信息估计方法，所述方法包括：
[0008]获得在当前时刻目标对象所设置的多图像采集设备采集的当前图像及其他传感器采集的当前传感器数据，其中，所述其他传感器包括IMU；
[0009]利用所述当前时刻的前一时刻对应的IMU数据以及所述前一时刻所述目标对象的前一状态信息，确定所述目标对象在所述当前时刻的初始状态信息；
[0010]利用各当前图像所检测出的特征点以及各当前图像对应的图像采集设备之间的相对位姿关系，确定各当前图像之间的匹配点对，作为当前图像对应的第一匹配点对；
[0011]利用各当前图像所检测出的特征点及其前一图像所检测出的特征点，确定各当前图像与其前一图像之间的匹配点对，作为当前图像对应的第二匹配点对；
[0012]基于当前图像对应的第一匹配点对及第二匹配点对以及所述当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对，确定各待利用特征点对应的三维位置信息；
[0013]基于所述三维位置信息、各待利用特征点的图像位置信息、所述初始状态信息及所述当前传感器数据，确定所述目标对象在当前时刻的当前状态信息。
[0014]可选的，所述初始状态信息包括：初始速度信息及初始位姿信息，其中，所述初始位姿信息包括：初始姿态信息以及初始位置信息；
[0015]所述利用所述当前时刻的前一时刻对应的IMU数据以及所述前一时刻所述目标对
象的前一状态信息，确定所述目标对象在所述当前时刻的初始状态信息的步骤，包括：
[0016]利用所述当前时刻的前一时刻对应的IMU数据，确定所述前一时刻所述目标对象的角速度信息和加速度信息；
[0017]利用所述前一时刻的角速度信息以及所述前一状态信息中的前一姿态信息，构建第一状态转移方程；利用所述第一状态转移方程确定所述目标对象在所述当前时刻的初始姿态信息；
[0018]利用所述前一姿态信息、所述前一时刻的加速度信息以及所述前一状态信息中的前一速度信息，构建第二状态转移方程；利用所述第二状态转移方程确定所述目标对象在所述当前时刻的初始速度信息；
[0019]利用所述初始速度信息、所述前一速度信息以及所述前一状态信息中的前一位置信息，构建第三状态转移方程；利用所述第三状态转移方程确定所述目标对象在所述当前时刻的初始位置信息。
[0020]可选的，所述基于当前图像对应的第一匹配点对及第二匹配点对以及当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对，确定各待利用特征点对应的三维位置信息的步骤，包括：
[0021]按照三角测量算法，基于当前图像对应的第一匹配点对及第二匹配点对、当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对、各当前图像对应的图像采集设备的设备位姿信息、各前N时刻图像对应的图像采集设备的设备位姿信息以及所述当前图像及各前N时刻图像对应的目标对象的位姿信息，确定各待利用特征点对应的三维位置信息。
[0022]可选的，所述基于所述三维位置信息、所述各待利用特征点的图像位置信息、所述初始状态信息以及所述当前传感器数据，确定所述目标对象在当前时刻的当前状态信息的步骤，包括：
[0023]基于所述当前传感器数据以及所述初始状态信息，确定所述目标对象在当前时刻的中间状态信息；
[0024]利用所述三维位置信息、所述中间状态信息中的中间位姿信息、前N时刻图像各时刻图像所对应目标对象的状态信息中的对象位姿信息以及各图像采集设备的设备位姿信息和内参矩阵，确定各待利用特征点对应的空间点在其所在图像中投影点的投影位置信息；
[0025]基于各待利用特征点对应的投影位置信息和各待利用特征点在当前图像中的图像位置信息，构建重投影误差方程；
[0026]基于所述重投影误差方程，确定所述目标对象在当前时刻的当前状态信息。
[0027]可选的，所述基于所述重投影误差方程，确定所述目标对象在当前时刻的当前状态信息的步骤，包括：
[0028]基于所述重投影误差方程，构建目标测量方程；
[0029]利用所述目标测量方程以及滤波更新方程，确定所述目标对象在当前时刻的当前状态信息。
[0030]第二方面，本专利技术实施例提供了一种状态信息估计装置，所述装置包括：
[0031]第一获得模块，被配置为获得在当前时刻目标对象所设置的多图像采集设备采集的当前图像及其他传感器采集的当前传感器数据，其中，所述其他传感器包括IMU；
[0032]第一确定模块，被配置为利用所述当前时刻的前一时刻对应的IMU数据以及所述前一时刻所述目标对象的前一状态信息，确定所述目标对象在所述当前时刻的初始状态信息；
[0033]第二确定模块，被配置为利用各当前图像所检测出的特征点以及各当前图像对应的图像采集设备之间的相对位姿关系，确定各当前图像之间的匹配点对，作为当前图像对应的第一匹配点对；
[0034]第三确定模块，被配置为利用各当前图像所检测出的特征点及其前一图像所检测出的特征点，确定各当前图像与其前一图像之间的匹配点对，作为当前图像对应的第二匹配点对；
[0035]第四确定模块，被配置为基于当前图像对应的第一匹配点对及第二匹配点对以及所述当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对，确定各待利用特征点对应的三维位置信息；
[0036]第五确定模块，被配置为基于所述三维位置信息、各待利用特征点的图像位置信息、所述初始状态信息及所述当前传感器数据，确定所述目标对象在当前时刻的当前状态信息。
[0037]可选的，所述初始状态信息包括：初始速度本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种状态信息估计方法，其特征在于，所述方法包括：获得在当前时刻目标对象所设置的多图像采集设备采集的当前图像及其他传感器采集的当前传感器数据，其中，所述其他传感器包括IMU；利用所述当前时刻的前一时刻对应的IMU数据以及所述前一时刻所述目标对象的前一状态信息，确定所述目标对象在所述当前时刻的初始状态信息；利用各当前图像所检测出的特征点以及各当前图像对应的图像采集设备之间的相对位姿关系，确定各当前图像之间的匹配点对，作为当前图像对应的第一匹配点对；利用各当前图像所检测出的特征点及其前一图像所检测出的特征点，确定各当前图像与其前一图像之间的匹配点对，作为当前图像对应的第二匹配点对；基于当前图像对应的第一匹配点对及第二匹配点对以及所述当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对，确定各待利用特征点对应的三维位置信息；基于所述三维位置信息、各待利用特征点的图像位置信息、所述初始状态信息及所述当前传感器数据，确定所述目标对象在当前时刻的当前状态信息。2.如权利要求1所述的方法，其特征在于，所述初始状态信息包括：初始速度信息及初始位姿信息，其中，所述初始位姿信息包括：初始姿态信息以及初始位置信息；所述利用所述当前时刻的前一时刻对应的IMU数据以及所述前一时刻所述目标对象的前一状态信息，确定所述目标对象在所述当前时刻的初始状态信息的步骤，包括：利用所述当前时刻的前一时刻对应的IMU数据，确定所述前一时刻所述目标对象的角速度信息和加速度信息；利用所述前一时刻的角速度信息以及所述前一状态信息中的前一姿态信息，构建第一状态转移方程；利用所述第一状态转移方程确定所述目标对象在所述当前时刻的初始姿态信息；利用所述前一姿态信息、所述前一时刻的加速度信息以及所述前一状态信息中的前一速度信息，构建第二状态转移方程；利用所述第二状态转移方程确定所述目标对象在所述当前时刻的初始速度信息；利用所述初始速度信息、所述前一速度信息以及所述前一状态信息中的前一位置信息，构建第三状态转移方程；利用所述第三状态转移方程确定所述目标对象在所述当前时刻的初始位置信息。3.如权利要求1所述的方法，其特征在于，所述基于当前图像对应的第一匹配点对及第二匹配点对以及当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对，确定各待利用特征点对应的三维位置信息的步骤，包括：按照三角测量算法，基于当前图像对应的第一匹配点对及第二匹配点对、当前时刻的前N时刻图像对应的第一匹配点对及第二匹配点对、各当前图像对应的图像采集设备的设备位姿信息、各前N时刻图像对应的图像采集设备的设备位姿信息以及所述当前图像及各前N时刻图像对应的目标对象的位姿信息，确定各待利用特征点对应的三维位置信息。4.如权利要求1
‑
3任一项所述的方法，其特征在于，所述基于所述三维位置信息、所述各待利用特征点的图像位置信息、所述初始状态信息以及所述当前传感器数据，确定所述目标对象在当前时刻的当前状态信息的步骤，包括：基于所述当前传感器数据以及所述初始状态信息，确定所述目标对象在当前时刻的中
间状态信息；利用所述三维位置信息、所述中间状态信息中的中间位姿信息、前N时刻图像各时刻图像所对应目标对象的状态信息中的对象位姿信息以及各图像采集设备的设备位姿信息和内参矩阵，确定各待利用特征点对应的空间点在其所在图像中投影点的投影位置信息；基于各待利用特征点对应的投影位置信息和各待利用特征点在当前图像中的图像位置信息，构建重投影误差方程；基于所述重投影误差方程，确定所述目标对象在当前时刻的当前状态信息。5.如权利要求4所述的方法，其特征在于，所述基于所述重投影误差方程，确定所述目标对象在当前时刻的当前状态信息的步骤，包括：基于所述重投影误差方程...

【专利技术属性】
技术研发人员：蔡之奡，李天威，王笑非，穆北鹏，刘一龙，童哲航，王宇桐，
申请(专利权)人：魔门塔苏州科技有限公司，
类型：发明
国别省市：