算法涉及基本的 [[FBG传感器模型]]，一般通过刻有若干个布拉格光栅的光纤集成到被测物体上，当光纤与被测物体同时发生形变的时候，布拉格光栅产生应变，从而产生波长的漂移，通过对入射光与反射光波长漂移的检测，可以计算得到光纤上若干个点的曲率与挠率。
在微分几何中，可以通过 [[Frenet–Serret 标架]]对曲线的局部信息进行描述，其利用单位切向量 \(\mathrm{T}\) ，单位法向量 \(\mathrm{N}\) ，单位副法向量 \(\mathrm{B}\) 坐标系并结合曲线的曲率 \(\kappa\) 和挠率 \(\tau\),得到[[Frenet–Serret 方程组]]：
\[\begin{eqnarray}
\frac{\mathrm{d} \mathrm{T} }{\mathrm{d} s} & = & \kappa \mathrm{N} \\
\frac{\mathrm{d} \mathrm{N} }{\mathrm{d} s} & = & -\kappa \mathrm{T} + \tau \mathrm{B} \\
\frac{\mathrm{d} \mathrm{B} }{\mathrm{d} s} & = & -\tau \mathrm{N}
\end{eqnarray}
\]
本算法将利用 FBG 传感器得到的曲线上若干数量的曲率 \(\kappa[s]\) 和挠率 \(\tau[s]\), 通过数值方法求解 Frenet–Serret 方程组，重建光纤的实际形状。
由于没有实际的传感器，这里将通过仿真产生相关的数据进行算法的验证。
构造曲线数据
这里选用了空间螺旋线作为示例，其参数形式为
\[\begin{eqnarray}
x(t) & = & r*\cos(t) \\
y(t) & = & r*\sin(t) \\
z(t) & = & p*t
\end{eqnarray}
\]
其中，\(r\) 是螺旋线的半径，\(p\) 是螺旋线的节距。
容易得到其一二三阶的导数分别为
\[\begin{eqnarray}
x'(t) & = & -r*\sin(t) \\
y'(t) & = & r*\cos(t) \\
z'(t) & = & p
\end{eqnarray}
\]
\[\begin{eqnarray}
x''(t) & = & -r*\cos(t) \\
y''(t) & = & -r*\sin(t) \\
z''(t) & = & 0
\end{eqnarray}
\]
\[\begin{eqnarray}
x'''(t) & = & r*\sin(t) \\
y'''(t) & = & -r*\cos(t) \\
z'''(t) & = & 0
\end{eqnarray}
\]
对应的代码实现分别是
function r = cal_helic_pos(radius, pitch, t)
x = radius * cos(t);
y = radius * sin(t);
z = pitch * t;
r = [x; y; z];
end
function v= cal_helic_vel(radius, pitch, t)
vx = -radius * sin(t);
vy = radius * cos(t);
vz = pitch * ones(1, length(t));
v = [vx; vy; vz];
end
function a= cal_helic_acc(radius, pitch, t)
ax = -radius * cos(t);
ay = -radius * sin(t);
az = 0 * t;
a = [ax; ay; az];
end
function j= cal_helic_jerk(radius, pitch, t)
jx = radius * sin(j);
jy = -radius * cos(j);
jz = 0 * j;
j = [jx; jy; jz];
end
由于 FBG 传感器所能刻蚀的光栅有限（一般而言都是 38 个以下），本次仿真中假设光纤整体长度 900 mm，每间隔 30 mm 分布一个布拉格光栅，总共 31 个光栅。在仿真中，我们需要在 31 个光栅的位置计算其曲率和挠率（替代传感器的真实数据）。
空间曲线的曲率与挠率
由微积分基本知识，可以得到
\[\kappa(t) = \frac{\left | r'(t) \times r''(t) \right | }{\left | r'(t) \right |^3}
\]
\[\tau(t) = \frac{ (r'(t) \times r''(t))\cdot r'''(t) }{\left | r'(t) \times r''(t) \right |^2}
\]
通过以上关系可以得到 31 个光栅位置的曲率和挠率，算法将使用这些离散的曲率和挠率对空间曲线进行重建。
求解 Frenet–Serret 方程组
数值积分求解微分方程组
Frenet–Serret 方程组是一个典型的常微分方程组，由于实际情况中没有函数的解析形式，所以可以采用数值积分的方法对方程组进行求解。关于数值积分方法可以参考。欧拉法是最基本的数值积分求解微分方程的方法，但是一阶欧拉积分的精度太低，不能满足要求，实际情况中很少使用。龙格-库塔法是一种非线性常微分方程的数值解法。由数学家卡尔·龙格和马丁·威尔海姆·库塔于1900年左右发明。 Runge-Kutta 公式的思路就是利用区间内一些特殊点的一阶导数值的线性组合来替代某点处的 n 阶导数值,这样就可以仅通过一系列一阶导数值来得到某点幂级数展开的预测效果。龙格－库塔法（Runge-Kutta methods）是用于非线性常微分方程的解的重要的一类隐式或显式迭代法。在工程中最常用的是四阶龙格－库塔积分，也就是 RK4 积分。
我们先从欧拉方法考虑，可以将 [[Frenet–Serret 方程组]]改写成离散的差分形式
\[ \begin{array}{l}{{\hat{\mathbf{t}}_{k+1}=\hat{\mathbf{t}}_{k}+\kappa_{k}\Delta s\hat{\mathbf{n}}_{k},}}\\ {{\hat{\mathbf{n}}_{k+1}=\mathbf{\hat{n}}_{k}-\kappa_{k}\Delta s\hat{\mathbf{t}}_{k}+\tau_{k}\Delta s\hat{\mathbf{b}}_{k},}}\\ {{\hat{\mathbf{b}}_{k+1}=\mathbf{\hat{b}}_{k}-\tau_{k}\Delta s\hat{\mathbf{n}}_{k}.}}\end{array}
\]
这种差分的方式是对真实情况比较差的拟合，因为它假定了在局部的 \(\hat{\mathbf{t}}_{k}, \hat{\mathbf{n}}_{k}, \hat{\mathbf{b}}_{k}\) 都是恒定的。另外，经过这种迭代后，\(\hat{\mathbf{t}}_{k+1}, \hat{\mathbf{n}}_{k+1}, \hat{\mathbf{b}}_{k+1}\) 失去了正交性，且不再是单位向量。我们需要进行一定的处理，
\[\begin{eqnarray}
\hat{\mathbf{t}}_{k+1}^{rect} & = & \frac{\hat{\mathbf{t}}_{k+1}}{|\hat{\mathbf{t}}_{k+1}|},\\
\hat{\mathbf{n}}_{k+1}^{rect} & = & \frac{\hat{\mathbf{b}}_{k+1}\times\hat{\mathbf{t}}_{k+1}^{rect}}{|\hat{\mathbf{b}}_{k+1}\times\hat{\mathbf{t}}_{k+1}^{rect}|}, \\
\hat{\mathbf{b}}_{k+1}^{rect} & = & \hat{\mathbf{t}}_{k+1} \times \hat{\mathbf{n}}_{k+1}^{rect}.
\end{eqnarray}\]
T(i,:) = T(i,:)/norm(T(i,:));
N(i,:) = cross(B(i,:),T(i,:))/norm(cross(B(i,:),T(i,:)));
B(i,:) = cross(T(i,:),N(i,:));
这种方式可以保证标架的单位正交性，但是并不能消除积分过程中的累积误差。
为了减少积分过程中的累积误差，可以将上述仅考虑一阶微分关系的差分方程改成引入高阶微分关系的差分方程，即龙格－库塔法。我们这里使用四阶龙格－库塔的方法，可以将积分的关系式写为
\[\begin{eqnarray}
\hat{\mathbf{t}}_{k+1} &=&\hat{\bf{t}}_{k}+\frac{\Delta{s}}{6}(a_{t1}+2a_{t2}+2a_{t3}+a_{t4}), \\
\hat{\mathbf{n}}_{k+1} &=&\hat{\bf{n}}_{k}+\frac{\Delta{s}}{6}(a_{n1}+2a_{n2}+2a_{n3}+a_{n4}), \\
\hat{\mathbf{b}}_{k+1} &=&\hat{\bf{b}}_{k}+\frac{\Delta{s}}{6}(a_{b1}+2a_{b2}+2a_{b3}+a_{b4}).
\end{eqnarray} \]
其中，
\[\begin{eqnarray}
a_{t1}&=&\kappa_{k}\hat{\bf n}_{k},\\
a_{n1}&=&-\kappa_{k}\hat{\bf t}_{k}+\tau_{k}\hat{\bf b}_{k},\\
a_{b1}&=&-\tau_{k}\hat{\bf n}_{k},
\end{eqnarray}\]
\[\begin{eqnarray}
a_{t2}&=&\kappa_{k}\left(\hat{\bf n}_{k}+\frac{\Delta s}{2}a_{n1}\right),\\
a_{n2}&=&-\kappa_{k}(\hat{\bf t}_{k}+\frac{\Delta s}{2}a_{t1})+\tau_{k}(\hat{\bf b}_{k}+ \frac{\Delta s}{2} a_{b1}),\\
a_{b2}&=&-\tau_{k}\left(\hat{\bf n}_{k}+\frac{\Delta s}{2}a_{n1}\right),
\end{eqnarray}
\]
\[\begin{eqnarray}
a_{t3}&=&\kappa_{k}\left(\hat{\bf n}_{k}+\frac{\Delta s}{2}a_{n2}\right),\\
a_{n3}&=&-\kappa_{k}(\hat{\bf t}_{k}+\frac{\Delta s}{2}a_{t2})+\tau_{k}(\hat{\bf b}_{k}+ \frac{\Delta s}{2} a_{b2}),\\
a_{b3}&=&-\tau_{k}\left(\hat{\bf n}_{k}+\frac{\Delta s}{2}a_{n2}\right),
\end{eqnarray}
\]
\[\begin{eqnarray}
a_{t4}&=&\kappa_{k}\left(\hat{\bf n}_{k}+\Delta sa_{n3}\right),\\
a_{n4}&=&-\kappa_{k}(\hat{\bf t}_{k}+\Delta sa_{t3})+\tau_{k}(\hat{\bf b}_{k}+ \Delta s a_{b3}),\\
a_{b4}&=&-\tau_{k}\left(\hat{\bf n}_{k}+\Delta sa_{n3}\right).
\end{eqnarray}
\]
最终整理可以得到
\[\begin{eqnarray}
\hat{\mathbf{t}}_{k+1} &=& \left[1 + \frac{\kappa^{2}\Delta s^{2}}{2}\,+\,\frac{(\kappa^{4}+\tau^{2})\Delta s^{4}}{4}\right]\hat{\bf t}_{k}
+ \left[\kappa\Delta s-\frac{\left(\kappa^{3}-\kappa\tau^{2}\right)\Delta s^{3}}{6}\right]\hat{\bf n}_{k}
+ \left[ \frac{\kappa\tau\Delta s^{2}}{2} - \frac{(\kappa^3\tau+\kappa\tau^3)\Delta s^{4}} {24} \right ]\hat{\bf b}_{k}, \\
\hat{\mathbf{n}}_{k+1} &=& \left[-\kappa \Delta s + \frac{（\kappa\tau^{2} + \kappa^{3}）\Delta s^{3}}{6} \right]\hat{\bf t}_{k} + \left[1\,-\,\frac{\left(\kappa^{2}+\tau^{2}\right)\Delta s^{2}}{2}\,+\,\frac{\left(\kappa^{2}+\tau^{2}\right)^{2}\Delta s^{4}}{24}\right]\hat{\bf n}_{k}
+ \left[\tau \Delta s - \frac{（\kappa^{2}\tau + \tau^{3}）\Delta s^{3}}{6} \right]\hat{\bf b}_{k} , \\
\hat{\mathbf{b}}_{k+1} &=& \left[\frac{\kappa\tau\Delta s^{2}}{2} - \frac{（\kappa\tau^{3} + \kappa^{3}\tau）\Delta s^{4}}{24} \right]\hat{\bf t}_{k} + \left[-\tau \Delta s + \frac{\left(\kappa^{2}\tau + \tau^{3}\right)\Delta s^{3}}{6}\right]\hat{\bf n}_{k}
+ \left[1\,-\,\frac{\tau^{2}\Delta s^{2}}{2}\,+\,\frac{\left(\kappa^{2}\tau^{2}+\tau^4\right)\Delta s^{4}}{24}\right]\hat{\bf b}_{k}.
\end{eqnarray}\]
相应的代码为
T(i,:) = (1+k(i-1)^2*ds^2/2+(k(i-1)^4+k(i-1)^2*t(i-1)^2)*ds^4/4)*T(i-1,:)...
+ (k(i-1)*ds-(k(i-1)^3-k(i-1)*t(i-1)^2)*ds^3/6)*N(i-1,:)...
+ (k(i-1)*t(i-1)*ds^2/2-(k(i-1)^3*t(i-1)+k(i-1)*t(i-1)^3)*ds^4/24)*B(i-1,:);
B(i,:) = (k(i-1)*t(i-1)*ds^2/2 - (k(i-1)*t(i-1)^3+k(i-1)^3*t(i-1))*ds^4/24)*T(i-1,:)...
+ (-t(i-1)*ds+(k(i-1)^2*t(i-1)+t(i-1)^3)*ds^3/6)*N(i-1,:)...
+ (1-t(i-1)^2*ds^2/2 + (k(i-1)^2*t(i-1)^2+t(i-1)^4)*ds^4/24)*B(i-1,:);
N(i,:) = (-k(i-1)*ds + (k(i-1)*t(i-1)^2+k(i-1)^3)*ds^3/6)*T(i-1,:)...
+ (1-(k(i-1)^2+t(i-1)^2)*ds^2/2+(k(i-1)^2+t(i-1)^2)^2*ds^4/24)*N(i-1,:)...
+ (t(i-1)*ds - (k(i-1)^2*t(i-1)+t(i-1)^3)*ds^3/6)*B(i-1,:);
同样，经过迭代之后，依然需要按照前面的方法将其恢复为单位正交的向量。
螺旋扩展法
对于一个固定非零的曲率与挠率的曲线而言，其在空间中的形态是螺旋线。我们假设固定非零的曲率与挠率的螺旋线其螺旋的半径为 \(R\)，轴线方向的节距是 \(H\)，旋转的变化率是 \(\Omega\) ，那么从螺旋线的起点到终点的 TNB 标架存在一定的变换关系，其几何意义如图所示。
假设一螺旋线绕 \(Z\) 轴延伸，其参数方程可写成
\[\mathbf{C}(s)=\begin{bmatrix}R\cos\Omega s\\R\sin\Omega s\\Hs\end{bmatrix}
\]
根据参数方程，我们可以得到曲线上任意位置的 TNB 标架的形式
\[\begin{align}
\mathbf{\hat{t}}(s)=\begin{bmatrix}-R\Omega\sin\Omega s\\R\Omega\cos\Omega s\\H\end{bmatrix},
\\ \mathbf{\hat{n}}(s)=\begin{bmatrix}-\cos\Omega s\\-\sin\Omega s\\0\end{bmatrix},
\\ \mathbf{\hat{b}}(s)=\begin{bmatrix}H\sin\Omega s\\-H\cos\Omega s\\R\Omega\end{bmatrix}.
\end{align}
\]
同样也可以得到曲率与挠率的计算方式
\[\kappa=R\Omega^2,\quad\tau=H\Omega
\]
反过来，我们可以通过曲率和挠率求出螺旋线的几何参数
\[\begin{gathered}
R=\frac{\kappa}{\kappa^{2}+\tau^{2}}, \\
H=\frac{\tau}{\sqrt{\kappa^{2}+\tau^{2}}}, \\
\Omega=\sqrt{\kappa^{2}+\tau^{2}},
\end{gathered}
\]
以及单位长度下 TNB 标架所绕轴线的旋转向量（公式的推导方式：可以把单位长度曲线展平到一个平面内，这样 H 就是一个直角三角形）
\[\mathbf{\hat{w}}=H\mathbf{\hat{t}}+R\Omega\mathbf{\hat{b}}.
\]
基于以上的内容，我们将曲线假设均匀分成了若干段的微小曲线段，且每个曲线段内部都是常曲率与挠率，那么对于每一个微小曲线段都可以使用螺旋线描述。那么同一曲线段的起点到终点的 TNB 标架变换可以描述为内部的旋转变换，即绕 \(\mathbf{\hat{w}}\) 旋转角度为 \(\gamma=\Omega\Delta s\) 的旋转运动。运用 Rodrigues 公式可得到绕轴的旋转运动可描述为
\[\begin{aligned}\mathbf{v}_{\mathrm{rot}}&=\cos\gamma\mathbf{v}+\sin\gamma(\mathbf{\hat{w}}\times\mathbf{v})+(1-\cos\gamma)(\mathbf{\hat{w}}\cdot\mathbf{v})\mathbf{\hat{w}}\\&=\mathrm{~Rv.}\end{aligned}
\]
转换成旋转矩阵的形式为
\[\mathbf{R}=\begin{bmatrix}X+w_1^2(1-X)&w_1w_2(1-X)-w_3Y&w_3w_1(1-X)+w_2Y\\w_1w_2(1-X)+w_3Y&X+w_2^2(1-X)&w_2w_3(1-X)-w_1Y\\w_3w_1(1-X)-w_2Y&w_2w_3(1-X)+w_1Y&X+w_3^2(1-X)\end{bmatrix}
\]
其中，\(X=\cos\gamma\) 以及 \(Y=\sin\gamma\)。
所以前后每一微小曲线段的 TNB 标架的转换可以写成
\[\mathbf{\hat{t}}_{k+1}=\mathbf{R}\mathbf{\hat{t}}_k,\mathbf{\hat{n}}_{k+1}=\mathbf{R}\mathbf{\hat{n}}_k,\mathbf{\hat{b}}_{k+1}=\mathbf{R}\mathbf{\hat{b}}_k
\]
另外，根据得到的 TNB 标架，我们还可以求得 TNB 标架坐标的位置变化关系
\[\mathbf{C}_{k+1}=\mathbf{C}_k+R\mathbf{\hat{n}}_k+H\Delta s\mathbf{\hat{w}}-R\mathbf{\hat{n}}_{k+1}
\]
根据位置迭代关系，我们可以求得曲线上所有点的坐标。
螺旋扩展方法的对应代码为.
w = H(i) * T(:,i) + R(i)*omega(i)*B(:,i);
gamma = omega(i) * ds;
X = cos(gamma);
Y = sin(gamma);
Rot = [X + w(1)^2*(1-X), w(1)*w(2)*(1-X)-w(3)*Y, w(3)*w(1)*(1-X)+w(2)*Y;
w(2)*w(1)*(1-X)+w(3)*Y, X + w(2)^2*(1-X), w(3)*w(2)*(1-X)-w(1)*Y;
w(3)*w(1)*(1-X)-w(2)*Y, w(3)*w(2)*(1-X)+w(1)*Y, X + w(3)^2*(1-X);];
T(:,i+1) = Rot * T(:,i);
B(:,i+1) = Rot * B(:,i);
N(:,i+1) = Rot * N(:,i);
helic_end = curve(i,:)' + R(i)*N(:,i)+ H(i) *ds*w - R(i)*N(:,i+1);
样条曲线插值获取对应步长的函数值
由于光栅的数量小于数值积分所需要的采样点的数量，可以通过样条曲线插值的方式获得两两光栅之间更密集的采样点，并将数值积分的步长固定在密集采样点之间的步长。
实际过程中，我们对900 mm 的光纤中的 31 个光栅位置的曲率与挠率进行了样条曲线重采样。
位置积分
通过上述方法得到所有重采样点上的 TNB 标架，可以沿曲线 \(s\) 在 \(\hat{\bf t}_{k}\) 方向进行积分，得到曲线点的位置
\[r_{k+1}=r_{k}+\frac{\Delta s}{2}\left(\hat{\bf t}_{k}+ {\hat{\bf t}_{k+1}}\right)
\]
最终效果
More Reading
Lim, Sungyeop, and Soonhung Han. 2017. “Helical Extension Method for Solving the Natural Equation of a Space Curve.” Surface Topography: Metrology and Properties 5 (3): 035002. https://doi.org/10.1088/2051-672X/aa7ea8.
Reference
