CN108680923A - 一种基于角锥器激光反射的水下机器人三维定位通信装置及其方法 - Google Patents

Abstract

本发明公开了一种基于角锥器激光反射的水下机器人三维定位通信装置及其方法，属于水下机器人的定位与通信领域。装置包括位于海底的激光发射端基站和安装再水下机器人上的激光反射端；激光发射端基站包括带有窗口镜的第一密封腔体以及设置在第一密封腔体内的激光发生器、振镜系统、电荷耦合摄像头和发射端处理器；激光反射端包括光敏接收器、通信角锥器和反射端处理器，通信角锥器的外侧周向固定有聚合物分散液晶；第二密封腔体外设置有用于将激光反射至电荷耦合摄像头的定位角锥器阵列。实现了水下机器人与海底基站的双向通信。且其结构简单、功耗低、体积小、可靠性高、成本低。

Description

一种基于角锥器激光反射的水下机器人三维定位通信装置及 其方法

技术领域

本发明涉及水下机器人的定位与通信领域，具体涉及一种基于角锥器激光反射的水下机器人三维定位通信装置及其方法。

背景技术

水下机器人常用的定位通信方式是基于声学的方法，具有通信传输距离远的特点，但其功耗较大。光学定位具有空间分辨率高的特点，适合于近距离高精度定位。常用的方法是利用脉冲激光光源并辅助飞行时间采样对水下回射器进行距离探测，最大探测距离可达到100米，但飞行时间探测需要超高速模数转换器与大功率纳秒脉冲激光源，设备成本极高，功耗较大。基于飞行时间的测距精度高，且精度不受距离影响，但利用飞行时间测距对硬件要求较高，且需要抑制水体的后向散射(Han J,Yang K,Sun L,et al.Improvedtime-of-flight range acquisition technique in underwater lidar experiments[J].Applied Optics.2015,54(18):5715-5725.)，实施难度相对较高。

发明内容

本发明的目的为提供一种基于角锥器激光反射的水下机器人三维定位通信装置，本发明的另一目的为提供一种基于角锥器激光反射的水下机器人三维定位通信方法，该装置及其方法克服现有的水下声学定位通信方法以及基于脉冲激光光源并辅助飞行时间采样的定位方法成本高、功耗大、实现较为复杂的缺陷，实现海底基站对水下机器人的三维定位以及双向通信。

为了实现上述目的，本发明提供的基于角锥器激光反射的水下机器人三维定位通信装置包括位于海底的激光发射端基站和安装在水下机器人上的激光反射端；

激光发射端基站包括带有窗口镜的第一密封腔体以及设置在第一密封腔体内的：

激光发生器，用于发射加载有下行通信数据的激光；

振镜系统，将激光反射至激光反射端并调整激光反射出去的角度；

电荷耦合摄像头，接收从激光反射端反射回来的激光；

发射端处理器，根据所接收到的激光对应的振镜系统的角度信息计算水下机器人的位置并从电荷耦合摄像头接收到的调制激光的图像中解调出上行通信数据；

激光反射端包括：

光敏接收器，接收由激光发射端基站出射的加载有下行通信数据的激光；

定位角锥器阵列，由多个定位角锥器组成，用于反射由激光发射端基站出射的激光；

通信角锥器，反射由激光发射端基站出射的激光；

聚合物分散液晶，置于通信角锥器的外周，将上行通信数据调制入由通信角锥器反射的激光；

反射端处理器，处理由光敏接收器接收的激光，并控制聚合物分散液晶进行激光调制。

上述技术方案中，激光发生器为蓝光光源激光器或绿光光源激光器中的一种，用于产生蓝激光或绿激光，优选采用532nm激光。激光发射端基站根据跟踪过程中水下机器人的位置计算出水下机器人端光敏接收器的位置，基站将对机器人的控制指令通过调制激光的形式发送至水下机器人上的光敏接收器，完成水下机器人端的下行通信；与此同时，激光发射端基站根据跟踪过程中水下机器人的位置计算出水下机器人上的通信角锥器的位置，并发射激光经过聚合物分散液晶至通信角锥器，水下机器人用加电激光发射端改变透光量的聚合物分散液晶将角锥器回射到基站的激光光强编码调制，并回传至基站的电荷耦合摄像头，发射端处理器经过解调处理，实现水下机器人到基站的上行通信。从而实现水下机器人与海底基站的双向通信。优选第一密封腔体采用亚克力材料制成，其密封效果好。该装置结构简单、功耗低、体积小、可靠性高、且成本低。

