前言

在水面点对点导航规划中，通常用GPS和惯导进行导航，这里我主要是通过建立简化模型，分析惯导航偏航角，GPS动态精度对导航的影响，需要注意的是以下推导使用的位置信息只是单纯GPS信息，没有和惯导做数据的位置借解算融合。

1. 惯导偏航角误差对导航的影响

1.1 惯导航向误差

点对点路径规划中，在判断是否到达目标点时，通常做法会设定一个阈值，当进入以该阈值为半径的圆内，即可认为到达该目标点，我们要回答该阈值如何设定的问题，以下分析为当惯性导航航向存在误差时，对该阈值取值的影响，需要注意的是通常航行误差由两部分误差引起：

惯导器件本身的误差，主要是陀螺仪的飘移，像中等精度的光纤惯导航向飘移大约为0.05°/h0.05°/h0.05°/h。控制系统中的定航误差，由于定向过程中通常存在收敛或者震荡的过程，可假设为高斯噪声。

1.2 分析过程

详细分析如下：

设在东北（天）坐标系下，AUV需要从坐标(Xx,nE,Xy,nN)\left(X_{x, n}^{E}, X_{y, n}^{N}\right)(Xx,nE​,Xy,nN​)运动到(0,0)(0,0)(0,0)，惯导偏航角为θy\theta_{y}θy​，惯导偏航角误差为αy(t)\alpha_{y}(t)αy​(t)，误差为关于ttt的函数，方向逆时针为正方向，设速度ϑyAUV=1\vartheta_{y}^{A U V}=1ϑyAUV​=1，迭代时间间隔∇t=1\nabla \mathrm{t}=1∇t=1，在不考虑测流引起的速度ϑxAUV\vartheta_{x}^{A U V}ϑxAUV​的情况下，有以下公式：

[Xx,nEXy,nN]=[Xx,n−1EXy,n−1N]+[cos⁡(θy,n−1+αy(t))sin⁡(θy,n−1+αy(t))]

\left[\begin{array}{c}

{X_{x, n}^{E}} \\

{X_{y, n}^{N}}

\end{array}\right]=\left[\begin{array}{c}

{X_{x, n-1}^{E}} \\

{X_{y, n-1}^{N}}

\end{array}\right]+\left[\begin{array}{c}

{\cos \left(\theta_{y, n-1}+\alpha_{y}(t)\right)} \\

{\sin \left(\theta_{y, n-1}+\alpha_{y}(t)\right)}

\end{array}\right]

[Xx,nE​Xy,nN​​]=[Xx,n−1E​Xy,n−1N​​]+[cos(θy,n−1​+αy​(t))sin(θy,n−1​+αy​(t))​]

θy,n−1=arctan⁡(−y−x)=arctan⁡(yx)+π

\theta_{y, n-1}=\arctan \left(\frac{-y}{-x}\right)=\arctan \left(\frac{y}{x}\right)+\pi

θy,n−1​=arctan(−x−y​)=arctan(xy​)+π

以上公式可以转换为：

Xx,n2+Xy,n2=Xx,n−12+Xy,n−12+1+2(Xx,n−1⋅cos⁡(θy,n−1+αy(t))+Xy,n−1⋅sin⁡(θy,n−1+αy(t)))

\begin{aligned}

X_{x, n}^{2}+X_{y, n}^{2}=& X_{x, n-1}^{2}+X_{y, n-1}^{2}+1+2\left(X_{x, n-1} \cdot \cos \left(\theta_{y, n-1}+\alpha_{y}(t)\right)+X_{y, n-1}\right.\\

&\left.\cdot \sin \left(\theta_{y, n-1}+\alpha_{y}(t)\right)\right)

\end{aligned}

Xx,n2​+Xy,n2​=​Xx,n−12​+Xy,n−12​+1+2(Xx,n−1​⋅cos(θy,n−1​+αy​(t))+Xy,n−1​⋅sin(θy,n−1​+αy​(t)))​

优化的目标函数为：

argmin⁡Xx,n,xy,n(Xx,n2+Xy,n2)

\operatorname{argmin}_{X_{x, n},} x_{y, n}\left(X_{x, n}^{2}+X_{y, n}^{2}\right)

