Planar motor space six-degree-of-freedom pose detection method

Abstract

The invention discloses a planar motor space six-degree-of-freedom pose detection method, and belongs to the technical field of displacement detection. The problems that an existing planar motor two-dimensional array position detection algorithm is complex, and the hardware cost is high are solved. Comprising the steps that six three-axis Hall sensors are arranged on a detection plate, any one pair of mutually orthogonal straight lines in the plane of the detection plate at most passes through the three three-axis Hall sensors, and the distance between any two three-axis Hall sensors is smaller than the polar distance; arranging a detection plate in a magnetic field of the two-dimensional permanent magnet array of the planar motor; three target three-axis Hall sensors are selected to calculate target moment three-axis position coordinates, starting moment center point coordinates of the three target three-axis Hall sensors, and target moment center point coordinates of the three target three-axis Hall sensors; starting moment coordinate system base vectors and target moment coordinate system base vectors of the three target three-axis Hall sensors are calculated at the same time; and calculating a rotation matrix and a translation matrix to obtain the central six-degree-of-freedom pose of the square detection plate. The method is used for plane motor pose detection.

Description

Technical Field

[0001] The present invention relates to a method for detecting the six-degree-of-freedom pose of a planar motor in space, belonging to the technical field of displacement detection. Background Art

[0002] The secondary permanent magnets of a planar motor usually adopt a form composed of four one-dimensional arrays. Compared with the secondary composed of a two-dimensional array, although the difficulty of decoupling and the position detection scheme are simpler, there are certain limitations in aspects such as the uniformity of the magnetic field strength and the content of harmonics.

[0003] For the existing publicly disclosed methods for detecting the six-degree-of-freedom position of a motor, one is based on a single-axis Hall, and the detection object is the magnetic field generated by the combination of four one-dimensional permanent magnet arrays. It performs combined calculations through a large number of Hall splices, with high calculation costs and complexities; another uses a Hall array to train in the way of a neural network. This way can take into account the influence of the stator current on the detected magnetic field, but the deployment of the neural network has high requirements for the processor, greatly increasing the cost during long-stroke applications; there is also a six-degree-of-freedom displacement detection method based on a two-dimensional magnetic field. It uses multiple Hall sensors, and the position detection algorithm is very complex and time-consuming, and an explicit expression of the six-degree-of-freedom pose cannot be obtained, only a numerical solution can be obtained. At the same time, it also depends on the position of the previous cycle, and the calculation accuracy is very low.

[0004] Therefore, aiming at the difficulties in position detection of a two-dimensional array, a new six-degree-of-freedom position detection scheme is needed to realize the position detection of a low-cost planar motor. Summary of the Invention

[0005] Aiming at the problems of complex position detection algorithms and high hardware costs of the existing two-dimensional arrays of planar motors, the present invention provides a method for detecting the six-degree-of-freedom pose of a planar motor in space.

[0006] A method for detecting the six-degree-of-freedom pose of a planar motor in space according to the present invention includes:

[0007] Select a square detection board, and the side length of the detection board is not greater than the pole pitch of the permanent magnet.

[0008] Arrange six three-axis Hall sensors on the detection board so that at most three three-axis Hall sensors pass through any pair of mutually orthogonal straight lines in the plane of the detection board, and the distance between any two three-axis Hall sensors is less than the pole pitch; then coincide the center of the detection board with the center of a square area formed by four adjacent permanent magnets of the two-dimensional permanent magnet array of the planar motor and arrange them in the magnetic field of the two-dimensional permanent magnet array of the planar motor.

[0009] Select target three-axis Hall sensors with non-zero magnetic flux density readings in three directions perpendicular to the two-dimensional permanent magnet array according to the readings of the three-axis Hall sensors; calculate the three-axis position coordinates of the target three-axis Hall sensors at the target moment based on the magnetic flux density readings of the target three-axis Hall sensors; calculate the center point coordinates of the three target three-axis Hall sensors at the starting moment and the center point coordinates at the target moment in combination with the three-axis position coordinates of the target three-axis Hall sensors at the starting moment; simultaneously calculate the coordinate system basis vectors of the three target three-axis Hall sensors at the starting moment and the coordinate system basis vectors at the target moment;

