High-precision two-axis turntable system and working method

Abstract

The invention provides a high-precision two-axis rotary table system and a working method, and the system comprises a wheat wheel driving system which achieves the functions of advancing, retreating, steering, in-situ rotation and translation; the servo driving system forms an inner-loop current closed loop and an outer-loop speed closed loop to realize speed regulation; the battery system is directly powered by a lithium battery carried by the vehicle body; the safety system fully surrounds the periphery of the wheat wheel driving system and triggers digital signals to be directly transmitted to the PLC master controller, so that the sudden stop effect is achieved; the jacking system and the vertical and horizontal double shafts are provided with tilt angle sensors, vertical and horizontal angle and azimuth angle information of a star body is obtained in real time, and full-closed-loop feedback control is carried out; and the control system is used for walking control, double-shaft posture adjustment control, single-shaft servo drive control, manual operation control and man-machine interaction. The device has the height adjusting function, the direction adjusting function and the horizontal adjusting function, the part installing efficiency is improved, and meanwhile the installing precision is guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of attitude adjustment of aerospace equipment, and in particular to a high-precision two-axis turntable system and a working method. Background Art

[0002] The attitude control of aerospace equipment in space is the basis for its stable operation and mission completion. The attitude adjustment (i.e., attitude adjustment) of aerospace equipment refers to changing the attitude angle of aerospace equipment to ensure that it flies according to the predetermined orbit and attitude. During the installation of the sailboard, the aerospace equipment needs to be flipped, accurately positioned, and attitude maintained to achieve zero-stress docking and assembly of the sailboard. In terms of attitude adjustment of aerospace equipment, traditional attitude control methods may have problems such as insufficient accuracy and poor stability. Therefore, in order to improve the production capacity of aerospace equipment in factories, a more accurate, stable and reliable attitude control system is needed.

[0003] Patent application document CN111664749A discloses a high-dynamic vertical five-axis electric turntable, including a base bracket, a yaw assembly installed in the base bracket, an azimuth assembly mounted on the upper end of the yaw axis of the yaw assembly, a pitch assembly installed on the top of the yaw axis, a roll assembly installed inside the pitch assembly, and a height assembly installed on the top of the azimuth assembly. However, this patent cannot completely solve the existing technical problems, nor can it meet the needs of the present invention. Summary of the invention

[0004] In view of the defects in the prior art, an object of the present invention is to provide a high-precision two-axis turntable system and a working method.

[0005] The high-precision two-axis turntable system provided by the present invention includes: a McLennan drive system, a servo drive system, a battery system, a safety system, a jacking system and a control system; The McDonnell Douglas drive system uses a four-McDonnell Douglas configuration to achieve forward, backward, steering, in-situ rotation and translation functions; The servo drive system is a single-axis servo drive control, which forms an inner current closed loop and an outer speed closed loop to achieve fine speed regulation; The battery system is directly powered by a lithium battery carried by the vehicle body; The safety system includes: a safety touch edge that surrounds the McWheel drive system all around, triggering a digital signal that is directly transmitted to the PLC master control to achieve an emergency stop effect; In the described lifting system, both the vertical and horizontal axes are equipped with inclination sensors to obtain the vertical and horizontal angles and azimuth angle information of the star in real time and perform full closed-loop feedback control; The control system is used for walking control, dual-axis posture adjustment control, single-axis servo drive control, manual operation control and human-computer interaction.

[0006] Preferably, the McLennan driving system is installed at the four corners of the platform body and adopts an independent electric four-wheel drive mode to realize the turntable's forward and backward straight-line walking, lateral translation, two-wheel guided steering and four-wheel drive in-situ rotation functions.

[0007] Preferably, the inner current closed loop includes: using a current transformer or a Hall effect sensor embedded in the driver as a feedback element to accurately adjust the motor current; The outer speed closed loop includes: using the encoder to feedback the real-time speed information of the servo motor to accurately control the motor speed, and its output signal is used as the input of the current closed loop to achieve fine speed regulation; A speed controller generates a control signal based on the speed error signal, which is converted into a current given signal and transmitted to the current regulator. The current regulator generates a control signal based on the current error signal to adjust the current of the motor. At the same time, the actual value of the current is detected in real time and fed back to the speed controller for further adjustment in the outer speed closed loop.

