Method for calculating specific force integral conversion increment of unmanned sweeper
By collecting the inertial sensor output in real time, calculating the rotation coordinate system and the specific force integral conversion increment, the problem of speed update accuracy decrease caused by the limited bandwidth of the inertial sensor in the unmanned sweeper is solved, and the precise specific force integral under high dynamic motion is achieved Convert incremental calculation improves calculation efficiency and accuracy.
Patent Information
- Application Number
- CN202411101568.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-12
- Publication Date
- 2024-12-06
AI Technical Summary
In unmanned sweepers, inertial sensors with limited bandwidth cause rapid changes in the specific force information in high dynamic motion scenarios, resulting in a decrease in the accuracy of speed update. The existing method is derived based on the second-order approximation of the equivalent rotation vector equation. There are principle errors and the high-order compensation process is complex, and the accuracy is improved at the cost of calculation.
By setting the sampling period, the inertial sensor output is collected in real time, the relationship matrix of the rotation coordinate system and the specific force integral conversion increment are calculated using the gyroscope and accelerometer, and the specific force integral conversion increment is realized for the unmanned sweeper, and the specific force integral conversion increment is updated, using recursive impulse Method calculates the rotation angular velocity and the specific force integral conversion increment to improve the accuracy of speed update.
In highly dynamic motion scenarios, accurate and efficient incremental calculation of specific force integral conversion is realized, which reduces the error of speed update, improves calculation accuracy, and reduces the amount of calculation.
Smart Images

