Ball joint multi-degree-of-freedom attitude measurement system and method

Abstract

The invention belongs to the related technical field of sensor measurement, and discloses a ball joint multi-degree-of-freedom attitude measurement system and method. The system comprises a magnetic field measurement module, a light field measurement module and a calculation module, wherein the magnetic field measurement module measures the change of a magnetic field when a ball joint moving end moves relative to a fixed end; the light field measurement module measures the change of a light signal when the ball joint moving end moves relative to the fixed end; the calculation module is used for calculating the movement angle and angular velocity of the movement end of the ball joint relative to the fixed end at the current moment according to the magnetic field change measured by the magnetic field measurement module and the optical signal change measured by the optical field measurement module; and fusing the angle and the angular velocity at the current moment to obtain the rotation angle of the moving end of the ball joint relative to the fixed end at the next moment. According to the invention, advantage complementation is realized, and the precision and robustness of the measurement system are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to sensor measurement, and more specifically, relates to a ball joint multi-degree-of-freedom posture measurement system and method. Background Art

[0002] Ball joints with integrated multi-rotational degrees of freedom can greatly reduce the complexity of the transmission structure, and their industrial application scenarios are increasing day by day. In ball joint motion control, the high-precision and real-time requirements of ball joint multi-degree-of-freedom attitude feedback are the basis for the application and control of ball joints. However, the coupling effect of multi-rotational degrees of freedom increases the difficulty of attitude measurement.

[0003] The existing multi-degree-of-freedom attitude measurement of ball joints generally adopts contact measurement and non-contact measurement using a single sensor. The contact attitude measurement of ball joints usually selects a combination of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer to solve the attitude, but this solution can only output the angle of the moving end of the ball joint relative to the inertial system but cannot output the angle of the moving end relative to the fixed end. In addition, contact with the moving end will introduce external disturbances that affect the control effect. At the same time, the accelerometer is susceptible to high-frequency interference, the gyroscope has an integral drift error, and the magnetometer cannot be used in a strong magnetic environment. These reasons lead to great limitations in the use scenarios of this solution; for non-contact measurement, the research mainly uses a single type of sensor, which can output the angle of the moving end relative to the fixed end without interference from external disturbances, but the measurement of a single type of sensor has an integral drift error, making it difficult to measure for a long time. There is no zero point position and can only be calculated from the attitude at zero point, or there is a large noise that makes the measurement accuracy low. Summary of the invention

[0004] In response to the above defects or improvement needs of the prior art, the present invention provides a ball joint multi-degree-of-freedom posture measurement system and method to solve the problems of contact measurement introducing external disturbances, inability to output the relative angle of the moving end, and non-contact measurement with high noise, no zero point position, and drift errors.

[0005] To achieve the above object, according to one aspect of the present invention, a ball joint multi-degree-of-freedom posture measurement system is provided, characterized in that the system comprises a magnetic field measurement module, a light field measurement module and a calculation module, wherein:

[0006] The magnetic field measurement module measures the change in the magnetic field when the moving end of the ball joint moves relative to the fixed end;

[0007] The light field measurement module measures the change of the light signal when the moving end of the ball joint moves relative to the fixed end;

[0008] The calculation module is used to calculate the angle and angular velocity of the ball joint moving end relative to the fixed end at the current moment according to the magnetic field change measured by the magnetic field measurement module and the light signal change measured by the light field measurement module, and then fuse the angle and angular velocity at the current moment to obtain the angle of rotation of the ball joint moving end relative to the fixed end at the next moment.

[0009] Further preferably, the magnetic field measurement module comprises a permanent magnet and a magnetic sensor, the permanent magnet is arranged on the moving end of the ball joint, and the magnetic sensor is arranged on the fixed end of the ball joint.

[0010] Further preferably, the light field measurement module includes a detection surface, a light sensor and a lens, wherein the light sensor and the lens are arranged at the fixed end of the ball joint, the detection surface is arranged at the moving end of the ball joint, and the lens is arranged between the light sensor and the detection surface. The light emitted by the light sensor passes through the lens and is reflected or scattered on the detection surface. The reflected or scattered light is converged by the lens and enters the light sensor, thereby realizing the measurement of the light signal.

[0011] Further preferably, the number of permanent magnets is greater than or equal to 1, and the number of magnetic sensors is greater than or equal to 3.