[0008] Preferably, the battery system is built into the vehicle body to power the vehicle body moving equipment and the two rotating shafts at the same time.

[0009] Preferably, the jacking system adopts an integrated frame welding, forms a dual-axis posture adjustment system through the vertical and horizontal control axes and the azimuth control axes, and is equipped with high-precision sensors.

[0010] Preferably, the lifting system includes: an AC servo motor and a worm gear reducer; after the AC servo motor drives the worm gear reducer to increase the force, it drives the screw slider and the pitch platform to slide along the linear guide to achieve pitch flipping; a proximity switch is provided within the travel range of the linear guide, and when the slider approaches the switch range, a signal is sent to stop movement; a safety block is provided at the mechanical limit travel to prevent overtravel and provide electrical overload protection; the worm gear reducer is self-locking when a fault occurs during the flipping process.

[0011] Preferably, the tilt sensor is installed on the pitch platform to detect the pitch angle in real time and feed it back to the servo drive system for closed-loop control.

[0012] Preferably, the jacking system also includes: a gapless coupling, a planetary reducer and a worm gear pair; the AC servo motor drives the planetary reducer through a gapless coupling, and then drives the worm gear pair to achieve precise transmission; when the axis of the star is vertical, the azimuth turntable is in the working position, rotates within a range of 360°, and stops at any position; when the axis of the star is horizontal, the control system adjusts the speed of the azimuth motor within the speed range set by the user to ensure the stability of the azimuth speed.

[0013] Preferably, the manual operation control uses a handheld controller to achieve wireless connection with the PLC through a wireless transmitter and receiver to manually control the movement of the walking mechanism and the operation of the dual-axis posture adjustment system.

[0014] Preferably, the human-computer interaction includes: the main interface of the display screen displays the battery power, current size, wheel position status information of the mobile platform and the power information of the remote control; the touch screen interface supports manual setting of the operation mode and setting of the dual-axis attitude operation of the attitude adjustment system; the operating status and fault information of the McLennan drive system and the dual-axis attitude adjustment system are fed back in real time to the PLC operation interface, and the operation interface supports setting and adjusting the parameters of the control process.

[0015] The working method of the high-precision two-axis turntable system provided by the present invention comprises the following steps: Step 1: In the walking state, the two-axis turntable moves to the equipment that needs to be adjusted; Step 2: Enter the posture adjustment state; Step 3: The driver drives the azimuth and vertical motors to control the vertical angle and azimuth angle in real time; through the double worm gear, the self-locking function at any angle and the power-off self-locking function can be realized; Step 4: Two inclination sensors are set up in the vertical and horizontal axes to realize real-time feedback of two angles; Step 5: PLC collects sensor data to accurately calculate the rotation angle and angular velocity of the platform and control the current data of the motor; Step 6: Achieve full closed-loop dual-axis angle precision control and complete posture adjustment; In step 5, for the calculation of the rotation angle, an incremental encoder is used to output a preset number of pulses each time the motor shaft rotates. If the angle value represented by each pulse is known, the following formula is used to calculate the angle:

[0016] in, is the rotation angle, is the number of pulses accumulated from the reference point, It is the total number of pulses sent by the encoder per revolution; Angular velocity refers to the change in angle of rotation per unit time. The average angular velocity is calculated by the following formula:

[0017] in, is the angular velocity, is the angle change, is the time interval; The instantaneous angular velocity can be obtained by using a shorter time interval or the differential method, and the expression is:

[0018] The torque of the motor is proportional to the current, while the speed of the motor depends on the applied voltage. Therefore, to control the speed of the motor or maintain a preset angle, a PID controller is used to adjust the motor current. The expression is:

[0019] in, , and are the proportional, integral and differential gain coefficients respectively; is the error between the set value (expected value) and the measured value; output It is used to adjust the driving current of the motor to achieve the purpose of precise control of the motor.

[0020] Compared with the prior art, the present invention has the following beneficial effects: (1) The present invention adopts an independent suspension wheel set design, which improves the shock absorption ability, can maintain the ground contact of the wheels under complex terrain, and improve the stability and controllability of the vehicle.