[0010] Calculate the rotation matrix based on the coordinate system basis vectors of the three target three-axis Hall sensors at the starting moment and the coordinate system basis vectors at the target moment, and calculate the translation matrix in combination with the center point coordinates of the three target three-axis Hall sensors at the starting moment and the center point coordinates at the target moment; obtain the six-degree-of-freedom pose of the center of the square detection plate according to the rotation matrix and the translation matrix, and realize the six-degree-of-freedom detection of the plane motor in space.

[0011] According to the method for detecting the six-degree-of-freedom pose of the plane motor in space of the present invention, the method for calculating the X-axis and Y-axis position coordinates of the target three-axis Hall sensors at the target moment is as follows:

[0012]

[0013] where x i is the X-axis coordinate of the i-th target three-axis Hall sensor, τ is the pole pitch, B iz is the Z-axis magnetic flux density of the i-th target three-axis Hall sensor, B ix is the X-axis magnetic flux density of the i-th target three-axis Hall sensor, n ix is the number of magnetic field periods in the X-axis direction of the i-th target three-axis Hall sensor; i = 1, 2, 3;

[0014] y i is the Y-axis coordinate of the i-th target three-axis Hall sensor, B iy is the Y-axis magnetic flux density of the i-th target three-axis Hall sensor, n iy is the number of magnetic field periods in the Y-axis direction of the i-th target three-axis Hall sensor;

[0015] The origin of the XYZ coordinate system is the center of the square area 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 method for detecting the six-degree-of-freedom pose of the plane motor in space of the present invention, n ix and n iy are calculated in the same way;

[0017]

[0018] where n x is the number of X-axis magnetic field periods in the previous detection period, dx is the X-axis displacement between adjacent detection periods, and xm is the magnetic field period jump threshold between adjacent detection periods.

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

[0020]

[0021] where z i is the Z-axis coordinate of the i-th target three-axis Hall sensor, and B 0xy is the fundamental wave magnetic flux density amplitude in the X-axis direction or Y-axis direction of the two-dimensional permanent magnet array magnetic field 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 central point coordinates of the three target three-axis Hall sensors at the starting moment are represented as C P :

[0023]

[0024] where P i is the position coordinate of the i-th target three-axis Hall sensor at the starting moment;

[0025] The central point coordinates of the three target three-axis Hall sensors at the target moment are represented as C Q :

[0026]

[0027] where Q i is the position coordinate of the i-th target three-axis Hall sensor at the target moment.

[0028] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the calculation method of the coordinate system base vectors of the three target three-axis Hall sensors at the starting moment is as follows:

[0029]

[0030] e3 = e1 × e2,

[0031] where e1 is the first base vector of the coordinate system at the starting moment, e2 is the second base vector of the coordinate system at the starting moment, and e3 is the third base vector of the coordinate system at the starting moment;

[0032] The coordinate system matrix R P = [e1 e2 e3].

[0033] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the calculation method for the base vectors of the coordinate system of three target three-axis Hall sensors at the target moment is as follows:

[0034]

[0035] f3 = f1 × f2,

[0036] where f1 is the first base vector of the coordinate system at the target moment, f2 is the second base vector of the coordinate system at the target moment, and f3 is the third base vector of the coordinate system at the target moment;

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

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

[0039]

[0040] where R is the rotation matrix.

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

[0042] T = C Q - RC P ,

[0043] where T is the translation matrix.

[0044] According to the planar motor spatial six-degree-of-freedom pose detection method of the present invention, the six-degree-of-freedom pose of the center of the square detection plate is expressed 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 rotation matrix about the X-axis, Ry is the rotation matrix about the Y-axis, and Rz is the rotation matrix about the Z-axis;

[0045] According to

[0046] The elements in the rotation matrix are determined by calculation;

[0047] Obtain:

[0048]

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

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

[0051]

[0052] Advantages of the present invention: The method of the present invention performs six-degree-of-freedom position detection of the motor based on the magnetic field information of the two-dimensional permanent magnet array. It has low hardware cost, uses fewer sensors, and has a low computational complexity. It can meet the detection requirements of the six-degree-of-freedom motion of the planar motor and achieve low-cost and high-speed six-degree-of-freedom pose detection of the planar motor.