[0012] Further preferably, the number of the detection surfaces, the optical sensors and the lenses is the same, and the number of the optical sensors is greater than or equal to 2.

[0013] Further preferably, the straight-line distance between the detection surface and the bottom of the lens is 1.6-2.4 mm.

[0014] According to another aspect of the present invention, a method for measuring the multi-degree-of-freedom posture of a ball joint is provided, the method comprising the following steps:

[0015] S1 The moving end of the ball joint rotates relative to the fixed end, using the changes in the magnetic field and the changes in the optical signal in the above-mentioned measurement system;

[0016] S2 uses the measured magnetic field changes to calculate the angle of the moving end of the ball joint relative to the fixed end at the current moment;

[0017] S3 uses the change of the light field to calculate the angular velocity of the moving end of the ball joint relative to the fixed end at the current moment;

[0018] S4 uses a Kalman filter to fuse the angular velocity at the current moment calculated in steps S2 and S3 to obtain the rotation angle of the moving end of the ball joint relative to the fixed end at the next moment.

[0019] Further preferably, the angle is obtained according to the following steps:

[0020] Constructing a data set of one-to-one correspondence between the angle of rotation of the ball joint moving end relative to the fixed end and the vector corresponding to the magnetic field measured by the magnetic sensor, wherein the angle is obtained by IMU measurement;

[0021] Using the data set to train a deep learning model, the input of the deep learning model is the magnetic field measured by the magnetic sensor, and the output is the angle at which the moving end of the ball joint rotates relative to the fixed end;

[0022] A magnetic sensor is used to measure the magnetic field when the moving end of the ball joint rotates relative to the fixed end to obtain the vector corresponding to the magnetic field. The vector is input into the trained deep learning model to output the real-time rotation angle of the moving end of the ball joint relative to the fixed end.

[0023] Further preferably, the angular velocity is calculated according to the following formula:

[0024]

[0025] Among them, q′ OPT represents the angular velocity of the moving end of the ball joint relative to the fixed end calculated by the light field measurement module, q KF is the angle of the moving end of the ball joint relative to the fixed end at the last moment, v represents the column vector composed of the velocity values ​​measured by all optical sensors, r represents the radius in the sensor spherical coordinate parameters, Γ l The kinematic equations when the number of light sensors is l are represented as follows: is Γ l is the pseudo-inverse of , and l represents the number of light sensors.

[0026] Further preferably, the rotation angle of the ball joint moving end relative to the fixed end at the next moment is calculated according to the following formula:

[0027]

[0028] Among them, q KF It is the angle of the moving end of the ball joint relative to the fixed end at the previous moment, x represents the state variable, x1, x2, x3 represent the 1st, 2nd, and 3rd components of the state variable x respectively, k represents the current moment, and k+1 represents the next moment.

[0029] In general, the above technical solutions conceived by the present invention have the following beneficial effects compared with the prior art:

[0030] 1. The present invention adopts a light field measurement module and a magnetic field measurement module to obtain the angle and angular velocity of the moving end of the ball joint relative to the fixed end at the current moment from the two perspectives of light signal and magnetic field signal respectively, and then fuses the two to calculate the angle of the moving end relative to the fixed end at the next moment. Among them, magnetic sensing is used for angle measurement to effectively avoid zero drift and drift errors, and optical sensing can obtain high measurement accuracy and low noise due to its high resolution and tracking speed; optimizing the layout of the two types of sensors and combining them through a fusion algorithm can achieve complementary advantages and improve the accuracy and robustness of the system.

[0031] 2. The measurement system provided by the present invention can directly and simultaneously output the angle of the moving end relative to the fixed end of the ball-joint multi-degree-of-freedom motion system and is non-contact. Compared with traditional measurement modules such as encoders and IMUs, it solves the problems of requiring contact measurement to introduce external disturbances (for encoders and IMUs), requiring dispersed arrangement of encoders for measuring multi-axis rotation resulting in low integration (for encoders), only being able to output the angle of the motion system relative to the inertial system (for IMUs), and being unable to use a magnetometer to calibrate the yaw angle in a strong magnetic environment (for IMUs), and has strong versatility.