[0021] (2) The present invention uses lead-acid batteries to power the vehicle body moving equipment and the two rotating shafts at the same time, without the need for an external AC power supply. The entire moving system is more flexible and has a wider range of movement. (3) Both the vertical and horizontal axes of the present invention are equipped with inclination sensors, which can obtain the vertical and horizontal angles and azimuth angle information of the star body in real time to form a closed-loop feedback system. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings: Figure 1 It is a schematic diagram of the overall structure of the high-precision two-axis turntable system of the present invention; Figure 2 It is a structural schematic diagram of the vehicle body system of the present invention; Figure 3 This is the control logic diagram of the system of the present invention; Figure numerals: control system 1, azimuth rotation system 2, human-machine operating system 3, body system 4, lifting ring design 41, battery system 42, wheat wheel drive system 43, safety system 44, motor 45, jacking system 5, vertical and horizontal flipping system 6. DETAILED DESCRIPTION

[0023] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0024] Example The present invention provides a high-precision two-axis turntable system, such as Figure 1 and Figure 2 As shown, it includes: a body system 4, a lifting system 5, a control system 1 and a human-machine operating system 3; the body system includes: a McDonnell wheel drive system 43, a battery system 42, a safety system 44 and a servo drive system 45; the McDonnell wheel drive system uses a four-McDonnell wheel group configuration, which can realize forward, backward, steering, rotation in place, translation and other movement functions; in the lifting system, the selected vertical and horizontal axes are equipped with inclination sensors, which can obtain the vertical and horizontal angles and azimuth information of the star in real time and perform full closed-loop feedback control; the battery system is directly powered by the lithium battery carried by the body and is located on the bottom side of the high-precision two-axis turntable system; the servo drive system performs single-axis servo drive control through a single-axis servo drive control system, forming an inner loop current closed loop and an outer loop speed closed loop to achieve fine speed adjustment, and the servo drive system is located on the other side of the bottom of the high-precision two-axis turntable system; the control system includes a walking control system, a dual-axis posture adjustment control system, a single-axis servo drive control system, a manual operation control and a human-computer interaction interface.

[0025] Furthermore, the McLennan drive system in the body system is used for translation and steering of parts during use, which improves the shock absorption capacity. The system is installed at the four corners of the platform body and adopts an independent electric four-wheel drive mode. It can realize the functions of the turntable's front and rear straight-line travel, lateral translation, two-wheel guided steering and four-wheel drive rotation on the spot, etc. It can maintain the ground contact of the wheels in complex terrain, improve the stability and controllability of the vehicle, and can move flexibly in narrow or crowded spaces.

[0026] Furthermore, the servo drive system in the vehicle body system includes a drive unit assembly including Mecanum wheels, a suspension mechanism, a variable frequency motor, a reducer, and other components. Each wheel set is equipped with an independent motor, and the suspension mechanism ensures that the wheels are pressed tightly against the ground. In uneven ground conditions, the four wheel sets can touch the ground at the same time.

[0027] Furthermore, the servo drive system is composed of a current control subsystem and a speed control subsystem, which respectively constitute an inner current closed loop and an outer speed closed loop. The current closed loop subsystem uses the current transformer or Hall effect sensor embedded in the driver as a feedback element to accurately adjust the motor current to improve the performance of the mechanical system. The speed closed loop subsystem uses the real-time speed information of the servo motor fed back by the encoder to accurately control the motor speed, and its output signal is used as the input of the current closed loop to achieve fine speed regulation. In this system, there is a close connection between the outer speed closed loop and the inner current closed loop. The speed controller generates a control signal according to the speed error signal, which is converted into a current given signal and transmitted to the current regulator: the current regulator generates a control signal according to the current error signal to adjust the current of the motor. At the same time, the actual value of the current is also detected in real time and fed back to the speed controller for further adjustment in the speed loop. In summary, the inner current closed loop and the outer speed closed loop work together in the dual closed loop speed regulation system to achieve precise control of the motor speed and current.

