Azimuth determination method, navigation device, self-mobile device and storage medium

Abstract

Embodiments of the present invention provide a method for determining an azimuth angle, a navigation device, a self-moving device, and a storage medium, all belonging to the field of positioning and navigation. The method includes: obtaining a first position of a self-moving device in a world coordinate system at a first moment and a second position at a second moment; obtaining motion data of the self-moving device between the first moment and the second moment; determining a third position and a first azimuth angle of the self-moving device in a carrier coordinate system at the second moment based on the motion data, wherein the origin of the carrier coordinate system is the first position; and determining a second azimuth angle of the self-moving device in the world coordinate system at the second moment based on the first position, the second position, the third position, and the first azimuth angle. This method improves the accuracy and real-time performance of azimuth angles.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to an azimuth determination method, a navigation device, a self-moving device and a storage medium. Background Art

[0002] For positioning and navigation of a mobile device, it is necessary to determine the azimuth of the mobile device. The azimuth refers to the horizontal angle between the direction of motion of the mobile device and the direction of true north. Currently, satellite positioning systems such as the Global Positioning System (GPS) or Real-time kinematic (RTK) are mainly used for positioning and navigation of mobile devices. However, the azimuth of the mobile device is usually determined by a single antenna using the principle of speed measurement. However, the accuracy and real-time performance of the azimuth determined by the single antenna using the principle of speed measurement are low. Summary of the Invention

[0003] The embodiments of the present invention provide an azimuth determination method, a navigation device, a self-moving device, and a storage medium, aiming to reduce costs while improving the accuracy and real-time performance of azimuth during positioning and navigation of the self-moving device.

[0004] In a first aspect, an embodiment of the present invention provides an azimuth angle determination method, comprising: obtaining a first position of a self-moving device at a first moment and a second position at a second moment in a world coordinate system; obtaining motion data of the self-moving device between the first moment and the second moment; determining a third position and a first azimuth angle of the self-moving device in a carrier coordinate system at the second moment based on the motion data; wherein the origin of the carrier coordinate system is the first position; and determining the second azimuth angle of the self-moving device in the world coordinate system at the second moment based on the first position, the second position, the third position and the first azimuth angle.

[0005] In a second aspect, an embodiment of the present invention further provides a navigation device, comprising a motion sensor and a processor, wherein the processor executes the azimuth angle determination method as described in the first aspect to obtain the second azimuth angle, and uses the second azimuth angle for navigation.

[0006] In a third aspect, an embodiment of the present invention further provides a self-moving device, which includes a processor, a memory, a computer program stored in the memory and executable by the processor, and a data bus for realizing connection and communication between the processor and the memory, wherein when the computer program is executed by the processor, the azimuth angle determination method as described above is implemented.

[0007] In a fourth aspect, an embodiment of the present invention further provides a storage medium for computer-readable storage, wherein the storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the azimuth angle determination method as described above.

[0008] An embodiment of the present invention provides an azimuth determination method, a navigation device, a self-mobile device and a storage medium. The azimuth determination method obtains a first position of a self-mobile device at a first moment and a second position at a second moment in a world coordinate system, and obtains motion data of the self-mobile device between the first moment and the second moment, and then establishes a carrier coordinate system of the self-mobile device with the first position as the origin, so that when the self-mobile device performs positioning and navigation, the first azimuth and third position of the self-mobile device in the carrier coordinate system at the second moment can be calculated based on the motion data of the self-mobile device, and then combined with the first position, second position, third position and first azimuth, that is, through the position change of the self-mobile device in the world coordinate system, and combined with the azimuth and position of the motion data in the carrier coordinate system, when converting the azimuth of the self-mobile device, the mapping change between the coordinate systems can be referred to, and then the second azimuth can be calculated accurately and in real time, which greatly improves the accuracy and real-time performance of the azimuth during positioning and navigation of the self-mobile device. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the description of the embodiments. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0010] Figure 1 1 is a flow chart of a method for determining an azimuth angle provided by an embodiment of the present invention;

[0011] Figure 2 is a schematic diagram of a world coordinate system in an embodiment of the present invention;

[0012] Figure 3 is another schematic diagram of the world coordinate system in an embodiment of the present invention;

[0013] Figure 4 is a schematic diagram of a world coordinate system and a carrier coordinate system in an embodiment of the present invention;

[0014] Figure 5 yes Figure 1 A schematic flow chart of a sub-step of the azimuth angle determination method in FIG.

[0015] Figure 6 yes Figure 1A schematic flow chart of another sub-step of the method for determining the azimuth angle;

[0016] Figure 7 yes Figure 1 A schematic flow chart of another sub-step of the azimuth angle determination method in FIG.

[0017] Figure 8 This is a schematic diagram of a scenario for determining an azimuth angle in an embodiment of the present invention;

[0018] Figure 9 This is a schematic block diagram of the structure of a self-moving device provided by an embodiment of the present invention;

[0019] Figure 10 This is a schematic block diagram of the structure of a navigation device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0020] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0021] The flowcharts shown in the accompanying drawings are for illustrative purposes only and do not necessarily include all contents and operations / steps, nor must they be executed in the order described. For example, some operations / steps may be decomposed, combined, or partially merged, so the actual execution order may vary depending on the actual situation.

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

[0023] The following embodiments of the present invention are described in detail with reference to the accompanying drawings. In the absence of conflict, the following embodiments and features therein may be combined with each other.

[0024] Please refer to Figure 1 , Figure 1 It is a flowchart of a method for determining an azimuth angle provided by an embodiment of the present invention.

[0025] like Figure 1 As shown, the azimuth angle determination method includes steps S101 to S104.

[0026] Step S101: Obtain a first position of a mobile device at a first moment and a second position at a second moment in a world coordinate system.

