Radar electromechanical servo control system and method

Abstract

The invention discloses a radar electromechanical servo control system, which comprises a PLC (Programmable Logic Controller), a servo driver and at least two radars, and is characterized in that each radar is provided with a servo motor and an encoder; the servo driver is in communication connection with the servo motor, and the PLC is in communication connection with the servo driver through CANOpen; wherein the servo driver is used for receiving a control signal from the PLC controller and driving the servo motor to enable or brake; and the PLC is also in serial port communication connection with the encoder so as to receive a feedback signal of the encoder and judge whether the system is abnormal or not, and when the system is abnormal, the state of the system is set as a fault, and the servo motors of all radars are closed to enable and brake. Correspondingly, the invention further discloses a radar electromechanical servo control method which is implemented based on the radar electromechanical servo control system. Therefore, the system and the method designed by the invention can greatly improve the safety and the reliability of radar operation.

Description

Technical Field

[0001] The present invention relates to the technical field of mechatronics, and in particular to a radar mechatronics servo control system and method. Background Art

[0002] In the current existing technology, in the radar field, the electromechanical equipment such as programmable logic controllers (PLCs), servo drives and corresponding external sensors used in it still have poor performance in core performance and system stability.

[0003] Inadequate performance of radar products and poor system stability may lead to unpredictable accidents involving personnel and equipment. At the same time, for radar electromechanical servo systems, multiple large-mass radars are moved to corresponding positions at high speed and with high precision. Accidents in this process may cause great damage, and safety design of the movement strategy is required to ensure that the impact is within an allowable range.

[0004] Therefore, there is an urgent need to design a radar electromechanical servo control system and system that takes safety into consideration to ensure that the radar electromechanical servo can work smoothly and without failure, and avoid personnel and equipment accidents. Summary of the invention

[0005] The technical problem to be solved by the present invention is: the present invention discloses a radar electromechanical servo control system and system to solve the problems of poor safety, poor stability and easy accidental failure caused by insufficient performance of existing radar electromechanical equipment.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is: a radar electromechanical servo control system, characterized in that it includes: a PLC controller, a servo driver and at least two radars, each of which is provided with a servo motor and an encoder; the servo driver is communicatively connected with the servo motor, and the PLC controller and the servo driver are communicatively connected using CANOpen; wherein the servo driver receives a control signal from the PLC controller and drives the servo motor to enable or brake; The PLC controller also establishes a serial communication connection with the encoder to receive the feedback signal of the encoder and determine whether there is an abnormality in the system. When the system is abnormal, the system state is set to fault, and the servo motors of all radars are enabled and braked.

[0007] In the above technical scheme of the present invention, the problems of poor safety, poor stability and easy occurrence of unexpected failures caused by insufficient performance of existing radar electromechanical equipment are addressed. The present invention discloses a new radar electromechanical servo control system, in which the PLC controller can establish a serial communication connection with the encoder to receive the feedback signal of the encoder set on the servo motor of each radar, so as to analyze the movement status of each radar and judge whether the system is abnormal. When the system is abnormal, the system state is set to fault, the servo motor of all radars is turned off and braked, so as to minimize the possible safety accidents and the equipment losses caused, and improve the safety and stability of radar electromechanical equipment, which has good promotion prospects and application value.

[0008] Furthermore, in the radar electromechanical servo control system described in the present invention, the servo motor is enabled before the servo motor is released, and the servo motor is braked before the servo motor is enabled; wherein, there is a preset CANOpen communication cycle between the servo motor enabling and the servo motor releasing, or the servo motor braking and the servo motor enabling.

[0009] In the above technical solution of the present invention, in actual application, since the servo motor is switched between different operating states based on the PLC controller sending a control signal to the servo driver, the above preset CANOpen communication cycle can be specifically controlled to be 5 CANOpen communication cycles, thereby reserving sufficient time for the PLC controller to receive the feedback signal of the encoder and enable the backcheck process, thereby ensuring safe control of the servo motor.

[0010] Furthermore, in the radar electromechanical servo control system of the present invention, the sampling frequency of the encoder by the PLC controller is at least twice higher than the program running frequency of the PLC controller.

[0011] Further, in the radar electromechanical servo control system of the present invention, the PLC controller obtains the feedback signal of the encoder and obtains the error signal, so as to calculate the speed setting value given to the servo driver according to the error signal, and uses the speed filter to filter the speed setting value to output the filtered speed value, and sends the control signal to the servo driver based on the filtered speed value; Among them, the speed filtering formula of the speed filter is:

[0012] in, The speed setpoint is calculated by the PID algorithm of the PLC controller according to the error signal, The filtered speed value output by the speed filter at the current moment, The filtered speed value output by the speed filter at the next moment, a To set the acceleration, T s It is the program running cycle of the PLC controller.

[0013] In the above technical solution of the present invention, the PLC controller in the radar electromechanical servo control system needs to calculate the output of the control signal of the servo driver according to the feedback signal of the encoder; wherein, when actually controlling the movement of the radar, in order to avoid the impact caused by excessive acceleration of the radar and affect the safety and stability of the equipment, a speed filter is also provided in the PLC controller of the radar electromechanical servo control system, so as to limit the acceleration based on the speed filtering formula of the above speed filter to ensure that the acceleration is not too large, and thereby send a control signal to the servo driver based on the filtered speed value obtained after filtering, so as to control the radar to perform normal movement and ensure smooth acceleration and deceleration movement of large-mass radar equipment.

[0014] Further, in the radar electromechanical servo control system of the present invention, when at least two of the radars move in coordination, a synchronization lock is enabled, and the servo motors of all the radars are allocated to: a master motor and at least one slave motor; The slave motor generates an additional control quantity according to the displacement difference calculated by the encoder corresponding to the master motor and the encoder corresponding to the slave motor to compensate for its speed, and the formula is:

[0015] in, is the speed value of the slave motor after compensation; is the slave motor speed value before compensation; v c is the speed compensation value; The displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; x set The preset displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; x l , x u They are the upper and lower bounds of the absolute value of the displacement difference for enabling velocity compensation control.

[0016] Furthermore, in the radar electromechanical servo control system described in the present invention, the system abnormality includes: at least one of: encoder reading abnormality, error signal abnormality, micro switch signal abnormality, servo drive reporting abnormality, collision warning, PLC controller abnormality and coordinated motion abnormality.

[0017] Furthermore, in the radar electromechanical servo control system of the present invention, the error signal abnormality includes: The PLC controller obtains the feedback signal of the encoder and calculates the error value of the error signal to determine according to the following formula:

[0018] in, is the error value calculated by the PLC controller in the current frame, is the error value calculated by the PLC controller in the next frame, v max To preset the maximum speed, T c It is the operation cycle of the PLC controller program; If the error value of the error signal does not satisfy the above formula, it can be determined that a spike is generated in the PLC controller and the error signal is judged to be abnormal.

[0019] Furthermore, in the radar electromechanical servo control system of the present invention, it also includes: the collision warning is specifically: The collision judgment formula for any two radar arrays is as follows:

[0020] In the formula, V i , V j The space occupied by two radar arrays; , Space reserved for two radar collision brakes.

[0021] Furthermore, in the radar electromechanical servo control system described in the present invention, it also includes a display screen, which is communicatively connected to the PLC controller, and the operation interface of the display screen only includes three command issuing methods: state switching, emergency stop, and alarm recovery; wherein, state switching requires clicking on the target state and confirming it twice, and state switching is not allowed during the motion process.

[0022] Taking into account the radar electromechanical servo system has many actuators, if an operational error occurs, it may cause equipment damage; therefore, in order to further optimize the safety and reliability of the radar electromechanical servo control system, a fool-proof design is further carried out for the operator. The fool-proof design is also called error proofreading. The purpose of the design is to reduce or eliminate human errors through various means and methods, so that the operator does not need to spend attention, experience and professional knowledge, and can complete the operation accurately based on intuition.

[0023] Therefore, in the above technical solution of the present invention, a display screen for the operator is also provided in the radar electromechanical servo control system, and an operation interface is provided in the display screen, and the operation interface only includes three command issuing methods: state switching, emergency stop, and alarm recovery; wherein, state switching requires clicking on the target state and confirming it twice, and state switching is not allowed during the movement process to avoid possible manual errors.

