已知四顶点坐标求四面体外接球球心坐标

最近在做一个关于机器人标定的问题，发现求空间一点相对六轴机器人的坐标问题实际是一个求已知四顶点坐标求四面体外接球球心坐标的问题。首先控制机器人（携带工具）以四个不同姿态接触被测点，要注意四个姿态要保证机器人末端的四个点尽可能不在同一面，读取这四个点机器人末端的坐标，则这四个姿态下机器人末端到所求点的距离是一定的，所以所求点是这四个末端点所构成四面体的外接圆球心。
接下来就是设计程序计算球心坐标了。具体思路是，四点中先取三点（如取a、b、c）构成外接球的一个切平面abc，则该切平面与外接球相交的轨迹正是三角形abc的外接圆，过该外接圆的圆心做切平面的垂线，则外接球的球心落在这条垂线上。再取三点，则可以得到另一条垂线，两垂线的交点即为所求。
为了精确求解，下面的算法通过一些代入化解避开了解方程。matlab代码如下：

%输入四个顶点坐标abcd，求出圆心坐标result
function result = BaseMark(a,b,c,d)
[p1, v1] = ThreePointsLine(a,b,c);
[p2, v2] = ThreePointsLine(b,c,d);
result = Lines2point(p1,v1,p2,v2);
plot3([a(1),b(1),c(1),d(1)],[a(2),b(2),c(2),d(2)],[a(3),b(3),c(3),d(3)],'b*');
hold on;
plot3(result(1),result(2),result(3),'r*');
plot3([a(1),result(1)],[a(2),result(2)],[a(3),result(3)],'b');
plot3([b(1),result(1)],[b(2),result(2)],[b(3),result(3)],'b');
plot3([c(1),result(1)],[c(2),result(2)],[c(3),result(3)],'b');
plot3([d(1),result(1)],[d(2),result(2)],[d(3),result(3)],'b');
axis equal;
end

%求在平面abc的法向量和三角形abc的外心
function [p,v] = ThreePointsLine(a,b,c)
cen_ab = (a + b)/2;
cen_bc = (b + c)/2;
v = cross((a - b), (b - c));%平面abc的法向量
normal_ab = cross((a - b), v); %直线ab在平面abc上的垂直向量
normal_bc = cross((b - c), v); %直线bc在平面abc上的垂直向量
p = Lines2point(cen_ab, normal_ab, cen_bc, normal_bc);
end

%求空间中两直线的交点，其中直线1过p1，方向为v1，直线2同理
function result = Lines2point(p1,v1,p2,v2)
if v2(1) * v1(2) - v2(2) * v1(1) ~= 0
    t = (v2(2) * (p1(1) - p2(1)) - v2(1) * (p1(2) - p2(2))) / (v2(1) * v1(2) - v2(2) * v1(1));
elseif v2(1) * v1(3) - v2(3) * v1(1) ~= 0
    t = (v2(3) * (p1(1) - p2(1)) - v2(1) * (p1(3) - p2(3))) / (v2(1) * v1(3) - v2(3) * v1(1));
elseif v2(2) * v1(3) - v2(3) * v1(2) ~= 0
    t = (v2(3) * (p1(2) - p2(2)) - v2(2) * (p1(3) - p2(3))) / (v2(2) * v1(3) - v2(3) * v1(2));
end
result = v1 * t + p1;