今天分配的一项任务挺有意思的，在这里做个记录.需求很简单，已知条件是平面上两个点、的坐标，它们是曲率圆上的任意两点；一个实数，它的大小表示圆的半径，它和半径的差别在于它区分正负；一个点的坐标，表示车辆前轴中心点.现在过，作一个半径为的圆，然后求车辆到这个圆的最小距离，圆的方位通过的正负号判定，正号为满足右手定则的行驶方向，右手从旋向，圆心在握拳的一侧，负号则为另一侧.

这个问题看上去很简单，但笔者先用Matlab和Mathematica计算了一下圆心坐标，才发现表达式非常冗杂.

Mathematica计算结果

这个结果不光冗杂，而且不敢用.于是笔者在网上找了一下别人的解法，原来Mathematica的这个结果可以通过约定几个参数进行化简.在前人的基础上，笔者设计了这样一个Matlab脚本，贴在下方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
clear;close all;
A = [1, 3];% t时刻解算的圆上A点
B = [-1, 2];% t+1时刻解算的圆上B点
C = [0, 3];% 圆外点
R = -2% 半径
% 判断AB点的距离是否比半径大，确认确实可以作圆
if (A(1) - B(1))*(A(1) - B(1)) + (A(2) - B(2))*(A(2) - B(2)) > 4*R*R
    Res = 'Error'
% 如果AB点的纵坐标相同，则else的参数N会发散，需作为特殊情况计算圆心，1E-15是计算精度
elseif abs(A(1) - B(1)) <= 1E-15
    %K1,K2,K3是一元二次方程K1*X^2 + K2*X + K3 = 0的三个参数，其中X表示圆心横坐标
    K1 = 1;
    K2 = 2*A(1);
    K3 = A(1)*A(1) + (A(2)*A(2) + B(2)*B(2) - 2*A(2)*B(2))/4 - R*R;
    %一元二次方程有两个实根O1、02，对比 AB与AO的叉乘 和 R的符号 是否相反来判断根的选取
    O(1) = (-K2 - sqrt(K2*K2 - 4*K1*K3))/(2*K1);
    AB = B - A;
    AO = O - A;
    wedge = AB(1)*AO(2) - AO(1)*AB(2)
    if sign(wedge) ~= sign(R)
        O(1) = (-K2 + sqrt(K2*K2 - 4*K1*K3))/(2*K1);
    end
    O(2) = (A(2) + B(2))/2;  
else
    M = (B(1)*B(1) - A(1)*A(1) + B(2)*B(2) - A(2)*A(2))/(2*B(1)-2*A(1));
    N = (B(2) - A(2))/(B(1) - A(1));
    %K1,K2,K3是一元二次方程K1*X^2 + K2*X + K3 = 0的三个参数，其中X表示圆心横坐标
    K1 = 1 + N*N;
    K2 = 2*(A(1) - M)*N - 2*A(2);
    K3 = (A(1) - M)*(A(1) - M) + A(2)*A(2) - R*R;
    %一元二次方程有两个实根O1、02，对比 AB与AO的叉乘 和 R的符号 是否相反来判断根的选取
    O(2) = (-K2 - sqrt(K2*K2 - 4*K1*K3))/(2*K1);
    AB = B - A;
    AO = O - A;
    wedge = AB(1)*AO(2) - AO(1)*AB(2)
    if sign(wedge) ~= sign(R)
        O(2) = (-K2 + sqrt(K2*K2 - 4*K1*K3))/(2*K1);
    end
    O(1) = M - N*O(2);
end
% 计算点C到圆心的距离d = ||OC||，判定C在圆内还是圆外
d = sqrt((C(1)-O(1))*(C(1)-O(1)) + (C(2)-O(2))*(C(2)-O(2)));
if d > abs(R)
    d = d - abs(R)
else
    d = abs(R) - d
end
% 画图
t = 0:0.01:2*pi;
t = t';
X(:,1) = abs(R)*cos(t) + O(1);
X(:,2) = abs(R)*sin(t) + O(2);
figure(1);
hold on;
plot(A(1),A(2),'*r');
text(A(1),A(2),' A');
plot(B(1),B(2),'*r');
text(B(1),B(2),' B');
plot(C(1),C(2),'*k');
text(C(1),C(2),' C');
plot(O(1),O(2),'*b');
text(O(1),O(2),' O');
plot(X(:,1),X(:,2),':m');

笔者带入了一些例子测试，绘图结果如下.

Matlab计算结果1
Matlab计算结果3
Matlab计算结果3
Matlab计算结果4