A radar electro-mechanical servo control system and method

Abstract

The present invention discloses a radar electro-mechanical servo control system, which includes: a PLC controller, a servo driver, and at least two radars. Each radar is provided with a servo motor and an encoder; the servo driver is communicatively connected to the servo motor, and the PLC controller is communicatively connected to the servo driver by 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 is also serially communicatively connected to 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 a fault, the enabling of the servo motors of all radars is turned off and the brakes are applied. Correspondingly, the present invention also discloses a radar electro-mechanical servo control method, which is implemented based on the above-mentioned radar electro-mechanical servo control system of the present invention. Thus, the system and method designed by the present invention can greatly improve the safety and reliability of radar operation.

Description

Technical Field

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

[0002] In the current prior art, in the field of radar, the electro-mechanical devices such as programmable logic controllers (PLCs), servo drivers, and corresponding external sensors still have problems with poor core performance and system stability.

[0003] Due to the insufficient performance and poor system stability of radar products, unpredictable personnel and equipment accidents may occur; at the same time, for the radar electro-mechanical servo system, moving multiple large-mass radars to corresponding positions at high speed and precisely in coordination, the damage caused by accidents in this process is large, and it is necessary to design a safe motion strategy to ensure that the impact is within the allowable range.

[0004] Therefore, there is an urgent need to design a radar electro-mechanical servo control system and system that takes into account safety design to ensure that the radar electro-mechanical servo can work smoothly and without faults, 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 electro-mechanical servo control system and system to solve the problems of poor safety, poor stability, and easy occurrence of accidental faults caused by insufficient performance of existing radar electro-mechanical devices.

[0006] To solve the above technical problem, the technical solution adopted by the present invention is: A radar electro-mechanical servo control system, characterized in that it includes: a PLC controller, a servo driver, and at least two radars, and servo motors and encoders are provided on each of the radars; the servo driver is communicatively connected to the servo motor, and the PLC controller and the servo driver are communicatively connected by CANOpen; wherein, the servo driver receives a control signal from the PLC controller and drives the servo motor to enable or brake.

[0007] The PLC controller also establishes a serial communication connection with the encoder to receive the feedback signal of the encoder and determine whether the system is abnormal. When the system is abnormal, the system state is set to a fault, and the enabling of the servo motors of all radars is turned off and the brakes are applied.

[0008] In the above technical solution of the present invention, aiming at the problems of poor safety, poor stability and easy occurrence of accidental failures caused by insufficient performance of existing radar electromechanical equipment. The present invention discloses a new radar electromechanical servo control system. The PLC controller in the radar electromechanical servo control system can establish a serial communication connection with the encoder to receive the feedback signals of the encoders set on the servo motors of each radar, so as to analyze the motion conditions of each radar, and thereby judge whether there is an abnormality in the system. When the system is abnormal, the system state is set to a fault, the servo motor enables of all radars are turned off and the brakes are applied, so as to minimize the possible safety accidents and the resulting equipment losses, and improve the safety and stability of the radar electromechanical equipment, which has good promotion prospects and application value.

[0009] Further, in the radar electromechanical servo control system of the present invention, the servo motor enable is before the servo motor brake release, and the servo motor brake is before the servo motor enable is turned off; wherein, there is a preset CANOpen communication cycle between the servo motor enable and the servo motor brake release, or between the servo motor brake and the servo motor enable being turned off.

[0010] In the above technical solution of the present invention, in actual application, since the control signal is sent from the PLC controller to the servo driver to realize the switching of different operating states of the servo motor, the preset CANOpen communication cycle can be specifically controlled to be 5 CANOpen communication cycles, so as to reserve sufficient time for the PLC controller to receive the feedback signal of the encoder and perform the enable back-check process, ensuring the safe control of the servo motor.

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