[0053] The method of the present invention shows significant advantages in six-degree-of-freedom position detection, which are specifically reflected in the following aspects:

[0054] High-precision positioning: The position detection accuracy is only limited by the sensors and the acquisition of sensing data. Through multi-point layout and analytical solution, the cumulative errors of traditional multi-point fitting or recursive methods are eliminated. Real-time response ability: The analytical solution is simple to calculate, without the need for complex numerical optimization algorithms, and has strong real-time performance. It is suitable for high-speed motion scenarios such as precision machining and semiconductor manufacturing. Simple structure and cost optimization: The number of sensors is small, and only six sensors are required to achieve complete six-degree-of-freedom detection, reducing the hardware cost. There is no need for complex sensor arrangement and precise calibration, and the installation and maintenance are simple. Wide application range: It is applicable to various planar motor systems, such as cable-free suspension transportation, planar robots, and high-precision positioning platforms. It has strong scalability. By adjusting the magnetic field period and sensor layout, it can be adapted to devices of different sizes and strokes. In summary, the present invention is superior to the prior art in terms of accuracy, speed, system complexity, etc., and provides an efficient and reliable technical solution for the six-degree-of-freedom precise control in the field of high-end equipment manufacturing. Description of the Drawings

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

[0056] Figure 2 is a schematic diagram of the setting of six three-axis Hall sensors on the detection board;

[0057] Figure 3 is a flowchart of the six-degree-of-freedom pose detection method of the planar motor in the present invention. Detailed Embodiment

[0058] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0059] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0060] The present invention will be further described below in conjunction with the accompanying drawings, but it is not limited to the present invention.

[0061] Combined with Figures 1 to 3 As shown, the present invention provides a method for detecting the six-degree-of-freedom pose of a planar motor in space, including:

[0062] Select a square detection board, and the side length of the detection board is not greater than the pole pitch of the permanent magnet.

[0063] Arrange six three-axis Hall sensors on the detection board, so that at most three three-axis Hall sensors pass through any pair of mutually orthogonal straight lines in the plane of the detection board, and the distance between any two three-axis Hall sensors is less than the pole pitch; then coincide the center of the detection board with the center of a square area formed by four adjacent permanent magnets in the two-dimensional permanent magnet array of the planar motor and arrange them in the magnetic field of the two-dimensional permanent magnet array of the planar motor.

[0064] According to the readings of the three-axis Hall sensors, select three target three-axis Hall sensors with non-zero magnetic flux density readings in the direction perpendicular to the two-dimensional permanent magnet array; calculate the three-axis position coordinates of the target three-axis Hall sensors at the target moment according to the magnetic flux density readings of the target three-axis Hall sensors; combine the three-axis position coordinates of the target three-axis Hall sensors at the starting moment to calculate the center point coordinates of the three target three-axis Hall sensors at the starting moment and the center point coordinates at the target moment; at the same time, calculate the coordinate system basis vectors of the three target three-axis Hall sensors at the starting moment and the coordinate system basis vectors at the target moment.

[0065] Calculate the rotation matrix based on the coordinate system basis vectors of the three target three-axis Hall sensors at the starting moment and the coordinate system basis vectors at the target moment, and calculate the translation matrix by combining the center point coordinates of the three target three-axis Hall sensors at the starting moment and the center point coordinates at the target moment; obtain the six-degree-of-freedom pose of the center of the square detection board according to the rotation matrix and the translation matrix, and realize the six-degree-of-freedom detection of the planar motor in space.

[0066] The core idea of this embodiment is to arrange multiple three-axis Hall sensors in the magnetic field of the two-dimensional permanent magnet array, utilize the magnetic field characteristics and mathematical models, and calculate the six-degree-of-freedom pose of the target through the measurement of the sensors to obtain the three-axis magnetic field data.

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

[0068]

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

[0070] Then design a Hall sensor array. At least six three-axis Hall sensors are required on a detection array, and the arrangement of the six sensors meets the following requirements:

[0071] For any pair of mutually perpendicular straight lines in the plane, at most only three sensors can be passed through. That is, for any mutually perpendicular straight line, at least three sensors are not passed through by the straight line. The distance between any two sensors should be less than a pole pitch.

[0072] Determine three sensors for calculation. Read the three-axis magnetic field signals of the six sensors, and select three sensors whose Bz signal component is not 0. And mark the order as 1, 2, and 3.