[0027] In the embodiment of the present invention, Figure 2 As shown, the north direction is defined as the x-axis of the world coordinate system, the west direction is defined as the y-axis of the world coordinate system, and the preset longitude, latitude, and altitude are used as the origin of the world coordinate system. The direction of the x-axis in the world coordinate system is the azimuth of 0 degrees, and the clockwise direction from the x-axis is the positive azimuth, and the counterclockwise direction is the negative azimuth. Therefore, the west direction is 90 degrees, the east direction is -90 degrees, and the south direction is the overlap of 180 degrees and -180 degrees. Figure 3 As shown, the first position of the mobile device at the first moment t1 in the world coordinate system is P1(x o ,y o ), the second moment t n The second position of A(x a ,y a ).

[0028] In one embodiment, the latitude, longitude, and altitude information collected by a positioning module in a mobile device at a first moment is obtained, and the latitude, longitude, and altitude information collected by the positioning module at the first moment is converted into plane coordinates of the mobile device in a world coordinate system to obtain a first position of the mobile device at the first moment in the world coordinate system. The latitude, longitude, and altitude information collected by the positioning module in the mobile device at a second moment is obtained, and the latitude, longitude, and altitude information collected by the positioning module at the second moment is converted into plane coordinates of the mobile device in the world coordinate system to obtain a second position of the mobile device at the second moment in the world coordinate system. Converting the latitude and longitude information of the mobile device into plane coordinates can reduce the difficulty of azimuth data processing and increase the calculation speed of azimuth.

[0029] The positioning module may include a global navigation satellite system (GNSS), including but not limited to the global positioning system (GPS), the BeiDou Navigation Satellite System (BDS), the GLONASS (GLOBAL NAVIGATION SATELLITE SYSTEM), and the Galileo satellite positioning system. It is understood that the above-mentioned global navigation satellite systems may use real-time kinematic (RTK) technology for positioning.

[0030] For example, the positioning module collects latitude, longitude, and altitude information once every preset time period, with the first moment and the second moment separated by a preset time period. The preset time period can be set based on actual circumstances and is not specifically limited in this embodiment of the present invention. For example, if the preset time period is 5 minutes, the positioning module collects latitude, longitude, and altitude information from the mobile device every 5 minutes.

[0031] Step S102: Acquire motion data from a mobile device between a first moment and a second moment.

[0032] In an embodiment of the present invention, the motion data includes three-axis acceleration and three-axis angular velocity. The three-axis acceleration is the three-axis acceleration of the self-moving device in the carrier coordinate system, and the three-axis angular velocity is the three-axis angular velocity of the self-moving device in the carrier coordinate system. Alternatively, the motion data includes the travel distance of multiple driving wheels of the self-moving device collected at each sampling interval. The period between the first moment and the second moment includes multiple collection moments, and the period between two adjacent collection moments is one sampling interval. The multiple driving wheels of the self-moving device are used to drive the self-moving device to move. For example, the self-moving device includes a first driving wheel and a second driving wheel. Of course, the self-moving device can also include more driving wheels, and the embodiment of the present invention does not specifically limit this.

[0033] In one embodiment, motion data collected by a motion sensor between a first moment and a second moment is obtained. The motion sensor may include an inertial measurement unit and / or a code wheel tachometer, wherein the inertial measurement unit may be 3-axis, 6-axis, or 9-axis. Multiple collection moments are included between the first moment and the second moment, with a sampling interval between two adjacent collection moments. When a 6-axis inertial measurement unit is used, the motion data collected at each collection moment includes three-axis acceleration and three-axis angular velocity. For code wheel tachometers, the motion data includes the travel distance of multiple drive wheels of the self-propelled device collected at each sampling interval.

[0034] For example, the first moment t1 and the second moment t n There are n acquisition moments between them, which can be expressed as [t1, t2, t3, ..., t n ], the motion data may include the inertial measurement unit at the acquisition time t1, the acquisition time t2, the acquisition time t3, ..., the acquisition time t n The collected three-axis acceleration and three-axis angular velocity and / or code wheel speed meter are collected at sampling intervals t2-t1, sampling intervals t3-t2, ..., sampling intervals t n -t n-1 The collected travel distances of multiple driving wheels of the self-moving device, where n ≥ 2.

[0035] Step S103: Determine a third position and a first azimuth angle of the mobile device in the carrier coordinate system at a second moment based on the motion data, wherein the origin of the carrier coordinate system is the first position.

[0036] In the embodiment of the present invention, Figure 4 As shown, the carrier coordinate system is based on the first position P1 (x o ,y o ) is the origin, the forward direction of the mobile device 11 is the x-axis of the carrier coordinate system, and the direction perpendicular to the forward direction of the mobile device 11 is the y-axis of the carrier coordinate system. The azimuth angle of the mobile device in the carrier coordinate system is defined as follows: the positive direction of the x-axis of the carrier coordinate system is 0 degrees, clockwise rotation is positive, and counterclockwise rotation is negative, that is, the positive direction of the y-axis is 90 degrees, the negative direction of the y-axis is -90 degrees, and the negative direction of the x-axis is 180 degrees and -180 degrees coincides.

[0037] In one embodiment, the motion data includes the three-axis acceleration and three-axis angular velocity collected by the inertial measurement unit at each collection moment. Figure 5 As shown, step S103 includes: sub-steps S1031 to S1033.

[0038] Sub-step S1031: obtaining the moving speed of the mobile device at the first moment.

[0039] Among them, the moving speed of the mobile device at the first moment can also be determined based on the historical position of the mobile device in the world coordinate system, the first position and the collection interval time between the historical position and the first position, and the historical position is the position collected at the historical collection moment before the first moment.

[0040] Exemplarily, the historical position of the mobile device in the world coordinate system and the collection time of the historical position are obtained; the movement distance of the mobile device is calculated based on the first position and the historical position; the collection interval time between the first moment and the collection time of the historical position is determined, and the movement distance of the mobile device is divided by the collection interval time to obtain the movement speed of the mobile device at the first moment.