Figure CN119085638A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of speed update calculation of unmanned sweeping vehicles, and in particular to a method for calculating the incremental conversion of specific force integrals of unmanned sweeping vehicles. Background Art
[0002] The inertial sensors in the unmanned sweeper are directly connected to it and experience the same angular and linear motion as it. In practical applications, the output of the inertial device can only be sampled at a limited sampling frequency. In other words, the sampling bandwidth of the inertial device is limited. In high-dynamic motion scenarios, the attitude conversion matrix and specific force information will change rapidly over time, and the specific force vector conversion integral term is essentially the integration of two fast variables. The angular velocity and specific force signals sampled by the limited bandwidth inertial sensor component will introduce non-commutative errors in the speed calculation. It is difficult to obtain an accurate conversion specific force integral increment in the speed numerical integration, resulting in a decrease in the accuracy of the speed update. Therefore, it is very necessary to study the calculation of the specific force integral conversion increment part in the speed update information of the unmanned sweeper.
[0003] However, in published articles, such as the article "An Improved Strapdown Inertial Navigation Three-Sample Rowing Error Compensation Algorithm" by Geng Tengfei, Liu Ming, and Chen Shiqiang in Volume 42, Issue 2 of "Ship Electronic Engineering", the authors used the duality between cone error compensation and rowing error compensation to propose an improved three-sample rowing error compensation using the velocity increment and angle increment of the previous cycle, and used the drift error minimum principle to derive the error compensation coefficient in detail, and based on this scheme, the traditional rowing three-sample compensation coefficient was obtained. From the simulation results, the designed method has significantly improved the compensation accuracy compared with the conventional three-sample rowing error method.
[0004] In addition, Wang Zhaogang, Liu Wenchao, Li Dong, etc. proposed a velocity update method based on Adams-PECE for inertial sensors in the article "A Numerical Update Algorithm of Strapdown Inertial Guidance Based on Adams-PECE" published in the Journal of Chinese Inertial Technology. The current velocity and position are estimated by the Adams prediction formula, and the prediction parameters are used to participate in the calculation of the attitude matrix and the current acceleration; the current velocity and position are calculated by the Adams correction formula, and the attitude matrix is updated. The proposed method does not need to calculate the paddling and rotation errors, and the integration interval is a sampling interval. Its accuracy is significantly better than the traditional multi-sample method.
[0005] The above published articles have described and explored the calculation method of the specific force integral conversion increment in the speed update based on the inertial sensor. However, the above methods are all derived on the basis of the second-order approximation of the equivalent rotating vector equation. Even if the high-order approximation of the equivalent rotating vector equation is considered, there are still principle errors, and the derivation process of the high-order compensation scheme is very complicated. In addition, the improvement of the accuracy of the above methods is at the expense of the amount of calculation. Therefore, it is innovative and of practical engineering value to study a method suitable for unmanned sweeping vehicles that can achieve accurate specific force integral conversion increment calculation. Summary of the invention
[0006] In order to solve the above technical problems, the present invention proposes a method for calculating the incremental conversion of specific force integral of an unmanned sweeping vehicle.
[0007] The technical problem to be solved by the present invention is achieved by adopting the following technical solutions:
[0008] A method for calculating the conversion increment of specific force integral of an unmanned sweeping vehicle comprises the following steps:
[0009] Step 1: Set the sampling period h in the system to collect the angular velocity and specific force signals output by the inertial sensors, namely the gyroscope and accelerometer, in real time, and obtain t through navigation solution. k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G
[0010] Step 2: Use the gyroscope output to recursively calculate the measured rotational angular velocity of the first rotating coordinate system s1 relative to the carrier coordinate system b The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1
[0011] The rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system b is calculated by using the gyro output angular velocity sampled within the speed solution period H = 3h through the conversion matrix method. The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1
[0012] Step 3: Use the gyroscope output in step 1 and the calculation result in step 2 to recursively calculate the second rotating coordinate system from time t to t k Measurement relationship matrix of time
[0013] Using t k Gyro output angular velocity at time The t obtained in step 2 k+1 The rotational angular velocity α of the first rotating coordinate system relative to the carrier coordinate system at the momenttk+1 and the rotational angular velocity λ of the second rotating coordinate system relative to the first rotating coordinate system tk+1 , calculate the second rotating coordinate system from time t to t k The relationship matrix of the moment
[0014] Step 4: Use the accelerometer output in step 1 and the calculation results of steps 1 and 3 to recursively deduce t k+1 Time measurement ratio integral conversion increment Complete the incremental update of the comparative points conversion of the unmanned sweeping vehicle and realize the speed update of the unmanned sweeping vehicle:
[0015] Using t k The accelerometer output at time f B (t k ), the relationship matrix between the carrier coordinate system B obtained in step 1 and the geographic coordinate system n The second rotation coordinate system obtained in step 3 is from time t to t k The relationship matrix of the moment Calculate t k+1 Time comparison integral conversion increment
[0016] As a further improvement of the present invention, in step 1, k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G as follows:
[0017]
[0018] C 11 =cosγ k cosψ k +sinγ k sinψ k sinθ k
[0019] C 21 = -cosγ k sinψ k +sinγ k cosψ k sinθ k
[0020] C 31 = -sinγ k cosθ k
[0021] C 12 = sinψ k cosθ k
[0022] C 22 =cosψ k cosθ k
[0023] C 32 = sinθ k
[0024] C 13 = sinγ k cosψ k -cosγ k sinψ k sinθ k
[0025] C 23 = -sinγ k sinψ k -cosγ k cosψ k sinθ k
[0026] C 33 =cosγ k cosθ k
[0027] Among them, γ, θ, and ψ are pitch, roll, and heading attitude angle information respectively, cos is the cosine calculation symbol, and sin is the sine calculation symbol.
[0028] As a further improvement of the present invention, in step 2, the gyroscope output angular velocity sampled within the speed calculation period H = 3h is used to recursively calculate the rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system B based on the matrix conversion method. The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 as follows:
[0029]
[0030] in,
[0031]
[0032] Among them, U ii ,Y ii , i=1,2,3 are the related elements of transformation matrices U and Y.
[0033] As a further improvement of the present invention, in step 3, the gyroscope output and the calculation result of step 2 are used to recursively calculate the second rotating coordinate system from time t to t k Measurement relationship matrix of time as follows:
[0034]
[0035] As a further improvement of the present invention, in step 4, t is recursively deduced using the accelerometer output and the calculation results of steps 1 and 3. k+1 Time measurement ratio integral conversion increment as follows:
[0036]
[0037] The beneficial effects of the present invention are:
[0038] The present invention provides a method for calculating the incremental conversion of specific force integrals of an unmanned sweeping vehicle, which can realize accurate and efficient incremental conversion calculation of specific force integrals in high-dynamic motion scenarios. BRIEF DESCRIPTION OF THE DRAWINGS
[0039] The present invention is further described below in conjunction with the accompanying drawings and embodiments:
[0040] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION
[0041] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further described below with reference to the accompanying drawings and embodiments.
[0042] like Figure 1 As shown, a method for calculating the conversion increment of the specific force integral of an unmanned sweeping vehicle comprises the following steps:
[0043] Step 1: Set the sampling period h in the system to collect the angular velocity and specific force signals output by the inertial sensors, namely the gyroscope and accelerometer, in real time, and obtain t through navigation solution. k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G
[0044] t k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G as follows:
[0045]
[0046] C 11 =cosγ k cosψ k +sinγ k sinψ k sinθ k
[0047] C 21= -cosγ k sinψ k +sinγ k cosψ k sinθ k
[0048] C 31 = -sinγ k cosθ k
[0049] C 12 = sinψ k cosθ k
[0050] C 22 =cosψ k cosθ k
[0051] C 32 = sinθ k
[0052] C 13 = sinγ k cosψ k -cosγ k sinψ k sinθ k
[0053] C 23 = -sinγ k sinψ k -cosγ k cosψ k sinθ k
[0054] C 33 =cosγ k cosθ k
[0055] Among them, γ, θ, and ψ are pitch, roll, and heading attitude angle information respectively, cos is the cosine calculation symbol, and sin is the sine calculation symbol.
[0056] Step 2: Use the gyroscope output to recursively calculate the measured rotational angular velocity of the first rotating coordinate system s1 relative to the carrier coordinate system b The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 The angular velocity of the gyroscope output sampled within the speed calculation period H = 3h is used to calculate the rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system b by the conversion matrix method. The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1
[0057] The rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system B The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 as follows:
[0058]
[0059] in,
[0060]
[0061] Among them, U ii ,Y ii , i=1,2,3 are the related elements of transformation matrices U and Y.
[0062] Step 3: Use the gyroscope output in step 1 and the calculation result in step 2 to recursively calculate the second rotating coordinate system from time t to t k Measurement relationship matrix of time Using t k The gyroscope output angular velocity at time The t obtained in step 2 k+1 The angular velocity of the first rotating coordinate system relative to the carrier coordinate system at the moment and the angular velocity of the second rotating coordinate system relative to the first rotating coordinate system Calculate the second rotating coordinate system from time t to t k The relationship matrix of the moment
[0063] From time t to t k Measurement relationship matrix of time as follows:
[0064]
[0065] Step 4: Use the accelerometer output in step 1 and the calculation results of steps 1 and 3 to recursively deduce t k+1 Time measurement ratio integral conversion increment Complete the incremental update of the comparative integral conversion of the unmanned sweeping car and realize the speed update of the unmanned sweeping car: use t k The accelerometer output at time f B (t k ), the relationship matrix between the carrier coordinate system B obtained in step 1 and the geographic coordinate system n The second rotation coordinate system obtained in step 3 is from time t to t kThe relationship matrix of the moment Calculate t k+1 Time comparison integral conversion increment
[0066] t k+1 Time measurement ratio integral conversion increment as follows:
[0067]
[0068] The calculation method provided in this embodiment can realize accurate and efficient specific force integral conversion increment calculation in high dynamic motion scenes. In order to verify the beneficial effect of the method, simulation experiments are carried out on the present invention and the traditional algorithms commonly used in engineering practice under typical rowing motion. The typical rowing motion expression is:
[0069]
[0070] Take the angular motion amplitude of typical rowing motion as c = 1° / h and the linear motion amplitude as d = 9.8 m / s 2 , rowing frequency Ω=20πrad / s, gyroscope sampling frequency is 100Hz, and simulation time is 60s. The error is mainly reflected in the Z axis and the error diverges over time. The results are shown in Table 1 below: Group A is the simulation result of the traditional algorithm, and Group B is the simulation result of the present invention.
[0071] Table 1
[0072]
[0073] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions only describe the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached claims and their equivalents.
Claims
1. A method for calculating the conversion increment of specific force integral of an unmanned sweeping vehicle, characterized by: The following steps are involved: Step 1: Set the sampling period h in the system to collect the angular velocity and specific force signals output by the inertial sensors, namely the gyroscope and accelerometer, in real time, and obtain t through navigation solution. k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G Step 2: Use the gyroscope output to recursively calculate the measured rotational angular velocity of the first rotating coordinate system s1 relative to the carrier coordinate system b The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 The angular velocity of the gyroscope output sampled within the speed calculation period H = 3h is used to calculate the rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system b by the conversion matrix method. The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 Step 3: Use the gyroscope output in step 1 and the calculation result in step 2 to recursively calculate the second rotating coordinate system from time t to t k Measurement relationship matrix of time Using t k Gyro output angular velocity at time The t obtained in step 2 k+1 The angular velocity of the first rotating coordinate system relative to the carrier coordinate system at the moment and the angular velocity of the second rotating coordinate system relative to the first rotating coordinate system Calculate the second rotating coordinate system from time t to t k The relationship matrix of the moment Step 4: Use the accelerometer output in step 1 and the calculation results of steps 1 and 3 to recursively deduce t k+1 Time measurement ratio integral conversion increment Complete the incremental update of the comparative points conversion of the unmanned sweeping vehicle and realize the speed update of the unmanned sweeping vehicle: Using t k The accelerometer output at time f B (t k ), the relationship matrix between the carrier coordinate system B obtained in step 1 and the geographic coordinate system n The second rotation coordinate system obtained in step 3 is from time t to t k The relationship matrix of the moment Calculate t k+1 Time comparison integral conversion increment 2. The method for calculating the conversion increment of the specific force integral of an unmanned sweeping vehicle according to claim 1, characterized in that: Step 1 k Relationship matrix between the time carrier coordinate system B and the geographic coordinate system G as follows: C 11 =cosγ k cosψ k +singγ k sinψ k sinth k C 21 =-cosγ k sinψ k +singγ k cosψ k sinth k C 31 =-sinγ k cosθ k C 12 =sinψ k cosθ k C 22 =cosψ k cosθ k C 32 =sinθ k C 13 =sinγ k cosψ k -cosγ k sinψ k sinth k C 23 =-sinγ k sinψ k -cosγ k cosψ k sinth k C 33 =cosγ k cosθ k Among them, γ, θ, and ψ are pitch, roll, and heading attitude angle information respectively, cos is the cosine calculation symbol, and sin is the sine calculation symbol.
3. The method for calculating the conversion increment of the specific force integral of an unmanned sweeping vehicle according to claim 1, characterized in that: In step 2, the angular velocity of the gyroscope output sampled within the speed calculation period H = 3h is used to recursively calculate the rotational angular velocity of the first rotating coordinate system S1 relative to the carrier coordinate system B based on the matrix conversion method. The angular velocity of the second rotating coordinate system S2 relative to the first rotating coordinate system S1 as follows: in, Among them, U ii ,Y ii , i=1,2,3 are the related elements of transformation matrices U and Y.
4. The method for calculating the conversion increment of specific force integral of an unmanned sweeping vehicle according to claim 1, characterized in that: In step 3, the gyroscope output and the calculation result of step 2 are used to recursively calculate the second rotating coordinate system from time t to t k Measurement relationship matrix of time as follows:
5. The method for calculating the conversion increment of specific force integral of an unmanned sweeping vehicle according to claim 1, characterized in that: In step 4, t is recursively calculated using the accelerometer output and the calculation results of steps 1 and 3. k+1 Time measurement ratio integral conversion increment as follows:
