【Matlab案例】求三维空间中圆和直线的最短距离 分析及 matlab程序

1 空间中圆的参数方程

  三维空间中的圆可以看做是$XoY$平面上的圆经过位姿变换得到的。
  在空间$XoY$平面上有一个圆$A$，其圆心坐标为$A(0,0,0)$，半径为$r$。则圆$A$的法向量${\vec{n}}_A=(0,0,1)$(右手定则)。则圆$A$上一点$P(x_p,y_p,z_p)$的齐次坐标可以表示为
$$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\\ 1\end{array}\right]=\left[\begin{array}{c}r\cos \theta \\ r\sin \theta \\ 0\\ 1\end{array}\right]\text{\hspace{1em}(1)}$$
假设空间中有另一个圆$B$，圆$B$与圆$A$半径相同。圆$B$的中心坐标为$B(x_B,y_B,z_B)$，圆$B$的法向量${\vec{n}}_B$，则圆$B$可以由圆$A$通过位姿变换得到，则圆$B$上一点$Q(x_q,y_q,z_q)$的齐次坐标可以表示为

$$\left[\begin{array}{c}{x}_q\\ y_q\\ z_p\\ 1\end{array}\right]=T\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\\ 1\end{array}\right]={\left[\begin{array}{cc}{{}_B^{A}R}_{3\times 3}& {{}_B^{A}P}_{3\times 1}\\ \begin{array}{ccc}0& 0& 0\end{array}& 1\end{array}\right]}_{4\times 4}\left[\begin{array}{c}r\cos \theta \\ r\sin \theta \\ 0\\ 1\end{array}\right]\text{\hspace{1em}(2)}$$

  其中$T$是齐次变换矩阵，${}_B^A{P}_{3\times 1}$可以由圆$A$和圆$B$的中心位置得到，${}_B^A{R}_{3\times 3}$可以由圆$A$的法向量${\vec{n}}_A$和圆$B$的法向量${\vec{n}}_B$得到。
  这样，只要我们知道空间中一圆的中心坐标，和它的法向量，我们就能知道该圆的参数方程。

上图中蓝色圆为圆$A$，红色圆为圆$B$，matlab代码如下（需要用到Robotic Toolbox中的函数）

%------线段首末端点------
% A=[-3;2;1];%可自己修改
% B=[3;0;3];
%-----圆的半径----
r=2;
%求圆B到圆A的齐次变换矩阵
nA=[0;0;1];%XOY平面圆A的法向量
nB=[0;1;0];%空间圆B的法向量
OP=[2;2;2];%圆B中心在空间中的坐标
Rt=VectorRotMartix(nA,nB);%求两向量的旋转矩阵
Pt=transl(OP);%平移矩阵
T=Pt*Rt;%齐次变换矩阵
%圆A在XOY平面的参数方程
theta=[0:10:360];
xr=r*cosd(theta);
yr=r*sind(theta);
%XOY平面的圆A的齐次坐标
CircleA(1,:)=xr;
CircleA(2,:)=yr;
CircleA(3,:)=0;
CircleA(4,:)=1;
%求位姿变换后的圆B得齐次坐标
CircleB=T*CircleA;

plot3(CircleA(1,:),CircleA(2,:),CircleA(3,:),'b','LineWidth',1.5)
hold on
quiver3(0,0,0,nA(1),nA(2),nA(3),'b','maxheadsize',2,'AutoScale','on');
plot3(CircleB(1,:),CircleB(2,:),CircleB(3,:),'r','LineWidth',1.5)
hold on
quiver3(OP(1),OP(2),OP(3),nB(1),nB(2),nB(3),'r','maxheadsize',2,'AutoScale','on');
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on
axis equal

其中 VectorRotMartix() 函数的代码如下

function R=VectorRotMartix(OA,OB)
% VectorRotMartix(OA,OB)
% 返回向量OA到OB的齐次旋转矩阵 4×4
sigma=acos(dot(OA,OB)/(norm(OA)*norm(OB)));%弧度制
if sigma==0 %排除OA OB夹角为零的特殊情况
    R=eye(4);
else
    normalVector=cross(OA,OB);%求法向量
    normalVector=(normalVector/norm(normalVector));%单位化
    R=angvec2tr(sigma, normalVector);%需要用到机器人工具箱
end
end

2 直线到圆的最短距离 约束方程

  空间中有一条线段，首末端点为A $(x_1,y_1,z_1)$，B $(x_2,y_2,z_2)$。设P是直线AB上的一点，想象P点可以在直线AB上滑动，采用参数方程描述P$(x_p,y_p,z_p)$。