[0041] Sub-step S1032: Calculate the third position according to the three-axis acceleration and the moving speed.

[0042] Among them, the first moment t1 and the second moment t n There are n acquisition moments between them, which can be expressed as [t1, t2, t3, ..., t n ], the motion data may include the inertial measurement unit at the acquisition time t1, the acquisition time t2, the acquisition time t3, ..., the acquisition time t nThe collected three-axis acceleration and three-axis angular velocity can be used by the mobile device to accurately calculate the third position and first azimuth of the mobile device in the carrier coordinate system at the second moment using a discrete integration method and the inertial measurement unit (IMU). Discrete integration methods include the Euler integration method, the median integration method, or the Runge-Kutta integration method. The following explanation uses the Euler integration method.

[0043] Exemplarily, according to the first position P1, the three-axis acceleration a1 collected by the inertial measurement unit at the first moment t1, and the moving speed v1 of the self-mobile device at the first moment t1, the position P2 of the self-mobile device in the carrier coordinate system at the collection moment t2 is calculated; according to the three-axis acceleration a2 collected by the inertial measurement unit at the collection moment t2, the moving speed v1 of the self-mobile device at the first moment t1 and the sampling interval △t=t2-t1, the moving speed v2 of the self-mobile device at the collection moment t2 is calculated; according to the position P2, the three-axis acceleration a2 collected by the inertial measurement unit at the collection moment t2, and the moving speed v2 of the self-mobile device at the collection moment t2, the position P3 of the self-mobile device in the carrier coordinate system at the collection moment t3 is calculated, and according to the three-axis acceleration a3 collected by the inertial measurement unit at the collection moment t3, the moving speed v2 of the self-mobile device at the collection moment t2 and the sampling interval △t=t3-t2, the moving speed v3 of the self-mobile device at the collection moment t3 is calculated; in the same way as above, the position P3 of the self-mobile device in the carrier coordinate system at the collection moment t3 can be calculated by continuous iteration. n-1 Position P n-1 and movement speed v n-1 , so that according to the position P n-1 , the inertial measurement unit at the acquisition time t n-1 The collected three-axis acceleration a n-1 And the mobile device at the collection time t n-1 Movement speed v n-1 , calculate the second time t n The third position P of the mobile device in the carrier coordinate system n .

[0044] Among them, through the formula The position P of the mobile device in the carrier coordinate system at the acquisition time t+1 can be calculated t+1 , through the formula v t+1 =v t +a t+1 Δt can be used to calculate the moving speed v of the self-mobile device at the collection time t+1 t+1 , P t is the position of the mobile device in the carrier coordinate system at the acquisition time t, a t+1is the three-axis acceleration collected by the inertial measurement unit at the collection time t+1, △t = collection time t+1-collection time t, collection time t+1 and collection time t are two adjacent collection times, v t is the velocity of the self-moving device at the acquisition time t. For example, by substituting the first position P1, the three-axis acceleration a1 acquired by the inertial measurement unit at the first time t1, and the velocity v1 of the self-moving device at the first time t1 into the above formula, the position P2 of the self-moving device in the carrier coordinate system at the acquisition time t2 can be calculated.

[0045] Sub-step S1033: Calculate the yaw angle of the mobile device in the carrier coordinate system at the second moment based on the three-axis angular velocity, and determine the yaw angle as the first azimuth angle.

[0046] For example, the rotation angle q1 of the mobile device in the carrier coordinate system at the first moment t1 is obtained, and the rotation angle q2 of the mobile device in the carrier coordinate system at the acquisition moment t2 is calculated based on the rotation angle q1 and the three-axis angular velocity w1 collected by the inertial measurement unit at the first moment t1; the rotation angle q3 of the mobile device in the carrier coordinate system at the acquisition moment t3 is calculated based on the rotation angle q2 of the mobile device in the carrier coordinate system at the acquisition moment t2 and the three-axis angular velocity w2 collected by the inertial measurement unit at the acquisition moment t2; in the same way as above, the acquisition moment t can be calculated by continuous iteration. n-1 The rotation angle q of the mobile device in the carrier coordinate system n-1 , so that according to the rotation angle q n-1 and the inertial measurement unit at the second time t n The collected three-axis angular velocity w n , calculate the second time t n The rotation angle q of the mobile device in the carrier coordinate system n ; The rotation angle q expressed as a quaternion n Perform Euler angle conversion to obtain the roll angle, pitch angle and yaw angle of the mobile device in the carrier coordinate system at the second moment, and determine the yaw angle as the first azimuth angle of the mobile device in the carrier coordinate system at the second moment.

[0047] For example, by the formula The rotation angle q of the mobile device in the carrier coordinate system at the acquisition time t+1 can be calculated t+1 ,q t is the rotation angle of the mobile device in the carrier coordinate system at the acquisition time t, w t+1is the three-axis angular velocity collected by the IMU at acquisition time t+1. For example, by substituting the rotation angle q1 of the self-mobile device in the carrier coordinate system at the first time t1 and the three-axis angular velocity w1 collected by the IMU at the first time t1 into the above formula, the rotation angle q2 of the self-mobile device in the carrier coordinate system at the acquisition time t2 can be calculated.

[0048] In an embodiment of the present invention, the motion data collected by the inertial measurement unit can be combined with the position collected by a single-antenna GPS or RTK to obtain the azimuth angle when performing positioning and navigation on a self-mobile device. This eliminates the need for components such as a dual-antenna satellite positioning receiver or magnetometer required by related technologies, thereby reducing costs. Furthermore, compared to solutions that use only a single-antenna GPS or RTK receiver to measure speed and calculate azimuth angles, the azimuth angle calculated by combining the motion data collected by the inertial measurement unit with the position collected by GPS or RTK is more accurate. Furthermore, since the inertial measurement unit is a high-frequency data sensor, the calculated azimuth angle is also more real-time, thereby improving the accuracy and real-time performance of the azimuth angle during positioning and navigation on a self-mobile device while reducing costs.

