Method for detecting the position of a planar motor

Abstract

The application discloses a kind of planar motor space six degrees of freedom pose detection method, belongs to displacement detection technical field.The application is in view of the problems of existing planar motor two-dimensional array position detection algorithm complexity, and high hardware cost.Problems include that six three-axis hall sensors are arranged on detection plate, so that any pair of mutually orthogonal straight lines in the plane of detection plate passes through at most three three-axis hall sensors, and the distance between any two three-axis hall sensors is less than the polar distance;Detection plate is arranged in the magnetic field of planar motor two-dimensional permanent magnet array;Three target three-axis hall sensors are selected to calculate three-axis position coordinates at target time and center point coordinates at target time and center point coordinates at starting time of three target three-axis hall sensors;The coordinate system base vector at starting time and the coordinate system base vector at target time of three target three-axis hall sensors are calculated simultaneously;Rotation matrix and translation matrix are calculated to obtain the center six degrees of freedom pose of square detection plate.The application is used for planar motor pose detection.

Description

Technical Field

[0001] This invention relates to a spatial six-degree-of-freedom pose detection method for a planar motor, belonging to the field of displacement detection technology. Background Technology

[0002] The secondary permanent magnet of a planar motor is usually composed of four one-dimensional arrays. Compared with the secondary composed of two-dimensional arrays, although the decoupling difficulty and position detection scheme are simpler, there are certain limitations in terms of magnetic field strength uniformity and harmonic content.

[0003] Existing publicly available six-degree-of-freedom (6DOF) position detection methods for motors include: one based on a single-axis Hall effect sensor, which detects the magnetic field generated by a combination of four one-dimensional permanent magnet arrays; this method involves combining and calculating a large number of Hall effect sensors, resulting in high computational cost and complexity; another method uses a Hall effect array trained with a neural network, which can take into account the influence of stator current on the detected magnetic field, but the deployment of the neural network places high demands on the processor, significantly increasing the cost for long-stroke applications; and a third method based on a two-dimensional magnetic field, which utilizes multiple Hall effect sensors. This method has a very complex and time-consuming position detection algorithm, and cannot obtain an explicit expression for the six-degree-of-freedom pose, only a numerical solution. Furthermore, it relies on the position of the previous cycle, resulting in very low computational accuracy.

[0004] Therefore, to address the challenges of position detection for two-dimensional arrays, a new six-degree-of-freedom position detection scheme is needed to achieve low-cost planar motor position detection. Summary of the Invention

[0005] To address the issues of complex and costly position detection algorithms for existing planar motor two-dimensional arrays, this invention provides a planar motor spatial six-degree-of-freedom pose detection method.

[0006] The present invention provides a planar motor spatial six-degree-of-freedom pose detection method, comprising,

[0007] Choose a square detection plate, and ensure that the side length of the detection plate is not greater than the pole distance of the permanent magnet;

[0008] Six triaxial Hall sensors are placed on the detection plate, such that any pair of mutually orthogonal straight lines in the plane of the detection plate pass through at most three triaxial Hall sensors, and the distance between any two triaxial Hall sensors is less than the pole pitch; then the center of the detection plate is arranged in the magnetic field of the two-dimensional permanent magnet array of the planar motor, coinciding with the center of a square area formed by four adjacent permanent magnets of the planar motor.

[0009] Based on the readings of the triaxial Hall sensors, select three target triaxial Hall sensors whose magnetic flux density readings are not equal to zero in the direction perpendicular to the two-dimensional permanent magnet array; calculate the triaxial position coordinates of the target triaxial Hall sensors at the target time based on the magnetic flux density readings of the target triaxial Hall sensors; combine the triaxial position coordinates of the target triaxial Hall sensors at the initial time to calculate the center point coordinates of the three target triaxial Hall sensors at the initial time and the center point coordinates at the target time; simultaneously calculate the basis vectors of the coordinate system at the initial time and the basis vectors of the coordinate system at the target time for the three target triaxial Hall sensors.

