A method and system for adjusting the position of a loading arm based on the fusion of state parameters

Abstract

This invention belongs to the field of automatic control technology, specifically relating to a method and system for loading arm position adjustment control based on state parameter fusion. The method includes: acquiring the actual position of the motion axis, the actual speed of the motion axis, and the original target position; determining the direction of the operation and forming a position compensation range; determining the effective position boundary based on the actual position of the telescopic motion axis and forming the allowable range of the final position target; determining a compensation-compatible nominal position domain based on the allowable range of the final position target and the position compensation range; restricting the original target position within the compensation-compatible nominal position domain to form a restricted nominal position target; and superimposing the current cumulative position compensation amount to form the final servo position target. This invention can reduce the repeated restriction of position commands near the boundary and the frequent deceleration, pauses, or reverse adjustments of the motion axis, improving the continuity, stability, and controllability of the loading arm position adjustment process.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology. More specifically, this invention relates to a method and system for adjusting the position of an arm-mounted loading arm based on state parameter fusion. Background Technology

[0002] Loading arms are used in fluid handling operations to connect fixed pipelines to mobile receiving equipment such as tank trucks and cargo trucks. To adapt to changes in vehicle parking positions and interface positions, automated loading arms are typically equipped with multiple rotary and telescopic axes. Servo drive mechanisms drive these axes to complete deployment, docking, and position adjustment, and perform closed-loop control based on target positions and position feedback.

[0003] Existing loading arm position control typically sets mechanical limits, software limits, or position command limits for each motion axis to prevent exceeding the mechanical travel or the permissible operating range on site. Simultaneously, to reduce backlash, connection clearance, and position response lag during reversal, position compensation or position correction mechanisms can be implemented, with the compensation amount added to the position command. Position limits and position compensation are executed separately.

[0004] However, loading arms are long cantilever, multi-joint motion devices. The position range that each motion axis can execute is not only limited by the fixed mechanical stroke, but also varies with the extension / retraction state, actual position, and movement speed. When a motion axis approaches the position boundary and is still moving in the direction of the boundary, even if it has not crossed the boundary, there may be insufficient space for deceleration. At the same time, the compensation amount after reversal may continue to change, causing the final position target to deviate. This means that the target that currently meets the position boundary requirements may still exceed the position range that the loading arm can currently execute after subsequent compensation is added. This causes the position limiting mechanism to intervene again and repeatedly truncate or correct the position command, resulting in frequent deceleration, pauses, or reverse adjustments of the motion axis near the boundary. In severe cases, it may even trigger limit protection and interrupt the current position adjustment process. Summary of the Invention

[0005] To address the technical problem of incoordination between the positional limitations near the edge of the loading arm and the subsequent continuous positional compensation, the present invention provides solutions in the following aspects.

[0006] In a first aspect, the present invention provides a method for adjusting the position of an arm-mounted loading arm based on state parameter fusion, comprising:

[0007] Obtain the actual position, actual speed and original target position of each motion axis; determine the service reversal based on the change of the original target position between continuous control cycles.

[0008] When the business changes direction, the final value of the current location compensation is determined, so that the current cumulative location compensation amount changes monotonically from zero to the final value of the current location compensation in each cycle, and the minimum and maximum values ​​of the current cumulative location compensation amount and the final value of the current location compensation are used as the lower boundary and upper boundary of the location compensation range, respectively.

[0009] The negative and positive effective position boundaries of the corresponding motion axis are determined based on the actual position of the telescopic motion axis, and the final position target allowable range is formed based on the actual position of the motion axis, the actual speed of the motion axis, the negative effective position boundary, and the positive effective position boundary.

[0010] The compensation-compatible nominal position domain is determined based on the allowable range of the final position target and the position compensation range, so that the position value formed by superimposing any cumulative position compensation amount within the position compensation range on the nominal position within the compensation-compatible nominal position domain is within the allowable range of the final position target.

[0011] When the compensation-compatible nominal position domain is not empty, the original business target position is restricted to the compensation-compatible nominal position domain to form a restricted nominal position target. The current cumulative position compensation is added to the restricted nominal position target to form the final servo position target, and the final servo position target is used as the position target input of the corresponding servo drive mechanism.

[0012] Preferably, determining the service reversal based on the change in the original service target location between continuous control cycles includes:

[0013] The effective control period is defined as the control period in which the actual position of the motion axis, the actual speed of the motion axis, and the original business target position are all valid data. In the first effective control period after the control task is started, the current original business target position is saved as the initial original business target position, and no business reversal is determined.

[0014] Starting from the next continuous effective control cycle, the current original business target position is compared with the original business target position of the previous effective control cycle. When the current original business target position increases, the business movement direction is determined to be positive. When the current original business target position decreases, the business movement direction is determined to be negative. When the current original business target position does not change, no new business movement direction is formed, and the non-zero business movement direction formed in the most recent cycle is maintained.

[0015] When a non-zero original business target location changes for the first time, the corresponding business movement direction is saved as the initial business movement direction. Subsequently, when the currently formed non-zero business movement direction is opposite to the most recently saved non-zero business movement direction, a business reversal is determined, and the currently formed non-zero business movement direction is saved as the new business movement direction.

[0016] Preferably, determining the final value of the position compensation during the service reversal includes:

[0017] Based on the direction of change of the motion axis that has undergone a service reversal and the original service target position, the total position compensation amount is read from the device during the debugging phase for the corresponding service reversal situation. The total position compensation amount is read as the final position compensation value for this time. The total position compensation amount is the cumulative additional position amount required to make the drive side cross the reversal space and drive the output end of the motion axis to generate a position change in the direction after the reversal after the service reversal occurs.

[0018] Preferably, determining the final value of the position compensation during the service reversal includes:

[0019] Move the motion axis under test at low speed in one direction. After the output position of the motion axis changes continuously, change the driving direction and record the driving side position when the driving direction is changed. Continue to move in the changed driving direction. When the output position of the motion axis first produces a position change that exceeds the position feedback resolution in the changed direction, record the driving side position again.

[0020] The difference between the two recorded drive-side positions is converted into the position difference of the corresponding motion axis output end, and the position difference is assigned a positive or negative sign according to the changed motion direction. The resulting signed position quantity is used as the total position compensation for the corresponding service reversal situation and saved.

[0021] When a service is switched, the total position compensation saved for the corresponding service switching situation is read based on the motion axis of the service switching and the direction of change of the original service target position. The total position compensation read is then used as the final position compensation value for this service switching.

[0022] Preferably, determining the negative and positive effective position boundaries of the corresponding motion axis based on its actual position includes:

[0023] Multiple calibrated telescopic positions are set within the allowable operating range of the telescopic motion shaft. Under each calibrated telescopic position, the mechanical allowable position range and the field space allowable position range of the corresponding motion shaft are determined. The maximum value between the lower boundary of the mechanical allowable position range and the lower boundary of the field space allowable position range is taken as the negative effective position boundary of the corresponding calibrated telescopic position. The minimum value between the upper boundary of the mechanical allowable position range and the upper boundary of the field space allowable position range is taken as the positive effective position boundary of the corresponding calibrated telescopic position. The correspondence between each calibrated telescopic position and the negative and positive effective position boundaries is saved.