[0032] 3. The present invention adopts a Kalman filter to fuse the angle and angular velocity of the current moment of the ball joint moving end relative to the fixed end. The fusion algorithm complements the advantages of the non-contact measurement method and the contact measurement method. The magnetic measurement provides an angle, which can eliminate the problem of no zero point position and drift error. The optical measurement can obtain high-precision angular velocity because the optical sensor has a very high optical resolution, which makes up for the defect of large noise in the magnetic sensor measurement. A suitable fusion algorithm can achieve a measurement effect with zero point position, no drift error, and low noise; the Kalman filter is a statistical recursive data processing algorithm for data fusion, and its goal is to minimize the noise error in the process of minimizing the error covariance estimation. The optimal estimated value of the result can be calculated based on the real-time adjustment of the proportional coefficient from the noise, and a smoother and more accurate tracking signal can be provided. In addition, any measurement method with a mapping relationship with the posture can be used as a data source for the posture solution Kalman filter, and the application range is wider than that of the Mahony filter and the Madgwick filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a cross-sectional view of a ball joint multi-degree-of-freedom posture measurement system constructed according to a preferred embodiment of the present invention;

[0034] Figure 2 is a cross-sectional view of a ball joint multi-degree-of-freedom posture measurement system constructed according to another preferred embodiment of the present invention;

[0035] Figure 3 is a front structural schematic diagram of an optical measurement module constructed according to a preferred embodiment of the present invention;

[0036] Figure 4 is a schematic diagram of the back structure of an optical measurement module constructed according to a preferred embodiment of the present invention;

[0037] Figure 5 is a side view of a light measurement module constructed in accordance with a preferred embodiment of the present invention;

[0038] Figure 6 It is a schematic diagram of the principle of a ball joint multi-degree-of-freedom posture measurement method constructed according to a preferred embodiment of the present invention.

[0039] Throughout the drawings, the same reference numerals are used to denote the same elements or structures, wherein:

[0040] 1-ball joint fixed end, 2-ball joint moving end, 3-permanent magnet, 4-ball bearing, 5-detection surface, 6-light sensor, 7-magnetic sensor, 8-sensor circuit board, 9-lens. DETAILED DESCRIPTION

[0041] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0042] In non-contact measurement, optical sensing and magnetic sensing schemes can achieve high-precision measurement at a relatively low cost. Taking into account the shortcomings of single-sensor measurement schemes and the adjustment and processing of redundant data, multi-sensor fusion is introduced to achieve better posture measurement through multi-sensor integration and fusion technology. The present invention designs a low-cost, easy-to-install, multi-sensor integrated non-contact ball joint multi-degree-of-freedom posture fusion sensing system based on optical sensors and magnetic sensors. The system includes a magnetic field measurement module, an optical field measurement module and a calculation module, wherein:

[0043] The magnetic field measurement module measures the change of the magnetic field when the ball joint moving end moves relative to the fixed end; the light field measurement module measures the change of the light signal when the ball joint moving end moves relative to the fixed end; the calculation module is used to calculate the angle and angular velocity of the ball joint moving end relative to the fixed end based on the magnetic field change measured by the magnetic field measurement module and the light signal change measured by the light field measurement module.

[0044] The magnetic field measurement module includes a permanent magnet and a magnetic sensor, wherein the permanent magnet is arranged on the moving end of the ball joint, and the magnetic sensor is arranged on the fixed end of the ball joint. In the case of a single permanent magnet, the magnetic field vector values ​​generated by the permanent magnet at different positions are different, and the magnetic field values ​​of the magnetic sensors at different positions for the same permanent magnet are different; in the case of multiple permanent magnets, the magnetic fields of multiple permanent magnets can be vector-superimposed in the same magnetic sensor coordinate system to obtain the sum magnetic field, and the sum magnetic field can be regarded as a whole for calculation, so the number of permanent magnets is required to be greater than or equal to 1. An angle has three components, and at least three equations of magnetic field values ​​and angles need to be established to solve the angle, so three or more magnetic sensors are required. In addition, increasing the number of magnetic sensors 7 and reasonably arranging the positions of magnetic sensors 7 can increase the gradient of the magnetic field and the angle, and improve the measurement effect.

[0045] The permanent magnet 3 is fixed to the moving end of the ball joint. It is required that the magnetic field generated by all permanent magnets 3 at the magnetic sensor 7 does not exceed the measurement range of the magnetic sensor 7 under any rotation, and there is no shape requirement. The permanent magnet 3 and the magnetic sensor 7 are relatively close to each other. The close distance can reduce external magnetic field interference and increase the gradient of the magnetic field and angle. Increasing the gradient is conducive to reducing the error of deep learning.

[0046] The light field measurement module includes a detection surface 5, a light sensor 6 and a lens 9, wherein the light sensor 6 and the lens 9 are arranged at the fixed end 1 of the ball joint, the detection surface 5 is arranged at the moving end 2 of the ball joint, and the lens 9 is arranged between the light sensor 6 and the detection surface 5. The light emitted by the light sensor 6 is irradiated on the detection surface 5 through the lens 9 and is reflected or scattered. The reflected or scattered light is captured by the lens and enters the light sensor 6, thereby realizing the measurement of the light signal.

[0047] The number of detection surfaces 5, light sensors 6 and lenses 9 is the same, and the number of light sensors 6 is greater than or equal to 2. The detection surface 5 is made of non-transparent material, and the surface has a certain texture or roughness. There is no requirement for the specific material, and metal, resin, nylon, leather and other materials can be selected. The thickness and size of the detection surface 5 are determined according to the actual movement requirements of the ball joint. The thickness is required not to interfere with the movement of the ball joint and not to affect the range of movement. The size requires that the LED light beam can be irradiated on the detection surface 5 under any rotation of the light sensor so that the light beam can be normally captured by the photosensitive element.

[0048] The straight-line distance between the detection surface 5 and the bottom of the lens 9 is 1.6-2.4 mm. If the distance is too small, the accuracy of the light signal received by the photosensitive element of the optical sensor 6 will be affected. If the distance is too far, external light will pass through the lens 9 and be captured by the photosensitive element, affecting the data accuracy.

[0049] In two embodiments of the present invention, in one embodiment, Figure 1As shown, the ball joint moving end wraps the fixed end, the detection surface 5 is fixed to the inner surface of the moving end, and the light sensor 6 and the lens 9 are arranged outward, facing the detection surface 5. In another embodiment, as Figure 2 As shown, the fixed end of the ball joint wraps the moving end, the detection surface 5 is fixed on the outer surface of the moving end, and the light sensor 6 and the lens 9 are arranged inwards, facing the detection surface 5. The installation position of the measurement module can be designed according to the actual structure of the ball joint motion system.

[0050] In one embodiment of the present invention, Figures 3 to 5 As shown, the magnetic sensor 7, the optical sensor 6, the magnetic sensor 7 and the lens 9 are all arranged on the sensor circuit board 8, and the magnetic sensor 7 is at any position on the sensor circuit board 8. There is no requirement for the number of sensor circuit boards 8. Multiple optical sensors and magnetic sensors can be arranged in one sensor circuit. The number of sensor circuit boards can also be 1. There is no fixed requirement for the position of the sensor circuit board 8, which is determined according to the design of the optical detection surface. Among them, there is a rectangular groove in the center of the sensor circuit board 8. There are holes at both ends of the optical sensor 6, and the lens 9 is assembled according to the holes. The two are matching equipment and do not require additional fasteners. The optical sensor 6 has a built-in infrared LED. After the optical sensor 6 is powered on, the LED emits a light beam. The light beam passes through the lens 9 and irradiates the detection surface 5 to generate reflection and scattering. The photosensitive element in the center of the optical sensor captures the infrared light reflected and scattered and penetrates the lens 9, and the internal processor converts the received light signal into a plane displacement. The infrared light beam needs to be emitted from the optical sensor 6 and received, so a rectangular groove is required in the center of the sensor circuit board 8, and the size of the groove is specifically designed according to the optical sensor 6. In addition, the optical sensor 6 is a pin-type component with the pins facing downward, so the sensor circuit board 8 needs to be between the optical sensor 6 and the lens 9 .

[0051] In one embodiment of the present invention, four magnetic sensors 7 and one optical sensor 6 are arranged on a measuring circuit board 8, and each optical sensor 6 must be assembled with a lens 9, and a total of four sensor circuit boards 8 are arranged, with a total of sixteen magnetic sensors 7 and four optical sensors 6. In this embodiment, the number of optical sensors 6 is determined to be four according to the processor performance and the number of interfaces; in this embodiment, the processor can simultaneously access up to 16 magnetic sensors 7. In order to reduce the design and production costs, all sensor circuits are designed to be consistent, and four magnetic sensors 7 are arranged on each group of sensor circuits, and the positions of the four magnetic sensors 7 are determined according to the arrangement optimization.