[0010] A rotation matrix is ​​calculated based on the basis vectors of the initial coordinate system and the basis vectors of the target coordinate system of the three target triaxial Hall sensors, and a translation matrix is ​​calculated by combining the coordinates of the center point of the three target triaxial Hall sensors at the initial time and the center point of the target time. The six-degree-of-freedom pose of the center of the square detection plate is obtained according to the rotation matrix and the translation matrix, thereby realizing the six-degree-of-freedom spatial detection of the planar motor.

[0011] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the method for calculating the X-axis and Y-axis position coordinates of the target triaxial Hall sensor at the target time is as follows:

[0012]

[0013] In the formula x i Let be the X-axis coordinate of the i-th target triaxial Hall sensor, τ be the polar distance, and B be the polar distance. iz Let B be the Z-axis magnetic flux density of the i-th target triaxial Hall sensor. ix Let n be the X-axis magnetic flux density of the i-th target triaxial Hall sensor. ix Let be the number of magnetic field cycles in the X-axis direction of the i-th target triaxial Hall sensor; i = 1, 2, 3;

[0014] y i Let B be the Y-axis coordinate of the i-th target triaxial Hall sensor. iy Let n be the Y-axis magnetic flux density of the i-th target triaxial Hall sensor. iy Let be the number of magnetic field cycles in the Y-axis direction of the i-th target triaxial Hall sensor;

[0015] The origin of the XYZ coordinate system is the center of the square region formed by four adjacent permanent magnets. The XY plane is the plane where the two-dimensional permanent magnet array is located, and the Z axis is perpendicular to the plane where the two-dimensional permanent magnet array is located.

[0016] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, n ix With n iy The calculation method is the same;

[0017]

[0018] In the formula n x dx is the number of magnetic field cycles in the X-axis direction of the previous detection cycle, dx is the X-axis displacement of the adjacent detection cycle, and xm is the magnetic field cycle jump threshold of the adjacent detection cycle.

[0019] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the method for determining the Z-axis position coordinates of the target triaxial Hall sensor at the target time is as follows:

[0020]

[0021] In the formula z i Let B be the Z-axis coordinate of the i-th target triaxial Hall sensor. 0xy The amplitude of the fundamental magnetic flux density of the two-dimensional permanent magnet array in the X-axis or Y-axis direction when the Z-axis coordinate is 0.

[0022] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the coordinates of the center points of the three target triaxial Hall sensors at the initial moment are represented as C. P :

[0023]

[0024] In the formula P i Let be the initial position coordinates of the i-th target triaxial Hall sensor;

[0025] The coordinates of the center point of the three target triaxial Hall sensor at any given time are represented as C. Q :

[0026]

[0027] In the formula Q i Let be the target time position coordinates of the i-th target triaxial Hall sensor.

[0028] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the method for calculating the basis vectors of the coordinate system of the three target triaxial Hall sensors at the initial moment is as follows:

[0029]

[0030] e3 = e1 × e2,

[0031] In the formula, e1 is the first basis vector of the initial time coordinate system, e2 is the second basis vector of the initial time coordinate system, and e3 is the third basis vector of the initial time coordinate system;

[0032] Obtain the coordinate system matrix R at the initial time. P = [e1 e2 e3].

[0033] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the method for calculating the basis vectors of the target time coordinate system of the three target triaxial Hall sensors is as follows:

[0034]

[0035] f3 = f1 × f2,

[0036] In the formula, f1 is the first basis vector of the target time coordinate system, f2 is the second basis vector of the target time coordinate system, and f3 is the third basis vector of the target time coordinate system;

[0037] Obtain the target time coordinate system matrix R Q =[f1 f2 f3].

[0038] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the rotation matrix is ​​calculated as follows:

[0039]

[0040] In the formula, R is the rotation matrix.

