A method for calculating battery motion trajectory and speed using a three-axis gyroscope

Abstract

The invention discloses a method for calculating the motion trajectory and speed of a battery by using a three-axis gyroscope, comprising the following steps: step one, establishing an initial operation library; step two, updating the operation library data; step three, establishing an initial trajectory; step four, optimizing the trajectory path; step five, establishing a speed model; step six, refining the model nodes; and step seven, calculating the speed at a time point. The invention collects the running angular velocity of the battery by using a three-axis gyroscope, and then restores the running trajectory of the battery by calculating the attitude angle, thereby ensuring the accuracy of the established motion trajectory, and obtaining the battery speed direction at a corresponding time node by performing curvature optimization on the established running trajectory, thereby improving the practicability of the calculation method, and refining the nodes on the battery running trajectory and establishing a trajectory database, and then using the speed point data adjacent to a specific time point to calculate the instantaneous speed at a specific time, thereby meeting complex actual detection requirements.

Description

Technical Field

[0001] The present invention relates to the technical field of battery trajectory and speed analysis, and in particular to a method for calculating battery motion trajectory and speed by using a three-axis gyroscope. Background Art

[0002] With the maturity of single-chip microcomputer technology, intelligent instrument technology has also gradually developed. Intelligent instruments are detection equipment formed by organically combining computer technology and detection technology with microcomputers as the main body. They are simple parameter settings and high programming reliability. They have been widely used in the field of electronic detection. However, the existing battery operation trajectory calculation method has a single data source, which affects the accuracy of the calculated operation trajectory. In addition, the existing battery operation trajectory calculation method does not optimize the curvature of the operation trajectory according to the acceleration at the time node, and it is difficult to obtain the battery speed direction at the corresponding time node, which affects the practicality of the calculation method. At the same time, the existing calculation method uses the detection node as the data collection and analysis point, and cannot calculate the instantaneous speed at a specific time, which is difficult to meet the complex actual detection needs. Therefore, it is necessary to design a method for calculating the battery motion trajectory and speed through a three-axis gyroscope. Summary of the invention

[0003] The object of the present invention is to provide a method for calculating the motion trajectory and speed of a battery by using a three-axis gyroscope to solve the problems raised in the above background technology.

[0004] To achieve the above object, the present invention provides the following technical solution: a method for calculating battery motion trajectory and speed by a three-axis gyroscope, comprising the following steps: step 1, initial operation library establishment; step 2, operation library data update; step 3, initial trajectory establishment; step 4, trajectory path optimization; step 5, speed model establishment; step 6, model node refinement; step 7, time point speed calculation;

[0005] In the above step 1, the accelerometer and the three-axis gyroscope are used to collect the initial acceleration and the initial angular velocity of the battery respectively, and then the initial attitude angle of the battery is calculated according to the initial acceleration and the initial angular velocity of the battery, and then the initial quaternion of the battery is calculated according to the quaternion method, and then the initial quaternion is recorded and marked in the quaternion runtime library;

[0006] In the above step 2, the angular velocity of the battery during the whole process is collected by the three-axis gyroscope, and the data in the quaternion runtime library is updated, and then the node attitude angle corresponding to each node is calculated through the quaternions of all nodes in the quaternion runtime library;

[0007] In the above step 3, the direction of the battery at each time node is analyzed according to the data in the quaternion runtime library, and the running distance in the direction of travel is calculated in combination with the angular velocity, and then the trajectory point at the next time node is marked in combination with the attitude angle and running distance of the node, and then the initial running trajectory model of the battery is established according to the trajectory point;

[0008] In the above step 4, a motion camera collects visual images of the battery during movement, and then processes the visual images to calculate feature points in the images, and then marks the feature points in the motion database, and then combines the feature points in the motion database, the angular velocity collected by the three-axis gyroscope, and the node acceleration collected by the accelerometer to establish a trajectory error equation, and then adjusts the position corresponding to each node in the running trajectory model through nonlinear optimization calculation, and the adjusted trajectory model is the running trajectory model of the battery;

[0009] In the above step 5, the nodes on the battery running trajectory are refined, and each refined point is used as a speed extraction point to obtain a battery speed extraction model;

[0010] In the above step 6, the speed points are processed individually in the battery speed extraction model, and after processing, the running direction and running speed of each speed point are recorded in the trajectory database;