[0012] Further, in the radar electromechanical servo control system of the present invention, the PLC controller obtains the feedback signal of the encoder and obtains an error signal, so as to calculate the speed set value given to the servo driver according to the error signal, and uses a speed filter to filter the speed set value to output a filtered speed value, and sends a control signal to the servo driver based on the filtered speed value;

[0013] Among them, the speed filtering formula of the speed filter is:

[0014]

[0015] Among them, is the speed set value 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 The set acceleration, T s Is the program operation cycle of the PLC controller.

[0016] In the above technical solution of the present invention, the PLC controller in the radar electro-mechanical servo control system needs to calculate according to the feedback signal of the encoder for the output of the control signal to the servo driver; among them, when actually controlling the movement of the radar, in order to avoid the impact caused by excessive radar acceleration and affect the safety and stability of the equipment, a speed filter is also set in the PLC controller of the radar electro-mechanical servo control system. Based on the speed filtering formula of the above speed filter, the acceleration is limited to ensure that the acceleration is not too large. Based on the filtered speed value obtained after filtering, a control signal is sent to the servo driver to control the radar to move normally and ensure the smooth acceleration and deceleration of the large-mass radar equipment.

[0017] Further, in the radar electro-mechanical servo control system of the present invention, when at least two radars move in coordination, a synchronization lock is enabled, and all the servo motors of the radars are allocated as: one main motor and at least one slave motor;

[0018] The slave motor will generate an additional control amount according to the displacement difference calculated by the encoder corresponding to the main motor and the encoder corresponding to the slave motor to compensate its speed. The formula is:

[0019]

[0020] Wherein, Is the speed value of the slave motor after compensation; Is the speed value of the slave motor before compensation; v c Is the speed compensation value; Is the displacement difference between the encoder corresponding to the main motor and the encoder corresponding to the slave motor; x set Is the preset displacement difference between the encoder corresponding to the main motor and the encoder corresponding to the slave motor; x l , x u Are respectively the upper and lower bounds of the absolute value of the displacement difference for enabling speed compensation control.

[0021] Further, in the radar electro-mechanical servo control system of the present invention, the system anomalies include at least one of the following: encoder reading anomaly, error signal anomaly, microswitch signal anomaly, servo driver reported anomaly, collision warning, PLC controller anomaly, and coordinated movement anomaly.

[0022] Further, in the radar electro-mechanical servo control system of the present invention, the abnormal error signal includes:

[0023] The PLC controller obtains the feedback signal of the encoder and calculates the error value of the error signal to judge according to the following formula:

[0024]

[0025] where 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 is the preset maximum speed, T c is the operating cycle of the PLC controller program;

[0026] 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.

[0027] Further, in the radar electro-mechanical servo control system of the present invention, it further includes: The collision warning is specifically:

[0028] The collision judgment formula for any two radar arrays is as follows:

[0029]

[0030] In the formula, V i , V j is the space occupied by two radar arrays; , is the space reserved for two radar collisions to brake.

[0031] Further, in the radar electro-mechanical servo control system of the present invention, it further includes a display screen. The display screen is communicatively connected to the PLC controller. The operation interface of the display screen only includes three instruction issuing methods: status switching, emergency stop, and alarm recovery; among them, status switching requires clicking on the target status and then confirming twice, and status switching is not allowed during the movement process.

[0032] Considering that there are many actuators in the radar electro-mechanical servo system, equipment damage may occur if operational errors occur; therefore, in order to further optimize the safety and reliability of the radar electro-mechanical servo control system, a foolproof design has been further carried out for the operator. The foolproof 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 can complete the operation accurately without spending attention, experience or professional knowledge, and by intuition.

[0033] Therefore, in the above technical solution of the present invention, a display screen for the operator is also provided in the radar electro-mechanical servo control system. An operation interface is set in the display screen, and the operation interface only includes three instruction issuing methods: state switching, emergency stop, and alarm recovery; among them, for state switching, it is necessary to click on the target state and then confirm it twice, and state switching is not allowed during the movement process to avoid possible manual misoperations.