[0041] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the translation matrix is ​​calculated as follows:

[0042] T = C Q -RC P ,

[0043] In the formula, T is the translation matrix.

[0044] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the center six-degree-of-freedom pose of the square detection plate is represented as (x,y,z,Rx,Ry,Rz), where x is the X-axis coordinate of the detection plate center, y is the Y-axis coordinate of the detection plate center, z is the Z-axis coordinate of the detection plate center, Rx is the X-axis rotation matrix, Ry is the Y-axis rotation matrix, and Rz is the Z-axis rotation matrix.

[0045] according to

[0046] Elements in the rotation matrix are passed through The calculation determines;

[0047] get:

[0048]

[0049] The elements in T are determined according to T=C Q -RC P The calculation determines;

[0050] The center six-degree-of-freedom pose of the square detection plate is:

[0051]

[0052] The beneficial effects of the present invention are as follows: The method of the present invention is based on the magnetic field information of a two-dimensional permanent magnet array to detect the six degrees of freedom position of a motor. It has low hardware cost, uses fewer sensors, and has low computational complexity. It can meet the detection requirements of the six degrees of freedom motion of a planar motor in space and realize low-cost and high-speed planar motor six degrees of freedom pose detection.

[0053] The method of this invention exhibits significant advantages in six-degree-of-freedom position detection, specifically in the following aspects:

[0054] High-precision positioning: Position detection accuracy is limited only by the sensor and sensor data acquisition. Through multi-point layout and analytical solutions, the cumulative error of traditional multi-point fitting or recursive methods is eliminated. Real-time response capability: The analytical solution is simple to calculate, requiring no complex numerical optimization algorithms, and offers strong real-time performance. Suitable for high-speed motion scenarios, such as precision machining and semiconductor manufacturing. Simple structure and optimized cost: Fewer sensors are needed; only six are required to achieve complete six-degree-of-freedom detection, reducing hardware costs. No complex sensor arrangement or precision calibration is required, simplifying installation and maintenance. Wide applicability: Applicable to various planar motor systems, such as cableless levitation transportation, planar robots, and high-precision positioning platforms. High scalability: By adjusting the magnetic field period and sensor layout, it can be adapted to devices of different sizes and strokes. In summary, this invention surpasses existing technologies in terms of accuracy, speed, and system complexity, providing an efficient and reliable technical solution for precise six-degree-of-freedom control in the field of high-end equipment manufacturing. Attached Figure Description

[0055] Figure 1 This is a schematic diagram of a two-dimensional Halbach permanent magnet array;

[0056] Figure 2 This is a schematic diagram showing the setup of six triaxial Hall sensors on the detection board;

[0057] Figure 3 This is a flowchart of the planar motor spatial six-degree-of-freedom pose detection method described in this invention. Detailed Implementation

[0058] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0060] The present invention will be further described below with reference to the accompanying drawings, but this should not be construed as limiting the invention.

[0061] Combination Figures 1 to 3 As shown, this invention provides a method for detecting the six-degree-of-freedom spatial pose of a planar motor, including:

[0062] Choose a square detection plate, and ensure that the side length of the detection plate is not greater than the pole distance of the permanent magnet;

[0063] Six triaxial Hall sensors are placed on the detection plate, such that any pair of mutually orthogonal straight lines in the plane of the detection plate pass through at most three triaxial Hall sensors, and the distance between any two triaxial Hall sensors is less than the pole pitch; then the center of the detection plate is arranged in the magnetic field of the two-dimensional permanent magnet array of the planar motor, coinciding with the center of a square area formed by four adjacent permanent magnets of the planar motor.

