A method and device for detecting stuck position and escaping from a water surface robot

Abstract

The present invention relates to a method and device for detecting and escaping a stuck position of a water surface robot, the method comprising: obtaining navigation data of the water surface robot during navigation; filtering the navigation data to obtain processed data; judging whether the acceleration data in the processed data exceeds the corresponding value in the motion state; if it exceeds, judging whether the speed data is lower than the corresponding value of the current throttle; if it is lower, the water surface robot is stuck and an escape instruction is triggered; detecting and obtaining the stuck position direction of the water surface robot; obtaining the absolute coordinates of the virtual obstacle and the absolute coordinates of the stuck position; the water surface robot performs a predetermined action according to a predetermined action library to obtain the current position coordinates; judging whether the distance value between the current position coordinates and the absolute coordinates of the stuck position is greater than the set distance value; if so, the water surface robot is successfully escaped. The present invention can detect the collision and stuck position of the water surface robot and the obstacle, effectively complete the escape, and realize the subsequent early avoidance.

Description

Technical Field

[0001] The invention relates to the technical field of positioning of a water surface robot, and in particular to a method and a device for detecting positioning and escaping from an obstacle of a water surface robot. Background Art

[0002] In recent years, with the rapid development of unmanned driving technology, the unmanned driving market has continued to grow. Taking unmanned ships as an example, various surface robots have gradually been promoted to various operating scenarios such as water quality testing, surface maintenance, bridge inspection, water body mapping, maritime search and rescue, etc., to assist people in their work. The premise for unmanned ships to operate effectively is to ensure stable operation. Generally, unmanned driving sensors can often effectively detect environmental information on the water surface for effective obstacle avoidance, but cannot effectively detect and avoid underwater obstacles. Large ships are often equipped with sonar for underwater detection, but such sensors are highly professional and expensive, which is not conducive to the use of small surface ships. Summary of the invention

[0003] The purpose of the present invention is to overcome the deficiencies of the prior art and to provide a method and device for detecting a stuck position and escaping from an underwater robot.

[0004] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0005] A method for detecting a stuck position and escaping a water surface robot comprises the following steps:

[0006] Obtain navigation data of the surface robot during navigation;

[0007] Performing filtering on the navigation data to obtain processed data;

[0008] Determine whether the acceleration data in the processed data exceeds the corresponding value in the motion state;

[0009] If it exceeds, determine whether the speed data is lower than the corresponding value of the current throttle;

[0010] If it is lower than that, the surface robot will be stuck and the escape command will be triggered;

[0011] According to the escape command, the positioning direction of the surface robot is detected;

[0012] A virtual obstacle coordinate system is established according to the size and positioning direction of the surface robot, and then the virtual obstacle is transformed from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and the absolute coordinates of the positioning position are measured by the positioning system at the positioning moment;

[0013] According to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, the surface robot performs a predetermined action according to the predetermined action library to obtain the current position coordinates;

[0014] Determine whether the distance between the current position coordinates and the absolute coordinates of the card position is greater than the set distance value;

[0015] If so, the surface robot escapes successfully.

[0016] Its further technical solution is: before the step of obtaining the navigation data of the surface robot during navigation, it also includes: completing the power calibration of the surface robot.

[0017] Its further technical solution is: the power calibration of the surface robot includes: setting a specific route section so that the ship completes the set action, and making the ship complete the set action in sequence by remote control or autonomous means, and at the same time using the ship-borne inertial navigation to record the ship operation data, and performing data cleaning and low-pass filtering on the ship operation data to obtain the data cleaning result, and obtaining the steady-state data of the ship's motion process according to the data cleaning result, and then changing the control throttle input several times to obtain the final calibration table, that is, the ship power calibration.

[0018] A further technical solution is: in the step of filtering the navigation data to obtain processed data, gravity offset and high-frequency noise filtering are performed on the navigation data to obtain processed data.

[0019] Its further technical solution is: if so, then after the step of successfully escaping the surface robot, it also includes: when the surface robot subsequently sails, real-time detection of the distance between the current position and the absolute coordinates of the virtual obstacle, if the distance is less than 2-3 times the length of the surface robot and the bow is facing the virtual obstacle, obstacle avoidance operation is performed.

