本发明涉及距离传感器的位置标定技术,更具体地,涉及距离传感器外参数的标定方法。
背景技术:
在环境感知测量系统中,为了保证感知系统的鲁棒性,采用多个传感器获取环境信息是未来发展趋势。例如道路三维测量系统,自动驾驶系统,机器人环境感知系统中等,通常会采用多个距离传感器如激光扫描传感器来获取环境信息。
多传感器位姿标定是进行多传感器信息融合的必要处理步骤。例如,在使用来自多个激光扫描传感器的数据之前,需要对各个传感器的相对位置和姿态进行标定,以便将各激光传感器的数据统一到同一个坐标系下进行计算。
由于激光雷达获取的是距离数据,不包含纹理特征,且激光雷达扫描时发出的激光为不可见光,因此很难获取同名特征点以用于标定计算,使得多激光传感器6dof(六自由度)外参数标定难以实现。
目前通常采用圆柱长杆和棋盘格标定板来实现多激光传感器外参数标定。采用圆柱长杆标定只适用于同一平面内的多激光传感器的标定,即只能实现3dof(三自由度)外参数标定。采用棋盘格标定方法可以实现6dof参数标定,但是,一方面需要借助相机来辅助,同时也使得标定精度会受到相机自身标定精度的影响。另一方面,需要在不同位置摆放标定板并多次采集数据,使得标定过程费时费力。
技术实现要素:
本发明的目的是解决多个距离传感器之间的外参数自动校准问题。传感器的外参数包括位置和姿态(x,y,z,yaw,pitch,roll)等6个变量。为此,本发明中提出了一种新型的多个距离传感器6dof外参数标定装置和标定方法。
本发明的距离传感器外参数标定方法包括:
s1,布置一个三脚架,三脚架位于待标定的距离传感器的扫描范围内,三脚架包括s、a、b和c四个顶点,其中点a、b、c位于底面上;
s2,测量三脚架的三个斜边sa、sb、sc长度及三个底边ab、bc、ac的长度,建立三脚架坐标系,求解所述三脚架坐标系的各顶点坐标;
s3,根据各个距离传感器的扫描数据,计算各个距离传感器在所述三脚架坐标系下的位置参数;以及
s4,根据各个距离传感器相对于三脚架坐标系下的旋转矩阵和平移矩阵,选择其中一个距离传感器为参考坐标系,计算其他距离传感器相对与该距离传感器参考坐标系下的旋转矩阵和平移矩阵。
本发明还提出一种距离传感器外参数标定系统,包括:处理器,所述处理器被配置为执行如下步骤:
s1,布置一个三脚架,并使三脚架位于待标定的距离传感器的扫描范围内,三脚架包括s、a、b和c四个顶点,其中点a、b、c位于底面上;
s2,测量三脚架的三个斜边sa、sb、sc长度及三个底边ab、bc、ac的长度,建立三脚架坐标系,求解所述三脚架坐标系的各顶点坐标;
s3,根据各个距离传感器的扫描数据,计算各个距离传感器在所述三脚架坐标系下的位置参数;以及
s4,根据各个距离传感器相对于三脚架坐标系下的旋转矩阵和平移矩阵,选择其中一个距离传感器为参考坐标系,计算其他距离传感器相对与该距离传感器参考坐标系下的旋转矩阵和平移矩阵。
本发明的技术效果包括:
1、本发明中提出了一种新型的标定装置和标定算法,无需借助相机即可实现多激光传感器的6dof位置参数配准。
2、由于标定中不需要使用相机,使得标定结果不受相机本身的标定精度影响,避免了标定过程中误差的累积,标定结果精度高。
3、本发明无需移动或者旋转待标定激光雷达,并且可实现单次摆放标定装置即可完成两个或者两个以上激光雷达之间的标定,提高标定效率。
4、使用单次测量数据即可完成标定工作,可大幅提高标定效率。
5、本发明中采用三脚架作为标定装置,制作简单,实用性好,易于应用推广。
附图说明
图1为本发明的方法的流程图;
图2为本发明的方法的原理图;
图3为本发明的方法的实施示意图;
图4为本发明的三脚架坐标系的示意图;
图5为本发明的实验结果的示意图。
具体实施方式
下面参照附图描述本发明的实施方式,其中相同的部件用相同的附图标记表示。
本发明以激光扫描传感器为例来进行描述,但是本发明不仅仅适用于激光扫描传感器,其他距离传感器也适用,比如kinect和rangesensor。
本发明的主要原理是,利用三脚架作为各个激光雷达的共同参照坐标系,标定时先计算各个激光雷达在该参照坐标系下的位置参数(6dof),然后再计算各个激光雷达之间的相对位置参数,得到标定结果。无需像传统方法一样通过找到同名点的方式,实现传感器之间的位姿标定。
图1-2显示了本发明的方法的流程图。
在s1中,布置一个三脚架,各个激光雷达围绕该三脚架放置,其中三脚架包括四个顶点为s、a、b和c,其中点a、b、c位于底面上,如图3所示。三脚架放置在各个激光雷达的检测重合范围内。
对于两个激光雷达之间的标定,需要将三角架放在两个激光雷达的检测范围之内。
对于三个以上的激光雷达之间的标定,至少需要将三脚架放在两个及以上的激光雷达的检测重合范围之内。例如:如需标定三个激光雷达a、b、c之间的位置关系,第一次先将三脚架放置于激光雷达a,b的扫描范围内,标定a、b之间的位置关系;第二次将三角架放置于激光雷达b、c的扫描范围内,标定激光雷达b、c的位置关系;然后通过坐标转换则可实现激光雷达a、c之间的标定。
若三脚架同时位于a、b、c三个激光雷达的扫描范围内,则放置一次三角架即可实现三个激光雷达之间的标定。
再次参考图1,在s2中,测量三脚架的三个斜边sa、sb、sc长度及三个底边ab、bc、ac的长度,建立三脚架坐标系,求解所述三脚架坐标系的各顶点坐标。
具体为,以三脚架底面作为三脚架坐标系xy平面,以ac所在边为x轴,以过b点且与ac边垂直的直线为y轴,以与xy轴垂直且过xy轴交点的直线为z轴,建立以三脚架为参照系的直角坐标系(oxyz)。
如图4所示,根据三脚架各斜边长度,利用几何关系建立方程,求解三脚架顶点s、a、b、c在所述三脚架参考坐标系下的坐标值(oa,ob,oc,os):a点坐标为(-m,0,0),b点坐标为(0,l,0),c点坐标为(n,0,0),s点坐标为(0,f,h)。
已知三斜边边长|ab|,|bc|,|bc|,求解l,m,n的值。根据几何关系,可得如下方程:
令
m2+l2=|ab|2
n2+l2=|bc|2
已知四面体六边边长,根据海伦公式可求解h的值,已知h,|ob|,|sb|长,根据三角关系可求解f的值。
再次参考图1,在s3中,根据各个激光雷达的扫描数据,计算各个激光雷达在所述三脚架坐标系下的位置参数,即标定各个激光雷达与所述三脚架的相对位置关系。步骤s3具体包括:
s3-1,对激光雷达获取的三脚架的扫描点进行聚类运算,将激光扫描点分成三类,并计算各类的中心点a,b,c的坐标(pa,pb,pc);
由于三脚架有三个斜边,激光雷达扫描时候会分别扫描到三脚架的三边。聚类运算就是提取激光扫描数据中对应于这三条斜边的激光扫描点。各类中心点代表激光扫描平面与三角架的三斜边的理论交点。
当激光雷达扫描到其中的某一条斜边时候,可能得到多个激光雷达扫描点,对激光点进行聚类并计算该类的中心,目的是为了更准确地计算该边与激光扫描平面的交点。其原理是把多个扫描点的平均值(即中心点)当做交点,减少单个激光扫描点的测量误差带来的影响。
s3-2,依据所述中心点a,b,c的坐标,计算三个中心点a,b,c之间的距离(lab,lbc,lac)。
s3-3,依据几何关系,求解三个中心点a,b,c在所述三脚架坐标系下的坐标值(oα,ob,oc)。计算方法如下:
假设激光雷达l扫描平面与三脚架的交点为a,b,c。其在激光雷达坐标系下的坐标(x,y)可记为pa,pb,pc。点a,b,c在三脚架坐标系下的三维坐标(x,y,z)可记作oa,ob,oc,该坐标值为未知量。根据几何关系,可以得到如下方程。
lsa=||os-oa||(1)
laa=||oa-oa||(2)
lsa=||os-oa||(3)
lsb=||os-ob||(4)
lbb=||ob-ob||(5)
lsb=||os-ob||(6)
lsc=||os-oc||(7)
lcc=||oc-oc||(8)
lsc=||os-oc||(9)
lsa=lsa+laa(10)
lsb=lsb+lbb(11)
lsc=lsc+lcc(12)
lab=||oa-ob||=||pa-pb||(13)
lac=||oa-oc||=||pa-pc||(14)
lbc=||ob-oc||=||pb-pc||(15)
记三脚架斜边sa与sb的夹角为α,三脚架斜边sa与sc的夹角为β,三脚架斜边sb与sc的夹角为γ,根据三角关系,可得如下方程:
lsa2+lsb2-2lsalsbcosα=lab2(16)
lsa2+lsc2-2lsalsccosβ=lac2(17)
lsb2+lsc2-2lsb-lsccosγ=lbc2(18)
联立方程(1)~(18),可计算出a,b,c三点在三脚架坐标系下的坐标oa,ob,oc。
s3-4,根据三个中心点a,b,c在三脚架坐标系下的坐标和在激光雷达坐标系下的坐标,计算旋转矩阵r和平移矩阵t。其计算方法如下:
从该激光雷达传感器坐标系到三脚架坐标系的坐标变换中,记旋转矩阵为r,平移矩阵为t。则有:
oa=rpa+t(19)
ob=rpb+t(20)
oc=rpc+t(21)
联立方程(19)~(21),可计算出旋转矩阵r和平移矩阵t。
s3-5,按步骤s3-3和s3-4依次计算各激光雷达相对于三脚架坐标系下的旋转矩阵ri和平移矩阵ti。
再次参考图1,在s4中,根据各个激光雷达相对于三脚架坐标系下的旋转矩阵和平移矩阵,选择其中一个激光雷达为参考坐标系,计算其他激光雷达相对与该激光雷达参考坐标系下的旋转矩阵
具体地,对三脚架坐标系下任意一点(x,y,z),根据上述结果可得如下方程:
联立方程(22)~(23),可得到两激光雷达之间的相对位置关系,完成校准。
如图5所示,本发明已经通过实验和模拟实验两种方法进行了实验验证,证明了本发明的方法可行,可以实现多个距离传感器的6dof外参数标定。在上述实验中,通过本发明所提出的标定方法,计算出了三个激光雷达的相互位置关系,并在2维和3维空间中将标定后的激光雷达的相互位置关系可视化。通过可视化,使得不可见的激光扫描平面及激光与三脚架的交点等变为可见,可用于辅助确认各传感器位置和姿态标定的结果。
本发明还对应提出一种距离传感器外参数标定系统,其包括处理器,所述处理器装载有程序,所述程序运行时,实现如上所述的各方法步骤。
以上所述的实施例,只是本发明较优选的具体实施方式,本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。