[0024] When the actual position of the telescopic motion axis is the same as a certain calibrated telescopic position, the negative and positive effective position boundaries corresponding to that calibrated telescopic position are used. When the actual position of the telescopic motion axis is between two adjacent calibrated telescopic positions, the negative effective position boundaries corresponding to the two adjacent calibrated telescopic positions are linearly interpolated according to the proportion of the actual position of the telescopic motion axis between the two adjacent calibrated telescopic positions, and the positive effective position boundaries corresponding to the two adjacent calibrated telescopic positions are linearly interpolated to obtain the negative and positive effective position boundaries of the current control cycle.

[0025] Preferably, the step of forming the final position target allowable range based on the actual position of the motion axis, the actual velocity of the motion axis, the effective position boundary in the negative direction, and the effective position boundary in the positive direction includes:

[0026] The difference between the effective position boundary in the positive direction and the actual position of the motion axis is taken as the remaining position margin in the positive direction, and the difference between the actual position of the motion axis and the effective position boundary in the negative direction is taken as the remaining position margin in the negative direction.

[0027] Based on the actual speed of the motion axis and the predetermined deceleration amplitude in the corresponding direction of motion, a deceleration reserve position margin is formed in the corresponding direction. The deceleration reserve position margin includes the position amount corresponding to the motion axis continuing to move at the current actual speed during the period from the current state acquisition to the next control cycle to re-form the position target, and the position amount required to reduce the current actual speed to zero.

[0028] The deceleration reserve position margin for the corresponding direction is subtracted from the remaining position margin in the positive direction and the remaining position margin in the negative direction, respectively. If the result after subtraction is greater than zero, the result is taken as the available position margin in the corresponding direction. If the result after subtraction is less than or equal to zero, the available position margin in the corresponding direction is set to zero.

[0029] Based on the actual position of the motion axis, the lower boundary of the allowable range of the final position target is formed by extending the available position margin in the negative direction, and the upper boundary of the allowable range of the final position target is formed by extending the available position margin in the positive direction.

[0030] Preferably, determining the compensation-compatible nominal location domain based on the allowable range of the final location target and the location compensation effective range includes:

[0031] The lower boundary of the compensation-effective interval is obtained by subtracting the lower boundary of the lower boundary of the allowable range of the final position target as the lower boundary of the compensation-compatible nominal position domain, and the upper boundary of the compensation-compatible nominal position domain is obtained by subtracting the upper boundary of the position compensation-effective interval from the upper boundary of the allowable range of the final position target as the upper boundary of the compensation-compatible nominal position domain.

[0032] Preferably, it further includes:

[0033] When the lower boundary of the compensation-compatible nominal position domain is greater than the upper boundary of the compensation-compatible nominal position domain, the compensation-compatible nominal position domain is determined to be empty, the current position compensation is terminated, the final value of the current position compensation and the current cumulative position compensation amount are both set to zero, the position compensation range is reset to the zero range, and the current actual position of the motion axis is used as the final servo position target of the current control cycle; the terminated position compensation is not re-established before the next service reversal occurs.

[0034] Secondly, the present invention provides a loading arm position adjustment control system based on state parameter fusion, including a processor and a memory, wherein the memory stores computer program instructions, and when the computer program instructions are executed by the processor, the above-mentioned loading arm position adjustment control method based on state parameter fusion is implemented.

[0035] By adopting the above technical solution, a computer program is generated from the above-mentioned method for adjusting and controlling the position of the loading arm based on state parameter fusion, and stored in the memory so that it can be loaded and executed by the processor. In this way, a terminal device can be made based on the memory and the processor for convenient use.

[0036] The beneficial effects of this invention are as follows: This invention incorporates the subsequent position compensation between the current cumulative position compensation amount and the final value of the current position compensation into the current position adjustment, and combines the actual position of the motion axis, the actual speed of the motion axis, and the actual position of the telescopic motion axis to form the allowable range of the final position target. On this basis, a compensation-compatible nominal position domain is determined, so that the original business target position first forms a restricted nominal position target according to the current executable position conditions, and then the current cumulative position compensation amount is superimposed to form the final servo position target. In this way, necessary position space is reserved in advance for subsequent position compensation during the continued change of position compensation, avoiding the current position target that meets the position limit from exceeding the executable range again after the subsequent compensation continues to act. This reduces the situation where position commands near the boundary are repeatedly restricted, and the motion axis frequently decelerates, stops, or reverses, thus improving the continuity, stability, and controllability of the loading arm position adjustment process. Attached Figure Description

[0037] Figure 1 This is a flowchart of a loading arm position adjustment control method based on state parameter fusion in this invention. Detailed Implementation

[0038] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0039] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0040] This invention discloses a method for adjusting the position of an arm-mounted loading arm based on state parameter fusion, referring to... Figure 1 This includes steps S1-S5:

[0041] S1: Obtain the actual position, actual speed and original target position of each motion axis, and determine the service reversal based on the change of the original target position between continuous control cycles.

[0042] Each motion axis is established with its own position reference. That is, during equipment debugging, the mechanical position corresponding to each motion axis after returning to zero is set as the position zero point of that motion axis, and the position value is increased in the direction specified by the positive motion direction set by the equipment. The actual position of the motion axis, the original business target position, and the position quantities formed during the position adjustment process are all represented according to the position zero point and the positive motion direction of that motion axis. Among them, the rotary motion axis uses angular position, angular velocity, and angular acceleration as the units, and the telescopic motion axis uses linear position, linear velocity, and linear acceleration as the units.

[0043] Within each control cycle, position data for each motion axis is collected to form the actual position of the motion axis, and speed data for each motion axis is read to form the actual speed of the motion axis. Simultaneously, the original business target position given by the current loading / unloading task for each motion axis is obtained. Specifically, for motion axes equipped with load-side position feedback, the load-side position feedback calibrated according to the zero point and positive direction of motion of the motion axis is used as the actual position of the motion axis. For motion axes without load-side position feedback, motor-side position feedback is used. Specifically, the rotational motion axis's position is determined based on the transmission ratio of the corresponding transmission mechanism and the motor-side position obtained from equipment debugging and calibration, and the actual position of the motion axis is compared with the input position of the motion axis. The positional correspondence between the output positions is determined by converting the motor-side position feedback into the output angular position. For the telescopic motion axis, based on the transmission relationship between the rotation angle and telescopic displacement of the corresponding drive mechanism and the positional correspondence obtained from equipment debugging and calibration, the motor-side position feedback is converted into the output linear displacement. The conversion result is used as the actual position of the motion axis. When the servo driver's speed feedback is the motor-side speed, the rotary motion axis converts the speed feedback into the output angular velocity based on the transmission ratio of the corresponding transmission mechanism. Similarly, the telescopic motion axis converts the speed feedback into the output linear velocity based on the transmission relationship between the rotation angle and telescopic displacement of the corresponding drive mechanism. The conversion result is used as the actual speed of the motion axis.

[0044] The original business target position is the target position given by the current loading and unloading task for the corresponding motion axis, and is represented according to the position zero point and positive motion direction of the corresponding motion axis. The original business target positions obtained in each control cycle are saved in chronological order to form the relationship of changes in the original business target positions between consecutive control cycles.

[0045] The original target position is directly derived from the current loading / unloading task, and its continuous changes reflect the direction in which the movement axis needs to be adjusted during the loading / unloading task itself. During position adjustment, when restrictions are imposed on the actual executed position value or additional positional actions are applied, this position value may change due to variations in the permissible operating space or mechanical state. Such changes do not necessarily indicate a change in the direction of movement for the loading / unloading task. Therefore, the subsequent operational movement direction used to characterize the direction of position change in the loading / unloading task, and the operational reversal identified accordingly, are all determined based on the changes in the original target position between continuous control cycles, thus distinguishing the intended movement of the loading / unloading task from the changes in the executed position formed during position adjustment.

