求两点连线与其中一点为球心球的交点—

在轨迹规划过程中，常将其它智能体视为具有一定半径的球形障碍物（或圆形障碍物）。若求智能体与障碍物最近的边缘点，其本质即求两点连线与其中一点为球心球的交点。

求的方法有多种，这里采用雅克比矩阵的方法。

雅克比矩阵求坐标

我们设定智能体（其中一点）的坐标为 $x=[x_1,x_2,x_3]^T$ ，障碍物中心（另一点，同时为球心）坐标为 $x_{o}=[x_{o,1},x_{o,2},x_{o,3}]^T$ ，障碍物为半径为 $r$ 。则智能体与障碍物中心距离 $d$ 为

$d=\left \| x-x_o \right \|$

则，可求其雅可比矩阵为：

$J= \begin{bmatrix} \frac{x_1-x_{o,1}}{d} & \frac{x_2-x_{o,2}}{d} & \frac{x_3-x_{o,3}}{d} \end{bmatrix}$

由雅克比矩阵我们可得到：

$\Delta d=J\Delta x$

其中， $\Delta d$ 为沿连线远离球心方向上变化的距离， $\Delta x$ 为三维坐标的变化。由此，可得

$J^\dagger \Delta d=\Delta x$

其中， $J^\dagger$ 为雅可比矩阵的伪逆，可求得通过

$J^\dagger=J^T (J J^T)^{-1}$

由此，我们得到相距球心 $r$ 且在两点连线上的点的坐标为：

$x=x+J^\dagger r$

MATLAB函数

由以上公式可构建MATLAB函数为：

function fpo=State3(xi,xo,r)
pd=norm(xi-xo,2);
JA=[(xi(1,1)-xo(1,1))/pd,(xi(2,1)-xo(2,1))/pd,(xi(3,1)-xo(3,1))/pd];
pJA=pinv(JA);
fpo=xo+pJA*r;
end

MATLAB实例

clear;
clc;
close all;%%
%参数设定
Rer=1.3;                            %球半径
UAV1=[5;5;5];                       %障碍物坐标
UAV2=[6.5; 6.5; 5.8];               %另一点坐标
fpo=State3(UAV2,UAV1,Rer);          %计算坐标
%%
figure
h1=plot3(UAV1(1,1),UAV1(2,1),UAV1(3,1),'^g','LineWidth',2); hold on;
h2=plot3(UAV2(1,1),UAV2(2,1),UAV2(3,1),'b^','LineWidth',2);
h3=plot3(fpo(1,1),fpo(2,1),fpo(3,1),'r*','LineWidth',2);
h4=plot3([UAV1(1,1),fpo(1,1)],[UAV1(2,1),fpo(2,1)],[UAV1(3,1),fpo(3,1)],':r','LineWidth',2);
plot3([UAV2(1,1),fpo(1,1)],[UAV2(2,1),fpo(2,1)],[UAV2(3,1),fpo(3,1)],'b','LineWidth',1);
[x,y,z]=ellipsoid(UAV1(1,1),UAV1(2,1),UAV1(3,1),Rer,Rer,Rer);
re=[1 1 1];
colormap(re)
surf(x,y,z);
alpha(0.7);
legend([h2,h1,h3,h4],'智能体中心','障碍物中心','交点','球体半径');
xlabel("x (m)");
ylabel("y (m)");
zlabel("z (m)");
axis equal;%%
function fpo=State3(xi,xo,r)
pd=norm(xi-xo,2);
JA=[(xi(1,1)-xo(1,1))/pd,(xi(2,1)-xo(2,1))/pd,(xi(3,1)-xo(3,1))/pd];
pJA=pinv(JA);
fpo=xo+pJA*r;
end