最小二乘拟合空间圆——拉格朗日乘子法

一、拉格朗日乘子法

  拉格朗日乘子法又称为拉格朗日乘数法，用来求解函数$f(x_1,x_2,...)$在$g(x_1,x_2,...)=0$的约束条件下的极值。拉格朗日乘子法一般分为３个步骤：首先，引入一个新的参数λ（称为拉格朗日乘子）；其次，联立约束条件及原函数，使得变量数量与方程数量相同；最后，求出方程中各个变量的解。拉格朗日乘子法在微分几何、数学物理方程中有着重要应用，同时在经济学及工程学等实际领域中也有着广泛的应用。

二、空间圆拟合

  传统方法并非直接对空间圆弧进行拟合，其拟合精度易受噪声影响且鲁棒性差。因此，在拉格朗日乘子法的基础上，基于平面条件约束建立目标函数，进而推导出空间圆弧拟合方程。
  基于拉格朗日乘子法的空间圆弧拟合步骤如下：

2.1、建立空间圆方程

$$\{\begin{array}{l}Ax+By+Cz+D=0\\ (x－x_0{)}^2+(y－y_0{)}^2+(z－z_0{)}^2=R^2\end{array}\text{\hspace{1em}(1)}$$
式中：$Ax+By+Cz+D=0$表示一个法向量为$(A,B,C)$的平面方程；
$(x－x_0{)}^2+(y－y_0{)}^2+(z－z_0{)}^2=R^2$表示球心坐标为：$(x_0,y_0,z_0)$、半径为$R$的球面方程。
  然后，通过三维数据点建立空间平面方程：

$$\left[\begin{array}{cccc}{x}_1& y_1& z_1& 1\\ x_2& y_2& z_2& 1\\ ⋮& ⋮& ⋮& 1\\ x_n& y_n& z_n& 1\end{array}\right]\left[\begin{array}{c}A\\ B\\ C\\ D\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ ⋮\\ 0\end{array}\right]\text{\hspace{1em}(2)}$$

式中：$(x_1,y_1,z_1)$、$(x_2,y_2,z_2)$、…、$(x_n,y_n,z_n)$为三维数据点。
  根据式(2)，利用最小二乘法拟合得到平面方程的系数$A、B、C、D$，进而可得出空间圆所在空间的平面方程。将全部点云数据$(x_i,y_i,z_i)$投影到平面$Ax＋By＋Cz＋D=0$上，得到投影点$(x_i^{{}^{\prime }},y_i^{{}^{\prime }},z_i^{{}^{\prime }})$，各点投影公式为：
$$\{\begin{array}{l}{x}_i^{{}^{\prime }}=A\cdot k+x_i\\ y_i^{{}^{\prime }}=A\cdot k+y_i\\ z_i^{{}^{\prime }}=A\cdot k+z_i\end{array}\text{\hspace{1em}(3)}$$
式中，$$k=-\frac{A{x}_i+B{y}_i+C{z}_i+D}{A^2+B^2+C^2}$$

2.2、构建Lagrange目标函数

  由解析几何知识可得，投影点$(x_i^{{}^{\prime }},y_i^{{}^{\prime }},z_i^{{}^{\prime }})$定是平面与球面的相交点，可利用最小二乘法进行求解。将空间圆方程组当作一个多元函数条件极值问题，引入新的参数$\lambda$（拉格朗日乘子），构建Lagrange目标函数：
$$\begin{gathered} F(x_0,y_0,z_0,R,\lambda )=\sum _{i=1}^n[(x_i^{{}^{\prime }}-x_0{)}^2+(y_i^{{}^{\prime }}-y_0{)}^2+(z_i^{{}^{\prime }}-z_0{)}^2-R^2{]}^2 \\ +\lambda (A{x}_0+B{y}_0+C{z}_0+D)\text{\hspace{1em}(4)} \end{gathered}$$
进而根据极值条件建立方程组：
$$F_{x_0}^{{}^{\prime }}=F_{y_0}^{{}^{\prime }}=F_{z_0}^{{}^{\prime }}=F_{R_0}^{{}^{\prime }}=F_{\lambda }^{{}^{\prime }}=0\text{\hspace{1em}(5)}$$
联立方程进行求解。
在解方程组过程中假设式(6)成立：
$$(x_i^{{}^{\prime }}-x_0{)}^2+(y_i^{{}^{\prime }}-y_0{)}^2+(z_i^{{}^{\prime }}-z_0{)}^2-R^2\approx 0\text{\hspace{1em}(6)}$$
然后消去多项式中的高次项，从而获得计算圆心坐标与半径的参数方程，如式(7)所示：

$$\left[\begin{array}{ccc}8M& -4N^T& O^T\\ 2N& U& 0\\ 0& 0& 0\end{array}\right]X=T\text{\hspace{1em}(7)}$$
式中：
$$M=\left[\begin{array}{ccc}{\sum }_{i=1}^n{x}_i^{{}^{\prime }2}& {\sum }_{i=1}^n{x}_i^{{}^{\prime }}{y}_i^{{}^{\prime }}& {\sum }_{i=1}^n{x}_i^{{}^{\prime }}{z}_i^{{}^{\prime }}\\ {\sum }_{i=1}^n{x}_i^{{}^{\prime }}{y}_i^{{}^{\prime }}& {\sum }_{i=1}^n{y}_i^{{}^{\prime }2}& {\sum }_{i=1}^n{y}_i^{{}^{\prime }}{z}_i^{{}^{\prime }}\\ {\sum }_{i=1}^n{x}_i^{{}^{\prime }}{z}_i^{{}^{\prime }}& {\sum }_{i=1}^n{y}_i^{{}^{\prime }}{z}_i^{{}^{\prime }}& {\sum }_{i=1}^n{z}_i^{{}^{\prime }2}\end{array}\right]$$

$$N=\left[\begin{array}{ccc}{\sum }_{i=1}^n{x}_i^{{}^{\prime }}& {\sum }_{i=1}^n{y}_i^{{}^{\prime }}& {\sum }_{i=1}^n{z}_i^{{}^{\prime }}\end{array}\right]$$

$$O=\left[\begin{array}{ccc}{\sum }_{i=1}^{n}A& {\sum }_{i=1}^{n}B& {\sum }_{i=1}^{n}C\end{array}\right]$$

$$U=\sum _{i=1}^n(-1)$$

$$X=\left[\begin{array}{ccccc}{x}_0& y_0& z_0& l& \lambda \end{array}\right]$$

$$T=\left[\begin{array}{c}4{\sum }_{i=1}^n(x_i^{{}^{\prime }3}+x_i^{{}^{\prime }}{y}_i^{{}^{\prime }2}+x_i^{{}^{\prime }}{z}_i^{{}^{\prime }2})\\ 4{\sum }_{i=1}^n(y_i^{{}^{\prime }}{x}_i^{{}^{\prime }2}+y_i^{{}^{\prime }3}+y_i^{{}^{\prime }}{z}_i^{{}^{\prime }2})\\ 4{\sum }_{i=1}^n(z_i^{{}^{\prime }}{x}_i^{{}^{\prime }2}+z_i^{{}^{\prime }}{y}_i^{{}^{\prime }2}+z_i^{{}^{\prime }3})\\ {\sum }_{i=1}^n(x_i^{{}^{\prime }2}+y_i^{{}^{\prime }2}+z_i^{{}^{\prime }2})\\ {\sum }_{i=1}^n(-D)\end{array}\right]$$

2.3、求解圆心和半径

  将式(3)中的投影点$(x_i^{{}^{\prime }},y_i^{{}^{\prime }},z_i^{{}^{\prime }})$代入方程(6)，进而得到空间圆的圆心坐标和$l$值，将圆心坐标代入$l＝x_0^2+y_0^2+z_0^2-R^2$ 即可解出空间圆的半径$R$。

2.4、参考文献

[1] 化春键,熊雪梅,陈莹.基于拉格朗日乘子法的空间圆弧拟合优化方法[J].工程设计学报,2018,25(06):661-667.

三、算法伪码

%% -------------------------------读取点云---------------------------------
pc = PointCloudReader('Circle.pcd');
%% ------------------------------PCA拟合平面-------------------------------
coeff = pcaFitPlane(pc);
% 获取最小特征值对应的特征向量 
A = coeff(1);
B = coeff(2);
C = coeff(3);
% 计算原点到拟合平面的距离
centroid = mean(pc);
D = -dot(A B C,centroid);
%% ----------------------------投影到拟合平面------------------------------
x' = x*(B * B + C * C) - A * (y*B + z.*C + D)/norm([A,B,C]);
y' = y*(A * A + C * C) - B * (x*A + z*C + D)/norm([A,B,C]);
z' = z*(A * A + B * B) - C * (x*A + y*B + D)/norm([A,B,C]);
%% -------------------------拉格朗日乘子法拟合圆---------------------------
% 构建U所需的元素
U = -1*n;
% 构建矩阵T所需的元素
sumE = sum(x'^3 + x'*y'*y' + x'*z'*z');
sumF = sum(y'*x'*x' + y'^3 + y'*z'*z');
sumG = sum(z'*x'*x' + z'*y'*y' + z'^3);
sumH = sum(x'^2 + y'^2 + z'^2);
sumI = -D*n;
% 矩阵M
M = [sumXX sumXY sumXZ;
	sumXY sumYY sumYZ;
	sumXZ sumYZ sumZZ];
% 矩阵N
N = [sumX sumY sumZ];
% 矩阵O
O = [sumA sumB sumC];
% 矩阵T
T = [4*sumE 4*sumF 4*sumG sumH sumI]';
% 系数矩阵W
W = [8 * M -4 * N' O' 2 * N U];
% 最小二乘求解
X = W\T;
% 空间圆半径
r = sqrt(norm(X(1:3))-X(4));
% 空间圆圆心
center = X(1:3);
%% ------------------------输出空间圆的拟合参数---------------------------
fprintf('圆的半径为: %f\n', r);
fprintf('圆心坐标为: ');
center
fprintf('圆法向量为:');
[A,B,C]
%% ----------------------------结果可视化----------------------------------
figure
% 1、绘制原始点
h1 = plot3(x, y, z, '*');
% 2、绘制拟合空间圆
theta = (0:2 * pi / 100 : 2 * pi)';    %  theta角从0到2*pi
a = cross([A B C], [1 0 0]);                 % X与i叉乘，求取a向量
if ~any(a)                             % 如果a为零向量，将X与[0 1 0]叉乘
    a = cross([A B C], [0 1 0]);
end
b = cross([A B C], a);% 求取b向量
a = a / norm(a);      % 单位化a向量
b = b / norm(b);      % 单位化b向量

c1 = center(1) * ones(size(theta, 1), 1);
c2 = center(2) * ones(size(theta, 1), 1);
c3 = center(3) * ones(size(theta, 1), 1);

x = c1 + r * a(1) * cos(theta) + r * b(1) * sin(theta);% 圆上各点的x坐标
y = c2 + r * a(2) * cos(theta) + r * b(2) * sin(theta);% 圆上各点的y坐标
z = c3 + r * a(3) * cos(theta) + r * b(3) * sin(theta);% 圆上各点的z坐标

hold on;
h2 = plot3(x, y, z, '-r');
% 3、绘制拟合圆圆心
h3 = plot3(center(1),center(2),center(3),'go');
xlabel('x轴')
ylabel('y轴')
zlabel('z轴')
legend([h1 h2 h3], '原始点云点', '拟合空间圆', '圆心');
title("拉格朗日乘子法拟合空间圆")
grid on

四、算法效果