[0064] Based on the readings of the triaxial Hall sensors, select three target triaxial Hall sensors whose magnetic flux density readings are not equal to zero in the direction perpendicular to the two-dimensional permanent magnet array; calculate the triaxial position coordinates of the target triaxial Hall sensors at the target time based on the magnetic flux density readings of the target triaxial Hall sensors; combine the triaxial position coordinates of the target triaxial Hall sensors at the initial time to calculate the center point coordinates of the three target triaxial Hall sensors at the initial time and the center point coordinates at the target time; simultaneously calculate the basis vectors of the coordinate system at the initial time and the basis vectors of the coordinate system at the target time for the three target triaxial Hall sensors.

[0065] A rotation matrix is ​​calculated based on the basis vectors of the initial coordinate system and the basis vectors of the target coordinate system of the three target triaxial Hall sensors, and a translation matrix is ​​calculated by combining the coordinates of the center point of the three target triaxial Hall sensors at the initial time and the center point of the target time. The six-degree-of-freedom pose of the center of the square detection plate is obtained according to the rotation matrix and the translation matrix, thereby realizing the six-degree-of-freedom spatial detection of the planar motor.

[0066] The core idea of ​​this implementation is to arrange multiple triaxial Hall sensors in the magnetic field of a two-dimensional permanent magnet array, and use the magnetic field characteristics and mathematical model to calculate the six-degree-of-freedom pose of the target by obtaining triaxial magnetic field data through sensor measurements.

[0067] Combination Figure 1 As shown, the multiple permanent magnets in its two-dimensional permanent magnet array are arranged in a grid pattern with alternating N and S poles, and the distribution of the magnetic field in space follows the formula:

[0068]

[0069] In the formula, B is the magnetic flux density along the X, Y, and Z axes in space, and x0, y0, and z0 are the position coordinates of the points in the magnetic field coordinate system. In practical applications, it represents a constant to be measured. The three expressions in the formula, from top to bottom, correspond to B respectively. x B y and B z That is, the magnetic flux density along the X-axis, the magnetic flux density along the Y-axis, and the magnetic flux density along the Z-axis.

[0070] Then, the Hall sensor array is designed. A detection array requires at least six triaxial Hall sensors, and the arrangement of these six sensors must meet the following requirements:

[0071] Any pair of mutually orthogonal lines in a plane can pass through at most three sensors. That is, for any two mutually orthogonal lines, at least three sensors are not passed through by the line. The distance between any two sensors should be less than one polar distance.

[0072] Identify the three sensors to be used for the calculation. Read the triaxial magnetic field signals from the six sensors, and select the three sensors whose Bz signal component is not zero. Label them as 1, 2, and 3.

[0073] Based on the magnetic field strength, when the X-axis coordinate of the sensor is kτ, where k is an integer, both By and Bz are 0, so the Z-axis height of the sensor cannot be calculated. The same applies to the Y-axis.

[0074] The method for calculating the X-axis and Y-axis position coordinates of the target's triaxial Hall sensor at the target time is as follows:

[0075]

[0076] In the formula x i Let be the X-axis coordinate of the i-th target triaxial Hall sensor, τ be the polar distance, and B be the polar distance. iz Let B be the Z-axis magnetic flux density of the i-th target triaxial Hall sensor. ix Let n be the X-axis magnetic flux density of the i-th target triaxial Hall sensor. ix Let be the number of magnetic field cycles in the X-axis direction of the i-th target triaxial Hall sensor; i = 1, 2, 3;

[0077] y i Let B be the Y-axis coordinate of the i-th target triaxial Hall sensor. iy Let n be the Y-axis magnetic flux density of the i-th target triaxial Hall sensor. iy Let be the number of magnetic field cycles in the Y-axis direction of the i-th target triaxial Hall sensor;

[0078] The origin of the XYZ coordinate system is the center of the square region formed by four adjacent permanent magnets. The XY plane is the plane where the two-dimensional permanent magnet array is located, and the Z axis is perpendicular to the plane where the two-dimensional permanent magnet array is located.

[0079] In this embodiment, n ix With n iy The calculation method is the same;

[0080]

