一种深水机器人组合导航装置和方法制造方法及图纸

本发明专利技术涉及深水机器人技术领域，尤其涉及一种用于深水水机器人组合导航的装置和方法。装置包括测距器、信标、传感器和导航计算机。方法为：深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；判断初始位置是否有效；如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。本发明专利技术装置简单，继承性好，稳定可靠，修正结果准确，安装简单，使用寿命长，应用范围广。

【技术实现步骤摘要】
一种深水机器人组合导航装置和方法
本专利技术涉及深水机器人
，尤其涉及一种用于深水水机器人组合导航的装置和方法。
技术介绍
深水机器人航行深度可达数千米。在水下航行过程中，导航误差会随着路程和时间的增加而逐渐增大。当深水机器人航行一段距离后，需要对其位置进行修正，以提高导航精度。当前对深水机器人进行位置修正的方法主要是：通过声学定位系统输出的定位信息对深水机器人位置进行直接修正。但这种方法都存在一定的缺陷：1）无法消除声学传播时延引起的定位误差；2）深水机器人航行轨迹不平滑，不利于导航控制。
技术实现思路
为了克服现有方法的不足，本专利技术要解决的技术问题是提供一种深水机器人组合导航装置与方法，使得航行轨迹平滑且定位误差小。本专利技术为实现上述目的所采用的技术方案是：一种深水机器人组合导航装置和方法，包括测距器，安装于深水机器人壳体外部，连接导航计算机，用于通过声信号测量深水机器人到各个信标的距离；信标，至少三个，布设于海底；传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的的航向、航速和所处深度；导航计算机，用于采集测距器和传感器的信号并实时计算深水机器人当前时刻的位置。所述测距器按固定周期进行测距，周期在3秒～60秒之间。所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。一种深水机器人组合导航方法，包括以下步骤：深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；判断初始位置是否有效；如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。所述计算深水机器人的初始位置包括以下步骤：当测距周期内的测距数据数量大于3时，将初始值N置0，对深水机器人水平面坐标进行解算，解算公式如下：Ax＝v（1）式中，(xi,yi,zi)(i=1,2,3)为第i号信标的三维坐标，(x,y,z)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中Ri表示第i号信标的单程传播距离；根据解算的深水机器人水平面坐标推算航位：式中，(x0,y0)为初始点，Δt为采集周期，(xt,yt)为推算航位，ve=vsinθ，vn=vcosθ，θ为当前航向，v为当前航速。所述判断初始位置是否有效具体为：在下一个测距周期到来时，重新解算公式（1）中的深水机器人水平面坐标，将其与所述推算航位求差后求取均方根误差，即：如果e<=Bias则N=N+1，返回2），直到N>=Window时，判定当前测距周期内解算的初始位置有效。否则N=0，返回1）。上式中，Bias范围为1米～100米，Window范围为2～30。所述判断距离数据是否有效的方法为：判断公式S(k)=HkPk|k-1H'k|k-1+Rk是否成立，如果成立，则使用当前测量值zi；否则，舍弃；式中，γ范围为6～500，Hk为观测矩阵，Pk|k-1为预测协方差矩阵，H'k|k-1为预测观测矩阵，Rk为观测噪声。所述根据距离数据对深水机器人的位置进行在线修正包括以下步骤：设任意信标的坐标为(xi,yi)(i=1,2,...,)，则离散系统状态方程为sk=Ask-1+wk（2）zk=hk(sk)+νk其中，sk＝[xkyk]T，xk，yk分别表示当前时刻k的东向坐标、北向坐标；zk=c·τi，表示双程声传播距离，其中c表示声速，τi表示第i个信标的双程声传播时间，ve为深水机器人东向速度，xi为第i个信标的东向坐标，vn为深水机器人北向速度，yi为第i个信标的北向坐标，zi为第i个信标的深度,zk为深水机器人当前时刻k的深度；wk是一个二维向量，为过程驱动噪声，wk(1)和wk(2)为正值且不大于10；νk是一个一维向量，为观测噪声，νk为正值且不大于50；1）初始化2）预测Pk|k-1=APk|k-1A'+BQkB'3）修正Kk=Pk|k-1H'k(HkPk|k-1H'k|k-1+Rk)-1Pk|k=(I-KkHk)Pk|k-1其中Qk=diag([wk(1))2(wk(2))2])，本专利技术具有以下优点及有益效果：1.装置简单，继承性好。本专利技术装置仅需要几台信标和一台测距器，无需其它辅助装置，测距器安装简便，修正算法程序移植性好，可方便移植到各个深水机器人。2.稳定可靠，修正结果准确。本专利技术设计了初始位置窗口算法和测距波门算法，保证了初始值和测量值的可靠、有效，位置估计能够稳定收敛，结果准确。3.安装简单，使用寿命长。本专利技术装置采用的信标和测距器，采用现有产品，使用寿命长，测距器安装于深水机器人背部，对安装精度无要求。4.应用范围广。本专利技术不但可以应用于深水机器人，还可以用于其它海洋相关设备，可适用于全海深位置修正。附图说明图1是本专利技术的组成示意图；图2是本专利技术的深水机器人导航传感器配置图；图3是本专利技术的组合导航方法流程图。具体实施方式下面结合附图及实施例对本专利技术做进一步的详细说明。本专利技术由不少于3台的信标、测距器和深水机器人组成，如图1所示。深水机器人导航传感器配置如图2所示，由测距器、航向传感器、航速传感器、深度传感器组成。组合导航算法如图3所示，深水机器人下水后进行数据采集，状态置为’A’,当收到测距数据时，采用本专利技术的初始位置窗口算法计算初始位置。获取有效的初始位置后，状态置为’B’，启动本专利技术设计的滤波算法，利用距离信息对深水机器人的位置进行在线修正。修正前，采用本专利技术的测距波门算法判断测距数据是否为野值，如果不是野值，就使用当前测距数据进行修正，否则，舍弃不用。采用本专利技术设计的初始位置窗口算法对初始位置有效性进行判断，直到初始位置有效，则启动本专利技术设计的组合导航算法。采用本专利技术设计的测距波门算法对测距数据是否为野值进行判断。如果判为野值，则舍弃该测距数据；否则，使用该测距数据。所述初始位置窗口算法为：1）当测距周期内的测距数据数量大于3个时，将初始值N置0，对深水机器人位置进行解算。解算算法如下：Ax＝v（1）式中(xi,yi,zi)(i=1,2,3)指第i号信标的三维坐标，已知。(x,y,z)指深水机器人水平面坐标和深度，深度已知。是第i号海底信标到坐标系原点的水平间距表示各信标到深水机器人的水平距离，其中Ri表示第i信标的单程传播距离。2）以式（1）解算的位置作为深水机器人初始点进行航位推算，设θ为当前航向，v为当前航速，则航位推算算法为：式中(x0,y0)为初始点，(xt,yt)为推算航位，Δt为采集周期，ve=vsinθ，vn=vcosθ。3）在下一个测距周期到来时，返回1）得到（1）解算的位置(x,y)，将其与2）得到的推算位置求差后求取均方根误差，如下：如果e<=Bias则N=N+1，返回2），直到N>=Window时，判定当前测距周期内解算的初始位置有效。否则N=0，返回1）。上式中，Bias范围为1米～100米，Window范围为2～30。所述滤波算法为：设任意信标的坐标为(xi,yi)(i=1,2,..本文档来自技高网...

【技术保护点】
一种深水机器人组合导航装置，其特征在于，包括测距器，安装于深水机器人壳体外部，连接导航计算机，用于通过声信号测量深水机器人到各个信标的距离；信标，至少三个，布设于海底；传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的的航向、航速和所处深度；导航计算机，用于采集测距器和传感器的信号并实时计算深水机器人当前时刻的位置。

【技术特征摘要】
1.一种深水机器人组合导航方法，其特征在于，包括以下步骤：深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；判断初始位置是否有效；如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。2.根据权利要求1所述的一种深水机器人组合导航方法，其特征在于，所述计算深水机器人的初始位置包括以下步骤：1)当测距周期内的测距数据数量大于3时，将初始值N置0，对深水机器人水平面坐标进行解算，解算公式如下：Ax＝v(1)式中，(xi,yi,zi)(i＝1,2,3)为第i号信标的三维坐标，(x,y,z)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中Ri表示第i号信标的单程传播距离；2)根据解算的深水机器人水平面坐标推算航位：式中，(x0,y0)为初始点，Δt为采集周期，(xt,yt)为推算航位，ve＝vsinθ，vn＝vcosθ，θ为当前航向，v为当前航速。3.根据权利要求2所述的一种深水机器人组合导航方法，其特征在于，所述判断初始位置是否有效具体为：在下一个测距周期到来时，重新解算公式(1)中的深水机器人水平面坐标，将其与所述推算航位求差后求取均方根误差，即：如果e＜＝Bias则N＝N+1，返回2)，直到N＞＝Window时，判定当前测距周期内解算的初始位置...

【专利技术属性】
技术研发人员：冀大雄，刘健，刘铁军，徐春晖，石凯，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁;21