[0020] A stuck position detection and escape device for a water surface robot, comprising: an acquisition unit, a processing unit, a first judgment unit, a second judgment unit, a trigger unit, a detection unit, a building unit, an execution unit and a third judgment unit;

[0021] The acquisition unit is used to acquire navigation data of the surface robot during navigation;

[0022] The processing unit is used to filter the navigation data to obtain processed data;

[0023] The first judgment unit is used to judge whether the acceleration data in the processed data exceeds the corresponding value in the motion state;

[0024] The second judgment unit is used to judge whether the speed data is lower than the corresponding value of the current throttle;

[0025] The trigger unit is used to trigger an escape instruction;

[0026] The detection unit is used to detect the stuck direction of the water surface robot according to the escape instruction;

[0027] The establishing unit is used to establish a virtual obstacle coordinate system according to the size and positioning direction of the water surface robot, and then transform the virtual obstacle from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and obtain the absolute coordinates of the positioning position by measuring the positioning system at the positioning moment;

[0028] The execution unit is used to execute a predetermined action according to the predetermined action library of the surface robot according to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, so as to obtain the current position coordinates;

[0029] The third judgment unit is used to judge whether the distance value between the current position coordinates and the absolute coordinates of the stuck position is greater than a set distance value.

[0030] Its further technical solution is: it also includes: a calibration unit, which is used to complete the power calibration of the surface robot.

[0031] Its further technical solution is: the power calibration of the surface robot includes: setting a specific route section so that the ship completes the set action, and making the ship complete the set action in sequence by remote control or autonomous means, and at the same time using the ship-borne inertial navigation to record the ship operation data, and performing data cleaning and low-pass filtering on the ship operation data to obtain the data cleaning result, and obtaining the steady-state data of the ship's motion process according to the data cleaning result, and then changing the control throttle input several times to obtain the final calibration table, that is, the ship power calibration.

[0032] A further technical solution is: in the processing unit, gravity offset and high-frequency noise filtering are performed on the navigation data to obtain processed data.

[0033] Its further technical solution is: it also includes: an obstacle avoidance unit, which is used to detect the distance between the current position and the absolute coordinates of the virtual obstacle in real time when the surface robot is sailing subsequently. If the distance is less than 2-3 times the length of the surface robot and the bow is facing the virtual obstacle, an obstacle avoidance operation is performed.

[0034] Compared with the prior art, the beneficial effects of the present invention are: it can effectively detect the collision and blocking position between the water surface robot and the obstacle, effectively complete the escape, and when the water surface robot drives to this place next time, it can give an early warning and bypass the blocking point, and realize subsequent early avoidance.

[0035] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0037] Figure 1 A schematic flow chart of a method for detecting a stuck position and escaping a water surface robot provided by an embodiment of the present invention;

[0038] Figure 2 A schematic diagram showing the power increase and time of a water surface robot provided by an embodiment of the present invention;

[0039] Figure 3 A schematic block diagram of a stuck position detection and escape device for a water surface robot provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0040] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0041] It should be understood that when used in this specification and the appended claims, the terms "include" and "comprises" indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof.

[0042] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification of the present invention and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

[0043] It should be further understood that the term "and / or" used in the present description and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0044] In this embodiment, the surface robot is an unmanned vessel.

[0045] See also Figures 1 to 3 The specific embodiment shown, wherein please refer to Figure 1 to Figure 2 As shown, the present invention discloses a method for detecting a stuck position and escaping from a water surface robot, comprising the following steps:

[0046] S1, obtaining navigation data of the surface robot during navigation;

[0047] Among them, in this embodiment, before step S1, it also includes: completing the power calibration of the water surface robot.

[0048] Among them, the unmanned ship can complete specific route actions through remote control or autonomous navigation, and record the shipboard inertial navigation data and throttle command data at the same time, and then clean the data offline to realize the ship power calibration. Specifically, it includes:

[0049] a. Set a specific route section so that the ship can complete the following 8 actions: stationary to maximum speed forward, stationary to maximum speed backward, stationary to maximum angular velocity left turn, stationary to maximum angular velocity right turn, stationary to maximum angular velocity left turn and maximum speed forward, stationary to maximum angular velocity left turn and maximum speed backward, stationary to maximum angular velocity right turn and maximum speed forward, stationary to maximum angular velocity right turn and maximum speed backward; the acquisition time length should be more than twice the power rise time tp, such as Figure 2 shown.