[0024] Accordingly, another object of the present invention is to disclose a radar electromechanical servo control method, which can be easily applied to the above-mentioned radar electromechanical servo control system of the present invention to improve the safety and stability of the radar electromechanical servo control system, and specifically comprises the steps of: S1: The PLC controller outputs a control signal to the servo driver, so as to use the servo driver to control the radar servo motor to enable; S2: The PLC controller receives the feedback signal from the encoder and determines whether the system is abnormal; if it is determined to be abnormal, execute step S3; S3: When the system is abnormal, the system status is set to fault, the servo motors of all radars are enabled and braked, and the fault code is recorded.

[0025] The beneficial effect of the present invention is that the radar electromechanical servo control system and system designed by the present invention can effectively solve the operational abnormality problem caused by the insufficient product core performance and system stability of the radar electromechanical system in the prior art. In practical application, the radar motor servo control method and system designed by the present invention can further improve the safety of the electromechanical system during operation by implementing an optimized safety control strategy, so as to execute corresponding safety protection measures during normal operation or when the system has an abnormal condition, thereby ensuring the safety and reliability of the system during normal operation, and being able to terminate the task in time when the software or machinery has an abnormality, which has good promotion prospects and application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 The figure is a flow chart of the steps of the radar electromechanical servo control method according to one embodiment of the present invention.

[0027] Figure 2 It is a block diagram of an abnormal fault problem existing in the radar electromechanical servo control system described in the present invention in one implementation manner. DETAILED DESCRIPTION

[0028] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0029] Research has found that current radar products have problems such as poor safety, poor stability, and prone to unexpected failures due to insufficient core performance and system stability. Figure 1 as well as Figure 2 As shown, the present invention discloses a new radar electromechanical servo control method and system to solve the above technical problems and add a new safety servo control design to the radar electromechanical system.

[0030] See also Figure 1 , Figure 2 As shown, the present invention discloses a radar electromechanical servo control system, which specifically includes: a PLC controller (programmable logic controller), a servo driver and at least two radars, each of which is provided with a servo motor and an encoder. The encoder is specifically arranged on the servo motor of the corresponding radar, and the encoder is a sensor, and the main function of the encoder is to convert mechanical motion (such as rotation or linear motion) into an electrical signal output for measurement, control or data transmission.

[0031] It should be pointed out that, in the present invention, the above-mentioned PLC controller is communicatively connected with the servo drive and the encoder respectively; wherein, the PLC controller and the encoder can be specifically communicatively connected in the form of synchronous serial communication; at the same time, the PLC controller and the servo drive can be specifically communicatively connected using the CANOpen communication protocol; CANopen is a high-level communication protocol based on the CAN (Controller Area Network) bus, which is a standardized protocol.

[0032] In the radar electromechanical servo control system of the present invention, the above-mentioned servo driver can be specifically connected to the servo motor on the radar for communication. The servo driver can receive the control signal from the PLC controller and drive the servo motor on the radar to enable or brake, thereby driving the radar to move or stop.

[0033] In the present invention, since the encoder is arranged on the servo motor of the corresponding radar, the encoder can generate a feedback signal in the form of an electrical signal of the mechanical movement of the corresponding radar and send it to the PLC controller, so that the PLC controller can receive the feedback signal of the encoder and determine whether there is an abnormality in the system; and, see Figure 1 As shown, when it is determined that there is an abnormality in the system, the PLC controller can set the system status to a fault, and turn off the servo motor enable and brake of all radars.

[0034] In the actual application of the radar electromechanical servo control system designed by the present invention, in order to improve the safety of the system during operation and further improve the stability of the system, it can also specifically implement the following safety control measures to ensure the normal operation of the system: In the radar electromechanical servo control system of the present invention, controlling the servo motor on the radar can meet the following requirements, namely: the servo motor is enabled before the servo motor is released, and the servo motor is braked before the servo motor is turned off; wherein, there is a preset CANOpen communication cycle between the servo motor enabling and the servo motor releasing, or the servo motor braking and the servo motor turning off.

[0035] In actual application, since the servo motor is switched to different operating states based on the control signal sent by the PLC controller to the servo driver, the above-mentioned preset CANOpen communication cycle can be specifically controlled to be 5 CANOpen communication cycles, thereby reserving sufficient time for the PLC controller to receive the feedback signal of the encoder and enable backchecking, thereby ensuring safe control of the servo motor.

