激光SLAM 二维坐标系的旋转和变换 四参数法

本文章用于平面坐标系的二维旋转，具体问题是：笔者在室内定位过程中，使用 LEGO-LOAM算法得到小车的平面位置后，需要将小车的位置转换到真值坐标系下，进而遇到了二维坐标系轨迹对齐问题， 真值坐标值是由TS60全站仪测得的，精度达到毫米级，解决后特此记录如下。

下图是笔者在图书馆地下车库进行测试的场景，小车上面放置了棱镜，小车在移动时，全站仪可以自动跟踪小车的棱镜。小车的运动中心为最上面的16线雷达，全站仪观测的是小车上的棱镜，去除杆臂误差，即可对二者的轨迹进行匹配。

最小二乘原理

采用间接平差的方法对二维坐标系进行转换，需要构造条件方程，下式中m为1，因此未知数共有三个：$\Delta$x，$\Delta$y，$\alpha$。

笔者参考了链接: link，上述链接中m不为1，需要求出缩放因子m。笔者在推导过程中，对上述链接中的最小二乘的过程做了简单的改变，将m直接置为1，构造条件方程，具体如下：

构造完成之后，求得矩阵B、L、P，其中P阵为单位阵，可以不需要加入求解。
最后采用最小二乘的公式求解：

匹配效果

匹配前的效果：

匹配后的效果：”

Matlab代码

%% 最小二乘
% 根据对应的同名点批量求L阵和B阵
% 生成L阵：减完之后转置，转置之后再全部整成一列，目的是使得L矩阵排成
% X1-x1;Y1-y1;X2-x1;Y2-y1 其中大写的X Y为目标坐标系，小写的x和y为待转换的坐标系
matrixL = (stationPoints - vehiclePoints)';
matrixL = matrixL(:);
%生成B阵
matrixB = zeros(pointNums * 2, 4);
for num = 1:1:pointNums
    matrixB((num*2-1):(num*2),:) = [1,0,vehiclePoints(num,1),-vehiclePoints(num,2);...
           0,1,vehiclePoints(num,2),vehiclePoints(num,1);];
end
matrixX = (matrixB'*matrixB) \ (matrixB' * matrixL);
% 求出对应的四参数
deltX = matrixX(1,1); deltY = matrixX(2,1); 
a = matrixX(3,1); b = matrixX(4,1);
sinAef = b ;
cosAef = (a + 1) ;
rotation = [cosAef, -sinAef; sinAef, cosAef;];% 旋转矩阵
% 将小车的位置转换成新的坐标
tempVehicleTrj = vehicleTrj';
vehicleNums = size(tempVehicleTrj,2);
transformVehicleTrj = zeros(2,vehicleNums);
for n = 1:1:vehicleNums
    transformVehicleTrj(:,n) = [deltX; deltY;] +  rotation * tempVehicleTrj(:,n);
end
transformVehicleTrj = transformVehicleTrj';
pos2plotAccurary(stationCurTrj,transformVehicleTrj); axis equal;
legend('轨迹起始点','轨迹终止点','全站仪','SLAM');

详细代码见附件链接: link，也可以在评论区问笔者要。

链接: link 1
链接: link 2