[0011] In the above step 7, the speed time point to be queried is input, and then the running speeds of the two adjacent time points corresponding to the speed time point are retrieved from the trajectory database established in step 6, and then the average running speed of the two adjacent time points is calculated, which is the running speed of the battery at the queried speed time point.

[0012] Preferably, in step 1, the signal acquisition rate of the accelerometer and the three-axis gyroscope is 20 ms / time, the update rate in the quaternion runtime library is 20 ms / time, and the initial quaternion is the initial data in the quaternion runtime library.

[0013] Preferably, in step 2, the process of updating the quaternion runtime library according to the collected angular velocity is: calculating the node quaternion corresponding to each collection node by the quaternion method, and then recording and marking the node quaternion in the quaternion runtime library in sequence to complete the data update of the quaternion runtime library.

[0014] Preferably, in the step three, the process of establishing the battery's operating trajectory model is: first establish a spatial rectangular coordinate system, then use the origin of the spatial rectangular coordinate system as the starting point of the battery's operating trajectory, then extract the trajectory points at each time node, mark them in the established spatial rectangular coordinate system in sequence, then connect each trajectory point in sequence to form the original trajectory, and then use curvature calculation to eliminate the inflection points on the original trajectory, thereby forming the battery's operating trajectory model.

[0015] Preferably, in step 4, the acquisition rate of the motion camera for acquiring visual graphics is 20 ms / time, and the initial acquisition time of the motion camera is consistent with the initial acquisition time of the three-axis gyroscope.

[0016] Preferably, in step five, the refinement process is: first extract the nth node position, then extract the n+1th node position, then calculate the trajectory curve distance between the nth node and the n+1th node, and simultaneously extract the node acceleration of the corresponding nth node and n+1th node detected by the accelerometer, and then refine the extraction point corresponding to the unit time on the trajectory between the nth node and the n+1th node according to the rate of change of the node acceleration.

[0017] Preferably, in step six, the individual processing process of the speed points is: first mark the mth individual speed point, then generate a unit vector about the mth speed point in the battery speed extraction curve, and this unit vector is the running direction of this speed point, and then extract the m+1th and m-1th node positions, and then calculate the trajectory curve distance between the m+1th and m-1th nodes, and then calculate the instantaneous speed of the mth speed point according to the trajectory curve distance between the nodes and the node time difference.

[0018] Compared with the prior art, the beneficial effects of the present invention are as follows: the method for calculating the battery motion trajectory and speed by means of a three-axis gyroscope uses the three-axis gyroscope to collect the battery's running angular velocity during the process, and then calculates the attitude angle at each time node through the established quaternion runtime library to restore the battery's running trajectory, thereby improving the accuracy of the established trajectory, establishing a trajectory error equation through the measured feature points, angular velocity and acceleration, and performing curvature optimization on the established running trajectory, thereby facilitating the acquisition of the battery speed direction at the corresponding time node, thereby improving the practicality of the calculation method, establishing a battery speed extraction model by refining the nodes on the battery running trajectory, and then recording the running direction and running speed of each speed point in the trajectory database, and then calculating the instantaneous speed at a specific time through the data of adjacent speed points, thereby meeting complex actual detection requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 The figure is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0020] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0021] See also Figure 1 The present invention provides a technical solution: a method for calculating the battery motion trajectory and speed by a three-axis gyroscope, comprising the following steps: step 1, initial operation library establishment; step 2, operation library data update; step 3, initial trajectory establishment; step 4, trajectory path optimization; step 5, speed model establishment; step 6, model node refinement; step 7, time point speed calculation;

[0022] In the above step 1, the accelerometer and the three-axis gyroscope are first used to collect the initial acceleration and the initial angular velocity of the battery respectively, and then the initial attitude angle of the battery is calculated according to the initial acceleration and the initial angular velocity of the battery, and then the initial quaternion of the battery is calculated according to the quaternion method, and then the initial quaternion is recorded and marked in the quaternion runtime library, the signal acquisition rate of the accelerometer and the three-axis gyroscope is 20ms / time, the update rate in the quaternion runtime library is 20ms / time, and the initial quaternion is the initial data in the quaternion runtime library;