[0049] In one embodiment, the motion data includes the travel distance of multiple driving wheels of the self-moving device collected by the code wheel speed meter at each sampling interval. Figure 6 As shown, step S103 includes: sub-steps S1034 to S1037.

[0050] Sub-step S1034: Calculate the moving distance of the mobile device in each sampling interval based on the walking distance collected in each sampling interval and a preset moving distance calculation formula.

[0051] In the embodiment of the present invention, the preset movement distance calculation formula is related to the motion model of the self-moving device. For example, the self-moving device includes a first driving wheel and a second driving wheel. When the first driving wheel and the second driving wheel rotate, the self-moving device can be driven forward or backward, but cannot perform lateral movement. This indicates that the motion model of the self-moving device is a differential wheel motion model. Therefore, the lateral movement distance d of the self-moving device in the y direction in the carrier coordinate system is my = 0, in the forward direction of the self-moving device, the travel distance of the first driving wheel is d L , the travel distance of the second driving wheel is d R , then the calculation formula of the moving distance of the mobile device in the x-axis direction in the carrier coordinate system can be expressed as: d mx =(d L +d R ) / 2. It is understandable that different motion models of self-moving devices lead to different calculation formulas for moving distances, and the present invention does not impose any specific limitation on this.

[0052] For example, within the sampling interval t2-t1, the travel distance of the first driving wheel and the travel distance of the second driving wheel of the self-moving device are d L1 and d R1 In the sampling interval t3-t2, the travel distance of the first driving wheel and the travel distance of the second driving wheel of the self-moving device are d L2 and d R2 ,..., in the sampling interval t n -t n-1 The travel distances of the first driving wheel and the second driving wheel of the self-propelled device are d Ln and d Rn Calculate the moving distance using the formula d mx =(d L +d R ) / 2, we can calculate the sampling interval t2-t1, sampling interval t3-t2, ..., sampling interval t n -t n-1 The moving distances within are d mx1 =(d L1 +d R1 ) / 2, d mx2 =(d L2 +d R2 ) / 2, ..., d mxn-1 =(d Ln +d Rn ) / 2.

[0053] For example, the distance traveled by the drive wheel is related to the number of pulses per rotation of the encoder wheel tachometer motor, the gearbox reduction ratio between the motor and the drive wheel, and the outer radius of the drive wheel. For example, if the number of pulses per rotation of the encoder wheel tachometer motor is m, the gearbox reduction ratio between the motor and the drive wheel is k, the outer radius of the drive wheel is r, and the number of pulses output by the encoder wheel tachometer in a sampling interval is e, then the distance traveled by the drive wheel in a sampling interval can be expressed as: d = 2πre / (k×m).

[0054] Sub-step S1035: Calculate the azimuth change of the mobile device in each sampling interval based on the walking distance collected in each sampling interval and a preset azimuth change calculation formula.

[0055] In the embodiment of the present invention, the preset azimuth angle change calculation formula is related to the motion model of the self-moving device. For example, the self-moving device includes a first drive wheel and a second drive wheel. When the first drive wheel and the second drive wheel rotate, the self-moving device can be driven forward or backward, but cannot perform lateral movement. This indicates that the motion model of the self-moving device is a differential wheel motion model. Let the travel distance of the first drive wheel be d L , the travel distance of the second driving wheel is dR , then the azimuth angle change calculation formula can be expressed as Δα=(d L -d R ) / 2. It is understandable that the calculation formula for the azimuth angle change is different depending on the motion model of the self-moving device, and the present invention does not make any specific limitation on this.

[0056] For example, within the sampling interval t2-t1, the travel distance of the first driving wheel and the travel distance of the second driving wheel of the self-moving device are d L1 and d R1 In the sampling interval t3-t2, the travel distance of the first driving wheel and the travel distance of the second driving wheel of the self-moving device are d L2 and d R2 ,..., in the sampling interval t n -t n-1 The travel distances of the first driving wheel and the second driving wheel of the self-propelled device are d Ln and d Rn The azimuth angle change is calculated by the formula Δα=(d L -d R ) / 2, we can calculate the sampling interval t2-t1, sampling interval t3-t2, ..., sampling interval t n -t n-1 The azimuth angle changes are Δα1=(d L1 -d R1 ) / 2, Δα2=(d L2 -d R2 ) / 2, ..., Δα n-1 =(d Ln -d Rn ) / 2.

[0057] Sub-step S1036: Accumulate the third azimuth angle of the mobile device at the first moment in the carrier coordinate system and the azimuth angle change in each sampling interval to obtain a first azimuth angle.

[0058] For example, let the third position angle of the mobile device at the first moment t1 in the carrier coordinate system be α1, and the mobile device at the sampling interval t2-t1, sampling interval t3-t2, ..., sampling interval t n -t n-1 The azimuth angle changes are Δα1=(d L1 -d R1 ) / 2, Δα2=(d L2 -d R2 ) / 2, ..., Δα n-1 =(d Ln -d Rn) / 2, then α1, Δα1, Δα2,..., Δα n-1 Accumulate and get the first azimuth angle α n .

[0059] Sub-step S1037: Calculate the third position of the mobile device in the carrier coordinate system at the second moment according to each moving distance and the azimuth angle change in each sampling interval.

