2024年美赛B题算法解析

惯性定位系统确定潜水器的位置解算

基础惯性定位方法推导

原始数据来源：

陀螺仪和加速度计能够收集的数据(最好能弄一个表)：

陀螺仪:${\omega }_x,{\omega }_y,{\omega }_z$,分别是潜水器以动系x,y,z轴的角速度大小

加速度计：$a_x,a_y,a_z$,分别是潜水器沿动系$x,y,z$轴的加速度大小

原始数据定义

在一下表示中，前缀为$E$的是在定系下的坐标信息，前缀为$G$的是在动系下的坐标信息，设在某一时刻$t$的位置信息$ELocatio{n}_t$=$[{\xi }_t,{\eta }_t,{\zeta }_t]$，${\xi }_t,{\eta }_t,{\zeta }_t$分别为潜水器(动系原点)在定系下的三维坐标，某一时刻的姿态信息$Postur{e}_t=[{\phi }_t,{\theta }_t,{\psi }_t]$,其中${\phi }_t,{\theta }_t,{\psi }_t$分别是动系对于定系的三个姿态角，某一时刻$t$的速度信息是$G{V}_t=[v_{tx},v_{ty},v_{tz}{]}^T$,其中$v_{tx},v_{ty},v_{tz}$分别表示在$t$时刻潜水器在动系下的速度信息

初始化：将潜水器放入海里之前，其动系原点在定系下的坐标为$[0,0,0]$，姿态角也是$[0,0,0]$

计算过程:

如果定义陀螺仪与加速度计采集信息的时间间隔的最小公倍数是$\Delta t$，那么在$t+\Delta t$时刻潜水器相对于$t$时刻的位置坐标为
$$GLocatio{n}_{t+\Delta t}=[{\int }_t^{t+\Delta t}(v_{tx}+{\int }_t^{t+\Delta t}{a}_{tx}dt)dt,{\int }_t^{t+\Delta t}(v_{ty}+{\int }_t^{t+\Delta t}{a}_{ty}dt)dt,{\int }_t^{t+\Delta t}(v_{tz}+{\int }_t^{t+\Delta t}{a}_{tz}dt)dt]$$

其中$v_{tx},v_{ty},v_{tz}$分别为$t$时刻潜水器沿$x,y,z$轴的速度，根据假设3，可以认为潜水器在$\Delta t$内的变化是均匀的，即$GLocation$公式可简化为:
$$GLocatio{n}_{t+\Delta t}=[(v_{tx}+a_{tx}\Delta t)\Delta t,(v_{ty}+a_{ty}\Delta t)\Delta t,(v_{tz}+a_{tz}\Delta t)\Delta t]$$
在$t+\Delta t$时刻的姿态信息
$$\begin{gathered} Postur{e}_{t+\Delta t}=[{\phi }_t+{\omega }_{tx}\Delta t,{\theta }_t+{\omega }_{ty}\Delta t,{\psi }_t++{\omega }_{tz}\Delta t] \\ =\left[\begin{array}{ccc}1& \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0& \cos \phi & -\sin \phi \\ 0& \sin \phi /\cos \theta & \cos \phi /\cos \theta \end{array}\right]\times [{\phi }_t,{\theta }_t,{\psi }_t{]}^T \end{gathered}$$
即${\phi }_{t+\Delta t}={\phi }_t+{\omega }_{tx}\Delta t,{\theta }_{x+\Delta t}={\theta }_t+{\omega }_{ty}\Delta t,{\psi }_{t+\Delta t}={\psi }_t++{\omega }_{tz}\Delta t$

定义变换矩阵S1：
$$S_1=\left[\begin{array}{ccc}1& \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0& \cos \phi & -\sin \phi \\ 0& \sin \phi /\cos \theta & \cos \phi /\cos \theta \end{array}\right]$$
则有：$Postur{e}_{t+\Delta t}=S_1\times Postur{e}_t$
$$S_2=\left[\begin{array}{ccc}\cos \psi \cos \theta & \cos \psi \sin \theta \sin \phi -\sin \psi \cos \phi & \cos \psi \sin \theta \cos \phi +\sin psi\sin \phi \\ \sin \psi \cos \theta & \sin psi\sin \theta \sin \phi +\cos \psi \cos \phi & \sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi \\ -\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi \end{array}\right]$$
根据转换矩阵$S_2$可以得到$ELocatio{n}_{t+\Delta t}=S_2\cdot GLocatio{n}_{t+\Delta t}^T+GLocatio{n}_t$