[0023] In the above step 2, the angular velocity of the battery during the whole process is collected by the three-axis gyroscope, and the data in the quaternion runtime library is updated. The process of updating the quaternion runtime library according to the collected angular velocity is as follows: the node quaternion corresponding to each collected node is calculated by the quaternion method, and then the node quaternion is recorded and marked in the quaternion runtime library in sequence to complete the data update of the quaternion runtime library, and then the node attitude angle corresponding to each node is calculated by all the node quaternions in the quaternion runtime library;

[0024] Among them, in the above step three, the direction of travel of the battery at each time node is analyzed according to the data in the quaternion operation library, and the running distance in the direction of travel is calculated in combination with the angular velocity, and then the trajectory point at the next time node is marked in combination with the node's attitude angle and running distance, and then the initial operation trajectory model of the battery is established according to the trajectory point. The process of establishing the initial operation trajectory model of the battery is as follows: first, a spatial rectangular coordinate system is established, and then the origin of the spatial rectangular coordinate system is used as the starting point of the battery operation trajectory, and then the trajectory points at each time node are extracted, and marked in turn in the established spatial rectangular coordinate system, and then each trajectory point is connected in turn to form an original trajectory, and then the curvature calculation is used to eliminate the inflection point on the original trajectory, that is, the initial operation trajectory model of the battery is formed;

[0025] In the above step 4, a motion camera is used to collect visual images of the battery during movement. The acquisition rate of the motion camera to collect visual graphics is 20ms / time, and the initial acquisition time of the motion camera is consistent with the initial acquisition time of the three-axis gyroscope. The visual image is then processed to calculate the feature points in the image, and then the feature points are marked in the motion database. Then, a trajectory error equation is established by combining the feature points in the motion database, the angular velocity collected by the three-axis gyroscope, and the node acceleration collected by the accelerometer. Then, the position corresponding to each node in the running trajectory model is adjusted through nonlinear optimization calculation. The adjusted trajectory model is the running trajectory model of the battery.

[0026] In the above step 5, the nodes on the battery running trajectory are refined, and the refinement process is as follows: first, the position of the nth node is extracted, and then the position of the n+1th node is extracted, and then the trajectory curve distance between the nth node and the n+1th node is calculated, and at the same time, the node acceleration of the corresponding nth node and the n+1th node detected by the accelerometer is extracted, and then according to the change rate of the node acceleration, the extraction point corresponding to the unit time is refined on the trajectory between the nth node and the n+1th node, and each refined point is used as the speed extraction point to obtain the battery speed extraction model;

[0027] In the above step 6, the speed points are processed separately in the battery speed extraction model. The separate processing process of the speed points is as follows: first, mark the mth separate speed point, then generate a unit vector about the mth speed point in the battery speed extraction curve, and this unit vector is the running direction of this speed point, then extract the m+1th and m-1th node positions, then calculate the trajectory curve distance between the m+1th and m-1th nodes, and then calculate the instantaneous speed of the mth speed point according to the trajectory curve distance between the nodes and the node time difference, and after processing, record the running direction and running speed of each speed point into the trajectory database;

[0028] In the above step 7, the speed time point to be queried is input, and then the running speeds of the two adjacent time points corresponding to the speed time point are retrieved from the trajectory database established in step 6, and then the average running speed of the two adjacent time points is calculated, which is the running speed of the battery at the queried speed time point.

[0029] Based on the above, the advantage of the present invention is that when the present invention is in use, a three-axis gyroscope is used to collect the running angular velocity of the battery in the whole process, and then the attitude angle at each time node is calculated by establishing and updating the quaternion runtime library, and then the running trajectory of the battery is restored according to the attitude angle, thereby improving the accuracy of the established trajectory, and by combining the measured characteristic points, angular velocity and acceleration to establish a trajectory error equation to optimize the curvature of the running trajectory, it is convenient to obtain the battery speed direction at the corresponding time node, and the practicality of the calculation method is improved, and a battery speed extraction model is established by refining the nodes on the battery running trajectory, and then the running direction and running speed of each speed point are recorded in the trajectory database, and then the speed point data adjacent to the specific time point is used to calculate the instantaneous speed at a specific time, thereby meeting complex actual detection needs.

[0030] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

Claims

