Disclosure of Invention
The invention provides a control method of a track type transfer robot, a control device of the track type transfer robot and a control system of the track type transfer robot, which solve the problem of low control precision in the related technology.
As a first aspect of the present invention, there is provided a control method of a rail type transfer robot, which is applied to a transfer robot including a transfer table and a plurality of driving wheel devices located at a lower surface of the transfer table, each of the driving wheel devices including a driving wheel and a driving mechanism corresponding thereto, the control method of the rail type transfer robot comprising:
Respectively acquiring the real-time position of the transfer robot and the load distribution information of each driving wheel;
constructing maximum driving force constraint of each driving wheel according to the load distribution information of each driving wheel, and constructing total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel;
controlling the butt joint of the transfer robot according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot;
constructing a driving force distribution strategy of each driving wheel according to the difference value between the real-time position and the target docking position of the transfer robot and combining the maximum driving force constraint of each driving wheel;
a drive signal is generated in accordance with a drive force distribution strategy for each drive wheel such that a drive mechanism for each drive wheel drives movement of the corresponding drive wheel in accordance with the drive signal.
Further, generating a drive signal according to a drive force distribution strategy for each drive wheel includes:
constructing a yaw moment constraint strategy according to the driving force distribution strategy of each driving wheel;
determining a desired drive torque for each drive wheel according to a yaw moment constraint strategy;
a drive signal for each drive wheel is generated based on the desired drive torque for each drive wheel.
Further, constructing a yaw moment constraint strategy according to the driving force distribution strategy of each driving wheel, including:
calculating a yaw moment of the transfer robot according to the distributed driving force of each driving wheel;
judging whether the yaw moment of the transfer robot exceeds a preset yaw moment constraint;
if the yaw moment of the transfer robot exceeds the preset yaw moment constraint, determining a driving force adjustment mode of the driving force according to the driving force adjustment rule,
wherein the driving force setting rule includes:
if the yaw moment generated by the upper side and the lower side are opposite in direction, the driving force of the largest driving wheel in all driving wheels is reduced;
if the yaw moment directions generated by the upper side and the lower side are the same, respectively reducing the driving force of one driving wheel with the largest driving force on the upper side and the lower side;
wherein, use the one side that transfer robot is close to transfer robot direction of advance as the upside, the one side that deviates from the direction of advance is the downside.
Further, generating a drive signal according to a drive force distribution strategy for each drive wheel, further comprises:
an actuator time lag compensation strategy is constructed according to a yaw moment constraint strategy, wherein the actuator time lag compensation strategy comprises the steps of estimating time lag of each driving wheel in a uniform acceleration stage of the transfer robot to obtain time lag estimation information, and compensating the actuator time lag according to the time lag estimation information in a uniform deceleration stage of the transfer robot;
And generating a driving signal of each driving wheel according to the actuator time lag compensation strategy.
Further, constructing a maximum driving force constraint of each driving wheel according to the load distribution information of each driving wheel, and constructing a total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel, comprising:
calculating the maximum driving force of each driving wheel according to the load distribution information of each driving wheel, wherein the calculation formula of the maximum driving force of each driving wheel is as follows:
,
wherein i represents the number of the driving wheel;represents the maximum driving force of the i-th driving wheel; />Representing the load mass of the ith drive wheel; g represents a gravitational constant; />Representing the coefficient of friction;
determining the maximum driving force constraint of each driving wheel and the total driving force constraint of the transfer robot according to the maximum driving force of each driving wheel and the maximum output driving force of the driving mechanism corresponding to the driving force,
,
wherein ,maximum driving force constraint for the ith driving wheel, for example>Maximum driving force for the ith driving wheel; />Represents the maximum output driving force of the driving mechanism; />Representing the total driving force constraint of the transfer robot; n represents the number of driving wheels.
Further, controlling docking of the transfer robot according to the transfer robot total driving force constraint and the real-time position of the transfer robot, comprising:
Determining a desired position of the transfer robot according to the total driving force constraint of the transfer robot, wherein the desired position of the transfer robot is expressed as follows:
,
wherein ,i represents the number of the driving wheel; />The acceleration of the transfer robot is represented, and the acceleration in the uniform deceleration and uniform acceleration stages is set to be the same; />Representing the total driving force constraint of the transfer robot; />Representing the i-th drive wheel load mass; />Representing a desired position of the transfer robot, t representing time; />A time point for indicating the end of the uniform acceleration phase of the transfer robot; />A time point for starting a uniform deceleration stage of the transfer robot is shown;
determining a moving path of the transfer robot according to the expected position of the transfer robot;
when the transfer robot moves to the target docking position according to the moving path, judging whether the difference value between the real-time position of the transfer robot and the target docking position meets a docking completion condition, wherein the expression of the docking completion condition is as follows:
,
wherein ,representing a target docking position; />Real-time position for transfer robot; />Representing the docking accuracy;
and if the docking completion condition is met, determining that the transfer robot completes docking.
Further, constructing a driving force distribution strategy of each driving wheel according to the difference value between the real-time position and the target docking position of the transfer robot and combining the maximum driving force constraint of each driving wheel, wherein the driving force distribution strategy comprises the following steps:
Determining a position closed-loop control incremental PID control law according to the difference value between the real-time position and the target docking position of the transfer robot, wherein the position closed-loop control incremental PID control law has the following expression:
,
wherein ,k represents a time sequence number; />The increment of the total driving force of the transfer robot at the moment k is represented; />Representing a scaling factor; />Representing an integral coefficient; />Representing the differential coefficient; />Representing a position error of the transfer robot; />Representing a desired position of the transfer robot; />Representing a real-time position of the transfer robot; />Representing the position error of the transfer robot at the moment k; />The total driving force of the transfer robot at the moment k is represented; />Representing the total driving force of the transfer robot at the moment k-1;
and distributing the total driving force of the transfer robot according to the position closed-loop control incremental PID control law and the maximum driving force constraint of each driving wheel, and obtaining the driving force corresponding to each driving wheel.
Further, the distribution of the total driving force of the transfer robot according to the position closed-loop control incremental PID control law and the maximum driving force constraint of each driving wheel comprises the following steps:
calculating the average driving force of each driving wheel according to the total driving force of the transfer robot, wherein the calculation formula of the average driving force is as follows:
,
wherein ,represents an average driving force; />Indicating the total driving force of the transfer robot; n represents the number of driving wheels;
comparing the maximum driving force constraint of each driving wheel with the average driving force to determine whether there is a situation in which the average driving force exceeds the maximum driving force constraint of at least one driving wheel;
if so, setting the driving force of the at least one driving wheel as the maximum driving force of the driving wheels, and distributing the remaining undivided driving force of the total driving force of the transfer robot to the remaining driving wheels according to a preset distribution rule, wherein the remaining driving wheels comprise driving wheels with the maximum driving force constraint not exceeded by the average driving force;
if the total driving force does not exist, the total driving force of the transfer robot is distributed to all driving wheels according to a preset distribution rule.
As another aspect of the present invention, there is provided a control device for a rail type transfer robot for implementing the control method for a rail type transfer robot, including:
the acquisition module is used for respectively acquiring the real-time position of the transfer robot and the load distribution information of each driving wheel;
the first construction module is used for constructing the maximum driving force constraint of each driving wheel according to the load distribution information of each driving wheel and constructing the total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel;
The control module is used for controlling the butt joint of the transfer robot according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot;
the second construction module is used for constructing a driving force distribution strategy of each driving wheel according to the difference value between the real-time position and the target docking position of the transfer robot and combining the maximum driving force constraint of each driving wheel;
and the generating module is used for generating a driving signal according to the driving force distribution strategy of each driving wheel so that the driving mechanism of each driving wheel drives the corresponding driving wheel to move according to the driving signal.
As another aspect of the present invention, there is provided a control system of a rail type transfer robot, including: the transfer robot comprises a transfer table and a plurality of driving wheel devices positioned on the lower surface of the transfer table, each driving wheel device comprises a driving wheel and a driving mechanism corresponding to the driving wheel, the position acquisition device is arranged on the transfer table, each driving wheel is provided with one load information acquisition device, and the position acquisition device, the load information acquisition device and the driving wheel devices are all in communication connection with the control device of the track type transfer robot;
The position acquisition device can acquire the real-time position of the transfer robot in real time;
the load information acquisition device can acquire load distribution information of each driving wheel;
the control device of the track type transfer robot can determine driving signals of the driving mechanism according to the load distribution information of each driving wheel and the real-time position of the transfer robot;
the driving mechanism can drive the motion of each driving wheel according to the driving signals so as to drive the migration table to move to the target butt joint position to finish butt joint, wherein the migration table is used for bearing the workpieces to be transported.
According to the control method of the track type transfer robot, the maximum driving force constraint of each driving wheel can be constructed according to the load distribution information of each driving wheel, so that the total driving force constraint of the transfer robot is constructed according to the maximum driving force constraint of each driving wheel, further, the butt joint of the table top track and the target track of the transfer robot is controlled according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot, in order to improve the butt joint precision, the driving force distribution strategy of each driving wheel is constructed according to the difference value of the real-time position and the target butt joint position of the transfer robot and combined with the maximum driving force constraint of each driving wheel, and the driving signals of each driving wheel are generated according to the driving force distribution strategy, so that the driving mechanism of each driving wheel can move to the butt joint position meeting the precision according to the driving force corresponding to the driving type driving force. Therefore, according to the control method of the track type transfer robot, provided by the embodiment of the invention, the driving force distribution strategy can be designed according to the load distribution of each wheel by acquiring the load information of each wheel end, the difference of the control effect of each wheel caused by the uneven load distribution is compensated, and the butt joint precision of the table surface track and the target track of the transfer robot is improved.
Detailed Description
It should be noted that, without conflict, the embodiments of the present invention and features of the embodiments may be combined with each other. The invention will be described in detail below with reference to the drawings in connection with embodiments.
In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in which it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
It should be noted that the terms "first," "second," and the like in the description and the claims of the present invention and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate in order to describe the embodiments of the invention herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
Because manual remote control is open-loop control, the control precision is poor, repeated calibration is needed for many times in the track butt joint stage, and the car body transfer efficiency is low. Because the weight of different transferring workpieces is greatly changed, uneven load is easily caused when the transferring workpieces are placed at the position of the transferring table, and the control effect of the bearing wheels at different positions is influenced; meanwhile, the problem of large time lag of an actuator exists in the large transfer device, and the problem of accurate parking of the transfer table is solved.
Based on this, in the present embodiment, there is provided a control system of a track-type transfer robot, as shown in fig. 1 and 2, including: the transfer robot 100, the position acquisition device 200, the load information acquisition device 300 and a control device 400 of the track type transfer robot described below, wherein the transfer robot 100 comprises a transfer table 1 and a plurality of driving wheel devices 4 positioned on the lower surface of the transfer table 1, each driving wheel device comprises a driving wheel 41 and a driving mechanism corresponding to the driving wheel device, the position acquisition device 200 is installed on the transfer table 1, one load information acquisition device 300 is installed on each driving wheel 41, and the position acquisition device 200, the load information acquisition device 300 and the driving wheel devices 4 are all in communication connection with the control device 400 of the track type transfer robot;
the position acquisition device 200 can acquire the real-time position of the transfer robot 100 in real time;
the load information acquisition device 300 is capable of acquiring load distribution information of each driving wheel;
the control device 400 of the track type transfer robot can determine a driving signal of a driving mechanism according to the load distribution information of each driving wheel and the real-time position of the transfer robot 100;
The driving mechanism can drive the motion of each driving wheel according to the driving signals so as to drive the migration table to move to the target butt joint position to finish butt joint, wherein the migration table is used for bearing the workpieces to be transported.
It should be understood that, in the embodiment of the present invention, the real-time position of the transfer robot is acquired by the position acquisition device 200, and the load information acquisition device 300 on each driving wheel acquires the load distribution information of each driving wheel, so that the control device 400 of the track type transfer robot can determine the driving force of each driving wheel according to the position information and the load distribution information, and each driving wheel device 4 can also feed back the current driving wheel state to the control device 400 of the track type transfer robot, so that the closed loop control on the driving wheel can be formed. Therefore, the control system of the track type transfer robot can control different driving forces according to the load distribution on each driving wheel, and eliminates the control difference of each driving wheel caused by uneven load distribution, thereby improving the butt joint precision of the transfer robot and the track.
In the embodiment of the present invention, the position acquisition device 200 may specifically include a laser radar, and the laser radar scanning result obtained during laser radar scanning is sent to the control device 400 of the track type transfer robot, where the control device 400 of the track type transfer robot can perform position calculation according to the laser radar scanning result, so as to obtain the real-time position of the transfer robot.
Of course, the position acquisition device 200 can also be implemented in a visual calibration mode, and the real-time position of the transfer robot is obtained by calibrating in combination with the visually acquired image.
With respect to the implementation form of the position acquisition device 200, embodiments of the present invention are not limited and may be selected according to needs.
In addition, the control device 400 of the track type transfer robot provided by the embodiment of the invention can be specifically realized by a single chip microcomputer, and of course, the control device can also be realized by an industrial personal computer, and the embodiment of the invention is not limited in specific realization form.
In the embodiment of the present invention, the position acquisition device 200 is exemplified by including a laser radar, the driving mechanism is exemplified by a driving motor, and the load information acquisition device 300 is exemplified by a load sensor. As shown in fig. 2 and 3, the transfer robot 100 includes a transfer table 1, a table track 2 on the upper surface of the transfer table, a control cabinet 9, a support column 7, and a laser radar 6 mounted on the column, a driving wheel device 4 mounted on the lower surface of the transfer table 1, where the driving wheel device includes a driving wheel and a driving mechanism, in this embodiment of the present invention, the driving mechanism is specifically a driving motor, a load sensor is mounted above each driving wheel, an industrial personal computer 10 is placed in the control cabinet 9, the industrial personal computer 10 includes a control device 400 of the track transfer robot, and the transfer robot runs on an external track 8.
In the embodiment of the invention, when the transfer robot transfers the workpiece, the transfer table 1 moves along the external track 8 in a one-dimensional direction, the transfer table 1 moves to the vicinity of the target track 11, and the workpiece is transferred from the table track 2 to the target track 11 by butting the table track 2 on the transfer table 1 with the target track 11. The control device 400 of the track type transfer robot calculates the position information of the transfer robot according to the laser radar scanning result, and obtains the load distribution information of each driving wheel according to the load sensor on each driving wheel, so that the driving force of each driving wheel can be determined, then the driving wheel is fed back in real time according to the state of the driving wheel which is connected with the driving motor of each driving wheel, the closed-loop control of the driving wheel is realized, the operation of the driving wheel can be finally realized, and the accurate butt joint of the table top track 2 and the target track 11 of the transfer robot is realized on the basis.
When the control device 400 of the track-type transfer robot precisely interfaces the control table track 2 with the target track 11, the maximum driving force constraint of each driving wheel and the maximum total driving force constraint of the robot are obtained by obtaining the load distribution information of each driving wheel according to the load sensor as shown in fig. 4 in order to solve the problem that the relative slip of the driving wheel and the large time lag of the actuator affect the control performance due to the uneven load of the transfer robot at the interface. And planning the position of the robot according to the maximum total driving force constraint of the robot, and performing position closed-loop PID control and butt joint completion judgment according to the position information of the robot of the positioning module. And designing a driving force distribution strategy, redistributing driving forces of the driving wheels calculated by PID according to the maximum driving force constraint of the driving wheels, performing robot yaw moment constraint on the distributed driving forces, compensating time lag of an actuator, and finally sending a control command to a driving motor.
As another embodiment of the present invention, there is provided a control device 400 of a track-type transfer robot, which is applied to the control system of the track-type transfer robot, for implementing a control method of the track-type transfer robot, as shown in fig. 5, including:
an acquisition module 410, configured to acquire real-time position of the transfer robot and load distribution information of each driving wheel respectively;
a first construction module 420, configured to construct a maximum driving force constraint of each driving wheel according to load distribution information of each driving wheel, and construct a total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel;
a control module 430 for controlling docking of the transfer robot according to the transfer robot total driving force constraint and the real-time position of the transfer robot;
a second construction module 440 for constructing a driving force distribution strategy for each driving wheel according to a difference between the real-time position and the target docking position of the transfer robot in combination with a maximum driving force constraint for each driving wheel;
a generating module 450 for generating a driving signal according to the driving force distribution strategy of each driving wheel, so that the driving mechanism of each driving wheel drives the motion of the corresponding driving wheel according to the driving signal.
In the embodiment of the present invention, it should be understood that, after the acquiring module 410 acquires the real-time position of the transfer robot and the load distribution information of each driving wheel, the first constructing module 420 constructs the maximum driving force constraint of each driving wheel according to the load distribution information of each driving wheel, so as to construct the total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel, the control module 430 can control the docking of the table track of the transfer robot with the target track according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot, and in order to improve the docking precision, the second constructing module 440 can construct the driving force distribution strategy of each driving wheel according to the difference between the real-time position of the transfer robot and the target docking position and in combination with the maximum driving force constraint of each driving wheel, so as to generate the driving signal of each driving wheel according to the driving force distribution strategy, so that the driving mechanism of each driving wheel can move to the docking position meeting the precision according to the driving force corresponding to the driving force of the driving wheel.
Therefore, the control device of the track type transfer robot provided by the embodiment of the invention can design a driving force distribution strategy according to the load distribution of each wheel by acquiring the load information of each wheel end, compensate the difference of the control effect of each wheel caused by the uneven load distribution and improve the butt joint precision of the table surface track and the target track of the transfer robot.
The following describes the specific process of the control device of the transfer robot according to the embodiment of the present invention in detail in connection with the control method of the large-lane transfer robot.
As another embodiment of the present invention, there is provided a control method of a rail type transfer robot, which is applied to a transfer robot including a transfer table and a plurality of driving wheel devices located at a lower surface of the transfer table, each of the driving wheel devices including a driving wheel and a driving mechanism corresponding thereto, as shown in fig. 6, the control method of the rail type transfer robot including:
s100, respectively acquiring the real-time position of the transfer robot and the load distribution information of each driving wheel;
in the embodiment of the present invention, as described above, scanning may be achieved by installing a laser radar on the transfer robot, and calculating the real-time position according to the scanning result to obtain the real-time position of the transfer robot, and the process of calculating the real-time position according to the laser radar scanning result is well known to those skilled in the art and will not be repeated herein. In addition, load distribution information of each driving wheel is obtained by installing a load sensor on each driving wheel.
S200, constructing maximum driving force constraint of each driving wheel according to the load distribution information of each driving wheel, and constructing total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel;
In the embodiment of the invention, the maximum driving force of each driving wheel can be obtained through the load distribution information of each driving wheel, and further the maximum driving force constraint of each driving wheel can be obtained, so that the driving force distributed by each driving wheel can meet the requirement of the maximum driving force constraint when a driving force distribution strategy is carried out later, and the normal operation of each driving wheel can be ensured and the control difference of each driving wheel caused by uneven load distribution can be eliminated.
S300, controlling the butt joint of the transfer robot according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot;
in the embodiment of the invention, the running position of the transfer robot is closed-loop controlled according to the total driving force constraint and the real-time position of the transfer robot, so that the accurate butt joint of the transfer robot can be ensured.
S400, constructing a driving force distribution strategy of each driving wheel according to the difference value between the real-time position and the target docking position of the transfer robot and combining the maximum driving force constraint of each driving wheel;
when closed-loop control is performed, the driving force of each driving wheel is distributed through the feedback result of the closed-loop control to construct a driving force distribution strategy, so that each driving wheel can obtain the current optimal driving force to meet the requirement of final accurate butt joint.
S500, generating a driving signal according to a driving force distribution strategy of each driving wheel, so that a driving mechanism of each driving wheel drives the corresponding driving wheel to move according to the driving signal.
After the driving signals are generated according to the driving force distribution strategy, the driving mechanism of each driving wheel can drive the corresponding driving wheel to move according to the driving signals, so that accurate butting is completed.
In summary, according to the control method of the track type transfer robot provided by the embodiment of the invention, the maximum driving force constraint of each driving wheel can be constructed according to the load distribution information of each driving wheel, so that the total driving force constraint of the transfer robot is constructed according to the maximum driving force constraint of each driving wheel, further, the butt joint of the table top track and the target track of the transfer robot is controlled according to the total driving force constraint of the transfer robot and the real-time position of the transfer robot, in order to improve the butt joint precision, the driving force distribution strategy of each driving wheel is constructed according to the difference value between the real-time position and the target butt joint position of the transfer robot and combined with the maximum driving force constraint of each driving wheel, so that the driving signal of each driving wheel is generated according to the driving force distribution strategy, and therefore, the driving mechanism of each driving wheel can move to the butt joint position meeting the precision according to the driving force corresponding to the driving type driving force. Therefore, according to the control method of the track type transfer robot, provided by the embodiment of the invention, the driving force distribution strategy can be designed according to the load distribution of each wheel by acquiring the load information of each wheel end, the difference of the control effect of each wheel caused by the uneven load distribution is compensated, and the butt joint precision of the table surface track and the target track of the transfer robot is improved.
In an embodiment of the present invention, constructing a maximum driving force constraint of each driving wheel according to load distribution information of each driving wheel, and constructing a total driving force constraint of the transfer robot according to the maximum driving force constraint of each driving wheel, including:
calculating the maximum driving force of each driving wheel according to the load distribution information of each driving wheel, wherein the calculation formula of the maximum driving force of each driving wheel is as follows:
,
wherein i represents the number of the driving wheel;represents the maximum driving force of the i-th driving wheel; />Representing the load mass of the ith drive wheel; g represents a gravitational constant; />Representing the coefficient of friction;
determining the maximum driving force constraint of each driving wheel and the total driving force constraint of the transfer robot according to the maximum driving force of each driving wheel and the maximum output driving force of the driving mechanism corresponding to the driving force,
,
wherein ,maximum driving force constraint for the ith driving wheel, for example>Maximum driving force for the ith driving wheel; />Represents the maximum output driving force of the driving mechanism; />Representing the total driving force constraint of the transfer robot; n represents the number of driving wheels.
As a specific embodiment of the present invention, controlling docking of a transfer robot according to a transfer robot total driving force constraint and a real-time position of the transfer robot includes:
Determining a desired position of the transfer robot according to the total driving force constraint of the transfer robot, wherein the desired position of the transfer robot is expressed as follows:
,
wherein ,i represents the number of the driving wheel; />The acceleration of the transfer robot is represented, and the acceleration in the uniform deceleration and uniform acceleration stages is set to be the same; />Representing the total driving force constraint of the transfer robot; />Representing the i-th drive wheel load mass; />Representing a desired position of the transfer robot, t representing time; />A time point for indicating the end of the uniform acceleration phase of the transfer robot; />A time point for starting a uniform deceleration stage of the transfer robot is shown;
determining a moving path of the transfer robot according to the expected position of the transfer robot;
when the transfer robot moves to the target docking position according to the moving path, judging whether the difference value between the real-time position of the transfer robot and the target docking position meets a docking completion condition, wherein the expression of the docking completion condition is as follows:
,
wherein ,representing a target docking position; />Real-time position for transfer robot; />Representing the docking accuracy;
and if the docking completion condition is met, determining that the transfer robot completes docking.
It should be understood that in the embodiment of the present invention, the determination of the docking completion condition may be performed according to the real-time position of the transfer robot, that is, when the real-time position of the transfer robot reaches the vicinity of the target docking position and satisfies the docking completion condition, the docking completion condition is determined, that is, the difference between the real-time position of the transfer robot and the target docking position is required to satisfy the docking precision. It should be understood herein that the target docking position is the docking position of the target rail and the table rail of the transfer robot described above.
According to the position planning schematic diagram shown in fig. 7 and the above formula of the docking completion condition, the real-time position of the transfer robot can be slightly larger or slightly smaller than the target docking position, so that the docking completion precision of the transfer robot is as follows。
In an embodiment of the present invention, in order to enable control of each driving wheel to meet a position planning requirement, a driving force distribution strategy of each driving wheel is constructed according to a difference value between a real-time position and a target docking position of the transfer robot and in combination with a maximum driving force constraint of each driving wheel, including:
determining a position closed-loop control incremental PID control law according to the difference value between the real-time position and the target docking position of the transfer robot, wherein the position closed-loop control incremental PID control law has the following expression:
,
wherein ,k represents a time sequence number; />The increment of the total driving force of the transfer robot at the moment k is represented; />Representing a scaling factor; />Representing an integral coefficient; />Representing the differential coefficient; />Representing a position error of a transfer robotDifference; />Representing a desired position of the transfer robot; />Representing a real-time position of the transfer robot; />Representing the position error of the transfer robot at the moment k; />The total driving force of the transfer robot at the moment k is represented; / >Representing the total driving force of the transfer robot at the moment k-1;
and distributing the total driving force of the transfer robot according to the position closed-loop control incremental PID control law and the maximum driving force constraint of each driving wheel, and obtaining the driving force corresponding to each driving wheel.
Further specifically, the allocation of total drive force to the transfer robot according to the position closed loop control incremental PID control law and the maximum drive force constraint for each drive wheel includes:
calculating the average driving force of each driving wheel according to the total driving force of the transfer robot, wherein the calculation formula of the average driving force is as follows:
,
wherein ,represents an average driving force; />Indicating the total driving force of the transfer robot; n represents the number of driving wheels;
comparing the maximum driving force constraint of each driving wheel with the average driving force to determine whether there is a situation in which the average driving force exceeds the maximum driving force constraint of at least one driving wheel;
if so, setting the driving force of the at least one driving wheel as the maximum driving force of the driving wheels, and distributing the remaining undivided driving force of the total driving force of the transfer robot to the remaining driving wheels according to a preset distribution rule, wherein the remaining driving wheels comprise driving wheels with the maximum driving force constraint not exceeded by the average driving force;
If the total driving force does not exist, the total driving force of the transfer robot is distributed to all driving wheels according to a preset distribution rule.
In the embodiment of the present invention, it may be specifically understood that the total driving force of the obtained robot is distributed according to the maximum driving force constraint of each driving wheel, and the driving force distribution strategy is designed as follows:
(1) Calculating the average driving force of each driving wheel according to the total driving force of the transfer robot:
,
wherein ,represents an average driving force; />Indicating the total driving force of the transfer robot; n represents the number of driving wheels.
Here, the average driving force is 200/4=50, taking the total driving force of the transfer robot as 200 and the number of driving wheels as 4 as an example. It should be understood herein that the embodiment of the present invention is not limited to the number of driving wheels, and may be selected as desired.
(2) If the average driving force exceeds the maximum driving force constraint of some of the driving wheels, the driving forces of these driving wheels are made equal to the driving wheel maximum driving force,
,
wherein the subscript h represents the drive wheel number for which the average drive force exceeds the maximum drive force constraint;represents the driving force of the h-th driving wheel, +.>Represents the maximum driving force of the h-th driving wheel, < >>Representing the average driving force.
Here, for example, if the average driving force is 50 and the maximum driving force of the 2 nd (h=2) driving wheel is 49, the driving force of the 2 nd driving wheel is set to 49.
(3) Since the average driving force exceeds the maximum driving force of the 2 nd driving wheel, the driving forces of the remaining 1 st, 3 rd and 4 th driving wheels are not exceeded by the average driving force, and thus the driving forces of the remaining 1 st, 3 rd and 4 th driving wheels distribute the total driving force of the transfer robot according to the ratio of the maximum driving force of the respective driving wheels, namely
,
Wherein, the subscript i represents a driving wheel serial number;representing the remaining transfer robot driving force; />Indicating the total driving force of the transfer robot; />Represents the driving force of the i-th driving wheel; />Representing the maximum driving force constraint of the ith driving wheel; />Represents an average driving force; n represents the number of driving wheels; h' represents the number of driving wheels whose average driving force exceeds the maximum driving force constraint.
Here, for example, if the number of driving wheels h' whose average driving force exceeds the maximum driving force constraint is 1, at this time, since it has been calculated that the driving force of the 2 nd driving wheel is set to 49, therefore,here 49, the remaining transfer robot driving force +. >200-49=151. That is, the driving forces of the remaining 1 st, 3 rd and 4 th driving wheels are proportionally distributed with the remaining transfer robot driving force +.>。
Here, it may be set that the maximum driving force of the 1 st driving wheel is 51, the maximum driving force constraint of the 3 rd driving wheel is 52, and the maximum driving force constraint of the 4 th driving wheel is 51, then the driving force of the 1 st driving wheel at this timeCan be specifically provided withThe calculation result is->That is, at this time, the driving force of the 1 st driving wheel is set to 50, and the driving force of the 3 rd driving wheel is sequentially calculated in this way to be set toThe driving force of the 4 th driving wheel is set to +.>。
It should be understood herein that if the maximum of any one of the drive wheelsThe driving forces are not exceeded by the average driving force, i.e. the maximum driving force of any one driving wheel is greater than the average driving force, at this timeI.e. the 4 driving wheels distribute the total driving force of the transfer robot according to the proportion of the respective maximum driving force.
In the embodiment of the present invention, after the total driving force of the transfer robot passes through the driving force distribution strategy, the distribution driving force of each driving wheel still has a difference, which may cause a yaw moment to exist, and further cause abrasion and even derailment of the driving wheels and the track, so, in order to avoid the occurrence of the yaw moment, in the embodiment of the present invention, a driving signal is generated according to the driving force distribution strategy of each driving wheel, including:
Constructing a yaw moment constraint strategy according to the driving force distribution strategy of each driving wheel;
determining a desired drive torque for each drive wheel according to a yaw moment constraint strategy;
a drive signal for each drive wheel is generated based on the desired drive torque for each drive wheel.
Further specifically, constructing a yaw moment constraint strategy according to the driving force distribution strategy of each driving wheel includes:
calculating a yaw moment of the transfer robot according to the distributed driving force of each driving wheel;
judging whether the yaw moment of the transfer robot exceeds a preset yaw moment constraint;
if the yaw moment of the transfer robot exceeds the preset yaw moment constraint, determining a driving force adjustment mode of the driving force according to the driving force adjustment rule,
wherein the driving force setting rule includes:
if the yaw moment generated by the upper side and the lower side are opposite in direction, the driving force of the largest driving wheel in all driving wheels is reduced;
if the yaw moment directions generated by the upper side and the lower side are the same, respectively reducing the driving force of one driving wheel with the largest driving force on the upper side and the lower side;
wherein, use the one side that transfer robot is close to transfer robot direction of advance as the upside, the one side that deviates from the direction of advance is the downside.
In the embodiment of the present invention, (1) the yaw moment of the transfer robot caused by the distributed driving force of each driving wheel is calculated first. The specific calculation process may be shown in fig. 8, where the transfer robot is further divided into an upper side and a lower side as shown in fig. 2 and 3, where the dividing line is a dividing line parallel to the direction of the table track of the transfer robot, as shown in fig. 2, the dotted line L represents a dividing line, and the transfer robot is divided into two parts by the dotted line L, where one side close to the advancing direction X is the upper side, and the other side is the lower side. The yaw moment caused by the difference in driving force of each wheel can be expressed as:
,
wherein ,representing yaw moment generated on the upper side; />Representing yaw moment generated at the lower side; />Representing a yaw moment of the transfer robot; />A moment generated for the center of the ith driving wheel pair; />Indicating the wheel tread on the left and right sides.
(2) Setting yaw moment constraint of the transfer robot:
,
wherein ,representing a yaw moment of the transfer robot; />Maximum values are allowed for the transfer robot yaw moment.
(3) And if the yaw moment of the transfer robot exceeds the yaw moment constraint, setting a driving force setting rule.
When the yaw moment generated at the upper and lower sides is opposite, the driving force of one wheel with the largest four-wheel driving force is reduced:
,
wherein ,representing maximum value calculation; />Driving force for the i-th driving wheel; />Representing a yaw moment of the transfer robot; />Representing a transfer robot yaw moment allowable maximum value; />Indicating the wheel tread on the left and right sides.
When the yaw moment directions generated by the upper side and the lower side are the same, the driving force of one wheel with the largest driving force on the upper side and the lower side is respectively reduced:
,
wherein ,representing maximum value calculation; />Driving force for the i-th driving wheel; />Representing a yaw moment of the transfer robot; />Representing a transfer robot yaw moment allowable maximum value; />Representing yaw moment generated on the upper side of the transfer robot; />Representing yaw moment generated at the lower side of the robot; />Indicating the wheel tread on the left and right sides.
(4) Calculating a desired drive torque for each drive wheel:
,
wherein ,indicating the desired driving force distance of the i-th driving wheel; />Represents the driving force of the i-th driving wheel; r represents the drive wheel radius.
In the embodiment of the invention, for the transfer robot, the parking error and the docking precision in the deceleration docking stage are particularly important, and the speed tracking precision in the uniform acceleration stage is not required to be high, so that the time lag estimation of the driving wheel is carried out in the uniform acceleration stage of the robot, and the estimated time lag information is utilized to compensate the time lag of the actuator in the uniform deceleration stage.
Specifically, the driving signal is generated according to the driving force distribution strategy of each driving wheel, and further includes:
an actuator time lag compensation strategy is constructed according to a yaw moment constraint strategy, wherein the actuator time lag compensation strategy comprises the steps of estimating time lag of each driving wheel in a uniform acceleration stage of the transfer robot to obtain time lag estimation information, and compensating the actuator time lag according to the time lag estimation information in a uniform deceleration stage of the transfer robot;
and generating a driving signal of each driving wheel according to the actuator time lag compensation strategy.
It should be understood that, in the embodiment of the invention, the response time lag of the actuator is mainly the time lag effect caused by slow response of the driving wheel when the driving motor sends torque to the driving wheel, so that the time lag information is estimated in the uniform acceleration stage of the transfer robot, and the time lag is compensated according to the time lag estimation information in the uniform deceleration stage to eliminate the time lag effect, thereby improving the docking precision of the final transfer robot with the target track when the transfer robot is parked.
Referring to fig. 9, the desired torque and the actual torque of the driving wheel at each moment and the actual torque at the previous moment are recorded, and the actual time lag time obtained at the current moment and the actual time lag time at the previous moment are distributed by weights to obtain the estimated time lag time at the current moment. The method is obtained by solving the following formula:
,
Wherein k represents a time sequence number;representing the actual time lag time of the actuator at the kth moment; />Representing a system sampling time; />Representing the actual driving torque at the kth moment; />Representing the actual driving torque at the moment immediately above the kth moment; />Representing the desired drive torque at the kth time; />An actuator estimated time lag time representing a kth time; />Weight of kth time is expressed, let +.>;/>The actual time lag of the actuator at the moment immediately above the kth moment is shown.
According to the response time lag of each driving wheel, a control signal which can respond to the expected control quantity without time lag is reversely obtained through the reciprocal of a first-order inertia function, the control signal is used as a compensation control signal, meanwhile, in order to prevent the compensation driving moment from being overlarge, a weight coefficient is set, and the final output driving moment control quantity of each driving wheel in a deceleration stage is obtained after filtering:
,
wherein k represents a time sequence number; the subscript i represents the drive wheel serial number;representing the compensation driving moment of the ith driving wheel at the moment k; />Representing the final output driving torque of the ith driving wheel at the moment k; />Indicating the time lag of the ith drive wheelTime; />Representing a system sampling time; />Indicating the desired torque of the ith driving wheel at the time k; />Representing the actual driving moment of the ith driving wheel at the moment of the kth moment; p represents a weight coefficient, get- >。
In summary, according to the control method of the track type transfer robot provided by the embodiment of the invention, load distribution information of each driving wheel is obtained from the load sensor, and the maximum driving force constraint of each driving wheel and the maximum total driving force constraint of the robot are obtained. And planning the position of the robot according to the constraint of the maximum total driving force, and performing position closed-loop PID control and butt joint completion judgment according to the position information of the robot of the positioning module. And designing a driving force distribution strategy, redistributing driving forces of the driving wheels calculated by PID according to the maximum driving force constraint of the driving wheels, performing robot yaw moment constraint on the distributed driving forces, resetting the driving forces of the driving wheels through a setting rule when the yaw moment constraint is exceeded, compensating time lag of an actuator, and finally sending a control command to a driving motor. In addition, in order to eliminate the time lag influence, time lag estimation is carried out according to the expected control quantity and the actual control quantity of each driving wheel in the robot uniform acceleration stage, the time lag time of each driving wheel is obtained, time lag compensation is carried out on each driving wheel in the robot deceleration docking stage according to the time lag information of each driving wheel, and the control response lag is eliminated.
Therefore, according to the control method of the track type transfer robot, provided by the invention, the load distribution information is obtained according to the load sensors on each driving wheel, and the driving force distribution strategy is designed, so that the control difference of each driving wheel caused by uneven load distribution is eliminated; the time lag compensation strategy is designed, response time lag estimation of each driving wheel is carried out in the acceleration stage of the robot to obtain time lag information, time lag compensation solution is carried out in the deceleration docking stage of the robot according to the time lag information to obtain final control quantity, control response lag caused by the time lag of an actuator is eliminated, response speed of the driving wheels is accelerated, and docking precision of the robot in parking and rails is improved.
The control method of the track type transfer robot in the embodiment of the invention is not limited by the number of external target tracks, and the number of table tracks of the transfer robot is not limited.
It is to be understood that the above embodiments are merely illustrative of the application of the principles of the present invention, but not in limitation thereof. Various modifications and improvements may be made by those skilled in the art without departing from the spirit and substance of the invention, and are also considered to be within the scope of the invention.