[0050] b. Make the ship complete the above 8 actions in sequence through remote control or autonomous means, and use the ship-borne inertial navigation to record the ship's running speed v, angular velocity w, acceleration [ax, ay, az], Euler angles [yaw, pitch, roll], time t, straight throttle command thrust, and turning throttle command dire.

[0051] c. Since the acceleration recorded by the shipborne inertial navigation system includes gravity acceleration and high-frequency noise, it is necessary to perform data cleaning and low-pass filtering. The data cleaning process is to project the gravity acceleration in the inertial system to the ship system through Euler angle conversion to offset the constant gravity acceleration. The specific formula is as follows:

[0052] a2=a1+R world2ship ·g

[0053] Where a2 is the value of the three acceleration components after eliminating gravity, and a1 = [ax, ay, az]. world2ship Determined by the Euler angle rotation order set by the shipborne inertial navigation, the common zyx rotation order is:

[0054]

[0055]

[0056] in,

[0057]

[0058] After completing the gravity compensation, since the acceleration still contains a lot of high-frequency noise, it needs to be low-pass filtered. The filter uses Butterworth low-pass filtering, and the cut-off frequency is set to 50 Hz.

[0059] d. According to the data cleaning results, the dynamic acceleration time ti, steady-state speed vi, steady-state angular velocity wi, and acceleration / deceleration maximum acceleration ai of the ship's motion process can be obtained. Thus, the time t_max=max{ti}, v_min=min{vi}, w_min=min{wi}, ax_max=max{axi}, ax_max=max{axi}, ax_max=max{axi}.

[0060] e. Change the control throttle input, gradually reduce the straight throttle thrust and the turning throttle dire by 10% respectively, repeat steps b to d, and form a three-dimensional calibration table. The calibration table input is two throttle command inputs, and the output is the response of speed and angular velocity changing with time. At the same time, the maximum acceleration a_max of the corresponding process can also be obtained, forming the final calibration table, that is, the ship power calibration.

[0061] Among them, in step S1, the navigation data is monitored in real time, and the navigation data includes two sets of data: 1. Shipborne inertial navigation acceleration data for collision detection; 2. Throttle and speed matching data for secondary detection.

[0062] S2, filtering the navigation data to obtain processed data;

[0063] Among them, in this embodiment, gravity offset and high-frequency noise filtering are performed on the navigation data in real time by adopting the above-mentioned method to obtain processed data. Whether the amplitude of the acceleration data exceeds the corresponding value in the motion state, that is, a_max, is detected in real time. If it occurs, the time t is recorded. When it is detected that the amplitude of the acceleration data exceeds a_max, the sequence of the straight throttle thrust and the speed v, as well as the sequence of the turning throttle dire and the turning angular velocity w are recorded from time t; the sequence is compared with the calibration table. If the speed v or the angular velocity w is much less than 50% of the calibration value (for example: the speed v or the angular velocity w is 10% or 20% of the calibration value, etc.), or the speed v or the angular velocity w is close to zero, it can be determined that the ship has been stuck.

[0064] S3, determining whether the acceleration data in the processed data exceeds the corresponding value in the motion state; if not, it indicates that the ship is not stuck, and returning to step S1;

[0065] In this embodiment, the corresponding value in the motion state is a_max.

[0066] S4, if it exceeds, determine whether the speed data is lower than the corresponding value of the current throttle; if it is not lower, it indicates that the ship is not stuck, and return to execute step S1;

[0067] In this embodiment, the corresponding value of the current throttle is that the speed v or the angular velocity w is much less than 50% of the calibration value, or the speed v or the angular velocity w is close to zero.

[0068] S5: If it is lower than that, the surface robot will be stuck and the escape command will be triggered;

[0069] S6, according to the escape instruction, detecting and obtaining the stuck direction of the surface robot;

[0070] In this embodiment, by detecting whether the lateral acceleration of the ship is a negative value, it is obtained that the blocking direction is the left front blocking, the front blocking or the right front blocking.

[0071] S7, establishing a virtual obstacle coordinate system according to the size and positioning direction of the underwater robot, and then transforming the virtual obstacle from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and measuring the absolute coordinates of the positioning position through the positioning system at the positioning moment;

