本发明涉及slam，尤其涉及一种相机与imu联合标定方法。
背景技术：
1、在slam(simultaneous localization and mapping，同时定位与地图构建)中，若仅使用相机或者imu用来建图或者定位，精度都不高、鲁棒性不好。若仅使用相机，在纹理少的区域或者运动速度快(导致图像模糊)不能继续建图或者建图误差较大；若仅使用imu，由于imu容易受噪音干扰，时间一长就会有较大误差。因此，就有了相机和imu融合使用的方案提出，相机在纹理较好的地方，使用相机去校准imu，当出现在纹理较弱或者无纹理或者图像较模糊时，使用imu来提供建图和定位的信息，以此来提高建图和定位的精度和鲁棒性。
2、在对相机和imu的数据进行融合时，需要相机或者imu传感器能在同一坐标系下进行描述。标定的目的就是让两传感器之间能转换到相同坐标系下描述运动信息，并使得误差最小。
3、目前尚没有一种相机与imu联合标定的方法可以标定imu和相机之间的变换矩阵以及两个传感器之间的时间戳配准，从而方便融合时在同一参考坐标系下进行描述。
技术实现思路
1、本发明提供一种相机与imu联合标定方法，旨在解决现有技术中的缺陷，实现相机和imu的联合标定，效率高，准确度高。
2、为达到上述目的，本发明所采取的技术方案为：
3、本发明提供一种相机与imu联合标定方法，包括：
4、步骤1、对目标相机和目标imu进行初始化，获得初始化参数，所述初始化参数包括目标相机和目标imu时间的时间差、目标相机和目标imu之间坐标系的变换矩阵、目标imu陀螺仪的偏置、重力加速度在世界坐标系的值；
5、步骤2、构造第一误差l1，并使用lm算法进行迭代优化一次，所述第一误差其中，erepro表示重投影误差，ea表示加速度误差，eg表示角速度误差，表示角速度随机游走引起的误差；
6、步骤3、更新所述初始化参数及所述目标imu加速度的偏置，判断由所述第一误差确定的总误差是否小于设置的阈值，是则进入步骤7，否则进入下一步；
7、步骤4、根据更新后的所述目标imu各时刻的陀螺仪和加速度的偏置，分别得到陀螺仪噪声和加速度噪声；
8、步骤5、根据所述加速度噪声及第一误差l1构造第二误差l2，所述第二误差其中，表示加速度随机游走引起的误差；
9、步骤6、对所述第二总误差l2使用lm算法进行迭代优化，判断误差是否小于预设阈值，是则进入下一步，否则进入步骤3；
10、步骤7、获取标定量。
11、具体地，所述步骤1包括：
12、步骤101、将目标相机和目标imu安装在固定装置上，并采集所述目标相机拍摄的图片以及所述目标imu输出的加速度和角速度；
13、步骤102、根据所述目标相机拍摄的图片使用特征匹配解算出所述目标相机的离散位姿；
14、步骤103、对所述目标imu使用预积分计算各采集时刻的位姿；
15、步骤104、根据所述目标相机的离散位姿得到所述目标相机的旋转向量曲线和平移曲线；
16、步骤105、根据所述目标imu的加速度和角速度分别得到imu的加速度曲线和角速度曲线；
17、步骤106、根据所述旋转向量曲线、平移曲线、加速度曲线和角速度曲线，对所述目标相机和目标imu进行初始化，获得初始化参数，所述初始化包括：初始化所述目标相机和目标imu时间的时间差，初始化所述变换矩阵t，初始化所述目标imu各时刻陀螺仪的偏置，初始化重力加速度在世界坐标系的值。
18、具体地，所述步骤102包括：
19、步骤1021、对所述目标相机拍摄的图片进行特征提取与匹配；
20、步骤1022、根据匹配结果计算出基础矩阵或单应矩阵对应的变换矩阵；
21、步骤1023、根据所述变换矩阵计算所述目标相机的离散位姿，所述离散位姿包括旋转和位移。
22、具体地，初始化所述目标相机和目标imu时间的时间差包括：
23、步骤a1、根据所述旋转向量曲线及第一预设关系式获得所述目标imu采集时刻对应的所述目标相机时间点的旋转向量；
24、步骤a2、根据所述旋转向量获取所述目标相机任意采集时刻在相机坐标系下的角速度；
25、步骤a3、将所述目标imu的角速度转到同一坐标系下，并使两者角速度误差总和最小；
26、步骤a4、根据第二预设关系式计算所述时间差的初始值。
27、具体地，所述第一预设关系式为：
28、timu＝tcam+td
29、其中，td为时间差，tcam表示相机时刻，timu表示imu采集时刻；
30、所述第二预设关系式为：
31、
32、其中，表示imu在tj时刻的角速度，表示相机在tj-td时刻的角速度。
33、具体地，初始化所述目标相机和目标imu之间坐标系的变换矩阵t、所述目标imu各时刻陀螺仪的偏置包括：
34、步骤b1、根据所述旋转向量曲线获取所述目标相机任意采集时刻在所述相机坐标系下的角速度；
35、步骤b2、将同一采集时刻的所述目标imu的角速度转到同一坐标系下；
36、步骤b3、获取所述目标相机到所述目标imu之间的旋转，根据第三预设公式确定各时刻陀螺仪的偏置；
37、步骤b4、根据所述平移曲线确定任意时刻所述目标相机的平移；
38、步骤b5、将所述目标相机的平移转换到所述目标imu坐标系下得到所述目标相机位移在所述目标imu坐标系下的表示；
39、步骤b6、所述目标imu利用预积分得到位移；
40、步骤b7、通过第四预设关系式确定出所述位移的初始值。
41、具体地，所述第三预设关系式为：
42、
43、其中，ric表示所述目标相机到所述目标imu之间的旋转，表示imu在tj时刻的角速度，表示相机在tj-td时刻的角速度，表示在imu第j个时刻陀螺仪的偏置，*上标表示该参数待优化。
44、具体地，所述第四预设关系式为：
45、
46、其中，表示tj时刻世界坐标系原点到所述目标imu坐标系的平移；表示tj-td时刻世界坐标系到相机坐标系的平移，ric表示所述目标相机到所述目标imu之间的旋转。
47、具体地，所述初始化重力加速度在世界坐标系的值包括：根据第五预设关系式确定重力加速度在世界坐标系下的初始值，所述第五预设关系式为：
48、
49、其中，ric表示所述目标相机到所述目标imu之间的旋转，表示第j个imu采集时刻得到的目标imu加速度；表示第j个imu采集时刻对应的目标相机在世界坐标系下的旋转；gw表示重力加速度在世界坐标系下的值，*上标表示该参数待优化。
50、具体地，所述重投影误差由第六预设关系式确定，所述第六预设关系式为：
51、
52、其中，znm为第n个位姿下的第m个真实投影点，表示在第n个相机位姿下第m个投影点的预测值，参数中的尖括号表示该参数中含有待优化的量，∑表示信息矩阵。
53、具体地，所述加速度误差由第七预设关系式确定，所述第七预设关系式为：
54、
55、其中，表示imu第j个时刻世界坐标到相机坐标系的旋转；表示imu第j个时刻，相机在世界坐标系下的加速度；表示在imu坐标系下第j时刻的加速度；表示imu第j个采集时刻加速度的偏置，初始都为0；参数中的尖括号表示该参数中含有待优化的量。
56、具体地，所述角速度误差由第八预设关系式确定，所述第七预设关系式为：
57、
58、其中，表示imu在第j个采集时刻相机在世界坐标系下的角速度；表示第j个imu采集时刻陀螺仪的偏置，可从初始化过程中得到初值。上面带有尖括号的量表示待优化的变量。
59、本发明的有益效果在于：本发明通过对目标相机和目标imu进行初始化，获得初始化参数，并通过第一误差对初始化参数进行第一次迭代，然后根据第二误差对全部观测参数进行优化迭代，从而获取标定量，实现了相机和imu的联合标定，效率高，准确度高。
1.一种相机与imu联合标定方法，其特征在于，包括：
2.根据权利要求1所述的相机与imu联合标定方法，其特征在于，所述步骤1包括：
3.根据权利要求2所述的相机与imu联合标定方法，其特征在于，所述步骤102包括：
4.根据权利要求1所述的相机与imu联合标定方法，其特征在于，初始化所述目标相机和目标imu时间的时间差包括：
5.根据权利要求4所述的相机与imu联合标定方法，其特征在于，所述第一预设关系式为：
6.根据权利要求1所述的相机与imu联合标定方法，其特征在于，初始化所述目标相机和目标imu之间坐标系的变换矩阵t、所述目标imu各时刻陀螺仪的偏置包括：
7.根据权利要求6所述的相机与imu联合标定方法，其特征在于，所述第三预设关系式为：
8.根据权利要求1所述的相机与imu联合标定方法，其特征在于，所述初始化重力加速度在世界坐标系的值包括：根据第五预设关系式确定重力加速度在世界坐标系下的初始值，所述第五预设关系式为：
9.根据权利要求1所述的相机与imu联合标定方法，其特征在于，所述重投影误差由第六预设关系式确定，所述第六预设关系式为：