[0073] According to the magnetic field strength, when the X-axis coordinate of the sensor is kτ, where k is an integer, at this time both By and Bz are 0, then the Z-axis height of the sensor cannot be calculated. The same is true for the Y-axis.

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

[0075]

[0076] Where x i is the X-axis coordinate of the i-th target three-axis Hall sensor, τ is the pole pitch, B iz is the Z-axis magnetic flux density of the i-th target three-axis Hall sensor, B ix is the X-axis magnetic flux density of the i-th target three-axis Hall sensor, n ix is the number of magnetic field periods in the X-axis direction of the i-th target three-axis Hall sensor; i = 1, 2, 3;

[0077] y i is the Y-axis coordinate of the i-th target three-axis Hall sensor, B iy is the Y-axis magnetic flux density of the i-th target three-axis Hall sensor, n iy is the number of magnetic field periods in the Y-axis direction of the i-th target three-axis Hall sensor;

[0078] The origin of the XYZ coordinate system is the center of the square area 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 is the same as the calculation method of n iy ;

[0080]

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

[0082] According to the already calculated x i and y i , using the formula followed by the calculation of , the method for the Z-axis position coordinate of the target three-axis Hall sensor at the target moment is:

[0083]

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

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

[0086] The initial coordinates of the sensor array have been determined during design. After six-degree-of-freedom displacement, the three-axis coordinates of the three sensors for calculation are obtained through the calculations in the previous steps. Let the original coordinates of the three sensors for calculation be P1, P2, and P3, and the coordinates after movement be Q1, Q2, and Q3. The following gives a displacement calculation method:

[0087] The central point coordinates of the three target three-axis Hall sensors at the starting moment are represented as C P :

[0088]

[0089] In the formula, P i is the starting moment position coordinate of the i-th target three-axis Hall sensor;

[0090] The central point coordinates of the three target three-axis Hall sensors at the target moment are represented as C Q :

[0091]

[0092] In the formula, Q i is the target moment position coordinate of the i-th target three-axis Hall sensor.

[0093] The calculation method for the base vectors of the coordinate system at the starting moment of three target three-axis Hall sensors is as follows:

[0094]

[0095] e3 = e1 × e2,

[0096] where e1 is the first base vector of the coordinate system at the starting moment, e2 is the second base vector of the coordinate system at the starting moment, and e3 is the third base vector of the coordinate system at the starting moment;

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

[0098] The calculation method for the base vectors of the coordinate system at the target moment of three target three-axis Hall sensors is as follows:

[0099]

[0100]

[0101] f3 = f1 × f2,

[0102] where f1 is the first base vector of the coordinate system at the target moment, f2 is the second base vector of the coordinate system at the target moment, and f3 is the third base vector of the coordinate system at the target moment;

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

[0104] Furthermore, the calculation method for the rotation matrix is as follows:

[0105]

[0106] where R is the rotation matrix.

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

[0108] T = C Q - RC P ,

[0109] where T is the translation matrix.

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

[0111] According to the rotation matrix R being the product of three-axis rotation matrices, set:

[0112]

[0113] The elements in the rotation matrix are determined by calculation;

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

[0115] We get:

[0116]

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

[0118] Then the six-degree-of-freedom pose of the center of the square detection board is:

[0119]

[0120] Embodiment:

[0121] Combined with Figure 1 shown, first, according to the common two-dimensional Halbach array, calculate the magnetic field distribution as:

[0122]

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

[0124] In theory, in three-dimensional space, at least three non-collinear points are required to uniquely determine a plane, but the translation of the plane in space and its rotation about the vertical axis (Rz axis) cannot be fully determined. In this embodiment, since the initial coordinates are known and the coordinates after movement are obtained through measurement, these three points not only uniquely determine the plane after movement, but also imply the translation amount and the rotation amount about 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 of the magnetic field components in the three directions are 0, and at this time, it cannot be used to calculate the six-degree-of-freedom displacement. Such special positions where calculation is impossible form an orthogonal grid in space. Also, for any three points, there exists an orthogonal coordinate system that can make these three points fall on one coordinate axis and thus cannot be used for calculation. Therefore, to be able to satisfy the six-degree-of-freedom position calculation at any time, a Hall array in the magnetic field region should consist of at least six three-axis Hall sensors, and these sensors need to meet the following requirements:

[0125] 1. For any pair of mutually orthogonal straight lines in the plane, at most only three sensors can be passed through. That is, for any mutually orthogonal straight lines, at least three sensors are not passed through by the straight lines. Otherwise, there will not be enough sensors to calculate the six-degree-of-freedom displacement.

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

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

[0128] For the sake of simplified description, it is stipulated that the magnetic field array is fixed, and the position detection PCB board performs translation and rotation in space. When the rotational movement occurs, it first rotates about the X axis, then about the Y axis, and finally about the Z axis. It is stipulated that the center of rotation is the center of Sensor 1. And the detection center of the six-degree-of-freedom pose (x, y, z, Rx, Ry, Rz) is the center of the position detection board, that is, the center of Sensor 1. It should be emphasized that the above regulations have no impact on the calculation of this six-degree-of-freedom position detection method, and different rotation orders only affect the expression for solving the rotation angle from the rotation matrix.

[0129] Finally, it is the calculation method and process of position detection. The flowchart of the program is as Figure 3 shown.

[0130] 1. Selection of available sensors: According to the previous description, when the sensor is at a special position, the coordinates of the sensor cannot be fully solved and cannot be used for calculation. It can be screened by detecting the signal of the Bz component of the magnetic field. If the Bz signal is not zero, it can be used for calculation, and three sensors with non-zero Bz signals are selected.

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

[0132]

[0133] When n is the number of magnetic field periods exceeded during the cross-magnetic field periodic motion. When starting for the first time, it is necessary to ensure that all sensors are within one magnetic field period. It should be noted that due to the use of arctan inverse tangent calculation, from simple mathematical knowledge, x i and y i range from (-τ / 2, τ / 2). When the position of one or more axes of one or more sensors exceeds the aforementioned range due to translation or rotation, it is necessary to use the continuity of motion to distinguish. Taking the X-axis of the No. 2 sensor as an example, when x2 continuously increases and exceeds τ / 2, it will jump to -τ / 2 and continue to increase. Subtract the x2 obtained in the previous position detection period from the x2 calculated in the current period to get the difference dx, and set a positive jump threshold xm. Determine the increase and decrease of n. Calculate and record the value of n in each calculation period. The calculation of the Y-axis is the same as that of the X-axis.

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

[0135] In the second step, the x i and y i of each of the three sensors have been calculated. Using the mathematical model of the magnetic field, solve the height z of each of the three sensors. The magnetic induction intensity formulas in all three directions can be used to solve the height of the Z-axis. For example, the expression of B x can be used to complete the calculation.

[0136] Substitute the x i and y i of the i-th sensor into the calculation, and calculate the corresponding z i . According to the previous regulations, the center point of position detection is the center of the No. 1 sensor. Therefore, the z i calculated by the No. 1 sensor is the Z-axis position of the detection board.

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

[0138] The sensor array has its initial coordinates determined during the design process. After undergoing displacements in six degrees of freedom, the three-axis coordinates of the three sensors used for calculation are obtained through the calculations in the previous steps. Assume the original coordinates of the three sensors for calculation are P1, P2, and P3, and the coordinates after movement are Q1, Q2, and Q3. Solve for the original center and the transformed center of the three points.