[0028] Furthermore, the battery system in the vehicle body system is built into the vehicle body, and the battery supplies power to the vehicle body moving equipment and two rotating shafts at the same time, without the need for an external AC power supply, so that the entire mobile system is more flexible and has a wider range of movement.

[0029] Furthermore, the safety system in the vehicle body system has safety touch edges all around the walking system, and the trigger digital signal is directly transmitted to the PLC main control, so that an emergency stop effect can be achieved.

[0030] Furthermore, the jacking system adopts one-piece frame welding, and after welding, the key parts (car body, inclined arm, turntable frame) are issued with magnetic particle inspection reports. In addition to static analysis, flexible body dynamics simulation is added to the design review; the dual-axis posture adjustment system consists of a vertical and horizontal control axis and an azimuth control axis, and is equipped with high-precision sensors to achieve the accuracy of parts in the azimuth rotation system.

[0031] Furthermore, the vertical and horizontal control system in the jacking system is composed of an AC servo motor, a worm gear reducer, a rolling screw, a connecting rod, a linear guide, an inclination sensor, a nitrogen spring and other parts; the system adopts a crank slider structure to realize the mechanism flipping. After the servo motor drives the worm gear reducer to increase the force, it drives the screw slider and the pitching platform to slide along the linear guide to achieve pitch flipping. The rolling screw adopts a high-load ball screw and adopts a fixed pre-stretching installation method. The nut adopts an enlarged steel ball clearance treatment to ensure high-precision movement.

[0032] Furthermore, the inclination sensor in the jacking system is installed on the pitching platform to detect the pitching angle in real time and feed it back to the drive system for closed-loop control. To ensure safety, the system is equipped with a proximity switch within the travel range of the linear guide rail. When the slider approaches the switch range, it will send a signal to stop moving. At the same time, a safety block is set at the mechanical limit travel to prevent overtravel and provide electrical overload protection.

[0033] Furthermore, the worm gear reducer in the jacking system can ensure reliable self-locking when a fault occurs during the flipping process, which is convenient for emergency handling.

[0034] Furthermore, the azimuth rotation system in the jacking system is composed of a pitching platform, an AC servo motor (with a power-off brake), a high-precision planetary reducer, a gapless coupling, a high-precision double-lead worm gear pair and shaft system components, a high-precision turntable bearing, a high-precision rotary encoder, etc. The system is installed on the pitching platform and adopts a hollow turntable structure. The AC servo motor drives the high-precision planetary reducer through a gapless high-precision elastic coupling, and then drives the worm and worm gear pair to achieve precise transmission. The turntable is coaxially fixed to the turntable through the worm gear, and the star and the turntable rotate at the same speed coaxially, and the fine-tuning function can be achieved through an electronic handwheel. When the axis of the star is vertical, the azimuth turntable is in the working position and can rotate within the range of ±360° and stop accurately at any position; when the axis of the star is horizontal, the control system adjusts the speed of the azimuth motor within the speed range set by the user to ensure the stability of the azimuth speed. The rotary encoder is installed on the worm shaft to form a semi-closed loop control system. The worm gear support adopts high-precision turntable bearings, which can bear bidirectional axial loads, radial loads and overturning moments, and has no clearance, which is suitable for indexing turntables running at low speeds. The worm support adopts a fixed-floating method to ensure high-precision transmission and stability.

[0035] Furthermore, the walking control AGV part in the control system is composed of three main parts: industrial controller, servo control system and electrical system. The industrial controller is responsible for the implementation of human-machine interaction, motion calculation of the lower computer and the acceptance of servo system information status feedback; the servo system executes the motion instructions issued by the controller and feeds back the relevant status parameters to the controller; the electrical system realizes the power supply and circuit protection of the entire system.

[0036] Furthermore, the manual operation in the control system uses a handheld controller, which is wireless and realizes wireless connection with the PLC through a wireless transmitter and a receiver to manually control the movement of the walking mechanism and the operation of the dual-axis posture adjustment system.

[0037] Furthermore, the human-machine interaction interface in the control system and the main interface of the display screen display the battery power of the mobile platform, the current size, the position status information of the wheels, the power of the remote control, etc. The touch screen interface also supports manual setting of the operation mode, setting the dual-axis posture of the posture adjustment system, and other operations. The operating status and fault information of the walking system and the dual-axis posture adjustment system are fed back in real time to the PLC operation interface. At the same time, the operation interface also supports setting and adjusting the parameters of the control process, and the control effect is accurate.