[0034] Correspondingly, another object of the present invention is to disclose a radar electro-mechanical servo control method, which can be simply applied to the above-mentioned radar electro-mechanical servo control system of the present invention to improve the safety and stability of the radar electro-mechanical servo control system. The specific steps include:

[0035] S1: The PLC controller outputs a control signal to the servo driver to enable the radar servo motor by using the servo driver;

[0036] S2: The PLC controller receives the feedback signal of the encoder and judges whether there is an abnormality in the system; when it is judged to be in an abnormal state, step S3 is executed;

[0037] S3: When the system is abnormal, set the system state to a fault, turn off the enabling of all radar servo motors and apply the brake, and record the fault code.

[0038] The beneficial effect of the present invention is that a radar electro-mechanical servo control system and system designed by the present invention can effectively solve the problem of abnormal operation caused by the insufficient core performance and system stability of the radar electro-mechanical system in the prior art. In practical applications, the radar motor servo control method and system designed by the present invention can further improve the safety of the electro-mechanical system during operation by implementing an optimized safety control strategy, so as to execute corresponding safety protection measures when the system is working normally or an abnormal condition occurs, thereby not only ensuring the safety and reliability of the system during normal operation, but also being able to abort the task in time when software or machinery malfunctions. It has good promotion prospects and application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a flowchart of the steps of the radar electro-mechanical servo control method of the present invention in an implementation manner.

[0040] Figure 2 It is a block diagram showing the abnormal fault problems existing in a specific implementation mode of the radar electro-mechanical servo control system described in the present invention. Specific implementation mode

[0041] To describe in detail the technical content, achieved objectives and effects of the present invention, the following is described in conjunction with the implementation modes and with reference to the drawings.

[0042] It has been found through research that in current radar products, due to the insufficient core performance and system stability of the radar products, there are problems such as poor safety, poor stability and prone to accidental failures. Therefore, please refer to Figure 1 and Figure 2 As shown, the present invention discloses a new radar electro-mechanical servo control method and system to solve the above technical problems and add a new safety servo control design for the radar electro-mechanical system.

[0043] See Figure 1 , Figure 2 As shown, the present invention discloses a radar electro-mechanical servo control system, which specifically includes: a PLC controller (programmable logic controller), a servo driver and at least two radars, and servo motors and encoders are provided on each of these radars. Among them, the encoder is specifically arranged on the servo motor corresponding to the radar. The encoder is a kind of sensor, and the main function of the encoder is to convert mechanical motion (such as rotational or linear motion) into electrical signal output for measurement, control or data transmission.

[0044] It should be noted that in the present invention, the above PLC controller is respectively communicatively connected with the servo driver and the encoder; among them, the PLC controller and the encoder can be communicatively connected in the form of synchronous serial communication; at the same time, the PLC controller and the servo driver can specifically adopt the CANOpen communication protocol for communicative connection; CANopen is a high-level communication protocol based on the CAN (Controller Area Network) bus, and it belongs to a standardized protocol.

[0045] In the radar electro-mechanical servo control system of the present invention, the above servo driver can be specifically communicatively connected with the servo motor on the radar. The servo driver can receive the control signal from the PLC controller and drive the servo motor on the radar to enable or brake, so as to drive the radar to move or stop.

[0046] In the present invention, since the above-mentioned encoder is arranged on the servo motor corresponding to the radar, the encoder can specifically generate a feedback signal in the form of an electrical signal for the mechanical motion condition 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, referring to 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, turn off the enabling of the servo motors of all radars and apply the brakes.

[0047] When actually applying the radar electro-mechanical servo control system designed by the present invention, in order to improve the safety during system operation and further improve the stability of the system, it can also specifically perform the following safety control measures to ensure that the system can operate normally:

[0048] In the radar electro-mechanical servo control system of the present invention, controlling the servo motor on the radar can meet the following requirements, that is: the servo motor enabling is before the servo motor brake release, and the servo motor braking is before the servo motor enabling is turned off; among them, there is a preset CANOpen communication cycle interval between the servo motor enabling and the servo motor brake release, or between the servo motor braking and the servo motor enabling being turned off.

[0049] During actual application, since the control signal is sent from the PLC controller to the servo driver to realize the switching of different operating states of the servo motor, the above-mentioned preset CANOpen communication cycle can be specifically controlled to be 5 CANOpen communication cycles, so as to reserve sufficient time for the PLC controller to receive the feedback signal of the encoder and perform enabling back-check, ensuring the safety control of the servo motor.

[0050] In addition, during the actual application of this radar electro-mechanical servo control system, during 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 apply the brake, preventing the phenomenon of brake rush and avoiding damage to the brake. In addition, the servo motors of each radar are constantly in the braked state under power-off conditions to avoid displacement of the radar affected by natural environmental factors after power-off.

[0051] Correspondingly, in the present invention, in order to speed up the enabling back-check process of the above-mentioned PLC controller and avoid safety problems due to too slow back-check speed, according to the Shannon sampling theorem, 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 operation frequency of the PLC controller, so that the PLC controller can calculate the error signal for closed-loop control according to the received feedback signal. Among them, the above setting form is specifically represented by the following formula:

[0052]

[0053] Among them, is the program running frequency of the PLC controller, represents the sampling frequency of the input quantity that has a strong correlation with the output of the PLC controller, that is, the sampling frequency of the feedback signal sent by the encoder.

[0054] In addition, when the radar electro-mechanical servo control system designed by the present invention is operating normally, the reason for controlling the above PLC controller and servo driver to communicate and connect 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. Among them, it should be noted that too high a CAN bus load rate will cause data loss and transmission delay, and its calculation formula is as follows:

[0055]

[0056]

[0057] Among them, F represents the number of bits of each message; B filler represents the fill bit, is the floor function; B format is the format bit, and its specific value is 47; B data is the data bit. For a PDO message, its value is a multiple of 8 and does not exceed 64. For the 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 represents the i th number of bits occupied by the message. There are a total of n messages on this CAN bus. The above calculation ignores the device heartbeat whose period is longer than the CAN bus cycle.

[0058] Therefore, in order to avoid too high a CAN bus load rate, generally speaking, the CAN bus load rate should not exceed 50%. In the radar electro-mechanical servo control system designed by the present invention, it can be controlled within 20%.

[0059] It should be noted that in the radar electro-mechanical servo control system of the present invention, the PLC controller needs to calculate the error signal based on the feedback signal of the encoder before it can output the corresponding control signal to the servo driver. Among them, when actually controlling the movement of the radar, in order to avoid the impact caused by excessive radar acceleration and affect the safety and stability of the equipment, a speed filter is also set in the PLC controller of the radar electro-mechanical servo control system, and specifically:

[0060] The PLC controller obtains the feedback signal of the encoder and obtains the error signal, calculates the speed set value given to the servo driver according to the error signal, and uses the speed filter to filter the speed set value to output the filtered speed value. The speed filtering formula is:

[0061]

[0062] Wherein, is the speed set value calculated by the PID algorithm of the PLC controller according to the error signal, is the filtered speed value output by the speed filter at the current moment, is the filtered speed value output by the speed filter at the next moment, a is the set acceleration, T s is the program operation cycle of the PLC controller.

[0063] It can be seen that by setting a speed filter in the PLC controller and restricting 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 calculating the error signal is given to the servo driver with a certain gradient, and ensure the smooth acceleration and deceleration of the large-mass radar equipment.

[0064] It should be pointed out that in the radar electro-mechanical servo control system of the present invention, there is usually a dedicated PID function module in the PLC controller. PID control is a common feedback control algorithm and is widely used in industrial control systems to adjust and control various process variables.

