Mining area robot harmful acceleration compensation method and system and orientation method and system

Abstract

The invention provides a mining area robot harmful acceleration compensation method and system, and an orientation method and system, and the method comprises the steps: employing a staged reset method to initialize an inertial navigation system when motion acceleration occurs in the motion process of a mining area robot through employing the non-divergence characteristic of the inertial navigation algorithm during short-term operation, and enabling the inertial navigation system to be in a non-divergence state; the method comprises the following steps: acquiring motion acceleration of a robot under a navigation system, and then compensating original data containing harmful motion acceleration output by an accelerometer in a strapdown compass system to eliminate orientation errors generated by the motion acceleration. According to the invention, the strapdown compass system and the staged reset inertial navigation system are combined, the characteristic that the inertial navigation system is not divergent in short-term operation is utilized, harmful acceleration can be acquired and compensated, errors caused by motion acceleration in a strapdown compass alignment and orientation loop are eliminated, and the accuracy of the calibration is improved. The technical problems that in the prior art, due to output errors caused by complex maneuvering motion, normal use of mining area robots is affected, and safety risks are caused are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot navigation and positioning, and particularly relates to a method and system for compensating harmful acceleration of a mining area robot, and a method and system for orientation. Background Art

[0002] Robots are generally used in complex industrial and living scenarios. They are designed to help humans work in various toxic, harmful, and dangerous environments. These robots usually have high flexibility and certain intelligence, and can perform a variety of complex tasks. For example, mining area robots play an important role in mining operations. They can not only improve productivity and safety, reduce costs, but also promote the sustainable development of the mining industry. With the continuous progress of technology and the continuous deepening of applications, mining area robots will surely play a more important role in the complex mining production field.

[0003] Since most mining area environments are complex and changeable, during the use of mining area robots, precise navigation of them is an important guarantee for ensuring accurate positioning and path planning, improving autonomous operation ability, enhancing safety performance, and promoting the intelligent level of operations. The strapdown compass system has been widely used in the high-precision navigation and positioning of various mining area robots due to its advantages of strong autonomy, being less susceptible to external environmental interference, and high orientation accuracy.

[0004] In actual use, due to the complex and changeable environment roads, etc., mining area robots are often in a non-uniform motion state, and thus often have motion acceleration. For the strapdown compass system, motion acceleration is a main harmful acceleration. When there is motion acceleration, the strapdown compass system often has measurement errors, which affects the normal use of mining area robots. Therefore, it is of great significance to compensate for harmful acceleration and perform navigation and orientation on mining area robots in applications.

[0005] In the prior art, a recursive inertial navigation algorithm is often used to compensate for the harmful acceleration of the strapdown compass system of mining area robots. However, due to the calculation principle of the recursive algorithm and device errors, during long-term application, its output results will diverge over time, and there will still be problems of error accumulation during operation, which affects the normal use of mining area robots and even poses safety risks. Summary of the Invention

[0006] In order to solve the technical problems in the background art that when using a recursive inertial navigation algorithm to compensate for harmful acceleration, during long-term application, its output results will diverge over time, and there will be problems of error accumulation during operation, which affects the normal use of robots and even poses safety risks, the present invention provides a method and system for compensating harmful acceleration of a mining area robot, and a method and system for orientation.

[0007] To achieve the above object, the present invention adopts the following technical solutions:

[0008] In a first aspect, the present invention provides a method for compensating harmful acceleration of a mining area robot, including:

[0009] S1: Obtain the initial data of the strapdown compass system and the odometer as the first data;

[0010] S2: When there is a motion acceleration, initialize the inertial navigation system according to the first data, set the reset interval period of the inertial navigation system, and obtain the initial data of the inertial navigation system as the second data;

[0011] S3: Real-time obtain the running time of the inertial navigation system, compare the running time of the inertial navigation system with its reset interval period. If the running time is greater than or equal to the reset interval period, re-execute step S1; if the running time is less than the reset interval period, execute step S4;

[0012] S4: Based on the second data, calculate the motion acceleration of the inertial navigation system in the navigation system in real time;

[0013] S5: According to the motion acceleration of the inertial navigation system in the navigation system, perform harmful acceleration compensation in real time.

