CN116009557A - Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium - Google Patents
Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium Download PDFInfo
- Publication number
- CN116009557A CN116009557A CN202310123688.1A CN202310123688A CN116009557A CN 116009557 A CN116009557 A CN 116009557A CN 202310123688 A CN202310123688 A CN 202310123688A CN 116009557 A CN116009557 A CN 116009557A
- Authority
- CN
- China
- Prior art keywords
- angle
- self
- mobile device
- yaw angle
- determining
- 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.)
- Pending
Links
Images
Landscapes
- Navigation (AREA)
Abstract
The application is applicable to the technical field of automatic control, and provides an azimuth angle calculation method, an azimuth angle calculation device, azimuth angle calculation equipment and a storage medium, wherein the azimuth angle calculation method comprises the following steps: firstly, when the first angular velocity of the self-mobile device in the reference time period is in the angular velocity range, positioning data and an average yaw angle of the self-mobile device in the reference time period are obtained, then the first azimuth angle of the self-mobile device in the world coordinate system is determined according to the positioning data, then a compensation angle is obtained by calculating the difference value between the first azimuth angle and the average yaw angle, and finally a second azimuth angle of the self-mobile device in the world coordinate system is determined according to the yaw angle and the compensation angle at the current moment. Therefore, according to the yaw angle at the current moment, the yaw angle at the current moment is corrected by using the compensation angle, so that the second azimuth angle is determined, the influence of speed data noise is not needed to be considered, and the accuracy of azimuth angle calculation is improved.
Description
Technical Field
The application belongs to the technical field of automatic control, and particularly relates to an azimuth angle calculating method, an azimuth angle calculating device, azimuth angle calculating equipment and a storage medium.
Background
With the rapid development of automatic control, automated operation of self-mobile devices is becoming increasingly important. For positioning navigation of a self-mobile device, an azimuth angle of the self-mobile device needs to be determined, wherein the azimuth angle refers to a horizontal included angle between a motion direction of the self-mobile device and a north direction. In the related art, a self-mobile device generally uses a single-antenna satellite positioning receiver and determines an azimuth angle through a speed measurement principle, but this manner is easily affected by speed data noise, so that the accuracy of the finally obtained azimuth angle is low.
Disclosure of Invention
The embodiment of the application provides a method, a device, equipment and a storage medium for calculating an azimuth angle of self-mobile equipment, which can solve the problem that the self-mobile equipment usually utilizes a single-antenna satellite positioning receiver and determines the azimuth angle through a speed measurement principle, but the mode is easily influenced by speed data noise, so that the accuracy of the finally obtained azimuth angle is lower.
A first aspect of an embodiment of the present application provides an azimuth calculation method, including:
when the first angular velocity of the self-mobile device in the preset reference time period is in the preset angular velocity range, acquiring positioning data of the self-mobile device in the reference time period, and acquiring an average yaw angle of the self-mobile device in the reference time period;
determining a first azimuth angle of the self-mobile device under a world coordinate system according to the positioning data;
calculating a difference value between the first azimuth angle and the average yaw angle to obtain a compensation angle;
and determining a second azimuth angle of the self-mobile device under the world coordinate system according to the yaw angle and the compensation angle at the current moment.
In a possible implementation manner of the first aspect, before determining the second azimuth angle of the self-mobile device in the world coordinate system according to the yaw angle and the compensation angle at the current moment, the method further includes:
acquiring a yaw angle at the last moment of the mobile device and a second angular velocity at the current moment;
determining a time interval according to the previous time and the current time;
calculating the angle change amount of the self-mobile device in the time interval according to the second angular speed and the time interval;
and determining the yaw angle at the current moment according to the yaw angle and the angle change quantity at the previous moment.
In a possible implementation manner of the first aspect, before the acquiring the positioning data of the self-mobile device in the reference period and acquiring the average yaw angle of the self-mobile device in the reference period, the azimuth calculating method further includes:
acquiring a first angular velocity corresponding to each historical moment in a preset reference time period;
calculating standard deviation of each first angular velocity;
and when the standard deviation is smaller than a preset standard threshold value, determining that each first angular velocity is in a preset angular velocity range.
In a possible implementation manner of the first aspect, the acquiring the average yaw angle of the self-mobile device in the reference period includes:
acquiring yaw angles corresponding to each historical moment of the mobile equipment in a reference time period;
and calculating the average value of the yaw angles corresponding to the historical moments to obtain the average yaw angle.
In a possible implementation manner of the first aspect, the determining, according to the positioning data, a first azimuth angle of the self-mobile device in a world coordinate system includes:
performing straight line fitting on the positioning data to obtain a fitting straight line;
and determining an included angle between the fitting straight line and a preset coordinate axis of the world coordinate system as a first azimuth angle.
In a possible implementation manner of the first aspect, the azimuth angle calculating method further includes:
calculating the difference between the compensation angle obtained by the previous calculation and the compensation angle obtained by the current calculation to obtain a drift value;
and updating the yaw angle at the current moment according to the drift value to obtain the updated yaw angle at the current moment.
In a possible implementation manner of the first aspect, the azimuth angle calculating method further includes:
and when the first angular velocity of the self-mobile device in the preset reference time period is not in the preset angular velocity range, determining a second azimuth angle of the self-mobile device in the world coordinate system according to the yaw angle at the current moment and the compensation angle obtained by the last calculation.
A second aspect of embodiments of the present application provides an azimuth angle calculating device, including:
the acquisition module is used for acquiring positioning data of the self-mobile device in the reference time period and acquiring an average yaw angle of the self-mobile device in the reference time period when the first angular speed of the self-mobile device in the preset reference time period is in a preset angular speed range;
a first determining module for determining a first azimuth angle of the self-mobile device in a world coordinate system according to the positioning data;
the calculating module is used for calculating the difference between the first azimuth angle and the average yaw angle to obtain a compensation angle;
and the second determining module is used for determining a second azimuth angle of the self-mobile device under the world coordinate system according to the yaw angle and the compensation angle at the current moment.
A third aspect of the embodiments of the present application provides a terminal device, including: the azimuth angle calculating device comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the azimuth angle calculating method of the first aspect when executing the computer program.
A fourth aspect of the embodiments of the present application provides a computer-readable storage medium storing a computer program which, when executed by a processor, implements the azimuth angle calculation method of the first aspect described above.
A fifth aspect of embodiments of the present application provides a computer program product for, when run on a terminal device, causing the terminal device to perform the azimuth calculation method of the first aspect described above.
Compared with the related art, the embodiment of the application has the beneficial effects that:
the application discloses a method, a device, equipment and a storage medium for determining azimuth angle of self-mobile equipment, wherein the method comprises the following steps: firstly, when the first angular velocity of the self-mobile device in the reference time period is in the angular velocity range, positioning data and an average yaw angle of the self-mobile device in the reference time period are obtained, then the first azimuth angle of the self-mobile device in the world coordinate system is determined according to the positioning data, then a compensation angle is obtained by calculating the difference value between the first azimuth angle and the average yaw angle, and finally a second azimuth angle of the self-mobile device in the world coordinate system is determined according to the yaw angle and the compensation angle at the current moment. When the first angular velocity is maintained in the angular velocity range, it is indicated that the change of the travelling direction of the self-mobile device is regular, and at the moment, a relatively stable average yaw angle can be obtained, so that the difference value calculated by the first azimuth angle and the average yaw angle is more accurate, and therefore, according to the yaw angle at the current moment, the yaw angle at the current moment is corrected by using the compensation angle, the second azimuth angle is determined, the influence of velocity data noise is not needed to be considered, and the accuracy of azimuth angle calculation is improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following description will briefly introduce the drawings that are needed in the embodiments or the description of the prior art, it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic flow chart of an azimuth calculation method according to an embodiment of the present application;
FIG. 2 is a schematic diagram of coordinate system construction provided in an embodiment of the present application;
FIG. 3 is a schematic diagram of a straight line fit provided in an embodiment of the present application;
fig. 4 is a schematic structural diagram of an azimuth calculating device according to an embodiment of the present application;
fig. 5 is a schematic structural diagram of a terminal device according to an embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system configurations, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It should be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should be understood that the sequence number of each step in this embodiment does not mean the sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present application.
In the related art, a self-mobile device generally uses a single-antenna satellite positioning receiver and determines an azimuth angle through a speed measurement principle, but this manner is easily affected by speed data noise, so that the accuracy of the finally obtained azimuth angle is low.
In view of this, the embodiments of the present application provide a method, an apparatus, a terminal device, and a storage medium for calculating an azimuth angle, which correct the yaw angle at the current moment according to the yaw angle at the current moment and using the compensation angle, so as to determine a second azimuth angle, without considering the influence of the velocity data noise, and improve the accuracy of azimuth angle calculation.
In order to illustrate the technical solution of the present application, the following description is made by specific embodiments.
Referring to fig. 1, a flow chart of an azimuth calculation method provided in an embodiment of the present application is shown. As shown in fig. 1, the azimuth calculation method is applied to a self-mobile device, and may include the following steps:
In the embodiment of the application, the self-mobile device may be a device including a self-mobile auxiliary function, and may also be a semi-self-mobile device or a completely autonomous mobile device. The self-moving auxiliary function can be realized by a vehicle-mounted terminal, and the corresponding self-moving equipment can be a vehicle with the vehicle-mounted terminal. The self-mobile device may also be a semi-self-mobile device or a fully autonomous mobile device. Such as a mower, a sweeper, a robot with navigation function, etc.
The self-mobile device is provided with satellite positioning measurement equipment and a pose sensor. The satellite positioning measurement equipment is used for acquiring the positioning of the mobile equipment, and the pose sensor is used for acquiring the motion information such as yaw angle, angular velocity and the like of the mobile equipment.
The specific type of the above-mentioned pose sensor can be set according to actual conditions. For example, the above-described pose sensor may include any one or a combination of a plurality of types of sensors such as gyroscopes, inertial measurement units (Inertial Measurement Unit, IMU), and the like. For convenience of explanation, the following embodiments will be described by taking the pose sensor as an IMU as an example.
In the embodiment of the present application, for convenience of explanation, referring to fig. 2, a schematic diagram is constructed for a coordinate system provided in the embodiment of the present application. The satellite positioning measurement device and the IMU use the respective measured advancing directions of the mobile device as X axes, use clockwise rotation negative directions and use anticlockwise rotation positive directions to construct corresponding coordinate systems. For convenience in describing the scheme of the embodiment of the application, coordinate system conversion can be performed on the coordinate system corresponding to the satellite positioning measurement device and the IMU, so that the satellite positioning measurement device and the IMU data can be represented by XY plane coordinates under a world coordinate system. Therefore, the world coordinate system can be constructed by taking any longitude, latitude and altitude as the origin of the world coordinate system, taking the north direction as 0 degree, taking the clockwise direction as the negative rotation direction, and taking the anticlockwise direction as the positive rotation direction, the east direction is 90 degrees, and the south direction is 180 degrees and 180 degrees. It should be understood that all subsequent data in the coordinate system is converted to data in the world coordinate system.
The first angular velocity w' of the self-mobile device can be obtained through the IMU, the IMU can be used as a high-frequency data sensor, and obtained data through the IMU is more real-time. The positioning data of the self-mobile device can be obtained by a satellite positioning measurement device such as a GPS (Global Positioning System) or an RTK (Real-time kinematic) device mounted on the self-mobile device.
The reference time period represents any historical time period when the self-mobile device is in a motion state, the reference time period comprises a plurality of historical moments, and each historical moment can acquire positioning data and a first angular speed w' of the self-mobile device once.
In this embodiment of the present application, in a reference period, if each first angular velocity w' of the self-mobile device is within a preset angular velocity range, it indicates that the yaw angle of the self-mobile device is not changed or is being changed stably according to a certain angular change amount. The angular velocity range may be determined according to an actual application scenario, which is not limited in this application. For example, in a reference period, when each first angular velocity w' of the self-moving device is approximately 0, it means that the yaw angle of the self-moving device is not changed, that is, the self-moving device is performing linear motion; when the respective first angular velocities w' of the self-moving device are maintained around a certain value, it is indicated that the yaw angle of the self-moving device is changing at a constant or near constant speed, i.e. the self-moving device is performing a regular arc movement.
Further, since it is only determined whether the first angular velocity or the first angular velocities in the reference period are not likely to be representative in the preset angular velocity range, determining the standard deviation of each first angular velocity in the reference period may first, and then determining whether each first angular velocity is in the preset angular velocity range by determining whether the standard deviation is smaller than the preset standard threshold, that is, in a possible implementation manner of the embodiment of the present application, before obtaining the positioning data of the self-mobile device in the reference period and obtaining the average yaw angle of the self-mobile device in the reference period, the method may include:
acquiring a first angular velocity corresponding to each historical moment in a preset reference time period;
calculating standard deviation of each first angular velocity;
and when the standard deviation is smaller than a preset standard threshold value, determining that each first angular velocity is in a preset angular velocity range.
Specifically, the standard deviation S of each first angular velocity can be calculated by the following formula:
wherein w' i Representing a first angular velocity acquired at an i-th historical moment in a reference time period,
represents the average value of the first angular velocities acquired at each of the historical moments in the reference period, and n represents the total number of the first angular velocities acquired in the reference period.
In the embodiment of the application, since the standard deviation of a set of data can reflect the degree to which each data in the set of data deviates from its average value, a smaller standard deviation indicates that the set of data is closer, and a larger standard deviation indicates that the set of data fluctuates within a larger range of values. Therefore, when the standard deviation of each first angular velocity w ' in the reference period is smaller than the preset standard threshold, it may be indicated that each first angular velocity w ' in the reference period is within the preset angular velocity range, that is, each first angular velocity w ' is relatively close, so that it may be determined that the angular velocity of the self-mobile device in the reference period has no or little change.
It should be noted that, the preset standard threshold may be determined according to an actual application scenario, which is not limited in the present application.
In the embodiment of the application, the self-mobile device is at the motion starting time t 0 Is 0, assuming that the time interval between every two moments is Δt, the first moment t after the start of the movement 1 The acquired angular velocity is w 1 Thus, the movement start time t can be obtained 0 And at a first time t 1 Angle change of yaw angle delta theta 1 ＝w 1 Delta t, and thus the first time t 1 Collected yaw angle θ 1 ＝0+△θ 1 Similarly, the yaw angle at each moment in time can be obtained during the movement of the self-moving device.
Further, the average yaw angle θ of the self-mobile device during the reference period can be determined by calculating the average value avg That is, in one possible implementation manner of the present application, the acquiring the average yaw angle of the self-mobile device in the reference period may include:
acquiring yaw angles corresponding to each historical moment of the mobile equipment in a reference time period;
and calculating the average value of the yaw angles corresponding to the historical moments to obtain the average yaw angle.
For example, assume that the self-mobile device includes yaw angles corresponding to 7 historical moments within a reference period of time, respectively yaw angles θ a 、θ b 、θ c 、θ d 、θ e 、θ f 、θ g The average yaw angle θ of the self-moving device during the reference period can be obtained avg ＝(θ a +θ b +θ c +θ d +θ e +θ f +θ g )/7。
It will be appreciated that when the first angular velocity of the self-moving device within the reference period is within the preset angular velocity range, the average yaw angle of the self-moving device within the reference period is approximately equal to the yaw angle corresponding to the intermediate instant within the reference period. By calculating the average yaw angle of the self-mobile device in the reference time period, the posture change of the self-mobile device in the traveling process can be known, and the subsequent correction of the azimuth angle is facilitated.
Further, when the first angular velocity of the self-mobile device in the reference time period is within the preset angular velocity range, a fitted straight line may be obtained by performing straight line fitting on the positioning data, and then an included angle between the fitted straight line and the preset coordinate axis may be determined as the first azimuth on the XY axis plane on the world coordinate system, that is, in a possible implementation manner of the embodiment of the present application, the step 102 may include:
performing straight line fitting on the positioning data to obtain a fitting straight line;
and determining an included angle between the fitting straight line and a preset coordinate axis of the world coordinate system as a first azimuth angle.
In the embodiment of the application, by fitting the positioning data in the reference time period, the obtained fitting straight line can effectively represent the track direction of the self-mobile device walking in the reference time period, so that the track direction can be determined as the first azimuth angle.
The preset coordinate axis can be determined according to an actual application scene, the application is not limited to the determination, and for the convenience of calculation, the preset coordinate axis can be an X axis corresponding to the north direction of the world coordinate system.
Referring to fig. 3, a schematic straight line fitting diagram is provided in the embodiment of the present application, in which 7 sets of positioning data are subjected to straight line fitting to obtain a fitted straight line l, and an included angle between the fitted straight line l and an X axis in the north direction is a first azimuth angle α of the mobile device in a world coordinate system.
It will be appreciated that when the first angular velocity of the self-moving device within the reference period is within the preset angular velocity range, the fitted straight line l is obtained by straight line fitting of the positioning data of the self-moving device within the reference period, and when the self-moving device is at the midpoint of the fitted straight line, the orientation of the self-moving device (the line connecting the head and the tail of the self-moving device) is parallel to the fitted straight line l.
As a possible implementation manner, the linear fitting manner may be least square method, singular value decomposition, kalman filtering, nonlinear optimization solution, etc., and the application does not limit the linear fitting manner.
Wherein the compensation angle β=α - θ avg 。
In the embodiment of the application, on the premise that the first angular velocity of the self-mobile device in the reference time period is in the preset angular velocity range, the first azimuth angle alpha and the average yaw angle theta are used for avg The azimuth and yaw angles corresponding to the intermediate position or moment of the self-mobile device within the reference time period can be represented, so that a fixed difference relationship between the first azimuth and the average yaw angle of each moment of the self-mobile device under regular movement, namely the compensation angle beta, can be determined.
In the motion process of the self-mobile device, the yaw angle calculated by the data acquired by the motion gesture calculation sensor may have accumulated errors, and the positioning information acquired by the GPS or the RTK is based on geographic information without accumulated errors, so that the yaw angle errors generated by the motion gesture calculation sensor can be corrected by the positioning data.
It will be appreciated that if yaw angle alone is used as azimuth angle, the attitude sensor of the IMU will produce cumulative errors after a period of use, causing azimuth angle distortion. If the azimuth angle is determined by using the positioning information and the speed measurement principle acquired by the satellite positioning measurement equipment with a single antenna, the accuracy is low and the instantaneity is poor.
Therefore, the azimuth angle is not directly determined according to the positioning information, but the positioning information of a plurality of historical moments in the reference time period is used for straight line fitting, the first azimuth angle is determined according to the straight line obtained by fitting, and the stability and the accuracy of the first azimuth angle can be improved.
Then, the first azimuth angle is subjected to difference of the average yaw angle in the reference time period from the mobile device, and the accumulated error of the pose sensor, namely the compensation angle, can be obtained. Subsequently, the self-moving equipment can correct the currently detected yaw angle by using the compensation angle, so that the accumulated error of the yaw angle is eliminated, and the measurement accuracy of the pose sensor is improved.
In addition, the pose sensor (such as IMU) has higher measurement frequency and better real-time performance. Therefore, by the method, the azimuth angle with higher instantaneity can be obtained, the accuracy of the azimuth angle can be ensured, and the problems of poor instantaneity and low accuracy of the current single-antenna speed measuring scheme are solved.
Further, the yaw angle at the current time may be calculated by the yaw angle at the previous time and the second angular velocity w″ at the current time, that is, before the step 104, in a possible implementation manner of the embodiment of the present application, the method may further include:
acquiring a yaw angle at the last moment of the mobile device and a second angular velocity at the current moment;
determining a time interval according to the previous time and the current time;
calculating the angle change amount of the self-mobile device in the time interval according to the second angular speed and the time interval;
and determining the yaw angle at the current moment according to the yaw angle and the angle change quantity at the previous moment.
In the embodiment of the application, T is the last time t-1 And the current time T t Time interval Δt=t of (a) t -T t-1 Obtaining the angle variation delta theta=w ". Delta t of the self-mobile device in the time interval delta t from the product of the second angular velocity w' at the current moment and the time interval delta t, and finally obtaining the yaw angle theta according to the last moment t-1 Determining the yaw angle theta at the current moment by the angle variation delta theta t ＝θ t-1 +△θ。
In this embodiment of the present application, when the first angular velocity of the self-mobile device in the preset reference period is not within the preset angular velocity range, it indicates that the self-mobile device is performing irregular motion, and the second azimuth angle of the self-mobile device in the world coordinate system may be determined according to the yaw angle at the current time and the compensation angle obtained by the last calculation. Specifically, the sum of the yaw angle at the current moment and the compensation angle obtained by the last calculation is determined to be a second azimuth angle, and the second azimuth angle is adopted to guide the self-mobile device to move, so that an accurate basis is provided for the follow-up motion control and planning navigation of the self-mobile device. The compensation angle calculated last time is the compensation angle calculated from the first angular speed of the mobile device in the last reference time period within a preset angular speed range.
In this embodiment of the present application, when the self-mobile device moves, the compensation of the yaw angle at the current time by using the compensation angle β may not be the first compensation, and each time the first angular velocity of the self-mobile device in the preset reference period is newly appeared in the preset angular velocity range, the motion attitude calculation sensor may generate a continuous accumulated error, which causes the yaw angle drift at the current time, so that the second azimuth angle obtained by compensating the yaw angle at the current time by using the last calculated compensation angle is not accurate any more, and therefore, the compensation angle needs to be redetermined once, and the second azimuth angle obtained by correcting the yaw angle at the current time by using the recalculated compensation angle is more accurate.
In the embodiment of the present application, for the self-mobile device in the last reference period, the compensation angle β calculated last time is used t-1 Determining the second azimuth angle is accurate, but over time the motion attitude calculation sensor produces a cumulative error, resulting in the last calculated second azimuth angle β t-1 Slowly becoming inaccurate and the compensation angle beta is currently recalculated t For the current moment, the compensation angle beta t Is accurate and the compensation angle beta at the current moment t With the compensation angle beta calculated last time t-1 The deviation is caused by drift errors of the pose sensor during the time period of calculating the compensation angle in two adjacent times.
According to the azimuth angle calculating method disclosed by the embodiment of the application, firstly, when the first angular speed of the self-mobile device in the reference time period is in the angular speed range, positioning data and an average yaw angle of the self-mobile device in the reference time period are obtained, then, the first azimuth angle of the self-mobile device in the world coordinate system is determined according to the positioning data, then, the difference between the first azimuth angle and the average yaw angle is calculated to obtain a compensation angle, and finally, the second azimuth angle of the self-mobile device in the world coordinate system is determined according to the yaw angle and the compensation angle at the current moment. When the first angular velocity is maintained in the angular velocity range, it is indicated that the change of the travelling direction of the self-mobile device is regular, and at the moment, a relatively stable average yaw angle can be obtained, so that the difference value calculated by the first azimuth angle and the average yaw angle is more accurate, and therefore, according to the yaw angle at the current moment, the yaw angle at the current moment is corrected by using the compensation angle, the second azimuth angle is determined, the influence of velocity data noise is not needed to be considered, and the accuracy of azimuth angle calculation is improved.
Referring to fig. 4, a schematic structural diagram of an azimuth angle calculating device provided in an embodiment of the present application is shown, and for convenience of explanation, only a portion related to the embodiment of the present application is shown.
The azimuth angle calculating device may specifically include the following modules:
the obtaining module 401 is configured to obtain positioning data of the self-mobile device in the reference period and obtain an average yaw angle of the self-mobile device in the reference period when the first angular velocity of the self-mobile device in the preset reference period is within the preset angular velocity range.
A first determining module 402 is configured to determine a first azimuth angle of the self-mobile device in a world coordinate system according to the positioning data.
A calculation module 403, configured to calculate a difference between the first azimuth angle and the average yaw angle, to obtain a compensation angle.
A second determining module 404 is configured to determine a second azimuth angle of the self-mobile device in the world coordinate system according to the yaw angle and the compensation angle at the current moment.
The azimuth calculating device disclosed in the above embodiment of the present application includes: firstly, when the first angular velocity of the self-mobile device in the reference time period is in the angular velocity range, positioning data and an average yaw angle of the self-mobile device in the reference time period are obtained, then the first azimuth angle of the self-mobile device in the world coordinate system is determined according to the positioning data, then a compensation angle is obtained by calculating the difference value between the first azimuth angle and the average yaw angle, and finally a second azimuth angle of the self-mobile device in the world coordinate system is determined according to the yaw angle and the compensation angle at the current moment. When the first angular velocity is maintained in the angular velocity range, the traveling direction of the self-mobile device is regular, and at the moment, a stable average yaw angle can be obtained, so that the difference value calculated by the first azimuth angle and the average yaw angle is more accurate, and therefore, according to the yaw angle at the current moment, the yaw angle at the current moment is corrected by using the compensation angle, the second azimuth angle is determined, the influence of velocity data noise is not needed to be considered, and the accuracy of azimuth angle calculation is improved.
In one possible implementation manner of the embodiment of the present application, the obtaining module 401 may specifically include the following sub-modules, configured to determine, before the second azimuth angle of the self-mobile device in the world coordinate system according to the yaw angle and the compensation angle at the current moment:
the first acquisition sub-module is used for acquiring the yaw angle at the last moment of the mobile device and the second angular velocity at the current moment.
And the first determining submodule is used for determining the time interval according to the previous time and the current time.
And the first calculating sub-module is used for calculating the angle change quantity of the self-mobile device in the time interval according to the second angular speed and the time interval.
And the second determining submodule is used for determining the yaw angle at the current moment according to the yaw angle and the angle change quantity at the previous moment.
In one possible implementation manner of the embodiment of the present application, the obtaining module 401 may specifically include the following sub-modules, which are configured to, before obtaining the positioning data of the self-mobile device in the reference period, and obtaining the average yaw angle of the self-mobile device in the reference period:
the second acquisition sub-module is used for acquiring the first angular velocity corresponding to each historical moment in the preset reference time period.
And the second calculation sub-module is used for calculating the standard deviation of each first angular velocity.
And the third determination submodule is used for determining that each first angular velocity is in a preset angular velocity range when the standard deviation is smaller than a preset standard threshold value.
In one possible implementation manner of the embodiment of the present application, the above-mentioned obtaining module 401 may specifically include the following sub-modules:
and the third acquisition sub-module is used for acquiring yaw angles corresponding to each historical moment of the mobile equipment in the reference time period.
And the third calculation sub-module is used for calculating the average value of the yaw angles corresponding to each historical moment to obtain the average yaw angle.
In one possible implementation manner of the embodiment of the present application, the first determining module 402 may specifically include the following sub-modules:
and the straight line fitting sub-module is used for carrying out straight line fitting on the positioning data to obtain a fitting straight line.
And the fourth determining submodule is used for determining an included angle between the fitting straight line and a preset coordinate axis of the world coordinate system as a first azimuth angle.
In one possible implementation manner of the embodiment of the present application, the azimuth angle calculating device may specifically further include the following modules:
and a fifth determining submodule, configured to determine a second azimuth angle of the self-mobile device in the world coordinate system according to the yaw angle at the current time and the compensation angle obtained by the last calculation when the first angular velocity of the self-mobile device in the preset reference time period is not in the preset angular velocity range.
The azimuth calculating device provided in the embodiment of the present application may be applied to the foregoing method embodiment, and details of the foregoing method embodiment are referred to in the description of the foregoing method embodiment and are not repeated herein.
Fig. 5 is a schematic structural diagram of a terminal device provided in an embodiment of the present application. As shown in fig. 5, the terminal device 500 of this embodiment includes: at least one processor 510 (only one is shown in fig. 5), a memory 520, and a computer program 521 stored in the memory 520 and executable on the at least one processor 510, the processor 510 implementing the steps in the azimuth calculation method embodiments described above when executing the computer program 521.
The terminal device 500 may be a computing device such as a desktop computer, a notebook computer, a palm computer, a cloud server, etc. The terminal device may include, but is not limited to, a processor 510, a memory 520. It will be appreciated by those skilled in the art that fig. 5 is merely an example of a terminal device 500 and is not limiting of the terminal device 500, and may include more or fewer components than shown, or may combine certain components, or different components, such as may also include input-output devices, network access devices, etc.
The processor 510 may be a central processing unit (Central Processing Unit, CPU), the processor 510 may also be other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), off-the-shelf programmable gate arrays (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 520 may in some embodiments be an internal storage unit of the terminal device 500, such as a hard disk or a memory of the terminal device 500. The memory 520 may also be an external storage device of the terminal device 500 in other embodiments, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card) or the like, which are provided on the terminal device 500. Further, the memory 520 may also include both an internal storage unit and an external storage device of the terminal device 500. The memory 520 is used to store an operating system, application programs, boot Loader (Boot Loader), data, and other programs, such as program code of the computer program. The memory 520 may also be used to temporarily store data that has been output or is to be output.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of the functional units and modules is illustrated, and in practical application, the above-described functional distribution may be performed by different functional units and modules according to needs, i.e. the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-described functions. The functional units and modules in the embodiment may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit, where the integrated units may be implemented in a form of hardware or a form of a software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working process of the units and modules in the above system may refer to the corresponding process in the foregoing method embodiment, which is not described herein again.
In the foregoing embodiments, the descriptions of the embodiments are emphasized, and in part, not described or illustrated in any particular embodiment, reference is made to the related descriptions of other embodiments.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other manners. For example, the apparatus/terminal device embodiments described above are merely illustrative, e.g., the division of the modules or units is merely a logical function division, and there may be additional divisions in actual implementation, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection via interfaces, devices or units, which may be in electrical, mechanical or other forms.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in each embodiment of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated modules/units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the present application may implement all or part of the flow of the method of the above embodiment, or may be implemented by a computer program to instruct related hardware, where the computer program may be stored in a computer readable storage medium, and when the computer program is executed by a processor, the computer program may implement the steps of each method embodiment described above. Wherein the computer program comprises computer program code which may be in source code form, object code form, executable file or some intermediate form etc. The computer readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer Memory, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), an electrical carrier signal, a telecommunications signal, a software distribution medium, and so forth. It should be noted that the computer readable medium contains content that can be appropriately scaled according to the requirements of jurisdictions in which such content is subject to legislation and patent practice, such as in certain jurisdictions in which such content is subject to legislation and patent practice, the computer readable medium does not include electrical carrier signals and telecommunication signals.
The implementation of all or part of the flow of the method of the above embodiment may also be accomplished by a computer program product, which when run on a terminal device, causes the terminal device to perform the steps of the method embodiments described above.
The above embodiments are only for illustrating the technical solution of the present application, and are not limiting. Although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present application, and are intended to be included in the scope of the present application.
Claims (10)
1. An azimuth calculation method applied to a self-mobile device, comprising:
when the first angular velocity of the self-mobile device in a preset reference time period is in a preset angular velocity range, acquiring positioning data of the self-mobile device in the reference time period, and acquiring an average yaw angle of the self-mobile device in the reference time period;
determining a first azimuth angle of the self-mobile device in a world coordinate system according to the positioning data;
calculating a difference value between the first azimuth angle and the average yaw angle to obtain a compensation angle;
and determining a second azimuth angle of the self-mobile device under the world coordinate system according to the yaw angle at the current moment and the compensation angle.
2. The method of claim 1, further comprising, prior to said determining a second azimuth angle of the self-mobile device in the world coordinate system based on the yaw angle at the current time and the compensation angle:
acquiring a yaw angle at the last moment of the self-moving equipment and a second angular speed at the current moment;
determining a time interval according to the previous time and the current time;
calculating the angle change amount of the self-mobile device in the time interval according to the second angular speed and the time interval;
and determining the yaw angle at the current moment according to the yaw angle at the last moment and the angle change quantity.
3. The method of claim 1, wherein prior to the obtaining the positioning data of the self-mobile device for the reference period of time and obtaining the average yaw angle of the self-mobile device for the reference period of time, the method further comprises:
acquiring the first angular velocity corresponding to each historical moment in a preset reference time period;
calculating standard deviation of each first angular velocity;
and when the standard deviation is smaller than a preset standard threshold value, determining that each first angular velocity is in the preset angular velocity range.
4. The method of claim 1, wherein the obtaining the average yaw angle of the self-mobile device over the reference period of time comprises:
acquiring yaw angles corresponding to each historical moment of the self-mobile device in the reference time period;
and calculating the average value of the yaw angles corresponding to the historical moments to obtain the average yaw angle.
5. The method of any of claims 1-4, wherein the determining a first azimuth angle of the self-mobile device in a world coordinate system from the positioning data comprises:
performing straight line fitting on the positioning data to obtain a fitting straight line;
and determining an included angle between the fitting straight line and a preset coordinate axis of the world coordinate system as the first azimuth angle.
6. The method of any one of claims 1-4, wherein the method further comprises:
and when the first angular speed of the self-mobile equipment in the preset reference time period is not in the preset angular speed range, determining a second azimuth angle of the self-mobile equipment under the world coordinate system according to the yaw angle at the current moment and the compensation angle obtained by the last calculation.
7. An azimuth calculation device applied to a self-mobile device, comprising:
the acquisition module is used for acquiring positioning data of the self-mobile device in a preset reference time period and acquiring an average yaw angle of the self-mobile device in the reference time period when the first angular speed of the self-mobile device in the preset reference time period is in a preset angular speed range;
a first determining module, configured to determine a first azimuth angle of the self-mobile device in a world coordinate system according to the positioning data;
the calculating module is used for calculating the difference between the first azimuth angle and the average yaw angle to obtain a compensation angle;
and the second determining module is used for determining a second azimuth angle of the self-mobile equipment under the world coordinate system according to the yaw angle at the current moment and the compensation angle.
8. The azimuth angle computing device of claim 7, wherein the acquisition module comprises:
the first acquisition submodule is used for acquiring a yaw angle at the last moment of the mobile equipment and a second angular speed at the current moment;
the first determining submodule is used for determining a time interval according to the previous moment and the current moment;
a first calculation sub-module for calculating an angle variation of the self-mobile device within the time interval according to the second angular velocity and the time interval;
and the second determining submodule is used for determining the yaw angle at the current moment according to the yaw angle and the angle change quantity at the previous moment.
9. A terminal device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor implements the method according to any of claims 1 to 6 when executing the computer program.
10. A computer readable storage medium storing a computer program, characterized in that the computer program when executed by a processor implements the method according to any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310123688.1A CN116009557A (en) | 2023-01-18 | 2023-01-18 | Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310123688.1A CN116009557A (en) | 2023-01-18 | 2023-01-18 | Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116009557A true CN116009557A (en) | 2023-04-25 |
Family
ID=86030332
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310123688.1A Pending CN116009557A (en) | 2023-01-18 | 2023-01-18 | Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116009557A (en) |
-
2023
- 2023-01-18 CN CN202310123688.1A patent/CN116009557A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10852139B2 (en) | Positioning method, positioning device, and robot | |
| EP3842746A2 (en) | Dead reckoning method and apparatus for vehicle, device and storage medium | |
| US11279045B2 (en) | Robot pose estimation method and apparatus and robot using the same | |
| WO2019134180A1 (en) | Vehicle positioning method and device, electronic apparatus, and medium | |
| CN110986988B (en) | Track calculation method, medium, terminal and device integrating multi-sensor data | |
| CN114136315B (en) | Monocular vision-based auxiliary inertial integrated navigation method and system | |
| CN114179825B (en) | Method for obtaining confidence of measurement value through multi-sensor fusion and automatic driving vehicle | |
| CN113933818A (en) | Method, device, storage medium and program product for calibrating laser radar external parameter | |
| CN110243364B (en) | Unmanned aerial vehicle course determining method and device, unmanned aerial vehicle and storage medium | |
| CN112835085A (en) | Method and device for determining vehicle position | |
| CN114119744B (en) | Method, device, equipment and storage medium for constructing point cloud map | |
| CN115236714A (en) | Multi-source data fusion positioning method, device and equipment and computer storage medium | |
| CN113050660B (en) | Error compensation method, error compensation device, computer equipment and storage medium | |
| CN115683170B (en) | Calibration method based on radar point cloud data fusion error | |
| CN115542277B (en) | Radar normal calibration method, device, system, equipment and storage medium | |
| CN116009557A (en) | Azimuth calculation method, azimuth calculation device, azimuth calculation equipment and storage medium | |
| CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium | |
| CN112304281A (en) | Road slope measuring method, terminal equipment and storage medium | |
| CN114018241B (en) | Positioning method and device for unmanned aerial vehicle | |
| CN112835086B (en) | Method and device for determining vehicle position | |
| CN114488237A (en) | Positioning method and device, electronic equipment and intelligent driving method | |
| CN114323007A (en) | Carrier motion state estimation method and device | |
| CN113155156A (en) | Method and device for determining running information, storage medium and electronic device | |
| CN112649823A (en) | Unmanned aerial vehicle navigation positioning method and device | |
| CN115388914B (en) | Parameter calibration method and device for sensor, storage medium and electronic 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 |