[0060] For example, the mobile device is in the sampling interval t2-t1, the sampling interval t3-t2, ..., the sampling interval t n -t n-1 The moving distances within are d mx1 =(d L1 +d R1 ) / 2, d mx2 =(d L2 +d R2 ) / 2, ..., d mxn-1 =(d Ln +d Rn ) / 2, the self-mobile device is in the sampling interval t2-t1, sampling interval t3-t2, ..., sampling interval t n -t n-1 The azimuth angle changes are Δα1=(d L1 -d R1 ) / 2, Δα2=(d L2 -d R2 ) / 2, ..., Δα n-1 =(d Ln -d Rn ) / 2, then based on the above data, the second moment t n The third position of the self-moving device in the carrier coordinate system.

[0061] In one embodiment, the azimuth angle of the mobile device at each acquisition moment in the carrier coordinate system is calculated based on the third azimuth angle and each azimuth angle change; the position change of the mobile device in each sampling interval in the carrier coordinate system is calculated based on each moving distance and the azimuth angle at each acquisition moment; the position change in each sampling interval is accumulated to obtain the third position.

[0062] For example, the mobile device is in the sampling interval t2-t1, the sampling interval t3-t2, ..., the sampling interval t n -t n-1 The azimuth angle changes are Δα1=(d L1 -d R1 ) / 2, Δα2=(d L2 -d R2 ) / 2, ..., Δα n-1 =(dLn -d Rn ) / 2, then the mobile device is at the first moment t1, the collection moment t2, the collection moment t3, ..., the second moment t n The azimuth angles in the carrier coordinate system are α1, α2 = α1 + Δα1, α 3＝ α2+Δα2, ..., α n＝ α n-1 +Δα n-1 .

[0063] For example, based on the formula Δx=d mx ×cosα,Δy=d mx × sinα, calculate the position change of the mobile device in each sampling interval in the carrier coordinate system based on each moving distance and the azimuth angle at each sampling moment. mx is the moving distance of the mobile device within the sampling interval, and α is the azimuth angle of the mobile device at different sampling moments.

[0064] For example, the mobile device is in the sampling interval t2-t1, the sampling interval t3-t2, ..., the sampling interval t n -t n-1 The moving distances within are d mx1 =(d L1 +d R1 ) / 2, d mx2 =(d L2 +d R2 ) / 2, ..., d mxn =(d Ln +d Rn ) / 2, and the mobile device is at the first time t1, the collection time t2, the collection time t3, ..., the second time t n The azimuth angles in the carrier coordinate system are α1, α2, α3, ..., α n Then the mobile device is in the sampling interval t2-t1, sampling interval t3-t2, ..., sampling interval t n -t n-1 The position changes within are Δx1=d mx1 ×cosα2,Δy1=d mx1 ×sinα2、Δx2=d mx2 ×cosα3,Δy2=d mx2 ×sinα3、...、Δx n-1 =d mxn-1 ×cosα n ,Δy n-1 =d mxn-1 ×sinα n .

[0065] In embodiments of the present invention, the motion data collected by the encoder wheel tachometer can be combined with the position collected by a single-antenna GPS or RTK to obtain azimuth during positioning and navigation on a self-mobile device. This eliminates the need for dual-antenna satellite positioning receivers or magnetometers, thereby reducing costs. Furthermore, compared to solutions that use only a single-antenna GPS or RTK receiver to measure speed and calculate azimuth, the azimuth calculated by combining the motion data collected by the encoder wheel tachometer with the position collected by GPS or RTK is more accurate. Furthermore, since the encoder wheel tachometer is a high-frequency data sensor, the calculated azimuth is also more real-time. This improves the accuracy and real-time performance of azimuth during positioning and navigation on a self-mobile device while reducing costs.

[0066] Step S104: Determine a second azimuth angle of the mobile device in the world coordinate system at a second moment based on the first position, the second position, the third position, and the first azimuth angle.

[0067] In the embodiment of the present invention, the second azimuth angle of the mobile device in the world coordinate system at the second moment can be quickly calculated using the first position, the second position, the third position and the first azimuth angle.

[0068] In one embodiment, if Figure 7 As shown, step S104 includes: sub-steps S1041 to S1043.

[0069] Sub-step S1041: Calculate a first angle between the moving direction of the mobile device and the horizontal axis of the world coordinate system at the second moment based on the first position and the second position.

[0070] like Figure 8 As shown, the first position P1(x o ,y o ) and the second position A(x a ,y a ) and the auxiliary line 13 parallel to the horizontal axis of the world coordinate system is the first angle between the forward direction of the self-moving device and the horizontal axis of the world coordinate system at the second moment.

[0071] For example, the slope of the straight line formed by the first position and the second position is obtained, and the arc tangent of the slope is calculated to obtain the first angle. o ,y o ) and the second position A(x a ,y a ) is the slope of the straight line formed by ao =(y a -y o ) / (x a -x o ), then the first angle β=arctan(kao ).

[0072] Sub-step S1042: Calculate a second angle between the mobile device and the horizontal axis of the carrier coordinate system at the second moment based on the third position.

[0073] like Figure 8 As shown, at the second moment, the third position A′(x oa ,y oa ) and the origin of the carrier coordinate system and the angle between the horizontal axis of the carrier coordinate system is the second included angle between the mobile device and the horizontal axis of the carrier coordinate system at the second moment.

[0074] For example, the slope of the straight line formed by the third position of the mobile device in the carrier coordinate system at the second moment and the origin of the carrier coordinate system is calculated, and the arc tangent of the slope is calculated to obtain the second angle. For example, the slope of the straight line formed by the third position of the mobile device in the carrier coordinate system at the second moment and the origin of the carrier coordinate system is (y oa / x oa ), then the second angle

[0075] Sub-step S1043: Calculate a second azimuth angle of the mobile device in the world coordinate system at the second moment based on the first included angle, the second included angle, and the first azimuth angle.

[0076] Exemplarily, the second angle is subtracted from the first angle to obtain the azimuth of the mobile device in the world coordinate system at the first moment; the first azimuth and the azimuth of the mobile device in the world coordinate system at the first moment are added to obtain the second azimuth.