[0046] A control cycle in which the actual position, actual speed, and original target position of the motion axis are all valid data is defined as an effective control cycle. In the first effective control cycle after the control task starts, the current original target position is saved as the initial original target position, and no change in direction is determined. From the start of the next consecutive effective control cycle, the current original target position is compared with the original target position of the previous effective control cycle. When the current original target position increases, the direction of motion is positive; when the current original target position decreases, the direction of motion is negative. If the current original target position does not change, no new direction of motion is formed, and the most recently formed non-zero direction of motion is maintained. When a change in the original target position to non-zero occurs for the first time, the corresponding direction is saved as the initial direction of motion. Subsequently, when the currently formed non-zero direction of motion is opposite to the most recently saved non-zero direction of motion, a change in direction is determined, and the current direction of motion is saved as the new direction of motion.

[0047] When invalid data exists in the actual position, actual speed, or original target position of the motion axis, the corresponding motion axis exits the normal position adjustment of the current control cycle, clears the previous original target position and the saved motion direction, and stops using invalid data to form the position adjustment result of the current control cycle. In the first effective control cycle after acquiring complete and valid data, the current original target position is saved as the initial original target position. This control cycle does not determine the direction of service reversal, and the motion direction is redefined in subsequent consecutive effective control cycles. When the control task stops or the servo drive mechanism malfunctions, the previous original target position and the saved motion direction are also cleared, and the position adjustment of the corresponding motion axis is stopped. After restarting the position adjustment control, the corresponding control state is re-established based on the new actual position, actual speed, and original target position of the motion axis.

[0048] S2: When the service changes direction, determine the final value of the current location compensation, and form the location compensation range based on the current cumulative location compensation amount and the final value of the current location compensation.

[0049] After a change of direction occurs on the motion axis, the direction of motion of the drive mechanism changes accordingly. However, backlash, connection clearance, or other mechanical clearances in the transmission mechanism may create a reversal travel, causing the output end of the motion axis to not have a corresponding positional change even when the drive side has already changed position in the reversed direction. If this reversal travel is not compensated, the output end of the motion axis will experience a positional response lag in the initial stage after the change of direction, resulting in a corresponding positional deviation. Therefore, after a change of direction occurs on the motion axis, position compensation in the corresponding direction needs to be added to the motion axis position target to ensure that the drive side continues to exert positional force in the reversed direction and overcomes the reversal travel until it drives the output end of the motion axis to change position in the reversed direction again. To determine the cumulative position compensation amount that needs to be applied from the start to the completion of a single position compensation, the total position compensation amount for that single position compensation needs to be determined.

[0050] The total position compensation is the total additional position quantity applied to the target position of the motion axis after a change in direction, in order to enable the drive side to overcome the change-in idle distance and re-drive the output end of the motion axis to change position in the direction after the change in direction. The total position compensation uses the same physical dimensions as the position quantity of the corresponding motion axis, and is positive or negative according to the positive direction of the position of the motion axis. The compensation applied in the positive direction of the position is a positive value, and the compensation applied in the negative direction of the position is a negative value.

[0051] In one embodiment, during the equipment commissioning phase, the corresponding total position compensation is pre-determined and saved for different service reversal scenarios of each motion axis. When a service reversal occurs during the control task operation, the corresponding saved value is directly read. Specifically, the total position compensation when the original service target position changes from decreasing to increasing and from increasing to decreasing can be saved separately, and the corresponding value can be read according to the motion axis where the service reversal occurs and the direction of change of the original service target position.

[0052] In another embodiment, the total position compensation is obtained by measuring the reversing backlash. The motion shaft under test is moved at low speed in one direction. After continuous changes in the output position of the motion shaft, the driving direction is changed, and the driving side position at the time of the direction change is recorded. The motion continues in the changed driving direction. When the output position of the motion shaft first changes in the changed direction beyond the position feedback resolution, the driving side position is recorded again. The difference between the two recorded driving side positions is calculated. When the recorded value is the motor side position, for a rotary motion shaft, the position difference is converted into an output angular position difference according to the transmission ratio from the motor to the output end of the motion shaft; for a telescopic motion shaft, the position difference is converted into an output linear displacement difference according to the correspondence between the rotation angle of the drive mechanism and the telescopic displacement. When the recorded value has already been expressed as the output position of the motion shaft according to the corresponding transmission relationship, the difference between the two recorded values ​​is directly used. The obtained output position difference is assigned a positive or negative sign according to the changed motion direction, and the resulting signed position quantity is directly used as the total position compensation corresponding to that reversal. To reduce the error of a single measurement, the same reversing situation can be measured repeatedly, and the average value of the total position compensation measured multiple times is saved as the value.

[0053] In other embodiments, the total position compensation can be calibrated or calculated based on the mechanical transmission clearance size of the motion shaft, the position difference between the drive-side position feedback and the output-side position feedback during the service reversal process, or historical position data formed by multiple service reversals, as long as the obtained total position compensation can characterize the cumulative additional position amount required to make the drive side cross the reversal space and make the output end of the motion shaft re-change its position along the direction after the reversal after this service reversal.

[0054] When a motion axis undergoes a service reversal, if the previous position compensation has not yet been completed, the previous position compensation is terminated. Regardless of whether the previous position compensation has been completed, the current cumulative position compensation amount corresponding to the previous service reversal is cleared to zero. The pre-saved total position compensation amount is read based on the direction of change of the original service target position, or the total position compensation amount that has been calibrated or calculated for this service reversal is read, and the read total position compensation amount is directly used as the final value of this position compensation. The final value of this position compensation represents the value that the current cumulative position compensation amount should reach when all this position compensation is completed; the final value of this position compensation is determined when this service reversal occurs and remains unchanged until the next service reversal occurs.

[0055] After the current position compensation begins, position compensation is applied cycle by cycle, starting from the state where the current cumulative position compensation is zero. This causes the current cumulative position compensation to monotonically change in the direction of the final value of the current position compensation until it is reached. In each control cycle, the cumulative value of the position compensation actually applied up to the current control cycle is used as the current cumulative position compensation. The specific allocation method for cycle-by-cycle position compensation ensures that the current cumulative position compensation does not exceed the final value of the current position compensation and changes monotonically from zero to the final value of the current position compensation cycle by cycle. In one embodiment, the current position compensation is allocated to ten consecutive control cycles. One-tenth of the final value of the current position compensation is used as the signed position compensation increment for each control cycle. After each control cycle, this signed position compensation increment is added to the current cumulative position compensation, so that the current cumulative position compensation in the tenth control cycle reaches the final value of the current position compensation. In other embodiments, other cycle-by-cycle allocation methods that ensure the current cumulative position compensation is monotonically adjusted and does not exceed the final value of the current position compensation can also be used. Therefore, before the current position compensation is completed, the current cumulative position compensation amount represents the position compensation effect that has been formed up to the current control cycle, and the final value of the current position compensation represents the position that the cumulative position compensation amount will eventually reach when the current position compensation continues to be executed until completion.

[0056] If the position target to be used is determined solely based on the current cumulative position compensation amount before position compensation is completed, it only reflects the position compensation already applied up to the current control cycle, and does not reflect the position changes that will occur after the remaining position compensation is applied. Especially when the motion axis is close to the allowable operating boundary, even if the currently applied position compensation has not yet caused a position overshoot, the continued increase in remaining position compensation may still further push the motion axis position. Therefore, this invention simultaneously retains the current cumulative position compensation amount and the final value of this position compensation, using a continuous range to represent all the cumulative position compensation amounts that may be generated during the period from the current state to completion of this position compensation.