[0052] Figure 3 This is a flow chart of magnetic and optical fusion measurement of ball joint multi-degree-of-freedom measurement system. The figure mainly indicates the measurement methods of various sensors and the calculation method of sensor fusion.

[0053] The measurement method of the above-mentioned ball joint multi-degree-of-freedom posture measurement system will be introduced below.

[0054] (1) Measurement principle of magnetic measurement module

[0055] Under the condition that the geometric center of the permanent magnet is the source point in a single permanent magnet coordinate system, the magnetic field vector excited by the permanent magnet at any point in space is calculated by the following formula (Biosafar's law).

[0056]

[0057] Among them, q MAG It represents the angle of the moving end of the ball joint relative to the fixed end calculated by the magnetic field measurement module. PM B represents the magnetic field excited by the permanent magnet at the sensor in the permanent magnet coordinate system, μ0 represents the vacuum magnetic permeability, represents the gradient, M represents the polarization intensity of the permanent magnet, P represents the integrated point on the permanent magnet, MAG P represents the position vector of the point, n represents the normal vector of the permanent magnet surface, R1 represents the rotation matrix from the fixed coordinate system to the moving coordinate system, and q MAG related, represents volume fraction, Represents surface integral.

[0058] The permanent magnet is fixedly connected to the moving coordinate system, and the magnetic sensor is fixedly connected to the fixed coordinate system. Then, in the magnetic sensor coordinate system, the forward magnetic field model of a single sensor and a single permanent magnet is as follows.

[0059]

[0060] in, MAG B represents the measurement value of the magnetic sensor, that is, the magnetic field excited by the permanent magnet at the sensor in the magnetic sensor coordinate system. R2 represents the rotation matrix from the fixed coordinate system to the magnetic sensor coordinate system. The magnetic sensor is set at the fixed end of the ball joint, so R2 is a fixed value. Represents the rotation matrix from the moving coordinate system to the fixed coordinate system, which is the transpose of R1 and is the same as q MAG R3 represents the rotation matrix from the permanent magnet coordinate system to the moving coordinate system. The permanent magnet is set at the moving end of the ball joint, so R3 is a constant.

[0061] When the ball joint moves at the end, and R1 will be continuously updated, then MAG The value of B is also updated accordingly. The forward solution model is used to calculate the measured value of the magnetic sensor through the angle.

[0062] If the number of permanent magnets is not 1, the magnetic sensor measurement value is the sum of the magnetic field vectors excited by each permanent magnet at the magnetic sensor. The magnetic field values ​​are all expressed in the magnetic sensor coordinate system, so the magnetic field values ​​excited by different permanent magnets can be directly added numerically, as shown in the following formula. Where PM represents the permanent magnet, PMi represents the i-th permanent magnet, the total number is n, and R 3iRepresents the rotation matrix from the i-th permanent magnet coordinate system to the moving coordinate system.

[0063]

[0064] The magnetic field value of the permanent magnet at the same magnetic sensor may be the same in different postures, so multiple magnetic sensors need to be set, that is, the measurement values ​​of all magnetic sensors are not exactly the same in any posture of the permanent magnet, as shown in the following formula. Wherein, MAG represents the magnetic sensor, j represents the number of magnetic sensors, and B represents the column vector composed of all magnetic sensor measurement values.

[0065] B(q MAG )=[ MAG1 B(q MAG )… MAGj B(q MAG )] T

[0066] By arranging multiple magnetic sensors, there is a one-to-one mapping relationship between the posture and the magnetic sensor measurement value column vector B, that is, there is a bijective property, as shown in the second formula in the above formula. The three-degree-of-freedom forward magnetic field model cannot derive an explicit inverse expression, but deep learning can fit the magnetic sensor measurement value B and the angle q very well. MAG Numerical relationship, the inverse model can be replaced by a deep learning model, as shown in the following formula. Therefore, by measuring the magnetic field at multiple magnetic sensor positions, the angle of the moving end of the ball joint relative to the fixed end can be calculated.

[0067] q MAG =BPnet(B)

[0068] Among them, BPnet represents the deep learning model.

[0069] (2) Measurement principle of optical measurement module

[0070] The optical measurement scheme is used to measure angular velocity. The optical sensor, lens, and detection surface constitute the hardware part of the optical measurement. The optical sensor is fixed to the fixed end of the ball joint, used to emit infrared beams and receive optical signals and output x and y axis displacements; the lens is used to guide the emission of infrared beams and focus the reflected light to the photosensitive element for collection; the detection surface is fixed to the moving end of the ball joint, and the surface has a certain texture. The optical sensor outputs displacement information by collecting the texture changes of the moving detection surface.

[0071] The angular velocity of the moving end of the ball joint in the fixed coordinate system can be obtained by calculating the angular velocity in the moving coordinate system, as shown in the following formula.

[0072] ω(q′ OPT ,q KF ) = q′ OPT1 I(q KF )+q′ OPT2J′(q KF )+q′ OPT3 K″(q KF )

[0073] Among them, q′ OPT represents the angular velocity of the moving end of the ball joint relative to the fixed end calculated by the light field measurement module, q′ OPT1 ,q′ OPT2 ,q′ OPT3 They represent the angular velocity q′ of the ball joint moving end OPT The three components of the inertial coordinate system are: ω represents the angular velocity in the inertial coordinate system, I represents the x-axis direction vector, J′ represents the y-axis direction vector after rotating around the x-axis, K″ represents the z-axis direction vector after rotating around the x-axis and then around the y-axis, and q KF It is the angle of the moving end of the ball joint relative to the fixed end at the last moment. (Here, the xyz Euler angle sequence is used as an example, and the Euler angle sequence can be modified as needed)

[0074] When a point not at the center of the sphere rotates around the center of the sphere, the point will generate a tangential velocity perpendicular to the radial direction. The tangential velocity of the point can be calculated using the angle and angular velocity information. s v, as shown below. Where, s v represents the tangential velocity of the moving end of the ball joint in a fixed coordinate system, OPT P represents the light sensor position vector, and × represents vector cross product.

[0075] s v(q′ OPT ,q KF )=ω(q′ OPT ,q KF )× OPT P

[0076] The tangential velocity of the point is only in the x and y directions, and the velocity component in the z direction is 0. The three-dimensional velocity can be transferred to the two-dimensional coordinate system of the plane, and the kinematic model of the three-dimensional rotation angular velocity and the two-dimensional plane motion velocity can be established. If the plane coordinate system is selected as the optical sensor coordinate system, the following formula can be obtained.

[0077]

[0078] in, OPT v represents the optical sensor measurement value, that is, the velocity of the ball joint motion end in the optical sensor coordinate system, OPT v x , OPT v y They represent the x-axis and y-axis components of the light sensor measurement value, respectively, and θ represents the polar angle in the spherical coordinate parameters of the light sensor. represents the azimuth in the spherical coordinate parameters of the light sensor, r represents the radius in the spherical coordinate parameters of the sensor, θ, r are all constants, Γ is the forward kinematics equations, and q KF The analytical expression is shown in Figure 1. R4 represents the rotation matrix from the fixed coordinate system to the optical sensor coordinate system. The optical sensor is set at the fixed end of the ball joint, so R4 is a constant.

[0079] When the ball joint moves, the detection surface information detected by the optical sensor will change, thus outputting the displacement increments of the moving end surface on the x and y axes in the optical sensor plane coordinate system. The displacement increment is considered as the velocity per unit time, and then the kinematic model of the surface displacement increment and the Euler angle angular velocity is established, which is the above-mentioned OPT v expression. The optical sensor detects planar motion and only outputs the displacement increments of the x- and y-axis detection surfaces. Solving the three rotation angles requires that the number of optical sensors is greater than or equal to 2, and the inverse solution of the kinematic model has a unique solution. The optical sensor kinematic model is extended to multiple optical sensors, and the following formula can be obtained.

[0080] v(q′ OPT )=rΓ l (q KF )q′ OPT

[0081] Among them, Γ l represents the kinematic equations when the number of light sensors is l, l represents the number of light sensors, and v represents the column vector composed of the measurement values ​​of all light sensors.

[0082] By inversely solving the kinematic model, the angular velocity can be obtained as shown in the following equation. is Γ l The pseudo-inverse.

[0083]

[0084] The optical sensor records the relative position change of the detection surface in real time, obtains the displacement increments of the x and y axes, and brings the measured values ​​into the inverse model of the kinematic model to solve the Euler angular velocity, thereby realizing the angular velocity measurement of the moving end.

[0085] Multiple groups of sensor integrated circuits are arranged asymmetrically at the same radial position, and the asymmetrical arrangement ensures that the measured values ​​are not repeated. In addition, the increase in effective measured values ​​can improve the gradient of the magnetic field to the angle for magnetic measurement, thereby improving the measurement resolution of the angle. For optical measurement, the increase in the number of kinematic equations can obtain the least squares solution, which minimizes the calculation error and avoids the deviation of the results from the true value due to the distortion of the measured values ​​of individual sensors.

[0086] (3) Kalman filter fusion

[0087] Figure 6The figure shows the magnetic and optical fusion process of the ball joint measurement system. The Euler angular velocity obtained by optical measurement and the Euler angle obtained by magnetic measurement are fused through the Kalman filter. The state variable x in the Kalman filter is the angle q KF and angular velocity deviation q′ bKF The six-dimensional vector composed of the input value u is the Euler angular velocity q′ measured by the optical sensor OPT Substitute the measured value z into the angle q measured by the magnetic sensor MAG Substitution.

[0088] The representations of state variables, input values, measured values, state transition equations, and measurement equations are shown below, where the subscript k represents the time, starting from 0, and Δt represents the duration of a single measurement. The state variable x0 at the initial time is [0 0] T In addition, w represents system noise, e represents measurement noise, which needs to be calibrated by reading sensor data in the experiment, and [I]3 represents the third-order unit matrix.

[0089] u k =q′ OPTk ; z k =q MAGk

[0090]

[0091] z k (q MAGk )=[[I]30]x k +e k

[0092] The prediction equation of the Kalman filter is as follows, which mainly calculates the prior state variable estimate at time k+1 Estimation of the prior error covariance matrix The state variable x solved by the Kalman filter at time k k and the angular velocity q′ measured by the optical sensor at time k+1 OPT Calculated, The error covariance matrix P calculated by Kalman filtering at time k is k And the system noise covariance matrix calculation W at time k+1 k get.

[0093] First, calculate the result q′ of the optical and magnetic measurements at time k+1 OPT ( k+1 ) and q MAG(k+1) , then q′ OPT ( k+1 ) is substituted into the prediction equation of the Kalman filter to calculate the k+1 moment and update

[0094]

[0095] Among them, A and B represent x in the state transfer equation k with u k+1 , W represents the system noise covariance matrix, and P0 is a 6th-order unit matrix at the initial moment.

[0096]

[0097] Where W k By calculating the system noise w at time k k The covariance of is obtained and is calculated as follows.

[0098] W k = cov(w k )

[0099] The update equation of the Kalman filter is as follows, where K represents the Kalman gain matrix, which is the prior error covariance matrix at time k+1 Observation matrix H, measurement noise covariance matrix E at time k+1 k+1 Calculated. Then through K at time k+1 k+1 Update the prior state estimate and the prior error covariance estimate Get the state variable x at time k+1 k+1 and the error covariance matrix P k+1 .

[0100] Then calculate K at time k+1 k+1 ,use q MAG(k+1) , K k+1 Calculate x k+1 and P k+1

[0101]

[0102] Among them, E k By calculating the measurement noise e at time k k The covariance of is obtained and is calculated as follows.

[0103] E k = cov(e k )

[0104] Finally, take out the state variable x at time k+1 k+1 The first three terms of are the angle q between the moving end of the ball joint and the fixed end at time k+1. KF(k+1) .

[0105]

[0106] Among them, x 1(k+1) 、x 2(k+1) 、x 3(k+1) They represent the state variable x at time k+1 respectively. k+1 Items 1, 2 and 3 of .

[0107] Output angle q KF The kinematic model parameters of the optical measurement are used for the next calculation. Kalman filtering can provide the best estimate in the calculation process with system noise and measurement noise. Magnetic sensing can effectively avoid zero drift and drift errors, and optical sensing high resolution can achieve high measurement accuracy. The data measured by both are integrated through sensing to improve the stability and accuracy of the system and eliminate zero drift and drift errors.

[0108] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A ball joint multi-degree-of-freedom posture measurement system, characterized in that: The system includes a magnetic field measurement module, an optical field measurement module and a calculation module, wherein: The magnetic field measurement module measures the change in the magnetic field when the moving end of the ball joint moves relative to the fixed end; The light field measurement module measures the change of the light signal when the moving end of the ball joint moves relative to the fixed end; The calculation module is used to calculate the angle and angular velocity of the ball joint moving end relative to the fixed end at the current moment according to the magnetic field change measured by the magnetic field measurement module and the light signal change measured by the light field measurement module, and then fuse the angle and angular velocity at the current moment to obtain the angle of rotation of the ball joint moving end relative to the fixed end at the next moment.

2. A ball joint multi-degree-of-freedom posture measurement system as claimed in claim 1, characterized in that: The magnetic field measurement module comprises a permanent magnet (3) and a magnetic sensor (7); the permanent magnet (3) is arranged on the moving end of the ball joint, and the magnetic sensor (7) is arranged on the fixed end of the ball joint.

3. A ball joint multi-degree-of-freedom posture measurement system as claimed in claim 1, characterized in that: The light field measurement module comprises a detection surface (5), a light sensor (6) and a lens (9), wherein the light sensor (6) and the lens (9) are arranged at a fixed end of a ball joint, the detection surface (5) is arranged at a moving end of the ball joint, and the lens (9) is arranged between the light sensor (6) and the detection surface (5). The light emitted by the light sensor (6) passes through the lens (9) and is irradiated on the detection surface (5) to be reflected or scattered. The reflected or scattered light is converged by the lens (9) and enters the light sensor (6), thereby realizing the measurement of the light signal.

4. A ball joint multi-degree-of-freedom posture measurement system as claimed in claim 1, characterized in that: The number of the permanent magnets (3) is greater than or equal to 1, and the number of the magnetic sensors (7) is greater than or equal to 3.

5. A ball joint multi-degree-of-freedom posture measurement system as claimed in claim 1, characterized in that: The number of the detection surfaces (5), the optical sensors (6) and the lenses (9) is the same, and the number of the optical sensors (6) is greater than or equal to 2.

6. A ball joint multi-degree-of-freedom posture measurement system as claimed in claim 1, characterized in that: The straight-line distance between the detection surface (5) and the bottom of the lens (9) is 1.6 to 2.4 mm.

7. A method for measuring the multi-degree-of-freedom posture of a ball joint, characterized in that: The method comprises the following steps: S1 The moving end of the ball joint rotates relative to the fixed end, using the change of the magnetic field and the change of the optical signal in the measurement system according to any one of claims 1 to 6; S2 uses the measured magnetic field changes to calculate the angle of the moving end of the ball joint relative to the fixed end at the current moment; S3 uses the change of the light field to calculate the angular velocity of the moving end of the ball joint relative to the fixed end at the current moment; S4 uses a Kalman filter to fuse the angular velocity at the current moment calculated in steps S2 and S3 to obtain the rotation angle of the moving end of the ball joint relative to the fixed end at the next moment.

8. The method according to claim 7, characterized in that The angle is obtained by following the steps below: Constructing a data set of one-to-one correspondence between the angle of rotation of the ball joint moving end relative to the fixed end and the vector corresponding to the magnetic field measured by the magnetic sensor, wherein the angle is obtained by IMU measurement; Using the data set to train a deep learning model, the input of the deep learning model is the magnetic field measured by the magnetic sensor, and the output is the angle at which the moving end of the ball joint rotates relative to the fixed end; A magnetic sensor is used to measure the magnetic field when the moving end of the ball joint rotates relative to the fixed end to obtain the vector corresponding to the magnetic field. The vector is input into the trained deep learning model to output the real-time rotation angle of the moving end of the ball joint relative to the fixed end.

9. The method according to claim 7, characterized in that The angular velocity is calculated according to the following formula: Among them, q′ OPT represents the angular velocity of the moving end of the ball joint relative to the fixed end calculated by the light field measurement module, q KF is the angle of the moving end of the ball joint relative to the fixed end at the last moment, v represents the column vector composed of the velocity values ​​measured by all optical sensors, r represents the radius in the sensor spherical coordinate parameters, Γ l The kinematic equations when the number of light sensors is l are represented as follows: is Γ l is the pseudo-inverse of , and l represents the number of light sensors.

10. The method according to claim 7, characterized in that The rotation angle of the ball joint moving end relative to the fixed end at the next moment is calculated according to the following formula: Among them, q KF is the angle between the moving end of the ball joint and the fixed end, q′ OPT represents the angular velocity of the moving end of the ball joint relative to the fixed end calculated by the light field measurement module, q MAG It represents the angle of the moving end of the ball joint relative to the fixed end calculated by the magnetic field measurement module, x represents the state variable, x1, x2, x3 represent the 1st, 2nd, and 3rd components of the state variable x respectively, k represents the current moment, and k+1 represents the next moment.

Images