[0065] In addition, it is found that in the radar electro-mechanical servo control system, multiple radars are often set. For the radar electro-mechanical servo system with multiple arrays, the following requirements often occur:

[0066] 1. Multiple motors need to execute synchronously on the same dimension for the same controlled object;

[0067] 2. Different controlled objects need to move at the same speed on the same dimension. However, in the actual industrial field, the time delays from the bus to the servo driver and from the driver to the motor are different, resulting in asynchronous acceleration processes and thus displacement differences.

[0068] For the above requirement 1, if there is no synchronization, it will cause the mechanical structure of the controlled object to be subjected to bending stress, and in severe cases, it may even cause overall damage; for the above requirement 2, without synchronization, if the reserved gap is too small under collision protection, it may cause a collision.

[0069] Therefore, for a multi-front radar electro-mechanical servo system, since there are multiple radars moving, these radars are all involved in coordinated movement. Once the coordinated movement is not good, there is a high risk of collision between multiple radars. Therefore, in the radar electro-mechanical servo control system designed by the present invention, when coordinated movement is required, a synchronization lock is enabled, and the servo motors of all the radars are allocated as: one main motor and at least one slave motor; the slave motor will generate an additional control amount based on the displacement difference calculated by the encoder corresponding to the main motor and the encoder corresponding to the slave motor to compensate its speed, and the formula is:

[0070]

[0071] Wherein, is the speed value of the slave motor after compensation; is the speed value of the slave motor before compensation; v c is the speed compensation value; is the displacement difference between the encoder corresponding to the main motor and the encoder corresponding to the slave motor; x set is the preset displacement difference between the encoder corresponding to the main motor and the encoder corresponding to the slave motor; x l , x u are respectively the upper and lower bounds of the absolute value of the displacement difference for enabling speed compensation control. Exceeding the upper bound will be recognized as an abnormal trigger for protection.

[0072] Thus, based on this coordinated movement compensation strategy, the radar electro-mechanical servo control system of the present invention can timely compensate for the displacement difference generated by the asynchronous acceleration of the above-mentioned multiple radars during coordinated movement, ensuring that the movement synchronization of multiple radars is within 0.1 mm.

[0073] Correspondingly, when actually applying the radar electro-mechanical servo control system designed by the present invention, it is also necessary to use a PLC controller to receive the feedback signal of the encoder and judge whether there is an abnormality in the system, so as to take the following emergency protection measures when the system is abnormal:

[0074] Once the system is abnormal, the system state will be set to a fault, all motor enables will be turned off and the brakes will be applied. Finally, the fault code will be recorded for maintenance personnel to locate the cause of the fault and troubleshoot. Movement is only allowed to resume after the fault is cleared. Part of the abnormality detection of the radar electro-mechanical 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.

[0075] When actually applying this radar electro-mechanical servo control system, system anomalies may include the following problems:

[0076] (1) Abnormal encoder readings

[0077] The fault phenomena of abnormal encoder readings include: the encoder readings are constantly at full height and the online identifier is false (the encoder is offline), the readings jump (the encoder is interfered), and the readings are abnormally cleared. Among them, when the phenomenon of abnormal clearing of readings occurs during the process of powering off and then powering on again, it is judged that the power-off storage area of the PLC controller needs to be used. If the encoder value does not match the historical record value in the power-off storage area when reconnecting, an alarm will be triggered.

[0078] (2) Abnormal calculated error signal

[0079] The PLC controller of the radar electro-mechanical servo control system obtains the feedback signal of the encoder and calculates the error value of the error signal to judge according to the following formula:

[0080]

[0081] Wherein, 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 is the preset maximum speed, T c is the operation cycle of the PLC controller program;

[0082] If the error value of the above error signal does not satisfy the above formula, it can be determined that there is a spike in the PLC controller, the equipment movement will oscillate, and it is judged as an abnormal error signal. The encoder can be replaced or an additional spike filtering algorithm can be added according to the actual situation.