[0072] Among them, after the ship is stuck, a virtual obstacle of fixed size is defined according to the size of the ship, the direction of the ship's blocking is analyzed according to the ship-borne inertial navigation data (lateral acceleration), and a virtual obstacle is established in the environment map according to the bow direction and the ship's positioning point, including:

[0073] f. A virtual obstacle template of a fixed size is pre-set. The template is set as a rectangle of length×width. In this embodiment, the template size is set to 80% of the ship size.

[0074] g. Determine whether the collision occurs on the left or right side of the ship based on the direction of the lateral acceleration at time t obtained above. The judgment method is to judge the direction represented by the sign of the lateral acceleration and the direction of the shipborne inertial navigation installation. If it is greater than zero, it means that the collision occurs in the opposite direction of the shipborne inertial navigation installation. Otherwise, it means that the collision occurs in the direction of the shipborne inertial navigation installation.

[0075] h. According to the positioning coordinates (x, y) and the bow direction angle yaw of the ship when it is stuck, as well as the size and stuck direction of the virtual obstacle obtained in f and g, the coordinates of the four vertices of the virtual obstacle in the environment map can be calculated, as follows:

[0076]

[0077] Among them, x 1i ,y 1i is the vertex coordinate of the original rectangle, x 2i ,y 2i The coordinates of the vertices of the transformed rectangle.

[0078] i. Convert the relative coordinates of the four vertices of the rectangle into absolute coordinates, where x 2i ,y 2i is the relative coordinate in the north-east coordinate system. According to the conversion relationship between the north-east coordinate system and longitude and latitude, the four vertices of the rectangle are converted into absolute coordinates, and the virtual obstacle is added to the corresponding environment map. The conversion relationship between the north-east coordinate system and longitude and latitude is as follows:

[0079]

[0080]

[0081]

[0082] Among them, lat0, lng0 are the longitude and latitude corresponding to the origin of the northeast coordinate system, North and West correspond to the northeast coordinates, that is, relative coordinates, and Re represents the radius of the earth.

[0083] The absolute coordinates of the virtual obstacle are obtained by the above calculation, and the absolute coordinates of the blocking position are obtained by positioning the ship-borne positioning system at the blocking moment. The positioning system is a positioning system of existing publicly available technology.

[0084] S8, according to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, the surface robot performs a predetermined action according to the predetermined action library to obtain the current position coordinates;

[0085] When the absolute coordinates of the virtual obstacle are established, it is necessary to execute the actions pre-set in the action library according to the pre-set fixed action library. The settings of the pre-set action library are as follows:

[0086] Usually, when a ship is partially stuck, it can be freed by shaking the ship forward and backward or left and right; therefore, the following action flow can be set: drive the ship with 80% forward throttle for t1 time, then drive the ship with 80% reverse throttle for t1 time, then drive the ship with 80% left turn throttle for t2 time, then drive the ship with 80% right turn throttle for t2 time. The action is completed according to the action instructions set in the action library, and the distance between the current position of the ship and the position of the stuck position is detected when each sub-action is completed. When the distance difference between the two is greater than the set distance value, it is considered that the escape is successful, otherwise continue to the next predetermined action, and repeat the execution in the action library.

[0087] In this embodiment, the distance value is set to be greater than 50% of the length of the water surface robot.

[0088] Among them, when the ship's escape time is greater than the set time value (the meaning of the set time here is to prevent the ship from being completely stuck and causing the ship to perform the escape action for a long time, which is generally set to 40 times the action time t1+t2), it is determined that the escape has failed. At this time, the escape task is suspended and a prompt is given through the control end (such as a tablet, computer, etc.) to facilitate manual rescue.

[0089] S9, determining whether the distance between the current position coordinates and the absolute coordinates of the stuck position is greater than a set distance value; if so, the surface robot is successfully freed;

[0090] Among them, after step S9, it also includes: S10, if not, judging whether the time for the surface robot to perform the predetermined action exceeds the set time value; if so, the surface robot fails to escape; if not, returning to execute step S8.

[0091] After the ship is successfully freed, it plans its route based on the current location coordinates and the environment map containing the established virtual obstacles, including:

[0092] When the ship is freed from the difficulty, path planning is performed according to the environment map containing virtual obstacles established in fi. Commonly used planning algorithms include but are not limited to: VFH algorithm, A * Algorithm, D * Algorithms, etc.

