线扫激光点云拼接——运动机构法

假设线扫激光已经可以得到单帧点云。

滑台

运动方向标定：

其中(xstart,ystart,zstart)是标定板初始位姿在相机坐标系下的坐标，(xend,ystart,zstart)是标定板最终位姿在相机坐标系下的坐标。再将（△x，△y，△z）单位化即可得到滑台的运动方向。
在相机坐标系下拼接的坐标变换矩阵为

其中l为滑台运动的位移。

转台

旋转轴线标定：方法有多种，这里给出其中一种较简单的方法。
通过共面参照物标定的方法测出一个平面上的特征点在旋转过程中几个位置处的空间坐标,然后对分别对每个点做圆拟合,得到它对应的旋转中心,再对这些旋转中心做空间直线拟合,得到最后的旋转轴的方程。
上述的共面参照物在标定中可以选为标定板，特征点可以选为标定板的原点。
matlab三维空间点拟合圆的代码：

M=[-161.564 -62.7806 589.371 
-161.595 -67.8667 588.45
-161.639 -61.639 586.938
-161.684 -79.1419 584.965
-161.74 -84.5655 582.518
-161.818 -90.1652 579.386
-161.91 -96.0041 575.329
-162.005 -101.07 571.017
-162.107 -106.267 565.571
-162.232 -111.449 558.678
-162.363 -115.406 551.888
-162.494 -118.629 544.469
-162.611 -120.772 537.451
-162.765 -122.386 528.428
-162.886 -122.794 521.408
-162.973 -122.584 515.629
-163.104 -121.6 508.223
-163.238 -119.527 500.014
-163.559 -112.202 484.23
-163.634 -108.883 479.366
-163.714 -104.854 474.469
-163.759 -100.909 470.41
-163.828 -96.7168 466.732
-163.872 -92.4086 463.542
-163.884 -87.2752 460.273
    ];
 
[num dim]=size(M);
 
L1=ones(num,1);
A=inv(M'*M)*M'*L1;       % 求解平面法向量
 
B=zeros((num-1)*num/2,3);
 
count=0;
for i=1:num-1
    for j=i+1:num   
        count=count+1;
        B(count,:)=M(i+1,:)-M(i,:);
    end    
end
 
L2=zeros((num-1)*num/2,1);
count=0;
for i=1:num-1
    for j=i+1:num
        count=count+1;
        L2(count)=(M(i+1,1)^2+M(i+1,2)^2+M(i+1,3)^2-M(i,1)^2-M(i,2)^2-M(i,3)^2)/2;
    end
end
 
D=zeros(4,4);
D(1:3,1:3)=(B'*B);
D(4,1:3)=A';
D(1:3,4)=A;
 
L3=[B'*L2;1]
 
C=inv(D')*(L3)   % 求解空间圆圆心坐标
 
C=C(1:3);
 
radius=0;
for i=1:num
    tmp=M(i,:)-C';
    radius=radius+sqrt(tmp(1)^2+tmp(2)^2+tmp(3)^2);
end
r=radius/num            %  空间圆拟合半径
 
figure
h1=plot3(M(:,1),M(:,2),M(:,3),'*');
%set(gca,'xlim',[11.4 11.7]);
 
 
%%%%   绘制空间圆  %%%%
n=A;
c=C;
 
theta=(0:2*pi/100:2*pi)';    %  theta角从0到2*pi
a=cross(n,[1 0 0]);          %  n与i叉乘，求取a向量
if ~any(a)                   %  如果a为零向量，将n与j叉乘
    a=cross(n,[0 1 0]);
end
b=cross(n,a);      % 求取b向量
a=a/norm(a);       % 单位化a向量
b=b/norm(b);       % 单位化b向量
 
c1=c(1)*ones(size(theta,1),1);
c2=c(2)*ones(size(theta,1),1);
c3=c(3)*ones(size(theta,1),1);
 
x=c1+r*a(1)*cos(theta)+r*b(1)*sin(theta);  % 圆上各点的x坐标
y=c2+r*a(2)*cos(theta)+r*b(2)*sin(theta);  % 圆上各点的y坐标
z=c3+r*a(3)*cos(theta)+r*b(3)*sin(theta);  % 圆上各点的z坐标
 
hold on;
h2=plot3(x,y,z,'-r');
xlabel('x轴')
ylabel('y轴')
zlabel('z轴')
axis([-300,0,-150,150,350,650])
legend([h1 h2],'控制点','拟合圆');
grid on

在相机坐标系下拼接的坐标变换矩阵为

其中(u,v,w)为转轴的方向向量，（a,b,c）为转轴上一点，θ为旋转的角度。

机器人

机器人手眼标定，不做详细介绍。
坐标变换关系为
Base = ToolInBase * CamInToolPose * Camera
Camera为相机坐标系下的坐标，CamInToolPose由手眼标定得到，ToolInBase 由机器人运动学求解，Base为机器人基坐标系下的坐标。

运动机构标定C++工程下载地址：运动机构法标定.rar