[0057] Once the current cumulative position compensation amount and the final value of this position compensation are formed, the minimum of the two is taken as the lower boundary of the position compensation action range, and the maximum of the two is taken as the upper boundary of the position compensation action range, thus forming the position compensation action range. Since the current cumulative position compensation amount monotonically changes from its current value to the final value of this position compensation, this position compensation action range covers all cumulative position compensation amounts that may occur from the current control cycle until the completion of this position compensation. When this position compensation is not yet completed, this range reflects both the position compensation effect already formed up to the current control cycle and the position compensation effect that may form while this position compensation continues to be executed.

[0058] After this location compensation is completed, the current cumulative location compensation amount equals the final value of this location compensation, and this value remains until the next service reversal occurs. At this time, the location compensation range degenerates into a single-value range corresponding to the current cumulative location compensation amount. If no service reversal has occurred after the control task is started, both the current cumulative location compensation amount and the final value of this location compensation are set to zero, and the location compensation range becomes the zero-value range.

[0059] When invalid data exists for the actual position, actual speed, or original target position of the motion axis, or when the control task stops or the servo drive mechanism malfunctions, the currently executing position compensation is terminated, the current cumulative position compensation amount and the final value of this position compensation are both set to zero, and the position compensation range is reset to the zero range. After obtaining continuous and valid actual position, actual speed, and original target position of the motion axis again, the current cumulative position compensation amount is kept at zero, the final value of this position compensation is zero, and the position compensation range is in the zero range until a new business reversal occurs.

[0060] S3: Determine the effective position boundary in the negative direction and the effective position boundary in the positive direction of the corresponding motion axis based on the actual position of the telescopic motion axis, and form the allowable range of the final position target.

[0061] When the position of the telescopic motion axis of the loading arm changes, the extension length of the boom changes accordingly, and the range of positions within the permissible operating space of some motion axes also changes accordingly. Simultaneously, when a motion axis already has an actual speed in a certain direction, even if the current position has not yet reached the position limit in that direction, reducing the motion axis from the current speed to zero still requires a certain amount of space. Therefore, this invention first determines the range of positions where each motion axis can currently operate normally based on the actual position of the telescopic motion axis. Then, based on the actual position and actual speed of each motion axis, it reserves the space required to reduce the current actual speed from this range, thereby forming the range of positions that can be used for the final position target in the current control cycle.

[0062] For each motion axis, the minimum position value allowed to be reached by the motion axis in its telescopic state corresponding to its actual position is taken as the negative effective position boundary, and the maximum position value allowed to be reached by the motion axis is taken as the positive effective position boundary. The range between the two effective position boundaries represents the range of positions in which the motion axis can be used normally without exceeding the mechanical allowable stroke and the allowable operating space on site. Before the equipment is put into position adjustment control, multiple calibrated telescopic positions are set within the allowable operating range of the telescopic motion axis. The calibrated telescopic positions include at least the two endpoints of the allowable position range of the telescopic motion axis, and one or more intermediate calibrated positions are set between the two endpoints. In one embodiment, the allowable operating range of the telescopic motion axis is divided into four equal intervals, with the two endpoints and three equally divided positions serving as five calibrated telescopic positions.

[0063] At each calibrated telescopic position, the mechanically permissible position range and the on-site space permissible position range for each motion axis are determined. The mechanically permissible position range reflects the position that the corresponding motion axis can reach under the constraints of mechanical structure and stroke, while the on-site space permissible position range reflects the position that the corresponding motion axis can reach when the on-site space requirements are met in the installation environment. If either range is exceeded, the position cannot be used for normal position adjustment. Therefore, the currently usable position range must simultaneously meet both the mechanically permissible position range and the on-site space permissible position range. The two boundaries of the mechanically permissible position range are directly adopted from the permissible stroke boundaries of the corresponding motion axis given by the equipment mechanical design. When the on-site space restricts the position of the corresponding motion axis, while keeping the telescopic motion axis at the current calibrated telescopic position, the motion axis to be calibrated is moved at low speed in two directions to the limit positions that meet the minimum safety distance specified on site. The actual positions of the motion axis corresponding to the two limit positions are recorded to form the on-site space permissible position range. The maximum value between the lower boundary of the mechanically permissible position range and the lower boundary of the on-site space permissible position range is taken as the effective position boundary in the negative direction corresponding to the calibrated extension position. The minimum value between the upper boundary of the mechanically permissible position range and the upper boundary of the on-site space permissible position range is taken as the effective position boundary in the positive direction. When the on-site space does not impose additional restrictions on the corresponding motion axis, the two boundaries of the mechanically permissible position range are directly used as the effective position boundaries. Thus, the correspondence between each calibrated extension position and the effective position boundaries in the negative and positive directions of each motion axis is saved.

[0064] In actual operation, the telescopic motion axis can remain between adjacent calibrated telescopic positions. Simply saving the boundaries at the calibrated telescopic positions does not directly provide the effective position boundaries corresponding to the actual telescopic state between these positions. Therefore, in each control cycle, the actual position of the telescopic motion axis is read. If the actual position matches a calibrated telescopic position, the two effective position boundaries saved at that calibrated telescopic position are directly used. If the actual position is between two adjacent calibrated telescopic positions, the negative and positive effective position boundaries corresponding to the two adjacent calibrated telescopic positions on either side of the actual position are read. Linear interpolation is then performed on the two negative and two positive effective position boundaries according to the proportion of the actual position between the two adjacent calibrated telescopic positions. The two interpolated position values ​​are used as the negative and positive effective position boundaries for the current control cycle, respectively. For motion axes whose allowable position range is not affected by the position of the telescopic motion axis, the same negative and positive effective position boundaries are saved across all calibrated telescopic positions.

[0065] When the actual position of the motion axis is between the current effective position boundary in the negative direction and the effective position boundary in the positive direction, the difference between the effective position boundary in the positive direction and the actual position of the motion axis is taken as the remaining position margin in the positive direction, and the difference between the actual position of the motion axis and the effective position boundary in the negative direction is taken as the remaining position margin in the negative direction. The remaining position margin in the positive direction and the remaining position margin in the negative direction represent the remaining usable position space in the positive and negative directions, respectively, starting from the current actual position of the motion axis, provided that it does not exceed the current effective position boundary.

[0066] The positive and negative residual position margins only reflect the geometric distance between the actual position of the motion axis and the corresponding effective position boundary, and do not reflect the existing motion velocity of the motion axis. When the motion axis is moving towards a certain effective position boundary, even if it no longer continues to expand the position target along that direction, the motion axis will continue to occupy the position space in that direction during control calculations and deceleration. If all the geometrically remaining positions are used as available positions, no space is left to eliminate the existing motion trend. Therefore, when the actual velocity of the motion axis is not zero, it is necessary to reserve the position required to reduce the current actual velocity to zero from the residual position margin on the side pointed to by the actual velocity.

[0067] During the equipment commissioning phase, the corresponding deceleration amplitude is determined for each motion axis in both the positive and negative directions. Specifically: under the maximum allowable operating load of the equipment, the telescopic motion axes are held at each of the pre-set calibrated telescopic positions, and the motion axis under test is moved in both the positive and negative directions. At each calibrated telescopic position, a test is performed to decelerate from the maximum allowable operating speed to zero. Starting from the control cycle in which the speed feedback begins to decrease continuously, the corresponding deceleration amplitude is obtained by dividing the decrease in speed amplitude between two adjacent control cycles by the control cycle. The minimum deceleration amplitude obtained in all calibrated telescopic positions and all tests in the same motion direction is taken as the deceleration amplitude in that direction. The resulting deceleration amplitude is a positive number, representing the deceleration value used to calculate the position space required for deceleration when the motion axis moves in the corresponding direction.

