CN107816989B - Underwater robot course data processing method and device - Google Patents
Underwater robot course data processing method and device Download PDFInfo
- Publication number
- CN107816989B CN107816989B CN201710954848.1A CN201710954848A CN107816989B CN 107816989 B CN107816989 B CN 107816989B CN 201710954848 A CN201710954848 A CN 201710954848A CN 107816989 B CN107816989 B CN 107816989B
- Authority
- CN
- China
- Prior art keywords
- angle
- offset
- magnetic field
- course
- underwater robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a method and a device for processing course data of an underwater robot, wherein the method comprises the following steps: acquiring a course angle; acquiring an angle offset through an angular velocity sensor; calculating to obtain an offset angle according to the course angle and the angle offset; acquiring a magnetic field angle through a magnetic field induction angle sensor; detecting whether the difference value between the offset angle and the magnetic field angle is larger than a preset threshold value or not; and when the difference value between the offset angle and the magnetic field angle is larger than a preset threshold value, taking the offset angle as a course angle, and returning to the step of obtaining the angle offset through the angular speed sensor. When the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, the current underwater robot is greatly influenced by the magnetic field of the environment, the offset angle is used as a course angle, and the underwater robot is controlled to sail according to the course angle, so that the influence of the magnetic field of the environment on the course of the underwater robot can be reduced, the course control of the underwater robot is more stable, and the accuracy is higher.
Description
Technical Field
The invention relates to the technical field of underwater detection robot control, in particular to a method and a device for processing course data of an underwater robot.
Background
An underwater robot is also called an unmanned remote control submersible vehicle and is a limit operation robot working underwater. Underwater robots have become important tools for underwater operations because of the danger of the harsh underwater environment and the limited depth of human diving.
The course of the underwater robot is an important reference for motion control, and the current mainstream course sensor is a course sensor based on Kalman data fusion of an angular velocity sensor and a magnetic sensor. Such heading sensors are complex in algorithm and expensive.
In addition, when the robot approaches to hydraulic buildings such as a dam body and the like in the underwater operation process, the surrounding magnetic field can change greatly, and the course control stability of the underwater robot is poor due to the fact that the underwater strong magnetic field changes violently, when the course sensor is used in the underwater variable and strong magnetic field environment, on the other hand, the data updating frequency of a general magnetic sensor is low, the updating frequency is usually 1Hz to 5Hz, and the data updating period cannot meet the real-time requirement of a control system.
Disclosure of Invention
Therefore, it is necessary to provide a method and a device for processing course data of an underwater robot, aiming at the defects of poor course control stability, low precision and poor real-time performance caused by the fact that the course control of the traditional underwater robot is greatly influenced by the change of a magnetic field.
An underwater robot course data processing device comprises:
acquiring a course angle;
acquiring an angle offset through an angular velocity sensor;
calculating to obtain an offset angle according to the course angle and the angle offset;
acquiring a magnetic field angle through the magnetic field induction angle sensor;
detecting whether the difference value between the offset angle and the magnetic field angle is larger than a preset threshold value;
and when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, taking the offset angle as the course angle, and returning to the step of obtaining the angle offset through the angular speed sensor.
In one embodiment, the method further comprises:
and when the difference value between the offset angle and the magnetic field angle is smaller than or equal to the preset threshold value, taking the magnetic field angle as the course angle, and returning to the step of obtaining the angle offset through the angular speed sensor.
In one embodiment, the step of calculating an offset angle according to the heading angle and the angular offset further includes:
and controlling the underwater robot to sail according to the offset angle.
In one embodiment, the step of obtaining the angle offset by the angular velocity sensor comprises:
the angular offset is acquired by the acquired angular velocity sensor every first period.
In one embodiment, the step of calculating an offset angle according to the heading angle and the angular offset comprises:
and calculating to obtain an offset angle according to the course angle and the plurality of angle offsets.
In one embodiment, the step of obtaining the heading angle includes:
and acquiring the course angle through the magnetic field induction angle sensor.
An underwater robot course data processing device comprises: the system comprises a magnetic field induction angle sensor, an angular speed sensor and a course controller, wherein the magnetic field induction angle sensor and the angular speed sensor are connected with the course controller;
the angular velocity sensor is used for acquiring angular offset once every other first period;
the magnetic field induction angle sensor is used for acquiring a magnetic field angle once every other second period;
the course controller is used for obtaining the angle offset of the angular velocity sensor and the magnetic field angle of the magnetic field induction angle sensor, calculating to obtain an offset angle according to the angle offset and the magnetic field angle obtained in a second period, and taking the offset angle as the course angle when the difference value of the offset angle and the magnetic field angle in the next second period is larger than the preset threshold value.
In one embodiment, the second period is greater than the first period.
In one embodiment, the magnetic field induced angle sensor comprises an electronic compass.
In one embodiment, the angular velocity sensor comprises a fiber optic gyroscope.
According to the underwater robot course data processing method and device, after the course angle is obtained, the course angle is calculated according to the angle offset of the angular speed sensor to obtain the offset angle, the course angle of the underwater robot is obtained in real time, the magnetic field angle is obtained through the magnetic field induction angle sensor, when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, the fact that the current underwater robot is greatly influenced by the magnetic field of the environment is shown, the offset angle is taken as the course angle, the underwater robot is controlled to sail according to the course angle, therefore, the influence of the magnetic field of the environment on the course of the underwater robot can be reduced, the course control of the underwater robot is more stable, and the accuracy is higher.
Drawings
FIG. 1 is a schematic flow chart of a method for processing heading data of an underwater robot according to an embodiment;
FIG. 2 is a schematic block diagram of a heading data processing apparatus of an underwater robot according to an embodiment;
fig. 3 is a schematic diagram of a work cycle of each element of the underwater robot heading data processing device according to an embodiment.
Detailed Description
To facilitate an understanding of the invention, the invention will now be described more fully with reference to the accompanying drawings. Preferred embodiments of the present invention are shown in the drawings. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
For example, a method for processing heading data of an underwater robot includes: acquiring a course angle; acquiring an angle offset through an angular velocity sensor; calculating to obtain an offset angle according to the course angle and the angle offset; acquiring a magnetic field angle through the magnetic field induction angle sensor; detecting whether the difference value between the offset angle and the magnetic field angle is larger than a preset threshold value; and when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, controlling the underwater robot to sail by taking the offset angle as the course angle, and returning to the step of obtaining the angle offset through the angular speed sensor.
For example, an underwater robot heading data processing apparatus includes: the system comprises a magnetic field induction angle sensor, an angular speed sensor and a course controller, wherein the magnetic field induction angle sensor and the angular speed sensor are connected with the course controller; the angular velocity sensor is used for acquiring angular offset once every other first period; the magnetic field induction angle sensor is used for acquiring a magnetic field angle once every other second period; the course controller is used for obtaining the angle offset of the angular velocity sensor and the magnetic field angle of the magnetic field induction angle sensor, calculating to obtain an offset angle according to the angle offset and the magnetic field angle obtained in a second period, and controlling the underwater robot to sail by taking the offset angle as the course angle when the difference value between the offset angle and the magnetic field angle in the next second period is larger than the preset threshold value.
In the embodiment, after the course angle is obtained, the course angle is calculated according to the angle offset of the angular velocity sensor to obtain the offset angle, the course angle of the underwater robot is obtained in real time, the magnetic field angle is obtained through the magnetic field induction angle sensor, when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, the underwater robot is indicated to be greatly influenced by the magnetic field of the environment, the underwater robot is controlled to sail according to the course angle by taking the offset angle as the course angle, and therefore the influence of the magnetic field of the environment on the course of the underwater robot can be reduced, the course control of the underwater robot is enabled to be more stable, and the accuracy is higher.
In particular, the magnetic field induced angle sensor is used to induce a north pole direction of a magnetic field, for example comprising an electronic compass, to obtain an angle of the underwater robot. The angular velocity sensor is used for detecting the angular rate and angular deviation of the underwater robot, for example, the angular velocity sensor includes a gyroscope, for example, the angular velocity sensor includes a fiber optic gyroscope. In the following embodiments, an electronic compass and a fiber-optic gyroscope are further described as examples.
In one embodiment, as shown in fig. 1, there is provided a method for processing heading data of an underwater robot, including:
Specifically, the course angle is the actual angle of the current course of the underwater robot, the course controller takes the course angle as a navigation angle, and the underwater robot is controlled to navigate according to the course angle.
It should be noted that the heading angle may be a magnetic field angle obtained by the magnetic field induction angle sensor in an initial state, or an offset angle obtained by correcting the magnetic field angle by an angle offset of the angular velocity sensor.
And 104, acquiring an angle offset through the angular velocity sensor.
In this embodiment, the angular velocity sensor is an optical fiber gyro, the optical fiber gyro detects the angular velocity of the underwater robot in real time, and the detection of the angular velocity by the optical fiber gyro is obtained according to the self motion state of the underwater robot, rather than based on a magnetic field, so that the acquisition of the angular velocity by the optical fiber gyro is not affected by the magnetic field.
In the step, the angular velocity of the instant course of the underwater robot is detected through the angular velocity sensor, and the angle variation, namely the angle offset, is obtained through time integration, wherein the angle offset is a relative value, and reflects the angle variation of the course of the underwater robot in a period of time.
And 106, calculating to obtain an offset angle according to the course angle and the angle offset.
In this step, the current time and the angle offset obtained in step 102 are calculated to obtain an offset angle of the underwater robot, that is, the offset angle is a real-time course angle of the underwater robot calculated according to the angular velocity of the underwater robot. It can be understood that, since the underwater robot is moving or sailing continuously, the heading angle thereof will change in real time, and the offset angle is calculated according to the actual angle of the underwater robot and the angle offset, which is the current actual heading angle of the underwater robot.
For example, the offset angle is calculated from the sum of the heading angle and the angular offset.
It is worth mentioning that the offset angle can be used as a heading angle, and the heading controller performs heading control on the underwater robot according to the offset angle.
And step 108, acquiring a magnetic field angle through the magnetic field induction angle sensor.
In this embodiment, the magnetic field angle is obtained by an electronic compass, which is also called a digital compass. The electronic compass induces a magnetic field, and then detects a magnetic field angle. For example, the electronic compass has a north direction of zero degrees, and the obtained magnetic field angle ranges from 0 to 360 degrees. The electronic compass detects the angle of the underwater robot according to the magnetic field, wherein the angle is the magnetic field angle, and the magnetic field angle is an absolute value.
In the step, the offset angle and the magnetic field angle are compared, whether the difference value between the two is larger than a preset threshold value or not is detected, and when the difference value between the two is larger than the preset threshold value, the fact that the deviation between the real-time heading angle calculated according to the fiber-optic gyroscope and the magnetic field angle detected by the electronic compass is large indicates that the current electronic compass is greatly influenced by an environmental magnetic field is indicated.
It is worth mentioning that the ambient magnetic field is the magnetic field emitted by strong magnetic objects in the underwater environment, and not the magnetic field of the earth.
And 112, when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, taking the offset angle as the heading angle, and returning to the step 104.
Specifically, when the difference between the offset angle and the magnetic field angle is greater than the preset threshold, it indicates that the difference between the real-time heading angle calculated according to the fiber-optic gyroscope and the magnetic field angle detected by the electronic compass is large, and indicates that the current electronic compass is greatly influenced by the environmental magnetic field, and the magnetic field angle acquired by the electronic compass is inaccurate. Therefore, in the step, the deviation angle is calculated by using the angle deviation value acquired by the fiber-optic gyroscope in real time and is used as the course angle, and the underwater robot is controlled to sail according to the course angle.
In the embodiment, after the course angle is obtained, the course angle is calculated according to the angle offset of the angular velocity sensor to obtain the offset angle, the course angle of the underwater robot is obtained in real time, the magnetic field angle is obtained through the magnetic field induction angle sensor, when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, the underwater robot is indicated to be greatly influenced by the magnetic field of the environment, the underwater robot is controlled to sail according to the course angle by taking the offset angle as the course angle, and therefore the influence of the magnetic field of the environment on the course of the underwater robot can be reduced, the course control of the underwater robot is enabled to be more stable, and the accuracy is higher.
It should be understood that, in each embodiment, the controlling of the underwater robot to sail according to the heading angle may be directly controlling the underwater robot to sail according to the heading angle, or may be controlling the underwater robot to sail according to a control command based on the heading angle. Specifically, the heading controller controls the underwater robot to navigate according to the heading angle, the navigation can be performed by controlling the operation of the X-axis driver, the Y-axis driver and the Z-axis driver, the navigation control mode can be realized by adopting the prior art, and the description in the embodiment is not redundant.
In one embodiment, the method further comprises: and when the difference value between the offset angle and the magnetic field angle is smaller than or equal to the preset threshold value, taking the magnetic field angle as the course angle, and returning to the step of obtaining the angle offset through the angular speed sensor.
Specifically, when the difference between the offset angle and the magnetic field angle is smaller than or equal to a preset threshold, it indicates that the real-time heading angle calculated according to the fiber-optic gyroscope is closer to the magnetic field angle detected by the electronic compass, and it indicates that the current electronic compass is less affected by the environmental magnetic field. Therefore, the magnetic field angle is used as the course angle, and the underwater robot is controlled to sail according to the course angle.
It is worth mentioning that, because the angle offset obtained by the detection of the fiber-optic gyroscope is not affected by the magnetic field, the precise current heading angle can be calculated in real time according to the angle offset, the offset angle calculated by the angle offset of the fiber-optic gyroscope is more precise in a shorter time, and the error of the angle offset obtained by integrating the angular velocity increases along with the increase of the time, so that the detection error of the fiber-optic gyroscope on the angle offset in a long time can be reduced by detecting the magnetic field angle through the electronic compass and taking the magnetic field angle detected by the electronic compass as the heading angle.
In the above embodiment, when the difference between the offset angle and the magnetic field angle is less than or equal to the preset threshold, the magnetic field angle is taken as the heading angle, and when the difference between the offset angle and the magnetic field angle is greater than the preset threshold, the offset angle is taken as the heading angle, that is, the heading angle is updated by the angle offset detected by the fiber optic gyroscope when the influence of the environmental magnetic field is large, and the heading angle is updated by the magnetic field angle detected by the electronic compass when the influence of the environmental magnetic field is small, and the detection result of the fiber optic gyroscope and the detection result of the electronic compass are combined with each other, so that the influence of the environmental magnetic field on the heading angle of the underwater robot can be effectively reduced, and the deviation caused by long-time detection of the fiber optic gyroscope can be reduced.
It should be noted that the returning step in the above embodiment may be regarded as a loop, and one loop may be regarded as one period, in the period of one loop, the magnetic field angle is obtained once through the electronic compass, in the period of one loop, there are a plurality of small periods, each small period obtains an angle offset once through the fiber-optic gyroscope, and after a plurality of small periods, the heading angle is updated according to the angle offset.
In one embodiment, step 106 is followed by: and controlling the underwater robot to sail according to the offset angle.
In the embodiment, the angle offset is obtained by the fiber-optic gyroscope, the offset angle is calculated according to the course angle and the angle offset, and then the underwater robot is controlled to sail according to the offset angle, wherein the offset angle is the current real-time course angle, so that the underwater robot can sail according to the real-time course angle, and the course control precision of the underwater robot can be effectively improved.
In one embodiment, step 104 is to acquire the angular offset amount once every first period by the acquired angular velocity sensor.
Specifically, the update cycle of the angular offset amount of the fiber optic gyroscope is a first cycle, for example, the fiber optic gyroscope detects the angular velocity once every other first cycle, and updates the angular offset amount according to the angular velocity. For example, the update cycle of the angular velocity of the fiber optic gyroscope is 0.005 seconds, that is, the update frequency of the fiber optic gyroscope is 200Hz, and it should be understood that the higher the update frequency of the fiber optic gyroscope is, the closer the values of the angular velocities obtained in two adjacent times are, and therefore, the higher the detection accuracy of the angular displacement amount of the underwater robot is.
In one embodiment, step 106 is: and calculating to obtain an offset angle according to the course angle and the plurality of angle offsets.
Specifically, the calculation cycle of the offset angle is greater than the first cycle, that is, after the angle offsets are respectively obtained in the first cycles, when one calculation cycle is reached, the real-time course angle is calculated according to the course angle and the angle offsets, and the course angle is updated, so that the course angle is more accurate.
In one embodiment, step 102 comprises: and acquiring the course angle through the magnetic field induction angle sensor.
Specifically, the initial heading angle is the magnetic field angle detected by the electronic compass. For example, in the initial stage of the underwater robot, the initial heading angle of the underwater robot is detected through the electronic compass.
In this embodiment, the magnetic field angle detected by the electronic compass is taken as a heading angle, for example, in an initialization stage of the underwater robot, the magnetic field angle detected by the electronic compass is taken as a heading angle.
In one embodiment, the underwater robot heading data processing method further includes: and detecting the times that the difference value between the offset angle and the magnetic field angle is greater than a preset threshold, and increasing the preset threshold when the times that the difference value between the offset angle and the magnetic field angle is greater than the preset threshold is greater than the preset times. For example, the preset threshold is increased when the number of consecutive occurrences that the difference between the offset angle and the magnetic field angle is greater than a preset threshold is greater than a preset number, wherein the greater the number of consecutive occurrences that the difference between the offset angle and the magnetic field angle is greater than the preset threshold is.
Specifically, in this embodiment, after the step 112 is executed, the step 104 is returned to, so that the difference between the offset angle and the magnetic field angle is compared with the preset threshold value for a plurality of times in a plurality of cycle periods, when the difference between the offset angle and the magnetic field angle is greater than the preset threshold value as a result of the comparison between the difference and the preset threshold value, and the number of consecutive occurrences of the result is greater than the preset number, the preset threshold value is adjusted, and the larger the number of consecutive occurrences of the result is, the larger the preset threshold value is. Specifically, a preset threshold value compared with a difference value between a magnetic field angle acquired by the electronic compass and a deviation angle acquired by integrating the fiber-optic gyroscope is dynamically adjusted, the preset threshold value is dynamically adjusted according to the number of cycles of the electronic compass affected by the environmental magnetic field, and the larger the number of cycles affected continuously is, the larger the set threshold value is. It should be understood that the longer the period affected by the magnetic field, the greater the accumulated deviation obtained by the integration of the fiber optic gyroscope, if the heading controller continuously takes the offset angle of the fiber-optic gyroscope as the heading angle, the accumulated deviation of the integral of the fiber-optic gyroscope is continuously increased, if the heading angle is not corrected in time, the error is increased, so that the heading angle cannot be updated according to the magnetic field angle of the electronic compass, therefore, in the embodiment, when the number of times of detecting that the difference between the offset angle and the magnetic field angle is greater than the preset threshold is greater than the preset number of times, the preset threshold is increased so that the difference between the offset angle and the magnetic field angle can be made smaller than or equal to the preset threshold when the difference between the offset angle and the magnetic field angle of the next time is compared with the preset threshold, therefore, the course angle can be updated according to the magnetic field angle, and the accumulated deviation of the fiber-optic gyroscope can be effectively avoided.
In one embodiment, there is provided an underwater robot heading data processing apparatus including: the system comprises a magnetic field induction angle sensor 210, an angular velocity sensor 220 and a heading controller 230, wherein the magnetic field induction angle sensor 210 and the angular velocity sensor 220 are connected with the heading controller 230. The angular velocity sensor 220 is used to acquire the amount of angular displacement once every first period. The magnetic field induced angle sensor 210 is used to acquire the magnetic field angle once every second period.
The heading controller 230 is configured to obtain an angle offset of the angular velocity sensor 220 and a magnetic field angle of the magnetic field induction angle sensor 210, and the heading controller 230 is further configured to calculate an offset angle according to the angle offset and the magnetic field angle obtained in a second period, and when a difference between the offset angle and the magnetic field angle in a next second period is greater than the preset threshold, use the offset angle as the heading angle.
The heading controller 230 is further configured to use the magnetic field angle as the heading angle when a difference between the offset angle and the magnetic field angle in the next second period is smaller than or equal to the preset threshold. In this embodiment, the second period is greater than the first period.
Specifically, the course controller controls the underwater robot to navigate according to the course angle. For example, the magnetic field induced angle sensor is an electronic compass, for example, the angular velocity sensor is a fiber optic gyroscope.
In this embodiment, the underwater robot course data processing device obtains the magnetic field angle by inducing the magnetic field through the electronic compass, and performs navigation on the course angle according to the magnetic field angle. The electronic compass acquires the magnetic field angle once every second period, for example, the second period is 0.2 seconds, that is, the acquisition frequency of the electronic compass for the magnetic field angle is 5 Hz.
The underwater robot course data processing device obtains the angle offset through the optical fiber gyroscope, and updates the current course angle according to the angle offset. The first period is 0.005 second, namely the frequency of the fiber-optic gyroscope obtaining angle offset is 200 Hz. In a second period, a plurality of angle offsets may be obtained in the plurality of first periods, and the heading angle is updated according to the plurality of angle offsets, for example, if the heading angle is the magnetic field angle obtained in the second period, the updated heading angle is the sum of the magnetic field angle and the angle offset, and the sum of the magnetic field angle and the angle offset is the offset angle.
When the next second period comes, the electronic compass acquires the magnetic field angle again, the difference value between the offset angle and the magnetic field angle acquired in the next second period is compared, when the difference value between the offset angle and the magnetic field angle acquired in the next second period is larger than the preset threshold value, the electronic compass is influenced by the environmental magnetic field greatly, so that the deviation between the detection result of the electronic compass and the detection result of the optical fiber gyro is large, and because the second period time is short, the accumulated error of the angle offset continuously acquired by the optical fiber gyro in the second period is small, therefore, the offset angle acquired by calculating the angle offset detected by the optical fiber gyro is accurate, and therefore, the course controller takes the offset angle as the course angle, which is equivalent to updating the course angle, and controls the underwater robot to sail according to the course angle. And when the difference value between the offset angle and the magnetic field angle obtained in the next second period is smaller than or equal to the preset threshold value, the electronic compass is less influenced by the environmental magnetic field, therefore, the course controller takes the magnetic field angle obtained by the electronic compass as the course angle, which is equivalent to updating the course angle, and controls the underwater robot to sail according to the updated course angle.
It is worth mentioning that in a second period, the fiber optic gyroscope respectively obtains a plurality of angle offsets in a plurality of first periods, and respectively calculates and obtains a plurality of offset angles according to the plurality of angle offsets, and in a second period, the course controller respectively takes the plurality of offset angles as course angles, namely updates the course angles. That is to say, before the next second period does not come, the electronic compass does not obtain the next magnetic field angle, and then the course angle of the underwater robot is finely adjusted according to the offset angle, so that the navigation angle of the underwater robot is more accurate.
In one embodiment, the heading controller is further configured to increase the preset threshold when consecutive times that a difference between the offset angle and the magnetic field angle of the next second period is greater than the preset threshold are greater than preset times, wherein the greater the number of consecutive occurrences that the difference between the offset angle and the magnetic field angle of the next second period is greater than the preset threshold, the greater the preset threshold. Specifically, a preset threshold value compared with a difference value between a magnetic field angle acquired by the electronic compass and a deviation angle acquired by integrating the fiber-optic gyroscope is dynamically adjusted, the preset threshold value is dynamically adjusted according to the number of cycles of the electronic compass affected by electromagnetism, and the larger the number of cycles affected continuously is, the larger the set threshold value is. It should be understood that the longer the period affected by the magnetic field, the greater the accumulated deviation obtained by the integration of the fiber optic gyroscope, if the heading controller continuously takes the offset angle of the fiber-optic gyroscope as the heading angle, the accumulated deviation of the integral of the fiber-optic gyroscope is continuously increased, if the heading angle is not corrected in time, the error is increased, so that the heading angle cannot be updated according to the magnetic field angle of the electronic compass, therefore, in the embodiment, when the number of times of detecting that the difference between the offset angle and the magnetic field angle is greater than the preset threshold is greater than the preset number of times, the preset threshold is increased so that the difference between the offset angle and the magnetic field angle can be made smaller than or equal to the preset threshold when the difference between the offset angle and the magnetic field angle of the next time is compared with the preset threshold, therefore, the course angle can be updated according to the magnetic field angle, and the accumulated deviation of the fiber-optic gyroscope can be effectively avoided.
The following is a specific example:
in this embodiment, a combined manner of an electronic compass and a fiber-optic gyroscope is adopted to fuse the heading data of the underwater robot, so as to obtain stable heading data of the underwater robot, and a data updating period and an underwater robot control period of the electronic compass and the fiber-optic gyroscope are shown in fig. 3, wherein an updating period of the electronic compass is 5Hz, an updating period of the fiber-optic gyroscope is 200Hz, and a control period of the heading controller is 50 Hz. In fig. 3, T0 to T1 are update cycles of the electronic compass, T0 to T1 are divided into 40 cycles, i.e., T01, T02, T03 … … T039, each cycle is an update cycle of the fiber-optic gyroscope, and every four cycles are a control cycle of the heading controller.
Electronic compass: the updating period is 5Hz, the data output frequency is slow, the data output is the current magnetic field angle, the angle is an absolute value, the output range is 0-360 degrees, and the true north direction is zero degree. This magnetic field angle only is relevant with the magnetic field environment of measuring point, and is irrelevant with live time, can not produce the deviation along with the lapse of time, and long-time stability is better, and it is great that this magnetic field angle receives environmental magnetic field to influence, makes the magnetic field angle can produce great deviation when being close strong magnetic object, and instantaneous stability is not good.
A fiber optic gyroscope: the updating period is 200Hz, the data output frequency is high, the data output is the angular speed of the current heading, the angular speed is subjected to time integration to obtain the angle variation, namely the angular offset, and the angular offset is a relative value. The output angle of the fiber-optic gyroscope is not influenced by the surrounding magnetic field, the instantaneous stability is good, the accumulated deviation can be generated in the process of obtaining the angle offset by integrating the angular velocity, and the accumulated deviation is increased along with the increase of time, so that the long-term stability of the data of the fiber-optic gyroscope is poor, and the precision of the data is reduced along with the increase of time.
The control period of the course controller is 50Hz, the course angle is an important feedback control quantity of the course controller, a new course angle needs to be updated in each control period, otherwise, the control cycle is invalid.
In this embodiment, the course angle is updated by mutually combining the electronic compass and the fiber-optic gyroscope, the long-term stability mainly includes the electronic compass, and the instantaneous dynamic stability mainly includes the fiber-optic gyroscope, so that the following two functions are mainly completed:
(1) and eliminating the interference of the magnetic field in the working environment on the heading data.
(2) And in each updating period of the electronic compass, the data of the fiber-optic gyroscope is used for interpolating the updating data of the electronic compass so as to meet the requirement of a control system.
The course angle updating method based on the electronic compass and the fiber-optic gyroscope comprises the following steps:
1. and acquiring an initial magnetic field angle through an electronic compass, wherein the initial magnetic field angle is taken as a heading angle.
Initialization: under an open and non-strong magnetic environment, the current electronic compass value is obtained as the true value of the current course angle, and the true value is used as the integration starting point of the fiber-optic gyroscope.
2. In a second period, a plurality of angle offsets are obtained through the fiber-optic gyroscope in each first period, offset angles are respectively calculated and obtained according to the initial course angle and the plurality of angle offsets, and the course angle is updated by taking the offset angle as the course angle.
In each electronic compass updating period, as shown in a period from T0 to T1, integrating the angular speed from each fiber-optic gyroscope updating time T01 to T039 to obtain a relative angle D (T0m), i.e., a deviation angle, and the true value of the heading at each fiber-optic gyroscope data updating time is D (T0m) ═ D (T0) + D (T0 m); and each four updating periods of the fiber-optic gyroscope are a control period, namely D (T04), D (T08) and D (T12) … are heading control moments, the moments are synchronously controlled by the updating moments of the fiber-optic gyroscopes, and the heading controller controls the underwater robot to sail at the updated heading angle in each control period.
3. And updating the heading angle according to the magnetic field angle of the electronic compass and the offset angle of the optical fiber gyroscope when each second period comes.
And (3) performing mutual data correction on the electronic compass and the optical fiber gyro at the updating time of each electronic compass: at this moment, two heading angle values are obtained, one is the data read by the electronic compass (magnetic field angle), and the other is the integral angle of the fiber-optic gyroscope (offset angle) based on the last update moment of the electronic compass. And if the deviation between the two heading angles is within a set threshold value, the electronic compass is not influenced by a strong magnetic field at the moment, and the magnetic field angle of the electronic compass is taken as the true value of the heading angle at the moment. And if the deviation between the two heading angle data exceeds a set threshold value, the electronic compass is considered to be influenced by a strong magnetic field at the moment, and the deviation angle obtained by the integration of the fiber-optic gyroscope is taken as the true value of the heading angle at the moment. Whether the magnetic field angle of the electronic compass is used as the true value of the heading angle at the moment or the deviation angle obtained by integrating the fiber-optic gyroscope is used as the true value of the heading angle at the moment, the integral of the fiber-optic gyroscope in the next second period is integrated on the basis of the true value of the heading angle at the moment, namely the updated heading angle of the fiber-optic gyroscope in the next second period is integrated on the basis of the updated heading angle.
The course angle is updated by combining the electronic compass and the fiber-optic gyroscope, so that the course angle is more accurate, the influence of an environmental magnetic field on the course angle can be effectively avoided, and the condition that the accumulated deviation is larger due to the long-time integration of the fiber-optic gyroscope on one course angle is also avoided.
In this embodiment, the set threshold of the deviation between the compass value and the integral value of the fiber-optic gyroscope is dynamically adjusted according to the number of cycles of the electronic compass affected by the electromagnetism, and the set threshold is larger as the number of cycles affected continuously is larger.
In this embodiment, the integration of the fiber-optic gyroscope is performed only in the update period of one electronic compass, so that the accumulated deviation of the fiber-optic gyroscope is controlled in a small range, and the accuracy of the integral value can be ensured.
In addition, in the process of underwater robot operation, the time influenced by the strong magnetic field environment is not too long, most of the time is normal, and the long-time stability of course data is ensured by adopting the value of an electronic compass.
It should be noted that, in the above system embodiment, each included module is only divided according to functional logic, but is not limited to the above division as long as the corresponding function can be implemented; in addition, the specific names of the functional modules are only for convenience of distinguishing from each other and are not used for limiting the protection scope of the present invention.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (10)
1. A method for processing heading data of an underwater robot is characterized by comprising the following steps:
acquiring a course angle;
acquiring an angle offset through an angular velocity sensor; every other first period, acquiring the angle offset once through the acquired angular velocity sensor;
calculating to obtain an offset angle according to the course angle and the angle offset; calculating to obtain the offset angle according to the course angle and the plurality of angle offsets; calculating the offset angle according to the course angle and the angle offsets when the calculation period is up to one after the angle offsets are respectively obtained in the first periods, wherein the calculation period of the offset angle is longer than the first period, and the real-time course angle is obtained by calculating according to the course angle and the angle offsets;
acquiring a magnetic field angle through a magnetic field induction angle sensor;
detecting whether the difference value between the offset angle and the magnetic field angle is larger than a preset threshold value;
when the difference value between the offset angle and the magnetic field angle is larger than the preset threshold value, taking the offset angle as the course angle, and returning to the step of obtaining the angle offset through the angular velocity sensor; when the difference value between the offset angle and the magnetic field angle is smaller than or equal to the preset threshold value, taking the magnetic field angle as the course angle, and returning to the step of obtaining the angle offset through the angular speed sensor;
and detecting the times that the difference value between the offset angle and the magnetic field angle is greater than the preset threshold, and increasing the preset threshold when the times that the difference value between the offset angle and the magnetic field angle is greater than the preset threshold is greater than the preset times.
2. The underwater robot heading data processing method of claim 1, wherein the magnetic field induced angle sensor comprises an electronic compass and the angular velocity sensor comprises a fiber optic gyroscope.
3. The underwater robot heading data processing method of claim 1, wherein the step of calculating an offset angle from the heading angle and the angular offset further comprises, after the step of:
and controlling the underwater robot to sail according to the offset angle.
4. The underwater robot heading data processing method according to claim 1, wherein when the difference between the offset angle and the magnetic field angle is greater than the preset threshold, the step of taking the offset angle as the heading angle is:
and calculating an offset angle as a course angle by using the angle offset acquired in real time, and controlling the underwater robot to navigate according to the course angle.
5. The underwater robot heading data processing method as claimed in claim 1, wherein the step of calculating an offset angle from the heading angle and the angular offset is:
the angular velocity is detected every first period, and the angular offset is updated according to the angular velocity.
6. The underwater robot heading data processing method of claim 1, wherein the step of obtaining a heading angle comprises:
and acquiring the course angle through the magnetic field induction angle sensor.
7. An underwater robot course data processing device is characterized by comprising: the system comprises a magnetic field induction angle sensor, an angular speed sensor and a course controller, wherein the magnetic field induction angle sensor and the angular speed sensor are connected with the course controller;
the angular velocity sensor is used for acquiring angular offset once every other first period;
the magnetic field induction angle sensor is used for acquiring a magnetic field angle once every other second period;
the course controller is used for acquiring the angle offset of the angular velocity sensor and the magnetic field angle of the magnetic field induction angle sensor, calculating to obtain an offset angle according to the angle offset and the magnetic field angle acquired in a second period, and taking the offset angle as a course angle when the difference value between the offset angle and the magnetic field angle in the next second period is greater than a preset threshold value; when the difference value between the offset angle and the magnetic field angle in the next second period is smaller than or equal to a preset threshold value, taking the magnetic field angle as the course angle;
respectively acquiring a plurality of angle offsets in a plurality of first periods; calculating to obtain the offset angle according to the course angle and the plurality of angle offsets; calculating the offset angle according to the course angle and the angle offsets when the calculation period is up to one after the angle offsets are respectively obtained in the first periods, wherein the calculation period of the offset angle is longer than the first period, and the real-time course angle is obtained by calculating according to the course angle and the angle offsets; and detecting the times that the difference value between the offset angle and the magnetic field angle is greater than the preset threshold, and increasing the preset threshold when the times that the difference value between the offset angle and the magnetic field angle is greater than the preset threshold is greater than the preset times.
8. The underwater robot heading data processing apparatus of claim 7, wherein the second period is greater than the first period.
9. Underwater robot heading data processing apparatus according to claim 7, wherein the magnetic field induced angle sensor comprises an electronic compass.
10. Underwater robot heading data processing apparatus according to claim 7, wherein the angular velocity sensor comprises a fibre optic gyroscope.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710954848.1A CN107816989B (en) | 2017-10-13 | 2017-10-13 | Underwater robot course data processing method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710954848.1A CN107816989B (en) | 2017-10-13 | 2017-10-13 | Underwater robot course data processing method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107816989A CN107816989A (en) | 2018-03-20 |
| CN107816989B true CN107816989B (en) | 2021-01-08 |
Family
ID=61608035
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710954848.1A Active CN107816989B (en) | 2017-10-13 | 2017-10-13 | Underwater robot course data processing method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107816989B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104808675B (en) * | 2015-03-03 | 2018-05-04 | 广州亿航智能技术有限公司 | Body-sensing flight control system and terminal device based on intelligent terminal |
| TWI634403B (en) * | 2017-01-26 | 2018-09-01 | 好樣科技有限公司 | An automatic cleaning robot and a controlling method of the same |
| CN108919363B (en) * | 2018-08-07 | 2019-09-13 | 哈尔滨工业大学 | One kind is adaptively according to course Aeromagnetic data processing method |
| CN113836123A (en) * | 2021-07-22 | 2021-12-24 | 南京沃旭通讯科技有限公司 | Track cleaning method based on distance and angle |
| CN113534816B (en) * | 2021-08-16 | 2024-04-05 | 安徽元古纪智能科技有限公司 | Mobile robot navigation tracking method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012068772A (en) * | 2010-09-22 | 2012-04-05 | Nippon Hoso Kyokai <Nhk> | Camera tracking device and program therefor |
| CN102498712A (en) * | 2009-07-29 | 2012-06-13 | 索尼公司 | Control device, image-capturing system, control method, and program |
| CN102857249A (en) * | 2011-06-30 | 2013-01-02 | 重庆重邮信科通信技术有限公司 | Receiver setting method for synchronous search of mobile terminal |
| CN103885076A (en) * | 2014-03-06 | 2014-06-25 | 华南农业大学 | Multi-sensor information fusion method based on GPS for agricultural machine navigation |
| US9293075B2 (en) * | 2013-02-13 | 2016-03-22 | Samsung Electronics Co., Ltd. | Display apparatus and control method thereof |
| CN106767789A (en) * | 2017-01-12 | 2017-05-31 | 南京航空航天大学 | A kind of pedestrian course Optimal Fusion based on adaptive Kalman filter |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100580628B1 (en) * | 2003-11-08 | 2006-05-16 | 삼성전자주식회사 | A heading direction estimation method and system for a mobile body |
| JP2015046661A (en) * | 2013-08-27 | 2015-03-12 | ソニー株式会社 | Information processing device and information processing method |
| CN204329976U (en) * | 2015-01-14 | 2015-05-13 | 上海物景智能科技有限公司 | A kind of robot course-angle mensurement device and comprise the robot of this device |
| CN106153042A (en) * | 2015-03-31 | 2016-11-23 | 日本电气株式会社 | Course angle acquisition methods and device |
| CN105841698B (en) * | 2016-03-28 | 2018-10-26 | 中国船舶重工集团公司第七一〇研究所 | A kind of AUV rudder angle precision real time measuring systems without zeroing |
| CN106017459B (en) * | 2016-05-19 | 2019-03-19 | 极翼机器人(上海)有限公司 | A kind of method of diamagnetic interference |
| CN206249096U (en) * | 2016-12-06 | 2017-06-13 | 北京臻迪科技股份有限公司 | A kind of underwater robot |
-
2017
- 2017-10-13 CN CN201710954848.1A patent/CN107816989B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102498712A (en) * | 2009-07-29 | 2012-06-13 | 索尼公司 | Control device, image-capturing system, control method, and program |
| JP2012068772A (en) * | 2010-09-22 | 2012-04-05 | Nippon Hoso Kyokai <Nhk> | Camera tracking device and program therefor |
| CN102857249A (en) * | 2011-06-30 | 2013-01-02 | 重庆重邮信科通信技术有限公司 | Receiver setting method for synchronous search of mobile terminal |
| US9293075B2 (en) * | 2013-02-13 | 2016-03-22 | Samsung Electronics Co., Ltd. | Display apparatus and control method thereof |
| CN103885076A (en) * | 2014-03-06 | 2014-06-25 | 华南农业大学 | Multi-sensor information fusion method based on GPS for agricultural machine navigation |
| CN106767789A (en) * | 2017-01-12 | 2017-05-31 | 南京航空航天大学 | A kind of pedestrian course Optimal Fusion based on adaptive Kalman filter |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107816989A (en) | 2018-03-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107816989B (en) | Underwater robot course data processing method and device | |
| CN105698765B (en) | Object pose method under double IMU monocular visions measurement in a closed series noninertial systems | |
| Zhou et al. | A GA-based parameters tuning method for an ADRC controller of ISP for aerial remote sensing applications | |
| KR102017404B1 (en) | How to update the full posture angle of agricultural machinery based on 9 axis MEMS sensor | |
| CN111721289A (en) | Vehicle positioning method, device, equipment, storage medium and vehicle | |
| CN105352495B (en) | Acceleration and light stream Data Fusion of Sensor unmanned plane horizontal velocity control method | |
| CN106705936B (en) | A kind of unmanned plane height optimization method and device | |
| CN107490378B (en) | Indoor positioning and navigation method based on MPU6050 and smart phone | |
| CN103591955B (en) | Integrated navigation system | |
| US5060175A (en) | Measurement and control system for scanning sensors | |
| CN111024064A (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| JP5139412B2 (en) | Angle measuring method and angle measuring gyro system for implementing the same | |
| CN102937450B (en) | A kind of relative attitude defining method based on gyro to measure information | |
| KR20130127991A (en) | Method and system for estimating a path of a mobile element or body | |
| CN108344413B (en) | Underwater glider navigation system and low-precision and high-precision conversion method thereof | |
| CN110017850A (en) | A kind of gyroscopic drift estimation method, device and positioning system | |
| CN113155124B (en) | Multi-source auxiliary navigation method and device | |
| CN105547291B (en) | The adaptive static detection method of indoor occupant freedom positioning system | |
| CN104316058B (en) | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot | |
| CN108663067A (en) | A kind of adaptive calibration method and system of motion sensor | |
| CN108592907A (en) | A kind of quasi real time step-by-step movement pedestrian navigation method based on bidirectional filtering smoothing technique | |
| Bai et al. | A novel plug-and-play factor graph method for asynchronous absolute/relative measurements fusion in multisensor positioning | |
| CN112797979B (en) | Inertial attitude navigation system applied to AGV | |
| CN114459466A (en) | MEMS multi-sensor data fusion processing method based on fuzzy control | |
| CN112556688A (en) | Measuring device |
Legal Events
| 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 | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20210918 Address after: 650051 No. 3 East Renmin Road, Yunnan, Kunming Patentee after: NO.750 PROVING GROUND, CHINA SHIPBUILDING INDUSTRY Corp. Patentee after: SOUTHERN POWER GRID PEAK LOAD AND FREQUENCY REGULATION POWER GENERATION Co.,Ltd. Patentee after: Guangzhou peak frequency modulation technology development Co.,Ltd. Address before: No.3, Renmin East Road, Kunming, Yunnan 650000 Patentee before: NO.750 PROVING GROUND, CHINA SHIPBUILDING INDUSTRY Corp. Patentee before: China Southern Power Grid Tiaofeng Frequency Modulation Power Generation Company |
|
| TR01 | Transfer of patent right | ||
| CP03 | Change of name, title or address |
Address after: 650051, No. 3, Renmin East Road, Panlong District, Yunnan, Kunming Patentee after: China Shipbuilding Group Co.,Ltd. No 750 Testing Ground Patentee after: SOUTHERN POWER GRID PEAK LOAD AND FREQUENCY REGULATION POWER GENERATION Co.,Ltd. Patentee after: China Southern power grid peak shaving and frequency modulation (Guangdong) energy storage technology Co.,Ltd. Address before: 650051 No. 3 East Renmin Road, Yunnan, Kunming Patentee before: NO.750 PROVING GROUND, CHINA SHIPBUILDING INDUSTRY Corp. Patentee before: SOUTHERN POWER GRID PEAK LOAD AND FREQUENCY REGULATION POWER GENERATION Co.,Ltd. Patentee before: Guangzhou peak frequency modulation technology development Co.,Ltd. |
|
| CP03 | Change of name, title or address |