[0038] like Figure 3 , the workflow of the high-precision two-axis turntable system is: Step 1: In the walking state, the two-axis turntable moves to the equipment that needs to be adjusted; Step 2: Enter the posture adjustment state; Step 3: The driver drives the azimuth and vertical motors to control the vertical angle and azimuth in real time; at the same time, the double worm gears can realize the self-locking function at any angle and the self-locking function when the power is off; Step 4: Two inclination sensors are set up in the vertical and horizontal axes to realize real-time feedback of two angles; Step 5: PLC collects sensor data to accurately calculate the rotation angle and angular velocity of the platform and control the current data of the motor; Step 6: Achieve full closed-loop dual-axis angle precision control and complete posture adjustment.

[0039] In step 5, for the calculation of the rotation angle, assume that we have an incremental encoder that outputs a certain number of pulses each time the motor shaft rotates. If the angle value represented by each pulse is known (for example, there are N pulses in each 360-degree circle), the angle can be calculated using the following formula:

[0040] in, is the rotation angle, is the number of pulses accumulated from the reference point, It is the total number of pulses sent by the encoder per revolution.

[0041] Angular velocity refers to the change in angle of rotation per unit time. The average angular velocity can be calculated using the following formula:

[0042] in, is the angular velocity, is the angle change, is the time interval.

[0043] If the instantaneous angular velocity is needed, it can be obtained by using a shorter time interval or the differential method:

[0044] Controlling the motor current is usually done by adjusting the voltage or PWM (pulse width modulation) signal supplied to the motor. The torque of the motor is proportional to the current, while the speed of the motor depends on the applied voltage. Therefore, to control the speed of the motor or maintain a specific angle, a PID (proportional-integral-derivative) controller is used to adjust the motor current.

[0045] The output of the PID controller can be calculated by the following formula:

[0046] in, , and are the proportional, integral and differential gain coefficients respectively; is the error between the set value (expected value) and the measured value. It will be used to adjust the driving current of the motor to achieve the purpose of precise motor control.

[0047] In the description of the present application, it should be understood that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0048] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0049] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A high-precision two-axis turntable system, characterized in that: include: McLane drive system, servo drive system, battery system, safety system, lifting system and control system; The McDonnell Douglas drive system uses a four-McDonnell Douglas configuration to achieve forward, backward, steering, in-situ rotation and translation functions; The servo drive system is a single-axis servo drive control, which forms an inner current closed loop and an outer speed closed loop to achieve fine speed regulation; The battery system is directly powered by a lithium battery carried by the vehicle body; The safety system includes: a safety touch edge that surrounds the McWheel drive system all around, triggering a digital signal that is directly transmitted to the PLC master control to achieve an emergency stop effect; In the described lifting system, both the vertical and horizontal axes are equipped with inclination sensors to obtain the vertical and horizontal angles and azimuth angle information of the star in real time and perform full closed-loop feedback control; The control system is used for walking control, dual-axis posture adjustment control, single-axis servo drive control, manual operation control and human-computer interaction.

2. The high-precision two-axis turntable system according to claim 1, characterized in that: The McLennan driving system is installed at the four corners of the platform and adopts an independent electric four-wheel drive mode to realize the turntable's front and rear straight-line walking, lateral translation, two-wheel guided steering and four-wheel drive on-the-spot rotation functions.

3. The high-precision two-axis turntable system according to claim 1, characterized in that: The inner current closed loop includes: using a current transformer or a Hall effect sensor embedded in the driver as a feedback element to accurately adjust the motor current; The outer speed closed loop includes: using the encoder to feedback the real-time speed information of the servo motor to accurately control the motor speed, and its output signal is used as the input of the current closed loop to achieve fine speed regulation; A speed controller generates a control signal based on the speed error signal, which is converted into a current given signal and transmitted to the current regulator. The current regulator generates a control signal based on the current error signal to adjust the current of the motor. At the same time, the actual value of the current is detected in real time and fed back to the speed controller for further adjustment in the outer speed closed loop.