[0068] For the There are several axes of motion, and one of the directions, either positive or negative, is denoted as d. When the actual velocity of the axis of motion is along the direction... At that time, the absolute value of the actual velocity of the motion axis is taken as the actual velocity amplitude in that direction. When the actual speed of the motion axis is not along the direction At that time, the actual velocity amplitude in that direction is set to zero. The deceleration amplitude corresponding to direction d is denoted as... The current control cycle length is denoted as In this embodiment, the controller synchronously performs state acquisition, target calculation, and position target update using a fixed control cycle. The state data acquired in the current control cycle is calculated and sent out before the start of the next control cycle. Therefore, the maximum delay from the current state acquisition to the new position target taking effect is calculated based on one control cycle. To cover the possible position changes that the motion axis may continue to undergo at the current speed within this one control cycle, and to reserve the position required to subsequently reduce the current speed to zero, the direction... The allowance for the deceleration reserve position is:

[0069]

[0070] in, Indicates the first The motion axis in the first... Each control cycle along the direction The deceleration reserve position margin; Indicates the motion axis number; Indicates either positive or negative direction; Indicates the control cycle number; Indicates the first The motion axis in the first... Each control cycle along the direction The actual speed amplitude, when the actual speed of the motion axis is along the direction hour, Take the absolute value of the actual velocity of the motion axis. When the actual velocity of the motion axis is not along the direction... hour, Take zero; Indicates the length of the control cycle; Indicates the first The deceleration amplitude corresponding to each motion axis along direction d is taken as positive; This represents the position measurement corresponding to the continued movement at the current actual speed during the period from the current state to the time when the new position target takes effect; Indicates the magnitude of deceleration Will along the direction The amount of position required for the current actual velocity to decrease to zero. When the axis of motion is not moving along direction d. The value is zero, corresponding to the deceleration reserve position margin. It is also zero.

[0071] The deceleration reserve position margin for the corresponding direction is subtracted from the remaining position margin in both the positive and negative directions. If the result after subtraction is greater than zero, it is taken as the available position margin in the corresponding direction; if the result is less than or equal to zero, the available position margin in the corresponding direction is set to zero. When the result of subtracting the deceleration reserve position margin in the corresponding direction from the remaining position margin in that direction is less than or equal to zero, it indicates that the remaining position space between the current actual position of the motion axis and the effective position boundary in that direction is insufficient to continue forming a position target along that direction after reserving the position space required to reduce the current actual speed to zero. Therefore, the available position margin in that direction is set to zero, and the allowable range of the final position target is no longer extended along that direction.