argminXx,n​,​xy,n​(Xx,n2​+Xy,n2​)

因为我们要求得在惯导存在航向误差时，点对点之间导航时阈值的选取，即阈值的最小值，所以可以转化为求最小值的问题。

上式可以看作非线性优化问题，求得其解析解还是比较麻烦的一件事，下面从另外一种思路来分析。

如下图所示：

即 m=f(θ)\mathrm{m}=\mathrm{f}(\theta)m=f(θ)，根据余弦定理可得：

f(θ)=a2+b2−2abcos⁡(θ)

\mathrm{f}(\theta)=a^{2}+b^{2}-2 a b \cos (\theta)

f(θ)=a2+b2−2abcos(θ)

因为∣b∣<0|b|<0∣b∣<0，所以b2≈0b^{2} \approx 0b2≈0，对θ\thetaθ在不同取值范围时的mmm值分析如下：

cos⁡(θ)>0\cos (\theta)>0cos(θ)>0，并且f(θ)a2\mathrm{f}(\theta)>a^{2}f(θ)>a2，可以收敛，即 m>0m>0m>0。

1.3 仿真结果分析

图1.2仿真了不同偏航角误差对导航轨迹的影响。不同的导航轨迹中偏差角误差为5°-85°，间隔为5°，可以看到偏航角误差只是影响收敛的快慢，理论上在小于90°时都是收敛的，和上面的理论分析是一致的。

对收敛速度的影响如图1.3所示，可以看到当偏航角误差越大时，收敛速度也越慢，和上述的理论分析一致。

2. GPS动态精度对导航的影响

2.1 分析过程

分析过程和上面类似，所以不再采用建立解析方程的方法，而是在1中的仿真程序中做些修改，从而得出结论。

2.2 分析过程

首先设置GPS的动态偏差为定常数，当设置为10m10 m10m时，仿真结果如图1.4所示。

从图1.4中可以看到，距离目标点的收敛点最大不超过10m,即不超过GPS的动态精度。图1.5中显示的是不同GPS动态精度对导航的影响，GPS动态精度的范围为2m-20m，变化间隔为2m，可以看出不用GPS动态精度不影响最终收敛的时间，只影响最终的收敛点，最后的收敛点即为GPS动态精度。

实际情况下GPS的动态精度可以看作是满足高斯噪声的，这点在实际数据的分析中可以得到。当高斯噪声的均值为10m，标准差为4m时，仿真结果如图1.6所示。

为了保证系统的稳定性，在实际取值时可以参考高斯分布中的原则，当设置为时，在第一次检测到进入目标点阈值圈中时，有99.7699.76%99.76的概率程序检测成功，当设置为2时，有95.4495.44%95.44的概率，并且在实际中，即使第一次检测不成功也没有关系，程序会不断检测，直至收敛。

这里我们设在时间T=N⋅∇t\mathbf{T}=\mathbf{N} \cdot \nabla \mathbf{t}T=N⋅∇t内，一次检测成功的概率为P，则在时间内收敛（检测到达目标点）的概率为如下公式：

PS=1−(1−P)N

P_{S}=1-(1-P)^{N}

PS​=1−(1−P)N

所以，该概率和时间呈指数关系，想在短时间内收敛，阈值可以设置的很小，可以根据实际情况灵活取值。

2.3 GPS位置动态精度分析

GPS经纬度动态精度的分析图如图1.7所示，包含50000条数据，GPS读取速率为1Hz1Hz1Hz，所以经过的时间为13.8小时，历经的时间较长，数据的可信度较高。

从图中可以看到，GPS经纬度动态精度可以大致看作高斯分布，从工程角度出发，基于 [公式] 原则可以计算出误差均值为0.9m0.9m0.9m,标准差约为2.0m2.0m2.0m，因为以上计算的是动态精度，假设AUV实际的运动速度约为1m/s，所以可取以下数值：

mean gps≥2.0m

\text { mean }_{g p s} \geq 2.0 m

mean gps​≥2.0m

var⁡gps≥2.0m

\operatorname{var}_{g p s} \geq 2.0 \mathrm{m}

vargps​≥2.0m

从结果可以看出，该GPS的动态位置精度还是比较高的，根据相关理论分析，GPS的静态位置精度高于动态位置精度。