[0077] like Figure 8 As shown, the angle γ between the line segment 14 and the x-axis of the carrier coordinate system is the first azimuth angle, the angle θ1 between the auxiliary line 13 parallel to the horizontal axis of the world coordinate system and the horizontal axis of the carrier coordinate system is the azimuth angle of the self-moving device in the world coordinate system at the first moment, and the angle θ2 between the line segment 14 and the auxiliary line 12 parallel to the horizontal axis of the world coordinate system is the second azimuth angle of the self-moving device in the world coordinate system at the second moment. In the world coordinate system, the second angle It is obtained by rotating from the direction of P1B, passing through the north direction 0 degrees and then rotating to the direction of P1A. Therefore, the second angle In the formula, θ1 is on the clockwise rotation side of 0 degrees, so θ1 is a negative number, and β is on the counterclockwise rotation side of 0 degrees, so β ​​is a positive number. The azimuth angle θ1 of the mobile device at the first moment in the world coordinate system is obtained, and the second azimuth angle θ2 = θ1 + γ.

[0078] The azimuth angle determination method provided in the above embodiment obtains the first position of the mobile device at the first moment and the second position at the second moment in the world coordinate system, and obtains the motion data of the mobile device between the first moment and the second moment, and then establishes the carrier coordinate system of the mobile device with the first position as the origin, so that when the mobile device is performing positioning and navigation, the first azimuth angle and the third position of the mobile device in the carrier coordinate system at the second moment can be calculated based on the motion data of the mobile device, and then combined with the first position, the second position, the third position and the first azimuth angle, that is, through the position change of the mobile device in the world coordinate system, and combined with the azimuth angle and position of the motion data in the carrier coordinate system, when converting the azimuth angle of the mobile device, the mapping change between the coordinate systems can be referred to, and then the second azimuth angle can be calculated accurately and in real time, which greatly improves the accuracy and real-time performance of the azimuth angle during positioning and navigation of the mobile device. In addition, the azimuth angle determination method provided in the embodiment of the present invention does not require the use of additional components such as dual-antenna satellite positioning receivers or magnetometers, thereby reducing costs.

[0079] See also Figure 9 , Figure 9 This is a schematic block diagram of the structure of a self-moving device provided by an embodiment of the present invention.

[0080] like Figure 9 As shown, the mobile device 200 includes a processor 201 and a memory 202 , and the processor 201 and the memory 202 are connected via a bus 203 , such as an I 2 C (Inter-integrated Circuit) bus.

[0081] Specifically, processor 201 is used to provide computing and control capabilities to support the operation of the entire mobile device. Processor 201 can be a central processing unit (CPU), and processor 301 can also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. Among them, the general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc.

[0082] Specifically, the memory 202 may be a Flash chip, a read-only memory (ROM) disk, an optical disk, a USB flash drive, or a mobile hard disk.

[0083] Those skilled in the art will understand that Figure 9 The structure shown in the figure is merely a block diagram of a portion of the structure related to the embodiment of the present invention, and does not constitute a limitation on the self-moving device to which the embodiment of the present invention is applied. The specific self-moving device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0084] The processor 201 is configured to run a computer program stored in the memory 202 and implement any one of the azimuth angle determination methods provided by the embodiments of the present invention when executing the computer program.

[0085] In one embodiment, the processor 201 is configured to run a computer program stored in the memory 202, and implement the following steps when executing the computer program:

[0086] Obtaining a first position of the mobile device at a first moment and a second position at a second moment in the world coordinate system;

[0087] Acquire motion data of the mobile device between a first moment and a second moment;

[0088] Determining, based on the motion data, a third position and a first azimuth angle of the self-moving device in a carrier coordinate system at a second moment; wherein the origin of the carrier coordinate system is the first position;

[0089] A second azimuth angle of the self-moving device in the world coordinate system at a second moment is determined based on the first position, the second position, the third position and the first azimuth angle.

[0090] In one embodiment, the motion data includes three-axis acceleration and three-axis angular velocity. When determining the third position and the first azimuth angle of the self-moving device in the carrier coordinate system at the second moment based on the motion data, the processor 201 is configured to implement:

[0091] Obtaining a moving speed of the mobile device at the first moment;

[0092] Calculating a third position according to the three-axis acceleration and the moving speed;

[0093] The yaw angle of the self-moving device in the carrier coordinate system at the second moment is calculated according to the three-axis angular velocity, and the yaw angle is determined as the first azimuth angle.

[0094] In one embodiment, the first moment and the second moment include multiple collection moments, and the interval between two adjacent collection moments is a sampling interval. The motion data includes the travel distance of the multiple driving wheels of the self-moving device collected in each sampling interval. When implemented, the processor 201 is configured to:

[0095] Calculating the moving distance of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset moving distance calculation formula;

[0096] Calculating the azimuth change of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset azimuth change calculation formula;

[0097] Accumulate the third azimuth angle of the mobile device at the first moment in the carrier coordinate system and the azimuth angle change in each sampling interval to obtain the first azimuth angle;

[0098] The third position of the self-moving device in the carrier coordinate system at the second moment is calculated according to each moving distance and the azimuth angle change in each sampling interval.

[0099] In one embodiment, when the processor 201 calculates the third position of the mobile device in the carrier coordinate system at the second moment based on each movement distance and the azimuth angle change in each sampling interval, it is configured to implement:

[0100] Calculating the azimuth of the mobile device at each acquisition moment in the carrier coordinate system according to the third azimuth and each azimuth change;

[0101] Calculating the position change of the mobile device in each sampling interval in the carrier coordinate system according to each movement distance and the azimuth angle of each collection moment;

[0102] The position change amount in each sampling interval is accumulated to obtain the third position.

[0103] In one embodiment, when determining, based on the first position, the second position, the third position, and the first azimuth, the second azimuth of the self-mobile device in the world coordinate system at the second moment, the processor 201 is configured to implement:

[0104] Calculating, based on the first position and the second position, a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at a second moment;

[0105] Calculating, based on the third position, a second angle between the self-moving device and the horizontal axis of the carrier coordinate system at a second moment;

[0106] A second azimuth angle of the self-moving device in the world coordinate system at a second moment is calculated based on the first included angle, the second included angle, and the first azimuth angle.

[0107] In one embodiment, when the processor 201 calculates the second azimuth angle of the mobile device in the world coordinate system at the second moment based on the first angle, the second angle, and the first azimuth angle, it is configured to implement:

[0108] Subtracting the second angle from the first angle to obtain the azimuth of the mobile device in the world coordinate system at the first moment;

[0109] The first azimuth angle and the azimuth angle of the self-moving device in the world coordinate system at the first moment are added to obtain the second azimuth angle.

[0110] In one embodiment, when the processor 201 calculates, based on the first position and the second position, a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at the second moment, it is configured to implement:

[0111] The slope of a straight line formed by the first position and the second position is obtained, and an arc tangent value of the slope is calculated to obtain the first angle.

[0112] It should be noted that those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working process of the self-moving device described above can refer to the corresponding process in the aforementioned azimuth angle determination method embodiment, and will not be repeated here.

[0113] See also Figure 10 , Figure 10 This is a schematic block diagram of the structure of a navigation device provided by an embodiment of the present invention.

[0114] like Figure 10 As shown, the navigation device includes a motion sensor 301 and a processor 302, which are connected via a bus 303, such as an I2C (Inter-integrated Circuit) bus. The motion sensor 301 may include an inertial measurement unit and / or a wheel speedometer.

[0115] The processor 302 is configured to perform the following steps:

[0116] Obtaining a first position of the mobile device at a first moment and a second position at a second moment in the world coordinate system;

[0117] Acquire motion data of the mobile device between a first moment and a second moment;

[0118] Determining, based on the motion data, a third position and a first azimuth angle of the self-moving device in a carrier coordinate system at a second moment; wherein the origin of the carrier coordinate system is the first position;

[0119] determining a second azimuth angle of the self-moving device in the world coordinate system at a second moment based on the first position, the second position, the third position, and the first azimuth angle;

[0120] Navigation is performed using the second azimuth angle.

[0121] In one embodiment, the motion data includes three-axis acceleration and three-axis angular velocity. When determining the third position and the first azimuth angle of the self-moving device in the carrier coordinate system at the second moment based on the motion data, the processor 302 is configured to implement:

[0122] Obtaining a moving speed of the mobile device at the first moment;

[0123] Calculating a third position according to the three-axis acceleration and the moving speed;

[0124] The yaw angle of the self-moving device in the carrier coordinate system at the second moment is calculated according to the three-axis angular velocity, and the yaw angle is determined as the first azimuth angle.

[0125] In one embodiment, the first moment and the second moment include multiple collection moments, and the interval between two adjacent collection moments is a sampling interval. The motion data includes the travel distance of the multiple driving wheels of the self-moving device collected in each sampling interval. When implemented, the processor 302 is configured to:

[0126] Calculating the moving distance of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset moving distance calculation formula;

[0127] Calculating the azimuth change of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset azimuth change calculation formula;

[0128] Accumulate the third azimuth angle of the mobile device at the first moment in the carrier coordinate system and the azimuth angle change in each sampling interval to obtain the first azimuth angle;

[0129] The third position of the self-moving device in the carrier coordinate system at the second moment is calculated according to each moving distance and the azimuth angle change in each sampling interval.

[0130] In one embodiment, when the processor 302 calculates the third position of the mobile device in the carrier coordinate system at the second moment based on each movement distance and the azimuth angle change in each sampling interval, it is configured to implement:

[0131] Calculating the azimuth of the mobile device at each acquisition moment in the carrier coordinate system according to the third azimuth and each azimuth change;

[0132] Calculating the position change of the mobile device in each sampling interval in the carrier coordinate system according to each movement distance and the azimuth angle of each collection moment;

[0133] The position change amount in each sampling interval is accumulated to obtain the third position.

[0134] In one embodiment, when determining, based on the first position, the second position, the third position, and the first azimuth, the second azimuth of the self-mobile device in the world coordinate system at the second moment, the processor 302 is configured to implement:

[0135] Calculating, based on the first position and the second position, a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at a second moment;

[0136] Calculating a second angle between the self-moving device and the horizontal axis of the carrier coordinate system at a second moment based on the third position;

[0137] A second azimuth angle of the self-moving device in the world coordinate system at a second moment is calculated based on the first included angle, the second included angle, and the first azimuth angle.

[0138] In one embodiment, when the processor 302 calculates the second azimuth angle of the mobile device in the world coordinate system at the second moment based on the first included angle, the second included angle, and the first azimuth angle, it is configured to implement:

[0139] Subtracting the second angle from the first angle to obtain the azimuth of the mobile device in the world coordinate system at the first moment;

[0140] The first azimuth angle and the azimuth angle of the self-moving device in the world coordinate system at the first moment are added to obtain the second azimuth angle.

[0141] In one embodiment, when the processor 302 calculates, based on the first position and the second position, a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at the second moment, the processor 302 is configured to implement:

[0142] The slope of a straight line formed by the first position and the second position is obtained, and an arc tangent value of the slope is calculated to obtain the first angle.

[0143] It should be noted that those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working process of the navigation device described above can refer to the corresponding process in the aforementioned azimuth determination method embodiment, and will not be repeated here.

[0144] An embodiment of the present invention also provides a storage medium for computer-readable storage, wherein the storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement any azimuth angle determination method provided in the description of the embodiment of the present invention.

