Universal joint device with two-degree-of-freedom angular displacement feedback and snake-shaped arm robot

Abstract

The invention relates to a universal joint device with two-degree-of-freedom angular displacement feedback and a snake-shaped arm robot. The universal joint device mainly comprises a transmission and sensing integrated structure and an angle collecting and resolving module. The transmission and sensing integrated structure mainly comprises a cross shaft, a bearing seat and an angular displacement detection assembly, two ends of an X shaft and a Y shaft of the cross shaft are respectively matched with the bearing seat through bearings to realize two-degree-of-freedom rotation, one end of the X shaft and one end of the Y shaft are respectively connected with a magnet shaft, a Hall detection module is fixed with the bearing seat, and the magnet shafts rotate relative to the Hall detection module during rotation. And the Hall device outputs electric signals to change. The angle collecting and resolving module comprises a signal collecting circuit based on a single-chip microcomputer and corresponding software, the signal collecting circuit conducts AD collection on Hall electric signals, and two-degree-of-freedom angular displacement information is resolved through signal collecting software, self-adaptive filtering software, angle resolving software and error compensation software.

Description

Technical Field

[0001] The present invention relates to the field of sensor measurement technology, and in particular to a universal joint device with two-degree-of-freedom angular displacement feedback and a snake-arm robot. Background Art

[0002] With the development of robotics and the increasing maturity of control technology, today's intelligent manufacturing and robotics-related fields are no longer satisfied with the measurement of single physical quantities or one-dimensional physical quantities. The application of multi-degree-of-freedom motion is gradually increasing, especially in the production and manufacturing of large precision parts and their subsequent use and maintenance, which has higher requirements for the precise realization of multi-degree-of-freedom motion.

[0003] In the field of continuum robots, robots often have more redundant degrees of freedom. For example, in the design of snake-arm robots, universal joint structures are often used as connecting components at the robot joints. Through the universal joint structure, two joints can achieve two-degree-of-freedom rotation. The control of the position and posture of the continuum robot requires the control of the angles between all joints. However, due to the lack of angle measurement methods, semi-closed-loop control based on kinematic solution is generally used, and accurate closed-loop control cannot be achieved, resulting in the current low motion accuracy of such robots. Accurate and stable measurement of joint posture information is the basis for realizing closed-loop control of robot joint posture, but due to the mutual coupling of the two degrees of freedom of the universal joint, it is more complicated to measure its two angles. There is currently no method to directly measure the two-degree-of-freedom angular displacement information of the universal joint. Although there are many types of angular displacement sensors, it is difficult to install and apply them on snake-like continuum robots with universal joint structures. The present invention is based on the design concept of integrated transmission and sensing. While the appearance and volume remain basically unchanged, the angle sensing structure is integrated into the universal joint structure, which can realize mechanical transmission and two-degree-of-freedom angle measurement at the same time. It can be used for the measurement of two-degree-of-freedom angular displacement between joints of such serpentine continuum robotic arms based on the universal joint structure, and plays an important role in improving the position and posture control accuracy of the robot. Summary of the invention

[0004] The first object of the present invention is to provide a universal joint device with two-degree-of-freedom angular displacement feedback, so that it can provide more accurate two-dimensional angle measurement for posture feedback.

[0005] The second object of the present invention is to provide a snake-arm robot comprising the above-mentioned universal joint device.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A universal joint device, characterized in that it includes a transmission sensor integrated structure and an angle acquisition and resolution module, wherein the transmission sensor integrated structure includes:

[0008] A cross axis, the cross axis comprising a pair of ends coaxially arranged along the X axis and a pair of ends coaxially arranged along the Y axis;

[0009] Bearing seats, two bearing seats are respectively located on both sides of the cross axis along the Z axis of the cross axis, one of the bearing seats is rotatably connected to a pair of end heads coaxially arranged along the X axis, and the other bearing seat is rotatably connected to a pair of end heads coaxially arranged along the Y axis;