[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, the six-degree-of-freedom pose can finally be obtained.

[0144] Although the present 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 present invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed, as long as they do not depart from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the different dependent claims and the features described herein can be combined in a manner different from that described in the original claims. It should also be understood that the features described in connection with a single embodiment can be used in other described embodiments.

Claims

1. A method for detecting the six-degree-of-freedom position of a planar motor, characterized in that include, Select a square test plate, and the side length of the test plate should not be greater than the permanent magnet pole pitch; Six three-axis Hall sensors are arranged on a detection board, so that any pair of mutually orthogonal straight lines in the plane of the detection board pass through at most three three-axis Hall sensors, and the distance between any two three-axis Hall sensors is less than the pole pitch; then the center of the detection board is overlapped with the center of a square area formed by four adjacent permanent magnets of the two-dimensional permanent magnet array of the planar motor, and the detection board is arranged in the magnetic field of the two-dimensional permanent magnet array of the planar motor; According to the readings of the three-axis Hall sensor, three target three-axis Hall sensors whose magnetic flux density readings in the direction perpendicular to the two-dimensional permanent magnet array are not equal to zero are selected; the three-axis position coordinates of the target three-axis Hall sensor at the target time are calculated according to the magnetic flux density readings of the target three-axis Hall sensor; the center point coordinates of the three target three-axis Hall sensors at the starting time and the center point coordinates at the target time are calculated in combination with the three-axis position coordinates of the target three-axis Hall sensor at the starting time; and the coordinate system basis vectors of the three target three-axis Hall sensors at the starting time and the coordinate system basis vectors at the target time are calculated at the same time; The rotation matrix is ​​calculated based on the coordinate system basis vectors of the three target three-axis Hall sensors at the starting moment and the coordinate system basis vectors at the target moment, and the translation matrix is ​​calculated based on the center point coordinates of the three target three-axis Hall sensors at the starting moment and the center point coordinates at the target moment; the central six-degree-of-freedom posture of the square detection plate is obtained according to the rotation matrix and the translation matrix, thereby realizing the six-degree-of-freedom detection of the planar motor space.

2. The planar motor spatial six-degree-of-freedom posture detection method according to claim 1 is characterized in that: The method for calculating the X-axis and Y-axis position coordinates of the target three-axis Hall sensor at the target time is: Where x i is the X-axis coordinate of the i-th target three-axis Hall sensor, τ is the pole distance, B iz is the Z-axis magnetic flux density of the i-th target three-axis Hall sensor, B ix is the X-axis magnetic flux density of the i-th target three-axis Hall sensor, n ix is the number of magnetic field cycles in the X-axis direction of the i-th target three-axis Hall sensor; i = 1, 2, 3; y i is the Y-axis coordinate of the i-th target three-axis Hall sensor, B iy is the Y-axis magnetic flux density of the i-th target three-axis Hall sensor, n iy is the number of magnetic field cycles in the Y-axis direction of the i-th target three-axis Hall sensor; The origin of the XYZ coordinate system is the center of the square area 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.

3. The planar motor spatial six-degree-of-freedom posture detection method according to claim 2 is characterized in that: n ix With n iy The calculation method is the same as; Where n x 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.

4. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 3, characterized in that: The method for the Z-axis position coordinates of the target three-axis Hall sensor at the target time is: Where z i is the Z-axis coordinate of the i-th target three-axis Hall sensor, B 0xy is the fundamental magnetic flux density amplitude of the two-dimensional permanent magnet array magnetic field in the X-axis direction or the Y-axis direction when the Z-axis coordinate is 0.

5. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 4, characterized in that: The coordinates of the center points of the three target three-axis Hall sensors at the start time are expressed as C P : Where P i is the starting position coordinate of the i-th target three-axis Hall sensor; The coordinates of the three-axis Hall sensor center points at the target time are expressed as C Q : Where Q i is the target moment position coordinate of the i-th target three-axis Hall sensor.

6. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 5, characterized in that: The calculation method of the coordinate system basis vectors of the three target three-axis Hall sensors at the starting time is: e3=e1×e2, Where e1 is the first basis vector of the coordinate system at the starting time, e2 is the second basis vector of the coordinate system at the starting time, and e3 is the third basis vector of the coordinate system at the starting time; Get the starting time coordinate system matrix R P =[e1 e2 e3].

7. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 6, characterized in that: The calculation method of the target moment coordinate system basis vectors of the three target three-axis Hall sensor is: f3=f1×f2, Where 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; Get the target time coordinate system matrix R Q =[f1 f2 f3].

8. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 7, characterized in that: The calculation method of the rotation matrix is: Where R is the rotation matrix.

9. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 8, characterized in that: The translation matrix is ​​calculated as: T=C Q -RC P , Where T is the translation matrix.

10. The method for detecting the six-degree-of-freedom position of a planar motor in space according to claim 9, characterized in that: The six-degree-of-freedom pose of the center of the square detection plate is expressed 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 The elements of the rotation matrix are obtained by The calculation is determined; get: The elements in T are based on T=C Q -RC P The calculation is determined; The six-degree-of-freedom position of the center of the square detection plate is:

Images