[0036] In addition, in the actual application of the radar electromechanical servo control system, when it is in the shutdown process, it is necessary to wait for the speed of the servo motor on the radar to drop to zero before controlling the servo motor to brake, to prevent the brake from rushing and damage to the brake. In addition, the servo motors of each radar are kept in the brake state when the power is off, to prevent the radar from being displaced by natural environmental factors after the power is off.

[0037] Accordingly, in the present invention, in order to speed up the enabling backcheck process of the above-mentioned PLC controller and avoid safety problems caused by too slow backcheck speed, the sampling frequency of the feedback signal sent by the encoder on the radar by the PLC controller can be further set to be at least twice higher than the program running frequency of the PLC controller according to the Shannon sampling theorem, so that the PLC controller can calculate the error signal for closed-loop control based on the received feedback signal. The above-mentioned setting form is specifically expressed by the following formula:

[0038] in, is the program running frequency of the PLC controller, Represents the sampling frequency of the input quantity that the PLC controller has a strong correlation with its output, that is, the sampling frequency of the feedback signal sent by the encoder.

[0039] In addition, when the radar electromechanical servo control system designed by the present invention is normally operated, the reason why the PLC controller and the servo driver are controlled to communicate using the CANOpen communication protocol is that: the CANOpen communication data method can use PDO (Process Data Object) for efficient and real-time data transmission. It should be noted that a high CAN bus load rate will lead to data loss and transmission delay, and its calculation formula is as follows:

[0040]

[0041] in, F Represents the number of bits per message; B filler Represents the padding bit, To round down; B format It is the format bit, and its specific value is 47; B data Data bit. For a PDO message, its value is a multiple of 8 and does not exceed 64. For a 0x80 synchronization frame, its value is 0. is the bus load rate; T can is the bus cycle; S is the baud rate; F i Representative i The number of bits occupied by the message, there are a total of n The above calculation ignores the device heartbeat whose cycle is longer than the CAN bus cycle.

[0042] Therefore, in order to avoid the CAN bus load rate being too high, generally speaking, the CAN bus load rate should not exceed 50%, and can be controlled at 20% in the radar electromechanical servo control system designed by the present invention.

[0043] It should be noted that in the radar electromechanical servo control system of the present invention, the PLC controller needs to calculate the error signal according to the feedback signal of the encoder before it can output the corresponding control signal to the servo driver; wherein, when actually controlling the movement of the radar, in order to avoid the impact caused by excessive acceleration of the radar and affect the safety and stability of the equipment, a speed filter is also set in the PLC controller of the radar electromechanical servo control system, which is specifically: The PLC controller obtains the feedback signal of the encoder and obtains the error signal to calculate the speed setting value of the given servo drive according to the error signal, and uses the speed filter to filter the speed setting value to output the filtered speed value. The speed filtering formula is:

[0044] in, The speed setpoint is calculated by the PID algorithm of the PLC controller according to the error signal, The filtered speed value output by the speed filter at the current moment, The filtered speed value output by the speed filter at the next moment, a To set the acceleration,T s It is the program running cycle of the PLC controller.

[0045] It can be seen that by setting a speed filter in the PLC controller to limit the acceleration based on the above speed filtering formula, it can ensure that the acceleration is not too large, ensure that the speed value after the error signal is calculated is given to the servo driver through a certain gradient, and ensure smooth acceleration and deceleration of large-mass radar equipment.

[0046] It should be pointed out that in the radar electromechanical servo control system of the present invention, a special PID function module is usually included in the PLC controller. PID control is a common feedback control algorithm widely used in industrial control systems for regulating and controlling various process variables.

[0047] In addition, the study found that in the radar electromechanical servo control system, multiple radars are often set up. For the multi-array radar electromechanical servo system, the following requirements often arise: 1. The same controlled object requires multiple motors to execute synchronously in the same dimension; 2. Different controlled objects need to maintain the same speed in the same dimension. However, in actual industrial sites, the time delays from the bus to the servo drive and from the drive to the motor are different, resulting in asynchronous acceleration and displacement differences.

[0048] For the above requirement 1, if they are not synchronized, the mechanical structure of the controlled object will be subjected to bending stress, and even the entire structure will be damaged in severe cases; for the above requirement 2, if they are not synchronized, a collision may occur if the reserved gap is too small under collision protection.

[0049] Therefore, for a multi-plane radar electromechanical servo system, since multiple radars are arranged to move, these radars all involve coordinated movement. Once the coordinated movement is not good, it is easy for multiple radars to collide with each other. Therefore, in the radar electromechanical servo control system designed by the present invention, when coordinated movement is required, the synchronization lock is enabled, and the servo motors of all the radars are allocated to: a master motor and at least one slave motor; the slave motor will generate an additional control amount according to the displacement difference calculated by the encoder corresponding to the master motor and the encoder corresponding to the slave motor to compensate for its speed, and the formula is:

[0050] in, is the speed value of the slave motor after compensation; is the slave motor speed value before compensation; v c is the speed compensation value; The displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; xset The preset displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; x l , x u They are the upper and lower limits of the absolute value of the displacement difference for enabling speed compensation control. If the upper limit is exceeded, it will be considered as an abnormality and trigger protection.

[0051] Therefore, based on this collaborative motion compensation strategy, the radar electromechanical servo control system of the present invention can timely compensate for the displacement difference caused by the asynchronous acceleration of the above-mentioned multiple radars in the collaborative motion, and ensure that the motion synchronization of the multiple radars is within 0.1mm.

[0052] Accordingly, when the radar electromechanical servo control system designed by the present invention is actually applied, it is also necessary to use a PLC controller to receive the feedback signal of the encoder and determine whether the system is abnormal, so as to take the following emergency protection measures when the system is abnormal: Once the system is abnormal, the system status will be set to fault, all motors will be turned off and brakes will be applied, and finally the fault code will be recorded for maintenance personnel to locate the cause of the fault and troubleshoot and repair it. The movement will be allowed to resume only after the fault is cleared. Part of the abnormality detection of the radar electromechanical servo control system of the present invention is redundant detection, that is, it may be triggered simultaneously with other abnormality detections to ensure overall safety.

[0053] When the radar electromechanical servo control system is actually applied, system anomalies may include the following problems: (1) Abnormal encoder reading The fault phenomena of abnormal encoder reading include: the encoder reading is always full high and the online identifier is false (encoder offline), the reading jumps (encoder is disturbed) and the reading is abnormally cleared. Among them, when the reading is abnormally cleared to zero, it occurs in the process of powering on again after powering off, the power-off storage area of ​​the PLC controller is used for judgment. If the encoder value does not match the historical record value of the power-off storage area when the encoder is back online, an alarm will be triggered.

[0054] (2) The calculated error signal is abnormal The PLC controller of the radar electromechanical servo control system obtains the feedback signal of the encoder and calculates the error value of the error signal to determine according to the following formula:

[0055] in, is the error value calculated by the PLC controller in the current frame, is the error value calculated by the PLC controller in the next frame, v max To preset the maximum speed, Tc It is the operation cycle of the PLC controller program; If the error value of the error signal does not satisfy the above formula, it can be determined that the PLC controller has a spike, the equipment movement will oscillate, and it is judged that the error signal is abnormal. The encoder can be replaced or a spike filtering algorithm can be added according to the actual situation.

[0056] (3) Abnormal micro switch signal A micro switch is also provided in the radar electromechanical servo control system. The micro switch can be specifically installed at the end of all the radar's motion strokes, and the micro switch can be directly connected to the forward and reverse locking of the servo drive. The PLC controller can also obtain the locking status through the servo status code and control the servo motor brake in time to prevent the radar equipment from running out of the stroke.

[0057] (4) Servo drive reports an abnormality The abnormal fault phenomena reported by the servo drive include: servo drive disconnection, servo motor current over-limit, servo motor runaway, enable backcheck error, etc. Among them, the above-mentioned drive communication disconnection can be judged by the online identifier; the judgment of motor current over-limit needs to filter out the peak current in the start-stop stage to prevent false alarms, and protect based on 1.1 times the maximum current during acceleration and deceleration; the above-mentioned servo motor runaway protection is based on the use of 1.1 times the maximum speed of the servo motor as the benchmark for protection.

[0058] (5) Collision warning Collision warning includes: the radar array in a radar electromechanical servo control system with multiple radars does not move according to the predetermined trajectory due to various reasons, which may cause interference and collision problems.

[0059] To this end, the present invention designs a collision judgment formula for any two radar arrays. If the formula is satisfied, it means that a collision has occurred. The formula is as follows:

[0060] In the formula, V i , V j The space occupied by two radar arrays; , The space reserved for the two radars to brake when they collide. If the above formula is satisfied, it means a collision has occurred.

[0061] Usually, the mechanical structure of the electromechanical servo control system will limit the freedom of the array movement. In the radar electromechanical servo control system of the present invention, the above formula can be simplified:

[0062] In the formula, xi , y i The absolute horizontal coordinate and the absolute vertical coordinate respectively correspond to the center of the radar array of the first radar; x j , y j They correspond to the absolute horizontal coordinate and the absolute vertical coordinate of the center of the radar array of the second radar respectively; d is the side length of the square radar array, The distance reserved for braking; wherein, satisfying the above formula in different sports modes indicates a collision. In actual application, the user can rewrite the content of the above collision judgment formula according to the specific actual situation.

[0063] (6) PLC controller abnormality The fault phenomena of the main controller abnormality include but are not limited to: no signal reporting from the PLC controller, the PLC controller cannot send control signals, and the PLC controller crashes and all variables are kept. This type of protection requires the host computer of the PLC controller to perform a direct remote power-off, and the motor brake will automatically brake to release the out-of-control state.

[0064] (7) Abnormal coordinated movement In the radar electromechanical servo control system designed by the present invention, due to the use of a coordinated motion compensation strategy, in the coordinated motion compensation, if the position difference exceeds the set upper limit, the abnormality will be triggered. In addition, the radar electromechanical servo control system will time all movements, and when the movement time exceeds the set threshold, the timeout protection will also be triggered, judging that the coordinated motion is abnormal.

[0065] In addition, it should be noted that in the radar motor servo control system of the present invention, the control instructions from the host computer must send a header code, a tail code and a check code to reduce the error code problem caused by transmission interference; and, after receiving the corresponding information, the PLC controller will further determine whether the message is valid. After determining that the message is valid, it will additionally reply with corresponding information.

[0066] Taking into account the radar electromechanical servo system has many actuators, if an operational error occurs, it may cause equipment damage; therefore, in order to further optimize the safety and reliability of the radar electromechanical servo control system, a fool-proof design is further carried out for the operator. The fool-proof design is also called error proofreading. The purpose of the design is to reduce or eliminate human errors through various means and methods, so that the operator does not need to spend attention, experience and professional knowledge, and can complete the operation accurately based on intuition.

[0067] Therefore, in the above technical solution of the present invention, a display screen for the operator is also provided in the radar electromechanical servo control system, and an operation interface is provided in the display screen, and the operation interface only includes three command issuing methods: state switching, emergency stop, and alarm recovery; wherein, state switching requires clicking on the target state and confirming it twice, and state switching is not allowed during the motion process to avoid possible manual misoperation and prevent interference between radars caused by unplanned motion.

[0068] In addition, when an alarm occurs, the system fault must be checked and the alarm must be cleared before the task can be continued; in addition, the operation interface for maintenance personnel requires an advanced password to enter. This operation interface can individually control the motor inching, uniform motion and positioning motion in a certain dimension; at the same time, in actual application, the operation interface can also be equipped with complex functions such as zero-position calibration, coordinated motion recovery, and dual-motor synchronization lock release.

[0069] Accordingly, when the radar electromechanical servo control system is actually applied, the electromechanical equipment is not allowed to be turned on when the radar in the radar electromechanical servo control system is working; this is because: the high-frequency signal emitted by the radar will seriously interfere with encoders, horizontal sensors and other sensor equipment. At this time, turning on the electromechanical equipment will cause reading jumps and trigger system abnormal protection.

[0070] From the above description, it can be seen that the radar motor servo control method and system designed by the present invention further improves the safety of the electromechanical system during operation by implementing an optimized safety control strategy. It can execute corresponding safety protection measures during normal operation or when the system has an abnormal condition, thereby ensuring the safety and reliability of the system during normal operation, and being able to terminate the task urgently when software or machinery has an abnormality, thereby ensuring the rationality of the operator's operation. It has good promotion prospects and application value.