[0010] An angular displacement detection component, the angular displacement detection component includes a magnet shaft and a Hall detection module, the Hall detection module is fixedly arranged on the bearing seat, and the Hall detection module is located on the axis of the end of the cross shaft, a permanent magnet is arranged on the magnet shaft, and the magnet shaft and the end of the cross shaft are coaxially fixed to one end of the end of the cross shaft close to the Hall detection module;

[0011] The angle acquisition and solution modules include:

[0012] The signal acquisition circuit based on the single chip microcomputer includes a main control chip and a signal collector, and the Hall detection module is connected to the main control chip through the signal collector;

[0013] Angle acquisition and solution software, including signal acquisition, improved first-order digital filtering, angle solution and nonlinear error compensation algorithm, solves the two-degree-of-freedom angular displacement information based on the collected Hall electrical signal.

[0014] In a preferred embodiment of the present invention, the angular displacement detection assembly also includes a mounting bracket, the mounting bracket is fixed to the bearing seat, a mounting groove is provided on a surface of the mounting bracket facing away from the bearing seat, the Hall detection module is arranged in the mounting groove, and a accommodating hole is provided on the mounting bracket corresponding to the position of the Hall detection module, and the magnet shaft is gap-matched with the accommodating hole.

[0015] In a preferred embodiment of the present invention, the first surface of the mounting bracket is in contact with the outer surface of the bearing seat, the second surface of the mounting bracket is provided with the mounting groove, and the first surface and the second surface are arranged opposite to each other.

[0016] In a preferred embodiment of the present invention, the accommodating hole penetrates the mounting bracket along a direction from the first surface to the second surface.

[0017] In a preferred embodiment of the present invention, the Hall detection module includes a PCB circuit board and a Hall element arranged on the PCB circuit board, the shape of the PCB circuit board is adapted to the shape of the notch of the mounting slot, the PCB circuit board is detachably connected to the mounting bracket, and the PCB circuit board closes the notch of the mounting slot to form a space for accommodating the Hall element, and the Hall element is arranged relative to the magnet axis in the accommodating hole.

[0018] In a preferred embodiment of the present invention, the permanent magnet is a radial magnet, an end surface of one end of the cross shaft facing the Hall detection module is provided with an embedding groove, and the radial magnet is embedded in the embedding groove.

[0019] In a preferred embodiment of the present invention, the magnet shaft and the end of the cross shaft are fixedly connected by friction fit and / or profile fit to avoid relative rotation between the magnet shaft and the end of the cross shaft.

[0020] In a preferred embodiment of the present invention, one of the magnet shaft and the end of the cross shaft is provided with a slot, and the other of the magnet shaft and the end of the cross shaft is provided with a plug, and the plug is interference-fitted with the slot.

[0021] In a preferred embodiment of the present invention, the cross-sections of the plug and the slot are polygonal, elliptical, semicircular or irregular.

[0022] A serpentine arm robot comprises a multi-degree-of-freedom serpentine continuum robotic arm, wherein the multi-degree-of-freedom serpentine continuum robotic arm is composed of a plurality of joints connected in sequence using a universal joint device as described in any one of the above items, each joint is driven by a driving steel cable to achieve two-degree-of-freedom swing, and the two-degree-of-freedom relative angular displacement between the joints can be directly measured using the universal joint device.

[0023] It can be seen from the above technical scheme that the present invention discloses a universal joint device, including a transmission sensor integrated structure and an angle acquisition and resolution module, wherein the transmission sensor integrated structure includes a cross shaft, a bearing seat and an angular displacement detection component, the cross shaft includes a pair of end heads coaxially arranged along the X axis and a pair of end heads coaxially arranged along the Y axis; two bearing seats are respectively located on both sides of the cross shaft along the Z axis of the cross shaft, one of the bearing seats is rotatably connected to a pair of end heads coaxially arranged along the X axis, and the other bearing seat is rotatably connected to a pair of end heads coaxially arranged along the Y axis; the angular displacement detection component includes a magnet shaft and a Hall detection module, and the Hall detection module is fixedly arranged The invention relates to a method for realizing an angle acquisition and solving method of the present invention, wherein the angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft. The angle acquisition and solving method is a method for solving the angle of the cross shaft and the angle of the cross shaft.

[0024] The angular displacement detection component of the universal joint device is composed of a magnet shaft and a Hall detection module. During operation, when the cross shaft and the bearing seat rotate relative to each other, the magnet shaft on the cross shaft and the Hall detection module on the bearing seat rotate relative to each other. The Hall detection module can obtain the corresponding rotation angle by detecting the rotating magnetic field generated by the rotation of the magnet shaft. This detection method is not affected by the use environment, has a simple and reliable structure, a small size, low power consumption, and is easy to install and use. As long as there is relative rotation between the magnet shaft and the Hall detection module, detection can be performed, the response is timely, and it has good linearity. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0026] Figure 1 An exploded view of a universal joint device provided by an embodiment of the present invention;

[0027] Figure 2 A partial enlarged schematic diagram of the assembly of the magnet shaft and the cross shaft of the universal joint device provided by an embodiment of the present invention;

[0028] Figure 3A partial enlarged schematic diagram of the assembly location of the magnet shaft and the mounting bracket of the universal joint device provided by an embodiment of the present invention;

[0029] Figure 4 This is a schematic diagram comparing the effects before and after filtering;

[0030] Figure 5 This is a schematic diagram of the relative error after alpha angle compensation;

[0031] Figure 6 This is a schematic diagram of the relative error after beta angle compensation;

[0032] Figure 7 A schematic structural diagram of a snake-arm robot provided in an embodiment of the present invention.

[0033] In the figure:

[0034] 1 is a universal joint device; 101 is a cross axis; 1011 is an end; 1011a is a pair of ends coaxially arranged along the X axis; 1011b is a pair of ends coaxially arranged along the Y axis; 102 is a bearing seat; 103 is a bearing; 104 is an elastic retaining ring for the shaft; 105 is a magnet shaft; 106 is a Hall detection module; 107 is a mounting bracket; 108 is a radial magnet; 109 is a plug; 110 is a slot; 2 is a multi-degree-of-freedom serpentine continuum robot arm; 3 is a joint element; 4 is a drive cable; 5 is an end operating tool; 6 is a drive box. DETAILED DESCRIPTION

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

[0036] One of the cores of the present invention is to provide a universal joint device, the structural design of which enables it to provide relatively accurate multi-degree-of-freedom angle measurement for posture feedback, and has low requirements on the use environment, simple and reliable structure, high response frequency, and can be used for long-term measurement.

[0037] Another core of the present invention is to provide a snake-arm robot including the above-mentioned universal joint device.

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

[0039] See also Figure 1 , Figure 1 An exploded view of a universal joint device provided in an embodiment of the present invention.

[0040] The embodiment of the present invention discloses a universal joint device 1, which can be used for transmission between joints of a serpentine robot arm and angular displacement measurement, and can also be used as a two-degree-of-freedom angular displacement sensor. The universal joint device 1 includes a transmission sensing integrated structure and an angle acquisition and resolution module.

[0041] Among them, the transmission sensor integrated structure includes a cross shaft 101, a bearing seat 102 and an angular displacement detection component. The cross shaft 101 includes a pair of ends 1011a coaxially arranged along the X-axis and a pair of ends 1011b coaxially arranged along the Y-axis, that is, the cross shaft 101 has a total of 4 ends 1011.

[0042] The two bearing seats 102 are respectively located on both sides of the cross shaft 101 along the Z axis of the cross shaft 101, one of the bearing seats 102 is rotatably connected to a pair of end heads 1011a coaxially arranged along the X axis, and the other bearing seat 102 is rotatably connected to a pair of end heads 1011b coaxially arranged along the Y axis. Figure 1 As shown, the bearing seat 102 is a U-shaped structure as a whole. The bearing seat 102 includes a main body and two arms symmetrically arranged on the main body. A space for accommodating the cross shaft 101 is formed between the two arms, and coaxial bearing mounting holes are arranged on the two arms. In order to avoid mutual interference between the two bearing seats 102, the end of the arm of the bearing seat 102 away from the main body adopts a rounded corner design, and a bearing 103 is arranged in the bearing mounting hole. The two ends 1011 of the cross shaft 101 on the same axis are rotatably mounted in the bearing mounting holes of the two arms of the bearing seat 102 through the bearings 103, respectively. Figure 2 As shown, an elastic retaining ring 104 for the shaft is also provided in the bearing mounting hole, so as to axially limit the bearing 103 and the cross shaft 101 to prevent axial movement thereof.

[0043] like Figures 1 to 3As shown, the angular displacement detection component includes a magnet shaft 105 and a Hall detection module 106. The Hall detection module 106 is fixedly arranged on the bearing seat 102, that is, the Hall detection module 106 rotates with the bearing seat 102 relative to the cross shaft 101, and the Hall detection module 106 is located on the axis of the end 1011 of the cross shaft 101. A permanent magnet is arranged on the magnet shaft 105, and the magnet shaft 105 and the end 1011 of the cross shaft 101 are coaxially fixed at one end of the end 1011 of the cross shaft 101 close to the Hall detection module 106. The magnet shaft 105 rotates with the cross shaft 101 relative to the bearing seat 102, so that when the cross shaft 101 and the bearing seat 102 rotate relative to each other, the magnet shaft 105 and the Hall detection module 106 also rotate relative to each other synchronously, generating a magnetic field change, thereby realizing angle detection.

[0044] The angle acquisition and solution module includes a signal acquisition circuit based on a single-chip microcomputer and angle acquisition and solution software. The signal acquisition circuit includes a main control chip and a signal collector. The Hall detection module is connected to the main control chip through the signal collector. The angle acquisition and solution software includes signal acquisition, improved first-order digital filtering, angle solution and nonlinear error compensation algorithm. The two-degree-of-freedom angular displacement information is calculated based on the collected Hall electrical signal.

[0045] In the embodiment of the present invention, STM32F103C8T6 is selected as the main control chip, and the signal collector is a 12-bit AD collector.

[0046] Compared with the prior art, the angular displacement detection component of the universal joint device 1 provided in the embodiment of the present invention is composed of a magnet shaft 105 and a Hall detection module 106. During operation, when the cross shaft 101 and the bearing seat 102 rotate relative to each other, the magnet shaft 105 on the cross shaft 101 and the Hall detection module 106 on the bearing seat 102 rotate relative to each other. The Hall detection module 106 can obtain the corresponding rotation angle by detecting the rotating magnetic field generated by the rotation of the magnet shaft 105. This detection method is not affected by the use environment, has a simple and reliable structure, a small size, low power consumption, and is easy to install and use. As long as there is relative rotation between the magnet shaft 105 and the Hall detection module 106, detection can be performed, the response is timely, and it has good linearity.

[0047] like Figure 1As shown, to facilitate the installation of the angular displacement detection component, in an embodiment of the present invention, the angular displacement detection component also includes a mounting bracket 107, and the mounting bracket 107 is fixed to the bearing seat 102. A mounting groove is provided on the side surface of the mounting bracket 107 facing away from the bearing seat 102, and the Hall detection module 106 is arranged in the mounting groove. Since the magnet shaft 105 and the cross shaft 101 will protrude from the outer surface of the bearing seat 102 after installation, the mounting bracket 107 is provided with an accommodating hole corresponding to the position of the Hall detection module 106, and the magnet shaft 105 is gap-matched with the accommodating hole to cover the magnet shaft 105 inside to prevent the magnet shaft 105 from being subjected to external resistance and affecting the rotation of the magnet shaft 105.

[0048] from Figure 1 It can be seen that the mounting bracket 107 is mounted on the bearing seat 102 by three threaded fasteners. Correspondingly, three threaded holes are provided on the bearing seat 102, and the connecting lines of the axes of the three threaded holes are arranged in a triangle, wherein two threaded fasteners pass through the mounting bracket 107 and cooperate with the threaded holes, and the other threaded fastener passes through the Hall detection module 106 and the mounting bracket 107 and cooperates with the threaded holes, thereby fixing the Hall detection module 106 to the mounting bracket 107 and fixing the mounting bracket 107 to the bearing seat 102 at the same time, so that the mounting bracket 107 can be positioned at three points to ensure the stability of the mounting bracket 107.

[0049] like Figure 1 As shown, in order to prevent the mounting bracket 107 from affecting the relative rotation between the two bearing seats 102, in the embodiment of the present invention, the outer shape of the mounting bracket 107 is the same or approximately the same as the outer shape of the arm of the bearing seat 102. Specifically, in a specific embodiment of the present invention, the first surface of the mounting bracket 107 is in contact with the outer surface of the bearing seat 102, the second surface of the mounting bracket 107 is provided with a mounting groove, the first surface and the second surface are arranged opposite to each other, and in order to facilitate cooperation with the threaded fastener, the second surface is a plane.

[0050] Further optimize the above technical solution, such as Figure 1 and Figure 2 As shown, the receiving hole passes through the mounting bracket 107 from the first surface to the second surface, so that after installation, there is no obstruction between the permanent magnet of the magnet shaft 105 and the Hall detection module 106 to avoid affecting the detection result.

[0051] like Figure 1As shown, in an embodiment of the present invention, the Hall detection module 106 includes a PCB circuit board and a Hall element arranged on the PCB circuit board. The shape of the PCB circuit board is adapted to the shape of the notch of the mounting groove. The PCB circuit board is detachably connected to the mounting bracket 107, and the PCB circuit board closes the notch of the mounting groove to form a space for accommodating the Hall element. The Hall element is arranged relative to the magnet axis 105 in the accommodating hole.

[0052] A limiting groove is provided at a position corresponding to the installation groove and the accommodating hole, and the limiting groove is used to adapt to the above-mentioned Hall element to facilitate the positioning and installation of the Hall detection module 106.

[0053] like Figure 2 As shown, in one embodiment of the present invention, the permanent magnet is a radial magnet 108, and an embedding groove is provided on the end face of one end of the end 1011 of the cross shaft 101 facing the Hall detection module 106, and the radial magnet 108 is embedded in the embedding groove so that the surface of the radial magnet 108 facing the Hall detection module 106 is flush with the end face of the end 1011 of the cross shaft 101 facing the Hall detection module 106.

[0054] To further optimize the above technical solution, the magnet shaft 105 and the end 1011 of the cross shaft 101 can be relatively fixed in a variety of ways. For example, the magnet shaft 105 and the end 1011 of the cross shaft 101 are fixedly connected by friction fit and / or surface fit to avoid relative rotation between the magnet shaft 105 and the end 1011 of the cross shaft 101. Of course, it should be noted that the matching method between the magnet shaft 105 and the end 1011 of the cross shaft 101 is not limited to the above two methods. The magnet shaft 105 and the end 1011 of the cross shaft 101 can also be connected by a threaded connection.

[0055] Specifically, Figure 2 As shown, one of the magnet shaft 105 and the end 1011 of the cross shaft 101 is provided with a slot 110 , and the other of the magnet shaft 105 and the end 1011 of the cross shaft 101 is provided with a plug 109 , and the plug 109 is interference-fitted with the slot 110 .

[0056] Furthermore, the cross-section of the plug 109 and the slot 110 is polygonal, elliptical, semicircular or irregular, that is, the cross-section of the plug 109 and the slot 110 is non-circular, such as Figure 2 As shown, in a specific embodiment of the present invention, the cross-sections of the plug 109 and the slot 110 are both D-shaped, so that after the two are matched, the plug 109 and the slot 110 will not rotate relative to each other, thereby ensuring the relative fixation of the magnet axis 105 and the end 1011 of the cross axis 101.