[0093] After completing the route planning, the route is passed to the ship control module, so that the ship can complete automatic driving according to this route and complete the overall escape operation.

[0094] Among them, after step S9, it also includes: when the surface robot subsequently sails, real-time detection of the distance between the current position and the absolute coordinates of the virtual obstacle, if the distance is less than 2-3 times the length of the surface robot, and the bow is facing the virtual obstacle, obstacle avoidance operation is performed, which is efficient and convenient.

[0095] Specifically, the biggest role of virtual obstacles is that the next time a ship arrives near the area, it will know in advance that there has been a blockage here, so it can avoid it in advance. The specific process is as follows:

[0096] If yes, the surface robot escapes successfully. k ,y k )Upload to the server.

[0097] When the surface robot is not in the stuck state, the following two conditions are judged:

[0098] Calculate the real-time position (x, y) of the surface robot and the absolute coordinates (x k , y k ) k If r k When it is less than 2-3 times the length of the surface robot, condition 1 is met, otherwise it is not met. The distance calculation formula is

[0099]

[0100] If condition 1 is met, it is determined whether the virtual obstacle is in the direction of the surface robot. The determination method is:

[0101] Calculate the angle α between the absolute coordinates of the virtual obstacle and the line connecting the real-time position (x, y) of the surface robot in the absolute coordinate system

[0102] α=arctan2(yy k ,xx k )

[0103] The robot's moving direction yaw is obtained in real time according to the ship-borne imu. If |α-yaw| is less than 45°, it is determined whether the virtual obstacle is in the moving direction of the surface robot; if so, condition 2 is met, otherwise it is not met.

[0104] If conditions 1 and 2 are met at the same time, it means that there is a virtual obstacle in front of the ship, and the ship enters the obstacle avoidance state to avoid the obstacle in advance.

[0105] The present invention is based on general shipborne sensor configuration, does not require additional sensors, and can complete underwater and nearshore obstacle detection at a low cost, achieve autonomous escape, and realize subsequent early avoidance.

[0106] See also Figure 3As shown, the present invention also discloses a stuck position detection and escape device for a water surface robot, comprising: an acquisition unit 10, a processing unit 20, a first judgment unit 30, a second judgment unit 40, a trigger unit 50, a detection unit 60, a building unit 70, an execution unit 80 and a third judgment unit 90;

[0107] The acquisition unit 10 is used to acquire navigation data of the surface robot during navigation;

[0108] The processing unit 20 is used to filter the navigation data to obtain processed data;

[0109] The first judging unit 30 is used to judge whether the acceleration data in the processed data exceeds the corresponding value in the motion state;

[0110] The second judgment unit 40 is used to judge whether the speed data is lower than the corresponding value of the current throttle;

[0111] The trigger unit 50 is used to trigger an escape instruction;

[0112] The detection unit 60 is used to detect the stuck direction of the water surface robot according to the escape instruction;

[0113] The establishing unit 70 is used to establish a virtual obstacle coordinate system according to the size and positioning direction of the water surface robot, and then transform the virtual obstacle from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and measure the absolute coordinates of the positioning position through the positioning system at the positioning moment;

[0114] The execution unit 80 is used to execute a predetermined action according to the predetermined action library of the surface robot according to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, so as to obtain the current position coordinates;

[0115] The third judgment unit 90 is used to judge whether the distance value between the current position coordinates and the absolute coordinates of the stuck position is greater than a set distance value.

[0116] The device further includes: a fourth judgment unit 100, which is used to judge whether the time for the surface robot to perform a predetermined action exceeds a set time value.

[0117] The device also includes: a calibration unit, which is used to complete the power calibration of the surface robot.

[0118] Among them, completing the power calibration of the surface robot includes: setting a specific route section so that the ship completes the set action, and making the ship complete the set action in sequence by remote control or autonomous means, and at the same time using the ship-borne inertial navigation to record the ship operation data, and performing data cleaning and low-pass filtering on the ship operation data to obtain the data cleaning result, and obtaining the steady-state data of the ship's motion process according to the data cleaning result, and then changing the control throttle input several times to obtain the final calibration table, that is, the ship power calibration.

[0119] Wherein, in the processing unit 20, gravity offset and high-frequency noise filtering are performed on the navigation data to obtain processed data.