1. A method for calculating battery motion trajectory and speed using a three-axis gyroscope, comprising the following steps: Step 1: Initial runtime library establishment; Step 2: Runtime library data update; Step 3: Initial trajectory establishment; Step 4: Trajectory path optimization; Step 5: Speed ​​model establishment; Step 6: Model node refinement; Step 7: Time point speed calculation; It is characterized by: In the above step 1, the accelerometer and the three-axis gyroscope are used to collect the initial acceleration and the initial angular velocity of the battery respectively, and then the initial attitude angle of the battery is calculated according to the initial acceleration and the initial angular velocity of the battery, and then the initial quaternion of the battery is calculated according to the quaternion method, and then the initial quaternion is recorded and marked in the quaternion runtime library; In the above step 2, the angular velocity of the battery during the whole process is collected by the three-axis gyroscope, and the data in the quaternion runtime library is updated, and then the node attitude angle corresponding to each node is calculated through the quaternions of all nodes in the quaternion runtime library; In the above step 3, the direction of the battery at each time node is analyzed according to the data in the quaternion runtime library, and the running distance in the direction of travel is calculated in combination with the angular velocity, and then the trajectory point at the next time node is marked in combination with the attitude angle and running distance of the node, and then the initial running trajectory model of the battery is established according to the trajectory point; In the above step 4, a motion camera collects visual images of the battery during movement, and then processes the visual images to calculate feature points in the images, and then marks the feature points in the motion database, and then combines the feature points in the motion database, the angular velocity collected by the three-axis gyroscope, and the node acceleration collected by the accelerometer to establish a trajectory error equation, and then adjusts the position corresponding to each node in the running trajectory model through nonlinear optimization calculation, and the adjusted trajectory model is the running trajectory model of the battery; In the above step 5, the nodes on the battery running trajectory are refined, and each refined point is used as a speed extraction point to obtain a battery speed extraction model; the refinement process is: first extract the nth node position, then extract the n+1th node position, then calculate the trajectory curve distance between the nth node and the n+1th node, and at the same time extract the node acceleration of the corresponding nth node and the n+1th node detected by the accelerometer, and then refine the extraction point corresponding to the unit time on the trajectory between the nth node and the n+1th node according to the change rate of the node acceleration; In the above step 6, the speed points are processed individually in the battery speed extraction model, and the running direction and running speed of each speed point are recorded in the trajectory database after processing; the individual processing process of the speed points is as follows: firstly, the mth individual speed point is marked, and then a unit vector of the mth speed point in the battery speed extraction curve is generated, and this unit vector is the running direction of this speed point, and then the positions of the m+1th and m-1th nodes are extracted, and then the trajectory curve distance between the m+1th and m-1th nodes is calculated, and then the instantaneous speed of the mth speed point is calculated according to the trajectory curve distance between the nodes and the node time difference; In the above step 7, the speed time point to be queried is input, and then the running speeds of the two adjacent time points corresponding to the speed time point are retrieved from the trajectory database established in step 6, and then the average running speed of the two adjacent time points is calculated, which is the running speed of the battery at the queried speed time point.

2. The method for calculating the battery motion trajectory and speed by a three-axis gyroscope according to claim 1, characterized in that: In the step 1, the signal acquisition rate of the accelerometer and the three-axis gyroscope is 20 ms / time, the update rate in the quaternion runtime library is 20 ms / time, and the initial quaternion is the initial data in the quaternion runtime library.

3. The method for calculating the battery motion trajectory and speed by a three-axis gyroscope according to claim 1, characterized in that: In step 2, the process of updating the quaternion runtime library according to the collected angular velocity is: calculating the node quaternion corresponding to each collected node by the quaternion method, and then recording and marking the node quaternion in the quaternion runtime library in sequence to complete the data update of the quaternion runtime library.

4. The method for calculating the battery motion trajectory and speed by a three-axis gyroscope according to claim 1, characterized in that: In the step three, the process of establishing the battery's operating trajectory model is as follows: first, a spatial rectangular coordinate system is established, and then the origin of the spatial rectangular coordinate system is used as the starting point of the battery's operating trajectory. Then, the trajectory points at each time node are extracted and marked in the established spatial rectangular coordinate system in sequence. Then, each trajectory point is connected in sequence to form the original trajectory, and then the curvature calculation is used to eliminate the inflection points on the original trajectory, thereby forming the battery's operating trajectory model.

5. The method for calculating the battery motion trajectory and speed by a three-axis gyroscope according to claim 1, characterized in that: In the step 4, the acquisition rate of the motion camera to acquire visual graphics is 20 ms / time, and the initial acquisition time of the motion camera is consistent with the initial acquisition time of the three-axis gyroscope.

Images