[0057] In a specific embodiment of the present invention, the circuit structure of the Hall detection module 106 is mainly divided into three parts: a Hall circuit part, a signal acquisition and processing circuit part, and a data display part.

[0058] The Hall circuit part is composed of two two-dimensional Hall elements, and its power supply voltage is 3.3V or 5V, which can be directly powered by the microcontroller. The main function of this part is the conversion of physical quantities. It converts the physical quantity of magnetic field intensity into the physical quantity of electrical signals through the Hall effect, and then transmits the electrical signals to the subsequent signal acquisition part.

[0059] The signal acquisition and processing part mainly uses STM32F103C8T6 as the main control chip, and collects the analog signal output by the sensor through the ADC peripheral module of the STM32 F103C8T6 main control chip. The collected data is filtered through the digital filtering algorithm, and the sensor error is compensated through the error compensation algorithm.

[0060] The data display part can be divided into OLED display and host computer display. The collected and processed data can be displayed on a 0.96-inch OLED screen or uploaded to the host computer for subsequent processing through a TTL to USB circuit module.

[0061] The present invention uses Hall elements to detect the two-degree-of-freedom angle of the universal joint device 1, so two two-dimensional Hall elements are selected. Considering that the design area of ​​the PCB circuit board is small, the number of power lines and signal lines should be reduced as much as possible, and AS5600 is selected as the magnetic induction Hall element. The use method of this Hall element is relatively simple. It only needs to place the radially magnetized circular magnet parallel to the surface of the Hall element. The calculation process can be performed by the calculation unit integrated inside the Hall element through the table lookup instruction to obtain the angle value, and the user only needs to read the angle data through the analog port or IIC interface.

[0062] The components of the Hall circuit are Hall element AS5600, 1uF capacitor and 100nF capacitor. The present invention adopts 3.3V input mode. The analog signal output by the Hall circuit needs to be converted into digital quantity through AD acquisition and sent to the STM32F103C8T6 main control chip for processing. The software part is mainly written based on the standard library of the stm32f1 series microcontroller, mainly including the test system initialization program and signal processing program.

[0063] The system initialization procedure mainly includes GPIO initialization and ADC initialization.

[0064] The signal acquisition and processing circuit part is mainly composed of zero-crossing point marking program and digital filtering program.

[0065] The zero-crossing marking program is mainly used to prevent signal mutations when crossing the zero point. When the angle continues to rotate from 360°, the angle output voltage will suddenly drop to 0. To avoid this phenomenon, angle compensation needs to be performed at the software level.

[0066] The content of the angle compensation program is: Assuming that the initial angle value measured by the Hall element AS5600 in any initial state is angle_0 (angle_0 is between 0° and 360°), and the angle measured in this cycle is angle_1, then the angle change value is angle_1 minus angle_0. The next angle measurement value is recorded as angle_2. If the zero point is passed during the forward rotation, the zero point flag K is increased by 1 to record the number of forward zero point passing times.

[0067] The change in angle relative to angle_0 is:

[0068] det_angle=(K-1)*360+(360-angle_0+angle_2)

[0069] Similarly, when the reverse rotation passes through the zero point, let K decrease by 1. The angle change compared to angle_0 is:

[0070] det_angle=-(360+(angle_0-angle_2)-360*(K+1))

[0071] The filtering program is mainly used to filter out clutter signals in the signal. The present invention uses an improved first-order filtering algorithm to perform digital filtering.

[0072] The transfer function of the first-order filter system is G(S)=1 / (T s +1), where T = 1 / ω c ,ω c is the cut-off angular frequency.

[0073] The transfer function is discretized to obtain the following expression:

[0074] y(n)=ω c T s X(n) / (1+ω c T s )+y(n-1) / (1+ω c T s )

[0075] If ω c T s / (1+ω c T s )=a, then the above formula can be changed to:

[0076] y(n)=ax(n)+(1-a)y(n-1)

[0077] The filtering effect of this filtering algorithm is related to the coefficient a. The closer this coefficient is to 1, the higher the filtering sensitivity is but the worse the filtering stability is. On the contrary, if this coefficient is closer to 0, the filtering sensitivity is lower but the stability is better. In order to solve the problem that the response speed and stability of the first-order low-pass filtering system cannot be achieved at the same time, this invention proposes a first-order low-pass filtering algorithm with adaptive filtering coefficients based on the idea of ​​adaptive filtering algorithm. This algorithm can achieve a relative balance between stability and response speed. The algorithm idea is to linearly adjust the value of the filtering coefficient according to the rate of change of the input signal. For example Figure 4 As shown, Figure 4 This is a schematic diagram comparing the effects before and after filtering.

[0078] The algorithm takes into account both response speed and stability. When the data changes little or does not fluctuate, the filter output structure is close to a constant straight line. When the data changes rapidly, the system can still respond quickly and the adjustment time is short.

[0079] The angle calculation module is based on the calibration experiment of the two-degree-of-freedom angular displacement measurement process. Generally, linear fitting is used to obtain the ideal input-output curve of the sensor. In order to further improve the angle measurement accuracy, the present invention proposes an error compensation method based on polynomial fitting to perform nonlinear error compensation.

[0080] The present invention uses the principle that a polynomial function can approximate any continuous function in a closed interval with any accuracy, and uses the least squares method to establish a fitting polynomial equation. i ,y i (i=1,2,3…n) construct an m-order polynomial to approximate, the constructed polynomial function is as follows:

[0081] y m (x) = a0 + a1x + a2x 2 +......+a m x m (1)

[0082] In order to accurately represent the relationship between input and output, the least squares method is adopted, that is, the sum of squared deviations is minimized, that is, the following formula takes the minimum value.

[0083]

[0084] Take 7 pairs of a i The partial derivative of , set the partial derivative equal to 0 to solve the best ai value. Then substitute it into formula 6 to get the fitting function.

[0085] The sensor output obtained from the experiment is used as the independent variable of the fitting function, and the ideal angle is used as the dependent variable of the fitting function. The data obtained from the experiment at certain sample points are fitted with a polynomial to obtain the polynomial shown in Formula 6. In subsequent measurements, each sensor output is substituted into the fitting function to obtain the actual angle being measured.

[0086] After programming the fitting polynomial into the analog quantity acquisition program, the collected sensor output data is substituted in to calculate the nonlinearity of the Hall element alpha angle after linear compensation. It can be seen from the angle relative error data that the relative error of the system after linear calibration is also well controlled. The compensation measures for the other angle Hall element beta can be linearly calibrated using the compensation method introduced above, and the nonlinearity of the Hall element beta angle after linear compensation can be calculated. It can be seen from the relative error curve after compensation that after taking compensation measures, the relative errors of the measurements of the two angles are less than 0.1%, such as Figure 5 and Figure 6 shown.

[0087] like Figure 7 As shown, an embodiment of the present invention also provides an example of a serpentine arm robot solution with two degrees of freedom joint angular displacement feedback between joints. The serpentine arm robot includes a multi-degree-of-freedom serpentine continuum robot arm 2, an end operating tool 5 and a drive box 6. The multi-degree-of-freedom serpentine continuum robot arm 2 includes a plurality of joint units 3, each of which is sequentially connected by a universal joint device 1 as described in the above embodiment. The drive motor in the drive box 6 pulls the drive cable 4 according to program instructions to achieve relative rotation between each joint element 3, thereby obtaining the desired robot arm posture. Since the multi-degree-of-freedom serpentine continuum robot arm 2 of the serpentine arm robot adopts the universal joint device 1 in the above embodiment, the two-degree-of-freedom angular displacement of the relative rotation between each adjacent joint element 3 of the serpentine arm robot can obtain real-time feedback, thereby realizing closed-loop control of the entire serpentine arm posture. For the technical effect of angular displacement measurement, please refer to the above embodiment.