[0014] Optionally, in step S1, the first data includes: when in a static or uniform motion state, the attitude matrix of the strapdown compass system in the vehicle body coordinate system, the longitude and latitude of the mining area robot, and the speed of the odometer in the vehicle body coordinate system.

[0015] Optionally, in step S2, the second data includes: the initial longitude and latitude, the initial attitude matrix, and the initial speed of the inertial navigation system.

[0016] Optionally, when initializing the inertial navigation system in step S2, it specifically includes:

[0017] Based on the attitude matrix of the strapdown compass system in the vehicle body coordinate system and the speed of the odometer in the vehicle body coordinate system calculate the speed of the odometer in the navigation system

[0018] Take the speed of the odometer in the navigation system as the initial speed of the inertial navigation system

[0019] Based on the speed of the odometer in the navigation system and combining with the longitude and latitude of the mining area robot, calculate the initial longitude and latitude of the inertial navigation system;

[0020] Based on the attitude matrix of the strapdown compass system in the vehicle body coordinate system Calculate the initial attitude matrix of the inertial navigation system

[0021] Optionally, in step S2, obtain the initial velocity of the inertial navigation system The specific process is as follows:

[0022]

[0023] Among them, are the three-axis velocity components of the odometer in the navigation system. Among them, is the eastward velocity component of the odometer in the navigation system, unit: m / s, is the northward velocity component of the odometer in the navigation system, unit: m / s, is the upward velocity component of the odometer in the navigation system, unit: m / s;

[0024] Based on the velocity of the odometer in the navigation system Combined with the longitude and latitude of the mining area robot, the specific process of obtaining the initial longitude and latitude of the inertial navigation system is as follows:

[0025]

[0026] Among them, L 0_INS is the initial latitude of the inertial navigation system, unit: °, λ 0_INS is the initial longitude of the inertial navigation system, unit: °, L0 is the latitude of the mining area robot, unit: °, λ0 is the longitude of the mining area robot, unit: °, is the northward velocity component of the odometer in the navigation system, unit: m / s, is the eastward velocity component of the odometer in the navigation system, unit: m / s, R e is the radius of the earth, unit: m;

[0027] The initial attitude matrix of the inertial navigation system is consistent with the attitude matrix of the strapdown compass system in the navigation system That is

[0028] Optionally, in step S4, calculate the motion acceleration of the inertial navigation system in the navigation system is obtained based on the longitude and latitude [L i_INS λ i_INS output by the inertial navigation system in real time T , the attitude matrix in the navigation system and the velocity as well as the acceleration in the vehicle body coordinate system, where i = 1, 2, 3... k, and k is the number of times the inertial navigation system continuously outputs data after one initialization.

[0029] Optionally, in the step S5, the process of performing real-time harmful acceleration compensation is as follows:

[0030]

[0031] where is the real-time attitude matrix of the strapdown compass system, is the acceleration data output by the accelerometer after performing real-time harmful acceleration compensation in the navigation system, unit: m / s 2 , is the acceleration component output by the accelerometer after performing real-time harmful acceleration compensation in the navigation system, unit: m / s 2 , is the acceleration component output in real time by the accelerometer in the body frame before harmful acceleration compensation, unit: m / s 2 , i = 1, 2, 3... k, where k is the number of times the inertial navigation system continuously outputs data after one initialization.

[0032] In a second aspect, the present invention provides a harmful acceleration compensation system for a mining area robot, which is used for any one of the above-mentioned harmful acceleration compensation methods for mining area robots, and includes:

[0033] A data acquisition unit that acquires the initial data of the strapdown compass system and the odometer as first data, acquires the initial data of the inertial navigation system as second data, and acquires the running time of the inertial navigation system;

[0034] A judgment unit for judging whether there is a motion acceleration and judging the magnitude relationship between the running time of the inertial navigation system and its reset interval period;

[0035] A processing unit for initializing the inertial navigation system according to the first data and setting the reset interval period of the inertial navigation system; calculating in real time the motion acceleration of the inertial navigation system in the navigation system based on the second data; performing real-time harmful acceleration compensation according to the motion acceleration of the inertial navigation system in the navigation system;

[0036] An output unit that outputs the acceleration data output by the accelerometer after harmful acceleration compensation.

[0037] In a third aspect, the present invention provides a mining area robot orientation method, which is characterized in that it is based on any one of the above-mentioned harmful acceleration compensation methods for mining area robots, and includes: adopting any one of the above-mentioned harmful acceleration compensation methods for mining area robots to compensate the harmful acceleration of the mining area robot, and obtaining the acceleration data output by the accelerometer after compensation;

[0038] Based on the acceleration data output by the accelerometer after compensation and the speed of the odometer in the body frame, compensate the direction and attitude of the mining area robot measured by the strapdown compass system, and orient the mining area robot based on the compensated direction and attitude of the mining area robot.

[0039] Fourthly, the present invention provides a mining area robot orientation system for the above-mentioned mining area robot orientation method, including:

[0040] A harmful acceleration compensation unit, which is used to compensate the harmful acceleration of the mining area robot by using any one of the above-mentioned harmful acceleration compensation methods for mining area robots to obtain the acceleration data output by the accelerometer after compensation;

[0041] An orientation unit, which is used to compensate the direction and attitude of the mining area robot measured by the strapdown compass system based on the acceleration data output by the accelerometer after compensation and the speed of the odometer in the body frame, and orient the mining area robot based on the compensated direction and attitude of the mining area robot.

[0042] The beneficial effects of the present invention are:

[0043] The present invention provides a harmful acceleration compensation method and system for mining area robots, an orientation method and system. By utilizing the characteristic that the inertial navigation system does not diverge during short-term operation, during operation, when there is a motion acceleration, initialize the inertial navigation system with periodic reset, obtain the motion acceleration of the mining area robot in the navigation frame based on the initialization data of the inertial navigation system, and compensate the acceleration originally output by the accelerometer to eliminate the orientation error caused by the motion acceleration to the strapdown compass system during operation. The present invention combines the strapdown compass system and the inertial navigation system with periodic reset. During operation, whenever there is an acceleration, compensate the acceleration output by the accelerometer, so that the strapdown compass system can be compensated and aligned for orientation operations by using the compensated acceleration, eliminating the error caused by the motion acceleration in the strapdown compass alignment and orientation loop, and solving the technical problem in the prior art that when the mining area robot works in a complex mining area environment, due to the orientation measurement error, it affects the normal use of the mining area robot and even causes safety risks. Description of the Drawings

[0044] Figure 1 is a schematic diagram of the harmful acceleration compensation method for mining area robots in the present invention;

[0045] Figure 2 is a schematic diagram of the experimental equipment for the verification experiment in the present invention;

[0046] Figure 3 is a schematic diagram of the path trajectory in the verification experiment in the present invention;

[0047] Figure 4It is a schematic diagram of the output comparison of the pitch angle in the verification experiment of the present invention;

[0048] Figure 5 It is a schematic diagram of the output comparison of the roll angle in the verification experiment of the present invention;

[0049] Figure 6 It is a schematic diagram of the output comparison of the heading angle in the verification experiment of the present invention. Specific embodiments

[0050] In order to explain in detail the harmful acceleration compensation method and system, and the orientation method and system of the mining area robot provided in the present invention, the following will be described in conjunction with embodiments.

[0051] Embodiment 1

[0052] In the first aspect, referring to Figure 1 , it shows a schematic diagram of a harmful acceleration compensation method for a mining area robot described in the present application, including:

[0053] S1: Obtain the initial data of the strapdown compass system and the odometer as the first data.

[0054] Optionally, in step S1, the first data includes: when in a static or uniform motion state, the attitude matrix of the strapdown compass system in the vehicle coordinate system, the longitude and latitude of the mining area robot, and the speed of the odometer in the vehicle coordinate system.

[0055] Specifically, in the application process, when the mining area robot is in a static or uniform motion state and the initial alignment operation of the strapdown compass system is performed, the first data is obtained.

[0056] S2: When there is a motion acceleration, initialize the inertial navigation system according to the first data, set the reset interval period of the inertial navigation system, and obtain the initial data of the inertial navigation system as the second data.

[0057] Specifically, it can be judged whether there is an acceleration through the accelerometer carried on the mining area robot.

[0058] Specifically, the reset interval period of the inertial navigation system is determined according to the accuracy of the inertial navigation system. The higher the accuracy of the inertial navigation system, the longer its reset interval period.

[0059] Optionally, in step S2, the second data includes: the initial longitude and latitude of the inertial navigation system, the initial attitude matrix, and the initial speed.

[0060] Optionally, when initializing the inertial navigation system in step S2, it specifically includes:

[0061] Based on the attitude matrix of the strapdown compass system in the vehicle coordinate system and the speed of the odometer in the vehicle coordinate system Calculate the speed of the odometer in the navigation system

[0062] Use the speed of the odometer in the navigation system As the initial speed of the inertial navigation system

[0063] Based on the speed of the odometer in the navigation system Combine the longitude and latitude of the mining area robot to calculate the initial longitude and latitude of the inertial navigation system;

[0064] Based on the attitude matrix of the strapdown compass system in the vehicle coordinate system Calculate the initial attitude matrix of the inertial navigation system

[0065] Optionally, in step S2, the specific process of obtaining the initial speed of the inertial navigation system is as follows:

[0066]

[0067] Among them, are the three-axis speed components of the odometer in the navigation system, where is the eastward speed component of the odometer in the navigation system, unit: m / s, is the northward speed component of the odometer in the navigation system, unit: m / s, is the upward speed component of the odometer in the navigation system, unit: m / s;

[0068] Based on the speed of the odometer in the navigation system The specific process of combining the longitude and latitude of the mining area robot to obtain the initial longitude and latitude of the inertial navigation system is as follows:

[0069]

[0070] Among them, L 0_INS is the initial latitude of the inertial navigation system, unit: °, λ 0_INS is the initial longitude of the inertial navigation system, unit: °, L0 is the latitude of the mining area robot, unit: °, λ0 is the longitude of the mining area robot, unit: °, is the northward speed component of the odometer in the navigation system, unit: m / s, is the eastward speed component of the odometer in the navigation system, unit: m / s, R e is the radius of the earth, unit: m;

[0071] The initial attitude matrix of the inertial navigation system is the same as the attitude matrix of the strapdown compass system in the navigation system That is

[0072] S3: Obtain the running time of the inertial navigation system in real time, compare the running time of the inertial navigation system with its reset interval period. If the running time is greater than or equal to the reset interval period, execute step S1 again; if the running time is less than the reset interval period, execute step S4.

[0073] In this step, according to the running time of the inertial navigation system obtained in real time, compare the running time of the inertial navigation system with its reset interval period. During the operation of the inertial navigation system, when the running duration of the inertial navigation system has not reached its reset interval period, there is no divergence in the short-term operation of the inertial navigation system at this time, and step S4 can be continued to calculate and compensate the motion acceleration; when the running time of the inertial navigation system reaches or exceeds its reset interval period, there is a risk of divergence in the inertial navigation system at this time, then step S1 is executed again, and the inertial navigation system is initialized to perform the compensation operation for harmful acceleration.

[0074] S4: Based on the second data, calculate the motion acceleration of the inertial navigation system in the navigation system in real time.

[0075] Optionally, in step S4, calculate the motion acceleration of the inertial navigation system in the navigation system in real time is obtained based on the longitude and latitude [L i_INS λ i_INS T output by the inertial navigation system in real time, the attitude matrix in the navigation system and the speed as well as the acceleration in the vehicle system where i = 1, 2, 3... k, and k is the number of times the inertial navigation system continuously outputs data after one initialization.

[0076] Specifically, the process of calculating the motion acceleration of the inertial navigation system in the navigation system in real time is as follows:

[0077]

[0078]

[0079] where, is the component of the motion acceleration of the inertial navigation system in the navigation system calculated in real time on three axes, unit: m / s 2 , is the attitude matrix output by the strapdown compass system in real time, g is the gravitational acceleration, unit: m / s 2 , R e is the radius of the earth, unit: m, L i_INS is the latitude output by the inertial navigation system in real time, unit: °, ω e is the angular velocity of the earth's rotation, unit: rad / s, is the acceleration component output by the accelerometer in real time under the carrier system, unit: m / s 2 , is the velocity component output by the inertial navigation system in real time under the navigation system in three axial directions, unit: m / s, i = 1, 2, 3... k, where k is the number of times the inertial navigation system continuously outputs data after one initialization.

[0080] S5: According to the motion acceleration of the inertial navigation system under the navigation system, perform harmful acceleration compensation in real time.

[0081] Optionally, the specific process of performing harmful acceleration compensation in real time in step S5 is:

[0082]

[0083] where is the real-time attitude matrix of the strapdown compass system, is the acceleration data output by the accelerometer after performing harmful acceleration compensation in real time under the navigation system, unit: m / s 2 , is the acceleration component output by the accelerometer after performing harmful acceleration compensation in real time under the navigation system, unit: m / s 2 , is the acceleration component output by the accelerometer in real time under the carrier system before harmful acceleration compensation, unit: m / s 2 , i = 1, 2, 3... k, where k is the number of times the inertial navigation system continuously outputs data after one initialization.

[0084] In this embodiment, by using the characteristic that the inertial navigation system hardly diverges during short-term operation, during the operation of the mining area robot, when the mining area robot has an acceleration, the inertial navigation system with phased reset is initialized, and the motion acceleration of the mining area robot under the navigation system is calculated based on the initialization data of the inertial navigation system, so as to compensate the acceleration originally output by the accelerometer, and eliminate the error generated by the motion acceleration during the operation of the mining area robot. In the present invention, the strapdown compass system and the inertial navigation system with phased reset are combined. During the operation of the mining area robot, whenever the mining area robot has an acceleration, the acceleration output by its accelerometer is compensated, so that the strapdown compass system can be compensated and aligned by using the compensated acceleration, and the error caused by the motion acceleration in the strapdown compass alignment loop is eliminated, solving the technical problem in the prior art that due to the accumulation of errors, it affects the normal use of the mining area robot and even causes safety risks.

[0085] It should be noted that the harmful acceleration compensation method for the mining area robot provided by the present invention can also be applied to mining area robots in mining areas such as coal mines and iron mines. At the same time, it can also be applied to carriers in other fields, such as aircraft, vehicles, ships, etc. Those skilled in the art can specifically select the application scenarios according to actual usage requirements.

[0086] Embodiment 2

[0087] The present invention also provides a harmful acceleration compensation system for a mining area robot, which is used for any of the above-mentioned harmful acceleration compensation methods for mining area robots, and includes:

[0088] A data acquisition unit that acquires the initial data of the strapdown compass system and the odometer as the first data, acquires the initial data of the inertial navigation system as the second data, and acquires the operating time of the inertial navigation system;

[0089] A judgment unit for judging whether there is a motion acceleration and judging the magnitude relationship between the operating time of the inertial navigation system and its reset interval period;

[0090] A processing unit for initializing the inertial navigation system according to the first data and setting the reset interval period of the inertial navigation system; based on the second data, calculating the motion acceleration of the inertial navigation system in the navigation system in real time; and performing harmful acceleration compensation in real time according to the motion acceleration of the inertial navigation system in the navigation system;

[0091] An output unit that outputs the acceleration data output by the accelerometer after harmful acceleration compensation.

[0092] It should be noted that the harmful acceleration compensation system for the mining area robot in this embodiment corresponds to the harmful acceleration compensation method for the mining area robot in Embodiment 1, and its beneficial effects are also the same, so no further description will be given here.

[0093] Embodiment 3

[0094] The present invention also provides a mining area robot orientation method, which uses any of the harmful acceleration compensation methods described in Embodiment 1 to compensate for the harmful acceleration of the mining area robot to obtain the acceleration data output by the accelerometer after compensation;

[0095] Based on the acceleration data output by the accelerometer after compensation and the speed of the odometer in the body frame, compensate for the direction and attitude of the mining area robot measured by the strapdown compass system, and orient the mining area robot based on the direction and attitude of the mining area robot after compensation.

[0096] Specifically, the odometer can be used to collect the speed of the mining area robot in the body frame to As a third - party speed, it is used to assist in the compensation alignment and orientation of the strapdown compass system.

[0097] Specifically, the process of the strapdown compass system for aligning and orienting the mining area robot is as follows:

[0098] (1) The speed of the mining area robot in the navigation system

[0099] According to the odometer detection, the speed of the mining area robot in the vehicle system is obtained. At the same time, the attitude matrix of the mining area robot in the vehicle system is obtained.

[0100] Through Conversion, the speed of the mining area robot in the navigation system is obtained.

[0101] Specifically, in the calculation process, the attitude matrix at each moment Can be obtained through the conversion of the attitude matrix output by the strapdown compass system at the previous moment. The specific conversion process is the same as that described in Embodiment 1 and will not be elaborated here.

[0102] (2) Calculation of latitude

[0103] Based on Among them, L is the latitude of the robot updated in real - time based on the speed in the navigation system measured by the odometer, R e Is the radius of the earth, L0 is the local latitude at the start of alignment, which can be obtained externally, Is the north - ward speed in the navigation system measured by the odometer.

[0104] (3) Calculation of compensation value

[0105] Combining the latitude of the mining area robot and the speed in the navigation system obtained from the above calculations, the three - axis components of the local earth's angular velocity of rotation, the angular velocity of the navigation system relative to the earth system, and the harmful acceleration are calculated. Then, the above - calculated error term values are introduced into the attitude update and specific - force decomposition positions of the strapdown compass alignment and orientation method under static or quasi - static conditions such as uniform motion for compensation.

[0106] Embodiment 4

[0107] The present invention also provides a mining area robot orientation system for implementing the mining area robot orientation method described in Embodiment 3, including:

[0108] A harmful acceleration compensation unit, which is used to compensate the harmful acceleration of the mining area robot by using any one of the mining area robot harmful acceleration compensation methods described in Embodiment 1 to obtain the acceleration data output by the accelerometer after compensation;

[0109] An orientation unit is used to compensate for the direction and attitude of the mining area robot measured by the strapdown compass system based on the acceleration data output by the compensated accelerometer and the speed of the odometer in the vehicle coordinate system, and to orient the mining area robot based on the compensated direction and attitude of the mining area robot.

[0110] It should be noted that the usage method of the mining area robot orientation system in this embodiment corresponds to that in Embodiment 1 and Embodiment 3, and its beneficial effects are also the same, so details will not be repeated here.

[0111] Embodiment 5

[0112] The present invention also provides a storage medium in which instructions are stored, and the instructions are generated based on the harmful acceleration compensation method of the mining area robot in Embodiment 1.

[0113] It should be noted that the storage medium described in the present invention may specifically include a random access memory (RAM), memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, register, hard disk, removable disk or CD-ROM. It should be noted that those skilled in the art can specifically select the form and type of the storage medium according to actual production and use requirements, and no further limitation is made in this embodiment.

[0114] Embodiment 6

[0115] In order to verify the mining area robot orientation method in the present invention, in this embodiment, an experiment is used to verify the mining area robot orientation method provided in Embodiment 3.

[0116] In this embodiment, a road test is adopted, referring to Figure 2 , in this experiment, the experimental equipment mainly includes a fiber optic inertial measurement unit (IMU), differential GPS and a data acquisition device mounted on the experimental vehicle. The experimental path is about 4 kilometers, and large-scale dynamic maneuvering turns are carried out on the experimental path, and its path trajectory is as shown in Figure 3 . Specifically, the differential GPS in this embodiment is an RTK antenna.

[0117] During the experiment, the gyroscope zero-bias stability is, the angular random walk; the accelerometer zero-bias stability is, the rate random walk is, and the output frequency is. The odometer speed measurement noise is 0.1 m / s. The output frequency of the differential GPS is 1 Hz. The attitude data generated by the combined navigation of the fiber optic inertial measurement unit and the differential GPS is used as the evaluation reference true value of the harmful acceleration compensation method in Embodiment 1.

[0118] During the experiment, the data in the first 500 seconds is the static initial alignment stage, the period from 500 seconds to 700 seconds is the static idle state, and dynamic maneuvering motion is carried out after 700 seconds. Therefore, when processing and displaying the data, only the stage data between 500 seconds and 1500 seconds, which includes both static and dynamic maneuvers, is analyzed in detail.

[0119] Referring to Figure 4 and Figure 5 , it shows the comparison of the pitch angle and the roll angle after compensation using the navigation compensation method provided in the present invention. It can be clearly known from the figure that during the motion process, there are large oscillations in both the pitch angle and the roll angle before compensation, and the pitch angle and the roll angle after compensation are basically consistent with the true values. As can be seen from the error curve, the errors before and after compensation are basically the same in the static state between 500 seconds and 700 seconds. The attitude measurement errors of the pitch angle and the roll angle after compensation between 700 seconds and 1500 seconds both drop to near 0°, and there is no obvious fluctuation.

[0120] Referring to Figure 6 , it shows the comparison of the output of the heading angle after compensation using the navigation compensation method provided in the present invention. It can be intuitively known from the figure that when dynamic maneuvering motion occurs after 700 seconds, there will be large oscillations in the output of the heading angle before compensation, and the output of the heading angle after compensation is basically consistent with the reference true value. It can be seen that the dynamic heading error before compensation is large, and the heading angle error is significantly improved after compensation, which can effectively improve the usability of the strapdown compass heading under dynamic maneuvering conditions.

[0121] Referring to Table 1, it shows the root mean square errors of the attitude angle of the strapdown compass before and after compensation under dynamic maneuvering. It can be known that the root mean square error of the heading angle after compensation is 0.794, and the root mean square errors of the pitch angle and the lateral angle are both 0.030 or less. By using the robot orientation method based on harmful acceleration compensation provided in the present invention, the attitude angle accuracy of the strapdown compass system is greatly improved.

[0122] Table 1 Root Mean Square Errors of the Attitude Angle of the Strapdown Compass before and after Compensation under Dynamic Maneuvering

[0123]

[0124] In summary, it can be known that the compensation method for harmful acceleration and the robot orientation method provided in the present invention can effectively reduce the influence of motion acceleration on the strapdown compass system, improve the accuracy of the navigation system, and reduce errors. In the actual use process, it can effectively solve the technical problems in the prior art that the cumulative errors affect the normal use of the robot and even cause safety risks.

Claims

1. A harmful acceleration compensation method for a mining area robot, characterized in that The harmful acceleration compensation method for the mining area robot includes: S1: Obtain the initial data of the strapdown compass system and the odometer as the first data; S2: When there is a motion acceleration, initialize the inertial navigation system according to the first data, set the reset interval period of the inertial navigation system, and obtain the initial data of the inertial navigation system as the second data; S3: Real-time obtain the running time of the inertial navigation system, compare the running time of the inertial navigation system with its reset interval period. If the running time is greater than or equal to the reset interval period, re-execute step S1; if the running time is less than the reset interval period, execute step S4; S4: Based on the second data, calculate the motion acceleration of the inertial navigation system in the navigation system in real time; S5: Perform harmful acceleration compensation in real time according to the motion acceleration of the inertial navigation system in the navigation system.

2. The harmful acceleration compensation method for a mining area robot according to claim 1, wherein In step S1, the first data includes: the attitude matrix of the strapdown compass system in the vehicle coordinate system, the longitude and latitude of the mining area robot, and the speed of the odometer in the vehicle coordinate system when in a stationary or uniform motion state.

3. The harmful acceleration compensation method for a mining area robot according to claim 2, wherein In step S2, the second data includes: the initial longitude and latitude, the initial attitude matrix, and the initial speed of the inertial navigation system.

4. The harmful acceleration compensation method for a mining area robot according to claim 3, characterized in that When initializing the inertial navigation system in step S2, it specifically includes: Attitude Matrix of the Strapdown Compass System under the Vehicle Coordinate System and the Speed of the Odometer under the Vehicle Coordinate System Calculate the Speed of the Odometer under the Navigation Coordinate System The speed of the odometer under the navigation system As the initial speed of the inertial navigation system Speed based on the odometer under the navigation system Combine the longitude and latitude of the mining area robot to calculate the initial longitude and latitude of the inertial navigation system; Attitude Matrix of Strapdown Compass System Based on Carrier System Calculate the Initial Attitude Matrix of Inertial Navigation System 5. The method for compensating harmful acceleration of a mining area robot according to claim 4, characterized in that, In the step S2, the initial velocity of the inertial navigation system is obtained The specific process is as follows: Among them, are the three-axis velocity components of the odometer under the navigation system. Among them, is the eastward velocity component of the odometer under the navigation system, unit: m / s, is the northward velocity component of the odometer under the navigation system, unit: m / s, is the upward velocity component of the odometer under the navigation system, unit: m / s; Speed based on the odometer under the navigation system The specific process of obtaining the initial longitude and latitude of the inertial navigation system by combining the longitude and latitude of the mining area robot is as follows: Among them, L 0_INS is the initial latitude of the inertial navigation system, unit: °, λ 0_INS is the initial longitude of the inertial navigation system, unit: °, L0 is the latitude of the mining area robot, unit: °, λ0 is the longitude of the mining area robot, unit: °, is the northward velocity component of the odometer in the navigation system, unit: m / s, is the eastward velocity component of the odometer in the navigation system, unit: m / s, R e is the radius of the earth, unit: m; Initial attitude matrix of the inertial navigation system is consistent with the attitude matrix of the strapdown compass system in the navigation system, that is ​ 6. The harmful acceleration compensation method for a mining area robot according to claim 5, characterized in that In the step S4, the motion acceleration of the inertial navigation system in the navigation system is calculated in real time based on the longitude and latitude [L i_INS λ i_INS T output by the inertial navigation system in real time, the attitude matrix in the navigation system, and the speed as well as the acceleration in the vehicle body coordinate system, where i = 1, 2, 3... k, and k is the number of times the inertial navigation system continuously outputs data after one initialization.​ 7. The harmful acceleration compensation method for a mining area robot according to claim 6, wherein In step S5, the process of performing harmful acceleration compensation in real time is: Among them, is the real-time attitude matrix of the strapdown compass system, is the acceleration data output by the accelerometer after real-time harmful acceleration compensation in the navigation system, unit: m / s 2 , is the acceleration component output by the accelerometer after real-time harmful acceleration compensation in the navigation system, unit: m / s 2 , is the acceleration component output by the accelerometer in the body coordinate system in real time before harmful acceleration compensation, unit: m / s 2 , i = 1, 2, 3... k, where k is the number of times the inertial navigation system continuously outputs data after one initialization.

8. A harmful acceleration compensation system for a mining area robot, which is used for the harmful acceleration compensation method of the mining area robot according to any one of claims 1 to 7, and is characterized in that, Including: A data acquisition unit that acquires the initial data of the strapdown compass system and the odometer as the first data, acquires the initial data of the inertial navigation system as the second data, and acquires the running time of the inertial navigation system; A judgment unit for judging whether there is a motion acceleration and judging the magnitude relationship between the running time of the inertial navigation system and its reset interval period; A processing unit for initializing the inertial navigation system according to the first data and setting the reset interval period of the inertial navigation system; Based on the second data, calculate the motion acceleration of the inertial navigation system in the navigation system in real time; perform harmful acceleration compensation in real time according to the motion acceleration of the inertial navigation system in the navigation system; An output unit that outputs the acceleration data output by the accelerometer after harmful acceleration compensation.

9. A method for orienting a mining area robot, characterized in that, It is used based on the harmful acceleration compensation method for the mining area robot described in any one of claims 1 to 7, and includes: Adopt the harmful acceleration compensation method for the mining area robot described in any one of claims 1 to 7 to compensate the harmful acceleration of the mining area robot, and obtain the acceleration data output by the accelerometer after compensation; Based on the acceleration data output by the accelerometer after compensation and the speed of the odometer in the vehicle coordinate system, compensate the direction and attitude of the mining area robot measured by the strapdown compass system, and orient the mining area robot based on the compensated direction and attitude of the mining area robot.

10. A robot orientation system for a mining area, characterized in that, It is used for the mining area robot orientation method described in claim 9, and includes: A harmful acceleration compensation unit for adopting the harmful acceleration compensation method for the mining area robot described in any one of claims 1 to 7 to compensate the harmful acceleration of the mining area robot, and obtain the acceleration data output by the accelerometer after compensation; An orientation unit is used to compensate for the direction and attitude of the mining area robot measured by the strapdown compass system based on the acceleration data output by the compensated accelerometer and the speed of the odometer in the vehicle coordinate system, and to orient the mining area robot based on the compensated direction and attitude of the mining area robot.

Images