最小二乘拟合圆柱

一、算法原理

由圆柱面的几何特性可得，圆柱面上的点到其轴线的距离恒等于半径 $r_0$ ，如图1 所示。
在这里插入图片描述

其中， $P$ 为圆柱面上任意一点， $P_0( x_0，y_0，z_0)$ 为圆柱轴线上一点， $(a ， b ， c)$ 为圆柱轴线向
量， $r_0$ 为圆柱底圆半径。圆柱面上任意一点到其轴线的距离为半径 $r_0$ ，即:
$(x-x_0)^2+(y-y_0)^2+(z-z_0)^2-[a(x-x_0)+b(y-y_0)+c(z-z_0)]^2=r_0^2\tag{1}$

式( 1) 即为圆柱面方程。根据圆柱面上点的坐标 $(x ， y ， z)$ ，求出圆柱轴线上一点 $x_0，y_0，z_0)$ 、圆柱轴线向量 $(a ， b ， c)$ 、圆柱底圆半径 $r_0$ 这七个参数，就可以唯一确定一个圆柱。

柱面拟合方法详细步骤如下:
(1) 确定柱面模型参数初始值。圆柱拟合初值的估计原理如下:

搜寻圆柱面上任意一点的若干邻近点，将这些点拟合成平面，得到的平面法向量单位化即该点的单位法向量。对圆柱面上每个点处理，得到每个点的单位法向量。将每个点的单位法向量看成点，将这些点拟合成平面，得到平面法向量，即圆柱轴线向量初始值 $a_0、b_0、c_0$ ，该步骤使用了主成分分析法进行求解。
先对圆柱进行坐标转换，使圆柱轴线向量 $a_0，b_0，c_0)$ 变换为平行 $Z$ 轴的向量即为 $0，0，\sqrt{( a_0 )^ 2 + ( b_0)^2 + ( c_0)^ 2} )$ 。设旋转矩阵为 $Ｒ_{3 ×3}$ ，要使:
在这里插入图片描述

将旋转矩阵与圆柱上的点的坐标 $(x, y, z)$ 相乘，得到的新坐标 $(x^{'}, y^{'}, z^{'})$ ，将新坐标中的 $x^{'}, y^{'}$ 取出得新坐标系下圆柱底面的坐标，根据 $(x^{'}, y^{'})$ 可拟合一个圆。
圆的方程为 $x-x_1^{0'})^2+(y-y_1^{0'})^2=(r_0)^2$ 若知道圆心坐标 $x_1^{0'},y_1^{0'}$ 、半径 $r_0$ 这三个参数，就可以唯一确定一个圆面。
首先，圆坐标 $(x^{'}, y^{'})$ 准备完毕；其次，找误差方程，将圆的方程展开得到 $x^2+(x_1^{0'})^2+y^2+(y_1^{0'})^2-2yy_1^{0'}-(r_0)^2=0$ ，即 $2x(x_1^{0'})+2y(y_1^{0'})+(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2-(x^2+y^2)=0$ 。可以设 $x_1^{0'}、y_1^{0'}、(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2$ 为待求参数。于是误差方程为： $V=2x(x_1^{0'})+2y(y_1^{0'})+(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2-(x^2+y^2)$ ，写成矩阵的形式即：

在这里插入图片描述

假设获取的圆的点等精度，即等权。根据线性最小二乘原理，即 $V^TV=min$ ，求得位置参数 $X$ ：
$X=(B^TB)^{-1}B^TL\tag{5}$
求出新坐标系下圆柱轴线上一点的初始值 $x_1^{0'},y_1^{0'}$ 以及底圆半径的初始值 $r_0$ ，再根据公式：
在这里插入图片描述
将新坐标系下的 $x_1^{0'},y_1^{0'}$ 转换成旧坐标系下的坐标 $x_1^{0},y_1^{0}$ 。无论在新坐标系下还是在旧坐标系下，底圆半径的初值 $r_0$ 都不变。为了尽量减小误差，将圆柱轴线上一点的初值 $z_1^{0}$ 的值设为原坐标系下圆柱点 $z$ 的最大值与最小值和的一半。于是求出圆柱轴线上一点、圆柱底半径这四个参数的初始值 $x_1^{0},y_1^{0},z_1^{0},r_0$ 。
（2）建立改进误差方程式求解参数值。圆柱面方程为：
$(x-x_1)^2+(y-y_1)^2+(z-z_1)^2-[a(x-x_1)+b(y-y_1)+c(z-z_1)]^2=r^2\tag{7}$
传统方法，令
$f=(x-x_1)^2+(y-y_1)^2+(z-z_1)^2-[a(x-x_1)+b(y-y_1)+c(z-z_1)]^2-r^2\tag{8}$
对 $f$ 线性化得
在这里插入图片描述
这样，误差方程可列为：

其中

$V_{n\times 1}=B_{n\times 7}X_{7\times 1}-L_{n\times 1}\tag{23}$