[0088] It should be noted that the various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0089] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, the present invention can also be improved and modified, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A universal joint device, characterized in that: It includes a transmission sensor integrated structure and an angle acquisition and solution module, wherein the transmission sensor integrated structure includes: A cross axis, the cross axis comprising a pair of ends coaxially arranged along the X axis and a pair of ends coaxially arranged along the Y axis; Bearing seats, two bearing seats are respectively located on both sides of the cross axis along the Z axis of the cross axis, one of the bearing seats is rotatably connected to a pair of end heads coaxially arranged along the X axis, and the other bearing seat is rotatably connected to a pair of end heads coaxially arranged along the Y axis; An angular displacement detection component, the angular displacement detection component includes a magnet shaft and a Hall detection module, the Hall detection module is fixedly arranged on the bearing seat, and the Hall detection module is located on the axis of the end of the cross shaft, a permanent magnet is arranged on the magnet shaft, and the magnet shaft and the end of the cross shaft are coaxially fixed to one end of the end of the cross shaft close to the Hall detection module; The angle acquisition and solution modules include: The signal acquisition circuit based on the single chip microcomputer includes a main control chip and a signal collector, and the Hall detection module is connected to the main control chip through the signal collector; Angle acquisition and solution software, including signal acquisition, improved first-order digital filtering, angle solution and nonlinear error compensation algorithm, solves the two-degree-of-freedom angular displacement information based on the collected Hall electrical signal.

2. The universal joint device according to claim 1, characterized in that: The angular displacement detection assembly also includes a mounting bracket, which is fixed to the bearing seat. A mounting groove is provided on a surface of the mounting bracket facing away from the bearing seat. The Hall detection module is arranged in the mounting groove. The mounting bracket is provided with a receiving hole corresponding to the position of the Hall detection module, and the magnet shaft is gap-matched with the receiving hole.

3. The universal joint device according to claim 2, characterized in that: The first surface of the mounting bracket is in contact with the outer surface of the bearing seat, the second surface of the mounting bracket is provided with the mounting groove, and the first surface and the second surface are arranged opposite to each other.

4. The universal joint device according to claim 3, characterized in that: The accommodating hole penetrates the mounting bracket along a direction from the first surface to the second surface.

5. The universal joint device according to claim 4, characterized in that: The Hall detection module includes a PCB circuit board and a Hall element arranged on the PCB circuit board. The shape of the PCB circuit board is adapted to the shape of the notch of the mounting slot. The PCB circuit board is detachably connected to the mounting bracket, and the PCB circuit board closes the notch of the mounting slot to form a space for accommodating the Hall element. The Hall element is arranged opposite to the magnet axis in the accommodating hole.

6. The universal joint device according to any one of claims 1 to 5, characterized in that: The permanent magnet is a radial magnet, and an embedding groove is provided on the end surface of one end of the cross shaft facing the Hall detection module, and the radial magnet is embedded in the embedding groove.

7. The universal joint device according to any one of claims 1 to 5, characterized in that: The magnet shaft and the end of the cross shaft are fixedly connected by friction fit and / or profile fit to prevent the magnet shaft and the end of the cross shaft from rotating relative to each other.

8. The universal joint device according to claim 7, characterized in that: One of the magnet shaft and the end of the cross shaft is provided with a slot, and the other of the magnet shaft and the end of the cross shaft is provided with a plug, and the plug is interference-fitted with the slot.

9. The universal joint device according to claim 8, characterized in that: The cross-sections of the plug and the slot are polygonal, elliptical, semicircular or irregular.

10. A snake-arm robot, comprising a multi-degree-of-freedom snake-shaped continuum robot arm, characterized in that: The multi-degree-of-freedom serpentine continuum robotic arm is composed of a plurality of universal joint devices as described in any one of claims 1 to 9 connected in sequence.

Images