A spherical joint multi-degree-of-freedom posture measurement system and method

Abstract

The application belongs to the technical field of sensor measurement, and discloses a spherical joint multi-degree-of-freedom attitude measurement system and method. The system comprises a magnetic field measurement module, an optical field measurement module and a calculation module, wherein: the magnetic field measurement module measures the change of the magnetic field when the moving end of the spherical joint moves relative to the fixed end; the optical field measurement module measures the change of the optical signal when the moving end of the spherical joint moves relative to the fixed end; and the calculation module is used for calculating the angle and the angular velocity of the moving end of the spherical joint relative to the fixed end at the current moment according to the change of the magnetic field measured by the magnetic field measurement module and the change of the optical signal measured by the optical field measurement module respectively, and then fusing the angle and the angular velocity at the current moment to obtain the angle of rotation of the moving end of the spherical joint relative to the fixed end at the next moment. Through the application, the advantages are complementary, and the precision and the robustness of the measurement system are improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of sensor measurement, and more particularly relates to a spherical joint multi-degree-of-freedom attitude measurement system and method. BACKGROUND

[0002] The spherical joint integrated with multiple rotational degrees of freedom can greatly reduce the complexity of the transmission structure, and the industrial application scenarios are increasing day by day. In the motion control of the spherical joint, the high precision and real-time requirement of the spherical joint multi-degree-of-freedom attitude feedback is the basis for the application and control of the spherical joint, but the coupling effect of multiple rotational degrees of freedom increases the difficulty of attitude measurement.

[0003] The existing spherical joint multi-degree-of-freedom attitude measurement generally adopts contact measurement and non-contact measurement using a single sensor. The spherical joint contact attitude measurement usually selects a combination of three-axis accelerometer, three-axis gyroscope and three-axis geomagnetic meter to solve the attitude, but this scheme can only output the angle of the motion end relative to the inertial system and cannot output the angle of the motion end relative to the fixed end, and the contact with the motion end will introduce external disturbance to affect the control effect, at the same time, the accelerometer is easy to be disturbed by high frequency, the gyroscope has integral drift error, and the geomagnetic meter cannot be applied to strong magnetic environment, which leads to great limitations in the use of this scheme; for non-contact measurement, the research mainly uses a single type of sensor, which can output the angle of the motion end relative to the fixed end and will not have external disturbance, but the single type of sensor measurement has integral drift error and is difficult to measure for a long time, has no zero position and can only start calculating from the zero position of the attitude, or has large noise and low measurement accuracy. SUMMARY

[0004] In view of the above defects or improvement needs of the prior art, the present application provides a spherical joint multi-degree-of-freedom attitude measurement system and method, which solves the problems of external disturbance introduced by contact measurement, inability to output the relative angle of the motion end, and large noise, no zero position and drift error of non-contact measurement.

[0005] To achieve the above-mentioned purpose, according to one aspect of the present application, a spherical joint multi-degree-of-freedom attitude measurement system is provided, characterized in that the system comprises a magnetic field measurement module, an optical field measurement module and a calculation module, wherein:

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

[0007] The optical field measurement module measures the change of the optical signal when the motion end of the spherical joint moves relative to the fixed end;

[0008] The computing module is used for calculating the angle and the angular velocity of the motion of the motion end of the spherical joint relative to the fixed end at the current time according to the magnetic field change measured by the magnetic field measuring module and the light signal change measured by the light field measuring module respectively, and then fusing the angle and the angular velocity at the current time to obtain the angle of the rotation of the motion end of the spherical joint relative to the fixed end at the next time.

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

[0010] Further preferably, the light field measuring module comprises a detection surface, a light sensor and a lens, the light sensor and the lens are arranged on the fixed end of the spherical joint, the detection surface is arranged on the motion end of the spherical joint, the lens is arranged between the light sensor and the detection surface, the light sensor emits light to irradiate on the detection surface through the lens to be reflected or scattered, and the reflected or scattered light is converged into the light sensor by the lens, so as to realize the measurement of the light signal.

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

[0012] Further preferably, the number of the detection surfaces, the light sensors and the lenses is the same, and the number of the light 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 application, a method for measuring the posture of a spherical joint with multiple degrees of freedom is provided, which comprises the following steps:

[0015] S1, rotating the motion end of the spherical joint relative to the fixed end, and using the change of the magnetic field and the change of the light signal in the above-mentioned measuring system;

[0016] S2, calculating the angle of the motion end of the spherical joint relative to the fixed end at the current time by using the measured change of the magnetic field;

[0017] S3, calculating the angular velocity of the motion end of the spherical joint relative to the fixed end at the current time by using the change of the light field;

[0018] S4, fusing the angular velocity at the current time obtained by the steps S2 and S3 by using a Kalman filter to obtain the angle of the rotation of the motion end of the spherical joint relative to the fixed end at the next time.

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

[0020] construct a data set corresponding to the one-to-one correspondence between the angle of the rotating end of the spherical joint 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] train a deep learning model using the data set, wherein the input of the deep learning model is the magnetic field measured by the magnetic sensor, and the output is the angle of the rotating end of the spherical joint relative to the fixed end;

[0022] obtain the vector corresponding to the magnetic field measured by the magnetic sensor when the rotating end of the spherical joint rotates relative to the fixed end, input the vector into the trained deep learning model, and output the real-time angle of the rotating end of the spherical joint relative to the fixed end.

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

[0024]

[0025] wherein q' OPT represents the angular velocity of the rotating end of the spherical joint relative to the fixed end calculated by the light field measurement module, q KF is the angle of the rotating end of the spherical joint relative to the fixed end at the previous time, v represents a column vector composed of the speed values measured by all light sensors, r represents the radius in the spherical coordinate parameter of the sensor, and Γ l represents the kinematic equation group when the number of light sensors is l, is the pseudo-inverse of Γ l , and l represents the number of light sensors.

[0026] Further preferably, the angle of the rotating end of the spherical joint relative to the fixed end at the next time is calculated according to the following formula:

[0027]

[0028] wherein q KF is the angle of the rotating end of the spherical joint relative to the fixed end at the previous time, x represents the state variable, x1, x2, and x3 represent the 1st, 2nd, and 3rd components of the state variable x, respectively, k represents the current time, and k+1 represents the next time.

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

[0030] 1. This invention obtains the angle and angular velocity of the moving end of the ball joint relative to the fixed end at the current moment from two angles: an optical field measurement module and a magnetic field measurement module, respectively, using optical signals and magnetic field signals. Then, the two are fused 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, while optical sensing achieves high measurement accuracy and low noise due to its high resolution and tracking speed. Optimizing the arrangement 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 this invention can directly and simultaneously output the angle between the moving end and the fixed end of a ball joint multi-degree-of-freedom motion system, and it is non-contact. Compared with traditional measurement modules such as encoders and IMUs, it solves the problems of needing to contact measurement to introduce external disturbances (for encoders and IMUs), needing to distribute encoders to measure multi-axis rotation resulting in low integration (for encoders), only being able to output the angle between the moving system and the inertial system (for IMUs), and being unable to use a magnetometer to calibrate the yaw angle in a strong magnetic environment (for IMUs). It has strong versatility.

[0032] 3. This invention employs a Kalman filter to fuse the angle and angular velocity of the moving end of the ball joint relative to the fixed end at the current moment. This fusion algorithm complements the advantages of non-contact and contact measurement methods. Magnetic measurement provides the angle, eliminating the problems of zero-point position and drift error. Optical measurement, due to the very high optical resolution of optical sensors, can obtain high-precision angular velocity, compensating for the high noise of magnetic sensors. A suitable fusion algorithm can achieve measurement results with zero-point position, no drift error, and low noise. The Kalman filter is a statistical recursive data processing algorithm for data fusion. Its goal is to minimize noise error in the estimation process of minimizing the error covariance. It can adjust the scaling factor in real time based on the noise to calculate the optimal estimate of the result, providing a smoother and more accurate tracking signal. In addition, any measurement method that has a mapping relationship with attitude can be used as a data source for attitude calculation Kalman filtering, making its application range wider than Mahony filtering and Madgwick filtering. Attached Figure Description

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

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

[0035] Figure 3 This is a front structural 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 the optical measurement module constructed according to the preferred embodiment of the present application;

[0037] Figure 5 is a side view of the optical measurement module constructed according to the preferred embodiment of the present application;

[0038] Figure 6 is a schematic diagram of the principle of the spherical joint multi-degree-of-freedom attitude measurement method constructed according to the preferred embodiment of the present application.

[0039] In all the drawings, the same reference signs are used to represent the same elements or structures, wherein:

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

[0041] In order to make the objects, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0042] The optical sensing and magnetic sensing scheme in non-contact measurement can achieve high-precision measurement at a lower cost. Considering the shortcomings of single-sensing measurement scheme and the adjustment and processing of redundant data, multi-sensing fusion is introduced, and better pose measurement is achieved through multi-sensing integration and fusion technology. The present application designs a low-cost, easy-to-install, multi-sensing integrated non-contact spherical joint multi-degree-of-freedom attitude 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 moving end of the spherical joint moves relative to the fixed end; the optical field measurement module measures the change of the optical signal when the moving end of the spherical joint moves relative to the fixed end; and the calculation module is used to calculate the angle and angular velocity of the moving end of the spherical joint relative to the fixed end according to the change of the magnetic field measured by the magnetic field measurement module and the change of the optical signal measured by the optical field measurement module, respectively.

[0044] The magnetic field measurement module comprises a permanent magnet and a magnetic sensor, the permanent magnet is arranged on the spherical joint moving end, and the magnetic sensor is arranged on the spherical joint fixed end. 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 same permanent magnet detected by the magnetic sensors at different positions are different; in the case of multiple permanent magnets, the magnetic fields of the multiple permanent magnets can be vector superimposed in the same magnetic sensor coordinate system to obtain a magnetic field, and the magnetic field can be regarded as a whole to calculate, therefore, the number of permanent magnets is required to be greater than or equal to 1. The angle has three components, and at least three equations of magnetic field values and angles need to be established to solve the angle, therefore, three or more magnetic sensors are required. In addition, increasing the number of magnetic sensors 7 and reasonably arranging the positions of the 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 with the spherical joint moving end, and the size of the magnetic field generated by all the permanent magnets 3 at the magnetic sensor 7 in any rotating condition is required to be not more than the measurement range of the magnetic sensor 7, and there is no shape requirement. The relative positions of the permanent magnet 3 and the magnetic sensor 7 are close, and the close distance can reduce the interference of external magnetic fields and increase the gradient of the magnetic field and the angle, and the increased gradient is beneficial to reduce the error of deep learning.

[0046] 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 on the spherical joint fixed end 1, the detection surface 5 is arranged on the spherical joint moving end 2, 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, and the reflected or scattered light is captured by the lens 9 into the light sensor 6, so as to realize the measurement of the light signal.

[0047] The number of the detection surface 5, the light sensor 6 and the lens 9 is the same, and the number of the light sensor 6 is greater than or equal to 2. The detection surface 5 is made of non-transparent material, and has a certain texture or roughness on the surface, and the specific material has no requirement, and materials such as metal, resin, nylon and leather can be selected. The thickness and size of the detection surface 5 are determined according to the actual movement requirement of the spherical joint, the thickness requirement is that the detection surface 5 does not interfere with the movement of the spherical joint and does not affect the movement range, and the size requirement is that the light sensor can irradiate the light beam on the detection surface 5 in any rotating condition, 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.4mm, the distance is too small, which will affect the accuracy of the light signal received by the photosensitive element of the light sensor 6, and the distance is too far, which will have external light captured by the photosensitive element through the lens 9 and affect the data accuracy.

[0049] In the two embodiments of the present application, in one embodiment, as shown in Figure 1As shown, the moving end of the ball joint wraps around the fixed end, the detection surface 5 is fixed to the inner surface of the moving end, and the light sensor 6 and lens 9 are arranged facing outwards towards the detection surface 5. In another embodiment, as... Figure 2 As shown, the fixed end of the ball joint wraps around the moving end, the detection surface 5 is fixed to the outer surface of the moving end, and the optical sensor 6 and lens 9 are arranged facing inwards towards 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, such as Figures 3-5 As shown, the magnetic sensor 7 and the optical sensor 6 are all mounted on the sensing circuit board 8. The magnetic sensor 7 can be positioned anywhere on the sensing circuit board 8. There is no requirement for the number of sensing circuit boards 8; multiple optical and magnetic sensors can be arranged in one sensing circuit, or there can be only one sensing circuit board. The position of the sensing circuit board 8 is not fixed and is determined according to the design of the optical detection surface. The sensing circuit board 8 has a rectangular slot in its center. The optical sensor 6 has holes at both ends, and the lens 9 is assembled according to these holes. These two are complementary devices 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 illuminates the detection surface 5, producing reflection and scattering. The photosensitive element in the center of the optical sensor captures the reflected and scattered infrared light that has passed through the lens 9. The internal processor converts the received light signal into planar displacement. The infrared light beam needs to be emitted from and received by the optical sensor 6; therefore, the sensing circuit board 8 needs a rectangular slot in its center. The size of the slot depends on the specific design of the optical sensor 6. In addition, the light sensor 6 is a pin-type component with the pins facing downwards, so the sensing circuit board 8 needs to be located between the light 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. Each optical sensor 6 must be assembled with a lens 9. A total of four sensing 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 4 based on the processor performance and the number of interfaces. In this embodiment, the processor can connect to a maximum of 16 magnetic sensors 7 at the same time. To reduce design and production costs, all sensing circuits are designed to be consistent, with 4 magnetic sensors 7 arranged on each group of sensing circuits. The positions of the 4 magnetic sensors 7 are determined based on layout optimization.

[0052] Figure 3 This is a flowchart of the magnetic and optical fusion measurement process for a ball joint multi-degree-of-freedom measurement system. The diagram mainly illustrates the measurement methods for various sensors and the calculation methods for sensor fusion.

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

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

[0055] Under the condition that the geometric center of the permanent magnet is the source point in the 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 (Biot-Savart law).

[0056]

[0057] Wherein, q MAG represents the angle of the spherical joint movement end 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, and μ0 represents the vacuum permeability, represents the gradient, M represents the polarization intensity of the permanent magnet, and P represents the point to be integrated on the permanent magnet, MAG P represents the position vector of the point, n represents the surface normal vector of the permanent magnet, R1 represents the rotation matrix of the fixed coordinate system to the moving coordinate system, and q MAG is related, represents the volume integral, represents the area integral.

[0058] The permanent magnet is fixedly connected with the moving coordinate system, and the magnetic sensor is fixedly connected with the fixed coordinate system. Therefore, 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] Wherein, MAG B represents the magnetic sensor measurement value, i.e. the magnetic field excited by the permanent magnet at the sensor in the magnetic sensor coordinate system, R2 represents the rotation matrix of the fixed coordinate system to the magnetic sensor coordinate system, and the magnetic sensor is arranged at the fixed end of the spherical joint, so R2 is a constant value, represents the rotation matrix of the moving coordinate system to the fixed coordinate system, which is the transpose of R1 and is related to q MAG , R3 represents the rotation matrix of the permanent magnet coordinate system to the moving coordinate system, and the permanent magnet is arranged at the moving end of the spherical joint, so R3 is a constant value.

[0061] When the moving end of the spherical joint produces rotational motion, R1 will be updated constantly, and then MAG the value of B will also be updated. The measurement value of the magnetic sensor is calculated by the angle using the forward calculation model.

[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 represented in the magnetic sensor coordinate system, so the magnetic field values excited by different permanent magnets can be directly added in value, as shown in the following formula. Wherein, PM represents the permanent magnet, PMi represents the i-th permanent magnet, the total number is n, and R 3iRi(q) represents the rotation matrix of the ith permanent magnet coordinate system to the moving coordinate system.

[0063]

[0064] The magnetic field values of the permanent magnet at different postures may be the same at the same magnetic sensor, so multiple magnetic sensors need to be set, that is, the measured values of all magnetic sensors are not completely the same at any posture of the permanent magnet, as shown in the following formula. Wherein, MAG represents a magnetic sensor, j represents the number of magnetic sensors, and B represents a 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, the posture has a one-to-one mapping relationship with the magnetic sensor measurement column vector B, that is, there is a bijective relationship, as shown in the second formula in the above formula. The explicit inverse expression cannot be derived from the three-degree-of-freedom forward magnetic field model, but deep learning can very well fit the numerical relationship between the magnetic sensor measurement value B and the angle q MAG The numerical relationship can be replaced by an inverse model using a deep learning model, as shown in the following formula, so that the angle of the spherical joint moving end relative to the fixed end can be calculated by measuring the magnetic field at multiple magnetic sensor positions.

[0067] q MAG =BPnet(B)

[0068] Wherein, BPnet represents a deep learning model.

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

[0070] The optical measurement scheme is used to measure the angular velocity. The optical sensor, lens, and detection surface constitute the hardware part of the optical measurement. The optical sensor is fixed with the fixed end of the spherical joint and is used to emit an infrared light beam and receive a light signal and output x, y axis displacement; the lens is used to guide the emission of the infrared light beam and concentrate the reflected light to the photosensitive element for collection; the detection surface is fixed with the moving end of the spherical joint and has a certain texture on the surface. The optical sensor outputs displacement information by collecting the texture change of the moving detection surface.

[0071] The angular velocity of the spherical joint moving end 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] Where, q′ OPT q′ represents the angular velocity of the moving end of the ball joint relative to the fixed end, calculated by the light field measurement module. OPT1 ,q′ OPT2 ,q′ OPT3 These represent the angular velocities q′ and q′ at the moving ends of the ball joint, respectively. OPT The three components 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 between the moving end of the ball joint and the fixed end at the previous moment. (The xyz Euler angle order is used as an example here, and the Euler angle order can be modified as needed.)

[0074] When a point that is not at the center of a sphere rotates around the center, the point will generate a tangential velocity perpendicular to the radius. This tangential velocity can be calculated using the angle and angular velocity information. s v, as shown in the following formula. s v represents the tangential velocity of the moving end of the ball joint in a fixed coordinate system. OPT P represents the position vector of the optical sensor, and × represents the cross product of the vectors.

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

[0076] The tangential velocity of a point exists only in the x and y directions, with a zero velocity component in the z direction. This three-dimensional velocity can be transferred to a two-dimensional planar coordinate system, thus establishing a kinematic model of the three-dimensional rotational angular velocity and the two-dimensional planar motion velocity. Using the optical sensor coordinate system as the planar coordinate system, the following equation can be obtained.

[0077]

[0078] in, OPT v represents the measurement value from the optical sensor, that is, the velocity of the ball joint moving end in the optical sensor coordinate system. OPT v x , OPT v y Let x and y represent the x and y axis components of the optical sensor measurement, respectively, and θ represent the polar angle in the spherical coordinate parameters of the optical sensor. θ represents the azimuth angle in the spherical coordinate parameters of the optical sensor, r represents the radius in the spherical coordinate parameters of the sensor, and θ represents the radius in the spherical coordinate parameters of the sensor. r are constant values, Γ is a forward kinematics equation group, and q is a rotation angle KF The display analysis expression is shown in the following formula, R4 represents a rotation matrix of a fixed coordinate system to a light sensor coordinate system, and the light sensor is arranged at a fixed end of the spherical joint, and thus R4 is a constant value.

[0079] When the moving end of the spherical joint rotates, the detection surface information detected by the light sensor changes, and thus the displacement increments of the x and y axes of the moving end surface in the plane coordinate system of the light sensor are output, the displacement increments are considered as velocities in a unit time, and a kinematics model of the surface displacement increments and the angular velocities of the Euler angles is established, that is, the v expression mentioned above. OPT The light sensor detects the plane movement and only outputs the displacement increments of the x and y axes of the detection surface, and the solving of the three rotation angles requires that the number of the light sensors is greater than or equal to 2, and the inverse solution of the kinematics model has a unique solution. The kinematics model of the light sensor is extended to multiple light sensors, and the following formula can be obtained.

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

[0081] wherein Γ l represents a kinematics equation group when the number of the light sensors is l, l represents the number of the light sensors, and v represents a column vector composed of the measurement values of all the light sensors.

[0082] The angular velocity can be obtained through the inverse solution of the kinematics model, as shown in the following formula. Wherein, is a pseudo-inverse of Γ l .

[0083]

[0084] The light 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 measurement values into the inverse solution model of the kinematics model to solve the Euler angular velocity, so as to realize the measurement of the angular velocity of the moving end.

[0085] Multiple groups of sensing integrated circuits are arranged asymmetrically at the same radius position, and the asymmetric arrangement ensures that the measurement values are not repeated. In addition, the increase of the effective measurement values can improve the gradient of the magnetic field to the angle for the magnetic measurement, thereby improving the measurement resolution of the angle, and for the optical measurement, the least square solution can be obtained due to the increase of the number of the kinematics equations, so that the calculation error is minimized, and the deviation of the result from the true value caused by the distortion of the measurement value of the individual sensor is avoided.

[0086] (3) Fusion of Kalman filtering

[0087] Figure 6The shown is the process chart of the magnetic and optical fusion of the spherical joint measurement system. The Euler angle velocity obtained by optical measurement and the Euler angle obtained by magnetic measurement are fused by Kalman filter. The state variable x in Kalman filter is a six-dimensional vector composed of angle q KF and angle velocity deviation q′ bKF The input value u is the Euler angle velocity q′ OPT measured by optical sensor, and the measurement value z is the angle q MAG measured by magnetic sensor.

[0088] The state variable, the input value, the measurement value, the state transition equation and the measurement equation are shown as follows, respectively, wherein the subscript k represents the time, starting from 0, and Δt represents the length of single measurement. The state variable x0 at the initial time is [0 0] T . In addition, w represents system noise, e represents measurement noise, and needs to be calibrated by reading sensor data in experiments, and [I]3 represents a 3-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 Kalman filter is shown as follows, which mainly calculates the prior state variable estimation and the prior error covariance matrix estimation at k+1 time, by the state variable x k calculated by Kalman filter at k time and the angle velocity q′ OPT measured by optical sensor at k+1 time. The error covariance matrix P k calculated by Kalman filter at k time and the system noise covariance matrix W k calculated at k+1 time are obtained.

[0093] Firstly, the results of optical measurement and magnetic measurement q′ OPT ( k+1 ) and q MAG(k+1) at k+1 time are calculated, and then q′ OPT ( k+1 ) is substituted into the prediction equation of Kalman filter to calculate the k+1 time and update

[0094]

[0095] Where A and B represent x in the state transition equation k with u k+1 The parameter matrix is ​​given by W, which represents the system noise covariance matrix. At the initial time, P0 is a 6th-order identity matrix.

[0096]

[0097] Among them W k Calculate the system noise w at time k k The covariance is obtained and calculated as follows.

[0098] W k =cov(w k )

[0099] The update equation for the Kalman filter is shown below, where K represents the Kalman gain matrix, derived from the prior error covariance matrix at time k+1. Observation matrix H, measurement noise covariance matrix E at time k+1 k+1 The calculation is obtained. Then, K is obtained at time k+1. k+1 Update prior state estimates and prior error covariance estimation Obtain the state variable x at time k+1 k+1 With the error covariance matrix P k+1 .

[0100] Next, 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 The noise e is measured at time k. k The covariance is obtained and calculated as follows.

[0103] E k =cov(e k )

[0104] Finally, extract the state variable x at time k+1. k+1 The first three terms, namely the angle q between the moving end and the fixed end of the ball joint at time k+1, are the values ​​to be found. KF(k+1) .

[0105]

[0106] wherein x 1(k+1) , x 2(k+1) , x 3(k+1) respectively represent the 1st, 2nd, 3rd term of the state variable x k+1 at k+1 time.

[0107] The output angle q KF is taken as the kinematics model parameter of the optical measurement for the next calculation. Kalman filtering can provide optimal estimation in the calculation process with system noise and measurement noise, the magnetic sensor can effectively avoid zero drift and drift error, and the optical sensor high resolution can obtain high measurement accuracy. The measured data of the two are fused to improve the system stability and accuracy, and to eliminate the zero drift and drift error.

[0108] Those skilled in the art will easily understand that the above description is only the preferred embodiment of the present application, and is not intended to limit the present application, and any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A ball-joint multi-degree-of-freedom attitude 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 in 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 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, respectively. Then, the angle and angular velocity at the current moment are fused to obtain the angle of rotation of the ball joint moving end relative to the fixed end at the next moment.

2. The ball joint multi-degree-of-freedom attitude measurement system as described in claim 1, characterized in that, The magnetic field measurement module includes a permanent magnet (3) and a magnetic sensor (7). The permanent magnet (3) is disposed on the moving end of the ball joint, and the magnetic sensor (7) is disposed on the fixed end of the ball joint.

3. The ball joint multi-degree-of-freedom attitude measurement system as described in claim 1, characterized in that, The light field measurement module includes a detection surface (5), a light sensor (6), and a lens (9). The light sensor (6) and the lens (9) are located at the fixed end of the ball joint, the detection surface (5) is located at the moving end of the ball joint, and the lens (9) is located between the light sensor (6) and the detection surface (5). The light emitted by the light sensor (6) is reflected or scattered by the lens (9) onto the detection surface (5). The reflected or scattered light is focused by the lens (9) into the light sensor (6), thereby realizing the measurement of the light signal.

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

5. The ball joint multi-degree-of-freedom attitude measurement system as described in claim 3, characterized in that, 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.

6. The ball joint multi-degree-of-freedom attitude measurement system as described in claim 3, 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 attitude of a ball joint with multiple degrees of freedom, characterized in that, The method includes the following steps: S1 The moving end of the ball joint rotates relative to the fixed end, utilizing the changes in the magnetic field and the changes in the optical signal in the measurement system according to any one of claims 1-6; S2 calculates the angle between the moving end of the ball joint and the fixed end at the current moment using the measured change in the magnetic field; S3 calculates the angular velocity of the moving end of the ball joint relative to the fixed end at the current moment using the change in the light field; S4 uses a Kalman filter to fuse the current angle obtained in step S2 and the current angular velocity calculated in step S3 to obtain the angle of rotation of the ball joint moving end relative to the fixed end at the next moment.

8. The method as described in claim 7, characterized in that, The angle is obtained by the following steps: A dataset is constructed that corresponds one-to-one with the vectors of the angle of rotation of the ball joint's moving end relative to the fixed end and the magnetic field measured by the magnetic sensor, wherein the angle is obtained by IMU measurement; A deep learning model is trained using the dataset. The input of the deep learning model is the magnetic field measured by the magnetic sensor, and the output is the angle of rotation of the moving end of the ball joint 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, and the corresponding vector of the magnetic field is obtained. This vector is then input into the trained deep learning model to output the real-time angle of rotation of the moving end of the ball joint relative to the fixed end.

9. The method as described in claim 7, characterized in that, The angular velocity is calculated according to the following formula: in, q represents the angular velocity of the moving end of the ball joint relative to the fixed end, calculated by the light field measurement module. KF It is the angle between the moving end of the ball joint and the fixed end at the previous moment, and v represents the column vector composed of the velocity values ​​measured by all optical sensors. r Represents the radius in the sensor's spherical coordinate parameters. Indicates the number of optical sensors is l The kinematic equations of time, yes The false rebellion, l This indicates the number of optical sensors.

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

Images