由潜艇姿态角、旋转角速度的关系和两坐标系变换矩阵易得辅助方程：
$$\left[\begin{array}{c}\phi \\ \theta \\ \psi \\ \xi \\ \eta \\ \zeta \end{array}\right]=\left[\begin{array}{cc}S{1}_{3\times 3}& 0\\ 0& S{2}_{3\times 3}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\\ u\\ v\\ w\end{array}\right]$$

迭代过程：此时将$t+\Delta t$时刻的位置信息和姿态信息更新为新的动系坐标系啊，即动系原点在定系下的坐标更新为$GPoint=ELocatio{n}_{t+\Delta t}$，在动系下的姿态角更新为$[0,0,0]$.

以深度信息作为修正的惯性定位算法

原始数据获得：压强$P=\rho gh+{\delta }_h$,此时潜水器的位置坐标为$[\xi ,\eta ,\zeta ]$.

获得h：
$$h=\frac{P-{\delta }_h}{\rho g}$$
获得$\zeta$方向上的差值：${\delta }_{\zeta }=\zeta -h$,以及三个方向上的方向角：
$$\tan \alpha =\frac{\xi }{\zeta }$$

$$\tan \beta =\frac{\eta }{\zeta }$$

就可以获得另外两个方向上的修正值：
$${\delta }_{\eta }={\delta }_{\zeta }\cdot \tan \beta =\frac{\zeta \rho g-{\delta }_h}{\rho g}\cdot \frac{\eta }{\zeta }$$

$${\delta }_{\xi }={\delta }_{\zeta }\cdot \tan \alpha =\frac{\zeta \rho g-{\delta }_h}{\rho g}\cdot \frac{\xi }{\zeta }$$

从而获得通过深度信息修正后的坐标值$[{\xi }^{\prime },{\eta }^{\prime },{\zeta }^{\prime }]=[\xi +{\delta }_{\xi },\eta +\delta \eta ,\zeta +\delta \zeta ]$

卡尔曼滤波之后的惯性定位系统

为什么要用卡尔曼滤波？——误差

1.有误差

• 不存在完美的数学模型

• 系统的扰动不可控，也很难建模——系统噪声

• 测量传感器存在误差——测量噪声

  这三种误差对于实际系统的建模和监测都是常见的，卡尔曼滤波器在处理这些误差方面有一定优势：

  1.不存在完美的数学模型： 即使我们尽力建立一个精确的数学模型来描述系统，现实世界中的系统也会受到各种未知因素的影响，这些因素往往难以用数学模型准确描述。卡尔曼滤波器能够利用系统的动态模型和实际测量值，不断更新对系统状态的估计，从而在一定程度上弥补了数学模型的不完美性。(模型总是经过一定的假设和简化，并不能完全和问题相匹配)
  2.系统的扰动不可控，也很难建模——系统噪声： 系统在运行过程中可能会受到各种外部扰动的影响，如环境变化、机械振动等，这些扰动被称为系统噪声。系统噪声往往具有随机性和不可预测性，难以用简单的数学模型精确描述。卡尔曼滤波器通过不断更新状态估计和协方差矩阵，能够有效地抵消系统噪声对状态估计的影响，从而提高了状态估计的准确性和稳定性。（往往是通过模型的简化导致模型会出现一些误差，比如在原始建模之中将积分计算转换为直接相乘导致产生误差等等）
  3.测量传感器存在误差——测量噪声： 在实际测量过程中，由于传感器本身的精度限制、环境干扰等因素，测量值往往会存在误差，这些误差被称为测量噪声。卡尔曼滤波器能够将实际测量值与系统状态之间的关系建立在一个统计模型中，并通过卡尔曼增益的调整，有效地过滤掉测量噪声，提取出真实的系统状态信息。(传感器测量的值只是一个观测值，并不能十分准确的反应真实情况，存在很多扰动与噪声)

  综上所述，卡尔曼滤波器在面对存在不完美数学模型、系统噪声和测量噪声的情况下，能够通过动态地结合系统动态模型和实际测量值，提供对系统状态的最优估计。因此，在实际工程和科学应用中，卡尔曼滤波器被广泛应用于目标跟踪、导航定位、信号处理等领域，以提高系统的性能和鲁棒性。