[0072] Using the actual position of the motion axis as a reference, the lower boundary of the allowable range of the final position target is obtained by extending the available position margin in the negative direction, and the upper boundary of the allowable range of the final position target is obtained by extending the available position margin in the positive direction. This forms the first... The allowable range of the final position target for each motion axis in the current control cycle is denoted as . =[ , The final position target allowable range represents the range of positions that the final position target actually sent to the servo position control in the current control cycle is allowed to fall into. It is simultaneously limited by the current effective position boundary and the deceleration position space required by the current actual speed of the motion axis.

[0073] When the actual position of the telescopic motion axis exceeds the established calibrated telescopic position range, or when the actual position of any motion axis is less than the current negative direction effective position boundary or greater than the current positive direction effective position boundary, the motion axis affected by this state stops the normal position adjustment of the current cycle, does not continue to form a new business position target, and sets the corresponding current cumulative position compensation amount and the final value of this position compensation to zero, and resets the position compensation range to the zero value range. When the actual position data of the corresponding motion axis is valid, a position holding command is sent to the corresponding servo drive mechanism with the current actual position of the motion axis as the target, so that the motion axis no longer continues to move along the original position target. After the equipment safety is confirmed, the operator switches the control mode to manual recovery mode and uses low-speed jogging to make the telescopic motion axis that exceeds the calibrated telescopic position range return to the calibrated telescopic position range, or to make the motion axis that exceeds the effective position boundary return to between the current negative direction effective position boundary and the positive direction effective position boundary. When the actual position of the telescopic motion axis re-enters the calibrated telescopic position range, and the actual position of the corresponding motion axis is again located between the current negative direction effective position boundary and the positive direction effective position boundary, the manual recovery mode is exited, the previous original business target position and business movement direction saved before recovery are cleared, the current original business target position is saved again as the initial original business target position, and the first effective control cycle after exiting the manual recovery mode is used as the starting control cycle for re-establishing the continuous control state. This control cycle does not determine the business direction change. From the subsequent continuous effective control cycles, the business movement direction is re-determined and normal position adjustment is restored.

[0074] When the mechanical structure, permissible mechanical travel, or permissible operating space changes, the original correspondence between the calibrated telescopic position and the effective position boundary is discontinued. The permissible mechanical position range and the permissible position range in the field space are redefined at each calibrated telescopic position, and the effective position boundary is redefined and saved. During recalibration, the affected motion axis uses its current actual position as the position holding target, without forming a new operational position adjustment target. When the servo reduction capability, mechanical transmission configuration, or permissible operating load range changes, the test of decelerating from the maximum permissible operating speed under normal control to zero is re-executed at each calibrated telescopic position, and the deceleration amplitude in the corresponding direction is redefined. Before the new deceleration amplitude is determined, the affected motion axis also maintains its current actual position, without forming a new operational position adjustment target.

[0075] S4: Determine the compensation-compatible nominal location domain based on the allowable range of the final location target and the location compensation range.

[0076] The final servo position target actually sent to the servo position control in the current control cycle is formed by the position value before position compensation superposition and the current cumulative position compensation amount. The position value used to carry the current service position control requirements before position compensation superposition is called the nominal position. When position compensation is not yet completed, the current cumulative position compensation amount will continue to change towards the final value of this position compensation. Therefore, even if a certain nominal position is still within the allowable range of the final position target after superposition of the current cumulative position compensation amount, the superimposed final servo position target may still exceed this allowable range as subsequent position compensation continues to be executed. In order to ensure that the final servo position target formed during the period from the current state to the completion of this position compensation is within the allowable range of the final position target, this invention, in conjunction with the already formed position compensation range, further determines the range of values ​​that the nominal position can currently take.

[0077] For example, when the motion axis is close to the upper boundary of the allowable range of the final position target, and the current position compensation is still increasing in the positive direction, the same nominal position, after being superimposed with the current cumulative position compensation, may still be within the allowable range. However, as the cumulative position compensation continues to increase, the superposition result may exceed the upper boundary of the allowable range. If the nominal position is restricted again only after the remaining position compensation is actually formed, the nominal position used in the current control cycle will not reserve position space for subsequent compensation to continue. Therefore, when the nominal position allowable range is formed in the current control cycle, this invention includes the subsequent position compensation action covered by the position compensation action interval into the constraint.

[0078] For the Each motion axis has a lower boundary that allows for the final position target's range. The upper boundary is The lower boundary of the position compensation range is The upper boundary is The position compensation range covers all possible cumulative position compensation values ​​from the current cumulative position compensation value to the final position compensation value. Therefore, for a usable nominal position, it is necessary to ensure that the position value obtained by superimposing any cumulative position compensation value within the position compensation range onto this nominal position is not less than the lower boundary of the allowable range of the final position target and not greater than its upper boundary. Since... This is the minimum cumulative position compensation amount within the position compensation coverage area. To ensure that the result after superimposing this value is not less than [amount missing], [the following is a separate, unrelated statement:] The nominal position cannot be smaller than - ;because This is the maximum cumulative position compensation amount within the position compensation coverage area. To ensure that the result after superimposing this value is not greater than [a certain value], [the following is a separate, unrelated statement:] The nominal position cannot be greater than - This forms the first The motion axis in the first... Compensation compatible nominal position domain for each control cycle:

[0079]

[0080] in, Indicates the first The motion axis in the first... Compensation compatible with nominal position domain for each control cycle; Indicates the motion axis number; Indicates the control cycle number; Indicates the first The motion axis in the first... The lower boundary of the allowable range of the final position target in each control cycle; Indicates the first The motion axis in the first... The upper boundary of the allowable range of the final position target in each control cycle; Indicates the first The motion axis in the first... The lower boundary of the position compensation action range for each control cycle, i.e., the minimum cumulative position compensation amount in the position compensation action range; Indicates the first The motion axis in the first... The upper boundary of the position compensation action range for each control cycle, i.e., the maximum cumulative position compensation amount within the position compensation action range. , , and All correspond to the same motion axis. The position compensation amount and the position amount of the motion axis use the same physical dimensions, and the positive and negative values ​​are defined according to the positive direction of the position of the motion axis. Therefore, the allowable range of the final position target can be converted into the nominal position range before position compensation superposition by the addition and subtraction relationship between the boundary of the allowable range of the final position target and the boundary of the position compensation action interval.

[0081] Any position value in the compensation-compatible nominal position domain, when superimposed with any cumulative position compensation amount within the position compensation action interval, can remain within the allowable range of the current final position target. Therefore, before the current position compensation is completed, this position domain considers not only the current cumulative position compensation amount formed up to the current control cycle, but also the position compensation effect that may occur during the period when the current cumulative position compensation amount continues to change to the final value of the current position compensation. After the current position compensation is completed, the current cumulative position compensation amount equals the final value of the current position compensation, and the position compensation action interval degenerates into a single-value interval corresponding to the cumulative position compensation amount. At this time, the lower boundary of the compensation-compatible nominal position domain is equal to the lower boundary of the allowable range of the final position target minus the current cumulative position compensation amount, and the upper boundary is equal to the upper boundary of the allowable range of the final position target minus the current cumulative position compensation amount. When both the current cumulative position compensation amount and the final value of the current position compensation are zero, and the position compensation action interval is a zero-value interval, the compensation-compatible nominal position domain is the same as the allowable range of the final position target.

[0082] When the lower boundary of the compensation-compatible nominal position domain is less than or equal to the upper boundary, the compensation-compatible nominal position domain is not empty, indicating that there is at least one nominal position value such that the nominal position value, when added to any cumulative position compensation amount that may occur during the current position compensation process from the current state to completion, can remain within the allowable range of the final position target. At this time, the compensation-compatible nominal position domain is retained to constrain the nominal position adopted in the current control cycle.

[0083] When the lower boundary of the compensation-compatible nominal position domain is greater than the upper boundary, the compensation-compatible nominal position domain is empty, indicating that there is currently no nominal position value that can remain within the allowable range of the final position target after all possible cumulative position compensation amounts within the superimposed position compensation range. If the current position compensation continues, it is impossible to simultaneously satisfy the constraints of the allowable range of the final position target and the subsequent change requirements of this position compensation by adjusting the nominal position. Therefore, the current position compensation is terminated, the final value of this position compensation and the current cumulative position compensation amount are both set to zero, and the position compensation range is reset to the zero value range. The control state corresponding to this position compensation is cleared, and the actual displacement of the motion axis formed during the position compensation execution is still reflected by the current actual position of the motion axis. To avoid continuing to form position commands that expand in the direction of the original business target when the current position space is insufficient, the current actual position of the motion axis is used as the final servo position target of the current control cycle of the motion axis, so that the servo position control remains near the current actual position.

[0084] If the current location compensation is terminated due to the nominal location field being empty, the terminated location compensation will not be re-established while the current business movement direction remains unchanged. The current cumulative location compensation amount and the final value of this location compensation will remain zero, and the location compensation range will remain a zero value range. When a new business direction changes in the future, the corresponding location compensation status will be re-determined for the new business direction.

[0085] S5: The original business target location is restricted to the compensation-compatible nominal location domain to form a restricted nominal location target, and the current cumulative location compensation amount is added to form the final servo location target.

[0086] The original business target position always represents the movement intention of the current loading and unloading task in the judgment of business movement direction and business reversal, and is not rewritten due to position constraints or position compensation during the position adjustment process; the compensation-compatible nominal position domain represents the actual range of positions that can be used before the position compensation is superimposed, under the joint constraints of the current executable position space and the continued effect of this position compensation. By separating the two, the original required movement direction of the loading and unloading task can be retained, while only the position value actually used for execution in the current control cycle is restricted.

[0087] Based on this, when the original business target location is within the compensation-compatible nominal location domain, it can be directly used as the nominal location before location compensation is superimposed. However, when the original business target location exceeds the compensation-compatible nominal location domain, directly using the original business target location would not guarantee that the position value formed after the cumulative location compensation within the superimposed location compensation range will always be within the allowable range of the final location target. Therefore, for motion axes with non-empty compensation-compatible nominal location domains, the original business target location is restricted to within the compensation-compatible nominal location domain, forming a restricted nominal location target.

[0088] Specifically, the first The motion axis in the first... The lower boundary of the compensation-compatible nominal position domain for each control cycle is denoted as... The upper boundary is denoted as When the original business target location is greater than or equal to and less than or equal to When the original business target location is greater than a certain value, the original business target location is directly used as the restricted nominal location target; when the original business target location is greater than a certain value, the original business target location is used as the restricted nominal location target. At that time, As a restricted nominal location target; when the original business target location is smaller than... At that time, As a restricted nominal location target. Therefore, the first The restricted nominal position target of each motion axis is:

[0089]

[0090] in, Indicates the first The motion axis in the first... The constrained nominal position target for each control cycle, i.e., the nominal position value actually used for superimposing position compensation in the current control cycle; Indicates the first The motion axis in the first... The original business objective position for each control cycle; Indicates the first The motion axis in the first... The compensation compatibility nominal position domain lower boundary for each control cycle; Indicates the first The motion axis in the first... The compensation compatibility of the nominal location domain upper boundary for each control cycle; Indicates the motion axis number; Indicates the control cycle number; This indicates that the maximum value among the values ​​within the parentheses is selected. This indicates taking the minimum value among the values ​​within the parentheses. By restricting the original business target location to the compensation-compatible nominal location domain, the restricted nominal location target always remains within the current compensation-compatible nominal location domain.

[0091] The compensation-compatible nominal position domain, when formed, already reserves position space for the current position compensation to continue changing to its final value. However, the additional positional effect actually formed in the current control cycle is still represented by the current cumulative position compensation amount. Therefore, reserving space for subsequent position compensation does not mean that the current control cycle directly applies the entire final value of position compensation. The current control cycle only adds the already formed current cumulative position compensation amount, and the remaining position compensation continues to be formed according to subsequent control cycles.

[0092] After the constrained nominal position target is formed, the current cumulative position compensation is added to the constrained nominal position target to form the final servo position target that actually serves as the servo position control input for the corresponding motion axis in the current control cycle. The constrained nominal position target and the actual position of the corresponding motion axis use the same position representation reference. The current cumulative position compensation and the position quantity of the motion axis use the same physical dimensions and are defined by the same positive position direction, so the two can be directly added. The final servo position target for each motion axis is:

[0093]

[0094] in, Indicates the first The motion axis in the first... The final servo position target for each control cycle; Indicates the first The motion axis in the first... The restricted nominal position target for each control cycle; Indicates the first The motion axis in the first... The current cumulative position compensation amount for each control cycle; Indicates the motion axis number; Indicates the control cycle number.

[0095] Since the restricted nominal position target is located within the compensated compatible nominal position domain, and the current cumulative position compensation is located within the position compensation range, the final servo position target formed in the current control cycle is within the allowable range of the current final position target. Simultaneously, the compensated compatible nominal position domain has constrained the cumulative position compensation that may occur during the period from the current cumulative position compensation to the final value of the current position compensation, according to the position compensation range of the current control cycle, ensuring that the currently used restricted nominal position target is compatible with this position compensation range. In each subsequent effective control cycle, the actual position, actual speed, and original target position of the motion axis are reacquired, and the actual position of the telescopic motion axis is read. Based on the updated state of the current control cycle, the allowable range of the final position target and the compensated compatible nominal position domain are re-formed, and the restricted nominal position target and the final servo position target are redefined.

[0096] For a motion axis that has already formed a final servo position target, this final servo position target is used as the position target input for the corresponding servo drive mechanism, which then drives the motion axis to adjust its position according to this position target. Between consecutive control cycles, the current original service target position is compared with the original service target position of the previous effective control cycle to determine the service movement direction and service reversal, and the corresponding position compensation state is updated based on the resulting service reversal result; the constrained nominal position target is used to form the final servo position target for the current control cycle.

[0097] The following uses a rotary motion axis as an example to illustrate the position adjustment control process of this embodiment. The following values ​​are all illustrative values, used only to illustrate the calculation relationship, and do not constitute a limitation on the actual parameters of the equipment:

[0098] Assuming a control cycle length of 0.001s, the current actual position of the telescopic motion axis is 0.75m. The device has two pre-saved adjacent calibrated telescopic positions of 0.50m and 1.00m. At 0.50m, the effective position boundaries in the negative and positive directions of the rotary motion axis are 0.78rad and 1.22rad, respectively; at 1.00m, the two effective position boundaries are 0.82rad and 1.18rad, respectively. The current actual position of the telescopic motion axis at 0.75m is located between the two calibrated telescopic positions of 0.50m and 1.00m. After linear interpolation of the effective position boundaries in both directions, the current effective position boundary in the negative direction is found to be 0.80rad, and the current effective position boundary in the positive direction is found to be 1.20rad.

[0099] Let the current actual position of the rotating axis be 1.00 rad, the actual velocity of the axis be 0.10 rad / s in the positive direction, and the deceleration amplitude in both the positive and negative directions be 0.50 rad / s². The remaining position margin in the positive direction is 1.20 - 1.00 = 0.20 rad, and the remaining position margin in the negative direction is 1.00 - 0.80 = 0.20 rad. Since the axis is currently moving in the positive direction, the actual velocity amplitude in the positive direction is 0.10 rad / s, and the actual velocity amplitude in the negative direction is zero. Substituting the actual velocity amplitude in the positive direction, the control cycle length, and the deceleration amplitude in the positive direction into the expression for the deceleration reserve position margin, the deceleration reserve position margin in the positive direction is:

[0100]

[0101] The negative deceleration reserve is zero. Therefore, the available position margin in the positive direction is 0.20 - 0.0101 = 0.1899 rad, and the available position margin in the negative direction is 0.20 rad. Thus, the allowable range for the final target position is:

[0102]

[0103] In a continuous effective control cycle, assuming the most recently saved non-zero service movement direction is negative, the original service target position in the previous effective control cycle was 1.170 rad, and the original service target position in the current control cycle becomes 1.185 rad. Since the current original service target position is larger than that in the previous effective control cycle, the current service movement direction is positive and opposite to the most recently saved negative direction service movement direction, thus determining that a service reversal has occurred.

[0104] Let the total position compensation corresponding to this service reversal be 0.010 rad, thus the final value of this position compensation is 0.010 rad. In the current control cycle, a cumulative 0.004 rad of position compensation has been applied, therefore the current cumulative position compensation is 0.004 rad. Since the current cumulative position compensation is less than the final value of this position compensation, the position compensation range is formed by the minimum and maximum of these two values, i.e., [the range is missing from the original text]. .

[0105] Based on the current final target allowable range [0.8000, 1.1899] rad and the position compensation effective range [0.004, 0.010] rad, the lower boundary of the compensation-compatible nominal position domain is 0.8000 - 0.004 = 0.7960 rad, and the upper boundary is 1.1899 - 0.010 = 1.1799 rad. Therefore, the current compensation-compatible nominal position domain is... .

[0106] The current original target position is 1.185 rad, which is greater than the upper boundary of the compensated compatible nominal position domain (1.1799 rad). Therefore, the upper boundary of the compensated compatible nominal position domain is taken as the restricted nominal position target, i.e., the restricted nominal position target is 1.1799 rad. The current cumulative position compensation of 0.004 rad is added to this restricted nominal position target to obtain the final servo position target for the current control cycle. .

[0107] The final servo position target is located within the current allowable range of [0.8000, 1.1899] rad. Meanwhile, while the compensation-compatible nominal position domain formed in the current control cycle remains valid to illustrate its constraint effect, when the cumulative position compensation increases from the current 0.004 rad to the final value of this position compensation of 0.010 rad, the result of adding the current restricted nominal position target of 1.1799 rad to the final value of this position compensation yields 1.1899 rad, which still does not exceed the upper boundary of the current allowable range of the final position target.

[0108] In contrast, if the nominal position is limited solely by the current cumulative position compensation of 0.004 rad, without considering the possibility that the current position compensation might increase to 0.010 rad, the upper limit of the nominal position calculated from the current cumulative position compensation would be 1.1899 - 0.004 = 1.1859 rad. The current original operational target position of 1.185 rad would not be restricted; however, if the subsequent cumulative position compensation reaches 0.010 rad, the corresponding position value would reach 1.195 rad, exceeding the upper limit of the final target position range of 1.1899 rad used for comparison. Therefore, by incorporating the unfinished position compensation effect into the nominal position constraint in advance through the position compensation effect range, it is possible to avoid missing the subsequent position effect of this position compensation when forming the nominal position limit solely based on the current cumulative position compensation.

[0109] This invention also discloses a loading arm position adjustment control system based on state parameter fusion, including a processor and a memory. The memory stores computer program instructions, and when the computer program instructions are executed by the processor, a loading arm position adjustment control method based on state parameter fusion according to the present invention is implemented.

[0110] The system also includes other components well known to those skilled in the art, such as communication buses and communication interfaces, the settings and functions of which are known in the art and will not be described in detail here.

[0111] In the description of this specification, "multiple" or "several" means at least two, such as two, three or more, unless otherwise expressly and specifically defined.

[0112] While this specification has shown and described numerous embodiments of the invention, it will be apparent to those skilled in the art that such embodiments are provided by way of example only. Many modifications, alterations, and alternatives will occur to those skilled in the art without departing from the spirit and essence of the invention. It should be understood that various alternatives to the embodiments of the invention described herein may be employed in the practice of this invention.

Claims

1. A method for adjusting the position of an arm-mounted loading arm based on state parameter fusion, characterized in that, include: Obtain the actual position, actual speed and original target position of each motion axis; determine the service reversal based on the change of the original target position between continuous control cycles. When the business changes direction, the final value of the current location compensation is determined, so that the current cumulative location compensation amount changes monotonically from zero to the final value of the current location compensation in each cycle, and the minimum and maximum values ​​of the current cumulative location compensation amount and the final value of the current location compensation are used as the lower boundary and upper boundary of the location compensation range, respectively. The negative and positive effective position boundaries of the corresponding motion axis are determined based on the actual position of the telescopic motion axis, and the final position target allowable range is formed based on the actual position of the motion axis, the actual speed of the motion axis, the negative effective position boundary, and the positive effective position boundary. The compensation-compatible nominal position domain is determined based on the allowable range of the final position target and the position compensation range, so that the position value formed by superimposing any cumulative position compensation amount within the position compensation range on the nominal position within the compensation-compatible nominal position domain is within the allowable range of the final position target. When the compensation-compatible nominal position domain is not empty, the original business target position is restricted to the compensation-compatible nominal position domain to form a restricted nominal position target. The current cumulative position compensation is added to the restricted nominal position target to form the final servo position target, and the final servo position target is used as the position target input of the corresponding servo drive mechanism.

2. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The step of determining the business reversal based on the change in the original business target location between continuous control cycles includes: The effective control period is defined as the control period in which the actual position of the motion axis, the actual speed of the motion axis, and the original business target position are all valid data. In the first effective control period after the control task is started, the current original business target position is saved as the initial original business target position, and no business reversal is determined. Starting from the next continuous effective control cycle, the current original business target position is compared with the original business target position of the previous effective control cycle. When the current original business target position increases, the business movement direction is determined to be positive. When the current original business target position decreases, the business movement direction is determined to be negative. When the current original business target position does not change, no new business movement direction is formed, and the non-zero business movement direction formed in the most recent cycle is maintained. When a non-zero original business target location changes for the first time, the corresponding business movement direction is saved as the initial business movement direction. Subsequently, when the currently formed non-zero business movement direction is opposite to the most recently saved non-zero business movement direction, a business reversal is determined, and the currently formed non-zero business movement direction is saved as the new business movement direction.

3. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The determination of the final value of the position compensation during the service reversal includes: Based on the direction of change of the motion axis that has undergone a service reversal and the original service target position, the total position compensation amount is read from the device during the debugging phase for the corresponding service reversal situation. The total position compensation amount is read as the final position compensation value for this time. The total position compensation amount is the cumulative additional position amount required to make the drive side cross the reversal space and drive the output end of the motion axis to generate a position change in the direction after the reversal after the service reversal occurs.

4. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The determination of the final value of the position compensation during the service reversal includes: Move the motion axis under test at low speed in one direction. After the output position of the motion axis changes continuously, change the driving direction and record the driving side position when the driving direction is changed. Continue to move in the changed driving direction. When the output position of the motion axis first produces a position change that exceeds the position feedback resolution in the changed direction, record the driving side position again. The difference between the two recorded drive-side positions is converted into the position difference of the corresponding motion axis output end, and the position difference is assigned a positive or negative sign according to the changed motion direction. The resulting signed position quantity is used as the total position compensation for the corresponding service reversal situation and saved. When a service is switched, the total position compensation saved for the corresponding service switching situation is read based on the motion axis of the service switching and the direction of change of the original service target position. The total position compensation read is then used as the final position compensation value for this service switching.

5. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The step of determining the negative and positive effective position boundaries of the corresponding motion axis based on its actual position includes: Multiple calibrated telescopic positions are set within the allowable operating range of the telescopic motion shaft. Under each calibrated telescopic position, the mechanical allowable position range and the field space allowable position range of the corresponding motion shaft are determined. The maximum value between the lower boundary of the mechanical allowable position range and the lower boundary of the field space allowable position range is taken as the negative effective position boundary of the corresponding calibrated telescopic position. The minimum value between the upper boundary of the mechanical allowable position range and the upper boundary of the field space allowable position range is taken as the positive effective position boundary of the corresponding calibrated telescopic position. The correspondence between each calibrated telescopic position and the negative and positive effective position boundaries is saved. When the actual position of the telescopic motion axis is the same as a certain calibrated telescopic position, the negative and positive effective position boundaries corresponding to that calibrated telescopic position are used. When the actual position of the telescopic motion axis is between two adjacent calibrated telescopic positions, the negative effective position boundaries corresponding to the two adjacent calibrated telescopic positions are linearly interpolated according to the proportion of the actual position of the telescopic motion axis between the two adjacent calibrated telescopic positions, and the positive effective position boundaries corresponding to the two adjacent calibrated telescopic positions are linearly interpolated to obtain the negative and positive effective position boundaries of the current control cycle.

6. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The process of forming the final position target allowable range based on the actual position of the motion axis, the actual velocity of the motion axis, the effective position boundary in the negative direction, and the effective position boundary in the positive direction includes: The difference between the effective position boundary in the positive direction and the actual position of the motion axis is taken as the remaining position margin in the positive direction, and the difference between the actual position of the motion axis and the effective position boundary in the negative direction is taken as the remaining position margin in the negative direction. Based on the actual speed of the motion axis and the predetermined deceleration amplitude in the corresponding direction of motion, a deceleration reserve position margin is formed in the corresponding direction. The deceleration reserve position margin includes the position amount corresponding to the motion axis continuing to move at the current actual speed during the period from the current state acquisition to the next control cycle to re-form the position target, and the position amount required to reduce the current actual speed to zero. The deceleration reserve position margin for the corresponding direction is subtracted from the remaining position margin in the positive direction and the remaining position margin in the negative direction, respectively. If the result after subtraction is greater than zero, the result is taken as the available position margin in the corresponding direction. If the result after subtraction is less than or equal to zero, the available position margin in the corresponding direction is set to zero. Based on the actual position of the motion axis, the lower boundary of the allowable range of the final position target is formed by extending the available position margin in the negative direction, and the upper boundary of the allowable range of the final position target is formed by extending the available position margin in the positive direction.

7. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, The step of determining the compensation-compatible nominal location domain based on the allowable range of the final location target and the location compensation effective range includes: The lower boundary of the compensation-effective interval is obtained by subtracting the lower boundary of the lower boundary of the allowable range of the final position target as the lower boundary of the compensation-compatible nominal position domain, and the upper boundary of the compensation-compatible nominal position domain is obtained by subtracting the upper boundary of the position compensation-effective interval from the upper boundary of the allowable range of the final position target as the upper boundary of the compensation-compatible nominal position domain.

8. The method for adjusting and controlling the position of an arm-mounted loading arm based on state parameter fusion according to claim 1, characterized in that, Also includes: When the lower boundary of the compensation-compatible nominal position domain is greater than the upper boundary of the compensation-compatible nominal position domain, the compensation-compatible nominal position domain is determined to be empty, the current position compensation is terminated, the final value of the current position compensation and the current cumulative position compensation amount are both set to zero, the position compensation action range is reset to the zero value range, and the current actual position of the motion axis is used as the final servo position target of the current control cycle. The location compensation for the terminated location will not be re-established until the next business reversal occurs.

9. A loading arm position adjustment control system based on state parameter fusion, characterized in that, include: A processor and a memory, wherein the memory stores computer program instructions that, when executed by the processor, implement a loading arm position adjustment control method based on state parameter fusion according to any one of claims 1-8.

Images