具体的方案为第一密封腔体内设有位于激光发生器的发射端的扩束镜扩束镜，以及位于扩束镜扩束镜光路上的反射棱镜，该反射棱镜将经扩束镜扩束后的激光反射至振镜系统，经振镜系统调整后的激光从窗口镜射出。

扩束镜扩束镜用于在扩大激光光束束腰的同时，压缩激光光束的扩散角，使得激光反射端角锥器能够更多地回射激光。优选反射棱镜位于扩束镜前方且与之轴线成45度夹角，反射棱镜将激光反设至另一侧的振镜系统，振镜系统由两个轴向互成90度的振镜、驱动电机及其驱动模块组成，其中振镜的运动角度为-30°～30°。电荷耦合摄像头固定在振镜系统上方，用于监测激光反射端的激光信号。发射端处理器位于激光发生器的一侧。窗口镜开设在在振镜系统及电荷耦合摄像头前方。

为了使整个装置稳定运行，将第一密封腔体设置成圆筒形，并通过两个半圆形固定架固定在基站底座上。

另一个具体的方案为定位角锥器阵列包括沿同轴布置的第一定位角锥器、第二定位角锥器和第三定位角锥器。设置三个定位角锥器可提高测量精度，同时，将三个定位角锥器设置在同一垂线上使得计算更加方便。

进一步具体的方案为激光反射端还包括第二密封腔体，第二密封腔体的上端设有穿过腔体上端盖的上固定杆，下端设有固定在腔体下端盖上的下固定杆，上固定杆和下固定杆同轴设置；第一定位角锥器和第二定位角锥器间隔安装在上固定杆上，且位于第二密封腔体外，第三定位角锥器安装在下固定杆上；光敏接收器和通信角锥器均安装与上固定杆上，且位于第二密封腔体内。

通信角锥器的结构和组成与定位角锥器相同，聚合物分散液晶固定于通信角锥器的外侧周向，聚合物分散液晶被施加电压时，可以改变自身透光量，水下机器人通过聚合物分散液晶来调制通信角锥器反射到基站的激光光强，从而向基站传输数据。整体的激光反射端通过下固定杆与水下机器人进行螺纹连接。

更进一步具体的方案为第二密封腔体为由亚克力材料制成的透明结构。激光刻穿过透明的腔体壁进行信息传递。

另一个具体的方案为光敏接收器为由绕中心轴均匀排布的12个光敏二极管组成的360°全向型光敏接收器。保证了光敏接收器能够接收基站向水下机器人发送的数据。

另一个一个具体的方案为定位角锥器和通信角锥器均采用角锥棱镜。每个角锥器由多个角锥棱镜沿偏航角方向均布构成，角锥棱镜使用石英K9材料制作而成，并且优选在三个反射面上镀铜膜以便光线可以更好的被反射，角锥棱镜是由三面相互垂直的反射镜面构成的光学棱镜。当入射光线照射到角锥棱镜时，如果该光线能够依次和角锥棱镜的三个反射面相交，那么出射光线将平行于入射光线，并且出射光线和入射光线所在的平面经过角锥棱镜原点，且两条光线到角锥棱镜原点的距离相等。保证了水下机器人在偏航角方向发生360°旋转时，至少有一个角锥器的有效反射面能被激光发射基站发射的激光照射到。

再一个具体的方案为：

为了实现上述另一目的，本发明提供的基于角锥器激光反射的水下机器人三维定位通信方法基于上述水下机器人三维定位通信装置实现，其包括以下步骤：

1)激光发生器向水下机器人所处区域发射激光，由电荷耦合摄像头接收定位角锥器反射的激光，对水下机器人进行定位；

定位步骤包括：

1-1)建立水下机器人坐标系和激光发射端基站坐标系，设水下机器人坐标系原点O₂与基站窗口镜在水平方向的距离为d_prf，利用定位角锥器获得三组d_prf'的值，对三组d_prf'进行加权平均得到距离d_prf；

其中，d_prf'的获取方法为：

设对准激光反射端上第一角锥器与第二角锥器的两束激光在第一密封腔体内的俯仰角为θ_inq,1、θ_inq,2，两束激光在水体里的角度为θ′_inq,1、θ′_inq,2，两个角锥器原点在水下机器人坐标系下的位置为p_r,1＝(0,0,L_r,1)、p_r,2＝(0,0,L_r,2)，L_r,1为第一定位角锥器与水下机器人坐标系原点O₂的距离，L_r,2为第二定位角锥器与水下机器人坐标系原点O₂的距离，则d_prf'通过以下公式求解：

公式中d_itf表示激光发射端基站坐标系原点O₁与窗口镜的距离，ψ_inq,1表示激光在第一密封腔体内的偏航角，n_w表示水的折射率；

1-2)设第i个激光光束在水体中的直线l_lsr,i由点P_lsr,i＝(P_lsr,x,i,P_lsr,y,i,P_lsr,z,i)和单位方向向量δ_lsr,i＝(δ_lsr,x,i,δ_lsr,y,i,δ_lsr,z,i)来描述，其中P_lsr,i是第i个激光光束与窗口镜的交点，令cos(·)＝c(·)，sin(·)＝s(·)，则：

其中，T_refr表示原始零位激光光束到最终水体中激光光束的旋转矩阵，T_refr,1,1、T_refr,2,1与T_refr,3,1分别表示旋转矩阵T_refr的第1列第1～3行的元素，T₂表示经过窗口镜折射前、后激光光束的旋转矩阵，ψ_inq,i表示第i个激光光束在发射端第一密封腔体内的偏航角，θ_inq,i表示第i个激光光束在发射端第一密封腔体内的俯仰角，q是将激光从空气中变换到水体中旋转矩阵对应的四元数， 是激光光线从空气折射到水中偏转过程对应的偏转角度，n_refr是偏转轴单位向量，则水下机器人的定位位置为：

2)根据水下机器人的位置计算出位于水下机器人上的光敏接收器的位置，并将加载有控制指令的激光发送到该位置的光敏接收器，向水下机器人发送通信数据；

3)根据水下机器人的位置计算出位于水下机器人上的通信角锥器的位置，并发射激光至通信角锥器，通信角锥器对激光进行反射，并利用聚合物分散液晶对激光进行调制加载传输数据，并反射入激光发射端基站内，向激光发射端基站上传通信数据；对经调制后的激光进行解调处理，得到水下机器人的信息指令。

与现有技术相比，本发明的有益效果为：

本发明实现了海底基站对水下机器人的三维定位以及水下机器人与海底基站的双向通信。装置结构简单，控制方便，运行可靠。通过调制反射光的方法可以有效降低水下机器人的通信能耗，提升了水下机器人的续航能力。

附图说明

图1为本发明实施例的水下机器人三维定位通信装置的系统示意图；

图2为本发明实施例的激光发射端基站的结构示意图；

图3为本发明实施例的激光反射端的结构示意图；

图4为本发明实施例的激光三维定位的原理示意图。

其中：1、激光发射端基站；2、激光反射端；3、水下机器人；4、窗口镜；5、电荷耦合摄像头；6、振镜系统；7、第一密封腔体；8、发射端处理器；9、固定架；10、激光发生器；11、基站底座；12、扩束镜；13、反射棱镜；14、第一定位角锥器；15、第二定位角锥器；16、聚合物分散液晶；17、通信角锥器；18、光敏接收器；19、反射端处理器；20、第三定位角锥器；21、下固定杆；22、下端盖；23、第二密封腔体；24、上端盖；25、上固定杆。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合实施例及其附图对本发明作进一步说明。

装置实施例

参见图1至图3，本发明的基于角锥器激光反射的水下机器人三维定位通信装置包括位于海底的激光发射端基站1和安装在水下机器人3上的激光反射端2。

其中，激光发射端基站1包括第一密封腔体7，第一密封腔体7为圆柱形，其通过2个半圆形固定架9固定在基站底座11上。在第一密封腔体7内设有激光发生器10和扩束镜12，激光发生器10用于产生532nm激光，扩束镜12用于对激光进行扩束。扩束镜12前方设有与之轴线成45度夹角的反射棱镜13，反射棱镜13将激光反设至另一侧的振镜系统6，振镜系统6由2个轴向互成90度的振镜、驱动电机及其驱动模块组成，其中振镜的运动角度为-30°～30°。振镜系统6上方固定有用于监测激光反射端2的激光信号的电荷耦合摄像头5。激光发生器10的另一侧为发射端处理器8。所有装置均固定于第一密封腔体7内。在第一密封腔体的一端(即振镜系统6及电荷耦合摄像头5的前方)开有窗口镜4，反射棱镜13将经扩束镜12扩束后的激光反射至振镜系统6，经振镜系统6调整后的激光从窗口镜4射出。

激光反射端2安装于水下机器人3的上端，包括第二密封腔体23、第一定位角锥器14、第二定位角锥器15和第三定位角锥器20。第二密封腔体23同样为圆柱形，其上端设有上端盖24，下端设有下端盖22。第二密封腔体23的上端设有穿过上端盖24的上固定杆25，下端设有固定在下端盖22上的下固定杆21，上固定杆25和下固定杆21同轴设置。第一定位角锥器14和第二定位角锥器15间隔安装在上固定杆25上，且位于第二密封腔体23外，第三定位角锥器20安装在下固定杆21上，位于第二密封腔体23外。

在第二密封腔体23内设有光敏接收器18、通信角锥器17以及反射端处理器19，光敏接收器18和通信角锥器17均安装在上固定杆25上。通信角锥器17的外侧周向固定有聚合物分散液晶16。反射端处理器19用于存储光敏接收器18接收到的控制指令，同时控制聚合物分散液晶的透光量以对通信角锥器17反射的激光进行光强编码调制并反射给电荷耦合摄像头5。

本实施例的角锥器(包括定位角锥器和通信角锥器)均由多个角锥棱镜沿偏航角方向均布构成，角锥棱镜使用石英K9材料制作而成，并且在三个反射面上镀铜膜以便光线可以更好的被反射，角锥棱镜是由三面相互垂直的反射镜面构成的光学棱镜。当入射光线照射到角锥棱镜时，如果该光线能够依次和角锥棱镜的三个反射面相交，那么出射光线将平行于入射光线，并且出射光线和入射光线所在的平面经过角锥棱镜原点，且两条光线到角锥棱镜原点的距离相等。保证了水下机器人在偏航角方向发生360°旋转时，至少有一个角锥器的有效反射面能被基站激光照射到。

光敏接收器18接收基站向水下机器人发送的数据，其由12个光敏二极管绕固定杆轴向均匀排布成水平360°全向型光敏接收器。聚合物分散液晶16固定于通信角锥器17的外侧周向，聚合物分散液晶被施加电压时，可以改变自身透光量，水下机器人通过聚合物分散液晶来调制通信角锥器反射到基站的激光光强，可以向基站传输数据。从而实现水下机器人与基站的双向通信。整体的激光反射端通过下固定杆21与水下机器人3进行螺纹连接。

本实施例的第一密封腔体和第二密封腔体均由亚克力材料制成，其密封效果好。

本实施例的激光发射端基站根据跟踪过程中水下机器人的位置计算出水下机器人端光敏接收器的位置，基站将对机器人的控制指令通过调制激光的形式发送至水下机器人上的光敏接收器，完成水下机器人端的下行通信；与此同时，激光发射端基站根据跟踪过程中水下机器人的位置计算出水下机器人上的通信角锥器的位置，并发射激光经过聚合物分散液晶至通信角锥器，水下机器人用加电激光发射端改变透光量的聚合物分散液晶将角锥器回射到基站的激光光强编码调制，并回传至基站的电荷耦合摄像头，发射端处理器经过解调处理，实现水下机器人到基站的上行通信。从而实现水下机器人与海底基站的双向通信。

方法实施例

本实施例的基于角锥器激光反射的水下机器人三维定位通信方法基于装置实施例中的水下机器人三维定位通信装置实现，海底的激光发射端基站通过激光发生器发出532nm激光经过扩束镜扩束、棱镜反射、振镜系统反射过窗口镜射出，激光发射端基站通过调整振镜的角度改变激光发射角度，通过初始化激光搜寻方法在指定区域发射激光，直至激光射中水下机器人接收端的定位角锥器，定位角锥器将入射激光平行反射至发射基站的电荷耦合摄像头。利用激光发射端基站的振镜系统的俯仰角、偏航角信息和水下机器人定位角锥器阵列的位置基线信息，结合带折射补偿的定位算法计算水下机器人的位置，并迭代更新下一个周期问询激光的角度。水下机器人利用角锥器阵列将从基站发射过来的问询光线回射到基站，使基站对其进行三维跟踪定位(原理参见图4)。

其具体的步骤如下：

1)利用激光发生器追踪水下机器人，并利用水下机器人反射的激光情况对其进行定位；

1-1)建立水下机器人坐标系和激光发射端基站坐标系，设水下机器人坐标系原点O₂与基站窗口镜在水平方向的距离为d_prf，利用三个定位角锥器获得三组d_prf'的值，对三组d_prf'进行加权平均得到水下机器人原点与基站窗口镜在水平方向的距离d_prf；

d_prf'的获取方法为：

设对准激光反射端上第一角锥器与第二角锥器的两束激光在第一密封腔体内的俯仰角为θ_inq,1、θ_inq,2，两束激光在水体里的角度为θ_i′_nq,1、θ_i′_nq,2，两个角锥器原点在水下机器人坐标系下的位置为p_r,1＝(0,0,L_r,1)、p_r,2＝(0,0,L_r,2)，L_r,1为第一定位角锥器与水下机器人坐标系原点O₂的距离，L_r,2为第二定位角锥器与水下机器人坐标系原点O₂的距离，则d_prf'通过以下公式求解：

公式中d_itf表示激光发射端基站坐标系原点O₁与窗口镜的距离，ψ_inq,1表示激光在第一密封腔体内的偏航角，n_w表示水的折射率；

1-2)设第i个激光光束在水体中的直线l_lsr,i由点P_lsr,i＝(P_lsr,x,i,P_lsr,y,i,P_lsr,z,i)和单位方向向量δ_lsr,i＝(δ_lsr,x,i,δ_lsr,y,i,δ_lsr,z,i)来描述，其中P_lsr,i是第i个激光光束与窗口镜的交点，令cos(·)＝c(·)，sin(·)＝s(·)，则：

其中，T_refr表示原始零位激光光束到最终水体中激光光束的旋转矩阵，T_refr,1,1、T_refr,2,1与T_refr,3,1分别表示旋转矩阵T_refr的第1列第1～3行的元素，T₂表示经过窗口镜折射前、后激光光束的旋转矩阵，ψ_inq,i表示第i个激光光束在发射端第一密封腔体内的偏航角，θ_inq,i表示第i个激光光束在发射端第一密封腔体内的俯仰角，q是将激光从空气中变换到水体中旋转矩阵对应的四元数， 是激光光线从空气折射到水中偏转过程对应的偏转角度，n_refr是偏转轴单位向量，则水下机器人的定位位置为：

2)根据水下机器人的位置计算出位于水下机器人上的信号接收器的位置，并将对机器人的控制指令发送到该位置的信号接收器；

3)根据水下机器人的位置计算出位于水下机器人上的通信角锥器的位置，并发射激光至通信角锥器，通信角锥器对激光进行反射，并利用聚合物分散液晶对激光进行光强编码调制后传回至基站；对经光强编码调制后的激光进行解调处理，得到水下机器人的信息指令。从而实现水下机器人与海底基站的双向通信。

Claims (9)

1.一种基于角锥器激光反射的水下机器人三维定位通信装置，其特征在于，包括位于海底的激光发射端基站和安装在水下机器人上的激光反射端；

所述激光发射端基站包括带窗口镜的第一密封腔体以及设置在第一密封腔体内的：

激光发生器，用于发射加载有下行通信数据的激光；

振镜系统，将激光反射至激光反射端并调整激光反射出去的角度；

电荷耦合摄像头，接收从激光反射端反射回来的激光；

发射端处理器，根据所接收到的激光对应的振镜系统的角度信息计算所述水下机器人的位置并从电荷耦合摄像头接收到调制激光的图像中解调出上行通信数据；

所述激光反射端包括：

光敏接收器，接收由所述激光发射端基站出射的加载有下行通信数据的激光；

定位角锥器阵列，由多个定位角锥器组成，用于反射由所述激光发射端基站出射的激光；

通信角锥器，反射由所述激光发射端基站出射的激光；

聚合物分散液晶，置于所述通信角锥器的外周，将所述上行通信数据调制入由所述通信角锥器反射的激光；

反射端处理器，处理由所述光敏接收器接收的激光，并控制聚合物分散液晶进行激光调制。

2.根据权利要求1所述的水下机器人三维定位通信装置，其特征在于：

所述的第一密封腔体内设有位于所述激光发生器的发射端的扩束镜，以及位于所述扩束镜光路上的反射棱镜，该反射棱镜将经所述扩束镜扩束后的激光反射至所述的振镜系统，经振镜系统调整后的激光从所述窗口镜射出。

3.根据权利要求1所述的水下机器人三维定位通信装置，其特征在于：