2.卡尔曼滤波器是一种递归滤波器，用于估计动态系统中的状态，特别是在存在测量噪声和过程噪声的情况下。以下是一些卡尔曼滤波器的优点和应用场景：

1.融合传感器数据： 卡尔曼滤波器可以有效地融合来自不同传感器的信息，例如加速度计、陀螺仪和磁力计。通过融合多个传感器的数据，可以提高状态估计的准确性和稳定性。
2.估计动态系统状态： 卡尔曼滤波器在动态系统中估计状态时表现出色。它可以处理线性和高斯性噪声的情况，并且在这些条件下提供最优的状态估计。
3.适应性： 卡尔曼滤波器具有适应性，能够动态地调整滤波参数以适应系统和环境的变化。这种适应性使得它在实时应用中非常有用，特别是在环境条件变化或系统动态性较大的情况下。
4.最小均方误差估计： 卡尔曼滤波器以最小均方误差为目标，通过优化估计值和实际测量值之间的差异，提供了对系统状态的最优估计。
5.实时应用： 由于其递归性质，卡尔曼滤波器非常适合实时应用，可以在每个新的测量值到来时迭代地更新状态估计。
6.广泛应用： 卡尔曼滤波器在诸如导航、控制系统、机器人、信号处理等领域得到广泛应用。它在处理具有线性动态和高斯噪声的系统方面表现得尤为出色。

卡尔曼的使用(这里将$\Delta t$简化为1)：

需要的东西：状态向量

状态向量:$x_t=[{\xi }_t,{\eta }_t,{\zeta }_t,{\phi }_t,{\theta }_t,{\psi }_t]$，其中前三个状态为位置状态$ELocatio{n}_t=[{\xi }_t,{\eta }_t,{\zeta }_t]$，后三个状态是姿态向量$Postur{e}_t=[{\phi }_t,{\theta }_t,{\psi }_t]$，其各自的转换关系为:$Postur{e}_{t+\Delta t}=S_1\times Postur{e}_t$,$ELocatio{n}_{t+1}=S_2\cdot GLocatio{n}_{t+1}^T+GLocatio{n}_t$,

状态变量转换方程为$x_t^T=T_{6\times 6}{x}_{t-1}$,根据以上式(12)可得:
$$T=\left[\begin{array}{cc}S{1}_{3\times 3}& 0\\ 0& S{2}_{3\times 3}\end{array}\right]$$
在卡尔曼滤波中对于状态改变离散化之后的结果:
$$\begin{gathered} x_{[k]}=A\times x_{[k-1]}+B\times u_{[k-1]}+{\omega }_{[k-1]} \\ {z}_{[k]}=H\times x_{[k]} \end{gathered}$$
其中，各变量表征的意义为：
$x_k $系统状态变量
$z_k$ 状态阵的观测向量
$A$ 状态矩阵
$B$ 输入矩阵
$H$ 状态观测矩阵
$w_k-1$ 过程噪声
$v_k$ 测量噪声

这个模型引入了两个噪声项和一个量测方程。通常，这两个误差项被视为正态分布。卡尔曼滤波的主要思想是：我们可以通过这两个矩阵分别对系统状态进行“估计”和“量测”，然后再对两个结果进行“融合”，实现对系统状态的准确估计。

设目标的真实位置为 $z$，陀螺仪测量得到的位置为 $z_1 $，加速度计测量得到的位置为$ z_2 $。这两个传感器都受到一些噪声的影响，不失一般性的，让两个测量信号的误差都是正态分布，即$e_1\sim \mathcal{N}(0, \sigma_1^2)$,$e_2\sim \mathcal{N}(0, \sigma_2^{2)$,我们希望通过组合两个测量值来得到真实值的最优估计：$\hat{z}=z_1+K(z_2-z_1)$,为最小化方差对$K$求导并使倒数为0得到$K$的最优值为$K=\dfrac{\sigma_1}2}{\sigma_1^2+\sigma_22}$ ,

则使用卡尔曼滤波的的过程可总结为如下过程：