$$\{\begin{array}{l}{x}_p=x_1+s\left(x_2-x_1\right)\\ y_p=y_1+s\left(y_2-y_1\right)\\ z_p=z_1+s\left(z_2-z_1\right)\end{array}\text{\hspace{1em}(3)}$$
  空间中一圆也采用参数方程描述，圆上一点$Q(x_q,y_q,z_q)$参数方程如(2)式。
  则PQ之间距离可以用向量的模长表示
$$\Vert \overrightarrow{PQ}\Vert =\sqrt{{\left(x_p-x_q\right)}^2+{\left(y_p-y_q\right)}^2+{\left(z_p-z_q\right)}^2}$$
  由于开方不好计算，我们可以求PQ之间距离平方的最小值，写成函数形式即
$$f\left(s,\theta \right)={\Vert \overrightarrow{PQ}\Vert }^2={\left(x_p-x_q\right)}^2+{\left(y_p-y_q\right)}^2+{\left(z_p-z_q\right)}^2$$

利用matlab建立约束方程$f\left(s,\theta \right)$，代码如下，$\theta$用$t$表示。

%-----求解约束方程-----
syms s t
%s取0~1时P点在线段AB上滑动，s>1 P点在B端点外，s<1 P点在A端点外
P=A+s.*(B-A);
%空间圆的参数方程
Q=T*[r*cosd(t);r*sind(t);0;1];
Q=Q(1:3);
%求P点到Q点的距离的平方
Fst=sum((P-Q).^2);

3 偏导法

  要求直线到圆的最短距离，就是求二元函数$f\left(s,\theta \right)$的极小值。二元函数的极小值问题可以通过对函数$f\left(s,\theta \right)$分别关于$s$和$\theta$求偏导，并令零偏导数为0后解方程组，求得$s$和$\theta$。
$$\{\begin{array}{l}\frac{\partial f(s,\theta )}{\partial s}=0\\ \frac{\partial f(s,\theta )}{\partial \theta }=0\end{array}$$
matlab代码如下：

%%  利用偏导数为0 求解圆到直线的最小距离
eqns1=(diff(Fst,s)==0);
eqns2=(diff(Fst,t)==0);
eqns=[eqns1 eqns2];
%解二元方程组
[S,T]=solve(eqns,s,t)
%---去除复数解---
S=S(~logical(imag(S)));
T=T(~logical(imag(T)));
%绘图
num=size(S,1);
for i=1:num
    Pmin=double(subs(P,double(S(i))));
    Qmin=double(subs(Q,double(T(i))));
    %画线段AB、CD最短距离
plot3([Pmin(1),Qmin(1)],[Pmin(2),Qmin(2)],[Pmin(3),Qmin(3)],'m','LineWidth',1,'Marker','.','MarkerSize',20);
end

3.1 多解情况

当直线和圆处于一些特殊位置时可能会存在多解情况，比如当直线与圆所在平面平行时且在圆的正上方时，方程组存在4组解。

%------线段首末端点------
% A=[-3;2;1];
% B=[3;0;3];
%-----圆的半径----
r=2;%圆心位于原点

3.2 无解情况

同样，当直线和圆处于一些特殊位置时可能会存在无解情况，如

%------线段首末端点------
A=[-3;2;1];
B=[3;0;3];
%-----圆的半径----
r=2;%圆心位于原点

4 数值优化法

  求二元函数$f\left(s,\theta \right)$的极小值也可以看作是最优化问题，可以利用牛顿法等优化方法解决，这里使用matlab优化工具箱中的fminunc()函数。
  需要注意的是，一般的最优化方法都是用来解决单谷函数极小值问题（也叫凸优化问题）。而在本例中，$f\left(s,\theta \right)$不一定是单谷函数，它取决于圆和直线的相对位置。所以如果初始点选的不好，则最终可能近收敛到局部极小值点，而不是全局极小值。
  举例，一圆半径为3，圆心位于原点；圆中有一条垂直于圆的偏置直线，首末端点为(2,0,-1)，(2,0,4)。如若选择初始迭代点$(s,\theta )$为(0.5,0)，则最终收敛结果如下图。

  但若选择初始迭代点$(s,\theta )$为(0.5,180)，则最终收敛结果如下图。

  可见，初始迭代点的选取对于非单谷函数最优化问题影响十分严重。不过对于三维空间中圆和直线的最短距离的优化问题，我们可以选择2个截然相反的初始点$\theta$，比如一个$\theta =0$，另一个$\theta =180$，然后比较两种优化结果，选取最小的结果即可。
  对于偏导法无解的情况，优化法则可以轻松解决。

matlab代码如下：

%%  利用以最优化方法 求解圆到直线的最小距离
%----将约束方程转为函数句柄----
fun = matlabFunction(Fst,'Vars',{[s t]});
%两次不同迭代初值取最小值
for i=[0 180]
    x0 = [0.5,i];
    options = optimoptions(@fminunc,'Display','iter','Algorithm','quasi-newton');
    [x,fval] = fminunc(fun,x0,options);
    %----画最短距离----
    Pmin=double(subs(P,x(1)));
    Qmin=double(subs(Q,x(2)));
    %画线段AB、CD最短距离
    plot3([Pmin(1),Qmin(1)],[Pmin(2),Qmin(2)],[Pmin(3),Qmin(3)],'g','LineWidth',1,'Marker','o');
    hold on;
end

5 总结

在某些情况下，偏导法求出的并不是最小值。例如

A=[-4;0;3];
B=[3;6;5];
r=2;

下图中紫色线段为偏导法求出的最短距离，而绿色线段为数值优化法求出的最短距离，显然偏导法所求并不是最短距离。

总的来说，数值优化方法比偏导法要可靠的多。