技术领域technical field
本发明涉及无人水下航行器技术领域，具体而言，尤其涉及一种基于声呐的改进型模糊人工势场无人船避障方法。The invention relates to the technical field of unmanned underwater vehicles, in particular to an improved sonar-based fuzzy artificial potential field unmanned vessel obstacle avoidance method.
背景技术Background technique
水面无人船艇所采用的传统人工势场避障方法是：在水面无人船艇航行的海面环境中构造一个包括引力场和斥力场的人工势场，由航行的目标点提供引力场，由海面障碍物提供斥力场，使两者作用于无人船产生的引力和斥力构成合力，指导无人船的目标航向，朝着目标点运动。由于其原理简单、计算量小、实时性好等特点，人工势场法目前被广泛应用于水面无人船艇的路径规划和动态避障中。但是传统人工势场存在局部极小值问题和目标不可达问题，导致无人船无法完成避障要求抵达目标点，且传统人工势场法指导下的无人船航向角变化曲线振荡过大，不符合工程实际要求，因此现有人工势场法已经无法满足对无人船艇同时具备精准避障和平滑路径的更高要求。The traditional artificial potential field obstacle avoidance method adopted by surface unmanned ships is: construct an artificial potential field including gravitational field and repulsive field in the sea environment where surface unmanned ships sail, and provide the gravitational field by the target point of navigation, The repulsion field is provided by the obstacles on the sea surface, so that the gravitational and repulsive forces acting on the unmanned ship form a resultant force, guiding the target course of the unmanned ship and moving towards the target point. Due to its simple principle, small amount of calculation, and good real-time performance, the artificial potential field method is currently widely used in the path planning and dynamic obstacle avoidance of surface unmanned ships. However, the traditional artificial potential field has the problem of local minimum value and unreachable target, which makes the unmanned ship unable to complete the obstacle avoidance requirement to reach the target point, and the course angle change curve of the unmanned ship under the guidance of the traditional artificial potential field method oscillates too much. It does not meet the actual engineering requirements, so the existing artificial potential field method can no longer meet the higher requirements for unmanned ships to have precise obstacle avoidance and smooth paths.
水面无人船艇是一种能够自主导航、自动控制的海上智能交通设备，其要想在危险、复杂而又广阔的大海上应对一切突发状况又能平稳航行，必须具备准确感知外界信息、实时监控设备运行状况、做出相应正确决策、严格执行所有命令的能力，最直接的体现就在于能够及时躲避海面障碍物。因此，水面无人船艇面临的首要问题就是实时的路径规划及避障。传统人工势场是将目标点、障碍物及运动质点之间的关系抽象成为一种人工势场。目标点构成的引力场对运动质点产生吸引力，障碍物构成的斥力场对运动质点产生排斥力，二者向量之和则为合力，指导运动质点躲避障碍物，并按照一定的运动轨迹到达目的地。但是当障碍物位于无人船当前位置与目标点连线之间，或无人船受到多个障碍物斥力影响导致其所受斥力与引力大小相等方向相反时，无人船将陷入局部极小值点，无法判断下一步如何运动，导致在该点停止不动或不断徘徊，即存在陷入局部极小值点问题；当无人船离目标点越来越近时，根据传统人工势场引力与斥力公式可知，其所受目标点引力越来越小，因此若当无人船离目标点较近时途径障碍物，其所受斥力将远大于目标点的引力，使无人船无法抵达终点，即存在目标不可达问题。The surface unmanned vessel is a kind of maritime intelligent transportation equipment capable of autonomous navigation and automatic control. It must have the ability to accurately perceive external information, The ability to monitor the operating status of equipment in real time, make corresponding and correct decisions, and strictly execute all orders is most directly reflected in the ability to avoid sea obstacles in time. Therefore, the primary problem faced by surface unmanned ships is real-time path planning and obstacle avoidance. The traditional artificial potential field abstracts the relationship between target points, obstacles and moving particles into an artificial potential field. The gravitational field formed by the target points attracts the moving particles, and the repulsive field formed by the obstacles produces a repulsive force on the moving particles. The sum of the two vectors is the resultant force, which guides the moving particles to avoid obstacles and reach the goal according to a certain trajectory. land. However, when the obstacle is located between the current position of the unmanned ship and the line connecting the target point, or the unmanned ship is affected by the repulsive force of multiple obstacles so that the repulsive force it receives is equal to the gravitational force and the direction is opposite, the unmanned ship will fall into a local minimum value point, it is impossible to judge how to move in the next step, resulting in stopping or wandering at this point, that is, there is a problem of falling into a local minimum point; when the unmanned ship is getting closer and closer to the target point, according to the traditional artificial potential field gravity It can be seen from the repulsion formula that the gravitational force of the target point is getting smaller and smaller, so if the unmanned ship passes through obstacles when it is closer to the target point, the repulsive force it receives will be much greater than the gravitational force of the target point, making it impossible for the unmanned ship to reach The end point, that is, there is a problem that the target cannot be reached.
发明内容Contents of the invention
根据上述提出的技术问题，而提供一种基于声呐的改进型模糊人工势场无人船避障方法。本发明选用TriTech微型机械扫描声呐作为障碍物探测设备以实现快速、准确的感知无人船艇周围的障碍物分布情况。将障碍物、目标点、无人船之间的位置关系输入到电脑中，通过本文所述改进型人工势场法计算下一步无人船航向角，指导无人船避开障碍物向目标点运动。According to the technical problems raised above, an improved sonar-based fuzzy artificial potential field unmanned ship obstacle avoidance method is provided. The present invention selects TriTech micro-mechanical scanning sonar as obstacle detection equipment to realize fast and accurate perception of the distribution of obstacles around the unmanned boat. Input the positional relationship between obstacles, target points, and unmanned ships into the computer, and calculate the course angle of the next unmanned ship through the improved artificial potential field method described in this article, and guide the unmanned ships to avoid obstacles and move towards the target point. sports.
本发明采用负倒数形式的引力势场函数和指数形式的斥力势场函数，并用直接计算势场强度代替传统人工势场分析合力的方法，使用TriTech微型机械扫描声呐收集船前各点的合势场，结合模糊逻辑控制得到变化幅度较小的航向角输出，使无人船能够在障碍物分布情况未知的海域快速跳出局部极小值点，并平滑航向角变化趋势，优化避障路径。The present invention adopts the gravitational potential field function in negative reciprocal form and the repulsive force potential field function in exponential form, and replaces the traditional artificial potential field analysis resultant method with direct calculation of potential field strength, and uses TriTech micro-mechanical scanning sonar to collect the resultant potential of each point in front of the ship field, combined with fuzzy logic control to obtain a course angle output with a small change range, so that the unmanned ship can quickly jump out of the local minimum point in the sea area where the distribution of obstacles is unknown, and smooth the change trend of the course angle to optimize the obstacle avoidance path.
本发明采用的技术手段如下：The technical means adopted in the present invention are as follows:
一种基于声呐的改进型模糊人工势场无人船避障方法，包括如下步骤：An improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar, comprising the following steps:
S1、对地面站上位机的数据进行初始化；S1. Initialize the data of the upper computer of the ground station;
S2、采用声呐测距模块获取无人船周围障碍物位置信息；S2, using the sonar ranging module to obtain the location information of obstacles around the unmanned ship;
S3、基于改进型人工势场算法，计算合势场最小的航向角ψmin；S3. Based on the improved artificial potential field algorithm, calculate the minimum course angle ψmin of the resultant potential field;
S4、基于改进型人工势场算法，计算无人船舶当前航向角与上述步骤S3中合势场最小的航向角ψmin之间的夹角ψfuzzy；S4, based on the improved artificial potential field algorithm, calculate the angle ψfuzzy between the current course angle of the unmanned ship and the minimum course angle ψmin of the resultant potential field in the above step S3;
S5、基于改进型人工势场算法，计算无人船舶当前位置所受斥力势场强度和引力势场强度之和的绝对值，进而再计算出其绝对值与斥力势场强度和引力势场强度中的绝对值较大值的比值Ufuzzy；S5. Based on the improved artificial potential field algorithm, calculate the absolute value of the sum of the repulsive potential field strength and the gravitational potential field strength at the current position of the unmanned ship, and then calculate its absolute value and the repulsive potential field strength and the gravitational potential field strength The ratio Ufuzzy in which the absolute value is larger;
S6、将步骤S4中计算得到的夹角ψfuzzy和步骤S5中计算得到的比值Ufuzzy分别作为二维模糊控制模块的第一输入变量和第二输入变量，将无人船舶下一步运动航向角与当前航向角之差θ作为二维模糊控制模块的输出量进行模糊化处理，按照模糊控制规则在matlab中进行计算，得到输出量θ；S6, using the included angle ψfuzzy calculated in step S4 and the ratio Ufuzzy calculated in step S5 as the first input variable and the second input variable of the two-dimensional fuzzy control module respectively, and the next step motion heading angle of the unmanned ship The difference θ with the current heading angle is used as the output of the two-dimensional fuzzy control module for fuzzy processing, and is calculated in matlab according to the fuzzy control rules to obtain the output θ;
S7、将二维模糊控制模块的输出量与当前航向角相加得到无人船舶下一步航向角ψr，即ψr＝θ+ψ；S7. Add the output of the two-dimensional fuzzy control module to the current heading angle to obtain the next heading angle ψr of the unmanned ship, that is, ψr =θ+ψ;
S8、根据无人船舶下一步航向角ψr，确定无人船舶的下一步位置为S8. According to the next step heading angle ψr of the unmanned ship, determine the next step position of the unmanned ship as
进一步地，所述的声呐测距模块采用TriTech微型机械扫描声呐，设置在无人船舶的船头，由无人船舶的电源模块供电。Further, the sonar ranging module adopts TriTech micro-mechanical scanning sonar, is arranged on the bow of the unmanned ship, and is powered by the power supply module of the unmanned ship.
进一步地，所述的TriTech微型机械扫描声呐能够在360°范围内以700kHz的频率发射声波，声波的垂直宽度为35°，水平宽度为3°。Further, the TriTech micro-mechanical scanning sonar can emit sound waves at a frequency of 700 kHz within a range of 360°, with a vertical width of 35° and a horizontal width of 3°.
进一步地，所述步骤S2中的声呐测距模块获取无人船周围障碍物位置信息，具体为：Further, the sonar ranging module in the step S2 obtains the location information of obstacles around the unmanned ship, specifically:
S21、设置无人船舶的运动方向为0°，使TriTech微型机械扫描声呐扫描发射声波后接收反射回来的声波；其扫描覆盖的角度范围为船前-50°到50°之间，则得出该声呐扫描的距离范围为0.3m到75m；S21. Set the direction of motion of the unmanned ship to 0°, so that the TriTech micro-mechanical scanning sonar scans the emitted sound waves and receives the reflected sound waves; the angle range covered by the scan is between -50° and 50° in front of the ship, then it is obtained The distance range of the sonar scan is 0.3m to 75m;
S22、按10°的步长转换角度再次发射并接收声波，得到无人船舶的船前一定范围内各目标点的位置信息和势场信息。S22. Transmit and receive the sound wave again by changing the angle according to the step length of 10°, and obtain the position information and potential field information of each target point within a certain range in front of the unmanned ship.
进一步地，所述TriTech微型机械扫描声呐的扫描覆盖的角度范围为船前-50°到50°之间，扫描的距离范围为0.3m到75m。Further, the scanning angle range of the TriTech micro-mechanical scanning sonar is between -50° and 50° in front of the ship, and the scanning distance range is 0.3m to 75m.
进一步地，所述步骤S3中计算合势场最小的航向角ψmin的具体过程如下：Further, the specific process of calculating the minimum course angle ψmin of the resultant potential field in the step S3 is as follows:
S31、假设无人船在一未知海域的点P处，其空间坐标位置为[x,y]，驶向目标点D，其空间坐标位置为[xd,yd]，该海域分布有n个障碍物，其空间坐标位置分别为[x1,y1]...[xn,yn]，则无人船此时受到的引力势场函数Uatt的表达式为：S31. Assuming that the unmanned ship is at point P in an unknown sea area, its spatial coordinate position is [x, y], and it sails to the target point D, its spatial coordinate position is [xd , yd ], and there are n obstacles, whose spatial coordinates are [x1 , y1 ]...[xn , yn ], then the expression of the gravitational potential field function Uatt experienced by the unmanned ship at this time is:
式中，ka为引力系数，X为无人船当前位置[x,y]，Xd为目标点位置[xd,yd]；In the formula, ka is the gravitational coefficient, X is the current position of the unmanned ship [x, y], Xd is the position of the target point [xd , yd ];
S32、安装在无人船上的声呐当前位置周围没有障碍物时，使无人船舶沿着当前位置与目标点连线方向向目标点航行，定义此时航向角为ψd；S32. When there is no obstacle around the current position of the sonar installed on the unmanned ship, make the unmanned ship sail toward the target point along the direction of the line connecting the current position and the target point, and define the heading angle asψd at this time;
S33、安装在无人船上的声呐当前位置处于任意障碍物斥力影响范围内，则分析计算船前障碍物斥力影响范围处各个估算点的合势场强度；S33. The current position of the sonar installed on the unmanned ship is within the influence range of any obstacle repulsion, then analyze and calculate the resultant potential field strength of each estimation point at the influence range of the obstacle repulsion in front of the ship;
S34、各个估算点的合势场强度，得出合势场最小的估算点处的航向角ψmin。S34. The resultant potential field strength of each estimated point is obtained to obtain the heading angle ψmin at the estimated point where the resultant potential field is the smallest.
进一步地，所述步骤S33中分析计算船前障碍物斥力影响范围处各个估算点的合势场强度的具体步骤如下：Further, in the step S33, the specific steps of analyzing and calculating the resultant potential field strength at each estimation point at the influence range of the obstacle repulsion in front of the ship are as follows:
S331、定义上述步骤S33中估算点的数量为a，位置为M(i)，坐标为[xi,yi](i∈[1,a])，各个估算点代表的航向角为ψM(i)；S331. Define the number of estimated points in the above step S33 as a, the position as M(i), the coordinates as [xi ,yi ](i∈[1,a]), and the heading angle represented by each estimated point as ψM (i) ;
S332、目标点对无人船舶的引力势场方程为：S332. The gravitational potential field equation of the target point to the unmanned ship is:
S333、障碍物对无人船的斥力势场方程为：S333. The repulsion potential field equation of the obstacle to the unmanned ship is:
S334、结合步骤S332和步骤S333，得出M(i)处的合势场强度为：S334, in combination with step S332 and step S333, the resulting potential field strength at M(i) place is:
进一步地，所述步骤S6中的：Further, in the step S6:
第一输入变量ψfuzzy＝ψ-ψmin，其论域为[-π/3,π/3],模糊集为A＝{NB,NM,NS,ZO,PS,PM,PB}；The first input variable ψfuzzy = ψ-ψmin , its domain is [-π/3,π/3], and the fuzzy set is A={NB,NM,NS,ZO,PS,PM,PB};
第二输入变量其论域为[0,1]，模糊集为B＝{NB,NM,NS,ZO,PS,PM,PB}；second input variable Its domain is [0,1], and the fuzzy set is B={NB, NM, NS, ZO, PS, PM, PB};
输出量θ，其论域为[-π/24,π/24],模糊集为C＝{NB,NM,NS,ZO,PS,PM,PB}。The output quantity θ, its discourse domain is [-π/24, π/24], and the fuzzy set is C={NB, NM, NS, ZO, PS, PM, PB}.
进一步地，所述步骤S6中的模糊控制规则采用if A and B then C的形式构造。Further, the fuzzy control rules in the step S6 are constructed in the form of if A and B then C.
较现有技术相比，本发明具有以下优点：Compared with the prior art, the present invention has the following advantages:
1、本发明使用TriTech微型机械扫描声呐作为障碍物探测设备，其测量精度较好、体积小、功耗低、耐腐能力强，能够适应无人船艇这个特殊工作环境，可以搭载于无人船艇上实现高精度的扫描探测工作，帮助无人船完成对周围障碍物的精准感知，为无人船艇动态避障提供了硬件设备支持。1. The present invention uses TriTech micro-mechanical scanning sonar as obstacle detection equipment, which has better measurement accuracy, small size, low power consumption, and strong corrosion resistance. It can adapt to the special working environment of unmanned boats and can be carried on unmanned ships. The high-precision scanning and detection work on the ship helps the unmanned ship to complete the accurate perception of surrounding obstacles, and provides hardware equipment support for the dynamic obstacle avoidance of the unmanned ship.
2、本发明以人工势场法为基础进行无人船避障，但摒弃了传统人工势场分析合势力的方法，而采用直接分析势场函数的方法，使计算更简洁。2. The present invention uses the artificial potential field method as the basis for unmanned ship obstacle avoidance, but abandons the traditional method of artificial potential field analysis combined force, and adopts the method of directly analyzing the potential field function to make the calculation more concise.
3、本发明采用模糊控制模块，将人工势场法计算过程中的数据进行二次加工，使目标航向角曲线大大平滑，且变化幅度也大大减小，更符合实际工程要求，且在面对复杂障碍物分布情况的海域时，避障成功率更高。3. The present invention adopts the fuzzy control module to carry out secondary processing on the data in the calculation process of the artificial potential field method, so that the target course angle curve is greatly smoothed, and the variation range is also greatly reduced, which is more in line with the actual engineering requirements, and in the face of In sea areas with complex obstacle distribution, the success rate of obstacle avoidance is higher.
4、本发明使用负倒数形式的引力势场函数，和包含无人船与目标点相对位置信息且为指数形式的斥力势场函数，能够高效解决传统人工势场法存在的目标不可达问题和局部极小值问题。4. The present invention uses the gravitational potential field function in the negative reciprocal form, and the repulsive potential field function in exponential form that contains the relative position information between the unmanned ship and the target point, which can efficiently solve the problem of target inaccessibility and The problem of local minima.
5、本发明使用TriTech微型机械扫描声呐监测无人船周围的海况信息，其探测半径较大可覆盖0.3米至75米半径范围内的区域，可在距离障碍物较远的位置提前感知，有助于无人船较早驶离障碍物从而避免触礁等碰撞事件发生。5. The present invention uses TriTech micro-mechanical scanning sonar to monitor the sea state information around the unmanned ship. Its detection radius is relatively large and can cover areas within the radius range of 0.3 meters to 75 meters, and it can be sensed in advance at a position far away from obstacles. It helps the unmanned ship to leave the obstacle earlier to avoid collisions such as hitting the rocks.
6、本发明的TriTech微型机械扫描声呐由无人船艇电源模块供电，而无需额外的电源供电，使得系统体积更小，质量更轻。6. The TriTech micro-mechanical scanning sonar of the present invention is powered by the power module of the unmanned ship without an additional power supply, making the system smaller in size and lighter in weight.
基于上述理由本发明可在无人水下航行器等领域广泛推广。Based on the above reasons, the present invention can be widely promoted in fields such as unmanned underwater vehicles.
附图说明Description of drawings
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description These are some embodiments of the present invention. For those skilled in the art, other drawings can also be obtained according to these drawings without any creative effort.
图1为本发明方法的程序流程图。Fig. 1 is the program flowchart of the method of the present invention.
图2为本发明实施例提供的声呐扫描过程示意图。Fig. 2 is a schematic diagram of a sonar scanning process provided by an embodiment of the present invention.
图3为本发明实施例提供的各点势场分析示意图。Fig. 3 is a schematic diagram of potential field analysis at each point provided by the embodiment of the present invention.
图4为本发明实施例提供的模糊控制规则。Fig. 4 is the fuzzy control rule provided by the embodiment of the present invention.
图5为本发明实施例提供的改进后的无人船航迹及航向角(跳出局部极小值)。Fig. 5 is the improved track and heading angle of the unmanned ship provided by the embodiment of the present invention (jumping out of the local minimum).
图6为本发明实施例提供的未改进无人船航迹及航向角(陷入局部极小值)。Fig. 6 is an unimproved unmanned ship track and heading angle (falling into a local minimum) provided by the embodiment of the present invention.
图7为本发明实施例提供的改进后的无人船航迹及航向角(目标可达)。Fig. 7 shows the improved unmanned ship track and heading angle (target reachable) provided by the embodiment of the present invention.
图8为本发明实施例提供的未改进的无人船航迹及航向角(目标不可达)。Fig. 8 is the unimproved track and heading angle of the unmanned ship provided by the embodiment of the present invention (the target is unreachable).
图9为本发明实施例提供的改进后的无人船航迹及航向角(复杂障碍物分布情况抵达目标点)。Fig. 9 shows the improved track and heading angle of the unmanned ship provided by the embodiment of the present invention (arriving at the target point under complex obstacle distribution).
图10为本发明实施例提供的未改进的无人船航迹及航向角(复杂障碍物分布情况无法抵达目标点)。Fig. 10 is the unimproved track and heading angle of the unmanned ship provided by the embodiment of the present invention (complex obstacle distribution cannot reach the target point).
具体实施方式Detailed ways
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。It should be noted that, in the case of no conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and examples.
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments It is only some embodiments of the present invention, but not all embodiments. The following description of at least one exemplary embodiment is merely illustrative in nature and in no way taken as limiting the invention, its application or uses. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。It should be noted that the terminology used here is only for describing specific embodiments, and is not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural, and it should also be understood that when the terms "comprising" and/or "comprising" are used in this specification, they mean There are features, steps, operations, means, components and/or combinations thereof.
本发明为无人船舶提供了一种基于人工势场的避障方法。无人船舶在茫茫大海上航行过程中，经常会遇到各种各样的障碍物，如礁石、过往船只等。无人船要想完成能源勘探、远程救援、水文地理勘测等更进一步的任务，必须首先实现对障碍物的有效规避并驶向目标位置，如果没有较好的路径规划和避障方法使无人船驶向障碍物，比如驶向礁石等，会给无人船及其搭载的重要昂贵设备造成不必要的损失。因此行驶过程中的实时路径规划对使无人船艇在理想航向上运动的意义重大。The invention provides an obstacle avoidance method based on an artificial potential field for an unmanned ship. When unmanned ships sail in the vast sea, they often encounter various obstacles, such as reefs and passing ships. If an unmanned ship wants to complete further tasks such as energy exploration, remote rescue, and hydrographic survey, it must first achieve effective avoidance of obstacles and drive to the target position. If the ship sails towards an obstacle, such as a rock, it will cause unnecessary losses to the unmanned ship and its important and expensive equipment. Therefore, the real-time path planning in the driving process is of great significance to make the unmanned ship move on the ideal course.
如图1所示，本发明提供了一种基于声呐的改进型模糊人工势场无人船避障方法，包括如下步骤：As shown in Fig. 1, the present invention provides a kind of improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar, comprising the following steps:
S1、对地面站上位机的数据进行初始化；S1. Initialize the data of the upper computer of the ground station;
S2、采用声呐测距模块获取无人船周围障碍物位置信息；所述的声呐测距模块采用TriTech微型机械扫描声呐，设置在无人船舶的船头，由无人船舶的电源模块供电。TriTech微型机械扫描声呐能够在360°范围内以700kHz的频率发射声波，声波的垂直宽度为35°，水平宽度为3°。S2. Use the sonar ranging module to obtain the location information of obstacles around the unmanned ship; the sonar ranging module uses TriTech micro-mechanical scanning sonar, is installed on the bow of the unmanned ship, and is powered by the power module of the unmanned ship. TriTech micro-mechanical scanning sonar is capable of emitting sound waves at a frequency of 700kHz within a 360° range, with a vertical width of 35° and a horizontal width of 3°.
步骤S2中的声呐测距模块获取无人船周围障碍物位置信息，如图2所示，声呐扫描过程具体为：The sonar ranging module in step S2 obtains the location information of obstacles around the unmanned ship, as shown in Figure 2, the sonar scanning process is specifically:
S21、设置无人船舶的运动方向为0°，使TriTech微型机械扫描声呐扫描发射声波后接收反射回来的声波；其扫描覆盖的角度范围为船前-50°到50°之间，则得出该声呐扫描的距离范围为0.3m到75m；S21. Set the direction of motion of the unmanned ship to 0°, so that the TriTech micro-mechanical scanning sonar scans the emitted sound waves and receives the reflected sound waves; the angle range covered by the scan is between -50° and 50° in front of the ship, then it is obtained The distance range of the sonar scan is 0.3m to 75m;
S22、按10°的步长转换角度再次发射并接收声波，得到无人船舶的船前一定范围内各目标点的位置信息和势场信息。S22. Transmit and receive the sound wave again by changing the angle according to the step length of 10°, and obtain the position information and potential field information of each target point within a certain range in front of the unmanned ship.
S3、基于改进型人工势场算法，计算合势场最小的航向角ψmin；本实施例中，选取半径3米处的11个点作为估算点。则：S3. Based on the improved artificial potential field algorithm, calculate the minimum course angle ψmin of the resultant potential field; in this embodiment, 11 points at a radius of 3 meters are selected as estimation points. but:
步骤S3中计算合势场最小的航向角ψmin的具体过程如下：The specific process of calculating the minimum course angle ψmin of the resultant potential field in step S3 is as follows:
S31、假设无人船在一未知海域的点P处，其空间坐标位置为[x,y]，驶向目标点D，其空间坐标位置为[xd,yd]，该海域分布有n个障碍物，其空间坐标位置分别为[x1,y1]...[xn,yn]，则无人船此时受到的引力势场函数Uatt的表达式为：S31. Assuming that the unmanned ship is at point P in an unknown sea area, its spatial coordinate position is [x, y], and it sails to the target point D, its spatial coordinate position is [xd , yd ], and there are n obstacles, whose spatial coordinates are [x1 , y1 ]...[xn , yn ], then the expression of the gravitational potential field function Uatt experienced by the unmanned ship at this time is:
式中，ka为引力系数，X为无人船当前位置[x,y]，Xd为目标点位置[xd,yd]；In the formula, ka is the gravitational coefficient, X is the current position of the unmanned ship [x, y], Xd is the position of the target point [xd , yd ];
S32、安装在无人船上的声呐当前位置周围没有障碍物时，使无人船舶沿着当前位置与目标点连线方向向目标点航行，定义此时航向角为ψd；S32. When there is no obstacle around the current position of the sonar installed on the unmanned ship, make the unmanned ship sail toward the target point along the direction of the line connecting the current position and the target point, and define the heading angle asψd at this time;
S33、安装在无人船上的声呐当前位置处于任意障碍物斥力影响范围内，则分析计算船前障碍物斥力影响范围处各个估算点的合势场强度；S33. The current position of the sonar installed on the unmanned ship is within the influence range of any obstacle repulsion, then analyze and calculate the resultant potential field strength of each estimation point at the influence range of the obstacle repulsion in front of the ship;
步骤S33中分析计算船前障碍物斥力影响范围处各个估算点的合势场强度的具体步骤如下：In step S33, the specific steps of analyzing and calculating the resultant potential field strength at each estimated point at the range of influence of the obstacle repulsion in front of the ship are as follows:
S331、如图3所示，定义上述步骤S33中估算点的数量为11，位置为M(i)，坐标为[xi,yi](i∈[1,11])，各个估算点代表的航向角为ψM(i)；S331, as shown in Figure 3, define the number of estimated points in the above step S33 as 11, the position as M(i), and the coordinates as [xi ,yi ](i∈[1,11]), each estimated point represents The heading angle of is ψM(i) ;
S332、目标点对无人船舶的引力势场方程为：S332. The gravitational potential field equation of the target point to the unmanned ship is:
S333、障碍物对无人船的斥力势场方程为：S333. The repulsion potential field equation of the obstacle to the unmanned ship is:
S334、结合步骤S332和步骤S333，得出M(i)处的合势场强度为：S334, in combination with step S332 and step S333, the resulting potential field strength at M(i) place is:
S34、各个估算点的合势场强度，得出合势场最小的估算点处的航向角ψmin。S34. The resultant potential field strength of each estimated point is obtained to obtain the heading angle ψmin at the estimated point where the resultant potential field is the smallest.
S4、基于改进型人工势场算法，计算无人船舶当前航向角与上述步骤S3中合势场最小的航向角ψmin之间的夹角ψfuzzy；S4, based on the improved artificial potential field algorithm, calculate the angle ψfuzzy between the current course angle of the unmanned ship and the minimum course angle ψmin of the resultant potential field in the above step S3;
S5、基于改进型人工势场算法，计算无人船舶当前位置所受斥力势场强度和引力势场强度之和的绝对值，进而再计算出其绝对值与斥力势场强度和引力势场强度中的绝对值较大值的比值Ufuzzy；S5. Based on the improved artificial potential field algorithm, calculate the absolute value of the sum of the repulsive potential field strength and the gravitational potential field strength at the current position of the unmanned ship, and then calculate its absolute value and the repulsive potential field strength and the gravitational potential field strength The ratio Ufuzzy in which the absolute value is larger;
S6、为使航向角的变化更加平滑，本发明在改进型人工势场的基础上加入模糊控制算法，本发明采用由三角形隶属函数构造的二维模糊控制模块；将步骤S4中计算得到的夹角ψfuzzy和步骤S5中计算得到的比值Ufuzzy分别作为二维模糊控制模块的第一输入变量和第二输入变量，将无人船舶下一步运动航向角与当前航向角之差θ作为二维模糊控制模块的输出量进行模糊化处理，按照模糊控制规则在matlab中进行计算，得到输出量θ；S6, in order to make the change of course angle smoother, the present invention adds fuzzy control algorithm on the basis of improved artificial potential field, the present invention adopts the two-dimensional fuzzy control module that is constructed by triangular membership function; The angle ψfuzzy and the ratio Ufuzzy calculated in step S5 are respectively used as the first input variable and the second input variable of the two-dimensional fuzzy control module. The output of the fuzzy control module is fuzzified, calculated in matlab according to the fuzzy control rules, and the output θ is obtained;
第一输入变量ψfuzzy＝ψ-ψmin，其论域为[-π/3,π/3],模糊集为A＝{NB,NM,NS,ZO,PS,PM,PB}；The first input variable ψfuzzy = ψ-ψmin , its domain is [-π/3,π/3], and the fuzzy set is A={NB,NM,NS,ZO,PS,PM,PB};
第二输入变量其论域为[0,1]，模糊集为B＝{NB,NM,NS,ZO,PS,PM,PB}；second input variable Its domain is [0,1], and the fuzzy set is B={NB, NM, NS, ZO, PS, PM, PB};
输出量θ，其论域为[-π/24,π/24],模糊集为C＝{NB,NM,NS,ZO,PS,PM,PB}。The output quantity θ, its discourse domain is [-π/24, π/24], and the fuzzy set is C={NB, NM, NS, ZO, PS, PM, PB}.
S7、将二维模糊控制模块的输出量与当前航向角相加得到无人船舶下一步航向角ψr，即ψr＝θ+ψ；S7. Add the output of the two-dimensional fuzzy control module to the current heading angle to obtain the next heading angle ψr of the unmanned ship, that is, ψr =θ+ψ;
S8、根据无人船舶下一步航向角ψr，确定无人船舶的下一步位置为S8. According to the next step heading angle ψr of the unmanned ship, determine the next step position of the unmanned ship as
作为本发明优选的实施方式，所述步骤S6中的模糊控制规则采用if A and Bthen C的形式构造。具体模糊规则如图4所示。As a preferred embodiment of the present invention, the fuzzy control rules in step S6 are constructed in the form of if A and Then C. The specific fuzzy rules are shown in Figure 4.
针对无人船在未知海域使用传统人工势场法进行避障时面临的目标不可达和局部极小值问题，本发明提出的一种使用机械扫描声呐结合模糊逻辑控制的改进型人工势场法，使用负倒数形式的引力势场函数，和包含无人船与目标点相对位置信息且为指数形式的斥力势场函数，通过对船前11个点位置的合势场强度进行分析处理，改善了传统人工势场的两个缺点问题，同时能够减小无人船航向角转角角度，平滑航行路径，使其更符合工程实际要求。如图5-10所示，仿真结果表明，应用以上方法进行实时避障，无人船能够避免目标不可达问题，及时跳出局部极小值点，在复杂的障碍物分布环境下快速平滑的避开障碍物到达目标区域。Aiming at the unreachable target and local minimum problems faced by unmanned ships when using the traditional artificial potential field method to avoid obstacles in unknown sea areas, this invention proposes an improved artificial potential field method using mechanical scanning sonar combined with fuzzy logic control , using the gravitational potential field function in the form of negative reciprocal and the repulsive potential field function in exponential form that contains the relative position information between the unmanned ship and the target point, by analyzing and processing the resultant potential field strength of the 11 points in front of the ship, the improvement The two shortcomings of the traditional artificial potential field are solved, and at the same time, it can reduce the heading angle of the unmanned ship, smooth the navigation path, and make it more in line with the actual engineering requirements. As shown in Figure 5-10, the simulation results show that by applying the above method for real-time obstacle avoidance, the unmanned ship can avoid the problem of unreachable targets, jump out of the local minimum point in time, and quickly and smoothly avoid the obstacles in a complex obstacle distribution environment. Open obstacles to reach the target area.
综上，本发明设计的无人船艇避障方法具有简单实用、应用广泛的特点。使用TriTech微型机械扫描声呐探测障碍物使其具有低成本低功耗的优点，使用人工势场法作为基础使其原理简单易懂，结合模糊控制后使无人船航向角变化趋势平滑。In summary, the obstacle avoidance method for unmanned boats designed by the present invention has the characteristics of simplicity, practicality and wide application. The use of TriTech micro-mechanical scanning sonar to detect obstacles has the advantages of low cost and low power consumption. The artificial potential field method is used as the basis to make the principle simple and easy to understand. Combined with fuzzy control, the course angle of the unmanned ship can be smoothed.
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的范围。Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: it can still be Modifications are made to the technical solutions described in the foregoing embodiments, or equivalent replacements are made to some or all of the technical features; these modifications or replacements do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910817350.XACN110471425B (en) | 2019-08-30 | 2019-08-30 | Improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910817350.XACN110471425B (en) | 2019-08-30 | 2019-08-30 | Improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar |
| Publication Number | Publication Date |
|---|---|
| CN110471425Atrue CN110471425A (en) | 2019-11-19 |
| CN110471425B CN110471425B (en) | 2022-03-29 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910817350.XAExpired - Fee RelatedCN110471425B (en) | 2019-08-30 | 2019-08-30 | Improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar |
| Country | Link |
|---|---|
| CN (1) | CN110471425B (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111830978A (en)* | 2020-07-07 | 2020-10-27 | 大连海事大学 | An obstacle avoidance path planning and control method and system for an underactuated unmanned boat |
| CN112835349A (en)* | 2019-11-22 | 2021-05-25 | 中国科学院沈阳自动化研究所 | A Path Planning Method for Autonomous Underwater Robot Based on Fuzzy Logic |
| CN115220444A (en)* | 2022-06-28 | 2022-10-21 | 中国科学院自动化研究所 | An obstacle avoidance control method for underwater robots based on fuzzy artificial potential field technology |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120232739A1 (en)* | 2009-11-20 | 2012-09-13 | Keio University | Autonomous mobile body and control method of same |
| CN105629974A (en)* | 2016-02-04 | 2016-06-01 | 重庆大学 | Robot path planning method and system based on improved artificial potential field method |
| CN108614561A (en)* | 2018-05-31 | 2018-10-02 | 重庆大学 | A kind of Artificial Potential Field barrier-avoiding method suitable for omnidirectional's wheel mobile robot |
| CN109945873A (en)* | 2019-04-04 | 2019-06-28 | 东南大学 | A hybrid path planning method for motion control of indoor mobile robots |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120232739A1 (en)* | 2009-11-20 | 2012-09-13 | Keio University | Autonomous mobile body and control method of same |
| CN105629974A (en)* | 2016-02-04 | 2016-06-01 | 重庆大学 | Robot path planning method and system based on improved artificial potential field method |
| CN108614561A (en)* | 2018-05-31 | 2018-10-02 | 重庆大学 | A kind of Artificial Potential Field barrier-avoiding method suitable for omnidirectional's wheel mobile robot |
| CN109945873A (en)* | 2019-04-04 | 2019-06-28 | 东南大学 | A hybrid path planning method for motion control of indoor mobile robots |
| Title |
|---|
| SUN JIAYI，等: "Collision Avoidance for Cooperative UAVs With Optimized Artificial Potential Field Algorithm", 《IEEE ACCESS》* |
| XINPING. GU，等: "Intelligent Vehicle Path Planning Based on Improved Artificial Potential Field Algorithm", 《2019 INTERNATIONAL CONFERENCE ON HIGH PERFORMANCE BIG DATA AND INTELLIGENT SYSTEMS (HPBD&IS)》* |
| 仇恒坦: "基于多传感信息融合的移动机器人SLAM应用与算法研究", 《中国优秀博硕士学位论文全文数据库（硕士）工程科技II辑》* |
| 李昀澄: "基于双红外视觉的UUV水面避碰方法研究", 《中国优秀博硕士学位论文全文数据库（硕士）工程科技II辑》* |
| 林奕雪: "基于声呐和机器视觉技术的水下机器人避障与目标跟踪研究", 《中国优秀博硕士学位论文全文数据库（硕士）信息科技辑》* |
| 游文洋,等: "基于模糊改进人工势场法的机器人避障方法研究", 《传感器与微系统》* |
| 陈呈: "基于改进栅格法和人工势场法的无人艇路径规划研究", 《中国优秀博硕士学位论文全文数据库（硕士）工程科技II辑》* |
| 黄开启,等: "模糊改进的人工势场法机器人局部路径规划", 《组合机床与自动化加工技术》* |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112835349A (en)* | 2019-11-22 | 2021-05-25 | 中国科学院沈阳自动化研究所 | A Path Planning Method for Autonomous Underwater Robot Based on Fuzzy Logic |
| CN112835349B (en)* | 2019-11-22 | 2022-01-11 | 中国科学院沈阳自动化研究所 | A Path Planning Method for Autonomous Underwater Robot Based on Fuzzy Logic |
| CN111830978A (en)* | 2020-07-07 | 2020-10-27 | 大连海事大学 | An obstacle avoidance path planning and control method and system for an underactuated unmanned boat |
| CN115220444A (en)* | 2022-06-28 | 2022-10-21 | 中国科学院自动化研究所 | An obstacle avoidance control method for underwater robots based on fuzzy artificial potential field technology |
| Publication number | Publication date |
|---|---|
| CN110471425B (en) | 2022-03-29 |
| Publication | Publication Date | Title |
|---|---|---|
| CN109933067B (en) | Unmanned ship collision avoidance method based on genetic algorithm and particle swarm algorithm | |
| CN105589464B (en) | A UUV Dynamic Obstacle Avoidance Method Based on Velocity Obstacle Method | |
| Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
| CN108469828A (en) | A kind of AUV Route planners improving artificial potential field optimization algorithm | |
| CN110837255B (en) | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship | |
| CN111830978A (en) | An obstacle avoidance path planning and control method and system for an underactuated unmanned boat | |
| CN111487986A (en) | A collaborative target search method for underwater robots based on global information transfer mechanism | |
| CN110471425A (en) | A kind of improved Fuzzy Artificial Potential Field unmanned boat barrier-avoiding method based on sonar | |
| CN107942095B (en) | Wave glider actual heading prediction technique | |
| Chen et al. | Risk-aware trajectory sampling for quadrotor obstacle avoidance in dynamic environments | |
| Cai et al. | Long-range UWB positioning-based automatic docking trajectory design for unmanned surface vehicle | |
| CN117192569B (en) | A navigation control method for lane inspection robots | |
| US12091137B1 (en) | Intelligent assistance system and method for berthing and unberthing based on multi-tugboat collaboration | |
| Ahmed et al. | An energy efficient IoD static and dynamic collision avoidance approach based on gradient optimization | |
| CN109916400B (en) | An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method | |
| Lan et al. | Based on Deep Reinforcement Learning to path planning in uncertain ocean currents for Underwater Gliders | |
| Qu et al. | USV path planning under marine environment simulation using DWA and safe reinforcement learning | |
| Zhang et al. | Path planning of USV in confined waters based on improved A∗ and DWA fusion algorithm | |
| CN120178923A (en) | Underwater thruster intelligent obstacle avoidance system and control method thereof | |
| CN118936468A (en) | UUV path planning method in complex environment based on target attraction law adaptive particle swarm optimization algorithm | |
| Zhang et al. | Local path planning of unmanned underwater vehicle based on improved APF and rolling window method | |
| Chen et al. | Path planning of underwater terrain-aided navigation based on improved artificial potential field method | |
| Bykova et al. | Safe navigation algorithm for autonomous underwater vehicles | |
| Calado et al. | Obstacle avoidance using echo sounder sonar | |
| CN116766187A (en) | An environment-adaptive allosteric method for amphibious robots |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee | Granted publication date:20220329 |