[0081] In the formula n x dx is the number of magnetic field cycles in the X-axis direction of the previous detection cycle, dx is the X-axis displacement of the adjacent detection cycle, and xm is the magnetic field cycle jump threshold of the adjacent detection cycle.

[0082] Based on the already calculated x i and y i ,use The calculation follows the formula, and the method for determining the Z-axis position coordinates of the target's triaxial Hall sensor at the target time is as follows:

[0083]

[0084] In the formula z i Let B be the Z-axis coordinate of the i-th target triaxial Hall sensor. 0xy The amplitude of the fundamental magnetic flux density of the two-dimensional permanent magnet array in the X-axis or Y-axis direction when the Z-axis coordinate is 0.

[0085] Six-degree-of-freedom displacement calculation:

[0086] The sensor array has its initial coordinates determined during design. After six degrees of freedom displacement, the three-axis coordinates of the three sensors used for calculation are obtained through the previous steps. Let the original coordinates of the three sensors used for calculation be P1, P2, and P3, and their coordinates after motion be Q1, Q2, and Q3. The following is a displacement calculation method:

[0087] The coordinates of the center point of the three target triaxial Hall sensors at the start time are represented by C. P :

[0088]

[0089] In the formula P i Let be the initial position coordinates of the i-th target triaxial Hall sensor;

[0090] The coordinates of the center point of the three target triaxial Hall sensor at any given time are represented as C. Q :

[0091]

[0092] In the formula Q i Let be the target time position coordinates of the i-th target triaxial Hall sensor.

[0093] The method for calculating the basis vectors of the coordinate system at the initial moment of the three target triaxial Hall sensors is as follows:

[0094]

[0095] e3 = e1 × e2,

[0096] In the formula, e1 is the first basis vector of the initial time coordinate system, e2 is the second basis vector of the initial time coordinate system, and e3 is the third basis vector of the initial time coordinate system;

[0097] Obtain the coordinate system matrix R at the initial time. P = [e1 e2 e3].

[0098] The method for calculating the basis vectors of the time coordinate system of the three target triaxial Hall sensors is as follows:

[0099]

[0100]

[0101] f3 = f1 × f2,

[0102] In the formula, f1 is the first basis vector of the target time coordinate system, f2 is the second basis vector of the target time coordinate system, and f3 is the third basis vector of the target time coordinate system;

[0103] Obtain the target time coordinate system matrix R Q =[f1 f2 f3].

[0104] Furthermore, the rotation matrix is ​​calculated as follows:

[0105]

[0106] In the formula, R is the rotation matrix.

[0107] After obtaining R, the translation matrix can be calculated. The translation matrix is ​​calculated as follows:

[0108] T = C Q -RC P ,

[0109] In the formula, T is the translation matrix.

[0110] The six-degree-of-freedom pose of the center of the square detection plate is represented as (x,y,z,Rx,Ry,Rz), where x is the X-axis coordinate of the center of the detection plate, y is the Y-axis coordinate of the center of the detection plate, z is the Z-axis coordinate of the center of the detection plate, Rx is the X-axis rotation matrix, Ry is the Y-axis rotation matrix, and Rz is the Z-axis rotation matrix.

[0111] Since the rotation matrix R is the product of the three-axis rotation matrices, we set:

[0112]

[0113] Elements in the rotation matrix are passed through The calculation determines;

[0114] Assume the rotation order is first rotation along the Rx axis, then rotation along the Ry axis, and finally rotation along the Rz axis:

[0115] get:

[0116]

[0117] The elements in T are determined according to T=C Q -RC P The calculation determines;

[0118] The center six-degree-of-freedom pose of the square detection plate is:

[0119]

[0120] Example:

[0121] Combination Figure 1 As shown, the magnetic field distribution is first calculated based on a common two-dimensional Halbach array:

[0122]

