CN119437247A 用于海洋工程的水下机器人定位方法及系统 （中国石油大学(华东)）

用于海洋工程的水下机器人定位方法及系统对水下机器人获取当前任务及历史任务的位置与当前任务的线速度变化状态相似的时间点所器人由于运动状态变化导致定位及预测路径定2对水下机器人获取当前任务的位置参数及速度参数，以及若干历史分析当前任务与历史任务同方向坐标之间的距离，确定参考各时间点前历史任务与当前任务的线速度变化状态相似的时间点所对应的偏移将第个历史任务作为目标历史任务；目标历史任务的x方向坐标构成的时间序列为列为[ro1xoa,…,xojl，当前任务截止当前时刻共包含j个时间点；当前任务第j个时间点与目标历史任务第i个时间点在x方向上的偏移量的依据当前任务与目标历史任务各时间点之间的偏移量，构对当前任务第一个时间点及目标历史任务第一个时间点的偏移量分对所述偏移和矩阵中除首行元素及首列元素外其他元素，依据其已确对当前任务与目标历史任务构建jxi的偏移量矩阵；所述偏移量矩阵中同行元素为当3前任务相同的时间点与目标历史任务不同的时间点在x方向上的偏移量，所述偏移量矩阵基于当前任务与目标历史任务在x方向上的偏移量矩阵，构建当前任务与目标历史任务在x方向上的偏移和矩阵，所述偏移和矩阵中第一行第i个元素Q(rosxe)的计算方法i表示同一行中第i个元素的序数值；对于所述偏移和矩阵中第一列任意一个元素，将该元素及该元素在将第二行第二列元素作为当前元素；其相邻元素分别为第一行第一列元素Q(xo1,xk)Q(xo1,xk)进行线速度相似关系量化；预设一个时间窗口，当前元素在当前任务对应第二个时间点在x方向上的线速度，获取当前元素分别在当前任务及目标历史任务的线速度分其中，az(x,o)表示当前元素与第一行第一列元素在当前任务的线速度变化相似性，表示第二行第二列元素与第一行第一列元素分析下在当前任务的线速度分析段中所有线速度的均值，表示第一行第一列元素在当前任务的线速度分析段中所有线速度的均值，N(x,o)表示第一行第一列元素在当前任务的线速度分析段中线速度的数量，表示第二行第二列元素与第一行第一列元素分析下在当前任务的线速度分析段中获取当前元素与第一行第一列元素在目标历史任务的与第一行第一列元素分别在当前任务的线速度变化相似性及目标历史任务的线速度变化4第一行第一列元素对于当前元素的偏移参考量Q'(xoz-1,xez-1)的计算方法为：其中，az-1z-1(x)表示第一行第一列元素对于当前元素的线速度置信度，Q(xo1,xk)分别获取第二行第一列元素及第一行第二列元素对于当前元素的对所述偏移和矩阵中所有元素进行线性归一化，得到的结果作为每个元素的偏移率，标历史任务第u-1个时间点的x方向坐标，i表示目标历史任务的时间点数量，表示绝对对目标历史任务每个方向上的位置变化程度进行权重归一化，得到的实现如权利要求1_9任意一项所述用于海洋工程的水下5[0003]卡尔曼滤波算法中的状态运动方程是基于一定的运动假设来预测下一时刻的状人由于运动状态变化导致定位及预测路径定位不准确的问题，所采用的技术方案具体如参考各时间点前历史任务与当前任务的线速度变化状态相似的时间点所对应的为[e11xe,z…,xeil，目标历史任务中共包含i个时间点；当前任务的x方向坐标构成的时间序列为[ro1xoa,…,xojl，当前任务截止当前时刻共包含j个时间点；当前任务第j个时间点与目标历史任务第i个时间点在x方向上的偏移量6的计算方法为：其中，Xo,j表示当前任务第j个时间点的x方向坐标，Xk,i表示目标历史任务第i个对当前任务第一个时间点及目标历史任务第一个时间点的偏移量分别进行累加，对当前任务与目标历史任务构建jxi的偏移量矩阵；所述偏移量矩阵中同行元素为当前任务相同的时间点与目标历史任务不同的时间点在x方向上的偏移量，所述偏移量矩阵中同列元素为当前任务不同的时间点与目标历史任务相同的时间点在x方向上的偏移基于当前任务与目标历史任务在x方向上的偏移量矩阵，构建当前任务与目标历其中，d(xos,xr)表示当前任务第一个时间点与目标历史任务第u个时间点在x将第二行第二列元素作为当前元素；其相邻元素分别为第一行第一列元素7Q(xo1,xk)、第一行第二列元素Q(xo1,xx2)及第二行第一列元素Q(xo2,xk)，以当前元素与Q(xo1,xk)进行线速度相似关系量化；预设一个时间窗口，当前元素在当前任务对应第二个时间点在x方向上的线速度，获取当前元素分别在当前任务及目标历史任务的线速度性，表示第二行第二列元素与第一行第一列元素分析下在当前任务的线速度分析段中所有线速度的均值，表示第一行第一列元素在当前任务的线速度分析段中所有线表示第二行第二列元素与第一行第一列元素分析下在当前任务的线速度分析段中元素与第一行第一列元素分别在当前任务的线速度变化相似性及目标历史任务的线速度第一行第一列元素对于当前元素的偏移参考量Q'(xoz-1,xez-1)的计算方法为：Q(xo1,xk)表示所述偏移和矩阵中第一行第一列元素的偏移和；8示目标历史任务第u-1个时间点的x方向坐标，i表示目标历史任务的时间点数量，表示[0014]本发明的有益效果是：本发明基于DTW匹配思想对当前任务与历史任务进行同方相对偏移程度进行整体分析来获取整体偏移程度；通过整体偏移程度筛选相似历史任务，[0016]图1为本发明一个实施例所提供的用于海洋工程的水下机器人定位方法流程示意[0018]请参阅图1，其示出了本发明一个实施例所提供的用于海洋工程的水下机器人定9[0019]本实施例的目的是对水下机器人通过历史任务与当前任先需要通过惯性导航单元测量水下机器人的运动状态，同时实时记录水下机器人的位置，及z三个方向的线速度，本实施例通过惯性导航单元获取速度参数及位置参数的采样时间本实施例共获取水下机器人最近50次历史任务的位置参数器人从起始点到当前位置的移动轨迹，这其中涉及到机器人为完成任务所进行的各种操反映当前任务与历史任务的位置偏移量，避免直接进行整体位置偏移分析过程中的复杂标历史任务的x方向坐标构成的时间序列为[e11xe,z…,xeil，目标历史任务中共包含i个时间点；当前任务的x方向坐标构成的时间序列为[ro1xoa,…,xojl，当前任务截止当前时刻共包含j个时间点；则当前任务第j个时间点与目标历史任务第i个时间点在x方向上的其中，Xo,j表示当前任务第j个时间点的x方向坐标，Xk,i表示目标历史任务第i个对当前任务第一个时间点及目标历史任务第一个时间点的偏移量分别进行累加，前任务截止当前时刻共包含j个时间点，则对当前任务与目标历史任务构建jxi的偏移量在x方向上的偏移量，所述偏移量矩阵中同列元素为当前任务不同的时间点与目标历史任基于当前任务与目标历史任务在x方向上的偏移量矩阵，构建当前任务与目标历其中，d(xos,xr)表示当前任务第一个时间点与目标历史任务第u个时间点在x素为例，将第二行第二列元素作为当前元素；其相邻元素分别为第一行第一列元素线速度分析段，同时确保分析的相邻元素对应的线速度在线速度分析段中的次序值相等，例如第一行第一列元素在当前任务的线速度分析段长度为3，且待分析元素对应的线速度分析，获取当前元素与第一行第一列元素在当务的线速度分析段中所有线速度的均值，N(x,o)表示第一行第一列元素在当前任务的线速度分析段中线速度的数量，表示第二行第二列元素与第一行第一列元相似分析，获取当前元素与第一行第一列元素在目标历史任务的线速度变化相似性线速度分析段中所有线速度的均值，表示第一行第一列元素在目标历史任务的线速度分析段中所有线速度的均值，N(x,k)表示第一行第一列元素在目标历史任务的线速度度置信度；分别获取第二行第一列元素及第一行第二列元素对于当前元素的线速度置信一列元素对于当前元素的偏移参考量Q(xoz-1,xkz-)的计算方法为：其中，(z-12-1(x)表示第一行第一列元素对线速度Q(ro.xe1)表示所述偏移和矩阵中第一行第一列元素的偏移和。与所述偏移和矩阵的元素总数量的比值，作为当前任务与目标历史任务在x方向上的相对历史任务的整体偏移程度小于偏移阈值