[0145] The storage medium may be an internal storage unit of the mobile device described in the aforementioned embodiment, such as a hard disk or memory of the mobile device. The storage medium may also be an external storage device of the mobile device, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc., equipped on the mobile device.

[0146] It will be appreciated by those skilled in the art that all or some of the steps, systems, and functional modules / units in the methods disclosed above may be implemented as software, firmware, hardware, and appropriate combinations thereof. In a hardware embodiment, the division between the functional modules / units mentioned in the above description does not necessarily correspond to the division of physical components; for example, a physical component may have multiple functions, or a function or step may be performed by several physical components in cooperation. Some or all physical components may be implemented as software executed by a processor, such as a central processing unit, a digital signal processor, or a microprocessor, or implemented as hardware, or implemented as an integrated circuit, such as an application-specific integrated circuit. Such software may be distributed on a computer-readable medium, which may include a computer storage medium (or non-transitory medium) and a communication medium (or temporary medium). As is well known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable, and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules, or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to store the desired information and can be accessed by a computer. In addition, it is well known to those skilled in the art that communication media generally embodies computer-readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0147] It should be understood that the term "and / or" used in the present specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, including these combinations. It should be noted that, in this article, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system that includes a series of elements includes not only those elements, but also includes other elements that are not explicitly listed, or also includes elements that are inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "including a..." does not exclude the presence of other identical elements in the process, method, article or system that includes the element.

[0148] The serial numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments. The above description is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be based on the scope of protection of the claims.

Claims

1. A method for determining an azimuth angle, characterized in that: include: Obtaining a first position of the mobile device at a first moment and a second position at a second moment in the world coordinate system; Acquire motion data of the mobile device between a first moment and a second moment; Determining, based on the motion data, a third position and a first azimuth angle of the self-moving device in a carrier coordinate system at a second moment; wherein the origin of the carrier coordinate system is the first position; determining a second azimuth angle of the self-moving device in the world coordinate system at a second moment based on the first position, the second position, the third position, and the first azimuth angle; The step of determining a second azimuth angle of the mobile device in the world coordinate system at a second moment based on the first position, the second position, the third position, and the first azimuth angle includes: Calculating, based on the first position and the second position, a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at a second moment; Calculating, based on the third position, a second angle between the self-moving device and the horizontal axis of the carrier coordinate system at a second moment; A second azimuth angle of the self-moving device in the world coordinate system at a second moment is calculated based on the first included angle, the second included angle, and the first azimuth angle.

2. The method for determining the azimuth angle according to claim 1, wherein: The motion data includes three-axis acceleration and three-axis angular velocity, and determining, based on the motion data, a third position and a first azimuth angle of the self-moving device in the carrier coordinate system at a second moment, includes: Obtaining a moving speed of the mobile device at the first moment; Calculating a third position according to the three-axis acceleration and the moving speed; The yaw angle of the self-moving device in the carrier coordinate system at the second moment is calculated according to the three-axis angular velocity, and the yaw angle is determined as the first azimuth angle.

3. The method for determining an azimuth angle according to claim 1, wherein: The first moment and the second moment include multiple collection moments, and the interval between two adjacent collection moments is a sampling interval. The motion data includes the travel distance of the multiple driving wheels of the self-moving device collected in each sampling interval. Determining the third position and the first azimuth angle of the self-moving device in the carrier coordinate system at the second moment based on the motion data includes: Calculating the moving distance of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset moving distance calculation formula; Calculating the azimuth change of the mobile device in each sampling interval according to the walking distance collected in each sampling interval and a preset azimuth change calculation formula; Accumulate the third azimuth angle of the mobile device at the first moment in the carrier coordinate system and the azimuth angle change in each sampling interval to obtain the first azimuth angle; The third position of the self-moving device in the carrier coordinate system at the second moment is calculated according to each moving distance and the azimuth angle change in each sampling interval.

4. The method for determining the azimuth angle according to claim 3, wherein: Calculating the third position of the mobile device in the carrier coordinate system at the second moment according to each movement distance and the azimuth angle change in each sampling interval includes: Calculating the azimuth of the mobile device at each acquisition moment in the carrier coordinate system according to the third azimuth and each azimuth change; Calculating the position change of the mobile device in each sampling interval in the carrier coordinate system according to each movement distance and the azimuth angle of each collection moment; The position change amount in each sampling interval is accumulated to obtain the third position.

5. The azimuth angle determination method according to claim 1, wherein: Calculating a second azimuth angle of the mobile device in the world coordinate system at a second moment based on the first included angle, the second included angle, and the first azimuth angle includes: Subtracting the second angle from the first angle to obtain the azimuth of the mobile device in the world coordinate system at the first moment; The first azimuth angle and the azimuth angle of the self-moving device in the world coordinate system at the first moment are added to obtain the second azimuth angle.

6. The method for determining an azimuth angle according to claim 1, wherein: Calculating a first angle between the moving direction of the self-moving device and the horizontal axis of the world coordinate system at a second moment based on the first position and the second position includes: The slope of a straight line formed by the first position and the second position is obtained, and an arc tangent value of the slope is calculated to obtain the first angle.

7. A navigation device, characterized in that: The device comprises a motion sensor and a processor, wherein the processor executes the azimuth angle determination method according to any one of claims 1 to 6 to obtain the second azimuth angle, and uses the second azimuth angle for navigation.

8. A self-propelled device, characterized in that: The self-mobile device includes a processor, a memory, a computer program stored on the memory and executable by the processor, and a data bus for realizing connection and communication between the processor and the memory, wherein when the computer program is executed by the processor, the azimuth angle determination method according to any one of claims 1 to 6 is implemented.

9. A storage medium for computer-readable storage, characterized in that: The storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the azimuth angle determination method according to any one of claims 1 to 6.

Images