[0120] The device also includes: an obstacle avoidance unit, which is used to detect the distance between the current position and the absolute coordinates of the virtual obstacle in real time when the surface robot is sailing. If the distance is less than 2-3 times the length of the surface robot and the bow is facing the virtual obstacle, an obstacle avoidance operation is performed.

[0121] It should be noted that technicians in the relevant field can clearly understand that the specific implementation process of the above-mentioned positioning detection and escape device and each unit of the surface robot can refer to the corresponding description in the aforementioned method embodiment, and for the convenience and conciseness of the description, it will not be repeated here.

[0122] The above embodiments are preferred implementation schemes of the present invention. In addition, the present invention may also be implemented in other ways. Any obvious replacement without departing from the concept of the present technical solution is within the protection scope of the present invention.

Claims

1. A method for detecting and escaping a stuck position of a water surface robot, characterized in that: The following steps are involved: Obtain navigation data of the surface robot during navigation; Performing filtering on the navigation data to obtain processed data; Determine whether the acceleration data in the processed data exceeds the corresponding value in the current motion state; If it exceeds, determine whether the speed data is lower than the corresponding value of the current throttle; If it is lower than that, the surface robot will be stuck and the escape command will be triggered; According to the escape command, the positioning direction of the surface robot is detected; A virtual obstacle coordinate system is established according to the size and positioning direction of the surface robot, and then the virtual obstacle is transformed from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and the absolute coordinates of the positioning position are measured by the positioning system at the positioning moment; According to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, the surface robot performs a predetermined action according to the predetermined action library to obtain the current position coordinates; Determine whether the distance between the current position coordinates and the absolute coordinates of the card position is greater than the set distance value; If yes, the surface robot escapes successfully; If so, after the step of the surface robot successfully escaping from the predicament, the method further includes: when the surface robot subsequently sails, real-time detection of the distance between the current position and the absolute coordinates of the virtual obstacle is performed, and if the distance is less than 2-3 times the length of the surface robot and the bow is facing the virtual obstacle, an obstacle avoidance operation is performed; Among them, the absolute coordinates of the virtual obstacle position (x k ,y k ) upload to the server; when the surface robot is not in a stuck state, the following two conditions are judged: Calculate the real-time position (x, y) of the surface robot and the absolute coordinates (x k ,y k ) k If r k When it is less than 2-3 times the length of the surface robot, condition 1 is established, otherwise it is not established; the distance calculation formula is: If condition 1 is met, it is determined whether the virtual obstacle is in the direction of the surface robot. The determination method is: Calculate the angle α between the absolute coordinates of the position corresponding to the virtual obstacle and the line connecting the real-time position (x, y) of the surface robot in the absolute coordinate system; α=arctan2(yy k ,xx k ); The robot's moving direction yaw is obtained in real time according to the ship-borne imu. If |α-yaw| is less than 45°, it is determined whether the virtual obstacle is in the moving direction of the surface robot; if so, condition 2 is established, otherwise it is not established; if conditions 1 and 2 are established at the same time, it means that there is a virtual obstacle in front of the surface robot, and the surface robot enters the obstacle avoidance state to avoid the obstacle in advance; Among them, after the surface robot is stuck, a virtual object of fixed size is defined according to the size of the surface robot, and then the blocking direction of the surface robot is analyzed according to the ship-borne inertial navigation data, and according to the bow direction and the surface robot positioning point, a virtual obstacle is established in the environmental map by combining the virtual object, the blocking direction, the bow direction and the surface robot positioning point.

2. A method for detecting and escaping a stuck position of a water surface robot according to claim 1, characterized in that: Before the step of obtaining the navigation data of the surface robot during navigation, the method further includes: completing the power calibration of the surface robot.

3. A method for detecting and escaping a stuck position of a water surface robot according to claim 2, characterized in that: The power calibration of the surface robot includes: setting a specific route section so that the ship completes the set action, and making the ship complete the set action in sequence by remote control or autonomous means, and recording the ship operation data by using the ship-borne inertial navigation, and performing data cleaning and low-pass filtering on the ship operation data to obtain the data cleaning result, and obtaining the steady-state data of the ship movement process according to the data cleaning result, and then changing the control throttle input several times to obtain the final calibration table, that is, the ship power calibration.

