一种基于IMU和TOF的水下机器人单信标导航方法与流程
本发明属于机器人技术领域，涉及一种基于imu和tof的水下机器人单信标导航方法。
背景技术：
水下机器人系统在海洋研究、海底地形勘测、军事领域得到了广泛应用，导航是水下机器人系统的一个重要组成部分，当前水下导航系统主要有以下几个方向。
全球定位系统(gps)为地面、空中和水上机器人提供高精度和绝对位置测量，从而实现高精度导航解决方案，但是对于在水下机器人来说，因为射频信号在水中会快速衰减，因此全球定位系统(gps)不能直接应用到水下机器人系统中。
长基线(lbl)声学导航方法是将水下机器人位置信息通过三角定位方法，定位到水下固定的测量声学应答器上，即通过固定位置的测量声学应答器进行定位计算提供绝对位置信息，但是这种导航方法存在工作覆盖范围受限、需要耗时进行测量计算等缺点。
超短基线(usbl)声学导航不需要固定传感器，但需要将位置数据与水下机器人(auv)机载数据相融合，舰船通过声学包将超短基线(usbl)位置数据传输给水下机器人(auv)，这通常会导致时间和数据延迟。并且长基线(lbl)和超短基线(usbl)导航系统价格均很昂贵，不能实现普及应用。
单信标测距导航通过接收gps的水面信标并将其位置以声波包的形式发送给水下机器人，通过tof方法在信标和水下机器人之间精确测距，从而实现精准定位水下机器人坐标。
当前单信标测距导航研究是声学导航方法的一个新的研究方向，为精准定位水下机器人xy坐标，减少xy坐标误差提供了一种新的导航方法(z坐标是深度，是由压力深度传感器测量获得，因此不太需要考虑)。
单信标测距导航系统通过接收gps的水面信标并将其位置以声波包的形式发送给水下机器人，通过tof方法在信标和水下机器人之间精确测距。根据tof计算，水面信标和水下机器人之间的距离是通过测量水下声速来确定的，这个距离用来约束水下机器人的位置估计，从而限制其xy位置误差。单信标测距导航的主要优点是，除了精确定位水下xy位置外，还能够部署多个水下机器人，每个水下机器人同时接收来自同一地面信标的位置数据声学包，因此每个水下机器人的位置都可以实时更新。
可观测性是单信标测距中重要组成部分。由于单信标测距仅提供一个约束，即仅提供了一个相对位置测量，而不是绝对位置测量，因此只能观测到水下机器人部分轨迹，这就需要对可观测性方法进行研究，优化水下机器人运动轨迹，增强其可观测性，使水下机器人导航解更为精确。
imu惯性导航既可以作为导航传感器来确定水下机器人姿态，也可以作为里程计的输入，通过积分线性加速度来获得速度和位置。但是由于加速度计和陀螺仪的噪声水平、测量偏差和漂移误差较高，线性加速度的双重积分随着时间的推移会导致相当大的位置误差。
惯性导航(imu)通常包含加速度计、陀螺仪和磁强计，分别测量线加速度、角速率和磁场强度。因此，惯性导航(imu)既可以作为导航传感器来确定水下机器人的姿态，也可以作为里程计的输入，通过积分线性加速度来获得速度和位置。虽然imu的尺寸、功耗和成本都在降低，但是由于加速度计和陀螺仪的噪声水平、偏差和漂移误差较高，线性加速度的双重积分随着时间的推移会导致相当大的位置误差，并且偏差误差会随时间产生漂移，这样使得惯性导航(imu)不能满足许多导航应用要求。
声学导航包括长基线(lbl)声学导航和超短基线(usbl)声学导航，长基线(lbl)声学导航中，水下机器人(auv)需要将其传感器位置固定，限制任务覆盖范围。超短基线(usbl)声学导航不需要固定传感器，但需要将位置数据与水下机器人(auv)机载数据相融合，舰船通过声学包将超短基线(usbl)位置数据传输给水下机器人(auv)，这通常会导致时间和数据延迟。并且长基线(lbl)和超短基线(usbl)导航系统价格均很昂贵，不能实现普及应用。
技术实现要素：
本发明的目的是提供一种基于imu和tof的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的tof测距错误数据的技术问题。
为实现上述目的，本发明采用如下技术方案：
一种基于imu和tof的水下机器人单信标导航方法，包括如下步骤：
步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；
单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；
惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；
步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；
步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；
步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；
步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对该观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；
步骤8：误差计算模块进行tof测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。
优选的，所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。
优选的，在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，可通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；
所述位置数据声学包中的数据为单信标测距范围数据。
优选的，在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。
优选的，在计算速度时，采用以下公式进行：
δt＝k-t；
其中，x和y是水下机器人在世界坐标系中的估计位置坐标，vp为预定速度值，k和t分别是当前时间和最后一次范围更新时间。
优选的，在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。
本发明所述的一种基于imu和tof的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的tof测距错误数据的技术问题，本发明提出了一种基于惯性测量单元(imu)、水下机器人动态模型速度和单向行程时间声程测量(tof)的单信标测距导航解决方案，实现低成本、高精度的水下机器人导航功能。
附图说明
图1为本发明的流程图；
图2为本发明的单信标测距导航范围的示意图。
具体实施方式
如图1-图2所示的一种基于imu和tof的水下机器人单信标导航方法，包括如下步骤：
步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；
单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；
惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；
步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；
惯性导航通过测量水下机器人加速度、角速度、偏航角等信息，通过积分运算获得水下机器人实时速度和实时位置信息。
耦合距离滤波器的作用是滤除导致不稳定状态估计的距离测量数据，即有部分距离测量数据可能受到各类故障影响，产生奇异数据，而这写数据需要滤除掉；
故障范围包括：
1.由于发射机和接收机之间的时钟同步性差或声环境变化造成的。
2.声学环境由于可能存在多路径传播或反射现象的可能导致tof测量数据出现错误。
本步骤中采用的耦合距离滤波器包括最大工作范围滤波器和状态接收阈值滤波器两部分，而耦合距离滤波器中的最大工作范围滤波器和状态接收阈值滤波器与在数据处理中心中建立的最大工作范围滤波器和状态接收阈值滤波器的原理相同。
步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；
步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；
优选的，在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，可通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；
所述位置数据声学包中的数据为单信标测距范围数据。
步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对该观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；
优选的，在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。
优选的，在计算速度时，采用以下公式进行：
δt＝k-t；
其中，x和y是水下机器人在世界坐标系中的估计位置坐标，vp为预定速度值，k和t分别是当前时间和最后一次范围更新时间。
单信标测距导航在运行过程中，由于发射机和接收机之间的时钟同步性差或水声环境发生变化等因素，会造成运行故障和导航错误信息，例如水下机器人和信标系统中任何一个时钟上的少量漂移都会改变声学包的tol或toa，从而改变范围计算；水下多路径传输和物体、地形反射改变了声学环境也可能导致发射信标和接收水下机器人之间的距离测量发生错误，从而使得单信标测距测量数据不准确；在处理距离测量时，一旦超出单信标测距范围，发射信标状态估计中的任何不准确数据都很容易导致接收水下机器人状态估计中的产生错误。
为了解决这些问题，本发明通过将数据与距离滤波器进行耦合，防止处理导致不稳定状态估计的距离测量数据，耦合距离滤波器使用两个不同的过程来防止本发明处理错误的距离测量。这两个过程是最大工作范围滤波器和状态接受滤波器。
步骤8：误差计算模块进行tof测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。
优选的，所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。
优选的，在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。
预测模块根据以下公式来预测机器人的位置：
xk＝fxk-1；
pk＝fpk-1ft+q；
其中，x为机器人当前位置，q为过程协方差矩阵，p为信标预测位置，f为离散系统转移矩阵。
如图2所示，其中x轴为时间；y轴为单信标测距距离，由图2可以看出，没有耦合的距离滤波器，处理所有接收到的原始单信标测距距离，产生不稳定的轨迹，相反，通过耦合距离滤波器，观察到水下机器人xy轨迹有了显著改善。
本发明结合发射时的信标发射时间(tol)和水下机器人接收时间(toa)之间的距离以矢量形式建模，同时考虑增加时不变噪声vrng，其建模公式如下所示：
zrng＝(xtmtmx)1/2+vrng；
其中，m＝[jv-jb]，vrng～n(0,rrng)；
jv表示水下机器人和信标之间的toa数据信息；
jb表示水下机器人和信标之间的tol数据信息，
vrng表示时不变噪声；
rrng表示观测标准差；
t表示转置矩阵；
水下机器人位置方程：
xk＝xk-1+hk(zrng-hrngkxk-1)；
其中，x表示水下机器人位置信息；
k表示工作时间序列；
本发明所述的一种基于imu和tof的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的tof测距错误数据的技术问题，本发明提出了一种基于惯性测量单元(imu)、水下机器人动态模型速度和单向行程时间声程测量(tof)的单信标测距导航解决方案，实现低成本、高精度的水下机器人导航功能。
- 还没有人留言评论。精彩留言会获得点赞！