标签：仿真 tau mathbf frac 算法 FBG Delta kappa hat

算法涉及基本的 [[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 方程组是一个典型的常微分方程组，由于实际情况中没有函数的解析形式，所以可以采用数值积分的方法对方程组进行求解。关于数值积分方法可以参考^[1]。欧拉法是最基本的数值积分求解微分方程的方法，但是一阶欧拉积分的精度太低，不能满足要求，实际情况中很少使用。龙格-库塔法是一种非线性常微分方程的数值解法。由数学家卡尔·龙格和马丁·威尔海姆·库塔于1900年左右发明。 Runge-Kutta 公式的思路就是利用区间内一些特殊点的一阶导数值的线性组合来替代某点处的 n 阶导数值,这样就可以仅通过一系列一阶导数值来得到某点幂级数展开的预测效果。龙格－库塔法（Runge-Kutta methods）是用于非线性常微分方程的解的重要的一类隐式或显式迭代法。在工程中最常用的是四阶龙格－库塔积分，也就是 RK4 积分。
我们先从欧拉方法考虑，可以将 [[Frenet–Serret 方程组]]改写成离散的差分形式 $$
\begin{array}{l}{{\hat{\mathbf{t}}{k+1}=\hat{\mathbf{t}}+\kappa_{k}\Delta s\hat{\mathbf{n}}{k},}}\ {{\hat{\mathbf{n}}=\mathbf{\hat{n}}{k}-\kappa\Delta s\hat{\mathbf{t}}{k}+\tau\Delta s\hat{\mathbf{b}}{k},}}\ {{\hat{\mathbf{b}}=\mathbf{\hat{b}}{k}-\tau\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}}+\frac{\Delta{s}}{6}(a_{t1}+2a_{t2}+2a_{t3}+a_{t4}), \
\hat{\mathbf{n}}{k+1} &=&\hat{\bf{n}}+\frac{\Delta{s}}{6}(a_{n1}+2a_{n2}+2a_{n3}+a_{n4}), \
\hat{\mathbf{b}}{k+1} &=&\hat{\bf{b}}+\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}+\tau^{2})\Delta s^{4}}{4}\right]\hat{\bf t}

\left[\kappa\Delta s-\frac{\left(\kappa^{{3}-\kappa\tau}\right)\Delta s^{3}}{6}\right]\hat{\bf n}_
\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} + \left[1,-,\frac{\left(\kappa^{2}+\tau\right)\Delta s^{{2}}{2},+,\frac{\left(\kappa}+\tau^{2}\right)\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} + \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}\tau^{2}+\tau4\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,:);

同样，经过迭代之后，依然需要按照前面的方法将其恢复为单位正交的向量。

样条曲线插值获取对应步长的函数值

由于光栅的数量小于数值积分所需要的采样点的数量，可以通过样条曲线插值的方式获得两两光栅之间更密集的采样点，并将数值积分的步长固定在密集采样点之间的步长。
实际过程中，我们对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)$$

最终效果

![[FBG重建算法仿真_龙格库塔法.png]]

Reference

Butcher, John Charles. Numerical Methods for Ordinary Differential Equations. John Wiley & Sons, 2016. ↩︎

标签：仿真,tau,mathbf,frac,算法,FBG,Delta,kappa,hat
From： https://www.cnblogs.com/pomolnc/p/18164211

基于FBG传感器的形状重建算法仿真