4. A method for detecting and escaping a stuck position of a water surface robot according to claim 1, characterized in that: In the step of filtering the navigation data to obtain processed data, gravity offset and high-frequency noise filtering are performed on the navigation data to obtain processed data.

5. A device for detecting and escaping a stuck position of a water surface robot, characterized in that: include: An acquisition unit, a processing unit, a first judgment unit, a second judgment unit, a trigger unit, a detection unit, a creation unit, an execution unit and a third judgment unit; The acquisition unit is used to acquire navigation data of the surface robot during navigation; The processing unit is used to filter the navigation data to obtain processed data; The first judgment unit is used to judge whether the acceleration data in the processed data exceeds the corresponding value in the current motion state; The second judgment unit is used to judge whether the speed data is lower than the corresponding value of the current throttle; The trigger unit is used to trigger an escape instruction; The detection unit is used to detect the stuck direction of the water surface robot according to the escape instruction; The establishing unit is used to establish a virtual obstacle coordinate system according to the size and positioning direction of the water surface robot, and then transform the virtual obstacle from the virtual obstacle coordinate system to the absolute coordinate system to obtain the absolute coordinates of the virtual obstacle, and obtain the absolute coordinates of the positioning position by measuring the positioning system at the positioning moment; The execution unit is used to execute a predetermined action according to the predetermined action library of the surface robot according to the absolute coordinates of the virtual obstacle and the absolute coordinates of the blocking position, so as to obtain the current position coordinates; The third judgment unit is used to judge whether the distance value between the current position coordinates and the absolute coordinates of the stuck position is greater than a set distance value; The device further comprises: an obstacle avoidance unit, which is used to detect the distance between the current position and the absolute coordinates of the virtual obstacle in real time when the surface robot is sailing, and to perform obstacle avoidance operation if the distance is less than 2-3 times the length of the surface robot and the bow is facing the virtual obstacle; Among them, the absolute coordinates of the virtual obstacle position (x k ,y k ) upload to the server; when the surface robot is not in a stuck state, the following two conditions are judged: Calculate the real-time position (x, y) of the surface robot and the absolute coordinates (x k ,y k ) k If r k When it is less than 2-3 times the length of the surface robot, condition 1 is established, otherwise it is not established; the distance calculation formula is: If condition 1 is met, it is determined whether the virtual obstacle is in the direction of the surface robot. The determination method is: Calculate the angle α between the absolute coordinates of the position corresponding to the virtual obstacle and the line connecting the real-time position (x, y) of the surface robot in the absolute coordinate system; α=arctan2(yy k ,xx k ); The robot's moving direction yaw is obtained in real time according to the ship-borne imu. If α-yaw|<45°, it is determined whether the virtual obstacle is in the moving direction of the surface robot; if so, condition 2 is established, otherwise it is not established; if conditions 1 and 2 are established at the same time, it means that there is a virtual obstacle in front of the surface robot, and the surface robot enters the obstacle avoidance state to avoid the obstacle in advance; Among them, after the surface robot is stuck, a virtual object of fixed size is defined according to the size of the surface robot, and then the blocking direction of the surface robot is analyzed according to the ship-borne inertial navigation data, and according to the bow direction and the surface robot positioning point, a virtual obstacle is established in the environmental map by combining the virtual object, the blocking direction, the bow direction and the surface robot positioning point.

6. The device for detecting and escaping a stuck position of a water surface robot according to claim 5, characterized in that: Also includes: The calibration unit is used to complete the power calibration of the surface robot.

7. The device for detecting and escaping a stuck position of a water surface robot according to claim 6, characterized in that: The power calibration of the surface robot includes: setting a specific route section so that the ship completes the set action, and making the ship complete the set action in sequence by remote control or autonomous means, and recording the ship operation data by using the ship-borne inertial navigation, and performing data cleaning and low-pass filtering on the ship operation data to obtain the data cleaning result, and obtaining the steady-state data of the ship movement process according to the data cleaning result, and then changing the control throttle input several times to obtain the final calibration table, that is, the ship power calibration.

8. The device for detecting and escaping a stuck position of a water surface robot according to claim 5, characterized in that: In the processing unit, gravity offset and high-frequency noise filtering are performed on the navigation data to obtain processed data.

Images