4. The high-precision two-axis turntable system according to claim 1, characterized in that: The battery system is built into the vehicle body and supplies power to the vehicle body moving equipment and two rotating shafts at the same time.

5. The high-precision two-axis turntable system according to claim 1, characterized in that: The jacking system adopts an integrated frame welding, forms a dual-axis posture adjustment system through the vertical and horizontal control axes and the azimuth control axes, and is equipped with high-precision sensors.

6. The high-precision two-axis turntable system according to claim 1, characterized in that: The lifting system comprises: an AC servo motor and a worm gear reducer; after the AC servo motor drives the worm gear reducer to increase the force, the lead screw slider and the pitching platform body are driven to slide along the linear guide rail to achieve pitching and flipping; a proximity switch is provided within the travel range of the linear guide rail, and when the slider is within the range of the proximity switch, a signal is sent to stop the movement; a safety block is provided at the mechanical limit travel to prevent overtravel and provide electrical overload protection; the worm gear reducer is self-locking when a fault occurs during the flipping process; The jacking system also includes: a gapless coupling, a planetary reducer and a worm gear pair; the AC servo motor drives the planetary reducer through the gapless coupling, and then drives the worm gear pair to achieve precise transmission; when the axis of the star is vertical, the azimuth turntable is in the working position, rotates within a range of 360°, and stops at any position; when the axis of the star is horizontal, the control system adjusts the speed of the azimuth motor within the speed range set by the user to ensure the stability of the azimuth speed.

7. The high-precision two-axis turntable system according to claim 1, characterized in that: The tilt sensor is installed on the pitch platform to detect the pitch angle in real time and feed it back to the servo drive system for closed-loop control.

8. The high-precision two-axis turntable system according to claim 5, characterized in that: The manual operation control uses a handheld controller to achieve wireless connection with the PLC through a wireless transmitter and a receiver to manually control the movement of the walking mechanism and the operation of the dual-axis posture adjustment system.

9. The high-precision two-axis turntable system according to claim 5, characterized in that: The human-computer interaction includes: the main interface of the display screen displays the battery power, current size, wheel position status information of the mobile platform and the power information of the remote control; the touch screen interface supports manual setting of the operation mode and setting of the dual-axis attitude operation of the attitude adjustment system; the operating status and fault information of the McLennan drive system and the dual-axis attitude adjustment system are fed back in real time to the PLC operation interface, and the operation interface supports setting and adjusting the parameters of the control process.

10. A working method of a high-precision two-axis turntable system according to any one of claims 1 to 9, characterized in that: The steps include: Step 1: In the walking state, the two-axis turntable moves to the equipment that needs to be adjusted; Step 2: Enter the posture adjustment state; Step 3: The driver drives the azimuth and vertical motors to control the vertical angle and azimuth angle in real time; through the double worm gear, the self-locking function at any angle and the power-off self-locking function can be realized; Step 4: Two inclination sensors are set up in the vertical and horizontal axes to realize real-time feedback of two angles; Step 5: PLC collects sensor data to accurately calculate the rotation angle and angular velocity of the platform and control the current data of the motor; Step 6: Achieve full closed-loop dual-axis angle precision control and complete posture adjustment; In step 5, for the calculation of the rotation angle, an incremental encoder is used to output a preset number of pulses each time the motor shaft rotates. If the angle value represented by each pulse is known, the following formula is used to calculate the angle: in, is the rotation angle, is the number of pulses accumulated from the reference point, It is the total number of pulses sent by the encoder per revolution; Angular velocity refers to the change in angle of rotation per unit time. The average angular velocity is calculated by the following formula: in, is the angular velocity, is the angle change, is the time interval; The instantaneous angular velocity can be obtained by using a shorter time interval or differentiation method, and the expression is: The torque of the motor is proportional to the current, while the speed of the motor depends on the applied voltage. Therefore, to control the speed of the motor or maintain a preset angle, a PID controller is used to adjust the motor current. The expression is: in, , and are the proportional, integral and differential gain coefficients respectively; is the error between the set value and the measured value; output It is used to adjust the driving current of the motor to achieve the purpose of precise control of the motor.

Images