[发明专利]一种基于图像传感器和惯性传感器的定位方法及系统

权利要求书

1.一种基于图像传感器和惯性传感器的定位方法，其特征在于，所述定位方法包括如下步骤：

获取定位目标上部署的特征图案基于图像传感器的位姿信息；

所述获取定位目标上部署的特征图案基于图像传感器的位姿信息，具体包括：

采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；

将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；

采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；

根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；

其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息；

获取定位目标上部署的特征图案基于惯性传感器的位姿信息；

所述获取定位目标上部署的特征图案基于惯性传感器的位姿信息，具体包括：

对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标上部署的特征图案基于惯性传感器的位姿信息；

利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息；

所述利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息，具体包括：

利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值；

其中，x_k表示k时刻基于惯性传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，e_k表示k时刻基于图像传感器所获得位姿信息和基于惯性传感器所获得的位姿信息的差值；

判断所述差值是否小于预设阈值，得到判断结果；

若所述判断结果表示否，则优化基于惯性传感器的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值”；

若所述判断结果表示是，则输出优化后的基于惯性传感器的位姿信息作为定位目标的方位信息。

2.一种基于图像传感器和惯性传感器的定位系统，其特征在于，所述定位系统包括：

第一位姿信息获取模块，用于获取定位目标上部署的特征图案基于图像传感器的位姿信息；

所述第一位姿信息获取模块，具体包括：

标定子模块，用于采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；

区域图像获取子模块，用于将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；

定位目标图案识别子模块，用于采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；

坐标转换子模块，用于根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；

其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息；

第二位姿信息获取模块，用于获取定位目标上部署的特征图案基于惯性传感器的位姿信息；

所述第二位姿信息获取模块，具体包括：

二次积分子模块，用于对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标上部署的特征图案基于惯性传感器的位姿信息；

位姿信息优化模块，用于利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息；

所述位姿信息优化模块，具体包括：

差值计算子模块，用于利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值；

其中，x_k表示k时刻基于惯性传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，e_k表示k时刻基于图像传感器所获得位姿信息和基于惯性传感器所获得的位姿信息的差值；

判断子模块，用于判断所述差值是否小于预设阈值，得到判断结果；

位姿信息优化子模块，用于若所述判断结果表示否，则优化基于惯性传感器的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值”；

优化后的位姿信息输出子模块，用于若所述判断结果表示是，则输出优化后的基于惯性传感器的位姿信息作为定位目标的方位信息。