申请/专利权人：清华大学

申请日：2022-06-17

公开（公告）日：2024-04-09

公开（公告）号：CN115166701B

主分类号：G01S7/497

分类号：G01S7/497;G06T7/80

优先权：

专利状态码：有效-授权

法律状态：2024.04.09#授权;2022.10.28#实质审查的生效;2022.10.11#公开

摘要：本发明提出一种RGB‑D相机和激光雷达的系统标定方法及装置，属于移动机器人环境感知领域。其中，所述方法包括：获取标定物上各特征点分别在激光雷达坐标系下和在RGB‑D相机坐标系下坐标；利用激光雷达各向异性误差矩阵计算特征点经过激光雷达各项异性矫正后的坐标；利用RGB‑D相机各向异性误差矩阵计算特征点经过RGB‑D相机各项异性矫正后的坐标；根据特征点经过激光雷达各项异性矫正后的坐标和经过RGB‑D相机各项异性矫正后的坐标，计算激光雷达和RGB‑D相机之间的旋转矩阵和位移矩阵，完成标定。本发明通过使用各向异性加权矩阵对数据点进行校正，提升标定外参的精度，可提高RGB‑D相机与激光雷达的标定精度。

主权项：1.一种RGB-D相机和激光雷达的系统标定方法，其特征在于，包括：获取标定物上预设的特征点分别在激光雷达坐标系下和在RGB-D相机坐标系下的坐标；根据所述特征点在激光雷达坐标系下的坐标，利用激光雷达各向异性误差矩阵计算所述特征点经过激光雷达各项异性矫正后的坐标；根据所述特征点在RGB-D相机坐标系下的坐标，利用RGB-D相机各向异性误差矩阵计算所述特征点经过RGB-D相机各项异性矫正后的坐标；根据所述特征点经过激光雷达各项异性矫正后的坐标和经过RGB-D相机各项异性矫正后的坐标，计算所述激光雷达和所述RGB-D相机之间的旋转矩阵和位移矩阵，完成标定；其中，所述RGB-D相机各向异性误差矩阵构建方法如下：分别获取RGB-D相机的两块感光组件CCD1和CCD2的误差分别记作σx1,σy1,σz1和σx2,σy2,σz2，其中σx1,σy1,σz1为感光组件CCD1在x,y,z三个方向上的误差，σx2,σy2,σz2为感光组件CCD2在x,y,z三个方向上的误差；则RGB-D相机误差为： 其中，σx,σy,σz分别为RGB-D相机在x,y,z三个方向上的误差；得到RGB-D相机各向异性误差矩阵表达式如下： 所述激光雷达各向异性误差矩阵构建方法如下：1令激光雷达测量任一点P，得到该点在直角坐标系下的坐标x,y,z及该点在球标系下的坐标R,α,ω： 其中，R为激光飞行时间算法测得的激光雷达的激光模块与P点间的距离，α与ω分别为激光雷达的水平夹角与垂直夹角；2获取激光雷达的距离误差σR、水平夹角误差σα和垂直夹角误差σω；3构建激光雷达在球坐标系下的各向异性加权矩阵WLidar′： 4构建直角坐标系和球坐标系的关系转换矩阵如下： 5构建直角坐标系下激光雷达各向异性矩阵WLidar为：WLidar＝J·WLidar′·JT；所述特征点经过激光雷达各项异性矫正后的坐标计算表达式如下：PLidarWx,y,z＝WLidar·PLidarx,y,z其中，PLidarx,y,z表示任一特征点在激光雷达坐标系下的坐标，PLidarWx,y,z表示该特征点经过激光雷达各项异性矫正后的坐标；所述特征点经过RGB-D相机各项异性矫正后的坐标计算表达式如下：PCameraWx,y,z＝WCamera·PCamerax,y,z其中，PCamerax,y,z表示任一特征点在RGB-D相机坐标系下的坐标，PCameraWx,y,z表示该特征点经过RGB-D相机各项异性矫正后的坐标；所述根据所述特征点经过激光雷达各项异性矫正后的坐标和经过RGB-D相机各项异性矫正后的坐标，计算所述激光雷达和所述RGB-D相机之间的旋转矩阵和位移矩阵，包括：1任意选取一个特征点作为基准特征点，将所述基准特征点经过激光雷达各项异性矫正后的坐标记为PLidarW0，将所述基准特征点经过RGB-D相机各项异性矫正后的坐标记为PCameraW0；2利用奇异值分解法对PLidarW0和PCameraW0进行粗配准，获得初始旋转矩阵R0与初始位移矩阵T0，表达式如下：PLidarW0＝R0·PCameraW0+T03利用其余特征点分别经过激光雷达各项异性矫正后的坐标和经过RGB-D相机各项异性矫正后的坐标，通过对初始旋转矩阵和初始位移矩阵进行迭代求解，得到最终的旋转矩阵和位移矩阵；所述对初始旋转矩阵和初始位移矩阵进行迭代求解，包括：3-1将R0转换为I+ΔΘ0的形式，其中I是单位矩阵，ΔΘ0是R0与单位矩阵的差值矩阵，将ΔΘ0作为初始差值矩阵；令初始误差error0＝1；3-2在第n步迭代时，进行如下计算：3-2-1使用最小二乘法得到最小化的[I+ΔΘn,Tn]，其中最小二乘法表达式为： 其中，ΔΘn和Tn分别是第n次迭代时满足最小二乘法得到的差值矩阵和位移矩阵，是在n-1次迭代后得到的第i个特征点经过激光雷达各项异性矫正后的坐标，其迭代初始值为该特征点经过激光雷达各项异性矫正后的坐标；PCameraWi表示第i个特征点经过RGB-D相机各项异性矫正后的坐标；i＝1,2,3…M，不包含基准特征点；M为除基准特征点以外的特征点总数；3-2-2更新进行如下计算：UΛVT＝1+ΔΘn-1Rn＝UVT 其中，ΔΘn-1和Tn-1分别为第n-1次迭代时满足最小二乘法得到的差值矩阵和位移矩阵；U,Λ,VT是对1+ΔΘn-1做奇异值分解后得到的矩阵，Rn是第n次迭代时得到的旋转矩阵，是在第n次迭代时第i个特征点经过激光雷达各项异性矫正后的坐标；3-2-3计算第n次迭代的误差： 其中是第n-1次迭代后各特征点经过激光雷达各项异性矫正后的坐标的均值，errorn是第n次迭代后得到的误差值；3-4判定：如果errorn大于设定的误差阈值，则令n＝n+1，然后重新返回步骤3-2-1；否则，将Rn,Tn作为最终得到的RGB-D相机和激光雷达的系统外参矩阵，标定完成。

全文数据：

权利要求：

百度查询： 清华大学 一种RGB-D相机和激光雷达的系统标定方法及装置

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD