一种虚实融合的无人水面艇避碰测试方法
阅读说明：本技术 一种虚实融合的无人水面艇避碰测试方法 (Virtual-real fusion unmanned surface vessel collision avoidance test method ) 是由 张波 韦喜忠 陈奕宏 金建海 包涛 陈卓 周则兴 于 2020-08-03 设计创作，主要内容包括：本发明涉及一种虚实融合的无人水面艇避碰测试方法,提出了两种模拟测试类型,一种是既模拟无人艇的运动、又模拟障碍物的运动,可以用于无人艇避碰算法的陆上测试验证,第二种是只模拟障碍物的运动,可以用于无人艇下水后的湖上虚拟避碰测试；通过将障碍物感知、避碰算法、运动执行三个环节解耦,既可单独进行某个环节的测试,也可耦合在一起综合测试,从而加快了工程开发进度,并极易辨别出测试过程中的耦合性问题,获得避碰算法与执行环节的鲁棒性；另一方面,采用与传感器输出完全一致的数据格式,虚拟障碍物模拟模块可以无缝替代感知传感器,虚拟测试结束后只需要将虚拟障碍模块替换为真实的感知传感器,就可以转换为真实感知避碰。(The invention relates to a virtual-real fusion unmanned surface vehicle collision avoidance test method, which provides two simulation test types, wherein one simulation test type is used for simulating the motion of an unmanned vehicle and the motion of a barrier and can be used for onshore test verification of an unmanned vehicle collision avoidance algorithm, and the other simulation test type is used for simulating the motion of the barrier and can be used for lake virtual collision avoidance test after the unmanned vehicle is launched into water; by decoupling the three links of obstacle sensing, collision avoidance algorithm and motion execution, the test of a certain link can be carried out independently, and the comprehensive test can also be carried out together, so that the engineering development progress is accelerated, the problem of coupling in the test process is easily identified, and the robustness of the collision avoidance algorithm and the execution link is obtained; on the other hand, a data format completely consistent with the sensor output is adopted, the virtual obstacle simulation module can seamlessly replace a perception sensor, and after the virtual test is finished, the virtual obstacle simulation module can be converted into a real perception collision avoidance module only by replacing the virtual obstacle simulation module with a real perception sensor.)
技术领域
本发明涉及无人艇避碰测试技术领域，尤其是一种虚实融合的无人水面艇避碰测试方法。
背景技术
无人水面艇，简称无人艇，是一种轻型智能水面运载工具，具有体积小、造价低、速度快、机动性强等特点。随着控制技术、感知技术、无线通信技术的进步，无人艇获得了很大的发展。通过搭载不同的设备，无人艇可以应用在不同的领域，例如，当搭载单波束、多波束、浅底层剖面仪等声呐设备时，可以用于海底测绘、探雷反潜等；当搭载水质采样或检测设备时，可以用于环境检测；当搭载武器时，可以用于区域巡逻、海岸保护、护航、作战等任务。
无人艇在航行过程中可能遇到岛屿、暗礁、灯塔、浮标和航行的船只等障碍物，要保证无人艇能够在海洋中正常安全航行，无人艇必须能够自主避开这些障碍物，同时还要考虑无人艇自身的结构特征以及运动规律，所采取的避碰策略要遵循《国际海上避碰规则》。
现有技术中，无人艇海上避碰技术研究包括三个环节：障碍物感知、避碰算法与运动执行，其中障碍物感知可以通过搭载的感知传感器与感知数据处理模块单独测试，避碰算法也可以通过独立的仿真程序来测试，而运动执行只有通过实船的真实运动才能全面的测试执行机构的性能与可靠性、操控参数的合理性等；此外，障碍物感知环节由于雷达等感知传感器自身的环境适应性特点、数据处理有一定难度等原因，常常会出现一些难以预料的问题，要实现稳定可靠的探测识别需要较长时间的测试。
发明内容
本申请人针对上述现有生产技术中的缺点，提供一种结构合理的虚实融合的无人水面艇避碰测试方法，从而提出虚拟障碍物的方法来对无人艇避碰技术进行研究，有效助力于实船测试，大大缩短了实船测试周期，并有效避开了环境等因素的干扰，可靠性高。
本发明所采用的技术方案如下：
一种虚实融合的无人水面艇避碰测试方法，所述避碰测试方法中无人艇及障碍物均为虚拟模式，设定初始时刻时的船体坐标系为A坐标系，当前时刻为T1，T1时刻时的船体坐标系为B坐标系，下一时刻为T2时刻；包括如下步骤：
第一步：输入T1时刻的初始船位、障碍物在A坐标系中的初始相对速度(VAx，VAy)、初始相对位置(DAx，DAy)；
第二步：更新T1时刻时无人艇在大地坐标系下的船速、航向角，输出A坐标系X轴与B坐标系X轴之间的夹角θ、B坐标系原点在A坐标系中的坐标(DOBAx，DOBAy)；
第三步：根据障碍物在A坐标系中的相对速度(VAx，VAy)，第二步中的夹角θ和坐标(DOBAx，DOBAy)，输出T1时刻时的障碍物在B坐标系中的相对速度(VBx，VBy)；
第四步：根据障碍物在A坐标系中的位置(DAx，DAy)、第二步中的夹角θ和坐标(DOBAx，DOBAy)，输出T1时刻时的障碍物在B坐标系中的相对位置(DBx1，DBy1)；
第五步：根据船速和T1时刻的船位，更新无人艇在T2时刻时的船位，即T2时刻时无人艇在A坐标系中的坐标(Dtx，Dty)；
第六步：根据第三步中的相对速度(VBx，VBy)和第四步中的相对位置(DBx1，DBy1)，输出T2时刻时的障碍物在B坐标系中的相对位置(DBx2，DBy2)；
第七步：根据第六步中的相对位置(DBx2，DBy2)、第二步中A坐标系X轴与B坐标系X轴之间的夹角θ、第五步中的坐标(Dtx，Dty)，输出T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)；
第八步：将T2时刻作为初始时刻T1，将第七步中T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)作为第一步中的已知量，重复第二步至第七步，进行无人艇自主避碰测试；
通过上述循环，不断更新障碍物在B坐标系中的位置，即障碍物当前相对于无人艇的位置，从而模拟无人艇上传感器探测障碍物的过程。
作为上述技术方案的进一步改进：
根据初始时刻无人艇的船速，设定其速度方向与航向角一致，并以此速度方向作为X轴方向，以初始时刻的船位点为原点，该X轴方向以原点为圆心在水平面内右转90°作为Y轴方向，构建A坐标系；
以T1时刻无人艇速度方向作为X轴方向，以T1时刻的船位点为原点，该X轴方向以原点为圆心在水平面内右转90°作为Y轴方向，构建B坐标系。
第三步中，根据二维平面坐标系的旋转与平移变换公式，如下：
其中，R为坐标转换矩阵，θ为以A坐标系X轴为起边、以B坐标系X轴为终边的角度，逆时针为正，采用弧度制，范围(-π，π]；
从而获得T1时刻障碍物在B坐标系中的相对速度(VBx，VBy)；
同样的，第四步中，根据二维平面坐标系的旋转与平移变换公式，如下：
从而获得T1时刻时的障碍物在B坐标系中的相对位置(DBx1，DBy1)。
第七步中，根据二维平面坐标系的逆变换公式，如下：
其中，M为坐标转换矩阵，
从而获得T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)。
一种虚实融合的无人水面艇避碰测试方法，所述避碰测试方法中无人艇真实、障碍物为虚拟，测试时将无人艇放置于湖上，由船载传感器直接实时测量获得无人艇的航向角和船速；设定初始时刻为T1，当前时刻为T2，T1时刻时的大地坐标系为A坐标系，T2时刻时的无人艇船体雷达坐标系为B坐标系；包括如下步骤：
步骤一：更新T1时刻时障碍物在A坐标系中的初始速度(VAx，VAy)、初始相对位置(DAx，DAy)；由船载传感器测得T1时刻、T2时刻无人艇的航向角和船速，输出A坐标系X轴与B坐标系X轴之间的夹角θ、B坐标系原点在A坐标系中的坐标(DOBAx，DOBAy)；
步骤二：根据步骤一中的初始速度(VAx，VAy)、坐标(DOBAx，DOBAy)和夹角θ，输出障碍物在B坐标系中的速度(VBx，VBy)；
步骤三：根据步骤一中的初始相对位置(DAx，DAy)，坐标(DOBAx，DOBAy)和夹角θ，输出障碍物在B坐标系中的位置(DBx，DBy)；
步骤四：根据步骤一中初始速度(VAx，VAy)、初始相对位置(DAx，DAy)，输出T2时刻障碍物在A坐标系中的位置(DAx2，DAy2)；
步骤五：将T2时刻作为初始时刻T1，将步骤四中T2时刻障碍物在A坐标系中的位置(DAx2，DAy2)作为步骤一中的已知量，重复步骤一至步骤四，进行无人艇自主避碰测试；
通过上述循环，不断更新障碍物在B坐标系中的位置，即障碍物当前相对于无人艇的位置，从而模拟无人艇上传感器探测障碍物的过程。
作为上述技术方案的进一步改进：
步骤二中，根据二维平面坐标系的旋转与平移变换公式，如下：
其中，R为坐标转换矩阵，θ为以A坐标系X轴为起边、以B坐标系X轴为终边的角度，逆时针为正，采用弧度制，范围(-π，π]；
从而获得T1时刻障碍物在B坐标系中的相对速度(VBx，VBy)；
同样的，步骤三中，根据二维平面坐标系的旋转与平移变换公式，如下：
从而获得T1时刻时的障碍物在B坐标系中的相对位置(DBx，DBy)。
本发明的有益效果如下：
本发明结构紧凑、合理，操作方便，提出了虚拟障碍物的方式来对无人水面艇进行避碰测试，有效避开了障碍物感知环节中硬件传感器的问题，用虚拟感知模块，结合避碰算法和船体运动，来测试避碰算法和执行环节的性能及可靠性，大大加快了开发进度，有效助力于实船测试，可靠性好。
本发明还包括如下优点：
通过采用与传感器输出一致的数据格式，虚拟的障碍物模拟模块可以无缝替代感知传感器，在虚拟测试结束后只需要将虚拟障碍模块替换为真实的感知传感器，就可以转换为真实感知避碰；另一方面，测试过程中还可以只针对某一种感知传感器进行模拟，用虚拟传感器加上真实传感器的模式，测试感知数据融合功能。
第一种避碰模拟测试同时模拟了无人艇的运动和障碍物的运动，可以用于无人艇避碰算法的陆上测试验证，此种情况下无人艇不需要下水；第二种避碰模拟测试只模拟了障碍物的运动，可以用于无人艇下水后的湖上虚拟避碰测试，此种情况下可以在真实水上人为设置虚拟障碍物，测试无人艇在实际环境下的避碰性能。
附图说明
图1为本发明的避碰测试流程图。
具体实施方式
下面结合附图，说明本发明的具体实施方式。
如图1所示，本实施例的一种虚实融合的无人水面艇避碰测试方法，包括两种避碰模拟测试，一种为无人艇及障碍物均为虚拟的模拟测试，另一种为无人艇真实、障碍物虚拟的模拟测试；通过界面输入选择两种避碰模拟测试中的一种。
一、无人艇及障碍物均为虚拟的模拟测试
设定初始时刻时的船体坐标系为A坐标系，当前时刻为T1，T1时刻时的船体坐标系为B坐标系，下一时刻为T2时刻；包括如下步骤：
第一步：输入T1时刻的初始船位、障碍物在A坐标系中的初始相对速度(VAx，VAy)、初始相对位置(DAx，DAy)；
第二步：更新T1时刻时无人艇在大地坐标系下的船速、航向角，输出A坐标系X轴与B坐标系X轴之间的夹角θ、B坐标系原点在A坐标系中的坐标(DOBAx，DOBAy)；
第三步：根据障碍物在A坐标系中的相对速度(VAx，VAy)，第二步中的夹角θ和坐标(DOBAx，DOBAy)，输出T1时刻时的障碍物在B坐标系中的相对速度(VBx，VBy)；
根据二维平面坐标系的旋转与平移变换公式，如下：
其中，R为坐标转换矩阵，θ为以A坐标系X轴为起边、以B坐标系X轴为终边的角度，逆时针为正，采用弧度制，范围(-π,π]；
从而获得T1时刻障碍物在B坐标系中的相对速度(VBx，VBy)；
第四步：根据障碍物在A坐标系中的位置(DAx，DAy)、第二步中的夹角θ和坐标(DOBAx，DOBAy)，输出T1时刻时的障碍物在B坐标系中的相对位置(DBx1，DBy1)；
根据二维平面坐标系的旋转与平移变换公式，如下：
从而获得T1时刻时的障碍物在B坐标系中的相对位置(DBx1，DBy1)；
第五步：根据船速和T1时刻的船位，更新无人艇在T2时刻时的船位，即T2时刻时无人艇在A坐标系中的坐标(Dtx，Dty)；
第六步：根据第三步中的相对速度(VBx，VBy)和第四步中的相对位置(DBx1，DBy1)，输出T2时刻时的障碍物在B坐标系中的相对位置(DBx2，DBy2)；
第七步：根据第六步中的相对位置(DBx2，DBy2)、第二步中A坐标系X轴与B坐标系X轴之间的夹角θ、第五步中的坐标(Dtx，Dty)，输出T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)；
根据二维平面坐标系的逆变换公式，如下：
其中，M为坐标转换矩阵，
从而获得T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)；
第八步：将T2时刻作为初始时刻T1，将第七步中T2时刻时障碍物在A坐标系中的相对位置(DAx2，DAy2)作为第一步中的已知量，重复第二步至第七步，进行无人艇自主避碰测试；
通过上述循环，不断更新障碍物在B坐标系中的位置，即障碍物当前相对于无人艇的位置，从而模拟无人艇上传感器探测障碍物的过程；在无人艇上真实安装传感器时，是由无人艇上传感器探测从而实时得到障碍物相对于无人艇的位置信息的。
本实施例中，根据初始时刻无人艇的船速，设定其速度方向与航向角一致，并以此速度方向作为X轴方向，以初始时刻的船位点为原点，该X轴方向以原点为圆心在水平面内右转90°作为Y轴方向，构建A坐标系；以T1时刻无人艇速度方向作为X轴方向，以T1时刻的船位点为原点，该X轴方向以原点为圆心在水平面内右转90°作为Y轴方向，构建B坐标系。
二、无人艇真实、障碍物虚拟的模拟测试
测试时将无人艇放置于湖上，由船载传感器直接实时测量获得无人艇的航向角和船速；设定初始时刻为T1，当前时刻为T2,T1时刻时的大地坐标系为A坐标系，T2时刻时的无人艇船体雷达坐标系为B坐标系；包括如下步骤：
步骤一：更新T1时刻时障碍物在A坐标系中的初始速度(VAx，VAy)、初始相对位置(DAx，DAy)；由船载传感器测得T1时刻、T2时刻无人艇的航向角和船速，输出A坐标系X轴与B坐标系X轴之间的夹角θ、B坐标系原点在A坐标系中的坐标(DOBAx，DOBAy)；
步骤二：根据步骤一中的初始速度(VAx，VAy)、坐标(DOBAx，DOBAy)和夹角θ，输出障碍物在B坐标系中的速度(VBx，VBy)；
根据二维平面坐标系的旋转与平移变换公式，如下：
其中，R为坐标转换矩阵，θ为以A坐标系X轴为起边、以B坐标系X轴为终边的角度，逆时针为正，采用弧度制，范围(-π,π]；
从而获得T1时刻障碍物在B坐标系中的相对速度(VBx，VBy)；
步骤三：根据步骤一中的初始相对位置(DAx，DAy)，坐标(DOBAx，DOBAy)和夹角θ，输出障碍物在B坐标系中的位置(DBx，DBy)；
根据二维平面坐标系的旋转与平移变换公式，如下：
从而获得T1时刻时的障碍物在B坐标系中的相对位置(DBx，DBy)
步骤四：根据步骤一中初始速度(VAx，VAy)、初始相对位置(DAx，DAy)，输出T2时刻障碍物在A坐标系中的位置(DAx2，DAy2)；
步骤五：将T2时刻作为初始时刻T1，将步骤四中T2时刻障碍物在A坐标系中的位置(DAx2，DAy2)作为步骤一中的已知量，重复步骤一至步骤四，进行无人艇自主避碰测试；
通过上述循环，不断更新障碍物在B坐标系中的位置，即障碍物当前相对于无人艇的位置，从而模拟无人艇上传感器探测障碍物的过程。
本实施例中，第一种避碰模拟测试同时模拟了无人艇的运动和障碍物的运动，可以用于无人艇避碰算法的陆上测试验证，此种情况下无人艇不需要下水；通过不断更新无人艇运动信息与障碍物运动信息，从而可以在陆上实验室条件下，进行静态障碍物、以及各种不同运动速度和方向的动态障碍物情况下的无人艇自主避碰算法测试，节省了湖上测试时间；
第二种避碰模拟测试只模拟了障碍物的运动，可以用于无人艇下水后的湖上虚拟避碰测试，此种情况下可以在真实水上人为设置虚拟障碍物，测试无人艇在实际环境下的避碰性能；通过在无人艇运动的同时不断更新障碍物运动信息，在湖上试验时，不用布置真实的障碍物就可以开展静态障碍物、以及各种不同运动速度和方向的动态障碍物情况下的无人艇自主避碰算法测试，在测试自主避碰算法时可以不必考虑感知系统测不准带来的问题。
通过采用与传感器输出一致的数据格式，虚拟的障碍物模拟模块可以无缝替代感知传感器，在虚拟测试结束后只需要将虚拟障碍模块替换为真实的感知传感器，就可以转换为真实感知避碰；另一方面，测试过程中还可以只针对某一种感知传感器进行模拟，用虚拟传感器加上真实传感器的模式，测试感知数据融合功能。
本发明提出了虚拟障碍物的方式来对无人水面艇进行避碰测试，有效避开了障碍物感知环节中硬件传感器的问题，用虚拟感知模块，结合避碰算法和船体运动，来测试避碰算法和执行环节的性能及可靠性，大大加快了开发进度，有效助力于实船测试，可靠性好
以上描述是对本发明的解释，不是对发明的限定，本发明所限定的范围参见权利要求，在本发明的保护范围之内，可以作任何形式的修改。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：具有自修复及水下设施修复功能的海洋装备及方法