[0071] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A radar electromechanical servo control system, characterized in that: include: A PLC controller, a servo driver and at least two radars, each of which is provided with a servo motor and an encoder; the servo driver is connected to the servo motor by communication, and the PLC controller and the servo driver are connected by CANOpen communication; wherein the servo driver receives a control signal from the PLC controller and drives the servo motor to enable or brake; The PLC controller also establishes a serial communication connection with the encoder to receive the feedback signal of the encoder and determine whether there is an abnormality in the system. When the system is abnormal, the system state is set to fault, and the servo motors of all radars are enabled and braked.

2. The radar electromechanical servo control system according to claim 1, characterized in that: The servo motor is enabled before the servo motor is released, and the servo motor is braked before the servo motor is turned off. There is a preset CANOpen communication cycle between the servo motor enabling and the servo motor releasing, or between the servo motor braking and the servo motor turning off.

3. The radar electromechanical servo control system according to claim 1, characterized in that: The sampling frequency of the encoder by the PLC controller is at least twice higher than the program running frequency of the PLC controller.

4. The radar electromechanical servo control system according to claim 1, characterized in that: The PLC controller obtains the feedback signal of the encoder and obtains an error signal, so as to calculate a speed setting value given to the servo driver according to the error signal, and performs speed filtering on the speed setting value by using a speed filter to output a filtered speed value, and sends a control signal to the servo driver based on the filtered speed value; Among them, the speed filtering formula of the speed filter is: in, The speed setpoint is calculated by the PID algorithm of the PLC controller according to the error signal, The filtered speed value output by the speed filter at the current moment, The filtered speed value output by the speed filter at the next moment, a To set the acceleration, T s It is the program running cycle of the PLC controller.

5. The radar electromechanical servo control system according to claim 1, characterized in that: When at least two of the radars move in coordination, a synchronization lock is enabled, and the servo motors of all the radars are allocated to: a master motor and at least one slave motor; The slave motor generates an additional control quantity according to the displacement difference calculated by the encoder corresponding to the master motor and the encoder corresponding to the slave motor to compensate for its speed, and the formula is: in, is the speed value of the slave motor after compensation; is the slave motor speed value before compensation; v c is the speed compensation value; The displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; x set The preset displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; x l , x u They are the upper and lower bounds of the absolute value of the displacement difference for enabling velocity compensation control.

6. The radar electromechanical servo control system according to claim 1, characterized in that: The system abnormality includes: at least one of abnormal encoder reading, abnormal error signal, abnormal micro switch signal, abnormal servo drive reporting, collision warning, abnormal PLC controller and abnormal coordinated motion.

7. The radar electromechanical servo control system according to claim 6, characterized in that: The error signal anomaly includes: The PLC controller obtains the feedback signal of the encoder and calculates the error value of the error signal to determine according to the following formula: in, is the error value calculated by the PLC controller in the current frame, is the error value calculated by the PLC controller in the next frame, v max To preset the maximum speed, T c It is the operation cycle of the PLC controller program; If the error value of the error signal does not satisfy the above formula, it can be determined that a spike is generated in the PLC controller and the error signal is judged to be abnormal.

8. The radar electromechanical servo control system according to claim 6, characterized in that: The collision warning is specifically: The collision judgment formula for any two radar arrays is as follows: In the formula, V i , V j The space occupied by two radar arrays; , Space reserved for two radar collision brakes.

9. The radar electromechanical servo control system according to claim 1, characterized in that: It also includes a display screen, which is communicatively connected to the PLC controller, and the operation interface of the display screen only includes three command issuing methods: state switching, emergency stop, and alarm recovery; among which, state switching requires clicking on the target state and confirming it twice, and state switching is not allowed during the movement process.

10. A radar electromechanical servo control system, applied to the radar electromechanical servo control system according to any one of claims 1 to 9, characterized in that: Includes steps: S1: The PLC controller outputs a control signal to the servo driver, so as to use the servo driver to control the radar servo motor to enable; S2: The PLC controller receives the feedback signal from the encoder and determines whether the system is abnormal; if it is determined to be abnormal, execute step S3; S3: When the system is abnormal, the system status is set to fault, the servo motors of all radars are enabled and braked, and the fault code is recorded.

Images