[0123] Next, the layout design of the sensors on the position detection PCB is presented. In order to make full use of the periodic magnetic field characteristics of the two-dimensional Halbach permanent magnet array and to ensure full-dimensional detection of position and attitude, a Hall array consists of at least six triaxial Hall sensors in the magnetic field region. Each sensor can simultaneously measure the components of the magnetic field in three directions. The position arrangement of these sensors is carefully designed so that position calculation can be achieved at any position.

[0124] Theoretically, in three-dimensional space, at least three non-collinear points are needed to uniquely determine a plane, but the translation and rotation of that plane around the vertical axis (Rz axis) cannot be completely determined. In this embodiment, since the initial coordinates are known and the coordinates after motion are obtained through measurement, these three points not only uniquely determine the plane after motion, but also implicitly contain the translation and rotation around the Rz axis, making the system state uniquely determined in the global coordinate system. According to the analysis of the magnetic field distribution, when the x or y coordinate is kτ, where k is an integer, at least two magnetic field components in the three directions are 0, making it impossible to calculate the six-degree-of-freedom displacement. These special positions that cannot be calculated form an orthogonal grid in space. Since for any three points, there exists an orthogonal coordinate system that allows these three points to fall on a single coordinate axis, making them unusable for calculation. Therefore, to satisfy the six-degree-of-freedom position calculation at any given time, a Hall array must consist of at least six triaxial Hall sensors within the magnetic field region, and these sensors must satisfy the following:

[0125] 1. Any pair of mutually orthogonal lines in a plane can pass through at most three sensors. That is, for any two mutually orthogonal lines, at least three sensors must not be passed through by the lines. Otherwise, there would not be enough sensors to calculate the six degrees of freedom displacement.

[0126] 2. The distance between any two sensors should be less than one pole distance. Otherwise, if the two sensors are not in the same magnetic field period at the initial position, the position calculation will be incorrect.

[0127] The sensors are specifically arranged as follows: Figure 2 As shown. Sensor 1 is located at the origin (0, 0). Sensors 2 through 6 are distributed on a circle with sensor 1 as the center and radius d, with angles of 0°, 90°, 135°, 240°, and 330° respectively. As long as the sensor arrangement meets the aforementioned two requirements, six-degree-of-freedom position detection can be achieved. Generally, a value of d is recommended to be greater than τ / 4 and less than τ / 2. A value that is too large will cause the three sensors in the array to be out of sync with the same magnetic field period, while a value that is too small will increase the error.

[0128] For simplicity, the magnetic field array is fixed, while the position detection PCB board translates and rotates in space. During rotation, the rotation occurs first around the X-axis, then around the Y-axis, and finally around the Z-axis. The center of rotation is defined as the center of sensor number 1. Furthermore, the detection center of the six-degree-of-freedom pose (x, y, z, Rx, Ry, Rz) is the center of the position detection board, i.e., the center of sensor number 1. It is important to emphasize that the above stipulations do not affect the calculation of this six-degree-of-freedom position detection method; different rotation sequences only affect the expression for solving the rotation angle from the rotation matrix.

[0129] Finally, the calculation method and process for position detection are described, and the flowchart of the program is as follows: Figure 3 As shown.

[0130] 1. Selection of available sensors: As described above, when a sensor is located in a special position, its coordinates cannot be completely solved and cannot be used for calculation. The Bz component of the magnetic field can be detected to filter the sensors. If the Bz signal is not zero, it can be used for calculation. Select three sensors with non-zero Bz signals.

[0131] 2. Calculation of coordinates for single-point translation:

[0132]