[0083] (3) Abnormal microswitch signal

[0084] A microswitch is also set in the radar electro-mechanical servo control system. The above microswitch can be specifically installed at the end of all movement strokes of the radar, and the above microswitch can be directly connected to the forward and reverse locking of the servo driver. The PLC controller can also obtain the locking state through the servo status code and timely control the servo motor brake to prevent the radar equipment from rushing out of the stroke.

[0085] (4) Abnormal report from the servo driver

[0086] The fault phenomena reported by the servo driver include: the servo driver drops offline, the servo motor current exceeds the limit, the servo motor runs away, and the enable return inspection error, etc. Among them, the communication disconnection of the above driver can be judged by the online identifier; for the judgment of the motor current exceeding the limit, the peak current in the start-stop stage needs to be filtered to prevent false alarms, and protection is carried out based on 1.1 times the maximum current during acceleration and deceleration; the above protection against the servo motor running away is based on 1.1 times the set maximum speed of the servo motor as the benchmark for protection.

[0087] (5) Collision warning

[0088] The collision warning includes: due to various reasons, the radar array in the radar electro-mechanical servo control system with multiple radars does not move along the predetermined trajectory, which may lead to problems of interference and collision.

[0089] Therefore, 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:

[0090]

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

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

[0093]

[0094] In the formula, x i , y i respectively correspond to the absolute abscissa and absolute ordinate of the center of the radar array of the first radar; x j , y j respectively correspond to the absolute abscissa and absolute ordinate of the center of the radar array of the second radar; d is the side length of the square radar array, is the reserved braking distance; among them, if the above formula is satisfied in different motion modes, it means that a collision has occurred. In actual application, the user can rewrite the content of the above collision judgment formula according to the specific actual situation.

[0095] (6) Abnormality of the PLC controller

[0096] 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.

[0097] (7) Abnormal coordinated movement

[0098] 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.

[0099] 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.

[0100] 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.

[0101] 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.

[0102] 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.

[0103] Accordingly, when actually applying the radar electro-mechanical servo control system, when the radar in the radar electro-mechanical servo control system is working, it is not allowed to turn on the electro-mechanical equipment; this is because: the high-frequency signals emitted by the radar will seriously interfere with sensor devices such as encoders and horizontal sensors. At this time, turning on the electro-mechanical equipment will cause phenomena such as reading jumps and trigger system abnormal protection.

[0104] As can be seen from the above description, the radar motor servo control method and system designed by the present invention further improve the safety during the operation of the electro-mechanical system by implementing an optimized safety control strategy. It can execute corresponding safety protection measures when working normally or when the system has abnormal conditions, so as to not only ensure the safety and reliability of the system during normal operation, but also urgently abort the task when software or mechanical abnormalities occur, ensuring the rationality of the operator's operation. It has good promotion prospects and application value.

[0105] The above are only the embodiments of the present invention, and do not limit the patent scope of the present invention accordingly. All equivalent transformations made by using the content of the specification and drawings of the present invention, or directly or indirectly applied in related technical fields, are similarly 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 a feedback signal from the encoder and determine whether the system is abnormal. When the system is abnormal, the system state is set to fault, and the servo motors of all radars are enabled and braked; It also includes: a speed filter, the PLC controller obtains the feedback signal of the encoder and obtains an error signal, calculates a 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 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, It is the program running cycle of the PLC controller.

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: 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; is the speed compensation value; The displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; The preset displacement difference between the encoder corresponding to the master motor and the encoder corresponding to the slave motor; , They are the upper and lower bounds of the absolute value of the displacement difference for enabling velocity compensation control.

5. 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.

6. The radar electromechanical servo control system according to claim 5, 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, To preset the maximum speed, 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.

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

8. 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.

9. A radar electromechanical servo control system, applied to the radar electromechanical servo control system according to any one of claims 1 to 8, 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