所述的定位角锥器阵列包括沿同轴布置的第一定位角锥器、第二定位角锥器和第三定位角锥器。

4.根据权利要求3所述的水下机器人三维定位通信装置，其特征在于：

所述的激光反射端还包括第二密封腔体，所述的第二密封腔体的上端设有穿过腔体上端盖的上固定杆，下端设有固定在腔体下端盖上的下固定杆，所述上固定杆和下固定杆同轴设置；

所述的第一定位角锥器和第二定位角锥器间隔安装在所述上固定杆上，且位于第二密封腔体外，所述的第三定位角锥器安装在所述下固定杆上；

所述的光敏接收器和通信角锥器均安装与所述上固定杆上，且位于所述第二密封腔体内。

5.根据权利要求4所述的水下机器人三维定位通信装置，其特征在于：

所述的第二密封腔体为由亚克力材料制成的透明结构。

6.根据权利要求1所述的水下机器人三维定位通信装置，其特征在于：

所述的光敏接收器为由绕中心轴均匀排布的12个光敏二极管组成的360°全向型光敏接收器。

7.根据权利要求1所述的水下机器人三维定位通信装置，其特征在于：

所述的定位角锥器和通信角锥器均采用角锥棱镜。

8.一种基于角锥器激光反射的水下机器人三维定位通信方法，基于权利要求1至4任一项权利要求所述的水下机器人三维定位通信装置实现，其特征在于，包括以下步骤：

1)激光发生器向水下机器人所处区域发射激光，由电荷耦合摄像头接收定位角锥器反射的激光，对所述水下机器人进行定位；

2)根据水下机器人的位置计算出位于水下机器人上的光敏接收器的位置，并将加载有控制指令的激光发送到该位置的光敏接收器，向所述水下机器人发送通信数据；

3)根据水下机器人的位置计算出位于水下机器人上的通信角锥器的位置，并发射激光至通信角锥器，通信角锥器对激光进行反射，并利用聚合物分散液晶对激光进行调制加载传输数据，并反射入激光发射端基站内，向所述激光发射端基站上传通信数据。

9.根据权利要求8所述的水下机器人三维定位通信方法，其特征在于，步骤1)中，定位所述水下机器人的步骤包括：

1-1)建立水下机器人坐标系和激光发射端基站坐标系，设水下机器人坐标系原点O₂与基站窗口镜在水平方向的距离为d_prf，利用定位角锥器获得三组d_prf'的值，对三组d_prf'进行加权平均得到距离d_prf；

其中，d_prf'的获取方法为：

设对准激光反射端上第一角锥器与第二角锥器的两束激光在第一密封腔体内的俯仰角为θ_inq,1、θ_inq,2，两束激光在水体里的角度为θ′_inq,1、θ′_inq,2，两个角锥器原点在水下机器人坐标系下的位置为p_r,1＝(0,0,L_r,1)、p_r,2＝(0,0,L_r,2)，L_r,1为第一定位角锥器与水下机器人坐标系原点O₂的距离，L_r,2为第二定位角锥器与水下机器人坐标系原点O₂的距离，则d_prf'通过以下公式求解：

公式中d_itf表示激光发射端基站坐标系原点O₁与窗口镜的距离，ψ_inq,1表示激光在第一密封腔体内的偏航角，n_w表示水的折射率；

1-2)设第i个激光光束在水体中的直线l_lsr,i由点P_lsr,i＝(P_lsr,x,i,P_lsr,y,i,P_lsr,z,i)和单位方向向量δ_lsr,i＝(δ_lsr,x,i,δ_lsr,y,i,δ_lsr,z,i)来描述，其中P_lsr,i是第i个激光光束与窗口镜的交点，令cos(·)＝c(·)，sin(·)＝s(·)，则：

其中，T_refr表示原始零位激光光束到最终水体中激光光束的旋转矩阵，T_refr,1,1、T_refr,2,1与T_refr,3,1分别表示旋转矩阵T_refr的第1列第1～3行的元素，T₂表示经过窗口镜折射前、后激光光束的旋转矩阵，ψ_inq,i表示第i个激光光束在发射端第一密封腔体内的偏航角，θ_inq,i表示第i个激光光束在发射端第一密封腔体内的俯仰角，q是将激光从空气中变换到水体中旋转矩阵对应的四元数， 是激光光线从空气折射到水中偏转过程对应的偏转角度，n_refr是偏转轴单位向量，则水下机器人的定位位置为：