[0133] n represents the number of magnetic field cycles exceeded during the cross-magnetic field cycle motion. During the initial startup, it is necessary to ensure that all sensors are within one magnetic field cycle. It should be noted that, due to the use of arctanine calculations, x can be determined using simple mathematics. i and y i The range is (-τ / 2, τ / 2). When the position of one or more axes of one or more sensors exceeds the aforementioned range due to translation or rotation, the continuity of motion needs to be utilized for identification. Taking the X-axis of sensor 2 as an example, when x2 continuously increases beyond τ / 2, it will jump to -τ / 2 and continue to increase. Subtract the x2 obtained in the previous position detection cycle from the x2 calculated in the current cycle to obtain the difference dx, and set a positive jump threshold xm. Determine the increase and decrease of n. The value of n must be calculated and recorded in each calculation cycle. The calculation of the Y-axis is the same as that of the X-axis.

[0134] 3. Calculation of the height z at three points:

[0135] In the second step, the x values ​​for each of the three sensors have already been calculated. i and y i Using a mathematical model of the magnetic field, the height z of each of the three sensors can be calculated. The formulas for the magnetic induction intensity in all three directions can be used to calculate the height along the Z-axis. For example, B... x The calculation is performed using an expression.

[0136] Substitute x from sensor i into the calculation i and y i The corresponding z is calculated. i According to the preceding rules, the center point of the position detection is the center of sensor 1. Therefore, the z-axis calculated by sensor 1... i This is the Z-axis position of the detection plate.

[0137] 4. Calculate the position center before and after the movement:

[0138] The sensor array has its initial coordinates determined during the design phase. After six degrees of freedom displacement, the three-axis coordinates of the three sensors used for calculation are obtained through the previous steps. The original coordinates of the three sensors used for calculation are set as P1, P2, and P3, and the coordinates after motion are set as Q1, Q2, and Q3. The original center and the transformed center of the three points are then determined.

[0139] 5. Calculate the three basis vectors of the original coordinate system;

[0140] 6. Calculate the three basis vectors of the coordinate system after displacement;

[0141] 7. Calculate the rotation matrix R and solve for the rotation angle;

[0142] 8. Calculate the translation matrix;

[0143] After the above steps, a six-degree-of-freedom pose can be obtained.

[0144] While the invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that different dependent claims and features described herein can be combined in ways different from those described in the original claims. It is also understood that features described in conjunction with individual embodiments can be used in other described embodiments.

Claims

1. A method for detecting the spatial six-degree-of-freedom pose of a planar motor, characterized in that... include, Choose a square detection plate, and ensure that the side length of the detection plate is not greater than the pole distance of the permanent magnet; Six triaxial Hall sensors are placed on the detection plate, such that any pair of mutually orthogonal straight lines in the plane of the detection plate pass through at most three triaxial Hall sensors, and the distance between any two triaxial Hall sensors is less than the pole pitch; then the center of the detection plate is arranged in the magnetic field of the two-dimensional permanent magnet array of the planar motor, coinciding with the center of a square area formed by four adjacent permanent magnets of the planar motor. Based on the readings of the triaxial Hall sensors, select three target triaxial Hall sensors whose magnetic flux density readings are not equal to zero in the direction perpendicular to the two-dimensional permanent magnet array; calculate the triaxial position coordinates of the target triaxial Hall sensors at the target time based on the magnetic flux density readings of the target triaxial Hall sensors; combine the triaxial position coordinates of the target triaxial Hall sensors at the initial time to calculate the center point coordinates of the three target triaxial Hall sensors at the initial time and the center point coordinates at the target time; simultaneously calculate the basis vectors of the coordinate system at the initial time and the basis vectors of the coordinate system at the target time for the three target triaxial Hall sensors. The rotation matrix is ​​calculated based on the basis vectors of the initial coordinate system and the basis vectors of the target coordinate system of the three target triaxial Hall sensors, and the translation matrix is ​​calculated by combining the coordinates of the center point of the three target triaxial Hall sensors at the initial time and the center point of the target time; the six-degree-of-freedom pose of the center of the square detection plate is obtained according to the rotation matrix and the translation matrix, so as to realize the six-degree-of-freedom spatial detection of the planar motor. The method for calculating the X-axis and Y-axis position coordinates of the target's triaxial Hall sensor at the target time is as follows: ， ， In the formula Let X be the X-axis coordinate of the i-th target triaxial Hall sensor. For polar distance, Let Z be the magnetic flux density of the i-th target triaxial Hall sensor. Let X be the magnetic flux density of the i-th target triaxial Hall sensor. Let be the number of magnetic field cycles in the X-axis direction of the i-th target triaxial Hall sensor; i = 1, 2, 3; Let Y be the Y-axis coordinate of the i-th target triaxial Hall sensor. Let Y be the magnetic flux density of the i-th target triaxial Hall sensor. Let be the number of magnetic field cycles in the Y-axis direction of the i-th target triaxial Hall sensor; The origin of the XYZ coordinate system is the center of the square region formed by four adjacent permanent magnets. The XY plane is the plane where the two-dimensional permanent magnet array is located, and the Z axis is perpendicular to the plane where the two-dimensional permanent magnet array is located. and The calculation method is the same; ， In the formula This represents the number of magnetic field cycles in the X-axis direction of the previous detection cycle. This represents the X-axis displacement between adjacent detection cycles. The threshold for the periodic jump of the magnetic field between adjacent detection cycles; The method for determining the Z-axis position coordinates of the target's three-axis Hall sensor at the target time is as follows: ， In the formula Let Z be the Z-axis coordinate of the i-th target triaxial Hall sensor. The amplitude of the fundamental magnetic flux density of the two-dimensional permanent magnet array in the X-axis or Y-axis direction when the Z-axis coordinate is 0.