• 初始化状态估计 ${\hat{x}}_0$ 和协方差矩阵 $P_0$ 。

• 预测步骤（系统模型）：

• 1.使用线性系统模型进行状态预测:${\hat{x}}_{k+1}^{-}=f({\hat{x}}_k,u_k)$

• 2.计算雅可比矩阵$\mathbf{F}=\frac{\partial f}{\partial x}$

• 3.预测协方差：${\mathbf{P}}_{k+1}^{-}=\mathbf{F}{\mathbf{P}}_k{\mathbf{F}}^T+\mathbf{Q}$

• 更新步骤（测量模型）：

• 1.使用非线性测量模型进行状态更新： ${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+{\mathbf{K}}_{k+1}(z_{k+1}-{\hat{x}}_{k+1}^{-})$

• 2.计算雅可比矩阵$\mathbf{H}=\frac{\partial h}{\partial x}$

• 3.计算卡尔曼增益：${\mathbf{K}}_{k+1}={\mathbf{P}}_{k+1}^{-}{\mathbf{H}}^T(\mathbf{H}{\mathbf{P}}_{k+1}^{-}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$

• 4.更新协方差：$\mathbf{P}{k+1}=(\mathbf{I}-\mathbf{K}{k+1}\mathbf{H})\mathbf{P}_{k+1}^- $

  这里最好可以根据过程有一个流程图

  然后在利用2.2 惯性定位系统得到的状态转移方程，通过设定既定的航线(螺旋线，在PPT中有说明),然后就是在三个方向上面分别加上正态分布的误差，在每一步状态转移的过程中通过卡尔曼滤波来进行优化求解，

  接下来进行效果演示

。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。

基于TOPSIS综合评价模型的搜救设备选择模型

$$Score(i)=S(i)\times \omega (S)+C(i)\times \omega (C)+M(i)\times \omega (M)+U(i)\times \omega (U)+St(i)\times \omega (St)$$

$$Pla{n}_i=Communication(i)+Locate(i)+Navigate(i)+Pse(i)$$

$$Security(Pla{n}_i)=Security(C_i)+Security(L_i)+Security(N_i)+Security(Ps{e}_i)$$

基于动力学模型与聚类算法的搜救策略模型

Model Overview

首先面对

基于动力学模型的潜水器运动模型建立

可以将载人潜水器看作一个刚体，对其进行受力分析，采用刚体运动动量定理和动量矩定理，根据Junyuan Xie[谢俊元. 深海载人潜水器动力学建模及操作仿真器研制 [D] . 无锡；江南大学,2009]的方法来进行潜水器空间六自由度方程。其中外力和外力矩包括螺旋桨推力、水动力、重力和服力及力矩等，而环境引起的干扰可有作业环境具体分析。

潜艇做空间运动时的水动力包括艇体水动力、舵水动力和螺旋桨水动力。

这里把螺旋桨水动力分离出来,暂不考虑,并做如下假设:

• 除艇体和舵水动力外‚无其它水动力。

• 在研究的时段中‚流场特性保持相对不变。

  引入统一表达水动力和力矩的广义向量$G=[X,Y,Z,K,M,N]$对于给定流场无限深、广、静水中的一定的潜水器来说,流体动力取决于潜水器的运动情况,即有：$G=G(U,\dot{U},\Omega ,\dot{\Omega },R)$,设水动力各分力函数在展开点附近光滑‚并根据势流理论假设水动力的分量与角加速度线性相关,同时加速度与速度参数之间的祸合系数甚小,忽略不计。于是得到经过简化的水动力泰勒展开式如下:
  $$G=G_0+({\frac{\partial }{\partial {\dot{U}}^T}|}_0{\Delta \dot{U}+\frac{\partial }{\partial {\dot{\Omega }}^T}|}_0\Delta \dot{\Omega })G+\sum _{k=1}^{\infty }\frac{1}{k!}\{\left({\frac{\partial }{\partial U^T}|}_0{\Delta U+\frac{\partial }{\partial {\Omega }^T}|}_0{\Delta \Omega +\frac{\partial }{\partial R^T}|}_0{\Delta R)}^{k}G\right\}$$
  保留泰勒级数至三阶项己足够满足工程要求‚但根据习惯以及计算上的方便性,使用最多的是二阶展开式。选择潜艇等速直航的平衡状态作为工作点,即:$U_0={\left(\begin{array}{ccc}U& 0& 0\end{array}\right)}^T{\Omega }_0={\dot{\Omega }}_0=R_0={\dot{U}}_0=0$
  $$

  $$
  其中纵向、横向、转首模型如下：

$$\begin{array}{rl}\sum _{\mathbf{i}}{X}_i=& \begin{array}{r}\frac{1}{2}\rho L^4[X_{qq}^{\prime }{q}^2+X_{rr}^{\prime }{r}^2+X_{pr}^{\prime }pr]+\frac{1}{2}\rho L^3{X}_{\dot{u}}^{\prime }\dot{u}+\end{array}\frac{1}{2}\rho L^3[X_{\dot{u}}^{\prime }\dot{u}+X_{vr}^{\prime }or+X_{wq}^{\prime }wq]+\frac{1}{2}\rho L^2[X_{uu}^{\prime }{\mu }^2+\\ & X_{vv}^{\prime }{v}^2+X_{wu}^{\prime }{w}^2+X_{uw}^{\prime }uw]-(W-B)\sin \theta +X_T\text{ ,}\end{array}$$

$$\begin{array}{rl}\sum _{\mathbf{i}}{Y}_i=& \frac{1}{2}\rho L^4[Y_{\dot{r}}^{\prime }\dot{r}+Y_{\dot{p}}^{\prime }\dot{p}+Y_{r|r}^{\prime }|r|+Y_{p|p}^{\prime }p|p|+Y_{pq}^{\prime }pq+Y_{qr}^{\prime }qr]+\frac{1}{2}\rho L^3[Y_{\dot{v}}^{\prime }\dot{v}+Y_p^{\prime }up+Y_r^{\prime }ur+Y_{vq}^{\prime }pq+\\ & Y_{wp}^{\prime }wp+Y_{wr}^{\prime }wr]+\frac{1}{2}\rho L^3[Y_{v|r}^{\prime }\frac{v}{|v|}|(v^2+w^2{)}^{1/2}|r|+Y_{\mathbf{vwi\vartheta }w}^{\prime }]+\frac{1}{2}\rho L^2[Y_0^{\prime }{u}^2+Y_v^{\prime }uv+Y_{vu}^{\prime }vw]+\\ & \frac{1}{2}\rho L^2{Y}_{v|v}^{\prime }v\left|(v^2+w^2{)}^{\frac{1}{2}}\right|+(W-B)\cos \theta \sin \varphi +Y_T,\end{array}$$

$$\begin{gathered} \begin{array}{rl}\sum _{\mathbf{i}}{N}_i=& \frac{1}{2}\rho L^5\left[N_r^{\prime }\dot{r}+N_p^{\prime }\dot{p}+N_{\text{pq}}{}^{\prime }pq+N_{qr}^{\prime }qr+N_{r|r}^{\prime }|r|+N_{p|p|}^{\prime }p|p|\right]+\frac{1}{2}\rho L^4[N_v^{\prime }\dot{v}+N_{wr}^{\prime }wr+N_{wp}^{\prime }wp+N_{vq}^{\prime }vq+\\ & N_{vww}^{\prime }{w}^2+N_r^{\prime }ur+N_p^{\prime }up]+\frac{1}{2}\rho L^4{N}_{|v|r}^{\prime }\left|{\left(v^2+w^2\right)}^{1/2}r+\right|\frac{1}{2}\rho L^3\left[N_0^{\prime }{u}^2+N_v^{\prime }uv+N_{vw}^{\prime }vw\right]+ \\ \frac{1}{2}\rho L^3{N}_{v|v|}^{\prime }v|(v^2+{w^2)}^{1/2}+(x_{G}W-x_{C}B)\cos \theta \sin \varphi +N_T\end{array} \end{gathered}$$

经由以上动力学模型的建立可以推算在。。。。。。。。。。。。。。。。。。。。。。

基于聚类算法的潜水器初始搜救点设置

$$P=\frac{S_{Searched}}{S_{total}}=\frac{3\times \pi \times r^2}{S_1+S_2+S_3-S_{12}-S_{13}-S_{23}}=\frac{3\times \pi \times (at{)}^2}{S_1+S_2+S_3-S_{12}-S_{13}-S_{23}}$$