本发明公开一种基于任意三面体的雷达-相机系统外部参数标定方法。利用自然环境中的三面体场景，仅需两帧数据即求解系统的外部参数。首先利用三面体规定世界坐标系，在雷达系统中对观察到的三面体进行平面拟合，得到各个平面的参数，并求得世界坐标系和雷达坐标系的转换关系以及雷达坐标系下两帧数据间的相对运动；在相机系统中，利用前后两帧提取的匹配特征点求解本质矩阵，然后分解出在相机坐标系下的相对运动，并利用雷达坐标系下的参数对相机坐标系下的平面参数进行求解，最后求解雷达-相机的外部参数，并利用两个坐标系下对应平面上点的共面性进行最终优化。本发明所要求的场景比较简单，兼具抗干扰性强、实验设备简单、灵活性较强的特点。
1.一种基于任意三面体的雷达-相机系统外部参数标定方法，其特征在于该方法包括以下步骤：(1)利用雷达-相机系统，对存在任意三面体的标定场景采集两帧数据，并根据三面体定义世界坐标系Ow-XwYwZw，采集的两帧数据间存在相对运动，且每帧数据中有对应的雷达采集的三维点云和相机采集的彩色图像；
(2)求解雷达系统参数：在三维点云中标注三面体三个面上的点，表示为：其中下标l代表这些点在雷达坐标系Ol-XlYlZl下，i代表第i帧数据，j代表第j个面，k代表第k个点，每个面上点的个数为 同一平面上的点用参数 来表示：其中 为每个面的法向量， 为到坐标系原点的距离；
然后根据三面体在雷达两帧数据中点的共面性，对两帧共6个面的 进行非线性优化；
通过 得到两帧数据中世界坐标系到雷达坐标系的旋转矩阵和平移向量i＝1,2，并由此得到雷达-相机系统在雷达坐标系下两帧之间的运动参数，即第一帧相对于第二帧的旋转矩阵和平移向量(3)求解相机系统参数：利用SIFT算法在相机采集的前后两帧图像中三面体三个面的对应区域提取前后帧匹配的特征点，表示为：其中下标c代表这些点在相机坐标系Oc-XcYcZc下，i代表第i帧数据，j代表第j个面，k代表第k个特征点，前后两帧对应面的匹配特征点数目相等为根据Pc计算出两帧运动的本质矩阵E，并进一步得到雷达-相机系统在相机坐标系下两帧之间的运动参数 其中 为相机坐标系下第一帧相对于第二帧的旋转矩阵，为平移向量 归一化的结果：利用 初始化相机坐标系下的参数 ，此处
为三面体各个平面在相机坐标系下的参数；根据提取的特征点的对应投影关系，对进行优化；
通过 得到两帧数据中世界坐标系到相机坐标系的转换关系，即旋转矩阵和平移向量 i＝1,2；
(4)求解外部参数：利用Rwc,Twc,Rwl,Twl得到雷达坐标系到相机坐标系的转换关系，即旋转矩阵Rlc和平移向量Tlc；利用雷达坐标系和相机坐标系下三面体上点的共面性，对Rlc,Tlc进行非线性优化，得到优化后的转换关系Rlc,Tlc；
所述的步骤(2)求解雷达系统参数的具体方法为：利用线性最小二乘优化目标函数F1，对第i帧数据第j个面上的点 进行平面拟合，得到参数通过 得到世界坐标系到雷达坐标系的转换关系 i＝1,2：这里rwl1,rwl2,rwl3分别为Rwl的列向量；
通过 i＝1,2得到雷达坐标系下第一帧相对于第二帧的运动参数根据两帧数据中对应面上点的共面性，对所有的 进行非线性优化，目标函数表示为：其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量；利用优化后的对 进行更新；
所述的步骤(3)求解相机系统参数的具体方法为：图像上二维点Pc和相机坐标系下球面点Ps,c的转换关系为：Pc＝G(Ps,c)
此处G(·)为对应的投影函数，由已知的相机类型和内参确定；
根据前后帧对应点的极线几何约束关系，得到：-1 T -1
G (Ps,c1)EG (Ps,c2)＝0这里的E为本质矩阵；利用现有的成熟方法求解本质矩阵，并分解出相机坐标系下两帧之间的运动 其中 为归一化的平移向量：由于雷达-相机系统中雷达和相机的位置距离较近，坐标系间的旋转较小，所以根据雷达坐标系下参数初始估计相机坐标系下待求参数：然后，利用图像上匹配特征点利用 恢复出三维坐标后在另一帧图像上的重投影误差来优化其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量；
通过 得到世界坐标系到相机坐标系的转换关系 i＝1,2：这里rwc1,rwc2,rwc3分别为Rwc的列向量；
所述的步骤(4)求解外部参数的具体方法为：通过 i＝1,2中的任意一组数据，求解雷达和相机坐标系之间的转换关系Rlc,Tlc：
进一步的，利用雷达坐标系和相机坐标系下三面体对应面上点的共面性，根据目标函数F4对Rlc,Tlc进行非线性优化，得到优化后的Rlc,Tlc；
一种基于任意三面体的雷达-相机系统外部参数标定方法
技术领域
[0001] 本发明涉及雷达-相机系统外部参数标定的方法，具体来说是一种基于任意三面体的雷达-相机系统外部参数标定方法。
背景技术
[0002] 对地形环境的实时构建是移动机器人领域的一个基础问题，为了实现这一目的，通常采用双目相机系统或者一个雷达传感器来提供三维信息。但是双目相机系统由于需要进行致密的立体匹配来进行三维重建，耗时较大，容易受外部环境影响，所以无法满足实时性和准确性的要求；雷达传感器虽然可以提供实时三维信息，精度较高，但是缺少环境的颜色信息。因此，越来越多研究者将雷达和单目相机结合在一起构成一个雷达-相机系统来实时的提供具有颜色和三维信息的环境地图。
[0003] 近年来，许多研究者也提供了一些雷达-相机系统外部参数标定的方法。 文 献 1(Zhang,Q.；Pless,R，“Extrinsic calibration of a camera and laser range finder(improve camera calibration)”，IEEE International Conference on Intelligent Robots and Systems，IROS,2004,pp.2301–2306.) 和 文 献2(Unnikrishnan,R.；Hebert,M.，“Fast extrinsic calibration of a laser rangefinder to a camera”，Technical report,Carnegie Mellon University,Rotbotics Institute,2005.)利用棋盘格标定板来计算系统外部参数，棋盘格上的角点要在相机图像上手动提取，而且需要采集多幅图像，该方法很大程度上依赖于角点的提取准确程度，对光照也有一定要求。文献3(Rodriguez F,S.；Fremont,V.；Bonnifait,P，“Extrinsic calibration between a multi-layer lidar and a camera”，IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems,2008,pp.214–219.) 和 文 献 4(Li,G.；Liu,Y.；Dong,L.；Cai,X.；Zhou,D，“An algorithm for extrinsic parameters calibration of a camera and a laser range finder using line features”,IROS,2007,pp.3854–3859.)为了减小角点提取的误差，利用了特殊形状的标定物体，在雷达数据和图像数据中同时提取标定物的对应角点位置，但雷达数据本身的非致密性决定了该方法的准确性不会太高。近来，文献5(Pandey,G.；
McBride,J.R.；Savarese,S.；Eustice,R.M.，“Automatic targetless extrinsic calibration of a 3d lidar and camera by maximizing mutual information”，Proceedings of the AAAI National Conference on Artificial Intelligence,2012.)不依赖于任何的场景，仅利用雷达提供的三维点反射强度和相机图像的互信息来优化雷达-相机系统的外部参数，但是并不是所有的雷达都可以提供物体的反射强度，而且不同材质物体的反射强度并不相同。
发明内容
[0004] 为了将雷达-相机数据有效的结合在一起，本发明的目的在于提供一种基于任意三面体的雷达-相机系统外部参数标定方法，即求解雷达系统和相机系统的位置转换关系；利用自然场景中常见的任意三面体，不依赖手工输入信息的准确性，而且仅需要两帧数据即可快速的进行外部参数求解。
[0005] 本发明采用的技术方案的步骤如下：
[0006] (1)利用雷达-相机系统，对存在任意三面体的标定场景采集两帧数据，并根据三面体定义世界坐标系Ow-XwYwZw，采集的两帧数据间存在相对运动，且每帧数据中有对应的雷达采集的三维点云和相机采集的彩色图像；
[0007] (2)求解雷达系统参数：在三维点云中标注三面体三个面上的点，表示为：
[0008]
[0009] 其中下标l代表这些点在雷达坐标系Ol-XlYlZl下，i代表第i帧数据，j代表第j个面，k代表第k个点，每个面上点的个数为 同一平面上的点用参数 来表示：
[0010]
[0011] 其中 为每个面的法向量， 为到坐标系原点的距离；
[0012] 然后根据三面体在雷达两帧数据中点的共面性，对两帧共6个面的 进行非线性优化；
[0013] 通过 得到两帧数据中世界坐标系到雷达坐标系的旋转矩阵和平移向量并由此得到雷达-相机系统在雷达坐标系下两帧之间的运动参数，即第一帧相对于第二帧的旋转矩阵和平移向量
[0014] (3)求解相机系统参数：利用SIFT算法在相机采集的前后两帧图像中三面体三个面的对应区域提取前后帧匹配的特征点，表示为：
[0015]
[0016] 其中下标c代表这些点在相机坐标系Oc-XcYcZc下，i代表第i帧数据，j代表第j个面，k代表第k个特征点，前后两帧对应面的匹配特征点数目相等为
[0017] 根据Pc计算出两帧运动的本质矩阵E，并进一步得到雷达-相机系统在相机坐标系下两帧之间的运动参数 其中 为相机坐标系下第一帧相对于第二帧的旋转矩阵， 为平移向量 归一化的结果：
[0018]
[0019] 利用 初始化相机坐标系下的参数 ，此处为三面体各个平面在相机坐标系下的参数；根据提取的特征点的对应投影关系，对 进行优化；
[0020] 通过 得到两帧数据中世界坐标系到相机坐标系的转换关系，即旋转矩阵和平移向量
[0021] (4)求解外部参数：利用Rwc,Twc,Rwl,Twl得到雷达坐标系到相机坐标系的转换关系，即旋转矩阵Rlc和平移向量Tlc；利用雷达坐标系和相机坐标系下三面体上点的共面性，对Rlc,Tlc进行非线性优化，得到优化后的转换关系Rlc,Tlc。
[0022] 所述的步骤(2)求解雷达系统参数的具体方法为：
[0023] 利用线性最小二乘优化目标函数F1，对第i帧数据第j个面上的点 进行平面，得到拟合参数
[0024]
[0025] 通过 得到世界坐标系到雷达坐标系的转换关系
[0026]
[0027] 这里rwl1,rwl2,rwl3分别为Rwl的列向量；
[0028] 通过 得到雷达坐标系下第一帧相对于第二帧的运动参数
[0029]
[0030] 根据两帧数据中对应面上点的共面性，对所有的 进行非线性优化，目标函数表示为：
[0031]
[0032] 其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量；利用优化后的 对 进行更新。
[0033] 所述的步骤(3)求解相机系统参数的具体方法为：
[0034] 图像上二维点Pc和相机坐标系下球面点Ps,c的转换关系为：
[0035] Pc＝G(Ps,c)
[0036] 此处G(·)为对应的投影函数，由已知的相机类型和内参确定；
[0037] 根据前后帧对应点的极线几何约束关系，得到：
[0038] G-1(Ps,c1)TEG-1(Ps,c2)＝0
[0039] 这里的E为本质矩阵；利用现有的成熟方法求解本质矩阵，并分解出相机坐标系下两帧之间的运动 其中 为归一化的平移向量：
[0040]
[0041] 由于雷达-相机系统中雷达和相机的位置距离较近，坐标系间的旋转较小，所以根据雷达坐标系下参数初始估计相机坐标系下待求参数：
[0042]
[0043] 然后，利用图像上匹配特征点利用 恢复出三维坐标后在另一帧图像上的重投影误差来优化
[0044]
[0045] 其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量；
[0046] 通过 得到世界坐标系到相机坐标系的转换关系
[0047]
[0048] 这里rwc1,rwc2,rwc3分别为Rwc的列向量。
[0049] 所述的步骤(4)求解外部参数的具体方法为：
[0050] 通过 中的任意一组数据，求解雷达和相机坐标系之间的转换关系Rlc,Tlc：
[0051]
[0052] 进一步的，利用雷达坐标系和相机坐标系下三面体对应面上点的共面性，根据目标函数F4对Rlc,Tlc进行非线性优化，得到优化后的Rlc,Tlc。
[0053]
[0054] 本发明具有的有益效果是：
[0055] 本发明兼顾了抗噪性能、操作复杂度要求低，不需要对环境做特殊的设置，适合于任意的光照、天气等外部条件；本发明所要求的场景比较简单、常见，兼具抗干扰性强、实验设备简单、灵活性较强的特点。
附图说明
[0056] 图1是本发明的总体流程图。
[0057] 图2是雷达-相机系统标定过程中三个坐标系定义及关系示意图。
具体实施方式
[0058] 下面结合附图和具体实施方式对本发明做进一步的描述。
[0059] 图1给出了雷达-相机系统外部参数标定方法的技术流程。
[0060] 雷达-相机系统外部参数标定包括以下四个部分：①采集两帧雷达-相机系统数据；②求解雷达系统参数：利用雷达系统下三面体面上点的坐标，求解两帧雷达数据中各个平面的参数、相对运动参数以及与世界坐标系的转换关系；③求解相机系统参数：依据已知的相机参数，利用雷达系统下参数，求解两帧相机数据中各个平面的参数、相对运动参数以及与世界坐标系的转换关系；④求解外部参数：根据雷达和相机坐标系和世界坐标系的相对关系求解雷达和相机的转换关系，即雷达-相机系统的外部参数，并利用同一平面上点在不同传感器系统下的共面性进行最终优化。
[0061] 1、采集两帧雷达-相机系统数据：
[0062] 如图2所示，根据场景中的三面体定义世界坐标系Ow-XwYwZw：
[0063] 以三面体三个面的交点为原点Ow，平面1与平面3的交线为Xw轴，平面3的法向量方向为Zw轴，然后根据右手系法则可以确定Yw的方向。
[0064] 利用雷达-相机系统，对标定场景采集两帧数据，且采集两帧数据间有相对运动，要求两个传感器都可以同时观察到三面体的各个面。
[0065] 2、求解雷达系统参数：
[0066] 在雷达数据中标注出三面体三个面上的点，表示为：
[0067]
[0068] 其中下标l代表这些点在雷达坐标系Ol-XlYlZl下，i代表第i帧数据，j代表第j个面，k代表第k个点，每个面上点的个数为 同一平面上的点可以用参数 来表示：
[0069]
[0070] 其中 为每个面的法向量， 为到坐标系原点的距离。
[0071] 利用线性最小二乘优化目标函数F1，对第i帧数据第j个面上的点 进行平面拟合，得到参数
[0072]
[0073] 通过 可以得到世界坐标系到雷达坐标系的转换关系
[0074]
[0075] 这里rwl1,rwl2,rwl3分别为Rwl的列向量。通过 可以得到雷达坐标系下第一帧相对于第二帧的运动参数
[0076]
[0077] 根据两帧数据中对应面上点的共面性，对所有的 进行非线性优化，然后利用优化后的 对 进行更新。
[0078] 优化的目标函数为：
[0079]
[0080] 其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量，与 存在关系：
[0081]
[0082] 3、求解相机坐标系下参数：
[0083] 根 据SIFT算 法 (具 体 实 现 参 见 文 献 6(Lowe,David G.(1999)."Object recognition from local scale-invariant features".Proceedings of the International Conference on Computer Vision.2.pp.1150–1157.))在前后两帧图像对应面的进行特征点提取、匹配，表示为：
[0084]
[0085] 其中下标c代表这些点在相机坐标系Oc-XcYcZc下，i代表第i帧数据，j代表第j个面，k代表第k个特征点，前后两帧对应面的匹配特征点数目相等为
[0086] 图像上二维点Pc和相机坐标系下球面点Ps,c的转换关系为：
[0087] Pc＝G(Ps,c) (9)
[0088] 此处G(·)为对应的投影函数，由已知的相机类型和内参确定。
[0089] 根据前后帧对应点的极线几何约束关系，可以得到：-1 T -1
[0090] G (Ps,c1)EG (Ps,c2)＝0 (10)
[0091] 这 里 的 E 为 本 质 矩 阵。 可 以 利 用 文 献 7(R.I.Hartley and A.Zisserman,“Multiple View Geometry in Computer Vision”，Cambridge University Press,ISBN:0521540518,2004,2nd ed.)中成熟的8点法求解本质矩阵，并分解出相机坐标系下两帧之间的运动 其中 为归一化的平移向量：
[0092]
[0093] 由于雷达-相机系统中雷达和相机的位置距离较近，坐标系间的旋转较小，所以根据雷达坐标系下参数初始估计相机坐标系下待求参数：
[0094]
[0095] 然后，利用图像上匹配特征点利用 恢复出三维坐标后在另一帧图像上的重投影误差来优化
[0096]
[0097] 其中，式中的 为第二帧相对于第一帧的旋转矩阵和平移向量，与存在关系：
[0098]
[0099] 通过 可以得到世界坐标系到相机坐标系的转换关系
[0100]
[0101] 这里rwc1,rwc2,rwc3分别为Rwc的列向量。
[0102] 4、求解外部参数：
[0103] 通过 中的任意一组数据，可以求解雷达和相机坐标系之间的转换关系Rlc,Tlc：
[0104]
[0105] 进一步的，利用雷达坐标系和相机坐标系下三面体对应面上点的共面性，根据目标函数F4对Rlc,Tlc进行非线性优化，得到优化后的Rlc,Tlc。
[0106]
[0107] 至此为止，雷达-相机系统的外部参数标定完毕。
