Instruction control and processing method for multi-axis control system

Abstract

The invention belongs to the technical field of aircraft control, and relates to a multi-shaft control system instruction control and processing method, which comprises the following steps of: acquiring an output voltage signal of a control shaft instruction sensor; collecting the power supply voltage 4VPS of the control shaft instruction sensor; according to the output voltage signal of the control shaft instruction sensor and the power supply voltage 4VPS, the angle of the control shaft is obtained through calculation; according to the output voltage signal of the pitch axis instruction sensor and the power supply voltage 4VPS, a control shaft transient self-monitoring result is obtained through a control shaft sensor self-monitoring algorithm; through inter-channel data link transmission, two redundant channels of the control system exchange control shaft data with each other; and in the inter-channel data link transmission process, through a CCDL monitoring and data processing algorithm, obtaining control angles of other channels for voting processing and control shaft transient self-monitoring results of other channels.

Description

Technical Field

[0001] The invention belongs to the technical field of aircraft control and relates to a multi-axis control system instruction control and processing method. Background Art

[0002] With the rapid development of aviation technology, electric vertical take-off and landing aircraft (eVTOL) has attracted wide attention due to its unique performance and application prospects. eVTOL has the advantages of vertical take-off and landing, high efficiency and flexibility, and has great potential in urban transportation, logistics transportation, emergency rescue, tourism and other fields.

[0003] Manned eVTOL is an important branch of eVTOL, which requires the driver to be able to control the eVTOL to solve the safety problems of emergency conditions under existing technical conditions. The existing aircraft control system is usually composed of several products (side sticks, pedals, throttle platforms) to achieve multi-axis control, and does not have the ability to self-detect faults, which makes it difficult to meet the requirements of manned eVTOL for high detection rate, small size and light weight of the control system. With the development of manned eVTOL, the concept of multi-axis control system has been proposed, which integrates all multi-axis control tasks and signal processing together, outputs reliable and trustworthy digital command signals to the outside world, and achieves the goals of high detection rate, high safety, high reliability, high precision and small size / weight of manned eVTOL control system. However, there is still a blank in the command control and processing methods of multi-axis control systems.

[0004] In summary, in order to improve the performance and operational safety of manned eVTOL and meet the growing market demand, a new multi-axis control system command control and processing method is urgently needed to achieve high fault detection rate, high safety, high reliability and high control accuracy. Summary of the invention

[0005] The present invention provides a multi-axis control system command control and processing method, which can realize the control and processing of control commands in the four-axis control directions of pitch, roll, yaw and vertical of a multi-axis control device.

[0006] The technical solution adopted by the present invention to solve its technical problem is:

[0007] A multi-axis control system command control and processing method, the control command control and processing method of the three axes of pitch, roll and yaw of the multi-axis control device are the same.

[0008] The following takes a certain control direction (pitch axis) as an example to illustrate the technical solution of the present invention, and takes the following steps:

[0009] Step 1: Set the task cycle T of the control and processing of the control system's control instructions p ;

[0010] Step 2: synchronously collect the sine-end output voltage signal Pitch_Sin and the cosine-end output voltage signal Pitch_Cos of the pitch axis command sensor of the multi-axis manipulator, ensuring that the two output voltage signals are collected at the same time;

[0011] Step 3, collecting the power supply voltage 4V_PS of the pitch axis command sensor of the multi-axis manipulator;

[0012] Step 4: Calculate the pitch axis angle θ based on the sine terminal output voltage signal Pitch_Sin and cosine terminal output voltage signal Pitch_Cos of the pitch axis command sensor and the power supply voltage 4V_PS P ;

[0013] Step 5: According to the sine terminal output voltage signal Pitch_Sin and the cosine terminal output voltage signal Pitch_Cos of the pitch axis command sensor and the power supply voltage 4V_PS, the pitch axis sensor self-monitoring algorithm is used to obtain the pitch axis transient self-monitoring result theta_P_VF;

[0014] Step 6: The two redundant channels of the control system exchange pitch axis data (angle value and transient self-monitoring result value) with each other through the inter-channel data link (CCDL);

[0015] Step 7: During the inter-channel data link transmission (CCDL) process, the pitch axis angle θ of other channels used for voting is obtained through the pitch axis CCDL monitoring and data processing algorithm. P1 and the pitch axis transient self-monitoring results theta_P1_VF of other channels;

[0016] Step 8: According to the pitch axis angle θ of this channel P 、Other channel pitch axis angle θ P1 , the pitch axis transient self-monitoring result theta_P_VF of this channel and the pitch axis transient self-monitoring result theta_P1_VF of other channels, and the two-channel command signals are voted to obtain the pitch axis control command voting value θ P_I And the transient pitch axis control command flag PthVD;

[0017] Step 9: Vote on the pitch axis control command value θ P_I Perform the limiting process to obtain the pitch axis control command voting value θ after limiting P_IL ;

[0018] Step 10: According to the instantaneous pitch axis control command flag bit PthVD and the pitch axis control command voting value θ after limiting P_IL , calculate the pitch axis control command θ P_FCMDAnd the pitch axis control command flag FCSS_PthVD;

[0019] Step 11: The pitch axis control command θ after processing P_FCMD Perform secondary limiting processing to obtain the pitch axis control command θ after secondary limiting P_CMD_L2 ;

[0020] Step 12: After the secondary limit, the pitch axis control command θ P_CMD_L2 Perform neutral position processing to obtain the pitch axis control command θ sent to the CANFD / CAN bus P_CMD .

[0021] Preferably, in step 2, the signal is collected in a fixed periodic manner, and the collection period is the same as the task period of the control and processing of the operating system's operating instructions.

[0022] Preferably, in step 3, the signal acquisition method is periodic acquisition, and the acquisition period is the same as the task period of the control and processing of the control system control command. The acquisition time of the power supply voltage 4V_PS can be the same as or different from the acquisition time of the sine end output voltage signal Pitch_Sin and the cosine end output voltage signal Pitch_Cos.

[0023] Preferably, in step 4, the pitch axis angle θ P The calculation formula is Where pi is the circumference of a circle, θ P The unit of is °, the unit of Pitch_Sin, Pitch_Cos and 4V_PS is V.

[0024] Preferably, in step 5, the pitch axis sensor self-monitoring algorithm comprises the following specific steps:

[0025] S1. Determine whether the output voltage signal Pitch_Sin of the sinusoidal end of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range (the upper threshold value of the pitch axis self-monitoring threshold range is 93%*4V_PS, and the lower threshold value is 70%*4V_PS). If it exceeds the pitch axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault;

[0026] S2, determine whether the cosine terminal output voltage signal Pitch_Cos of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range (the upper threshold value of the pitch axis self-monitoring threshold range is 93%*4V_PS, and the lower threshold value is 7%*4V_PS). If it exceeds the pitch axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault;

[0027] S3, determine the combination of the sine terminal output voltage signal Pitch_Sin and the cosine terminal output voltage signal Pitch_Cos of the pitch axis sensor +Whether it exceeds the pitch axis combination threshold range (the upper threshold value of the pitch axis combination threshold range is 0.14*4V_PS 2 , the lower threshold is 0.011*4V_PS 2 If it exceeds the pitch axis combination threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault.

[0028] Preferably, in step 7, the CCDL monitoring and data processing algorithm comprises the following specific steps:

[0029] S1, the interval time T after this channel sends data through CCDL J Afterwards, the transient fault state of the CCDL is detected;

[0030] S2. If a CCDL transient fault occurs and lasts for 5 beats, it is considered a permanent CCDL fault. The pitch axis angle θ of other channels obtained by this channel P1 The pitch axis transient self-monitoring results theta_P1_VF of the other channels are all set to fail-safe values;

[0031] S3. If a CCDL transient fault occurs and does not last for 5 beats, the pitch axis angle θ of other channels obtained by this channel P1 The pitch axis transient self-monitoring results theta_P1_VF of other channels are all set to the normal value of the previous beat.

[0032] Preferably, in step eight, the two-channel command signal voting, the specific algorithm is: if the pitch axis angle θ of the channel P and the pitch axis angle θ of other channels P1 The absolute value of the difference is less than or equal to the pitch axis command threshold θ P_th If the pitch axis transient self-monitoring result theta_P_VF of this channel and the pitch axis transient self-monitoring result theta_P1_VF of other channels are both valid, then the pitch axis control command voting value θ P_J Set to (θ P +θ P1 ) / 2, the instantaneous pitch axis control command flag bit PthVD is set to be valid; otherwise, the pitch axis control command voting value θ P_J Set to fail-safe value θ P_J_S , the transient pitch axis control command flag PthVD is set to invalid.

[0033] Preferably, in step nine, the pitch axis control command voting value θ P_JThe algorithm for limiting processing is: if the pitch axis control command voting value θ P_J Exceeding the upper limit of the pitch axis limit θ P_I_Hmitup , then the pitch axis control command voting value θ after limiting P_IL Set as the upper limit of the pitch axis limit θ P_J_Hmitop ; If the pitch axis control command voting value θ P_I Exceeding the lower limit of the pitch axis θ P_I_Hmitdown , then the pitch axis control command voting value θ after limiting P_IL Set as the pitch axis limit lower threshold θ P_I_Hmitdown ; Otherwise, the pitch axis control command voting value θ after limiting P_IL Set as the pitch axis control command voting value θ P_I .

[0034] Preferably, in step 10, the pitch axis control instruction θ is obtained by calculation P_FCMD The method of determining the pitch axis control command flag bit FCSS_PthVD is as follows: if the instantaneous pitch axis control command flag bit PthVD is invalid and lasts for more than 10 beats, it is determined that a permanent fault has occurred, the pitch axis control command flag bit FCSS_PthVD is invalid, and the flag bit is not restored. The pitch axis control command θ P_FCMD is set to a fail-safe value; if the transient pitch axis control command flag bit PthVD is invalid and does not last for more than 10 beats, the processed pitch axis control command θ P_FCMD Set to the pitch axis control command voting value θ after the limit P_IL If the instantaneous pitch axis control command flag bit PthVD is valid and the pitch axis control command flag bit FCSS_PthVD is valid, then the processed pitch axis control command θ P_FCMD Set to the pitch axis control command voting value θ after the limit P_IL ; If the instantaneous pitch axis control command flag bit PthVD is valid and the pitch axis control command flag bit FCSS_PthVD is invalid, then the processed pitch axis control command θ P_FCMD Set to fail-safe value.

[0035] Preferably, in step 11, the algorithm of the secondary limiting processing is: if the pitch axis control instruction θ after processing P_FCMD Is a positive value and is within the positive threshold range of the secondary amplitude limit of the pitch axis (the upper positive threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_FMAX , the lower positive threshold of the secondary amplitude limit of the pitch axis is θ P_FMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set as the lower positive threshold of the secondary amplitude limit of the pitch axis θ P_FMAX ; If the pitch axis control command θ P_FCMDIs a positive value and is outside the positive threshold range of the secondary amplitude limit of the pitch axis (the upper positive threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_FMAX , the lower positive threshold of the secondary amplitude limit of the pitch axis is θ P_FMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set to the processed pitch axis control command θ P_FCMD ; If the pitch axis control command θ P_FCMD is a negative value and is within the negative threshold range of the secondary amplitude limit of the pitch axis (the negative upper threshold of the secondary amplitude limit of the pitch axis is θ P_BMAX , the negative lower threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_BMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set as the negative upper threshold of the secondary amplitude limit of the pitch axis θ P_BMAX ; If the pitch axis control command θ P_FCMD is a negative value and is outside the negative threshold range of the secondary amplitude limit of the pitch axis (the negative upper threshold of the secondary amplitude limit of the pitch axis is θ P_BMAX , the negative lower threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_BMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set to the processed pitch axis control command θ P_FCMD .

[0036] Preferably, in step 12, the algorithm for neutral position processing is: if the pitch axis control command θ after secondary limiting P_CMD_L2 Within the empty travel threshold range of the pitch axis neutral position (the upper limit of the empty travel threshold is θ P_ZERO , the lower limit of the empty travel threshold is -θ P_ZERO ), send the pitch axis control command θ of CANFD / CAN bus P_CMD Set to zero; if the pitch axis control command θ P_CMD_L2 Outside the pitch axis neutral travel range (-θ P_ZBRO , +θ P_ZERO ), send the pitch axis control command θ of CANFD / CAN bus P_CMD After setting to secondary limit, the pitch axis control command θ P_CMD_L2 .

[0037] Preferably, step 12 is an optional option. If step 12 is not used, the pitch axis control instruction θ of the CANFD / CAN bus is sent. P_CMD That is the pitch axis control command θ after secondary limiting P_CMD_L2 .

[0038] The command control and processing method of the vertical axis of the technical solution of the present invention adopts the following steps:

[0039] Step 1: Set the task cycle T of the control and processing of the control system's control instructions p;

[0040] Step 2: collecting the output voltage signal Vert_Out of the vertical axis command sensor of the multi-axis manipulator;

[0041] Step 3: Collect the power supply voltage 4V_PS of the vertical axis command sensor of the multi-axis manipulator;

[0042] Step 4: Calculate the vertical axis angle θ based on the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS V ;

[0043] Step 5: According to the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS, the vertical axis transient self-monitoring result theta_V_VF is obtained through the vertical axis sensor self-monitoring algorithm;

[0044] Step 6: The two redundant channels of the control system exchange vertical axis data (angle value and transient self-monitoring result value) with each other through the inter-channel data link transmission (CCDL);

[0045] Step 7: During the inter-channel data link transmission (CCDL) process, the vertical axis CCDL monitoring and data processing algorithm is used to obtain the vertical axis angle θ of other channels used for voting processing. V1 And the vertical axis transient self-monitoring results theta_V1_VF of other channels;

[0046] Step 8: According to the vertical axis angle θ of this channel V 、Other channel vertical axis angle θ V1 , the vertical axis transient self-monitoring result theta_V_VF of this channel and the vertical axis transient self-monitoring result theta_V1_VF of other channels, and the two-channel command signals are voted to obtain the vertical axis control command voting value θ V_J And the transient vertical axis control command flag ThrVD;

[0047] Step 9: Vote value θ for vertical axis control command V_I Perform the limiting process to obtain the vertical axis control command voting value θ after limiting V_IL ;

[0048] Step 10: According to the transient vertical axis control command flag ThrVD and the vertical axis control command voting value θ after limiting V_IL , calculate the vertical axis control command θ V_FCMD And the vertical axis manipulation instruction flag FCSS_THrVD;

[0049] Step 11: Control the vertical axis after processing V_FCMDPerform secondary limiting processing to obtain the vertical axis control command θ after secondary limiting V_CMD_L2 ;

[0050] Step 12: Control the vertical axis after secondary limiting θ V_CMD_L2 Perform neutral position processing to obtain the vertical axis control command θ sent to the CANFD / CAN bus V_CMD .

[0051] Preferably, in step 2, the signal is collected in a fixed periodic manner, and the collection period is the same as the task period of the control and processing of the operating system's operating instructions.

[0052] Preferably, in step 3, the signal acquisition method is periodic acquisition, and the acquisition period is the same as the task period of the control and processing of the control system control command. The acquisition time of the power supply voltage 4V_PS and the acquisition time of the vertical axis sensor output voltage signal Vert_Out can be the same or different.

[0053] Preferably, in step 4, the vertical axis angle θ V The calculation formula is where θ V The unit of is °, the unit of Vert_Out and 4V_PS is V.

[0054] Preferably, in step five, the vertical axis sensor self-monitors, and the specific algorithm is: determine whether the vertical axis sensor output voltage signal Vert_Out exceeds the vertical axis self-monitoring threshold range (the upper threshold of the vertical axis self-monitoring threshold range is Vert_Thup, and the lower threshold is Ver_Thdown); if it exceeds the vertical axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the vertical axis transient self-monitoring result theta_V_VF is set to a fault.

[0055] Preferably, in step 7, the CCDL monitoring and data processing algorithm comprises the following specific steps:

[0056] S1, the interval time T after this channel sends data through CCDL I Afterwards, the transient fault state of the CCDL is detected;

[0057] S2. If a CCDL transient fault occurs and lasts for 5 beats, it is considered a permanent CCDL fault. The vertical axis angle θ of other channels obtained by this channel is V1 and the vertical axis transient self-monitoring result theta_V1_VF, are set to fail-safe values;

[0058] S3. If a CCDL transient fault occurs and does not last for 5 beats, the vertical axis angle θ of other channels obtained by this channel V1And the vertical axis transient self-monitoring result theta_V1_VF are both set to the normal value of the previous beat.

[0059] Preferably, in step eight, the two-channel command signal voting, the specific algorithm is: if the vertical axis angle θ of the channel V and the vertical axis angle θ of other channels V1 The absolute value of the difference is less than or equal to the vertical axis command threshold θ V_th If the vertical axis transient self-monitoring result theta_V_VF of this channel and the vertical axis transient self-monitoring result theta_V1_VF of other channels are both valid, then the vertical axis control command voting value θ V_I Set to (θ V +θ V1 ) / 2, the transient vertical axis control command flag ThrVD is set to be valid; otherwise, the vertical axis control command voting value θ V_I It is set to a fail-safe value and the transient vertical axis manipulation command flag ThrVD is set to invalid.

[0060] Preferably, in step nine, the vertical axis control command voting value θ V_I The algorithm for limiting processing is: if the vertical axis control command voting value θ V_I Exceeding the upper limit of vertical axis limit θ V_I_Hmitup , then the vertical axis control command voting value after limiting is θ V_IL Set as the upper limit of the vertical axis limit θ V_I_Hmitup ; If the vertical axis control command voting value θ V_I Exceeding the lower vertical axis limit θ V_I_Hmitdown , then the vertical axis control command voting value after limiting is θ V_IL Set as the vertical axis limit lower threshold θ V_I_Hmitdown ; Otherwise, the vertical axis control command voting value after limiting is θ V_IL Set as the vertical axis control command voting value θ V_I .

[0061] Preferably, in step 10, the vertical axis control instruction θ is obtained by calculation V_FCMD The method of determining the vertical axis control command flag bit FCSS_ThrVD is as follows: if the transient vertical axis control command flag bit ThrVD is invalid and lasts for more than 10 beats, it is determined that a permanent fault has occurred, the vertical axis control command flag bit FCSS_ThrVD is invalid, and the flag bit is not restored. The processed vertical axis control command θ V_FCMD is set to a fail-safe value; if the transient vertical axis control command flag ThrVD is invalid and does not last for more than 10 beats, the processed vertical axis control command θ V_FCMD Set to the vertical axis control command voting value θ after limiting V_ILIf the transient vertical axis control command flag ThrVD is valid and the vertical axis control command flag FCSS_ThrVD is valid, then the processed vertical axis control command θ V_FCMD Set to the vertical axis control command voting value θ after limiting V_IL ; If the transient vertical axis control command flag ThrVD is valid and the vertical axis control command flag FCSS_ThrVD is invalid, the processed vertical axis control command θ V_FCMD Set to fail-safe value.

[0062] Preferably, in step 11, the algorithm of the secondary limiting processing is: if the processed vertical axis control instruction θ V_FCMD Is positive and within the vertical axis secondary limit positive threshold range (vertical axis secondary limit positive upper threshold is 1.1*θ V_FMAX , the lower positive threshold of the secondary amplitude limit of the vertical axis is θ V_FMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set as the vertical axis secondary limiting positive lower threshold θ V_FMAX ; If the processed vertical axis control command θ V_FCMD Is positive and outside the vertical axis secondary limit positive threshold range (vertical axis secondary limit positive upper threshold is 1.1*θ V_FMAX , the lower positive threshold of the secondary amplitude limit of the vertical axis is θ V_FMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set to the processed vertical axis control command θ V_FCMD ; If the processed vertical axis control command θ V_FCMD is negative and within the negative threshold of the secondary amplitude limit of the vertical axis (the negative upper threshold of the secondary amplitude limit of the vertical axis is θ V_BMAX , the negative lower threshold of the secondary amplitude limit of the vertical axis is 1.1*θ V_BMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set as the negative upper threshold of the secondary amplitude limit of the vertical axis θ V_BMAX ; If the processed vertical axis control command θ V_FCMD It is negative and outside the negative threshold range of the secondary amplitude limit of the vertical axis (the negative upper threshold of the secondary amplitude limit of the vertical axis is 1.1*θ V_BMAX , the negative lower threshold of the secondary amplitude limit of the vertical axis is θ V_BMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set to the processed vertical axis control command θ V_FCMD .

[0063] Preferably, in step 12, the algorithm for neutral position processing is: if the vertical axis control instruction θ after secondary limiting V_CMD_L2 In the vertical axis neutral position empty travel threshold range (empty travel threshold upper threshold is θ V_ZERO , the lower threshold of the empty travel threshold is θV_ZERO ), send the vertical axis control command θ of CANFD / CAN bus V_CMD Set to zero; if the vertical axis control command θ after secondary limiting V_CMD_L2 Outside the vertical axis neutral position travel range (-θ V_ZERO , +θ V_ZERO ), send the vertical axis control command θ of CANFD / CAN bus V_CMD After setting to secondary limit, the vertical axis control command θ V_CMD_L2 .

[0064] Preferably, step 12 is optional. If step 12 is not used, the vertical axis control instruction θ of the CANFD / CAN bus is sent. V_CMD That is, the vertical axis control command θ after secondary limiting V_CMD_L2 .

[0065] The advantages of the present invention are:

[0066] (1) High fault detection rate. The present invention adopts multiple self-monitoring methods, a complete CCDL monitoring algorithm, two-channel voting and other detection measures to greatly improve the fault detection rate of the four-axis command of the multi-axis control system and improve the credibility of the sent four-axis control command signal. (2) High safety. The present invention combines CCDL monitoring and self-monitoring and uses a strict voting method to greatly reduce the risk of outputting erroneous command signals. At the same time, it adopts multiple limiting processing to prevent large command output caused by command overflow, thereby improving the control safety of the aircraft; (3) High reliability. The present invention adopts a variety of transient fault marks and permanent fault marks. It can not only record occasional faults through transient fault marks, but also use permanent fault marks as the final judgment conditions, avoiding false alarms caused by interference and improving the reliability of command control and processing; (4) High control accuracy. The present invention compensates the collected power supply voltage into the calculation formula of the four-axis angle, reducing the interference caused by the sensor power supply fluctuation, so that the calculated four-axis angle has higher accuracy; (5) Good applicability. The present invention adopts a neutral position processing algorithm, which can solve the problem of inconsistent neutral position output command signals caused by manufacturing errors in mass-produced control device products, effectively prevent the output of non-control command signals, and has good product applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 It is a flow chart of the pitch axis control command control and processing method.

[0068] Figure 2 It is a flow chart of the vertical axis manipulation command control and processing method. DETAILED DESCRIPTION

[0069] Embodiment 1

[0070] A multi-axis control system command control and processing method, the control command control and processing method of the three axes of pitch, roll and yaw of the multi-axis control device are the same.

[0071] like Figure 1 As shown, taking the pitch axis as an example, the technical solution of the present invention is described, and the following steps are taken:

[0072] Step 1: Set the task cycle T of the control and processing of the control system's control instructions p (T p =10ms);

[0073] Step 2: synchronously collect the sine-end output voltage signal Pitch_Sin and the cosine-end output voltage signal Pitch_Cos of the pitch axis command sensor of the multi-axis manipulator, ensuring that the two output voltage signals are collected at the same time;

[0074] Step 3, collecting the power supply voltage 4V_PS of the pitch axis command sensor of the multi-axis manipulator;

[0075] Step 4: Calculate the pitch axis angle θ based on the sine terminal output voltage signal Pitch_Sin and cosine terminal output voltage signal Pitch_Cos of the pitch axis command sensor and the power supply voltage 4V_PS P ;

[0076] Step 5: According to the sine terminal output voltage signal Pitch_Sin and the cosine terminal output voltage signal Pitch_Cos of the pitch axis command sensor and the power supply voltage 4V_PS, the pitch axis sensor self-monitoring algorithm is used to obtain the pitch axis transient self-monitoring result theta_P_VF;

[0077] Step 6: The two redundant channels of the control system exchange pitch axis data (angle value and transient self-monitoring result value) with each other through the inter-channel data link (CCDL);

[0078] Step 7: During the inter-channel data link transmission (CCDL) process, the pitch axis angle θ of other channels used for voting is obtained through the pitch axis CCDL monitoring and data processing algorithm. P1 and the pitch axis transient self-monitoring results theta_P1_VF of other channels;

[0079] Step 8: According to the pitch axis angle θ of this channel P 、Other channel pitch axis angle θ P1 , the pitch axis transient self-monitoring result theta_P_VF of this channel and the pitch axis transient self-monitoring result theta_P1_VF of other channels, and the two-channel command signals are voted to obtain the pitch axis control command voting value θ P_JAnd the transient pitch axis control command flag PthVD;

[0080] Step 9: Vote on the pitch axis control command value θ P_J Perform the limiting process to obtain the pitch axis control command voting value θ after limiting P_JL ;

[0081] Step 10: According to the instantaneous pitch axis control command flag bit PthVD and the pitch axis control command voting value θ after limiting P_IL , calculate the pitch axis control command θ P_FCMD And the pitch axis control command flag FCSS_PthVD;

[0082] Step 11: The pitch axis control command θ after processing P_FCMD Perform secondary limiting processing to obtain the pitch axis control command θ after secondary limiting P_CMD_L2 ;

[0083] Step 12: After the secondary limit, the pitch axis control command θ P_CMD_L2 Perform neutral position processing to obtain the pitch axis control command θ sent to the CANFD / CAN bus P_CMD .

[0084] Preferably, in step 2, the signal is collected in a fixed periodic manner, and the collection period is the same as the task period of the control and processing of the operating system's operating instructions.

[0085] Preferably, in step 3, the signal acquisition method is periodic acquisition, and the acquisition period is the same as the task period of the control and processing of the control system control command. The acquisition time of the power supply voltage 4V_PS can be the same as or different from the acquisition time of the sine end output voltage signal Pitch_Sin and the cosine end output voltage signal Pitch_Cos.

[0086] Preferably, in step 4, the pitch axis angle θ P The calculation formula is Where pi is the circumference of a circle, θ P The unit of is °, the unit of Pitch_Sin, Pitch_Cos and 4V_PS is V.

[0087] Preferably, in step 5, the pitch axis sensor self-monitoring algorithm comprises the following specific steps:

[0088] S1. Determine whether the output voltage signal Pitch_Sin of the sinusoidal end of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range (the upper threshold value of the pitch axis self-monitoring threshold range is 93%*4V_PS, and the lower threshold value is 70%4V_PS). If it exceeds the pitch axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault ("1" means a fault);

[0089] S2, determine whether the cosine terminal output voltage signal Pitch_Cos of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range (the upper threshold value of the pitch axis self-monitoring threshold range is 93%*4V_PS, and the lower threshold value is 7%*4V_PS). If it exceeds the pitch axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault ("1" means a fault);

[0090] S3, determine the combination of the sine terminal output voltage signal Pitch_Sin and the cosine terminal output voltage signal Pitch_Cos of the pitch axis sensor Whether it exceeds the pitch axis combination threshold range (the upper threshold value of the pitch axis combination threshold range is 0.14*4V_PS 2 , the lower threshold is 0.011*4V_PS 2 If it exceeds the pitch axis combination threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault ("1" means a fault).

[0091] Preferably, in step 7, the CCDL monitoring and data processing algorithm comprises the following specific steps:

[0092] S1, the interval time T after this channel sends data through CCDL I (T I =3ms), detect the transient fault state of CCDL;

[0093] S2. If a CCDL transient fault occurs and lasts for 5 beats, it is considered a permanent CCDL fault. The pitch axis angle θ of other channels obtained by this channel P1 The pitch axis transient self-monitoring results theta_P1_VF of other channels are all set to fail-safe values ​​(the pitch axis angle θ P1 The fail-safe value is 0°);

[0094] S3. If a CCDL transient fault occurs and does not last for 5 beats, the pitch axis angle θ of other channels obtained by this channel P1 The pitch axis transient self-monitoring results theta_P1_VF of other channels are all set to the normal value of the previous beat.

[0095] Preferably, in step eight, the two-channel command signal voting, the specific algorithm is: if the pitch axis angle θ of the channel P and the pitch axis angle θ of other channels P1 The absolute value of the difference is less than or equal to the pitch axis command threshold θ P_th (θ P_th =2°) and the pitch axis transient self-monitoring result theta_P_VF of this channel and the pitch axis transient self-monitoring result theta_P1_VF of other channels are both valid (“1” is valid), then the pitch axis control command voting value θ P_I Set to (θ P +θ P1 ) / 2, the instantaneous pitch axis control command flag bit PthVD is set to be valid (“1” is valid); otherwise, the pitch axis control command voting value θ P_I Set to fail-safe value θ P_I_S (θ P_I_S =0°), the instantaneous pitch axis control command flag PthVD is set to invalid ("0" means invalid).

[0096] Preferably, in step nine, the pitch axis control command voting value θ P_I The algorithm for limiting processing is: if the pitch axis control command voting value θ P_I Exceeding the upper limit of the pitch axis P_I_Hmitup (θ P_I_Hmitup =30°), then the pitch axis control command voting value θ after limiting P_IL Set as the upper limit of the pitch axis limit θ P_I_Hmitup ; If the pitch axis control command voting value θ P_I Exceeding the lower limit of the pitch axis θ P_I_Hmitdown (θ P_I_Hmitdown =-30°), then the pitch axis control command voting value θ after limiting P_IL Set as the pitch axis limit lower threshold θ P_I_Hmitdown ; Otherwise, the pitch axis control command voting value θ after limiting P_IL Set as the pitch axis control command voting value θ P_I .

[0097] Preferably, in step 10, the pitch axis control instruction θ is obtained by calculation P_FCMD The method for determining the pitch axis control command flag bit FCSS_PthVD is as follows: if the transient pitch axis control command flag bit PthVD is invalid ("0" means invalid) and lasts for more than 10 beats, it is determined that a permanent fault has occurred, the pitch axis control command flag bit FCSS_PthVD is invalid ("0" means invalid), and the flag bit is not restored. The pitch axis control command θ P_FCMD= ... P_FCMD Set to the pitch axis control command voting value θ after the limit P_IL If the instantaneous pitch axis control command flag bit PthVD is valid (“1” is valid) and the pitch axis control command flag bit FCSS_PthVD is valid (“1” is valid), then the processed pitch axis control command θ P_FCMD Set to the pitch axis control command voting value θ after the limit P_IL If the instantaneous pitch axis control command flag bit PthVD is valid (“1” is valid) and the pitch axis control command flag bit FCSS_PthVD is invalid (“0” is invalid), then the processed pitch axis control command θ P_FCMD Set to fail-safe value (fail-safe value is 0°).

[0098] Preferably, in step 11, the algorithm of the secondary limiting processing is: if the pitch axis control instruction θ after processing P_FCMD Is a positive value and is within the positive threshold range of the secondary amplitude limit of the pitch axis (the upper positive threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_FMAX (θ P_FMAX =20°), the lower positive threshold of the secondary amplitude limit of the pitch axis is θ P_FMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set as the lower positive threshold of the secondary amplitude limit of the pitch axis θ P_FMAX ; If the pitch axis control command θ P_FCMD Is a positive value and is outside the positive threshold range of the secondary amplitude limit of the pitch axis (the upper positive threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_FMAX , the lower positive threshold of the secondary amplitude limit of the pitch axis is θ P_FMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set to the processed pitch axis control command θ P_FCMD ; If the pitch axis control command θ P_FCMD is a negative value and is within the negative threshold range of the secondary amplitude limit of the pitch axis (the negative upper threshold of the secondary amplitude limit of the pitch axis is θ P_BMAX (θ P_BMAX =-20°), the lower negative threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_BMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set as the negative upper threshold of the secondary amplitude limit of the pitch axis θ P_BMAX ; If the pitch axis control command θ P_FCMD It is a negative value and is outside the negative threshold range of the secondary amplitude limit of the pitch axis (the negative upper threshold of the secondary amplitude limit of the pitch axis is 1.1*θ P_BMAX , the negative lower threshold of the secondary amplitude limit of the pitch axis is θP_BMAX ), then the pitch axis control command θ after secondary limiting P_CMD_L2 Set to the processed pitch axis control command θ P_FCMD .

[0099] Preferably, in step 12, the algorithm for neutral position processing is: if the pitch axis control command θ after secondary limiting P_CMD_L2 Within the empty travel threshold range of the pitch axis neutral position (the upper limit of the empty travel threshold is θ P_ZERO (θ P_ZERO =1°), the lower limit of the empty travel threshold is -θ P_ZERO ), send the pitch axis control command θ of CANFD / CAN bus P_CMD Set to zero; if the pitch axis control command θ P_CMD_L2 Outside the pitch axis neutral travel range (-θ P_ZERO , +θ P_ZERO ), send the pitch axis control command θ of CANFD / CAN bus P_CMD After setting to secondary limit, the pitch axis control command θ P_CMD_L2 .

[0100] Preferably, step 12 is an optional option. If step 12 is not used, the pitch axis control instruction θ of the CANFD / CAN bus is sent. P_CMD That is the pitch axis control command θ after secondary limiting P_CMD_L2 .

[0101] Embodiment 2

[0102] like Figure 2 As shown, taking the vertical axis as an example, the technical solution of the present invention is described, and the following steps are taken:

[0103] Step 1: Set the task cycle T of the control and processing of the control system's control instructions p ;

[0104] Step 2: collecting the output voltage signal Vert_Out of the vertical axis command sensor of the multi-axis manipulator;

[0105] Step 3: Collect the power supply voltage 4V_PS of the vertical axis command sensor of the multi-axis manipulator;

[0106] Step 4: Calculate the vertical axis angle θ based on the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS V ;

[0107] Step 5: According to the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS, the vertical axis transient self-monitoring result theta_V_VF is obtained through the vertical axis sensor self-monitoring algorithm;

[0108] Step 6: The two redundant channels of the control system exchange vertical axis data (angle value and transient self-monitoring result value) with each other through the inter-channel data link transmission (CCDL);

[0109] Step 7: During the inter-channel data link transmission (CCDL) process, the vertical axis CCDL monitoring and data processing algorithm is used to obtain the vertical axis angle θ of other channels used for voting processing. V1 And the vertical axis transient self-monitoring results theta_V1_VF of other channels;

[0110] Step 8: According to the vertical axis angle θ of this channel V 、Other channel vertical axis angle θ V1 , the vertical axis transient self-monitoring result theta_V_VF of this channel and the vertical axis transient self-monitoring result theta_V1_VF of other channels, and the two-channel command signals are voted to obtain the vertical axis control command voting value θ V_I And the transient vertical axis control command flag ThrVD;

[0111] Step 9: Vote value θ for vertical axis control command V_I Perform the limiting process to obtain the vertical axis control command voting value θ after limiting V_IL ;

[0112] Step 10: According to the transient vertical axis control command flag ThrVD and the vertical axis control command voting value θ after limiting V_IL , calculate the vertical axis control command θ V_FCMD And the vertical axis control command flag FCSS_ThrVD;

[0113] Step 11: Control the vertical axis after processing V_FCMD Perform secondary limiting processing to obtain the vertical axis control command θ after secondary limiting V_CMD_L2 ;

[0114] Step 12: Control the vertical axis after secondary limiting θ V_CMD_L2 Perform neutral position processing to obtain the vertical axis control command θ sent to the CANFD / CAN bus V_CMD .

[0115] Preferably, in step 2, the signal is collected in a fixed periodic manner, and the collection period is the same as the task period of the control and processing of the operating system's operating instructions.

[0116] Preferably, in step 3, the signal acquisition method is periodic acquisition, and the acquisition period is the same as the task period of the control and processing of the control system control command. The acquisition time of the power supply voltage 4V_PS and the acquisition time of the vertical axis sensor output voltage signal Vert_Out can be the same or different.

[0117] Preferably, in step 4, the vertical axis angle θ V The calculation formula is where θ V The unit of is °, the unit of Vert_Out and 4V_PS is V.

[0118] Preferably, in step five, the vertical axis sensor self-monitors, and the specific algorithm is: determine whether the vertical axis sensor output voltage signal Vert_Out exceeds the vertical axis self-monitoring threshold range (the upper threshold of the vertical axis self-monitoring threshold range is Vert_Thup=3.9V, and the lower threshold Ver_thdown=0.1V); if it exceeds the vertical axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient, and the vertical axis transient self-monitoring result theta_V_VF is set to a fault ("1" is a fault).

[0119] Preferably, in step 7, the CCDL monitoring and data processing algorithm comprises the following specific steps:

[0120] S1, the interval time T after this channel sends data through CCDL I (T I =3ms), detect the transient fault state of CCDL;

[0121] S2. If a CCDL transient fault occurs and lasts for 5 beats, it is considered a permanent CCDL fault. The vertical axis angle θ of other channels obtained by this channel is V1 and the vertical axis transient self-monitoring result theta_V1_VF, are set to fail-safe values ​​(vertical axis angle θ V1 The fail-safe value is 0°);

[0122] S3. If a CCDL transient fault occurs and does not last for 5 beats, the vertical axis angle θ of other channels obtained by this channel V1 And the vertical axis transient self-monitoring result theta_V1_VF are both set to the normal value of the previous beat.

[0123] Preferably, in step eight, the two-channel command signal voting, the specific algorithm is: if the vertical axis angle θ of the channel V and the vertical axis angle θ of other channels V1 The absolute value of the difference is less than or equal to the vertical axis command threshold θ V_th (θ V_th=4°) and the vertical axis transient self-monitoring result theta_V_VF of this channel and the vertical axis transient self-monitoring result theta_V1_VF of other channels are both valid (“1” is valid), then the vertical axis control command voting value θ V_I Set to (θ V +θ V1 ) / 2, the transient vertical axis control command flag ThrVD is set to valid ("1" is valid); otherwise, the vertical axis control command voting value θ V_I It is set to a fail-safe value (the fail-safe value is 0°), and the transient vertical axis manipulation instruction flag ThrVD is set to invalid ("0" means invalid).

[0124] Preferably, in step nine, the vertical axis control command voting value θ V_I The algorithm for limiting processing is: if the vertical axis control command voting value θ V_I Exceeding the upper limit of the vertical axis limit θ V_I_Hmitup (θ V_I_Hmitup =30°), then the vertical axis control command voting value after limiting is θ V_IL Set as the upper limit of the vertical axis limit θ V_I_Hmitup ; If the vertical axis control command voting value θ V_I Exceeding the lower vertical axis limit θ V_I_Hmitdown (θ V_I_Hmitdown =-30°), then the vertical axis control command voting value after limiting is θ V_IL Set as the vertical axis limit lower threshold θ V_I_Hmidown ; Otherwise, the vertical axis control command voting value after limiting is θ V_IL Set as the vertical axis control command voting value θ V_I .

[0125] Preferably, in step 10, the vertical axis control instruction θ is obtained by calculation V_FCMD The method of determining the vertical axis control command flag bit FCSS_ThrVD is as follows: if the transient vertical axis control command flag bit ThrVD is invalid ("0" means invalid) and lasts for more than 10 beats, it is determined that a permanent fault has occurred, the vertical axis control command flag bit FCSS_THrVD is invalid ("0" means invalid), and the flag bit is not restored. The processed vertical axis control command θ V_FCMD Set to a fail-safe value (the fail-safe value is 0°); if the transient vertical axis control command flag ThrVD is invalid ("0" means invalid) and does not last for more than 10 beats, the processed vertical axis control command θ V_FCMD Set to the vertical axis control command voting value θ after limiting V_ILIf the transient vertical axis control command flag ThrVD is valid ("1" is valid) and the vertical axis control command flag FCSS_ThrVD is valid ("1" is valid), then the processed vertical axis control command θ V_PCMD Set to the vertical axis control command voting value θ after limiting V_IL ; If the transient vertical axis control command flag ThrVD is valid ("1" is valid) and the vertical axis control command flag PCSS_ThrVD is invalid ("0" is invalid), then the processed vertical axis control command θ V_FCMD Set to fail-safe value (fail-safe value is 0°).

[0126] Preferably, in step 11, the algorithm of the secondary limiting processing is: if the processed vertical axis control instruction θ V_FCMD Is positive and within the vertical axis secondary limit positive threshold range (vertical axis secondary limit positive upper threshold is 1.1*θ V_FMAX (θ V_FMAX =25°), the lower positive threshold of the secondary amplitude limit of the vertical axis is θ V_FMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set as the vertical axis secondary limiting positive lower threshold θ V_FMAX ; If the processed vertical axis control command θ V_FCMD It is positive and outside the vertical axis secondary amplitude limiting positive threshold range (the vertical axis secondary amplitude limiting positive upper threshold is 1.1θ V_FMAX , the lower positive threshold of the secondary amplitude limit of the vertical axis is θ V_FMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set to the processed vertical axis control command θ V_FCMD ; If the processed vertical axis control command θ V_FCMD is negative and within the negative threshold of the secondary amplitude limit of the vertical axis (the negative upper threshold of the secondary amplitude limit of the vertical axis is θ V_BMAX (θ V_FMAX =-25°), the negative lower threshold of the secondary amplitude limit of the vertical axis is 1.1*θ V_BMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set as the negative upper threshold of the secondary amplitude limit of the vertical axis θ V_BMAX ; If the processed vertical axis control command θ V_FCMD is a negative value and is outside the negative threshold range of the secondary amplitude limit of the vertical axis (the negative upper threshold of the secondary amplitude limit of the vertical axis is θ V_BMAX , the negative lower threshold of the secondary amplitude limit of the vertical axis is 1.1*θ V_BMAX ), then the vertical axis control command θ after secondary limiting V_CMD_L2 Set to the processed vertical axis control command θ V_FCMD .

[0127] Preferably, in step 12, the algorithm for neutral position processing is: if the vertical axis control instruction θ after secondary limiting V_CMD_L2 In the vertical axis neutral position empty travel threshold range (empty travel threshold upper threshold is θ V_ZERO (θ V_ZERO =1°), the lower limit of the empty travel threshold is -θ V_ZERO ), send the vertical axis control command θ of CANFD / CAN bus V_CMD Set to zero; if the vertical axis control command θ after secondary limiting V_CMD_L2 Outside the vertical axis neutral position travel range (-θ V_ZERO , +θ V_ZERO ), send the vertical axis control command θ of CANFD / CAN bus V_CMD After setting to secondary limit, the vertical axis control command θ V_CMD_L2 .

[0128] Preferably, step 12 is optional. If step 12 is not used, the vertical axis control instruction θ of the CANFD / CAN bus is sent. V_CMD That is, the vertical axis control command θ after secondary limiting V_CMD_L2 .

[0129] The above-mentioned embodiments only express the preferred implementation modes of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present invention. It should be pointed out that, for a person skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention patent shall be subject to the attached claims.

Claims

1. A multi-axis control system command control and processing method, characterized in that: The steps include: Step 1: Set the task cycle T of the control and processing of the control system's control instructions p ; Step 2: collecting the output voltage signal of the control axis command sensor; Step 3: Collect the power supply voltage 4V_PS of the control axis command sensor; Step 4: Calculate the control axis angle according to the output voltage signal of the control axis command sensor and the power supply voltage 4V_PS; Step 5: According to the output voltage signal of the pitch axis command sensor and the power supply voltage 4V_PS, the control axis sensor self-monitoring algorithm is used to obtain the control axis transient self-monitoring result; Step 6: The two redundant channels of the control system exchange the control axis data with each other through the inter-channel data link transmission; Step 7: During the data link transmission between channels, the control angles of other channels and the transient self-monitoring results of the control axes of other channels used for voting are obtained through CCDL monitoring and data processing algorithms; Step 8, voting is performed on the command signals of the two channels according to the control axis angle of the current channel, the control axis angles of other channels, the transient self-monitoring result of the control axis of the current channel and the transient self-monitoring result of the control axis of other channels, and the voting value of the control axis control command and the transient control axis control command flag are obtained; Step nine, performing a limiting process on the control axis control command voting value to obtain the control axis control command voting value after limiting; Step 10, calculating the control axis control instruction and the control axis control instruction flag according to the transient control axis control instruction flag and the control axis control instruction voting value after limiting; Step 11, performing secondary limiting processing on the processed control axis control command to obtain the control axis control command after secondary limiting; Step 12: Perform neutral position processing on the control shaft control command after the secondary limiting to obtain the control shaft control command sent to the CANFD / CAN bus.

2. The method according to claim 1, characterized in that: The control axes include: pitch axis, roll axis, yaw axis and vertical axis. The control and processing of the control commands of the pitch axis, roll axis and yaw axis are the same.

3. The method according to claim 2, characterized in that: Control and processing of pitch axis control commands: In step 2, the sine voltage signal Pitch_Sin and the cosine voltage signal Pitch_Cos output by the pitch axis command sensor are synchronously collected; In step 4, the pitch axis angle θP is calculated according to the sine voltage signal Pitch_Sin, the cosine voltage signal Pitch_Cos and the power supply voltage 4V_PS; In step five, according to the sinusoidal voltage signal Pitch_Sin, the cosine voltage signal Pitch_Cos and the power supply voltage 4V_PS, the pitch axis sensor self-monitoring algorithm is used to obtain the pitch axis transient self-monitoring result theta_P_VF.

4. The method according to claim 3, characterized in that: In step 4, the pitch axis angle θ p The calculation formula is as follows: Where pi is the circumference of a circle, θ p The unit of is °, the unit of Pitch_sin, Pitch_Cos and 4V_PS is V.

5. The method according to claim 4, characterized in that: In step 5, the pitch axis sensor self-monitoring algorithm process is as follows: S51, determine whether the sinusoidal voltage signal Pitch_Sin of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range, the upper threshold value is 93%*4V_PS, and the lower threshold value is 7%*4V_PS. If it exceeds the pitch axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault; S52, determining whether the cosine voltage signal Pitch_Cos of the pitch axis sensor exceeds the pitch axis self-monitoring threshold range; if so, determining that a self-monitoring transient fault occurs in the pitch axis sensor, and setting the pitch axis transient self-monitoring result theta_P_VF as a fault; S53: Determine the combination of the sine voltage signal Pitch_Sin and the cosine voltage signal Pitch_Cos of the pitch axis sensor Whether it exceeds the pitch axis combination threshold range. The upper threshold value of the pitch axis combination threshold range is 0.14*4V_PS 2 , the lower threshold is 0.011*4V_PS 2 If it exceeds the pitch axis combination threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the pitch axis transient self-monitoring result theta_P_VF is set to a fault.

6. The method according to claim 2, characterized in that: For the control and processing of vertical axis operation instructions: In step 2, the output voltage signal Vert_Out of the vertical axis command sensor is collected; In step 4, the vertical axis angle θ is calculated based on the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS. V ; In step five, according to the output voltage signal Vert_Out of the vertical axis command sensor and the power supply voltage 4V_PS, the vertical axis transient self-monitoring result theta_V_VF is obtained through the vertical axis sensor self-monitoring algorithm.

7. The method according to claim 6, characterized in that: In step 4, the vertical axis angle θ V The calculation formula is as follows: where θ V The unit of is °, the unit of Vert_Out and 4V_PS is V.

8. The method according to claim 7, characterized in that: In step 5, the vertical axis sensor self-monitoring algorithm process is as follows: Determine whether the vertical axis sensor output voltage signal Vert_Out exceeds the vertical axis self-monitoring threshold range. The upper threshold of the vertical axis self-monitoring threshold range is Vert_Thup, and the lower threshold is Vert_Thdown. If it exceeds the vertical axis self-monitoring threshold range, it is determined that the pitch axis sensor has a self-monitoring transient fault, and the vertical axis transient self-monitoring result theta_V_VF is set to a fault.

9. The method according to claim 1, characterized in that: In step 7, the CCDL monitoring and data processing algorithm process is as follows: S71, the interval time T after this channel sends data through CCDL I Afterwards, the transient fault state of the CCDL is detected; S72, if a CCDL transient fault occurs and lasts for 5 beats, it is considered that the CCDL has a permanent fault, and the control axis angles of other channels and the transient self-monitoring results of the control axes of other channels obtained by this channel are set to fail-safe values; S73. If a CCDL transient fault occurs and does not last for 5 beats, the control axis angles of other channels obtained by this channel and the transient self-monitoring results of the control axes of other channels are set to the normal values ​​of the previous beat.

10. The method according to claim 9, characterized in that: In step eight, the voting process is as follows: If the control axis angle θ of this channel P and other channels manipulate the axis angle θ P1 The absolute value of the difference is less than or equal to the control axis command threshold θ P_th If the control axis transient self-monitoring result theta_P_VF of this channel and the control axis transient self-monitoring result theta_P1_VF of other channels are both valid, then the control axis control command voting value θ P_J Set to (θ P +θ P1 ) / 2, the transient control axis control instruction flag bit PthVD is set to be valid; otherwise, the control axis control instruction voting value θ P_J Set to fail-safe value θ P_J_S , the transient control axis control instruction flag PthVD is set to invalid.

Images