2. The planar motor spatial six-degree-of-freedom pose detection method according to claim 1, characterized in that, The coordinates of the center points of the three target triaxial Hall sensors at the start time are expressed as follows: : ， In the formula Let be the initial position coordinates of the i-th target triaxial Hall sensor; The coordinates of the center point of the three target triaxial Hall sensor at any given time are represented as follows: : ， In the formula Let be the target time position coordinates of the i-th target triaxial Hall sensor.

3. The planar motor spatial six-degree-of-freedom pose detection method according to claim 2, characterized in that, The method for calculating the basis vectors of the coordinate system at the initial moment of the three target triaxial Hall sensors is as follows: ， ， ， In the formula Let be the first basis vector of the coordinate system at the initial time. Let be the second basis vector of the coordinate system at the initial time. The third basis vector of the coordinate system at the initial moment; Obtain the coordinate system matrix at the initial time. .

4. The planar motor spatial six-degree-of-freedom pose detection method according to claim 3, characterized in that, The method for calculating the basis vectors of the time coordinate system of the three target triaxial Hall sensors is as follows: ， ， ， In the formula Let be the first basis vector of the coordinate system at the target time. Let be the second basis vector of the target time coordinate system. The third basis vector of the target time coordinate system; Obtain the target time coordinate system matrix .

5. The planar motor spatial six-degree-of-freedom pose detection method according to claim 4, characterized in that, The rotation matrix is ​​calculated as follows: ， In the formula It is a rotation matrix.

6. The planar motor spatial six-degree-of-freedom pose detection method according to claim 5, characterized in that, The translation matrix is ​​calculated as follows: ， In the formula It is a translation matrix.

7. The planar motor spatial six-degree-of-freedom pose detection method according to claim 6, characterized in that, The six-degree-of-freedom pose of the center of the square detection plate is represented as (x, y, z, Rx, Ry, Rz), where x is the X-axis coordinate of the center of the detection plate, y is the Y-axis coordinate of the center of the detection plate, z is the Z-axis coordinate of the center of the detection plate, Rx is the X-axis rotation matrix, Ry is the Y-axis rotation matrix, and Rz is the Z-axis rotation matrix. according to , Elements in the rotation matrix are passed through The calculation determines; get: ; ， According to the elements The calculation determines; The center six-degree-of-freedom pose of the square detection plate is: 。

Images