Motion control of motion devices
The S-curve trajectory method addresses stability issues in material handling devices by optimizing motion control profiles, ensuring smooth and stable movement, reducing vibrations and item scatter.
Patent Information
- Application Number
- JP2023539009
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- Priority Date
- 2020-12-24
- Filing Date
- 2021-12-21
- Publication Date
- 2024-12-09
- Estimated Expiration
- 2041-12-21
AI Technical Summary
Existing material handling devices in storage systems experience stability issues during acceleration and deceleration, leading to vibrations and potential overturning, especially when transporting perishable items, due to unsynchronized wheel speeds and sudden movements.
A method for generating a time-optimal motion control profile using an S-curve trajectory that includes a continuous jerk reference to ensure smooth and accurate point-to-point movement by calculating trajectory segments based on jerk, acceleration, velocity, and position constraints, using a root-finding algorithm to minimize positional deviation.
The S-curve trajectory profile significantly reduces vibrations and ensures stable movement of material handling devices, preventing item scatter and enhancing the handling of delicate goods by maintaining smooth acceleration and deceleration.
Smart Images

Figure 0007596542000033 
Figure 0007596542000034 
Figure 0007596542000035
Abstract
Description
[Technical field]
[0001] The present invention relates to the field of storage or fulfillment systems in which stacks of bins or containers are arranged within a grid framework structure, and more particularly to generating and controlling the movement of material handling equipment operable to move one or more containers stored in the storage or fulfillment system. [Background technology]
[0002] Storage systems are known that comprise a three-dimensional storage grid structure in which storage containers / bins are stacked on top of each other. PCT Publication No. WO2015 / 185628A (Ocado) describes a known storage and fulfillment system in which a stack of bins or containers is arranged within a grid framework structure. The bins or containers are accessed by material handling equipment (also called bots) operable on a truck located on top of the grid framework structure. A system of this type is shown diagrammatically in Figures 1 to 3 of the accompanying drawings.
[0003] As shown in Figures 1 and 2, stackable containers, called storage containers or totes or bins 10, are stacked on top of each other to form a stack 12. The stacks 12 are arranged within a grid framework structure 14 in a warehousing or manufacturing environment. The grid framework structure is made up of a number of storage or grid rows. Each grid of the grid framework structure has at least one grid row for storage of a stack of containers. Figure 1 is a schematic perspective view of the grid framework structure 14, and Figure 2 is a top view showing the stack 12 of bins 10 arranged within the framework structure 14. Each bin 10 typically holds multiple product items (not shown), which may be the same or may be different product types depending on the application.
[0004] The grid framework structure 14 comprises a plurality of upright members 16 supporting horizontal members 18, 20. A first set of parallel horizontal grid members 18 are arranged orthogonally with a second set of parallel horizontal members 20 in a grid pattern to form a plurality of horizontal grid structures 15 supported by the upright members 16. The members 16, 18, 20 are typically fabricated from metal. As the bins 10 are stacked between the members 16, 18, 20 of the grid framework structure 14, the grid framework structure 14 prevents horizontal movement of the stack 12 of bins 10 and guides vertical movement of the bins 10.
[0005] The top level of the grid framework structure 14 comprises a grid or grid structure 15 including rails 22 arranged in a grid pattern across the top of the stacks 12. With further reference to FIG. 3, the rails or tracks 22 guide a plurality of load handling devices 30. A first set 22a of parallel rails 22 guide movement of the robotic load handling device 30 in a first direction (e.g., X direction) across the top of the grid framework structure 14, and a second set 22b of parallel rails 22 arranged orthogonally to the first set 22a guide movement of the robotic load handling device 30 in a second direction (e.g., Y direction) orthogonal to the first direction. In this manner, the rails 22 allow the robotic load handling device 30 to move laterally in two directions in a horizontal XY plane, so that the load handling device 30 can be moved into position above any of the stacks 12.
[0006] A known load handling device 30 shown in Figures 4 and 5 comprising a vehicle body 32 is described in PCT Patent Publication No. WO2015 / 019055 (Ocado), incorporated herein by reference, where each load handling device 30 covers only one grid space of the grid framework structure 14. Here, the load handling device 30 comprises a wheel assembly comprising a first set of wheels 34 consisting of a pair of wheels at the front of the vehicle body 32 and a pair of wheels 34 at the rear of the vehicle body 32 that engage with a first set of rails or tracks to guide the movement of the device in a first direction, and a second set of wheels 36 consisting of a pair of wheels 36 on each side of the vehicle body 32 that engage with a second set of rails or tracks to guide the movement of the device in a second direction. Each of the sets of wheels is driven to enable the movement of the vehicle in the X and Y directions respectively along the rails. One or both sets of wheels can be moved vertically to allow the vehicle to move in a desired direction on the grid by lifting each set of wheels from their respective rails.
[0007] The loading and unloading apparatus 30 includes a lifting apparatus or crane mechanism for lifting the storage container from above. The crane mechanism includes a winch tether or cable 38 wound on a spool or reel (not shown) and a gripping device 39. The lifting apparatus includes a set of vertically extending lifting tethers 38 connected at or near the four corners of a lifting frame 39, referred to as the gripping device (one tether near each of the four corners of the gripping device) for releasably connecting with the storage container 10. The gripping device 39 is configured to releasably grip the top of the storage container 10 to lift it from a stack of containers in a storage system of the type shown in Figures 1 and 2. One or more motors may be used to power the lifting apparatus.
[0008] The wheels 34, 36 are arranged around a cavity or recess in the lower portion, called a container-receiving recess or container-receiving space 40. The recess is sized to accommodate the container 10 when it is lifted by the crane mechanism, as shown in Figures 5(a and b). When in the recess, the container is lifted from the rails below so that the vehicle can move laterally to different positions. Once the target location, for example another stack, an access point in a storage system, or a conveyor belt, is reached, the bin or container can be lowered from the container-receiving portion and released from the gripping device.
[0009] Typically, one or more materials handling devices remotely operable on the grid structure are configured to receive instructions from a master controller to retrieve a storage container from a particular storage location within the grid framework structure. Wireless communications and networks may be used to provide a communications infrastructure from the master controller through one or more base stations to one or more materials handling devices operable on the grid structure. In response to receiving the instructions, a controller within the materials handling device is configured to control various drive mechanisms to control the movement of the materials handling device. For example, the materials handling device may be commanded to retrieve a container from a storage row at a particular location on the grid structure. The instructions may include various movements in the XY directions on the grid structure. Once at the storage row, a lifting mechanism is then operated to grab the storage container and lift the storage container into a container receiving space within the body of the materials handling device, where it is subsequently transported to another location, commonly referred to as a drop-off port, on the grid structure. The storage container is lowered to an appropriate picking station to enable removal of the item from the storage container. The movement of the materials handling device on the grid structure also includes the materials handling device being commanded to move to a charging station, typically located on the periphery of the grid structure.
[0010] To enable the controller to locate the loading equipment relative to the grid structure, each loading equipment may be equipped with one or more RFID tag readers or scanners, and multiple RFID tags may be provided across the top of the grid structure. The RFID tags may be fixed relative to the grid structure to provide a series of reference points on the grid structure. As the loading equipment moves across the grid structure, the RFID tag reader of each loading equipment reads signals from one or more of the RFID tags fixed at various locations on the grid structure. Typically, the RFID tags are fixed at track junctions or intersections in each grid cell.
[0011] To steer the loading equipment from one destination to another on the grid structure, each loading equipment includes a motor for driving the wheels of the loading equipment. The rotation of the wheels may be driven by one or more belts connected to the wheels or may be driven separately by a motor integrated in the wheels. Typically, in single-cell loading equipment, where the footprint of the loading equipment occupies one grid cell, the motor for driving the wheels is integrated in the wheels due to the limited availability of space in the vehicle body. The wheels of the single-cell loading equipment are driven by hub motors. Each hub motor includes an outer rotor including a plurality of permanent magnets arranged to rotate around a wheel hub including a coil that constitutes an inner stator.
[0012] One of the main drawbacks of a single-cell loading device is the stability of the loading device when moving on the track, which presents a problem especially during acceleration / deceleration of the loading device on the track. High acceleration or deceleration on the track can also cause the loading device to vibrate or tip over on the grid structure. Usually, the rotation of the loading device wheels is controlled individually so that the loading device is driven in a straight line on the track without yaw or deviation from its intended course.
[0013] Since the wheels are the same size, the wheels generally rotate at the same speed as the loading equipment moves on the truck. However, there are several factors that can affect the smoothness of the loading equipment movement on the truck and increase the likelihood that the loading equipment will yaw or drive at an off-lead angle. Such factors can include slippage of one or more wheels, sliding and / or loading of one or more of the wheels differently, all of which contribute to one or more of the wheels rotating at different speeds. Additionally, sudden acceleration changes of the loading equipment on the truck, also known as jerks, also affect the smoothness of the loading equipment movement on the truck. When transporting perishable or damageable items, such as foodstuffs, the sudden movement of the loading equipment when accelerating or decelerating can cause the items to be scattered within the storage container.
[0014] EP3535633 (Autostore Technology AS) teaches a method and a controller for controlling the movement of a robot from a start position to a stop position, the robot having a pair of wheels with drive means controlled by a local controller, where the master controller is configured to set individual speed drive sequences for the pair of wheels based on the current speed and angular position of each wheel, the current global position of the robot, and the start and stop positions of the robot. The speed drive sequences are sent to the local controller that controls the pair of wheels, thereby controlling the acceleration and deceleration of each wheel via the individual drive means. In EP3535633 (Autostore Technology AS), the local controller ensures that the speeds of the pair of wheels are kept synchronized. Thus, the teachings of EP3535633 (Autostore Technology AS) are very focused on controlling the individual speeds of the wheels and ensuring that the speeds of the pair of wheels are synchronized.
[0015] It is therefore an object of the present invention to provide a method and device for controlling the movement of a load handling equipment from a start position to a stop position with the aim of ensuring smooth movement of the load handling equipment when transitioning between different speeds on a grid structure.
[0016] This application claims priority to GB applications GB2020680.1, GB2020668.6, GB2020681.9, and GB2020684.3, all filed on December 24, 2020, the contents of which are incorporated herein by reference. Summary of the Invention
[0017] The present invention has alleviated the above problems by providing a system and method for efficiently generating time-optimal motion control profiles based on constraints, more particularly motion control profiles that are generated in real-time within a control system to achieve time-optimal point-to-point moves. To achieve smooth and accurate point-to-point moves, the present invention provides a trajectory generator or motion control generator that calculates a trajectory or motion control profile based on an S-curve profile. The trajectory generator generates a profile with a time-continuous jerk reference for at least one acceleration or deceleration segment of the motion control profile. By calculating a motion control profile that includes a time-varying jerk reference, the trajectory generator of the present invention can produce smooth and stable movements.
[0018] More particularly, the present invention provides a method for generating a trajectory for controlling movement of a motion device, comprising the steps of: i) receiving a specification of a trajectory, the specification comprising a command position; ii) receiving at least one jerk constraint; and iii) using at least one jerk constraint to generate a sequence of one or more trajectory segments, each of the one or more trajectory segments being a respective portion of said trajectory defining a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time; Equipped with The present invention provides a method in which one or more trajectory segments of a sequence of trajectory segments are generated based on predicting values of a parameter using a root-finding algorithm to find a root of an objective function, the objective function being an amount of position deviation from a commanded position of a magnitude of a position reference when a velocity reference and an acceleration reference are substantially zero, and the root corresponds to a value of the parameter at which the position deviation from the commanded position is less than a predetermined threshold.
[0019] Preferably, the method further comprises the step of receiving at least one velocity constraint, at least one acceleration constraint, and / or at least one deceleration constraint, wherein said at least one velocity constraint, at least one acceleration constraint, at least one deceleration constraint, and at least one jerk constraint define a plurality of constraints for generating a sequence of one or more trajectory segments.
[0020] Optionally, a value of at least one of the plurality of constraints is a function of a total mass of the loading equipment.
[0021] Optionally, a value of any one of the at least one velocity constraint, the at least one acceleration constraint, and / or the at least one deceleration constraint is inversely proportional to a total mass of the load handling equipment.
[0022] Preferably, the root finding method is one of Newton's root finding, secant root finding, bisection root finding, root finding by interpolation, root finding by inverse interpolation, Brent's root finding, Boudin-Fourier root finding, and Strum-chain root finding.
[0023] In the present invention, the terms "position reference" and "position" are used interchangeably to refer to the same characteristic. Similarly, the terms "velocity reference" and "velocity" are used interchangeably to refer to the same characteristic, the terms "acceleration reference" and "acceleration" are used interchangeably to refer to the same characteristic, and the terms "jerk reference" and "jerk" are used interchangeably to refer to the same characteristic. Preferably, the predetermined threshold is substantially zero, e.g., at the commanded position.
[0024] Preferably, each of the one or more trajectory segments has a start point and an end point, the start point of each of the one or more trajectory segments has an initial position reference, an initial velocity reference and an initial acceleration reference, and the end point of each of the one or more trajectory segments has an end point position reference, an end point velocity reference and an end point acceleration reference, and the parameters are based on a speed reference trajectory point having a desired optimal speed reference value.
[0025] The trajectory point, which is the value of the trajectory at any point, can be the point of the trajectory at the start or end point. Thus, the parameter can be the initial velocity reference or the end point reference. A root-finding algorithm is utilized to find the optimal parameters so that the trajectory can exactly reach the commanded position. In the present invention, "exactly reaching" on the commanded position or "exactly reaching" the commanded position covers the position reference of the trajectory with a position deviation from the commanded position of less than 5 mm, more preferably less than 1 mm, and even more preferably less than 0.5 mm.
[0026] More preferably, the parameter corresponds to a speed reference having a value substantially less than or equal to the speed constraint.
[0027] The optimum velocity reference represents a velocity reference having a magnitude such that the trajectory can be brought down to the commanded position within the trajectory constraints.
[0028] Preferably, one or more trajectory segments of the sequence of trajectory segments are generated by applying a speed transition algorithm comprising moving a magnitude of a speed reference of one or more trajectory segments at a given time to a desired final speed reference, the difference between the desired final speed reference and the speed reference defining a delta-V; The velocity transition algorithm comprises calculating a peak acceleration having a magnitude that is a function of delta-V.
[0029] In the present invention, the term "moving" covers "fitting". The process of generating a finite jerk (S-curve) trajectory relies heavily on solving what the present invention calls a velocity transition algorithm. The trajectory is generated based on multiple constraints using the S-curve mathematical formula. The process of generating one or more trajectory segments when generating a trajectory involves applying the S-curve trajectory mathematical formula. By devising a motion control profile or trajectory profile for a point-to-point move, taking into account the magnitude of the trajectory acceleration and whether the trajectory overshoots or undershoots the commanded position, the present invention fits the desired final velocity v from an initial trajectory point. f We have devised a velocity transition algorithm to determine the trajectory segments necessary to bring the orbit to a point where the magnitude of the orbital acceleration is essentially zero, where the desired final velocity v f To reach this, the magnitude of the orbital acceleration needs to be brought to essentially zero, otherwise the orbit will not move to the desired final velocity v f The key feature of the speed transition algorithm is to accelerate away from the desired final speed reference v f and the magnitude of the initial velocity reference v0, i.e., Δv=v f-v0 is that of some delta V that represents the velocity transition algorithm. The value of the velocity reference can be evaluated at any point along the trajectory and depends on the point in the trajectory at which the velocity transition algorithm is applied. For example, initially at the start of the trajectory, the velocity reference v0 can be an initial velocity reference, or it can be a desired final velocity v f If it is necessary for one or more trajectory segments to be generated before transitioning to the desired final speed reference vf (i.e. to address i) excessive acceleration, ii) the trajectory accelerating away from the desired final speed reference vf, or iii) the trajectory overshooting the desired final speed reference vf), the speed reference in determining delta Vv should be greater than or equal to the desired final speed reference vf. f The terminal velocity reference v in the last or previous trajectory segment when transitioning to E It is.
[0030] Thus, the value of delta-V Δv may vary depending on where in the trajectory the velocity transition algorithm according to the present invention is being applied. Preferably, delta-V is based on the difference between a desired final velocity reference and either an initial velocity reference or a terminal velocity reference of one or more trajectory segments.
[0031] In the present invention, delta V Δv is the peak acceleration a peak The peak acceleration is determined from a virtual triangular acceleration trajectory from an initial acceleration reference a0 to substantially zero acceleration, i.e.
[0032]
number
[0033] where: a peak is the peak acceleration, j inc is the value of the jerk constraint for increasing the magnitude of the acceleration, j decis the value of the jerk constraint for reducing the magnitude of the acceleration, Δv is delta V.
[0034] The magnitude of the calculated peak acceleration is determined from the delta V, so that the initial speed reference v0 is changed to the desired speed reference v f It is then possible to determine the trajectory segments required to transition to the desired final velocity v f is the speed of the trajectory from v0 to the desired final speed v f It can be any value to move to.
[0035] For example, in a point-to-point move, the velocity transition algorithm uses multiple desired final velocity references v f In the present invention, the term "moving" covers "fitting". The process of generating a finite jerk (S-curve) trajectory relies heavily on solving what we call a velocity transition algorithm, which is a method to achieve a desired peak velocity v before transitioning to zero velocity. peak The final velocity transition of a given move may be v f = 0. In the present invention, the term "corresponding" also encompasses "proportional to."
[0036] Preferably, the method comprises the steps of: i) whether the magnitude of the trajectory acceleration reference is substantially equal to the calculated peak acceleration; ii) whether the calculated peak acceleration is substantially below the acceleration constraint; and / or iii) Whether the calculated peak acceleration substantially exceeds the acceleration constraint generating one or more trajectory segments based on
[0037] Peak acceleration a peakOnce the magnitude of the desired final velocity reference v has been calculated, preferably one or more trajectory segments are generated by applying at least one jerk constraint to reach the magnitude of the desired final velocity reference v. f is effectively zero. However, one of the three possibilities defined above may be true. To address these three possibilities, the following steps are computed to generate one or more trajectory segments:
[0038] The magnitude of the orbital acceleration reference is substantially equal to the magnitude of the peak acceleration, i.e., |a0|=|a peak If |, then the trajectory is already at peak acceleration, so the magnitude of the trajectory velocity reference is equal to the desired final velocity v f To reach , it is only necessary to reduce the magnitude of the trajectory acceleration to zero. To accommodate for the magnitude of the trajectory acceleration reference being substantially equal to the calculated peak acceleration, preferably i) applying at least one jerk constraint such that an initial acceleration reference of the one or more trajectory segments has a magnitude substantially equal to the calculated peak acceleration, a terminal point acceleration of each of the one or more trajectory segments has a magnitude substantially equal to zero, and a terminal point velocity reference is substantially equal to a desired final velocity reference; generates one or more trajectory segments.
[0039] The magnitude of the peak acceleration is substantially less than or equal to the magnitude of the acceleration constraint, i.e., |a peak |≦a max If , the magnitude of the orbital acceleration is still |a peak | and then must be reduced to essentially zero, at which point the desired final velocity v f is satisfied. To address the calculated peak acceleration of one or more trajectory segments of the sequence of trajectory segments being less than or equal to the acceleration constraint, optionally the method further comprises: i) generating a first trajectory segment by applying at least one jerk constraint such that an end point acceleration reference of the first trajectory segment has a magnitude substantially equal to a peak acceleration; ii) generating a second trajectory segment by applying at least one jerk constraint such that an end point of the first trajectory segment is assigned to a start point of the second trajectory segment, a magnitude of an end point acceleration of the second trajectory segment is substantially equal to zero, and an end point velocity reference of the second trajectory segment is substantially equal to a desired final velocity reference; The method further comprises generating one or more trajectory segments by:
[0040] If the calculated peak acceleration is greater than the magnitude of the acceleration constraint, the magnitude of the orbital acceleration must be increased up to the acceleration constraint. The magnitude of the orbital acceleration must be maintained at the acceleration constraint for some time before finally increasing to the desired final velocity v f In this case, the magnitude of the orbit acceleration reference must be considered equal to the acceleration constraint, or the magnitude of the orbit acceleration reference must be reduced to zero to satisfy the acceleration constraint a max Depending on whether the magnitude of the trajectory acceleration reference is less than the acceleration constraint, two or three trajectory segments may be generated by applying at least one jerk constraint. If the magnitude of the trajectory acceleration reference is deemed to be less than the acceleration constraint, the trajectory is max (Acceleration constraint) must be increased to the maximum, and over a period of time max and then finally at the desired final velocity v f To address the calculated peak acceleration substantially exceeding the acceleration constraint, if the magnitude of the trajectory acceleration reference is substantially less than the acceleration constraint, then preferably one or more trajectory segments are adjusted to: i) generating a first trajectory segment by applying at least one jerk constraint such that an end point acceleration reference of the first trajectory segment has a magnitude substantially equal to an acceleration constraint; ii) generating a second trajectory segment by applying at least one jerk constraint having a substantially zero magnitude such that an end point of the first trajectory segment is assigned to a start point of the second trajectory segment and a magnitude of an end point acceleration of the second trajectory segment is substantially equal to the acceleration constraint; iii) generating a third trajectory segment by applying at least one jerk constraint such that an end point of the second trajectory segment is assigned to a start point of a third trajectory segment, a magnitude of an end point acceleration reference of the third trajectory segment is substantially equal to zero, and an end point velocity reference of the third trajectory segment is substantially equal to a desired final velocity reference; Generated by:
[0041] The magnitude of the orbital acceleration is the acceleration constraint a max To address the calculated peak acceleration exceeding the acceleration constraint when substantially equal to i) generating a first trajectory segment by applying at least one jerk constraint having a substantially zero magnitude such that an end point acceleration reference of the first trajectory segment has a magnitude substantially equal to a magnitude of the acceleration constraint; ii) generating a second trajectory segment by applying at least one jerk constraint such that an end point of the first trajectory segment is assigned to a start point of the second trajectory segment, a magnitude of an end point acceleration reference of the second trajectory segment is substantially equal to zero, and an end point velocity of the second trajectory segment is substantially equal to a desired final velocity reference; Generated by:
[0042] Because the velocity reference used to calculate the delta V Δv changes with each successive trajectory segment generated in the sequence, the value of the delta V Δv changes each time the velocity transition algorithm is applied to generate one or more trajectory segments in the sequence. For example, when solving a problem where the trajectory acceleration is less than or equal to the acceleration constraint, the desired final velocity reference v f The generated trajectory segment changes the speed reference for the next or subsequent problem until it reaches
[0043] According to the present invention, the application of the speed transition algorithm is a) The magnitude of the orbital acceleration is greater than the acceleration constraint, i.e. |a0|>|a max the magnitude of the orbital acceleration is excessive in the sense that | b) The trajectory has a desired final velocity v f is accelerating away from c) The trajectory has a desired final velocity v f Overshooting This includes overcoming the problem of
[0044] If any of conditions (a) through (c) are true, corrective measures must be implemented to resolve these problems before the peak acceleration can be calculated.
[0045] Before applying the velocity transition algorithm, the magnitude of the orbit acceleration is greater than the acceleration constraint, i.e., |a0|>|a max If the magnitude of the trajectory acceleration reference is excessive in the sense that |, then the magnitude of the trajectory acceleration reference needs to be reduced to the acceleration constraint. Preferably, generating the one or more trajectory segments in response to the magnitude of the trajectory acceleration reference exceeding the acceleration constraint comprises applying at least one jerk constraint such that an end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to the magnitude of the acceleration constraint.
[0046] The orbit acceleration reference is then adjusted to the desired final velocity v f If you find that the object is accelerating away from a peak Prior to calculating , the magnitude of the orbital acceleration needs to be reduced to substantially zero. Preferably, the method includes: i) determining whether the magnitude of delta-V is substantially equal to zero and the magnitude of the orbital acceleration reference is not substantially equal to zero; and / or ii) determining whether the product of the magnitude of delta-V and the magnitude of the orbital acceleration reference is substantially less than zero; determining whether the trajectory is accelerating away from a desired final velocity reference value by if either step (i) or (ii) is yes, generating one or more trajectory segments by applying at least one jerk constraint such that an end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; It further comprises:
[0047] The magnitude of the orbital velocity is the desired final velocity v f If the trajectory substantially overshoots the desired final velocity, then one or more trajectory segments must be generated to reduce the magnitude of the acceleration reference to substantially zero. Preferably, generating one or more trajectory segments in response to the trajectory overshooting the desired final velocity comprises: i) calculating a provisional trajectory segment by applying at least one jerk constraint such that a magnitude of a terminal point acceleration reference of the provisional trajectory segment is substantially zero, where the provisional trajectory segment comprises a start point comprising an initial position reference, an initial velocity reference, and an initial acceleration reference, and a terminal point comprising a terminal point position reference, a terminal point velocity reference, and a terminal point acceleration reference; ii) calculating an auxiliary delta-V corresponding to the difference between a desired final velocity reference and a tentative trajectory segment end point velocity reference; and iii) incorporating the tentative trajectory segment into the sequence of trajectory segments if the product of delta-V and auxiliary delta-V is substantially less than zero, and ignoring the tentative trajectory segment otherwise. Equipped with.
[0048] The tentative trajectory segment has a desired final velocity v f If the magnitude of delta-V does not overshoot, i.e., the product of delta-V and auxiliary delta-V is substantially greater than or equal to zero, then the step of generating provisional trajectory segments is ignored.
[0049] The method steps for solving problems (a) to (c) above result in new trajectory segments being generated, and as more trajectory segments are generated in the sequence of trajectory segments, the initial speed reference and the terminal speed reference change, so that each new trajectory segment generated results in a new value of delta V Δv, which then peak When applying the velocity transition algorithm according to the invention, the method further comprises the step of assigning the end point of each of one or more trajectory segments in the sequence of trajectory segments to the start point of one or more subsequent trajectory segments in the sequence. Thus, in a sequence of one or more trajectory segments, the end point of a trajectory segment is assigned to the start point of the subsequent trajectory segment in the sequence. This forms the basis of the sequence when dealing with the next sub-problem. This results in a continuously changing delta V Δv, since the start point of the last generated trajectory segment in the sequence changes. For example, the end point of a first trajectory segment is assigned to the start point of a second trajectory segment, i.e. (p0, v0, a0←p E , v E , a EThe delta V Δv calculated when solving problems (i) through (iii) is then applied to the calculation of the peak acceleration.
[0050] Preferably, the acceleration reference further comprises a deceleration reference, and the acceleration constraint further comprises a deceleration constraint having an upper deceleration constraint and a lower deceleration constraint, and one or more trajectory segments in the sequence of trajectory segments further define at least one deceleration reference as a function of time within the upper and lower constraints of the deceleration constraint or equal to either the upper or lower constraint of the deceleration constraint.
[0051] The plurality of constraints further introduces a deceleration constraint having an upper constraint and a lower constraint such that the trajectory further defines a deceleration reference within or equal to either the upper or lower constraint. The acceleration reference further comprises a deceleration reference resulting from decreasing the speed reference as well as increasing the speed reference, which is by definition an acceleration. The rate at which the speed reference is decreased is determined by application of the upper and lower deceleration constraints.
[0052] Preferably, the at least one jerk constraint is: i) a first jerk constraint for increasing the magnitude of the acceleration reference; ii) a second jerk constraint to reduce the magnitude of the acceleration reference; and iii) a third jerk constraint to increase the magnitude of the deceleration reference; and iv) a fourth jerk constraint to reduce the magnitude of the deceleration reference; Equipped with.
[0053] In another aspect of the invention, a forced deceleration algorithm is applied to the trajectory to bring it to a stationary state, i.e., so that the trajectory velocity is substantially zero. The forced deceleration algorithm may be applied in situations where the commanded position has changed, i.e., is approached. However, the forced deceleration algorithm cannot be applied if the velocity reference magnitude is increasing and / or the acceleration reference magnitude is excessive, greater than the deceleration constraint.
[0054] Preferably, the method comprises the steps of: i) generating one or more trajectory segments by applying at least one jerk constraint in response to an increase in the magnitude of the trajectory velocity such that an end point deceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; and / or ii) generating one or more trajectory segments by applying at least one jerk constraint in response to the magnitude of the deceleration reference being substantially greater than the upper deceleration constraint, such that an end point deceleration reference of the one or more trajectory segments has a magnitude substantially equal to the upper deceleration constraint. It further comprises:
[0055] Once the speed of the trajectory stops increasing, or once the trajectory also has and does not have excessive deceleration that exceeds the deceleration constraint, a velocity transition algorithm can be applied to transition the magnitude of the trajectory velocity from the start point to the terminal point such that the terminal point trajectory velocity has a magnitude substantially equal to zero at the commanded position. i) applying a velocity transition algorithm to generate one or more trajectory segments based on a deceleration constraint having a magnitude substantially equal to an upper deceleration constraint, such that a magnitude of an end point acceleration reference of the one or more trajectory segments is substantially equal to zero and an end point velocity reference of the one or more trajectory segments is substantially equal to a desired final velocity reference; Ideally, the terminal point velocity reference is equal to the desired final velocity at the commanded position.
[0056] However, if the use of an upper deceleration constraint in the velocity transition algorithm causes one or more generated trajectory segments to overshoot the commanded position, i.e., the magnitude of the terminal point position overshoots the commanded position, then in addition to applying the velocity transition algorithm to generate one or more trajectory segments whose position references overshoot the commanded position, an additional or second velocity transition algorithm is applied to reverse the trajectory from the overshoot position such that the magnitude of the terminal point acceleration reference of the one or more trajectory segments is substantially equal to zero and the terminal point velocity reference of the one or more trajectory segments generated as a result of application of the second velocity transition algorithm is substantially equal to the desired final velocity reference at the commanded position.
[0057] Preferably, the method comprises the steps of: i) determining whether an end point position reference of one or more trajectory segments overshoots a commanded position when a corresponding end point deceleration reference is substantially equal to zero and a corresponding end point velocity reference is substantially equal to a desired final velocity reference; ii) in response to the trajectory overshooting the commanded position, applying "reverse" to generate one or more trajectory segments to return the trajectory to the commanded position by applying a velocity transition algorithm to generate one or more trajectory segments such that at the commanded position, a magnitude of an end point acceleration reference is substantially equal to zero and an end point velocity reference of the one or more trajectory segments is substantially equal to a desired final velocity reference; The method further comprises applying a forced deceleration algorithm to generate one or more trajectory segments by
[0058] In the present invention, "reverse" is applied when the magnitude of the position reference overshoots the commanded position in the sense that the position magnitude is greater than the commanded position. As a result, a velocity transition is applied in the reverse direction such that at the commanded position, the magnitude of the acceleration reference is substantially equal to zero and the magnitude of the velocity reference is substantially equal to the desired final velocity reference.
[0059] However, if applying the velocity transition algorithm with an upper deceleration constraint results in too strong a deceleration and the generated trajectory would undershoot the commanded position in the sense that the calculated position reference is less than the commanded position, then a weaker deceleration can be used. i) determining whether an end point position reference of one or more trajectory segments of the trajectory undershoots a commanded position when a magnitude of a corresponding end point deceleration reference of the one or more trajectory segments is substantially zero and a corresponding end point velocity reference is substantially equal to a desired final velocity reference; ii) in response to the trajectory undershooting the commanded position, applying a velocity transition algorithm to generate one or more trajectory segments based on a deceleration constraint having a magnitude substantially equal to the lower deceleration constraint; The method further comprises generating one or more trajectory segments by:
[0060] If the trajectory still undershoots the commanded position when applying the velocity transition algorithm with the lower deceleration constraint, this is considered to be the minimum deceleration that can be achieved to bring the trajectory to rest. However, if using the upper deceleration constraint when applying the velocity transition algorithm is too strong and causes the trajectory to undershoot the commanded position, but using the lower deceleration constraint causes the trajectory to overshoot the commanded position, a deceleration constraint between the upper and lower deceleration constraints can be used in the velocity transition algorithm to generate one or more trajectory segments such that the magnitude of the terminal point velocity reference is substantially zero at the commanded position, i.e., the desired final velocity reference is substantially zero.
[0061] Preferably, the method comprises the steps of: i) predicting parameters corresponding to an optimal constrained deceleration constraint having a value between an upper constrained deceleration constraint and a lower constrained deceleration constraint using a root-finding algorithm; ii) applying a velocity transition algorithm to generate one or more trajectory segments with a deceleration constraint substantially equal to the optimal enforced deceleration constraint such that when a magnitude of position deviation of an end point reference of the one or more trajectory segments from a commanded position is less than a predetermined threshold, a magnitude of an end point acceleration reference is substantially equal to zero and an end point velocity reference of the one or more trajectory segments is substantially equal to a desired final velocity reference; The method further comprises generating one or more trajectory segments by:
[0062] By using a root finding algorithm, an optimal forced deceleration constraint can be calculated such that when a velocity transition algorithm is applied using the optimal forced deceleration constraint, the magnitudes of the terminal point acceleration reference and terminal point velocity reference of one or more trajectory segments are substantially zero when the magnitude of the position deviation of the terminal point position reference of one or more trajectory segments from a command position is less than a predetermined threshold.
[0063] The predetermined threshold is substantially zero such that the magnitudes of the terminal point acceleration reference and the terminal point velocity reference of one or more trajectory segments are substantially zero at the commanded position. The desired final velocity reference when applying the velocity transition algorithm is substantially equal to zero.
[0064] Preferably, the velocity transition algorithm is nested within the root finder algorithm such that the velocity transition algorithm generates one or more trajectory segments for each successive parameter predicted by the root finder algorithm.
[0065] Preferably, the trajectory is an S-shaped or finite jerk trajectory profile. Preferably, the trajectory is S-shaped such that one or more trajectory segments are generated using a mathematical formula for an S-curve.
[0066] More preferably, generating one or more trajectory segments by applying at least one jerk constraint generates a trajectory that defines an acceleration reference that varies continuously as a function of time for one or more trajectory segments in the sequence of trajectory segments.
[0067] The present invention provides a trajectory generator for generating a trajectory for controlling movement of a motion device, comprising: i) a memory; and ii) a processor configured to execute computer executable instructions stored on a memory, the processor configured to generate a motion control profile according to a method according to the present invention upon receiving a command position; The present invention further provides a trajectory generator comprising:
[0068] Preferably, the powered device is a load handling apparatus comprising a wheel assembly having a first pair of wheels and a second pair of wheels, a drive mechanism for driving the wheel assembly, and a controller connected to the drive mechanism that is instructed to drive the wheel assembly according to a motion control profile generated by the method of the present invention.
[0069] The present invention provides a non-transitory computer medium having stored thereon computer-executable instructions that, in response to execution, cause a computer system to perform operations in accordance with the present invention.
[0070] Additional features and aspects of the present invention will become apparent from the following detailed description of illustrative embodiments that proceeds with reference to the drawings. [Brief description of the drawings]
[0071] [Figure 1] 1 is a schematic diagram of a grid framework structure according to known systems; [Diagram 2] FIG. 2 is a schematic diagram showing a top view of a stack of bins arranged within the framework structure of FIG. 1. [Diagram 3] 1 is a schematic diagram of a known storage system showing load handling equipment operable on a grid framework structure; [Figure 4] FIG. 4 is a schematic perspective view of the loading / unloading device showing a lifting device that grips a container from above. [Diagram 5] 5 is a schematic perspective cutaway view of the loading device of FIG. 4 showing (a) a container accommodated within the container-receiving space of the loading device, and (b) the container-receiving space of the loading device. [Figure 6] 2 is a schematic perspective view of a load handling device over a portion of a grid structure in accordance with an embodiment of the present invention; FIG. [Figure 7] FIG. 2 is a schematic perspective view of a wheel of a loading device showing a central axis of rotation according to an embodiment of the present invention. [Figure 8] FIG. 2 is an exploded view of a hub motor used to drive a wheel about a central axis of rotation in accordance with an embodiment of the present invention. [Figure 9] Wheel assembly control and interface architecture according to an embodiment of the present invention. [Figure 10] FIG. 2 is a schematic diagram of communication between a load handling device and a master controller over a network in accordance with an embodiment of the present invention. [Figure 11] (a)-(c) Schematic of trapezoidal trajectory profile. [Figure 12] (a)-(d) Schematic of the S-curve trajectory profile. [Figure 13] 1 is a block diagram of a motion control system including a trajectory or motion profile generator in accordance with an embodiment of the present invention. [Figure 14] FIG. 1 is a perspective view of a robotic load handling device equipped with a position sensor according to an embodiment of the present invention. [Figure 15] 4 is a flow diagram of an example method for generating a trajectory profile according to an embodiment of the present invention. [Figure 16] 4 is a block diagram illustrating inputs and outputs of an exemplary motion profile generator in accordance with an embodiment of the present invention. [Figure 17] FIG. 2 is a block diagram illustrating inputs and outputs of an exemplary position controller in accordance with an embodiment of the present invention. [Figure 18] 4 is a flow diagram of an example method for generating a trajectory profile according to an embodiment of the present invention. [Figure 19] Forced deceleration step in Figure 18. [Figure 20] A corrective step of reducing the magnitude of the acceleration in FIG. 18 to the commanded acceleration. [Figure 21] A corrective step to reduce the magnitude of the velocity in FIG. 18 to substantially zero. [Figure 22] Corrective step applying reversal from overshoot in FIG. [Figure 23] A corrective step of reducing the magnitude of the acceleration in FIG. 18 to substantially zero and applying a reversal from the overshoot. [Figure 24] Corrective step for trajectory acceleration magnitudes greater than the commanded velocity. [Diagram 25] A corrective step of magnitude of the current trajectory velocity substantially equal to the commanded velocity. [Figure 26] A corrective step that increases the current trajectory speed up to the commanded speed. [Figure 27-1] 4 is an exemplary method flow diagram of a first speed transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 27-2] 4 is an exemplary method flow diagram of a first speed transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 27-3] 4 is an exemplary method flow diagram of a first speed transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 28-1] 5 is an exemplary method flow diagram of a second velocity transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 28-2] 5 is an exemplary method flow diagram of a second velocity transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 28-3] 5 is an exemplary method flow diagram of a second velocity transition sub-algorithm in accordance with an embodiment of the present invention. [Figure 29] 6 is a schematic imaginary triangle of orbital acceleration magnitude illustrating successive velocity transitions according to an embodiment of the present invention; [Diagram 30] 6 is a schematic imaginary trapezoid of orbital acceleration magnitude illustrating multiple velocity transitions in accordance with an embodiment of the present invention; [Figure 31(a)] 4A-4C are trajectory profiles when applying a velocity transition algorithm for different desired velocities vf according to an embodiment of the present invention. [Figure 31(b)] 4A-4C are trajectory profiles when applying a velocity transition algorithm for different desired velocities vf according to an embodiment of the present invention. [Figure 31(c)] 4A-4C are trajectory profiles when applying a velocity transition algorithm for different desired velocities vf according to an embodiment of the present invention. [Figure 31(d)] 4A-4C are trajectory profiles when applying a velocity transition algorithm for different desired velocities vf according to an embodiment of the present invention. [Figure 32(a)] 4A-4D are trajectory profiles when applying a root-finding algorithm in conjunction with a velocity transition algorithm for different peak velocities vpeak according to an embodiment of the present invention. [Figure 32(b)]4A-4D are trajectory profiles when applying a root-finding algorithm in conjunction with a velocity transition algorithm for different peak velocities vpeak according to an embodiment of the present invention. [Figure 32(c)] 4A-4D are trajectory profiles when applying a root-finding algorithm in conjunction with a velocity transition algorithm for different peak velocities vpeak according to an embodiment of the present invention. [Diagram 33] 33 is a plot of the objective function of the root-finding algorithm for different peak velocities of FIG. 32 in accordance with an embodiment of the present invention. [Figure 34-1] 4 is an exemplary method flow diagram of a forced deceleration algorithm in accordance with an embodiment of the present invention. [Figure 34-2] 4 is an exemplary method flow diagram of a forced deceleration algorithm in accordance with an embodiment of the present invention. [Figure 35(a)] Schematic of an example trajectory profile. [Figure 35(b)] Schematic of an example trajectory profile. [Figure 35(c)] Schematic of an example trajectory profile. [Diagram 36] 13 is a plot showing how the average current of a hoist motor varies with the mass of the contents of a storage container being hoisted by the motor. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0072] The present invention is conceived against the backdrop of known features of storage systems, such as grid framework structures and load handling equipment, as described above with reference to Figures 1 to 5. The present invention is defined by a method and system for controlling the movement of load handling equipment or "bots" on a grid structure.
[0073] The material handling apparatus 130 comprises a vehicle body 132 that includes a lifting mechanism (not shown) that includes a winch or crane mechanism for lifting a storage container or bin, also called a tote, from below. The crane mechanism includes a winch cable wound on a spool or reel and a gripping device that is configured to grip the top of a container to lift the container from a stack of containers in a storage system of the type shown in Figures 4 and 5.
[0074] The vehicle body 132 comprises an upper portion and a lower portion. The lower portion comprises a wheel assembly comprising two sets of wheels 134, 136 that run on rails on the top of a grid framework structure of the storage system. For purposes of this description, the two sets of wheels are defined as a first wheel set 134 and a second wheel set 136. The first wheel set 134 and the second wheel set 136 are disposed around the periphery of the loading device 130. Each of the first wheel set 134 and the second wheel set 136 are disposed on opposite sides of the lower portion of the vehicle body 132 and comprise a pair of wheels on opposite sides of the vehicle body. That is, each of the first wheel set and the second wheel set comprise a total of four wheels. In the particular embodiment of the invention shown in FIG. 6, the first wheel set 134 and the second wheel set are each rotatably mounted to the vehicle frame on the lower portion of the vehicle body 132 to allow the load handling equipment to move in the X and Y directions, respectively, along the track or rail. Although this particular embodiment of the invention shows a pair of wheels mounted on opposite sides of the vehicle body, the invention is not limited to load handling equipment with a pair of wheels mounted on each side of the vehicle body. Instead of a pair of wheels mounted on opposite sides of the vehicle, stability of the load handling equipment can also be achieved by having at least one wheel located on opposite sides of the vehicle body diagonally opposite each other. Thus, the first wheel set and the second wheel set each include at least one wheel mounted on opposite sides of the vehicle body diagonally opposite each other.
[0075] In this particular embodiment of the invention, the first set of wheels 134 and the second set of wheels 136 are arranged around the periphery of a cavity or recess called the container-receiving recess in the lower portion (see Figures 4 and 5). This recess is sized to accommodate the storage container or container 10 when it is lifted by the crane mechanism, as shown in Figures 5(a and b). This particular embodiment describes a container-receiving space arranged in the vehicle body, for example as described in WO2015 / 019055 (Ocado Innovation Limited), but the vehicle body may also include a cantilever as taught in WO2019 / 238702 (Autostore Technology AS), in which case the container-receiving space is located below the cantilever of the loading device. In this case, the gripping device is hoisted by the cantilever so that it can engage the container and lift it out of the stack into the container-receiving space below the cantilever.
[0076] The upper portion of the vehicle body 132 may house most of the bulky components of the loading device. Typically, the upper portion of the vehicle houses a drive mechanism for driving the lifting mechanism, along with an on-board rechargeable power source that powers the drive mechanism and the lifting mechanism. The rechargeable power source may be any suitable battery, such as, but not limited to, a lithium battery or a capacitor.
[0077] As shown in FIG. 6, the grid structure 115 comprises a first set of parallel grid members extending in a first direction and a second set of grid members extending in a second direction arranged in a grid pattern comprising a plurality of cells. For ease of explanation of the invention, movement of the load handling equipment in a first direction in a horizontal plane represents movement in an X direction, and movement in a second direction represents movement in a Y direction in the grid structure. To allow one or more load handling equipment to travel on the grid structure, the first set of grid members comprises a first set of tracks 122a and the second set of grid members comprises a second set of tracks 122b. Optionally, the first set of grid members comprises a first set of track supports (not shown) and the second set of grid members comprises a second set of track supports (not shown). Optionally, the first set of tracks 122a are snap-fitted to the first set of track supports and the second set of tracks 122b are snap-fitted to the second set of track supports. It is equally true in the present invention that the plurality of tracks 122a, 122b can be integrated with the first and second sets of track supports such that a grid member of the grid structure comprises both the tracks and the track supports.
[0078] To allow the vehicle or loading equipment to move in orthogonal directions by changing direction on the grid structure, each of the first wheel set 134 and the second wheel set 136 is configured to be moved vertically to lift off of their respective tracks or rails. For example, to change direction on the grid structure 115 (e.g., changing from movement in the X direction to movement in the Y direction), the first wheel set 134 is lifted off the first set of grid members or tracks 122a and the second wheel set 136 engages with the second set of grid members or tracks 122b. A drive mechanism (not shown), such as a motor, drives either the first wheel set 134 or the second wheel set 136 in the X or Y direction on the grid structure 115. In this particular embodiment of the invention, each wheel of the first wheel set 134 and the second wheel set 136 on the lower portion of the vehicle body 132 is individually driven by a hub motor to provide four-wheel drive capability for the loading equipment 130 on the grid structure 115. In other words, all of the wheels of the first and second wheel sets are driven by individual hub motors so that if any of the wheels of sets 134, 136 slip on the rails or tracks, the load handling equipment can still travel along the rails or tracks 122a, 122b on the grid structure 115. Figures 7 and 8 are perspective views of a wheel 150 of load handling equipment 130 according to an embodiment of the invention.
[0079] In particular, the hub motor 160 shown in Figures 7 and 8 comprises an outer rotor 162 with an outer surface configured to engage a grid structure (e.g., a track) and an inner surface with an annular permanent magnet 164, configured to rotate around a wheel hub or inner hub 166 that comprises a stator of the hub motor 160. Typically, the stator comprises the coil of the hub motor. To drive each wheel 150 of the first or second wheel set, thereby moving the loading equipment in a first or second direction on the grid structure, the outer rotor 162 of the hub motor 160 is configured to rotate about an axis of rotation AA corresponding to the central axis of the respective wheel. The outer surface of the rotor 162 may optionally comprise a tire 168 for engaging a track or rail. In this particular embodiment shown in Figure 8, the hub motor 160 comprises an outer rotor 162 that rotates about its axis of rotation on bearings (not shown) and has a permanent magnet 164 bonded to its inner surface. Each of the wheels 150 is coupled to the vehicle body of the load handling apparatus by coupling an inner hub or hubs 166 that include a stator of a hub motor to allow the outer rotor 162 to rotate relative to the wheel hub 166. While this particular embodiment describes the drive mechanism for each wheel 150 of the first wheel set 134 and the second wheel set 136 as including a hub motor, other means for rotatably driving the wheels are applicable to the present invention. For example, the pairs of wheels at the front and rear and on each side of the load handling apparatus could be driven by one or more motors connected to appropriate pulley or gear mechanisms.
[0080] In the block diagram of the wheel assembly shown in FIG. 9, each pair of wheels of the first wheel set 134 and the second wheel set 136 are commanded to rotate about their respective axes of rotation by a control module or controller 170. The control module 170 may comprise a computer system including one or more processors and a memory storing instructions that, when executed by the one or more processors, cause the one or more processors to command the first wheel set or the second wheel set to rotate synchronously about their respective axes of rotation. The memory may be any storage device generally known in the art and may include, but is not limited to, RAM, computer readable media, magnetic storage media, optical storage media, or other electronic storage media that may be used to store data and that may be accessed by a processor. The one or more processing devices may be any processing device known in the art. Representative examples include, but are not limited to, microprocessors. The drive mechanism and wheel positioning for each wheel of the first wheel set 134 and the second wheel set 136 are communicatively coupled to the control module via any suitable communication interface unit 172, including but not limited to any wired or wireless communications known in the art.
[0081] The controller 170 sends control commands to the drive mechanism which converts the control commands into appropriate drive current output signals to the wheel motors in accordance with the control commands to facilitate controlled movement of the load handling equipment on the track. The control signals can be provided directly to the wheel motors or to the motor drives which control the speed and direction of the wheel motors by varying the power delivered to the wheel motors. When the controller 170 determines that the drive mechanism must move the load handling equipment from a current position to a new position or change its speed, the controller 170 generates a trajectory, also called a motion control profile, which is converted into appropriate control signals to drive the load handling equipment from the current position or speed via the drive mechanism to a target position or commanded stop position. The motion control profile defines the kinematic states of the load handling equipment, such as speed, acceleration, and / or position over time as the load handling equipment moves from the current position to the commanded stop position. The current position may be from a stopped or resting state on the grid structure, in which case the initial speed and initial acceleration of the load handling equipment are both substantially zero, or may be while the load handling equipment is moving, in which case the load handling equipment has an initial speed greater than zero. The command location may be a desired storage location or grid cell on the grid structure, for example, when a storage container is to be retrieved or placed, or may be a location or grid cell on the grid structure near one or more charging points, for example, when a loading device is commanded to charge a rechargeable power source, typically located around the periphery of the grid structure.
[0082] Once this motion control profile is generated, the controller converts the motion control profile into appropriate control signals for moving the load handling equipment through a trajectory defined by the motion control profile. Various segments (or stages) of the motion control profile are calculated based on one or more predefined constraints corresponding to mechanical limitations of the drive mechanism. The one or more predefined constraints include, but are not limited to, a maximum velocity, a maximum acceleration, and a maximum deceleration. For example, a load handling equipment according to an embodiment of the present invention may travel at a maximum speed of 4 m / s and a maximum deceleration of 2 m / s. 2 The load is expected to accelerate at a constant speed. Given these constraints and a desired commanded position, the controller will calculate a motion control profile to execute the desired motion. The desired motion may include multiple positions in the X and Y directions on the grid structure that may be intermixed or executed sequentially in order for the load handling equipment to reach the desired storage row. Each of the multiple positions may comprise a single motion in either the X or Y direction on the grid structure. In this invention, the terms "commanded velocity" and "velocity constraint" are used interchangeably to mean the same function. Similarly, the terms "commanded acceleration" and "acceleration constraint" are used interchangeably to mean the same function.
[0083] The present invention relates to generating time-optimal motion control profiles based on constraints for point-to-point movements of load handling equipment on a grid structure. Point-to-point movements in the context of the present invention refer to movements from a start position to a commanded stop position. The final acceleration and final velocity become zero at the moment the load handling equipment reaches the programmed destination or commanded stop position. In certain embodiments of the present invention, inputs to the controllers are received via a master controller 174. The controllers may be coupled to the master controller 174 via a network 176 as shown in FIG. 10. The controllers are shown in FIG. 10 as being integrated into the load handling equipment 130. The network 176 may be any of a variety of types, such as a LAN (local area network), a WAN (wide area network), the Internet, or an intranet, among others. One or more base stations (not shown) may transmit input information to the controllers. In certain embodiments of the present invention, the inputs may include one or more constraints and a commanded or target position on the grid structure. The one or more constraints may also be received independently of the commanded position. For example, the master controller 174 may communicate the command position and one or more constraints to a motion control profile or trajectory generator stored separately within the controller of the load handling device 130 .
[0084] The motion control profile is generated or calculated (e.g., automatically by the controller) based on one or more constraints and receipt of a commanded position on the grid structure, which may be a desired storage or charging position on the grid structure. In accordance with the present invention, the motion control profile or trajectory includes a set of position, speed, and acceleration and / or jerk signals. The trajectory terminates (or reaches an end point) when the commanded position is reached and the remaining signals (speed, acceleration, jerk) are exactly zero.
[0085] The resulting motion control profile is a function of the type of profile the controller is configured to generate. These are the S-curve profile and its simplification, the trapezoidal profile.
[0086] 1.0 Trapezoidal and S-Curve Track Profiles In a point-to-point situation, a trapezoidal profile usually consists of three distinct motion phases with a triangular motion profile. The simplest form of a trapezoidal profile is shown in Figure 11 (a to c) and consists of a period of constant acceleration to reach maximum velocity followed by a period of constant deceleration back to zero velocity. The period of positive acceleration is called the "acceleration phase" and the period of negative acceleration is called the "deceleration phase". The acceleration and deceleration increase and decrease almost instantaneously as shown in the acceleration segment in Figure 11c. At these positions, the rate of change of acceleration (da / dt) is considered to be indefinite and equal to infinity. The rate of change of acceleration with respect to time, j=da / dt, is termed jerk and in high throughput applications, the indefinite jerk causes shock loads, vibration of the loading equipment on the grid structure and possibly audible noise. The motion control profile generated as a function of time includes a set of (i) acceleration reference (FIG. 11c), (ii) velocity reference (FIG. 11b), and position reference (FIG. 11a). As a result, the trapezoidal trajectory shown in FIG. 11(a-c) consists of signals related to position, velocity, and acceleration. The main inputs in the generation of a motion control profile based on a trapezoidal profile are (i) commanded position (position to be reached at the end of the trajectory), (ii) commanded velocity (maximum allowed velocity), and (iii) commanded acceleration (maximum allowed acceleration). The commanded velocity and commanded acceleration represent one or more constraints in the generation of a motion control profile based on a trapezoidal profile. Thus, the velocity reference in a trapezoidal profile is controlled by manipulating the acceleration reference.
[0087] Assuming initial conditions at time t=0, velocity v(t)=v0, and position p(t)=p0, and keeping the manipulated variable acceleration reference a constant (for times t≧t(0)), the velocity reference and position reference as functions of time for a trapezoidal trajectory profile are given by the following equations:
[0088]
number
[0089] At any time τ when the manipulated variable value (acceleration) changes, equations (1) and (2) can be reapplied by simply taking the values of the trajectory points at time τ as initial conditions, e.g., v0 ← v(τ), p0 ← p(τ), and resetting time to (t-τ). A trajectory point refers to the value of an acceleration reference, velocity reference, or position reference point at a particular time, which may be the start point or the end point of a trajectory segment, as explained below.
[0090] By "manipulated variable" we mean a variable whose value is set by the control system. In the trapezoidal profile, the manipulated variable is acceleration, so the value of acceleration is set by the control system. The other variables that are not manipulated variables (position and velocity) are calculated from the values of the manipulated variables by the above formulas.
[0091] The S-curve acceleration profile shown in Figure 12 (a to d) is an alternative to the traditional trapezoidal profile, which typically consists of seven distinct phases of motion. This acceleration profile can be broadly divided into three parts: acceleration phase (phases I, II, and III), constant velocity phase (phase IV), and deceleration phase (phases V, VI, and VII). The acceleration, constant velocity, and deceleration phases of the S-curve profile are equivalent to those of the trapezoidal profile described above with reference to Figures 11 (a to c), but the application of jerk makes the profile more complex, so a subdivision of the acceleration and deceleration phases is required to fully describe the trajectory. In phase I, the loading equipment begins to move from a rest or starting position with an acceleration phase that has a positive jerk value until the loading equipment reaches a maximum acceleration. In phase II, the profile accelerates at its maximum acceleration rate until it approaches a maximum speed and must begin to decline. In the trajectory profile shown in Figure 12c, the trajectory transitions to zero jerk during phase II. In phase III, the acceleration is decreased until it reaches zero, i.e. the jerk goes to negative jerk values until it reaches zero acceleration at the desired velocity. In phase IV, i.e. the constant velocity phase, the desired velocity is constant and both the acceleration and jerk are zero. Phases V, VI, and VII are deceleration phases where the profile decelerates symmetrically to phases I, II, and III. In the deceleration phase, the S-curve profile starts by applying a negative jerk value in phase V, applies zero jerk in phase VI, and finally increases the jerk to positive values to decelerate the trajectory until it reaches zero velocity in phase VII.
[0092] In contrast to generating a motion control profile based on a trapezoidal profile, the main inputs in generating a motion control profile based on an S-curve profile are (i) the command position (the position to be reached at the end of the trajectory), (ii) the command velocity (the maximum allowed velocity v max ), (iii) command acceleration (maximum allowable acceleration a max), and (iv) commanded jerk. Thus, the one or more constraints in generating the motion control profile include commanded velocity, commanded acceleration, and commanded jerk. The commanded acceleration can include a maximum positive acceleration in the acceleration phase and a maximum negative acceleration in the deceleration phase. Alternatively, different terms can be used if the constraints related to acceleration can be classified into commanded acceleration and commanded deceleration to distinguish between acceleration in the acceleration phase and acceleration in the deceleration phase. In the present description, the terms commanded velocity and velocity constraint are used interchangeably to mean the same function, and the terms commanded acceleration and acceleration constraint likewise mean the same function. The motion control profile generated as a function of time includes a set of (i) jerk reference, (ii) acceleration reference, (iii) velocity reference, and (iv) position reference. As a result, the S-curve trajectory shown in FIG. 12 consists of signals related to position, velocity, acceleration, and jerk.
[0093] Unlike a trapezoidal profile, where the transition between constant speed and acceleration is abrupt, the edges or "corners" of the speed profile provided by one or more commanded jerks in an S-curve motion profile are smoothed, reducing the abrupt changes in acceleration, thereby smoothing the movement of the load handling equipment on the grid structure. That is, an S-curve profile imparts dramatically less vibration energy to the attachment mechanism and the load handling equipment. This is especially true when the drive mechanism for driving the wheel assemblies is a servo or stepper motor. Compared to a trapezoidal profile, an S-curve profile provides a control mechanism for canceling vibrations in the load handling equipment by adjusting the ratio of the transition phase of the profile to the constant acceleration phase. With respect to jerk, the higher the jerk, the greater the amount of unwanted vibration energy that is generated, and the wider the frequency band of the vibration energy. This means that the more abrupt the change in acceleration, the stronger the vibration. By reducing the jerk, the abrupt changes in acceleration in the region where the acceleration transitions to constant acceleration at the edges or "corners" of the speed profile are reduced. This is manifested by the smoothing of the edges or "corners" of the speed profile.
[0094] Compared to the trapezoidal profile where acceleration is the manipulated variable, the position, velocity, and acceleration in the S-curve profile are all controlled by manipulating one or more jerk values. If the time at which the manipulated variable changes value is called a transition time (or transition), there are at least four transitions at which the manipulated variable changes value. In simple terms, the manipulated variable remains constant between each pair of successive transitions. In the case of the trapezoidal trajectory, the manipulated variable is acceleration, and in the case of the S-curve trajectory, the manipulated variable is jerk. The value of the trajectory at a transition is called a transition point. In terms of both the trapezoidal and S-curve profiles, a trajectory segment is a section of the profile where the manipulated variable (acceleration in the trapezoidal profile and jerk in the S-curve profile) remains constant. In other words, a trajectory segment starts and ends at a transition point. In the above description of the trapezoidal and S-curve trajectories with reference to Figures 11 and 12, the three phases of the trapezoidal trajectory and the seven phases of the S-curve trajectory may also be called trajectory segments. For purposes of this description, a trajectory segment is defined as a trajectory point (p0, v0, a0) and an end point (p E , v E , a E ), where p0, v0, and a0 are the initial position reference a0, the initial velocity reference v0, and the initial acceleration reference a0, respectively, and p E , v E , and a E are the terminal point position references p E , terminal point velocity reference v E , and the end point acceleration reference a E A motion control profile or trajectory is a collection or sequence of such trajectory segments that result from changes in the manipulated variables.
[0095] Assuming initial conditions at t=0, with a starting acceleration a(t)=a0, starting velocity v(t)=v0, and starting position p(t)=p0, and holding the manipulated variable jerk j constant for time t≧0, the relationship between the jerk reference, acceleration reference, velocity reference, and position reference as a function of time for an S-curve trajectory profile is given by the following equations:
[0096]
number
[0097] As with the trapezoidal profile, at any time τ when the manipulated variables change value, equations (3) through (5) can be reapplied by simply setting the initial conditions to the values of the trajectory points at time τ, e.g., a0 ← a(τ), v0 ← v(τ), p0 ← p(τ), and resetting time to (t-τ).
[0098] The trajectory profiles in Figures 11 and 12 show that in the case of a trapezoidal trajectory, discontinuities occur in the second derivative of the position, i.e., acceleration, and in the case of an S-curve trajectory, discontinuities occur in the third derivative of the position, i.e., jerk. Trapezoidal trajectories are often used instead of S-curve trajectories, since they provide a sufficiently good approximation of the system motion and are less mathematically (and therefore software implementation) complex. However, motion control profiles generated according to trapezoidal profiles are not ideal for controlling the kinematics of a loading device on a grid structure, due to the stability of the loading device. To realize the generation of the trajectory, in a trapezoidal trajectory, the manipulated variable, acceleration, is allowed to vary discontinuously. Unlike a trapezoidal profile, in an S-curve trajectory, the manipulated variable, jerk, is allowed to vary discontinuously. The time point at which the manipulated variable value changes, also called the transition time, affects the smoothness of the loading device on a grid structure. Since the change in acceleration affects the smoothness of the ride of the loading equipment on the grid structure, the transition time of the manipulated variable in the trapezoidal profile is too abrupt, which leads to vibration of the loading equipment on the grid structure, i.e., the acceleration changes discontinuously. By allowing the manipulated variable jerk to change discontinuously in the S-curve profile, the acceleration can be changed more continuously, allowing for a smoother ride.
[0099] In this particular embodiment of the invention, the generation of the motion control profile is described with reference to an S-curve profile.
[0100] 2.0 Trajectory Generator Components As discussed above, the generation or calculation of a motion control profile used to control the drive mechanism of a wheel assembly of a load handling device on a grid structure is performed in large part by a controller within the load handling device in response to receiving from a master controller one or more constraints and a desired specification of a trajectory (command position) on the grid structure. FIG. 13 is a block diagram of an exemplary, non-limiting motion control profile generation system 180 capable of generating a motion profile for a point-to-point movement of a load handling device on a grid structure. The motion control system includes a motion profile generator or trajectory generator 182 for generating motion control signals and a position controller 184 for converting the motion control signals into appropriate torque signals for driving a drive mechanism 186 according to one or more embodiments of the present invention. The terms "motion profile generator" and "trajectory generator" are used interchangeably herein.
[0101] The motion profile generator 182 may be, for example, a programmable logic controller (PLC) or other such controller that monitors and controls the drive mechanism of the load handling equipment. In this example, the motion profile generator may be a functional component of the controller's operating system and / or control software executed by one or more processors within the controller. The motion profile generator 182 may also be a hardware component within the controller, such as a circuit board or integrated circuit that exchanges data with other functional elements of the controller. Other suitable implementations of motion control profiles are within the scope of the present invention. Although the motion profile generator 182 is shown in FIG. 13 as an integrated component of the controller or control system, the motion profile generator 182 may also be a separate component from the controller. In such a configuration, the motion profile generator may exchange data with the controller or other components of the motion control generation system via any suitable communication means, such as, but not limited to, wired or wireless networking, hardwired data links, or other suitable communication links. In this particular embodiment of the present invention, the controller, and therefore the motion profile generator, is within the load handling equipment.
[0102] The generated motion profile defines a trajectory or motion control profile for controlling the movement of the load equipment from a current state (first position) or velocity to a commanded position (second position), where the motion control profile is defined in terms of one or more of a position reference, a velocity reference, an acceleration reference, and a jerk reference (in the case of an S-curve profile) as a function of time. When at the commanded position, the load equipment may be at a predetermined threshold distance from the commanded position. Preferably, the predetermined threshold is substantially less than 5 mm, more preferably less than 1 mm, and even more preferably less than 0.5 mm, such that the magnitudes of the terminal point acceleration reference and terminal point velocity reference of one or more trajectory segments are substantially zero at the commanded position. Preferably, the predetermined threshold is substantially zero, i.e., the commanded position.
[0103] 15 is a flow diagram 200 illustrating the main steps in adapting the motion constraints 188 in the motion profile generator 182 to position-based movements and translating the motion control profile into control signals sent to the drive mechanism to effectuate the load equipment transition. The operation begins with the trajectory generator receiving (204) an instruction for the load equipment 130 to proceed from a current position to a commanded position, in this case a particular storage row or grid cell. The trajectory generator 182 generates (206) a trajectory or motion control profile given the defined constraints and the commanded position. In this particular embodiment of the invention, the trajectory generator 182 is periodically advanced (208) with this motion profile in order to calculate the wheel motion torque demands to drive the load equipment 130.
[0104] Returning to FIG. 13, in operation, a destination position 190 is provided to the motion profile generator 182 by the master controller 174, optionally via a motion manager 192. The destination position 190 comprises a command to go from a current position or state of the load handling equipment on the grid structure to a command position. The current state of the load handling equipment may be a rest position of the load handling equipment on the grid structure or may be a current kinematic dynamic state (e.g., speed) of the load handling equipment on the grid structure. From the current state of the load handling equipment on the grid structure, the motion profile generator 182 calculates a motion control profile as a function of one or more motion constraints that may represent mechanical constraints or motion constraints 188 of the drive mechanism 186 or user preferences regarding the operation of the drive mechanism. As shown in FIG. 16, the motion profile generator 182 receives an input of a set of constraints 188. These constraints include an upper limit on speed 188a, an upper limit on acceleration 188b, an upper limit on deceleration 188c, and an upper limit on jerk 188d. These constraints 188 may be set once during deployment of the motion profile generator or may be reset each time the load equipment moves on the grid structure. For example, the motion profile generator 182 may be updated when a new commanded position is received or a different value for one of the other constraints is received. The motion profile generator allows for acceleration and deceleration limits to be set separately to accommodate motion control profiles with asymmetric acceleration and deceleration. In response to receiving a commanded position, the motion profile generator calculates a time-optimal motion control profile 194 based on the constraints that defines a trajectory for moving the load equipment from the current position to the commanded position.
[0105] As shown in FIG. 15, generating the motion control profile includes applying (206) a manipulated variable, which in the case of an S-curve profile is a jerk, to continuously increase and / or decrease the acceleration during the acceleration and deceleration phases of the motion control profile. As shown in FIG. 12 and FIG. 16, the motion control profile comprises one or more jerk references, acceleration references, velocity references, and position references. These references are related to each other as derivatives with respect to time in mathematics. A jerk reference is the derivative of acceleration, acceleration is the derivative of velocity, and velocity is the derivative of position. The motion profile generator periodically progresses the motion control profile by calculating one or more jerk references, acceleration references, velocity references, and position references for each of the different trajectory segments of the motion control profile. Typically, there are up to seven different stages in an S-curve motion profile, as described above with reference to FIG. 12. The motion control profile generator defines these references as a function of time for each of a set of defined motion profile stages of the motion control profile. The present invention is not limited to seven different stages of the S-curve motion profile, but may have any number of stages, from a minimum of four stages to more than seven stages. For example, under four stages, there may be no constant velocity stage of Phase III and the load handling device does not reach the maximum velocity constraint. Similarly, the number of stages may exceed seven, provided there is at least one change in the trajectory as a result of one or more constraints or changes in the commanded position. The generation of motion control profiles according to embodiments of the present invention is described in more detail below in the next section (Section 3.0). In the present invention, the terms "motion control profile", "motion profile" and "trajectory" are used interchangeably to refer to the same feature.
[0106] As shown in FIG. 14, the position of the load handling equipment relative to the grid structure is measured by one or more position sensors 198a, 198b. In this particular embodiment of the invention, the position sensors 198a, 198b comprise so-called "fifth" wheels. This means that the position sensor wheels are "fifth" wheels because the first and second wheel sets in this embodiment each consist of four wheels at the front pair and the rear pair of the load handling equipment. An additional "fifth" wheel is present between the first and second wheel sets that monitor the position of the load handling equipment on the grid structure in the first and second directions, respectively. As shown in FIG. 14, a first "fifth" wheel 198a is mounted adjacent to one of the first wheel sets and a second "fifth" wheel 198b is mounted adjacent to one of the second wheel sets. A first "fifth" wheel 198a corresponding to the first position sensor is configured to engage the track when the load equipment is traveling in a first direction such that rotation of the first "fifth" wheel is an indication of the position and direction of travel of the load equipment in time. Similarly, a second "fifth" wheel 198b corresponding to the second position sensor is configured to engage the track when the load equipment is traveling in a second direction such that rotation of the second "fifth" wheel is an indication of the position and direction of travel of the load equipment in time in the second direction. The first and second directions may be the X and Y directions along the track, respectively. In this particular embodiment of the invention, the one or more position sensors comprise an incremental encoder comprising a rotary electromechanical device that generates pulses indicative of the position and direction of rotation that can be translated into a displacement of the load equipment relative to the grid structure as the "fifth" wheel rotates. The "fifth" wheel is mounted on an arm and can be biased downward to engage the track. In addition to measurements from one or more position sensors (e.g., the "5th" wheel), the desired command position on the grid structure defined by the master controller 174 is communicated to the motion manager 192 where it is converted by the motion manager 192 into relevant position units before being communicated to the motion profile generator to effect generation of the motion control profile.For example, the motion manager 192 converts a commanded position from the master controller, expressed as the number of grid cells that the load equipment is commanded to move on the grid structure, into "position units" that correspond to readings from one or more position sensors 198 (the "fifth" wheels) that are understood by the controller or motion profile generator. The controller or motion profile generator 182 can then correlate the requested commanded position from the master controller with position readings from the "fifth" wheel or one or more position sensors, or with a position reference provided by a motion control profile. The generation of motion control profiles or trajectories for point-to-point movements of load equipment on a grid structure is described in more detail below.
[0107] Once the motion control profile is generated, the position controller 184 converts data related to the motion control profile 194 into control signals that can be sent directly to the drive mechanism 186 to effect the load equipment's transition to the commanded position. As mentioned above, the position controller 184 can form part of the load equipment controller or control system 170 in that the controller or control system comprises functional components related to the trajectory generator 182 and the position controller 184. FIG. 13 is a block diagram illustrating inputs and outputs of an exemplary position controller according to an embodiment of the present invention. The motion control profile data generated by the trajectory generator is communicated to the position controller 184, where it is converted into control signals for driving the wheels of the wheel assembly. In accordance with the motion control profile, the position controller 184 varies the power to the drive mechanism to vary the speed of the load equipment relative to the grid structure. As with the motion control generator, the position controller 184 can be a functional component present in the controller or can be a separate component from the motion profile generator with its own control system. For example, the wheel pairs of the wheel assembly can be connected to a local controller and configured to convert the motion control signals from the motion profile generator. Conversion of the motion control signals includes, but is not limited to, converting the motion control data into appropriate torque control signals for driving the wheel drives or motors.
[0108] As discussed above and shown in FIG. 16, the motion control profile defines the trajectory of the load handling equipment's point-to-point movement relative to the grid structure over time in terms of a position reference, a velocity reference, an acceleration reference, and a jerk reference (in the case of an S-curve profile). These references represent function calculations by the trajectory generator that define how each of the motion attributes of the drive mechanism are controlled as a function of time for a given point-to-point movement. These references are mathematically related to each other as differentials. The conversion of the motion control profile into control signals that instruct the load handling equipment to execute the desired point-to-point movement in accordance with the motion control profile involves the position controller generating a feedforward signal from the motion control profile, more specifically, an acceleration reference derived from the motion control profile.
[0109] FIG. 17 is an exemplary position controller 184 illustrating the architecture of the position controller for converting signals from a trajectory or motion profile generator to the drive mechanism of the load equipment. Since the mass of the load equipment is known, a feedforward signal 210 (shown as FFs in FIG. 17) represents a calculated torque demand proportional to an acceleration reference derived from the motion control profile, more specifically, proportional to the derivative of the velocity reference. The feedforward signal 210 can be converted to an appropriate signal for driving the wheels of the wheel assembly. However, to ensure that the point-to-point movements of the load equipment on the grid structure are aligned with the trajectory segments of the motion control profile, the calculated feedforward signal 210 is compensated by a feedback signal 212 (shown as FBs in FIG. 17) indicative of the actual kinematic state of the load equipment relative to the grid structure. The kinematic state of the load equipment on the grid structure can include, but is not limited to, the position of the load equipment relative to the grid structure and the velocity of the load equipment. The velocity of the load equipment can be derived from the first derivative with respect to time of the position measurements from one or more position sensors 198.
[0110] Based on the feedback signal 212, the position controller 184 will adjust or compensate the control signals from the one or more position sensors 198 as necessary to ensure that the load equipment moves as closely as possible to the motion control profile. In this particular embodiment of the invention, the feedback signal 212 from the one or more position sensors is fed back to the position controller to reflect the true state of the load equipment relative to the grid structure. In response to receiving data signals from the one or more position sensors, the position controller can be instructed to regenerate one or more trajectory segments of the motion control profile by varying the manipulated variable jerk to ensure that the load equipment arrives at the commanded position without undershooting or overshooting, i.e., when acceleration and velocity become zero.
[0111] In a particular embodiment of the invention, a feedback signal 212 from one or more position sensors is combined with a feedforward signal 210 to generate a total torque demand of the wheels or the drive mechanism driving the wheels. A trajectory position derived from the motion control profile is compared to a measured position from one or more position sensors. This comparison represents a position error, i.e., the difference between a trajectory position reference and the measured position as a function of time. The position error is fed to a PID (proportional, integral, and derivative) controller or PI controller 214 that corrects the position to a set point represented by the motion control profile based on the PID or PI. The output from the PID or PI 214 is a correction torque that, when applied, corrects the position error to bring the actual trajectory of the load handling equipment back into agreement with the motion control profile. The feedforward signal 210 represents the torque required to meet the velocity / acceleration required by the trajectory generator. The correction torque from the PID or PI 214 is combined with the torque demand of the feedforward signal 210 to generate a total torque demand. As shown in the communication paths in Figure 17, both the "acceleration feedforward" and "velocity feedforward" signals may be combined in the feedforward calculation and stored in a lookup table. In practice, the acceleration feedforward torque demand term and the velocity feedforward torque demand term are summed with a constant torque demand term, i.e., zero-order torque demand, which is a constant value (whose sign is sign(trajectory velocity)). The magnitudes of the acceleration feedforward and velocity feedforward torque demand terms are determined from the acceleration and velocity of the generated motion control profile or trajectory, respectively.
[0112] In one example, the total torque demand is calculated as follows:
[0113] Total torque demand = Feedforward torque demand + Feedback torque demand Where: Feedforward torque demand = acceleration feedforward torque demand + velocity feedforward torque demand + zero-order torque demand value Where: Acceleration feedforward torque demand = orbital acceleration x acceleration feedforward gain Velocity Feedforward Torque Demand = Trajectory Speed x Velocity Feedforward Gain Zeroth-order feedforward torque demand = sign(orbital speed) × zeroth-order torque demand value Here, the following three values have been empirically determined:
[0114] The acceleration feedforward gain is a value that depends on the estimated total mass of the loading equipment, including the container and its contents, such that the heavier the container is being carried by the loading equipment, the higher the gain value.
[0115] The velocity feedforward gain also depends on the estimated total mass of the loading device.
[0116] The zero order torque demand value is the total torque demand required just to keep the loading device moving at a constant near zero speed, i.e., it is a proportionality factor.
[0117] These three components, the acceleration feedforward torque demand term, the velocity feedforward torque demand term, and the zero-order torque demand value, are summed to form a feedforward torque demand that is stored in a lookup table. When compensating the calculated feedforward torque demand with a feedback torque signal, the controller retrieves the feedforward torque demand from the lookup table and combines it with the feedback torque measurement.
[0118] Alternatively, values for the acceleration feedforward gain, velocity feedforward gain, and zero order torque demand may be calculated rather than empirically determined.
[0119] The acceleration feedforward torque demand term is the component of the total torque demand that provides a torque that allows the loading equipment to match the required acceleration of the track. The thrust required to meet the required acceleration can be calculated by multiplying the required acceleration by the total mass of the loading equipment, including the storage container and contents. This thrust can be converted to a torque demand by multiplying by the wheel radius (torque = force x radius). In instances where the total mass of the loading equipment is known, the acceleration feedforward gain can be calculated rather than empirically determined.
[0120] Acceleration feedforward gain = total mass of the loading device x wheel radius The velocity feedforward torque demand term and the zero-order torque demand term are components of the total torque demand that provide torque that allows the loading equipment to overcome drag. A variety of different models are known in the art for calculating drag, which may have a component proportional to the speed, a component proportional to the square of the speed, and a constant component. Drag may be due to air resistance, rolling resistance of the wheels on the track, and / or grade resistance if the track is not perfectly level. In this application, it is likely that the velocity squared term of the drag is not important and may be omitted to make the torque demand calculation faster and less computationally burdensome.
[0121] The thrust required to overcome the rolling resistance can be calculated by multiplying the normal force (total weight of the loading equipment including the storage container and contents) by the rolling resistance coefficient, which is a property of the wheels and truck. This thrust term can be converted to a torque demand by multiplying by the vehicle radius (torque = force x radius). In instances where the total mass of the loading equipment is known and the rolling resistance coefficient of the wheels on the truck is known, the velocity feedforward gain can be calculated rather than empirically determined.
[0122] Velocity feedforward gain = total mass of the loading equipment x acceleration due to gravity x rolling resistance coefficient x wheel radius In practice, it may be easier to empirically determine the velocity feedforward gain and zero-order torque coefficient than to calculate them.
[0123] Determining the total mass of the loading equipment can be difficult because the mass of any storage container and contents must be taken into account. Typically, the loading equipment is configured to receive instructions from a master controller to remove a storage container from a particular storage location within the grid framework structure. In some examples, the master controller uses a database to store data about the location and contents of all storage containers within the grid framework structure, and therefore already has information about the mass of each storage container. When the loading equipment picks up a storage container from the grid framework structure, the master controller communicates the mass of that storage container to the controller on the loading equipment, so that this mass can be used in the calculation of the total mass of the loading equipment. Similarly, when the loading equipment returns a storage container to the grid framework structure, the master controller communicates to the controller on the loading equipment that there is no mass of the storage container to be considered. The master controller must keep track of the mass of each storage container and contents and constantly update it (e.g., when items are removed from the storage container to fulfill a customer order). In this way, the mass of any storage containers and contents carried by the loading device can be added to the mass of the loading device itself to calculate the total mass.
[0124] An alternative method of determining the mass of a storage container and its contents is to measure the current draw of the motor used to operate the lifting device and hoist the storage container into the container receiving space of the loading device. Since the current draw varies approximately linearly with the mass of the storage container, this provides a simple and convenient method of determining the mass and has the advantage that the master control system does not need to keep track of the mass of every single storage container and communicate this information to the controller on the loading device. Figure 36 is a graph plotting the measured average current draw of the hoist motor plotted against the mass of the contents of the storage container. It can be seen that the data points fall on a straight line. The intercept represents the current required to lift an empty storage container (approximately 6kA). Since the data points fall on a straight line, a linear fit can be used to capture the relationship between the current draw and the mass of the storage container, so the measured current can be used to easily and accurately estimate the mass of the container and its contents.
[0125] The torque demand calculation described above is one example of how the torque demand may be calculated. Those skilled in the art will appreciate that other methods of calculating, estimating, or measuring acceleration and drag forces may also be applied to the present invention.
[0126] In some examples, the velocity, acceleration, and / or jerk constraints can be modified depending on the mass of the storage container and contents.
[0127] Since the acceleration feedforward torque demand is the product of the track acceleration and the acceleration feedforward gain, and the acceleration feedforward gain is proportional to the total mass of the loading equipment (including any storage containers and contents), a change in the total mass of the loading equipment and its cargo results in a change in the acceleration feedforward torque demand. The acceleration feedforward torque demand in some instances is the most significant component of the total torque demand. Since the wheel motors are rated to provide a given maximum torque, loading equipment carrying particularly heavy loads will require more torque from the wheel motors than they can provide, resulting in the loading equipment being unable to maintain the commanded acceleration or at risk of overheating due to the wheel motors operating at a torque above their rated torque. Alternatively, if the wheel motors are over-rated to overcome this problem, the full torque capacity of the wheel motors will only occasionally be used, effectively over-designing them for the application and therefore making them heavier and more expensive than necessary.
[0128] To overcome this problem, in some examples, the acceleration constraint can be made dependent on the total mass of the loading equipment and its cargo. For example, if the value of the acceleration constraint is inversely proportional to the total mass of the loading equipment and its cargo, the acceleration feedforward torque demand will be substantially constant and substantially independent of the mass of any storage containers and contents being transported by the loading equipment. This has the advantage that the torque rating of the wheel motors can be chosen to match the expected acceleration feedforward torque demand, with the torque demand not varying substantially with mass.
[0129] Similarly, since the velocity feedforward torque demand is the product of the track velocity and the velocity feedforward gain, and the velocity feedforward gain is proportional to the total mass of the loading equipment (including any storage containers and contents), a change in the total mass of the loading equipment and its cargo will result in a change in the velocity feedforward torque demand. As with the acceleration constraint, if the velocity constraint were (for example) inversely proportional to the total mass of the loading equipment and its cargo, then the velocity feedforward torque demand would be substantially constant and substantially independent of the mass of any storage containers and contents carried by the loading equipment. Making both the acceleration constraint and the velocity constraint dependent (e.g., inversely proportional) on the total mass of the loading equipment means that none of the three terms in the feedforward torque demand equation are mass dependent, and therefore the torque demand is substantially constant with respect to the mass of the loading equipment and its cargo.
[0130] In some examples, the value of the jerk constraint may depend on the total mass of the loading equipment and any storage containers and contents located within the container-receiving space of the loading equipment. For example, loading equipment carrying heavy storage containers may be more stable due to a lower center of gravity, and therefore can tolerate larger values of jerk without risk of the loading equipment tipping over on the grid. The absolute value of the jerk constraint can be defined as a function of the total payload mass of the loading equipment, either as a continuous function or as a set of discrete values.
[0131] In other examples, the value of the jerk constraint may depend on the contents of the storage container, such as by using a smaller jerk value when the load handling equipment is transporting a storage container into its container-receiving space that contains delicate or fragile items, and a larger jerk value when the load handling equipment is transporting a storage container into its container-receiving space that contains more sturdy items or items that are less likely to roll and strike the sides of the container.
[0132] One method that can be used to determine the value of the jerk constraint is to apply a trapezoidal profile (theoretically infinite jerk) to the load handling equipment, measure the time it takes to achieve the commanded position, and measure the jerk of the actual movement of the load handling equipment. An S-curve profile can then be run with a range of jerk values around the measured jerk and an evaluation of the effect on the position error and total time it takes to reach the commanded position as compared to the trapezoidal profile.
[0133] Although specific examples have been described herein of how the jerk constraints, acceleration constraints, and velocity constraints may be varied with respect to the mass of the loading device, it will be understood that the scope of the invention encompasses other variations and other values or numbers of jerk constraints, acceleration constraints, and / or velocity constraints may be applied.
[0134] The calculated total torque demand is converted into appropriate control signals to drive the wheels. In this particular embodiment of the invention, the total torque demand is shared between the first wheel set or the second wheel set depending on whether the first wheel set is engaged with the grid structure or the track or the second wheel set is engaged with the grid structure. In this particular embodiment of the invention, as described above, each wheel of the first wheel set and the second wheel set is driven individually by a hub motor 220. The first wheel set comprises a wheel pair at the front of the body of the load handling device and a wheel pair at the rear of the body of the load handling device. For the second wheel set, there are similar wheel pairs, i.e., a first wheel pair and a second wheel pair, on either side of the body of the load handling device. In operation, the wheel pairs are driven synchronously as if they were driven by a "virtual or imaginary" axle. For example, the front wheel pairs are driven synchronously as if they were driven by the same axle and the rear wheel pairs are also driven synchronously as if they were driven by the same axle. The same principle applies to the second wheel set, although driven by individual hub motors. The invention is equally applicable when all four wheels of the first wheel set are driven synchronously. Similarly, all four wheels of the second wheel set may be driven synchronously. The advantage of driving the wheels of a wheel assembly individually by hub motors is that the torque to the wheels can be distributed in a variable manner, which is particularly important during wheel slip.
[0135] The torque derived from the motor control profile, or the compensated total torque demand, is distributed among the wheels of the wheel assembly by the biasing mechanism 216. The biasing mechanism 216 "splits" or distributes the total torque demand between the front "axle" and the rear "axle" to account for any weight transfer of the load equipment on the grid structure. For example, when the load equipment is accelerating on the grid structure, a greater percentage of the calculated total torque demand is transmitted to the front "axle". Conversely, when the load equipment is decelerating on the grid structure, the total torque demand is transmitted more to the rear "axle". In this manner, the biasing mechanism varies the amount of torque demand to the front "axle" and the rear "axle" to transfer the load equipment's weight between the front and rear portions of the load equipment accordingly when accelerating or decelerating on the grid structure.
[0136] The offset mechanism 216 applies a differential torque demand to the wheels, and more specifically to the motors driving the wheels, according to the track acceleration reference of the motion control profile. The track acceleration reference and / or track speed reference of the motion control profile provide an indication or reference point as to whether the load handling equipment is accelerating or decelerating on the grid structure. In other words, the differential torque applied to the wheels follows the track acceleration reference of the motion control profile, i.e., there is a proportional relationship between the torque distribution to the vehicles and the track acceleration reference of the motion control profile. In response to a signal from the motion control profile generator commanding to accelerate, the offset mechanism transmits more of the calculated total torque demand to the front axle, and conversely, when commanded to decelerate, more of the total torque demand is shifted to the rear axle, or the total torque demand is reduced to provide a "braking" force. Similarly, when moving at a constant speed on the grid structure, the total torque demand is split substantially equally between the front "axle" and the rear "axle".
[0137] The torque demand on the wheel motors as well as the differential torque applied to the front and rear "axles" can be controlled on either side of the load equipment body to control yaw or roll of the load equipment on the grid structure. For example, depending on the direction of travel of the load equipment on the grid structure, the torque demand can be used to control the speed to the left or right wheel set and thereby control the yaw or roll angle of the load equipment on the grid structure. Ideally, the grid members are arranged to extend in orthogonal directions, i.e., in the X and Y directions, and the differential torque to the left and right wheels is controlled to prevent the load equipment from yaw or roll on the track, i.e., to cause the load equipment to travel in a substantially straight line.
[0138] In addition to converting the motion control profile into a control signal for driving the drive mechanism, the biasing mechanism 216 can also be configured to control slip of the wheels on the track and periodically compensate the motion control profile in response to receiving a position signal from one or more position sensors and / or one or more position tracking sensors mounted on the load handling equipment. One or more position sensors can also be used to determine the slip of any one of the first and second wheels on the track. This is shown in FIG. 17 as a separate slip component 218 in the position control device. Wheel slip occurs when the rotational speed of any one of the wheels is greater than the kinematic condition of the load handling equipment on the grid structure. The kinematic condition of the load handling equipment on the grid structure can be determined or derived from the first derivative with respect to time of the position by one or more position sensors, for example, from the rotational speed of the first or second "fifth" wheel depending on whether the load handling equipment is traveling in a first or second direction. The individual rotational speed of each wheel of the first and second wheel sets is determined by one or more wheel encoders located adjacent to each wheel. The wheel encoder or encoders may be incremental encoders comprising a rotating electromechanical device that generates pulses indicative of the rotational speed of the wheel. In this particular embodiment of the invention, the slip control manager manages the slip of each wheel of the wheel assembly by individually comparing the rotational speed of the wheel to the kinematic dynamics of the load handling device on the grid structure determined from the first "fifth" wheel or the second "fifth" wheel depending on the direction of travel of the load handling device on the grid structure. In this particular embodiment of the invention, the slip control manager is located within the position controller, as shown in the schematic block diagram of the position controller in FIG. 17.
[0139] If the rotational speed of any one of the wheels is greater than the kinematic condition of the load handling device on the grid structure, the slip control manager removes or reduces the torque to that wheel by controlling the shifting of the torque demand for that wheel until the rotational speed of that wheel catches up with the kinematic condition of the load handling device. For example, the position controller can change the torque demand of the slipping wheel, i.e., reduce the torque demand, via a shifting mechanism, to slow down the rotational speed of the slipping wheel, i.e., to weaken the rotation of that wheel, or can completely remove the torque demand for that wheel. Alternatively, the slip control manager can redistribute torque from the slipping wheel to other wheels to balance the speed of the wheels of the wheel assembly to match the kinematic condition of the load handling device on the grid structure.
[0140] It should be understood that the architecture shown in Figure 17 is intended to be exemplary only, and that other arrangements of the components of the position controller are within the scope of the present invention. For example, the position controller may be integrated with the controller for generating the motion control profile or trajectory, or may be a separate controller.
[0141] 3.0 Trajectory Generation The generation of the trajectory or motion control profile involves controlling the number and duration of trajectory segments such that the acceleration and velocity are substantially zero at the commanded position when the trajectory segments are "stitched". According to the present invention, the generation of the trajectory segments relies on applying one or more jerk references that depend on the identification of one or more situations in the trajectory or one or more conditions of the trajectory being met. These situations may be one or more of the constraints and / or a change in the commanded position. In generating a trajectory, the trajectory generator must be able to determine which trajectory segments are required for the trajectory to reach its end point starting from the current point. In identifying these situations, the generator must be able to perform this decision process (i.e., generate the trajectory) regardless of the current trajectory point, since its inputs (notably but not limited to the commanded position) may change at any time. For example, the commanded position is typically modified during the passage of a grid cell based on feedback from the fifth wheel and / or grid sensors.
[0142] In its simplest form, the result of asking the generator to regenerate the trajectory (e.g., by modifying its inputs), whether the trajectory has ended or is still in progress, includes generating one or more trajectory segments, each of which: The value of the manipulated variable that applies from this point onwards, The length of time to apply the manipulated value (i.e. trajectory segment duration, transition time), and The endpoints of that segment (because this is useful information to cache) Equipped with.
[0143] As a result, a trajectory segment that is already in progress is replaced by a regenerated trajectory segment when the trajectory terminates or otherwise becomes the in progress trajectory segment.
[0144] The values of the manipulated variables may take any value (within the rules described below) regardless of their values prior to the request to regenerate the trajectory. In case of infinite jerk trajectories (S-curve trajectories), the number of trajectory segments is controlled by applying one or more jerk constraints to increase or decrease the magnitude of acceleration during different stages of the motion control profile. To facilitate the description of the invention, jerk constraints are applied in the acceleration phase (positive acceleration) and deceleration phase (negative acceleration). The acceleration phase corresponds to a trajectory where the trajectory velocity reference is increasing. Conversely, the deceleration phase corresponds to a trajectory where the trajectory velocity reference is decreasing. Thus, there is a set of jerk constraints for changing the acceleration in the acceleration phase and a set of jerk constraints for the deceleration phase. These are summarized as follows:
[0145] Acceleration Phase Jerk 1 - Increase the magnitude of acceleration when you want to increase the velocity.
[0146] Jerk 2 - Decreasing the magnitude of acceleration when you want to increase the velocity.
[0147] Deceleration Phase Jerk 3- Increasing the magnitude of acceleration when you want to decrease the velocity.
[0148] Jerk 4 - Decrease the magnitude of acceleration when you want to decrease the velocity.
[0149] Jerk 1, Jerk 2, Jerk 3, and Jerk 4 are sometimes referred to as J1, J2, J3, and J4, respectively. In this particular embodiment of the invention, four non-zero jerk constraints are used. Jerk 1 and Jerk 2 are used to increase and decrease the acceleration in the acceleration phase, respectively, and Jerk 3 and Jerk 4 are used to increase and decrease the magnitude of acceleration (deceleration) in the deceleration phase, respectively. In this particular embodiment of the invention, the motion control generator is configured to switch between Jerk 1 and Jerk 2 in one or more switching patterns to control the number and duration of trajectory segments in the acceleration phase, and to switch between Jerk 3 and Jerk 4 in one or more switching patterns to control the number and duration of trajectory segments in the motion control profile with the goal that the trajectory reaches the commanded position in the shortest possible time, and that the acceleration and velocity reach substantially zero at the desired commanded position. A zero value for the jerk constraint is used when it is desired to maintain a desired acceleration in the acceleration phase or a desired deceleration in the deceleration phase.
[0150] In this particular embodiment of the invention, the absolute values of the jerk constraints 1, 2, 3, and 4 are equal, and the motion control generator is configured to switch the sign S (+ or -) when it is desired to increase or decrease the acceleration magnitude in the acceleration phase. The positive sign "+" is used when it is desired to increase the acceleration magnitude, and the negative sign "-" is used when it is desired to decrease the acceleration magnitude. The absolute value of the jerks 1, 2, 3, and 4 chosen depends on the ability of the loading equipment to tolerate sudden changes in acceleration without tipping or overturning on the grid structure, which depends on the stability of the loading equipment on the grid structure. For a very stable loading equipment with a relatively low center of gravity, the loading equipment can tolerate much larger jerks compared to a relatively less stable loading equipment. In the case of a loading equipment with a footprint of one grid cell, the absolute value of the jerk is chosen such that the loading equipment can accelerate at a certain rate up to the maximum speed defined by the constraints without tipping or overturning. In this particular embodiment of the invention, the absolute value of the jerk is chosen to be 20 m / s 3 Therefore, Jerk 1 and Jerk 2 are +20m / s 3 and -20 m / s 3 Similarly, Jerk 3 and Jerk 4 will have a value of +20m / s 3 and -20 m / s 3 However, it is not necessary that jerks 1, 2, 3, and 4 have the same value to control the number of trajectory segments. The jerk constraints can also have different absolute values. Alternatively, the jerk constraint can be two values that are used to increase or decrease the acceleration magnitude in both the acceleration and deceleration phases, respectively.
[0151] As discussed above, in some examples, the value of the jerk constraint may depend on the total mass of the loading device and any storage containers and contents located within the container-receiving space of the loading device.
[0152] In generating the motion control profile, the jerk value can be switched between any of the above jerk constraints regardless of its value prior to the request to regenerate the trajectory. For example, if the trajectory is in the acceleration phase (i.e., speeding up towards the commanded velocity) and the commanded position is modified back to the starting position, the generator must immediately generate a new trajectory segment that appears to decelerate the trajectory (i.e., slow down the speed towards zero); the fact that the trajectory was in the acceleration phase has no bearing on the decision to generate a new trajectory segment that decelerates the trajectory.
[0153] As another example, if the trajectory velocity is already at the commanded velocity limit but the trajectory position is still far from the commanded position, then in order to meet the "shortest possible time" requirement, if the commanded velocity or constraint velocity (absolute value) is increased and all other constraints at the trajectory generator's input remain unchanged, the trajectory generator must start increasing the trajectory velocity, possibly up to the new commanded velocity, depending on the new value of the commanded position. The fact that the trajectory was in a constant velocity phase prior to the command to increase the velocity constraint or commanded velocity has no bearing on this decision.
[0154] Generating a trajectory involves establishing the status or state of the trajectory at a given time, and generating one or more trajectory segments according to the trajectory conditions at that time so that the trajectory can start from a current point and reach a desired destination (which may be a commanded position). A trajectory may comprise multiple trajectory segments that collectively represent a trajectory or motion control profile, and each trajectory segment has a respective trajectory segment end point. In the present invention, the term "end point" reflects the end trajectory point of a given trajectory segment.
[0155] The process of establishing the status of the trajectory at a given time is done by a decision process by elimination in which a series of questions are posed or queried to the controller or control system in a particular order until one of the answers reveals what course of action should be taken to respond to the new generator input values. In the first example above, the obvious course would be to start slowing the velocity down to zero by generating one or more trajectory segments, since the longer it takes to do that, the further the trajectory will be from the desired position. In the second example, the obvious course would be to increase the velocity, since not doing so (i.e. staying at the same velocity) would take the longer it would take for the trajectory to reach the desired position.
[0156] In the process of generating a trajectory, the generator may generate multiple trajectory segments, in which case the first trajectory segment is used and the others can be cached for later use. Each of the multiple trajectory segments can represent one or more velocity transitions. In simple terms, the trajectory is generated from a starting point (p0, v0, a0) to a terminal point (p e , v e , a e ), it may not be sufficient to generate a single trajectory segment to bring the trajectory to the commanded position, v, because the trajectory may be accelerating away from the commanded position and a trajectory segment is needed to get the trajectory back on course toward the commanded position, and / or because the trajectory may be overaccelerating to get the trajectory back to the commanded position without overshooting. f All of these conditions must be considered when transitioning to the desired final velocity value v, which may involve generating multiple trajectory segments. fmay be effectively zero in that the trajectory is a point-to-point move to a command position, and the control system is commanded to generate the trajectory of that point-to-point move such that the final velocity value at the command position is effectively zero. When a new request to regenerate a trajectory occurs (again as a result of a change in the generator inputs), the generator must clear all cached trajectory segments, since they may be invalid under the new generator inputs. The action of regenerating a trajectory, if it is only one, is to recalculate the new trajectory segments.
[0157] The following sections, in conjunction with the flow chart 300 shown in Figure 18, outline the decision process involved in generating a finite jerk trajectory according to an embodiment of the invention. Each section is structured in the form of a series of questions (in bold) representing logic checks that are performed to generate an answer, and a policy that must be taken depending on the answer to the question. Each policy is represented by a flow chart shown in Figures 19 to 26. Each question is intended to establish the status of the current trajectory before applying the policy. The series of questions in the generation of the trajectory are as follows:
[0158] A) Is a forced deceleration in progress (or currently requested) (302)? Forced deceleration is applied in situations where, for example, there is a change in the commanded position and a different trajectory segment needs to be generated to reach the updated commanded position, i.e., the trajectory is required to terminate sooner. This process uses a forced deceleration algorithm, described further below, to find a deceleration trajectory segment to reach the updated commanded position, i.e., such that the acceleration and velocity are substantially zero at the updated commanded position. To choose a deceleration trajectory such that the trajectory reaches the updated commanded position without overshooting and does not exceed the commanded deceleration, the control system may need to generate new inputs, including but not limited to a new constraint on the deceleration rate, to allow for a stronger deceleration of the trajectory. The method described below for forced deceleration involves applying a forced deceleration constraint to the trajectory. In some cases, an algorithm, more specifically a root-finding algorithm, may be applied to determine the appropriate deceleration constraint for the trajectory. The objective function is the degree of overshoot from the updated commanded position that the trajectory would experience if deceleration were performed under the chosen deceleration constraint, and the root is the deceleration at which the objective function is substantially zero. This algorithm successfully generates a more accurate approximation to the root, which corresponds to a deceleration value that would allow the trajectory to exactly reach the updated commanded position. For example, the deceleration phase trajectory may be required to reach a new commanded position that is closer than the previously requested commanded position. When applying the forced deceleration algorithm, the speed reference is adjusted from the initial starting point to a desired final speed v having a magnitude of substantially zero. f A velocity transition algorithm in accordance with the present invention may be executed to transition the magnitude of the velocity from the initial value to the desired final velocity v, which in this case is substantially zero. f Transitioning to may include generating one or more trajectory segments depending on the status of the start point or the current status of the trajectory.
[0159] If the answer to question A is yes (303), then the control system generates the trajectory segments necessary to bring the trajectory to the commanded position according to the method described in the Forced Deceleration section below and shown in Figure 34. Otherwise, if this trajectory condition does not exist (i.e., no forced deceleration is in progress or requested), the control system proceeds to the next question.
[0160] B) Does the (absolute value of) the current trajectory acceleration exceed the command acceleration (304)? In this situation, the absolute value of the current trajectory acceleration is compared to the commanded acceleration (acceleration constraint). If the absolute value of the current trajectory acceleration is greater than the commanded acceleration, a new trajectory segment needs to be calculated such that the magnitude of the trajectory acceleration is reduced to the commanded acceleration. In this situation, one or more jerk values are applied to the trajectory to reduce the trajectory acceleration (see FIG. 20), i.e., such that the terminal point of the acceleration reference has a reduced terminal point acceleration equivalent to the commanded acceleration. The magnitude of the trajectory acceleration is compared to the commanded acceleration each time a jerk is applied to the trajectory until the trajectory acceleration is reduced to the commanded acceleration. This is done by applying the following start (p0, v0, a0) and terminal (p e , v e , a e ) is exemplified by the generation of trajectory segments having
[0161] The manipulated variable (jerk) value used is J=J2 (reduce the orbital acceleration reference).
[0162] The duration of the orbital segment is
[0163]
number
[0164] is given by:
[0165] ·Terminal point acceleration a e , end point velocity v e , and the end point position p eis obtained by direct application of trajectory equations 3, 4, and 5 using the starting point (p0, v0, a0), J2, and the duration T of the segment.
[0166] If the situation is such that the current trajectory acceleration does not exceed the commanded acceleration, the control system proceeds to the next question.
[0167] C) If the orbit is now taken to a stationary position (zero velocity and zero acceleration), the stationary position is a) Is it essentially equal to the commanded position (i.e., to within 0.5 position units) (306)? b) Is it on the opposite side of the commanded position from the current position of the trajectory (i.e., does it overshoot) (308)? c) Is the trajectory further away from the commanded position than its current position (i.e., is it currently moving away from the commanded position) (310)? Knowing that the magnitude of the trajectory acceleration will not exceed the commanded acceleration, the control system checks whether the trajectory is at the commanded position when the trajectory is stopped. This also gives an indication of how far the trajectory is from the commanded position or whether the trajectory will overshoot the commanded position. A series of questions are posed to the control system and by a process of elimination the status of the trajectory in its current state can be established and the required counter measures applied to the trajectory to ensure that it is on course for its point-to-point movement.
[0168] If the answer to question (a) is yes, the control system calculates the acceleration a from the current trajectory point (p0, v0, a0) to the terminal point e and the terminal velocity v e The necessary steps are taken to bring the trajectory down to zero velocity at the commanded position so that it becomes a stationary position by generating one or more trajectory segments to an end point where p0, v0, a0 is substantially zero at the commanded position (see FIG. 21). This is done by using the following start point (p0, v0, a0) and end point (p e , v e , a eThis is exemplified by the creation of one trajectory segment having
[0169] The manipulated variable (jerk) value used is J=J2 (reduce the orbital acceleration reference).
[0170] The duration of the orbital segment is
[0171]
number
[0172] is given by:
[0173] ·Terminal point acceleration a e and the terminal velocity v e is essentially zero, and the end point p e is obtained by direct application of trajectory equations 3, 4, and 5 using the starting point (p0, v0, a0), J2, and the duration T of the segment.
[0174] However, if the answer is "no" to question (a) but yes to either of the last two questions (b) and (c) (308, 310) (see Figures 22 and 23), then the situation is that the trajectory is overshooting the commanded position. In this case, the corrective action is to force the trajectory velocity to a "reverse" value so that the trajectory can recover by "moving back" toward the commanded position. Similar to the forward trajectory toward the commanded position, one or more reverse trajectory segments are provided by applying one or more jerk values to cause a powered device, such as a loading device operating in reverse, to progress toward the commanded position. The duration or length of the reverse trajectory segments is determined by the degree of overshoot from the commanded position. For example, if the degree of overshoot is large, the trajectory will have enough time to reach the commanded velocity before descending to zero velocity at the commanded position. Conversely, if the degree of overshoot is short, the trajectory velocity may not reach the commanded velocity, and thus the desired peak velocity v , which allows the reverse trajectory to reach the commanded position in the shortest time possible, may be exceeded. peak In either case, the magnitude of the velocity is calculated from the initial or current value to the final velocity v f To transition the trajectory from an initial (current) state or trajectory point to a desired peak velocity v, a "rate transition algorithm" is applied. The rate transition algorithm is described in more detail below. Essentially, the trajectory is transitioned from an initial (current) state or trajectory point to a desired peak velocity v peak The terminal velocity v of the elementary trajectory segment corresponding to E The first velocity transition algorithm is used to get to the trajectory point where the velocity of the initial trajectory (p0, v0, a0) and the terminal velocity (v E ), command acceleration (a max ), and a particular value of the jerk constraint for increasing or decreasing the magnitude of the orbital acceleration reference. The endpoint of the first velocity transition algorithm (p E , v E , a E ) to a starting point (p0, v0, a0), the second speed transition algorithm scales the speed reference magnitude to the desired peak speed vpeak from an initial velocity v0 corresponding to a desired final velocity v, which has a value of substantially zero f The new terminal velocity v E , and the terminal acceleration a, which corresponds to essentially zero E The velocity transition algorithm is described in more detail below. If the trajectory is moving away from the commanded position, the velocity transition algorithm is applied to the desired final velocity v at the commanded position. f As shown in the flow diagram shown in FIG. 23, the magnitude of the trajectory acceleration is reduced to substantially zero before applying it to the desired final velocity v at the command position. f is effectively reduced to zero before applying the inversion.
[0175] However, if the answer to all three questions is "no," then the situation must be such that the trajectory will undershoot the commanded position by ramping down to zero velocity now. In this situation, the control system proceeds to the next question to establish the next stage of trajectory generation.
[0176] D) Does the orbital velocity exceed the commanded velocity (312)? According to an embodiment of the present invention, the generation of the motion control profile depends on the load handling equipment being able to achieve the maximum possible speed during the acceleration phase on the grid structure without causing the load handling equipment to overshoot the commanded position. Conversely, a speed that is too low will cause the load handling equipment to undershoot the commanded position. However, the peak or maximum speed at which the load handling equipment can travel is very limited to that defined by one of the maximum speed limit constraints mentioned above or the commanded speed. A peak speed v closer to the commanded speed of the drive mechanism is preferred. peak The ability to reach the commanded position depends on the step length on the grid structure. If the step length is long, there is enough time for the velocity to reach the commanded velocity before entering the deceleration phase. If the step length is short, there is not enough time for the velocity to reach the commanded velocity without overshooting the commanded position before entering the deceleration phase.
[0177] The controller always aims to reach a commanded speed, defined by the constraints, to achieve the shortest possible time to complete the point-to-point move. Reaching the commanded speed may not be feasible if the travel length is too short. In situations where the loading equipment is supposed to reach the commanded speed, it will overshoot the commanded position. Therefore, the optimal peak speed v that the loading equipment can reach between the commanded speed and the initial speed v0, which allows the loading equipment to complete the trajectory in the shortest possible time, is determined. peak It is necessary to choose.
[0178] In this particular situation, the control system determines whether the trajectory velocity exceeds the commanded velocity. If the answer to question D) is yes (313) (see FIG. 24), the control system executes a velocity transition algorithm to bring the trajectory velocity reference down to the commanded velocity by generating one or more trajectory segments whose end points correspond to the commanded velocity, otherwise the control system proceeds to the next question. As will be further described below, the principle of the velocity transition algorithm is to generate a trajectory from a starting point whose velocity magnitude exceeds the final velocity v f and to generate one or more trajectory segments necessary to bring the trajectory to a trajectory point where the terminal acceleration is substantially zero.
[0179] The desired peak velocity v that the trajectory can reach without overshooting or undershooting the commanded position. peak A root-finding algorithm is used to find v. The velocity transition algorithm according to an embodiment of the present invention converts the velocity magnitude into a peak velocity v peak Generate the trajectory segments necessary to transition from the target velocity to the target velocity at which the target velocity is substantially zero, and then transition back to substantially zero. The objective function is the degree of overshoot or undershoot, and the root is the desired peak velocity where the objective function is substantially zero.
[0180] E) Is the magnitude of the current orbital velocity the same as the commanded velocity (314)? If the answer to question E) is no, then proceed to the next question F. If it is yes (315) (see FIG. 25), then the situation is that the magnitude of the current trajectory velocity is the same as the commanded velocity, and the control system will calculate one or more trajectory segments for the remainder of the constant velocity phase. In order to calculate one or more trajectory segments for the remainder of the constant velocity or constant velocity move phase, the distance that the trajectory has traveled as it begins to descend towards zero velocity needs to be calculated. This will usually be calculated as a result of answering one of the questions above, specifically question C above.
[0181] The duration of the trajectory segment or segments for the remainder of the constant velocity phase, and therefore the distance the trajectory can still cover during the constant velocity phase, is determined by subtracting the distance the trajectory has traveled as it begins to descend towards zero velocity from the current remaining distance. This can be explained as follows:
[0182] (1) The trajectory can remain at the commanded velocity for some time T, such that the trajectory position changes to Δp cruise Only one step forward will be taken.
[0183] (2) After time T, the command speed changes to zero speed (i.e., v f =0), the orbit must undergo a velocity transition, which further increases the orbital position by Δp decel Only one step forward will be taken.
[0184] (3) Therefore, remaining distance = Δp cruise +Δp decel .
[0185] If the trajectory is at the current position, the remaining distance is calculated from the command position minus the current position (p0).
[0186] Δp cruise , Δp decel, and the values of T are calculated by the control system as follows:
[0187] Δp decel : The distance the trajectory has traveled when the speed transition (2) is performed. This is performed by a speed transition algorithm that produces the usual output: trajectory segment + distance traveled. This distance is calculated by how much the trajectory has traveled during the deceleration phase (from commanded speed to zero speed), i.e., Δp decel is equivalent to
[0188] By (3), the control system is cruise = remaining distance – Δp decel can be calculated.
[0189] Finally, the duration T of the trajectory segment in the constant velocity phase is given by
[0190]
number
[0191] is determined from.
[0192] Where: Δp cruise is the distance traveled while at a constant commanded velocity.
[0193] v max is the command speed.
[0194] The duration of the orbital segment of the constant velocity phase is the distance Δp cruise The absolute value of the command speed v maxThe value of the manipulated variable jerk for the trajectory segment in the constant velocity phase is zero. Since the trajectory velocity is constant during the constant velocity phase segment, the control system asserts that the magnitude of the trajectory acceleration is zero at this point; otherwise the action performed by question (C) above ("velocity overshoot") would not be performed correctly, and this calculation can be used as a check on the calculation of question (C).
[0195] If questions (A) through (E) above are all false, we can conclude that the orbital velocity can be increased.
[0196] By a process of elimination, we can conclude that the situation for the trajectory is that if brought to a stationary position, the trajectory will not overshoot, the trajectory is not currently exceeding either the commanded velocity or the commanded acceleration, and the trajectory is not yet proceeding at the commanded velocity. Therefore, the situation must be such that the magnitude of the velocity can be increased. By structuring the questions in this order, the shortest possible time for point-to-point movement is achieved.
[0197] F) Can the magnitude of the orbital velocity be increased to the commanded velocity (316)? In this situation, by elimination, the trajectory velocity magnitude is in a state where it can be increased to the commanded velocity. Now, the control system applies a first velocity transition algorithm to bring the trajectory velocity magnitude to the commanded velocity, and immediately thereafter applies a second velocity transition algorithm to bring the trajectory to a stationary position, i.e., the velocity magnitude is substantially zero. If the trajectory position resulting from two successive velocity transitions does not overshoot the commanded position, then the answer to this question is yes (316) (see FIG. 26), in which case the trajectory segment generated as part of the first velocity transition calculation is adopted as the immediate solution.
[0198] However, if the trajectory velocity cannot be increased to the commanded velocity without overshooting the commanded position, the peak velocity vpeak The desired final velocity v f is required, in which case the orbital velocity can be increased such that a second velocity transition is then performed to bring the orbital velocity to rest at the commanded position. In accordance with the present invention, this is achieved by obtaining the optimal peak velocity v peak The objective function f(t) is thus the degree of overshoot or undershoot, also called the deviation from the commanded position. The root is found by applying a root-finding algorithm to the objective problem in search of the commanded velocity v max and the peak velocity v between the initial velocity v0 peak where the deviation from the commanded position is essentially zero, i.e., f(t)=0. This root-finding algorithm finds the final peak velocity v peak One or more initial guesses of , and a final peak velocity v is obtained by applying a rate transition algorithm. peak The method begins by determining the trajectory segments required to achieve a velocity transition from v to v. The deviation or overshoot from the commanded position is determined by applying the trajectory equations (3 through 5). This is done by applying the root-finding algorithm to each peak velocity v peak Each iteration of the root-finding algorithm produces a more and more accurate approximation to the root. This iteration is repeated until the series of numbers, in this case the chosen peak velocity, converges to the root, i.e., the root objective function is substantially equal to zero. The chosen peak velocity v peak The obvious candidates for -The magnitude of the initial orbital velocity, ·Command speed, A weighted combination of the above two It is.
[0199] Further details on the application of the root finding algorithm are provided in Section 5.0 below. The root finding algorithm can be any one of the following: Newton's root finder, secant root finder, bisection root finder, root finder by interpolation, root finder by inverse interpolation, Brent's root finder, Boudin-Fourier root finder, and Strum-chain root finder. The trajectory segment obtained by the velocity transition (from the current velocity to the top velocity of the solution) is retained.
[0200] In the final stage of trajectory or motion control profile generation, if the velocity magnitude does not need to increase to the commanded velocity or velocity constraint, a velocity transition algorithm 318 is applied to bring the trajectory to zero velocity at the commanded position.
[0201] 4.0 Speed Transition Algorithm The process of generating a finite jerk ("S-curve") trajectory relies heavily on solving what we refer to as a velocity transition algorithm. The velocity transition algorithm converts a trajectory from an initial trajectory point of position p0, velocity v0, and acceleration a0 to a desired final velocity v0 with a final acceleration magnitude that is substantially zero. f The focus is on determining the trajectory segments required to bring the vehicle to its final velocity v f can be any value desired. For example, when the loading equipment needs to be commanded to come to a stop, v f can be set to essentially zero and the velocity transition algorithm will execute the trajectory segments necessary to bring the trajectory from the current trajectory point to a stop. This is done by max ), commanded speed (v max ), and one or more jerk constraints J1, J2, J3, J4. For ease of explanation and understanding of the velocity transition algorithm, the one or more jerk constraints are inc When the magnitude of the acceleration is decreased, it is called J dec It is called.
[0202] The velocity transition algorithm is decomposed into sub-algorithms, a first velocity transition sub-algorithm and a second velocity transition sub-algorithm, both of which need to be executed to complete the velocity transition algorithm according to the invention. The first velocity transition sub-algorithm checks the validity or state of the trajectory and whether one or more trajectory segments are required for the correct application of the second velocity transition sub-algorithm. The second velocity transition sub-algorithm calculates the desired final velocity v f Regardless of the value of v f The algorithm that actually performs the (speed) transition from to. For the avoidance of doubt, the terms "speed" and "speed reference" are used interchangeably to refer to the same function. Similarly, the terms "acceleration" and "acceleration reference" are used interchangeably to refer to the same function.
[0203] When applying the second velocity transition sub-algorithm, it is necessary to: i) ensure that the orbit acceleration is not excessive; ii) ensure that the final velocity v f and iii) addressing a trajectory that accelerates away from the f It is essential to address overshoot of the trajectory velocity v. If any of these conditions are not met, the control system applies the first velocity transition sub-algorithm to generate one or more trajectory segments to address these anomalies or situations before proceeding to the second velocity transition sub-algorithm. In other words, to address the general case of the velocity transition algorithm, the control system must first check these three checks: excessive acceleration, acceleration in the wrong direction, and overshoot of the trajectory velocity v f A check for overshoot of must be performed. By providing these corrective steps, or by generating trajectory segments to address these sub-problems as they arise in the trajectory, the problem is simplified to a simple problem that the second velocity transition sub-algorithm can solve.
[0204] FIG. 28 is a flow diagram detailing the application of the second rate transition sub-algorithm and the checks that are necessary for the second rate transition sub-algorithm to be implemented.
[0205] The output variables in generating one or more trajectory segments are the jerk value, J, and the time duration, T, of the trajectory segment. To explain the velocity transition algorithm, we consider the desired final velocity, v f and the difference between the velocity v0 at a given time Δv=v f Delta V Δv, defined as -v0, is used in assessing the status of the trajectory. The value of the velocity or delta V Δv can vary depending on where in the trajectory the velocity transition algorithm of the present invention is applied. This is because the velocity reference v(t) depends on where in the trajectory the velocity reference is taken, i.e., the value of the velocity trajectory at the trajectory point. Preferably, delta V is based on the difference between the desired final velocity reference and either the initial or terminal point velocity reference of one or more trajectory segments.
[0206] The process begins by initializing the output list of trajectory segments and determining if conditions are right to start the velocity transition algorithm.
[0207] 4.1 Application of the first speed transition sub-algorithm However, the second rate transition sub-algorithm cannot be initiated unless certain conditions are met or when there is an anomaly or condition during the orbit that needs to be addressed. The conditions for valid initiation of the second rate transition sub-algorithm are one of the following:
[0208] a) The trajectory starts with zero trajectory acceleration a0 and already reaches the desired final velocity v f Mathematically, this is given by the following condition:
[0209]
number
[0210] b) The trajectory still has the desired final velocity v f Instead, the orbit is v f but the trajectory acceleration does not exceed the commanded acceleration constraint or the initial acceleration is zero. Mathematically, this is given by the condition
[0211]
number
[0212] c) The trajectory still has the desired final velocity v f Instead, the initial acceleration a0 is zero. Mathematically, this is given by the condition
[0213]
number
[0214] These conditions are shown in FIG. 28 as decision blocks 504, 506, 508, and 510. In contrast to satisfying the conditions of the second velocity transition sub-algorithm, the second velocity transition sub-algorithm cannot be executed unless certain anomalies or conditions in the trajectory, such as excessive trajectory acceleration, need to be addressed. These sub-problems are addressed by the first velocity transition sub-algorithm. If these anomalies or conditions are not addressed, the second velocity transition sub-algorithm will not transition the velocity to the desired final velocity v f These are called invalid entry states, and are shown in the flow diagram of Figure 28 in a box labeled "Invalid Entry States" 512, and include the following:
[0215] a) The trajectory is already at the desired final velocity v f However, the initial acceleration a0 is not zero. In other words, the trajectory moves with the desired final velocity v fTo correct this anomaly, three or four orbits are required: one to bring the magnitude of the orbital acceleration to zero, after which the velocity transition sub-algorithm adjusts the magnitude of the orbital velocity to the desired final velocity v f Mathematically, the condition for this invalid start state is given by:
[0216]
number
[0217] b) The trajectory has a desired final velocity v f In this case, Δv and a0 have opposite signs (i.e., Δv is positive and a0 is negative, or Δv is negative and a0 is positive), and three or four orbital segments are required. One is to bring the magnitude of the orbital acceleration to zero, after which the velocity transition sub-algorithm changes the magnitude of the orbital velocity to the desired final velocity v f Two or three more trajectory segments can be generated to return to the original point. Mathematically, this null condition is given by:
[0218]
number
[0219] c) The initial acceleration a0 is the maximum constraint or command acceleration a max The trajectory acceleration exceeds the command acceleration a max One or more orbital segments are needed to bring it to . Mathematically, this invalidity condition is given by:
[0220]
number
[0221] Before completing the rate transition algorithm, anomalies in the trajectory that do not satisfy these conditions or are essential for the application of the second rate transition sub-algorithm are first addressed in order to make the trajectory in a state in which the second rate transition sub-algorithm can be applied. These conditions are called sub-problems that are addressed by the first rate transition sub-algorithm and need to be addressed in a specific order as noted in the flow chart 400 shown in Figure 27. Addressing these anomalies generates a sequence of one to five trajectory segments that are the solution of the rate transition algorithm.
[0222] In addressing the sub-problems and generating one or more trajectory segments, in some instances, J inc and J. dec In addition to the preemptive jerk value J red is introduced at the input 402.
[0223] [Table 1]
[0224] Table 1: Inputs and outputs in the application of the first speed transition sub-algorithm J red is a preemptive jerk J red J dec in that it is required to reduce the magnitude of the acceleration to essentially zero. dec In other words, J red J dec This reduces the magnitude of acceleration at a rate different from that of the final velocity v, and is required when dealing with the two initial checks mentioned below (excessive acceleration, and acceleration away from the final velocity). Thus, if there is excessive acceleration, or the trajectory is moving away from the desired final velocity v f When accelerating away from the target, the control system uses a preemptive jerk J to reduce the acceleration to essentially zero. red The above explanation applies to J red and J. dec This is true for examples where J has different values.red and J. dec The values of may be the same.
[0225] Flowchart 400 shown in FIG. 27, which will be described in detail below, details the solutions of the sub-problems of the first velocity transition sub-algorithm and the order in which they are addressed before the second velocity transition sub-algorithm according to the present invention is applied to generate a trajectory. These sub-problems are decomposed into one or more blocks, each of which represents a solution of a sub-problem addressed by the first velocity transition sub-algorithm. Each solution of a sub-problem generates one or more trajectory segments that are entered into an output list. Initially, the output list of trajectory segments is empty. For the purposes of the description of the velocity transition algorithm, the term "integrate" in this context means "apply trajectory equations 3, 4, and 5 (using the starting point, trajectory segment jerk, and segment duration) to arrive at the end point of the segment."
[0226] A) Dealing with excessive acceleration In the decision process, before applying the second velocity transition sub-algorithm of the present invention, the control system determines whether the magnitude of the initial acceleration |a0| is greater than the constraint command acceleration a max This is represented in FIG. 27 by decision block 404. If the answer is yes, the control system adds a trajectory segment 406 to the output list. This new trajectory segment is added to the output list by increasing the trajectory acceleration by a before attempting to solve the second velocity transition sub-algorithm. max to, in which case a trajectory segment must be generated, for which The manipulated variable jerk used is J=-sign(a0)×J red The negative sign is necessary to reduce the acceleration from the initial acceleration a0.
[0227] -Trajectory segment duration is T1=|a max -|a0|| / J red is given by:
[0228] By applying the manipulated variable jerk J, the terminal point trajectory acceleration a E1 is a E1 =sign(a0)×a max is given by:
[0229] Starting from an initial position p0 and initial velocity v0, the final point velocity v E1 and the end point position p E1 is obtained by direct application of trajectory equations 3, 4, and 5 with the starting point (a0, v0, p0), jerk J, and trajectory segment duration T1. This means that in the flow diagram, the resulting velocity v E1 and acceleration a E1 The trajectory from the starting point (v0, a0) is integrated using all the segments on the output list (408) to calculate
[0230] If a trajectory segment is generated in this step, its end point becomes the initial trajectory point of the next subproblem, i.e. (a0, v0, p0) ← (a E1 , v E1 , p E1 ) and delta V is Δv←v f -v E1 (412). In other words, the end point of the generated trajectory segment is assigned to the start point of the subsequent trajectory segment. This is represented in FIG. 27 by block 410.
[0231] However, excessive acceleration is a max The step of generating a new trajectory segment to lower the vehicle to the initial acceleration a0 is max If it is less than or equal, then no and the control system proceeds to the next subproblem.
[0232] The result of decision block 404 (i.e., the magnitude of acceleration a0 is a maxWhatever the acceleration (greater or not), the algorithm proceeds to steps 408 and 410. Step 408 integrates the entire trajectory from the starting point, including all trajectory segments in the output list. If the acceleration is excessive and a new trajectory segment has been generated in step 406, this segment is also integrated. If the acceleration is not excessive and no new trajectory segment has been generated, the output list of trajectory segments is empty and the integration step of 408 still proceeds but does not have any effect, so the resulting velocity v E1 and acceleration a E1 is equal to the initial velocity v0 and initial acceleration a0.
[0233] As with step 408, the subsequent steps of merging the trajectories at steps 420 and 436 are applied regardless of the outcome of the previous decision block. The merging steps are applied to the entire trajectory from the starting point, regardless of whether new trajectory segments have been added to the output list in previous steps.
[0234] B) the desired final velocity v f Dealing with an accelerating orbit away from This is represented in Figure 27 by decision blocks 414 and 416. The control system determines whether the magnitude of the velocity is v f However, v f An anomaly accelerating away from 414 and / or with a velocity of v f Not v f In the former anomaly 414, the control system applies a decision process to address the anomaly (condition) 416 in which the trajectory is accelerating away from the desired final velocity v. In the former anomaly 414, the control system determines if the delta V Δv is equal to zero, i.e., Δv=0, and the magnitude of the initial acceleration a0 is not equal to zero, i.e., a0≠0. If both of these conditions exist, this indicates that the trajectory is not accelerating away from the desired final velocity v f If neither of these conditions exist, the control system proceeds to decision block 416 and determines whether the trajectory is accelerating away from v fTo determine if this is the case, the control system determines (416) whether the product of the magnitude of the initial acceleration a0 and the delta V Δv is negative (Δv×a0<0). If this product is negative, this indicates that the trajectory is accelerating away from the desired final velocity v f The product Δv × a0 is negative if the sign of the acceleration a0 is opposite to the sign of the delta V Δv, i.e., the trajectory is accelerating in the opposite direction to that needed to reach the terminal velocity.
[0235] At either of decision blocks 414 and 416, the trajectory is adjusted to a desired final velocity v f If both conditions are positive, meaning that the vehicle is accelerating away from , then a trajectory segment 418 must be generated to correct this anomaly by reducing the trajectory acceleration to zero before the second velocity transition sub-algorithm can be applied. For this new trajectory segment 418, The manipulated variable jerk used is J=-sign(a0)×J red The negative sign is necessary to reduce the acceleration to essentially zero.
[0236] -Trajectory segment duration is
[0237]
number
[0238] is given by:
[0239] The magnitude of the end point acceleration is zero (a E2 =0). Starting from initial position a0 and initial velocity v0, the final point velocity v E2 and the end point position p E2is obtained by direct application of the trajectory equations (Equations 3, 4, and 5) with the starting point (a0, v0, p0), the jerk J, and the trajectory segment duration T2. This is expressed in the flow diagram as the terminal point velocity v E2 and terminal acceleration a E2 The trajectory from the starting point (v0, a0) is integrated (420) using all the segments on the output list to calculate
[0240] If such a trajectory segment is generated in this step, its end point becomes the initial trajectory point of the next subproblem, i.e. (a0, v0, p0) ← (a E2 , v E2 , p E2 ) and delta V is Δv←v f -v E2 (420, 422).
[0241] However, if neither of the conditions at decision blocks 414 and 416 are positive, then the trajectory is v f and the acceleration is essentially zero or the trajectory is v f , and the control system maintains the status of the initial trajectory point (a0, v0, p0) and proceeds to the next subproblem.
[0242] C) Dealing with overshoot of the final velocity of the orbit This third check is represented in Figure 27 by decision block 424. The first check in step A) ensured that the trajectory acceleration was not excessive, and the second check in step B) ensured that the trajectory was not accelerating in the "wrong" direction. So, by this point, either the trajectory acceleration a will be zero (in which case the third check does nothing, since the trajectory is not actually accelerating at that point), or the trajectory is approaching the desired final velocity v f In this scenario, before applying the second velocity transition sub-algorithm, the control system performs a final check to ensure that the trajectory is accelerating toward (and possibly overshooting) the desired final velocity vf This decision process begins in decision block 424 by checking whether the initial trajectory acceleration a0 is substantially equal to zero. If the magnitude of the initial trajectory acceleration a0 is substantially equal to zero, this is an indication that the conditions are valid for applying the second velocity transition sub-algorithm. However, if the trajectory acceleration is not substantially equal to zero, the orbital velocity may still be on an upward trajectory and the desired final velocity v f This check helps to catch this scenario when the third check is applied, and in doing so, checks whether the orbital velocity is greater than the desired final velocity v f We apply a trajectory segment that reduces the magnitude of acceleration to zero only if the trajectory intersects and continues moving away from
[0243] The orbital velocity is the desired final velocity v f If the speed transition crosses and exceeds the threshold, the following corrective actions are taken before applying the second speed transition sub-algorithm: First, the jerk applied is adjusted to the J used in the previous two checks. red Instead, J=J dec A tentative trajectory segment is calculated, but this tentative trajectory segment is not added to the output list. dec Then, in the second sub-algorithm, the velocity is adjusted to the desired final velocity v f The same jerk value J is applied when transitioning to the dec The reason we continue to use f This makes sense since the orbital velocity is v f Just because the trajectory may overshoot v (if the third check determines that it does), f v without overshooting f This does not prevent the use of the same jerk that would be used if the preemptive jerk J = J redis used in special situations where trajectory acceleration is excessive and / or the trajectory is accelerating in the wrong direction and needs to be redirected.
[0244] In step 426, the resultant velocity v that the load handling device would achieve at the end of the provisional trajectory segment is calculated. aux The trajectory is integrated using the provisional trajectory segments to calculate the velocity v. This velocity is calculated in the next step when the trajectory is accelerated to the final velocity v f This is needed to calculate whether the
[0245] At decision block 432, the orbital velocity is increased to the final velocity v f When determining whether a -The initial velocity v0 in the previous check to the desired final velocity v f A delta V Δv 428 is calculated that represents the velocity transition from initial velocity v0 to the desired final velocity v f This indicates the degree of overshoot from the
[0246] Delta V Δv aux 430 is the terminal velocity v of the provisional trajectory segment calculated in step 426 aux to the desired final velocity v f Therefore, the velocity transition to Δv aux is the currently calculated v aux The desired final velocity of v f This indicates the degree of overshoot from the
[0247] If the orbit segment overshoots at the final check, the product Δv × Δv aux is less than zero, i.e., negative (Δv × Δv aux <0), i.e. Delta V Δv or Δv aux becomes negative (see decision block 432 in FIG. 27), and (Δv × Δv aux <0), this means that the trajectory segment is at the desired final velocity v fIn this case, in step 434, the control system adds to the output list the tentative trajectory segment previously calculated to bring the acceleration magnitude to substantially zero. For this new trajectory segment (which is no longer tentative at this stage), The manipulated variable jerk value used is the preemptive jerk J red Not J dec Therefore, the applied jerk is J=-sign(a0). The negative sign indicates the jerk required to reduce the acceleration to essentially zero, i.e., deceleration.
[0248] - Orbital segment duration is T3=|a0| / J dec is given by:
[0249] The end point acceleration is zero (a E3 =0). Starting from initial position a0 and initial velocity v0, the final point velocity v E3 and the end point position p E3 is obtained by direct application of the trajectory equations (Equations 3, 4, and 5) with the starting point (a0, v0, p0), the jerk J, and the trajectory segment duration T3. This is shown in the flow diagram as the resulting velocity v E3 and acceleration a E3 The trajectory from the starting point (v0, a0) is integrated (436) using all segments on the output list to calculate the terminal velocity v E3 v aux is equal to.
[0250] Thus, the third check is whether the acceleration reference a is reduced to essentially zero and the desired final velocity v f This ensures that the trajectory segment is prevented from accelerating further away from the desired final velocity v f If the trajectory overshoots the desired final velocity v, this condition is accepted when the acceleration reference a is reduced to zero. fThe goal is to prevent accelerating away from the acceleration reference, thereby avoiding the need to reduce the acceleration reference to essentially zero. Again, the endpoint is the initial trajectory point for the velocity transition algorithm, i.e. (a0, v0, p0) ← (a E3 , v E3 , p E3 ) (438), and the speed or delta V is Δv = v f -v E3 Then the updated trajectory points become the initial trajectory points for the next subproblem.
[0251] This trajectory segment has the desired final velocity v f If the trajectory segment does not overshoot, then this trajectory segment is completely ignored, i.e., no corrections are required to the initial trajectory points or the delta V Δv.
[0252] Once the trajectory conditions are set (e.g., the acceleration magnitude is brought substantially to zero), which may include generating one or more trajectory segments in the first velocity transition sub-algorithm, the control system proceeds to the next stage of the velocity transition algorithm, i.e., the second velocity transition sub-algorithm (440), to transition the velocity magnitude to the desired final velocity. Finally, the total distance traveled by applying the first velocity transition sub-algorithm, Δp, may be determined by integrating all the generated trajectory segments (442). The application of the second velocity transition sub-algorithm is further described in Section 4.2 below.
[0253] Example 1 Consider the following example input value:
[0254] Initial orbital acceleration a0=1.0 Command acceleration a max =2.0 Initial speed v0=1.0 Desired final velocity v f =1.1
[0255]
number
[0256] Jerk value J red =J inc =J dec =0.1 A) First question: Is the acceleration excessive? No. |a0|=1.0 and |a max Since |=2.0, the starting acceleration a0 is not excessive.
[0257] B) Second question: The orbit is v f Is it accelerating away from v? No. Δv=0.1 is positive, and a0 is also positive, so the trajectory is v f is accelerating towards
[0258] C) Third question: If the acceleration starts immediately and is reduced to zero, the trajectory will remain constant v f Will this result in overshooting? Yes.
[0259] Jerk value J of 0.1 dec Using this, it will take t=|a0| / J=1.0 / 0.1=10 time units to ramp a0 down to zero. In the time it takes to ramp down to zero, the orbital velocity will increase by 10*1.0 / 2=5.0 (area of a right triangle with base=10 time units and height a0=1.0). Thus, even if the control system were to ramp down the orbital acceleration to zero, the orbital velocity would still change from v0=1.0 to 6.0, and therefore v f =1.1 will be overshot.
[0260] In this example, the first two checks do not result in any action (no action is required), but the third check is f must certainly be present to deal with the overshoot of , otherwise the second speed transition sub-algorithm cannot be invoked (invalid start condition).
[0261] Example 2 Command acceleration a max=2.0 Initial acceleration a0=-3.0 Initial speed v0=1.0 Desired final velocity v f =3.0
[0262]
number
[0263] A) First question: Is the acceleration excessive? Yes. |a0|=3.0 is a max Since the control system is larger than the duration T = |a max -|a0|| / J red =1.0 / 1.0=1.0, J=+J red = +1.0.
[0264] →The end point of this segment is a E (Terminal point acceleration)=-2.0, v E (Terminal velocity)=0.5.
[0265] →The acceleration is still negative because in this step, we are overshooting the max Note that this is because all that is required is to reduce the magnitude of the acceleration so that it is no longer greater than 1.
[0266] →Δv=v f -v E Recalculate = 3.0 - (-0.5) = 2.5.
[0267] →Reassign a0=-2.0 and v0=0.5.
[0268] B) Second question: The orbit is v f Is it accelerating away from v? Yes. Δv=2.5 (a positive value), and a0=-2.0 (a negative value), so the trajectory is v f Therefore, the duration is T=|a0| / J red = 2.0 / 1.0 = 2.0, J = +Jred = +1.0.
[0269] →The end point of this segment has an acceleration of a E =0, v E =-1.5.
[0270] →Δv=v f -v E Recalculate =3.0-(-1.5)=4.5.
[0271] →Reassign a0=0 and v0=-1.5.
[0272] C) Third question: If the acceleration starts immediately and is reduced to zero, the trajectory will remain constant v f Will we overshoot the acceleration? No, because the acceleration is currently already zero.
[0273] In example 2, the first two checks produced an action (one trajectory segment was generated each), but the third check did not produce any action (no action was required). From the previous three queries, the trajectory now has a0=0, v0=-1.5, and v f = 3.0, i.e., the acceleration has been reduced to essentially zero. Application of the second velocity transition sub-algorithm reduces the orbital velocity to v f (and zero acceleration). peak A max (Because it turned out to be 1.5, which is smaller than the peak The calculation of is essential for the application of the second velocity transition sub-algorithm.
[0274] 4.2 Application of the second speed transition sub-algorithm The inputs and outputs of the second speed transition sub-algorithm are shown below in Table 2. Once the previous sub-problems described above have been addressed by the first speed transition sub-algorithm, the control system can invoke the second speed transition sub-algorithm. The second speed transition sub-algorithm is f Regardless of the value of v f , which is the algorithm that actually performs the (velocity) transition to the selected velocity v. The second velocity transition sub-algorithm is only invoked when certain specific start conditions mentioned above are addressed. These include, but are not limited to, that the trajectory has excessive acceleration and / or that the trajectory is accelerating away from the desired final velocity. These checks are performed in blocks 504, 506, 508, and 510 in FIG. 28 and are explained above. The purpose of the velocity transition algorithm is to determine whether the selected velocity v f The goal is to generate a trajectory segment that reaches a final velocity v f can be any selected velocity subject to velocity constraints, except that if the velocity or speed delta Δv is substantially zero, the magnitude of the orbital velocity v0 is already greater than the desired final velocity v f and the magnitude of the acceleration a0 is substantially zero, in which case the trajectory will have the desired final velocity v f 28. The vehicle is not accelerating away from the desired final speed and conditions are ideal to already be at the desired final speed. This is illustrated in the second speed transition sub-algorithm by the path from decision block 506 to the end of the flow diagram (FIG. 28).
[0275] In accordance with the present invention, a useful concept for the second velocity transition sub-algorithm is to calculate the desired final velocity v f The concept of delta V Δv is defined as the difference between the initial speed v0 and the terminal speed reference v1 assigned to the initial speed v0 in the first speed transition sub-algorithm described above. E With reference to the flow chart 500 shown in FIG. peakis calculated (514), which has an area given by Δv and a rate of increase in acceleration (away from zero acceleration) J inc and the rate of decrease in acceleration (towards zero acceleration) J dec The acceleration value is expressed by the vertices of an imaginary triangle having sides defined by a and b (see FIG. 29). In other words, a peak is the rate of increase in the magnitude of acceleration J such that the area of the shape corresponds to or is equivalent to Δv inc and the rate of decrease in the magnitude of acceleration J dec To reach window 514, the conditions of decision windows 504, 508, and 510 must be met, i.e., valid entry conditions: (i) Δv≠0, (ii) Δv×a0≧0, and |A0|≦A max These conditions should be met since the trajectory has already gone through the first velocity transition sub-algorithm, and any excess acceleration and / or trajectory accelerating away from the terminal velocity should have been addressed by adding trajectory segments to the output list.
[0276] [Table 2]
[0277] In particular, the initial acceleration a0, the initial velocity v0 (defined by the output of the first velocity transition sub-algorithm), and the desired final velocity v f And delta V Δv=v f -v0 and increasing the magnitude of orbital acceleration (J inc ) and decrease (J dec ), we can draw a hypothetical "triangular acceleration trajectory" (see Figure 29) whose area to the right of a0 is equal to Δv. In other words, when we perform such a triangular change in the acceleration signal, we can change v0 to v (regardless of the number of trajectory segments). fFrom first principles, if we equate the area under the virtual triangular acceleration trajectory with the delta velocity Δv, then
[0278]
number
[0279] where A0, A1, and A2 are the areas shown in FIG. peak Let be the value of the acceleration at the vertex of this imaginary triangle. The part of the total area under this triangle that corresponds to Δv can be expressed as the sum of the three smaller areas, i.e., Δv = A0 + A1 + A, where:
[0280]
number
[0281] It is.
[0282]
number
[0283] When we solve for
[0284]
number
[0285] It becomes.
[0286] Therefore, the magnitude of the peak acceleration
[0287]
number
[0288] can be obtained by taking the square root of
[0289]
number
[0290] Peak acceleration a peak Once the magnitude of is calculated from Equation 21, one of three mutually exclusive possibilities, each of which is detailed below, can be true.
[0291] A) The magnitude of the initial acceleration a0 is the peak acceleration a peak corresponds to the size of
[0292] The magnitude of the initial acceleration is equal to the peak acceleration (516), i.e., |a0|=|a peak If |, then the trajectory is already at its peak acceleration, so the desired final velocity v f To reach this, we simply need to reduce the acceleration to zero (slow down).
[0293] In this case, one trajectory segment is generated (518), and for this trajectory segment: i) To decrease the acceleration, the value of the manipulated variable (jerk) is J = -sign(a0) × J dec and ii) The trajectory segment duration is
[0294]
number
[0295] is given by iii) The terminal acceleration is zero (a E4 =0), and the terminal velocity is v E4 =v f and the end point position p E4 is the starting point (a0, v0, p0), the jerk J dec , and the segment duration T, are obtained by direct application of the trajectory equations (Equations 3, 4, and 5).
[0296] B) Peak acceleration magnitude a peak But the maximum acceleration a max The magnitude of the acceleration command is less than or equal to the acceleration command.
[0297] The magnitude of the peak acceleration is less than the magnitude of the maximum acceleration (526), i.e., |a peak | max If , the magnitude of the orbital acceleration is still |a peak | and then decrease it to the desired final velocity v f We need to wrap it around to zero where
[0298] Therefore, in this case, two trajectory segments 528, 530 need to be generated, which can be described by an imaginary triangle in FIG. 29 showing a first trajectory segment 528b up to the peak acceleration and a second trajectory segment 530b down to substantially zero.
[0299] For the first trajectory segment 528b: The value of the manipulated variable (jerk) used is J=S×J inc The sign S depends on whether the initial acceleration a0=0, positive, or negative. As shown in the flow chart, if the initial acceleration a0 is substantially equal to zero, the sign S indicates that the trajectory segment will accelerate to a final velocity v f The sign of the orbital acceleration is the same as the initial velocity v0 because the orbital acceleration must be increased or decreased depending on the direction of the orbital velocity to reach v0. However, if the initial acceleration a0 is not equal to zero, the sign S is used to indicate that the magnitude of the velocity is greater than the final velocity v0. f To increase or decrease the orbital acceleration to reach
[0300] The duration of the first orbital segment is T=|a peak -a0| / J inc It is.
[0301] The end point acceleration is the peak acceleration (a E5 =a peak ), end point velocity v E5 and the end point position p E5 is the starting point a0, v0, p0, and the jerk J=J inc , and the segment duration T, are obtained by direct application of the trajectory equations (Equations 3, 4, and 5).
[0302] Reassign the end point to the initial trajectory point for the generation of the second trajectory segment, i.e. (a0, v0, p0) ← (a E5 , v E5 , p E5 )
[0303] For the second trajectory segment 530b: The value of the manipulated variable (jerk) is J=S×J dec It is.
[0304] The duration of the second orbital segment is T=|a peak | / J dec It is.
[0305] The end point acceleration is essentially zero (a E6 =0), end point velocity v E6 and the end point position p E6 is the initial orbit point a E5 , v E5 , p E5 , Jerk J=J dec , and the segment duration T, are obtained by direct application of the trajectory equations (Equations 3, 4, and 5).
[0306] C) The magnitude of the peak acceleration is the maximum acceleration a max is larger than the size of
[0307] |a peak | max When does not hold, |a peak | is a max The magnitude of the peak acceleration is greater than the magnitude of the maximum acceleration, i.e., a peak >a max If , the magnitude of the orbital acceleration is a max must be increased to a full value, and then over a period of time max and ultimately to the desired final velocity v f In other words, the acceleration orbit looks like a trapezoid. Therefore, in this case, the magnitude of the orbital acceleration is already a max or the magnitude of the orbital acceleration is still equal to a max Depending on whether the distance is less than 100 mm, two or three trajectory segments may be generated.
[0308] a max The value of the change in velocity due to the decrease in acceleration from
[0309]
number
[0310] To obtain a in Equation 21, peak =a0=a max It is calculated by substituting
[0311] This is shown in Figure 30. const The length (in time) of the constant acceleration phase, denoted as , can also be calculated as follows:
[0312]
number
[0313] where |Δv| is the ratio between the initial velocity v0 at the start of the constant acceleration phase and the desired final velocity v f The total area A2+A3 under the trajectory segments 540 and 542 of the constant acceleration phase and the acceleration drop shown in FIG. 30 is the magnitude of the velocity transition from f(where v0 is the velocity at the start of the constant acceleration phase). The area A3 under the acceleration drop segment 542 is calculated by Δv dec The area A2 can be calculated by subtracting the area A3 from the total area, so A2 = Δv - Δv dec As in equation 23, from the area A2, this area is multiplied by the acceleration a max By dividing by the duration T const can be calculated.
[0314] |a0| max If so, then three trajectory segments must be generated, as indicated in Figure 28 by decision block 532 and the additional trajectory segment 534 that is generated in addition to trajectory segments 540 and 542 in Figure 28, and illustrated in Figure 30 by the virtual trapezoid.
[0315] For the first trajectory segment 534 of FIG. 30 for duration T1: The value of the manipulated variable (jerk) used is J=S×J inc It is.
[0316] The duration of the orbital segment is
[0317]
number
[0318] is given by:
[0319] The terminal acceleration is the maximum acceleration (a E7 =sign(a0)×|a max |), end point velocity v E7 and the end point position p E7 are the starting points a0, v0, p0, and the jerk J inc , and the segment duration T1 is obtained by directly applying the trajectory equations (3, 4, and 5).
[0320] Reassign the end point to the initial trajectory point for the generation of the second trajectory segment, i.e. (a0, v0, p0) ← (a E7 , v E7 , p E7 )
[0321] T in Figure 30 const For a second trajectory segment 540 over a duration T2 denoted as The value of the manipulated variable (jerk) used is zero.
[0322] The duration of the segment is T2 = T const is given by:
[0323] The terminal acceleration is the maximum acceleration (a E8 =a0) (a0 = a from the previous calculation) E7 ), and the terminal velocity v E8 and the end point position p E8 is the starting point a E7 , v E7 , p E7 , zero jerk, and segment duration T2=T const is obtained by directly applying the orbit equation using
[0324] Reassign the end point to the initial trajectory point for the generation of the second trajectory segment, i.e. (a0, v0, p0) ← (a E8 , v E8 , p E8 )
[0325] For the third trajectory segment 542 of FIG. 30 for duration T3: The value of the manipulated variable (jerk) used is J=-S×J dec It is.
[0326] The duration of the orbital segment is
[0327]
number
[0328] is given by:
[0329] The end point acceleration is zero (a E9 =0), and the terminal velocity is v E9 =v f and the end point position p E9 is the starting point a E8 , v E8 , p E8 , Jerk J dec , and the segment duration T3 are obtained by direct application of the trajectory equations (Equations 3, 4, and 5).
[0330] However, from the first speed transition sub-algorithm, a0 = a max If so, then there are two trajectory segments 540 and 542, i.e., time T const A ma The first trajectory segment and acceleration at a max Only the second trajectory segment needs to be generated, which goes from |a0| down to zero. This is because in this trajectory, the flow chart shows decision block 532 (|a0| max ), the two trajectory segments that are added are represented by blocks 540 and 542 in FIG. 28 and FIG. 30. This means that |a0| max If the answer is "no", then one of the three checks mentioned above when applying the velocity transition algorithm is that the magnitude of the initial acceleration is a max Greater than, i.e. |a0|>a max That is, whether the acceleration is excessive or not. max To get to |a0|=a max would obviously be chosen (see block 406 in FIG. 27).
[0331] |a0|=a max So, the maximum acceleration a max The increase up to is ignored in Figure 30, and the magnitude of the orbital acceleration is already at the maximum acceleration a max It is.
[0332] Duration T const For the first orbit segment 540 spanning The value of the manipulated variable (jerk) used is zero.
[0333] The duration of the segment is T2=T const is given by:
[0334] The end point acceleration is the peak acceleration (a E8 =a0), end point velocity v E8 and the end point position p E8 is obtained by direct application of the trajectory equation with starting points a0, v0, p0, zero jerk, and segment duration T2.
[0335] For a second trajectory segment 542 of duration T3: The value of the manipulated variable (jerk) used is j=-S×j dec It is.
[0336] The duration of the orbital segment is
[0337]
number
[0338] is given by:
[0339] The end point acceleration is zero (a E9 = a0), and the terminal velocity is v E9 =v f and at position p E9 is the starting point a E8 , v E8 , p E8 , jerk j dec , and the segment duration T3 are obtained by direct application of the trajectory equations (Equations 3, 4, and 5).
[0340] The second velocity transition sub-algorithm according to the present invention calculates the desired final velocity v f The end occurs when the desired final velocity v f can have any value, such as being substantially zero when commanded to move to stop at the command position.
[0341] example Figure 31(a to d) shows the results of various final velocities v ranging from -34.052 to 18.167. f 31(a) shows various trajectories or motion control profiles that are generated when applying the velocity transition algorithm according to the present invention when transitioning to . All generated profiles share the same starting point (p0, v0, a0). In FIG. 31(b), different final velocities v f It can be clearly seen that the transition to results in a different travel length and therefore a different position reference, which results in a different deviation from the commanded position. The number of trajectory segments in the generation of the trajectory is determined by the final velocity v f Depending on the value of , up to four trajectory segments may be generated (see Figure 31d). The trajectory or motion control profile is used by the controller to generate or calculate the feedforward torque demands mentioned in section 2.0 above.
[0342] 5.0 Root-Finding Algorithms When generating a trajectory or motion control profile, it is essential that the trajectory velocity is controlled so that the trajectory reaches the exact commanded position in the shortest possible time without undershooting or overshooting. For example, if the trajectory distance is relatively short and the load handling equipment moves at the commanded velocity v max In a situation where there is little time to reach the commanded position, the trajectory will need to undergo several velocity transitions throughout the trajectory from the initial starting position to reach the commanded position accurately without undershooting or overshooting. This means that the trajectory will reach a peak velocity v peak and then proceed to the desired final velocity v f This includes going down to
[0343] The commanded speed v is set so that the loading device can complete the trajectory in the shortest possible time. max Towards the command speed v max The desired peak speed of travel to be no greater than v peak In accordance with an embodiment of the present invention, finding the initial velocity v0 and the maximum velocity constraint v max A root-finding algorithm is utilized to predict the "sweet spot" velocity (root) between the commanded position, where the objective function represents the deviation of the trajectory from the commanded position, i.e., the trajectory point where the terminal trajectory acceleration and terminal velocity are substantially zero. The use of the root-finding algorithm is not limited to when the trajectory is only at the beginning of the motion control profile. Depending on one or more conditions of the trajectory, such as one or more constraints or the commanded position, the motion profile generator can regenerate the motion control profile in response to the one or more conditions differing from any one of the initial one or more conditions. This regeneration of the motion control profile can be commanded during the acceleration and / or deceleration phases of the motion control profile. In this case, the root-finding algorithm can be applied at the motion control profile regeneration stage in predicting the "sweet spot" velocity for the remaining distance to be traveled to the commanded position.
[0344] The use of the root-finding algorithm is not limited to only the orbital velocity, but the root-finding algorithm can be applied to various parameters of the orbit, for example, the root-finding algorithm can be applied to find the ideal orbital acceleration or deceleration to reach the commanded position in the shortest possible time and / or without overshooting.
[0345] If the parameter is orbital velocity, the root-finding algorithm calculates the various peak velocities v from which orbital segments are generated to reach the various orbital velocities. peakをThe root finder algorithm will use the results of the trajectory segments produced by the velocity transition algorithm to determine the degree of overshoot or undershoot from the commanded position. So, for example, in the "peak velocity search" problem, this means that the root finder algorithm will find the "desired peak velocity" v peak We tried various values of and also applied a speed transition algorithm to find v where the overshoot is essentially zero. peak In accordance with the present invention, the root-finding algorithm searches for the value of the desired final velocity v f is the command speed v max The speed transition algorithm will be invoked when the speed is within the limits and can be increased to the commanded speed.
[0346] Each time the rate transition algorithm is called by the root finder, the rate magnitude is increased to the desired peak rate v peak and again to the desired final velocity v f One or more trajectory segments are generated to return to the target point.
[0347] From the initial position p0 to the final velocity v f and the desired peak velocity v to travel to a commanded position where acceleration is essentially zero. peak Take the example shown in Figure 32, where v is required. The top profile (Figure 32a) represents the magnitude of the position reference as a function of time, the middle profile (Figure 32b) represents the magnitude of the velocity reference as a function of time, and the bottom profile (Figure 32c) represents the magnitude of the acceleration reference as a function of time. Figure 32 shows that a relatively low peak velocity v A from a relatively large peak velocity v C , v ... max The ideal peak velocity v between the upper and lower bounds v0peak (see FIG. 32b). In the present description, the term "deviation" from the commanded position is broadly interpreted to cover the trajectory undershooting or overshooting the commanded position. In the example shown in FIG. 32, each candidate peak velocity v peak For this, the following two speed transitions will occur:
[0348] 1) From the initial velocity v0 to the desired peak velocity (say v A ) first speed transition.
[0349] 2) Initial velocity v A (Peak velocity v A is reassigned to the initial velocity) to the desired final velocity v, which corresponds essentially to zero f Second speed transition until
[0350] At each iteration of the root finding algorithm, a trajectory segment is generated by applying the first and second velocity transitions to determine the degree of deviation from the commanded position, i.e., the cumulative end point position p E The trajectory segments are integrated by applying the trajectory equations (3, 4, and 5) to generate the desired peak velocity v. The deviation of the cumulative end point from the commanded position for successive values of the velocity peak represents the degree of undershoot or overshoot, i.e., deviation, from the commanded position. Thus, with each iteration of the root-finding algorithm involving the application of the velocity transition algorithm, the root generated, in this case the desired peak velocity v peak An alternative way of looking at this is that one or more rate transition algorithms are nested within each iteration of the root-finding algorithm. In each iteration of the root-finding algorithm, at least one rate transition algorithm is applied to find the peak rate v peak The trajectory segment required to transition to, and therefore the degree of deviation from the commanded position, is calculated. The "sweet spot" velocity shown in the velocity vs. deviation plot of FIG. 33 is the desired peak velocity v between the initial velocity magnitude and the commanded velocity magnitude where the deviation is essentially zero.peak In the particular example shown in Figure 33, the "sweet spot" speed where the objective function is essentially zero is 29.63.
[0351] final velocity v f The trajectory or motion control profile that constitutes the trajectory segment generated by the velocity transition algorithm when it transitions to a "root" velocity such that is substantially zero at the commanded position is referred to as the optimal motion control profile or optimal trajectory. Application of the root finding algorithm is not limited to only finding the optimal peak velocity, the root finding algorithm is also invoked in embodiments of the present invention to find optimal deceleration constraints such that the trajectory can be effectively decelerated to exactly reach the commanded position when applying the velocity transition algorithm described above. Application of the root finding algorithm to find optimal deceleration constraints is described in further detail in Section 7.0 below.
[0352] 6.0 Reverse There may also be situations where the trajectory overshoots the commanded position. This is due to a sudden change in the commanded position, or being moved closer to the loading device, or the travel distance to the commanded position being too short, causing the trajectory to deviate from the desired final speed v f It is possible that the duration of the trajectory segment is such that it takes too long to reach the desired peak velocity v peak and transition to the desired peak velocity v peak from a final velocity v having a magnitude substantially equal to zero f The desired peak velocity v is generated so that the trajectory segment transitions from peak Generating a trajectory for moving backwards to a commanded position is very similar to generating a trajectory when moving towards a commanded position in that a root-finding algorithm is used in conjunction with a velocity transition algorithm to determine v. In the case of a long overshoot from the commanded position, the magnitude of the trajectory velocity is increased to within the maximum velocity constraint v before ramping back towards the commanded position. maxIn either case of overshoot, a velocity transition algorithm is applied to return to the commanded position.
[0353] 7.0 Forced deceleration The trajectory inputs are periodically reconsidered by the control system, and one or more of the constraints may be updated depending on whether there have been any changes to the trajectory, for example visiting a different destination or grid cell, or there is an obstruction on the grid structure. One or more of these changes may include a commanded position, causing the trajectory generator to regenerate the trajectory based on the updated commanded position. The updated commanded position may also result in a trajectory that terminates closer than originally expected. Thus, a deceleration trajectory is required such that the velocity and acceleration of the trajectory are substantially zero at the updated commanded position. This deceleration allows the controller or control system to apply the torque demand, i.e., braking force, required to decelerate the load handling equipment towards the updated commanded position. In certain embodiments of the invention, the forced deceleration algorithm reduces the speed from a current or initial speed to a substantially zero final speed v f The velocity transition algorithm described above is applied to the trajectory, which may involve generating one or more trajectory segments with an enforced deceleration FD constraint instead of a commanded acceleration to enforce deceleration of the trajectory such that the trajectory acceleration and velocity are substantially zero at the updated commanded position.
[0354] The inputs to the forced deceleration algorithm are shown in Table 3 below.
[0355] [Table 3]
[0356] FD Upper Limit FD max The value of FD may be different from the command acceleration or may be substantially the same, depending on the motor tolerances. maxis determined by the tolerance of the drive mechanism of the wheel assembly and / or the stability of the load handling equipment on the grid structure. The control system uses an upper forced deceleration FD to generate additional trajectory segments when applying the speed transition algorithm. max Is the lower limit of the forced deceleration FD used? min The calculated end point position p E The decision will be made based on the above.
[0357] The application of forced slowdown is summarized in the flow diagram 600 shown in Figure 34. After inputting constraints 602, the process performs an FD max or FD min The first two decision processes of the forced deceleration algorithm aim to bring the magnitude of the orbital velocity to essentially zero, i.e., v f = 0. In the sequence shown in the flow diagram, a first check 604 involves a determination process of whether the velocity is increasing. If the trajectory velocity is increasing, the control system applies an appropriate jerk constraint J4 to generate a trajectory segment 606 to reduce the acceleration a0 to substantially zero to prevent further increases in velocity. For example, a trajectory segment may be generated as follows:
[0358] The value of the manipulated variable (jerk) used is J=J4.
[0359] Segment duration is
[0360]
number
[0361] is given by:
[0362] The end point acceleration is the peak acceleration (a E1 =0), end point velocity v E1 and the end point position p E1 is obtained by direct application of the trajectory equations (Equations 3, 4, and 5) with the starting points a0, v0, p0, the jerk J=J4, and the segment duration T.
[0363] The end point of this trajectory segment is reassigned to the start point of the next trajectory segment (a0, v0, p0←a E1 , v E1 , p E1 ).
[0364] If the orbital velocity is not increasing, the control system checks whether the magnitude of the orbital acceleration is greater than the forced deceleration maximum constraint, i.e., |a0|>FD. max FD = 0. If the acceleration magnitude is excessive, the control system proceeds to a next decision process 610 to determine if the initial deceleration is excessive by determining whether FD = 0. If the acceleration magnitude is excessive, the control system reduces the acceleration magnitude to the mandatory deceleration constraint FD max 6. Generate one trajectory segment 612 to lower the trajectory to substantially coincide with
[0365] For example, one trajectory segment is generated as follows:
[0366] The value of the manipulated variable (jerk) used is J=J4.
[0367] Segment duration is
[0368]
number
[0369] is given by:
[0370] The end point acceleration is the peak acceleration (a E2 =FDmax ), end point velocity v E2 and the end point position p E2 is obtained by direct application of the trajectory equation with starting points a0, v0, p0, jerk J=J4, and segment duration T.
[0371] The end point of this trajectory segment is reassigned to the start point of the next trajectory segment (a0, v0, p0←a E2 , v E2 , p E2 )(614).
[0372] The next three decision processes 616, 620, and 624 determine whether the trajectory acceleration has not been increased and does not have excessive acceleration, so the velocity is increased to the desired final velocity v f , i.e. the desired final velocity v f The determination process includes applying a speed transition algorithm to transition the speed to a terminal speed having a magnitude substantially equal to a forced deceleration constraint FD max The process proceeds to determine (616) whether applying the velocity transition algorithm using v will cause the trajectory to exactly reach the commanded position. If so, the velocity magnitude is changed from the initial velocity v to a desired final velocity v that is substantially equal to zero. f (Acceleration constraint is FD max One or more trajectory segments are generated by determining Δv using a velocity transition algorithm 618 to transition to a forced deceleration FD max Sometimes it is not enough, or sometimes it is too strong. Forced deceleration (FD) max If applying FD to the trajectory is deemed insufficient and still causes the trajectory to overshoot the updated commanded position (620), then the only option is for the control system to apply a "reverse" 622, as described in Section 6.0, to recover from the overshoot and thereby exactly reach the updated commanded position. maxThen, one or more trajectory segments are generated to recover from the overshoot by applying a successive velocity transition algorithm to determine Δv, which reduces the magnitude of the velocity from an initial velocity v0 to a desired peak velocity v peak and the end point position p E Then, applying a first speed transition algorithm to transition the endpoint position p E is the command position, the peak velocity v peak To lower the speed, a second speed transition algorithm is applied.
[0373] However, the maximum forced deceleration FD max If the orbit position undershoots when the velocity transition algorithm is applied using the maximum forced deceleration FD max may be considered too strong. In this case, the more gentle forced deceleration constraint FD min needs to be applied to the orbit, i.e., FD min It is necessary to apply the speed transition algorithm using the command acceleration as the lower limit of the forced deceleration FD. min If the velocity transition algorithm using FD min A velocity transition algorithm is applied using (the magnitude of the orbital acceleration is FD min (626). FD min If the orbit undershoots when using FD, the magnitude of the orbital velocity is min We have already reached the lower limit that the updated command position can be exactly reached by using f reaches essentially zero at the end point before the commanded position.
[0374] Upper bound forced deceleration FD when applying speed transition algorithm max In this case, the trajectory undershoots the command position, but the lower limit of forced deceleration FD minIf applying FD causes the trajectory to overshoot the command position, max and F.D. min Between v and v, there is a forced deceleration constraint FD that can be applied to make the trajectory reach the commanded position exactly. In this situation, the control system f The root finder then proceeds to determine a forced deceleration constraint FD to allow the trajectory to reach the final position exactly when φ becomes substantially zero (628). ... max and the lower bound FD min The objective function is the degree of position deviation from the final position when a forced deceleration constraint selected between the upper and lower bounds of the forced deceleration constraint is applied. At each successive iteration of the root-finding algorithm, a velocity transition algorithm is applied to move the trajectory with the selected forced deceleration constraint from the initial starting point to a desired final velocity v that is substantially zero. f Then, all segments on the output list are used to determine the cumulative end point position p E Determine the total distance traveled by the solution by integrating the trajectory from the initial position p0. E is the final velocity v f is essentially zero. Each iteration of the root-finding algorithm produces a more and more accurate approximation of the root, which in this case is the forced deceleration FD. f It is the responsibility of the control system to determine the exact forced deceleration FD using a root-finding algorithm in conjunction with a velocity transition algorithm to transition from the initial starting point to v f A forced deceleration FD is applied to transition to an end point where Δt is substantially equal to zero.
[0375] While the particular embodiment described above relates to a robotic load handling device operable on a grid structure, it will be appreciated that this generation of trajectories or motion control profiles may also be applicable to other powered or robotic devices that require powered motion. For example, the generated trajectories may be used to control the point-to-point movement of a robotic arm.
[0376] It will be understood that the method illustrated and described above is not limited by the order of the blocks, as some blocks may occur in a different order than that illustrated and described herein and / or simultaneously with other blocks. It will also be understood that various other branches, flow paths, and orders of blocks may be implemented that yield the same results.
[0377] Example of application of the speed transition algorithm The application of the velocity transition algorithm according to the present invention can be illustrated by the plots of trajectories shown in Figure 35(a to c). The trajectory is a transition from an initial velocity reference v0 to a desired final velocity reference v f In a particular example of the invention shown in the speed plot of FIG. 35c, the speed transition is from an initial speed reference v0 of +18 to a desired final speed v of −16. f For ease of explanation, this trajectory is assumed to have a desired final velocity v f The plot shows the speed reference versus the desired final speed v f The stage is decomposed into stages 0, 1, 2, and 3 to represent the different orbital segments required to transition from
[0378] Initially, the magnitude of the acceleration is +ve, so the trajectory moves with the desired final velocity v f (Note that the desired final velocity is negative.) Apply the first velocity transition sub-algorithm as follows:
[0379] A) First question: Is the acceleration excessive? No.
[0380] B) Second question: The orbit is v f Are you accelerating away from it? Yes.
[0381] To address the second question, the first stage of application of the first velocity transition sub-algorithm, represented as stage 0 in the trajectory plot, is the time when the trajectory transitions to the desired final velocity v f The generation of a trajectory to reduce the magnitude of the orbital acceleration to zero to prevent accelerating away from the velocity plot in Figure 35. This is shown in the acceleration plot shown in Figure 35a. The initial delta V is given by Δv0, which is shown in the velocity plot in Figure 35c.
[0382] During this stage, a trajectory segment is generated, and the end point of the trajectory segment thus generated becomes the initial trajectory point of the next subproblem, i.e. (a0, v0, p0) ← (a E , v E , p E ) and the delta V is calculated accordingly, Δv1←(v f -vE).
[0383] C) Third question: If acceleration starts immediately and is reduced to zero, the trajectory will remain at v f Will we overshoot the acceleration? No, because the acceleration has already been reduced to zero by the trajectory segment applied in response to the previous question.
[0384] Once the first velocity transition sub-algorithm has been applied, the trajectory is in a state for application of the second velocity transition sub-algorithm. Stages (1), (2), and (3) shown in the trajectory plot represent the generation of trajectory segments in the application of the second velocity transition sub-algorithm.
[0385] The second speed transition sub-algorithm is applied to obtain the peak acceleration a from equation (21) assuming an initial speed reference. peak In this particular example, the peak acceleration a peakis greater than the maximum acceleration. As a result, application of the second speed transition sub-algorithm reduces the acceleration to a max The acceleration includes increasing the speed to a full speed over a period of time. max and then finally, the desired final velocity v f In other words, this portion of the orbit covered by the three orbital segments 1, 2, and 3 looks like a trapezoid.
[0386] The first trajectory segment, represented by (1) in FIG. 35a, is accelerated to the maximum command acceleration a by applying a jerk value. max This includes increasing the delta V to the maximum. f -v E ) is obtained, and the end point of the trajectory segment becomes the initial trajectory point of the next subproblem, i.e. (a0, v0, p0) ← (a E , v E , p E )
[0387] The second trajectory segment, represented by (2), reduces the acceleration to the maximum acceleration a by setting the jerk value equal to zero. max This includes maintaining the acceleration at a maximum acceleration a max The velocity changes linearly over a time duration where v is the velocity at which the acceleration is reduced as the trajectory enters the final stage (3). This allows delta V ← (v f -v E ) is obtained, and the end point of the trajectory segment is assigned to the initial trajectory point of the next subproblem, i.e. (a0, v0, p0) ← (a E , v E , p E )
[0388] The third and final trajectory segment, represented by (3), is the desired final velocity v f The final trajectory segment includes applying a jerk value to reduce the magnitude of the acceleration to satisfy the desired final velocity vf This is shown in FIG. 35c as the speed reference being reduced until it reaches the desired final speed v f This is indicated by a smooth slope up to The following is a summary of the claims as originally filed: [1] A method for generating a trajectory for controlling movement of a motion device, comprising: i) receiving a specification of the trajectory, where the specification includes a command position; ii) receiving at least one jerk constraint; and iii) using the at least one jerk constraint to generate a sequence of one or more trajectory segments, where each of the one or more of the trajectory segments is a respective portion of the trajectory defining a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time; Equipped with 11. The method of claim 10, wherein the one or more trajectory segments of the sequence of trajectory segments are generated based on predicting values of a parameter using a root-finding algorithm to find a root of an objective function, the objective function being an amount of position deviation of a magnitude of the position reference from the commanded position when the velocity reference and the acceleration reference are substantially zero, and a root of the objective function corresponds to a value of the parameter at which the position deviation from the commanded position is less than a predetermined threshold. [2] The method of [1], further comprising receiving at least one velocity constraint, at least one acceleration constraint, and / or at least one deceleration constraint, wherein the at least one velocity constraint, the at least one acceleration constraint, the at least one deceleration constraint, and the at least one jerk constraint define a plurality of constraints for generating a sequence of one or more trajectory segments. [3] The method according to [2], wherein the value of at least one of the plurality of constraints is a function of the total mass of the loading and unloading device. [4] The method described in [3], wherein the value of any one of the at least one velocity constraint, the at least one acceleration constraint, and / or the at least one deceleration constraint is inversely proportional to the total mass of the loading and unloading device. [5] A method according to any one of [2] to [4], wherein the parameter corresponds to the speed reference having a value substantially less than the speed constraint. [6] One or more trajectory segments of the sequence of trajectory segments are generated by applying a speed transition algorithm that includes moving a magnitude of the speed reference of the one or more trajectory segments at a given time to a desired final speed reference, the difference between the desired final speed reference and the speed reference defining a delta-V; The method of any one of claims 1 to 5, wherein the velocity transition algorithm comprises calculating a peak acceleration having a magnitude that is a function of the delta-V. [7] The method of [6], wherein each of the one or more trajectory segments has a start point and an end point, the start point of each of the one or more trajectory segments has an initial position reference, an initial velocity reference and an initial acceleration reference, and the end point of each of the one or more trajectory segments has an end point position reference, an end point velocity reference and an end point acceleration reference, and the parameters are based on the trajectory point of the velocity reference having a desired optimal velocity reference. [8] The method of [7], wherein the delta-V is based on a difference between the desired final speed reference and either the initial speed reference or the terminal point speed reference of the one or more trajectory segments. [9] generating the one or more trajectory segments comprises: i) whether the magnitude of the acceleration reference for the trajectory is substantially equal to the calculated peak acceleration; ii) whether the calculated peak acceleration is substantially less than or equal to the acceleration constraint; and / or iii) whether the calculated peak acceleration exceeds the acceleration constraint; The method according to [7] or [8],
[10] The method of [9], wherein the one or more trajectory segments are generated by applying the at least one jerk constraint such that the desired final velocity reference is substantially zero.
[11] In response to the acceleration reference being substantially equal to the calculated peak acceleration, generating the one or more trajectory segments includes: i) applying the at least one jerk constraint such that the initial acceleration reference of the one or more trajectory segments has a magnitude substantially equal to the calculated peak acceleration, a terminal point acceleration of each of the one or more trajectory segments has a magnitude substantially equal to zero, and the terminal point velocity reference is substantially equal to the desired final velocity reference.
[12] In response to the calculated peak acceleration being less than or equal to the acceleration constraint, generating one or more trajectory segments includes: i) generating a first trajectory segment by applying the at least one jerk constraint such that an end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the peak acceleration; ii) generating the second trajectory segment by applying the at least one jerk constraint such that an end point of the first trajectory segment is assigned to a start point of a second trajectory segment, a magnitude of an end point acceleration of the second trajectory segment is substantially equal to zero, and an end point velocity reference of the second trajectory segment is substantially equal to the desired final velocity reference; The method according to any one of [9] to
[11] , comprising:
[13] Generating one or more trajectory segments in response to the peak acceleration magnitude exceeding the acceleration constraint includes: i) generating the first trajectory segment by applying at least one jerk constraint such that the end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the acceleration constraint; ii) generating the second trajectory segment by applying the at least one jerk constraint having a magnitude substantially equal to zero such that the end point of the first trajectory segment is assigned to the start point of the second trajectory segment and a magnitude of the end point acceleration reference of the second trajectory segment is substantially equal to the acceleration constraint; iii) generating a third trajectory segment by applying the at least one jerk constraint such that the end point of the second trajectory segment is assigned to a start point of a third trajectory segment, a magnitude of an end point acceleration reference of the third trajectory segment is substantially equal to zero, and an end point velocity reference of the third trajectory segment is substantially equal to the desired final velocity reference; The method according to any one of [9] to
[12] , comprising:
[14] The method of
[13] , wherein the one or more trajectory segments are generated in response to the magnitude of the trajectory acceleration reference being substantially less than the acceleration constraint.
[15] Generating one or more trajectory segments in response to the peak acceleration magnitude exceeding the acceleration constraint includes: i) generating a first trajectory segment by applying the at least one jerk constraint having a magnitude substantially equal to zero such that the end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the acceleration constraint; ii) generating the second trajectory segment by applying the at least one jerk constraint such that the end point of the first trajectory segment is assigned to the start point of the second trajectory segment, a magnitude of the end point acceleration of the second trajectory segment is substantially equal to zero, and the end point velocity of the second trajectory segment is substantially equal to the desired final velocity reference; The method according to any one of [9] to
[14] , comprising:
[16] The method of
[15] , wherein the one or more trajectory segments are generated in response to the magnitude of the trajectory acceleration reference being substantially equal to the acceleration constraint.
[17] Applying the rate transition algorithm comprises: i) the magnitude of the acceleration reference exceeds the acceleration constraint; and / or ii) the trajectory is accelerating away from the value of the desired final velocity reference; and / or iii) the trajectory overshoots the value of the desired final velocity reference; The method of any one of [7] to
[16] , further comprising generating one or more trajectory segments in response to.
[18] The method of
[17] , wherein generating one or more trajectory segments in response to the magnitude of the trajectory acceleration reference exceeding the acceleration constraint comprises applying the at least one jerk constraint such that an end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to the magnitude of the acceleration constraint.
[19] i) determining whether the magnitude of the delta-V is substantially equal to zero and the magnitude of the orbital acceleration reference is not substantially equal to zero; and / or ii) determining whether the product of the magnitude of the delta-V and the magnitude of the orbital acceleration reference is substantially less than zero; determining whether the trajectory is accelerating away from the desired final velocity reference value by if either step (i) or (ii) is yes, generating one or more trajectory segments by applying the at least one jerk constraint such that the end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; The method according to
[17] or
[18] , further comprising:
[20] Generating one or more trajectory segments in response to the trajectory overshooting the desired final velocity includes: i) calculating a provisional trajectory segment by applying the at least one jerk constraint such that a magnitude of a terminal point acceleration reference of the provisional trajectory segment is substantially zero, wherein the provisional trajectory segment comprises a start point comprising an initial position reference, an initial velocity reference, and an initial acceleration reference, and a terminal point comprising a terminal point position reference, a terminal point velocity reference, and the terminal point acceleration reference; ii) calculating an auxiliary delta-V corresponding to the difference between the desired final velocity reference and the terminal point velocity reference of the provisional trajectory segment; and iii) incorporating the provisional trajectory segment into the sequence of trajectory segments if a product of the delta-V and the auxiliary delta-V is substantially less than zero, and ignoring the provisional trajectory segment otherwise. The method according to any one of
[17] to
[19] , comprising:
[21] The method of any one of [7] to
[20] , wherein the acceleration reference further includes a deceleration reference, the acceleration constraint further includes a deceleration constraint having an upper deceleration constraint and a lower deceleration constraint, and the one or more trajectory segments in the sequence of trajectory segments further define at least one deceleration reference as a function of time within the upper constraint and the lower constraint of the deceleration constraint or equal to either the upper constraint or the lower constraint of the deceleration constraint.
[22] The at least one jerk constraint: i) a first jerk constraint for increasing the magnitude of the acceleration reference; ii) a second jerk constraint for reducing the magnitude of the acceleration reference; and iii) a third jerk constraint for increasing the magnitude of the deceleration reference; and iv) a fourth jerk constraint for reducing the magnitude of the deceleration reference; The method according to claim 21, comprising:
[23] i) generating one or more trajectory segments by applying the at least one jerk constraint in response to an increase in the magnitude of the trajectory velocity such that the end point deceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; and / or ii) generating one or more trajectory segments by applying the at least one jerk constraint in response to the magnitude of the deceleration reference being substantially greater than the upper deceleration constraint, such that the endpoint deceleration reference of the one or more trajectory segments has a magnitude substantially equal to the upper deceleration constraint. The method of any one of claims 1 to 5, further comprising applying a forced deceleration algorithm to the trajectory by
[24] The method further comprising: i) providing the velocity transition algorithm to generate one or more trajectory segments based on the deceleration constraint having a magnitude substantially equal to the upper deceleration constraint such that the magnitude of the terminal point acceleration reference of the one or more trajectory segments is substantially equal to zero and the terminal point velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference. The method according to any one of
[21] to
[23] , further comprising:
[25] i) determining whether an end point position reference of the one or more trajectory segments overshoots the commanded position when a corresponding end point deceleration reference has a magnitude substantially equal to zero and a corresponding end point velocity reference is substantially equal to the desired final velocity reference; ii) in response to the trajectory overshooting the commanded position, applying "reverse" to generate one or more trajectory segments to return the trajectory to the commanded position by applying the velocity transition algorithm to generate the one or more trajectory segments such that at the commanded position, the magnitude of the terminal point acceleration reference is substantially equal to zero and the terminal point velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference; The method of claim 24, further comprising:
[26] i) determining whether an end point position reference of the one or more trajectory segments of the trajectory undershoots the commanded position when a magnitude of a corresponding end point deceleration reference of the one or more trajectory segments is substantially zero and a corresponding end point velocity reference is substantially equal to the desired final velocity reference; ii) in response to the trajectory undershooting the commanded position, applying the velocity transition algorithm to generate the one or more trajectory segments based on a deceleration constraint having a magnitude substantially equal to the lower deceleration constraint; The method of any one of
[21] to
[25] , further comprising the step of generating one or more trajectory segments by:
[27] i) predicting parameters corresponding to optimal constrained deceleration constraints having values between the upper constrained deceleration constraint and the lower constrained deceleration constraint using the root-finding algorithm; ii) applying the velocity transition algorithm to generate the one or more trajectory segments with a deceleration constraint substantially equal to the optimal enforced deceleration constraint such that when a magnitude of the position deviation of the endpoint position reference of the one or more trajectory segments from the commanded position is less than the predetermined threshold, a magnitude of the endpoint acceleration reference is substantially equal to zero and the endpoint velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference; The method of any one of
[21] to
[26] , further comprising the step of generating one or more trajectory segments by:
[28] The method of any one of [7] to
[27] , wherein the velocity transition algorithm is nested within the root-finding algorithm such that the velocity transition algorithm generates one or more trajectory segments for each successive parameter predicted by the root-finding algorithm.
[29] The method of any one of [7] to
[28] , further comprising a step of assigning an end point of each of the one or more trajectory segments to a start point of the one or more subsequent trajectory segments in the sequence such that the end point position reference, the end point velocity reference, and the end point acceleration reference of the one or more trajectory segments are respectively assigned to an initial position reference, an initial velocity reference, and an initial acceleration reference of the one or more subsequent trajectory segments in the sequence.
[30] The method of any one of [1] to
[29] , wherein the trajectory is S-shaped, such that the one or more trajectory segments are generated using a mathematical formula for an S-curve.
[31] A method according to any one of [1] to
[30] , generating a trajectory defining an acceleration reference that varies continuously as a function of time for the one or more trajectory segments in the sequence of trajectory segments by applying the at least one jerk constraint.
[32] A trajectory generator for generating a trajectory to control movement of a motion device, comprising: i) a memory; and ii) a processor configured to execute computer-executable instructions stored on a memory, wherein the processor is configured to generate a motion control profile according to a method according to any one of [1] to
[31] upon receiving a command position. A trajectory generator comprising:
[33] A load handling apparatus comprising: a wheel assembly having a first pair of wheels and a second pair of wheels; a drive mechanism for driving the wheel assembly; and a control device connected to the drive mechanism that is instructed to drive the wheel assembly according to a motion control profile generated by a method according to any one of [1] to
[31] .
[34] A non-transitory computer medium storing computer-executable instructions that, in response to execution, cause a computer system to perform the operations recited in any one of [1] to
[31] .
Claims
1. A method for generating a trajectory for controlling movement of a loading device, comprising: i) receiving a specification of the trajectory, wherein the specification includes a command position; ii) receiving at least one jerk constraint; iii) using the at least one jerk constraint to generate a sequence of one or more trajectory segments, each of the one or more trajectory segments being a respective portion of the trajectory defining a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time; Equipped with the one or more trajectory segments of the sequence of trajectory segments are generated based on predicting values of a parameter using a root-finding algorithm to find a root of an objective function, the objective function being an amount of position deviation of the magnitude of the position reference from the commanded position when the velocity reference and the acceleration reference are substantially zero, and a root of the objective function corresponds to a value of the parameter at which the position deviation from the commanded position is less than a predetermined threshold; The method further comprises receiving at least one velocity constraint, at least one acceleration constraint, and / or at least one deceleration constraint, wherein the at least one velocity constraint, the at least one acceleration constraint, the at least one deceleration constraint, and the at least one jerk constraint define a plurality of constraints for generating a sequence of one or more trajectory segments; A method wherein the value of at least one of the plurality of constraints is a function of a total mass of the loading equipment.
2. The method of claim 1 , wherein a value of any one of the at least one velocity constraint, the at least one acceleration constraint, and / or the at least one deceleration constraint is inversely proportional to the total mass of the loading equipment.
3. The method of claim 1 or 2, wherein the parameter corresponds to the speed reference having a value substantially less than or equal to the speed constraint.
4. One or more trajectory segments of the sequence of trajectory segments are generated by applying a speed transition algorithm that includes moving a magnitude of the speed reference of the one or more trajectory segments at a given time to a desired final speed reference, the difference between the desired final speed reference and the speed reference defining a delta-V; The method of claim 3 , wherein the velocity transition algorithm comprises calculating a peak acceleration having a magnitude that is a function of the delta-V.
5. 5. The method of claim 4, wherein each of the one or more trajectory segments comprises a start point and an end point, the start point of each of the one or more trajectory segments comprises an initial position reference, an initial velocity reference, and an initial acceleration reference, and the end point of each of the one or more trajectory segments comprises an end point position reference, an end point velocity reference, and an end point acceleration reference, and the parameters are based on a trajectory point of the velocity reference having a desired optimal velocity reference.
6. 6. The method of claim 5, wherein the delta-V is based on a difference between the desired final speed reference and either the initial speed reference or the terminal point speed reference of the one or more trajectory segments.
7. Generating the one or more trajectory segments includes: i) whether the magnitude of the acceleration reference for the trajectory is substantially equal to the calculated peak acceleration; and / or ii) whether the calculated peak acceleration is substantially less than or equal to the acceleration constraint; and / or iii) whether the calculated peak acceleration exceeds the acceleration constraint; The method according to claim 5 or 6, which is based on
8. The method of claim 7 , wherein the one or more trajectory segments are generated by applying the at least one jerk constraint such that the desired final velocity reference is substantially zero.
9. generating the one or more trajectory segments in response to the acceleration reference being substantially equal to the calculated peak acceleration includes:
9. The method according to claim 7 or 8, comprising the step of: i) applying the at least one jerk constraint such that the initial acceleration reference of the one or more trajectory segments has a magnitude substantially equal to the calculated peak acceleration, a terminal point acceleration of each of the one or more trajectory segments has a magnitude substantially equal to zero, and the terminal point velocity reference is substantially equal to the desired final velocity reference.
10. generating one or more trajectory segments in response to the calculated peak acceleration being less than or equal to the acceleration constraint, i) generating a first trajectory segment by applying the at least one jerk constraint such that an end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the peak acceleration; ii) generating the second trajectory segment by applying the at least one jerk constraint such that an end point of the first trajectory segment is assigned to a start point of a second trajectory segment, a magnitude of an end point acceleration of the second trajectory segment is substantially equal to zero, and an end point velocity reference of the second trajectory segment is substantially equal to the desired final velocity reference; The method according to any one of claims 7 to 9, comprising:
11. Generating one or more trajectory segments in response to the peak acceleration magnitude exceeding the acceleration constraint includes: i) generating a first trajectory segment by applying the at least one jerk constraint such that the end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the acceleration constraint; ii) generating the second trajectory segment by applying the at least one jerk constraint having a magnitude of substantially zero such that the end point of the first trajectory segment is assigned to the start point of a second trajectory segment and a magnitude of the end point acceleration reference of the second trajectory segment is substantially equal to the acceleration constraint; iii) generating a third trajectory segment by applying the at least one jerk constraint such that the end point of the second trajectory segment is assigned to a start point of a third trajectory segment, a magnitude of an end point acceleration reference of the third trajectory segment is substantially equal to zero, and an end point velocity reference of the third trajectory segment is substantially equal to the desired final velocity reference; The method according to any one of claims 7 to 10, comprising:
12. The method of claim 11 , wherein the one or more trajectory segments are generated in response to a magnitude of an acceleration reference for the trajectory being substantially less than the acceleration constraint.
13. Generating one or more trajectory segments in response to the peak acceleration magnitude exceeding the acceleration constraint includes: i) generating the first trajectory segment by applying the at least one jerk constraint having a magnitude substantially equal to zero such that the end point acceleration reference of the first trajectory segment has a magnitude substantially equal to the acceleration constraint; ii) generating the second trajectory segment by applying the at least one jerk constraint such that the endpoint of the first trajectory segment is assigned to the start point of a second trajectory segment, a magnitude of the endpoint acceleration reference of the second trajectory segment is substantially equal to zero, and the endpoint velocity reference of the second trajectory segment is substantially equal to the desired final velocity reference; The method of any one of claims 7 to 12, comprising:
14. The method of claim 13 , wherein the one or more trajectory segments are generated in response to a magnitude of an acceleration reference for the trajectory being substantially equal to the acceleration constraint.
15. Applying the rate transition algorithm comprises: i) the magnitude of the acceleration reference exceeds the acceleration constraint; and / or ii) the trajectory is accelerating away from the value of the desired final velocity reference; and / or iii) the trajectory overshoots the value of the desired final velocity reference; 15. The method of claim 5, further comprising generating one or more trajectory segments in response to.
16. 16. The method of claim 15, wherein generating one or more trajectory segments in response to a magnitude of the acceleration reference of the trajectory exceeding the acceleration constraint comprises applying the at least one jerk constraint such that an end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to a magnitude of the acceleration constraint.
17. i) determining whether the magnitude of the delta-V is substantially equal to zero and the magnitude of the trajectory acceleration reference is not substantially equal to zero; and / or ii) determining whether the product of the magnitude of the delta-V and the magnitude of the trajectory acceleration reference is substantially less than zero; determining whether the trajectory is accelerating away from the desired final velocity reference value by if either step (i) or (ii) is yes, generating one or more trajectory segments by applying the at least one jerk constraint such that the end point acceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; 17. The method of claim 15 or 16, further comprising:
18. generating one or more trajectory segments in response to the trajectory overshooting the desired final velocity includes: i) calculating a provisional trajectory segment by applying the at least one jerk constraint such that a magnitude of a terminal point acceleration reference of the provisional trajectory segment is substantially zero, wherein the provisional trajectory segment comprises a start point comprising an initial position reference, an initial velocity reference, and an initial acceleration reference, and a terminal point comprising a terminal point position reference, a terminal point velocity reference, and the terminal point acceleration reference; ii) calculating an auxiliary delta-V corresponding to the difference between the desired final speed reference and the terminal point speed reference of the provisional trajectory segment; and iii) incorporating the provisional trajectory segment into the sequence of trajectory segments if the product of the delta-V and the auxiliary delta-V is substantially less than zero, and ignoring the provisional trajectory segment otherwise.
18. The method of any one of claims 15 to 17, comprising:
19. 19. The method of claim 5, wherein the acceleration reference further comprises a deceleration reference, the acceleration constraint further comprises a deceleration constraint having an upper deceleration constraint and a lower deceleration constraint, and the one or more trajectory segments in the sequence of trajectory segments further define at least one deceleration reference as a function of time within or equal to either the upper deceleration constraint or the lower deceleration constraint of the deceleration constraint.
20. The at least one jerk constraint may be: i) a first jerk constraint for increasing the magnitude of the acceleration reference; ii) a second jerk constraint for reducing the magnitude of the acceleration reference; and iii) a third jerk constraint for increasing the magnitude of the deceleration reference; and iv) a fourth jerk constraint for reducing the magnitude of the deceleration reference; 20. The method of claim 19, comprising:
21. i) generating one or more trajectory segments by applying the at least one jerk constraint in response to an increase in magnitude of trajectory velocity such that the end point deceleration reference of the one or more trajectory segments has a magnitude substantially equal to zero; and / or ii) generating one or more trajectory segments by applying the at least one jerk constraint in response to the magnitude of the deceleration reference being substantially greater than the upper deceleration constraint, such that the endpoint deceleration reference of the one or more trajectory segments has a magnitude substantially equal to the upper deceleration constraint.
21. The method of claim 19 or 20, further comprising applying a forced deceleration algorithm to the trajectory by:
22. The method comprises: i) providing the velocity transition algorithm to generate one or more trajectory segments based on the deceleration constraint having a magnitude substantially equal to the upper deceleration constraint, such that the magnitude of the terminal point acceleration reference of the one or more trajectory segments is substantially equal to zero and the terminal point velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference.
22. The method of any one of claims 19 to 21, further comprising:
23. i) determining whether an end point position reference of the one or more trajectory segments overshoots the commanded position when a corresponding end point deceleration reference has a magnitude substantially equal to zero and a corresponding end point velocity reference is substantially equal to the desired final velocity reference; ii) in response to the trajectory overshooting the commanded position, applying "reverse" to generate one or more trajectory segments to return the trajectory to the commanded position by applying the velocity transition algorithm to generate the one or more trajectory segments such that at the commanded position, the magnitude of the terminal point acceleration reference is substantially equal to zero and the terminal point velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference; 23. The method of claim 22, further comprising:
24. i) determining whether an end point position reference of the one or more trajectory segments of the trajectory undershoots the commanded position when a magnitude of a corresponding end point deceleration reference of the one or more trajectory segments is substantially zero and a corresponding end point velocity reference is substantially equal to the desired final velocity reference; ii) in response to the trajectory undershooting the commanded position, applying the velocity transition algorithm to generate the one or more trajectory segments based on a deceleration constraint having a magnitude substantially equal to the lower deceleration constraint; 24. The method of claim 19, further comprising generating one or more trajectory segments by:
25. i) predicting parameters corresponding to an optimal mandatory deceleration constraint having a value between said upper mandatory deceleration constraint and said lower mandatory deceleration constraint using said root-finding algorithm; ii) applying the velocity transition algorithm to generate the one or more trajectory segments with a deceleration constraint substantially equal to the optimal mandatory deceleration constraint such that when a magnitude of the position deviation of the endpoint position reference of the one or more trajectory segments from the commanded position is less than the predetermined threshold, a magnitude of the endpoint acceleration reference is substantially equal to zero and the endpoint velocity reference of the one or more trajectory segments is substantially equal to the desired final velocity reference; 25. The method of claim 19, further comprising generating one or more trajectory segments by:
26. 26. The method of any one of claims 5 to 25, wherein the velocity transition algorithm is nested within the root finder such that the velocity transition algorithm generates one or more trajectory segments for each successive parameter predicted by the root finder.
27. 27. The method of claim 5, further comprising the step of assigning an end point of each of the one or more trajectory segments to a start point of the one or more subsequent trajectory segments in the sequence such that the end point position reference, the end point velocity reference, and the end point acceleration reference of the one or more trajectory segments are respectively assigned to an initial position reference, an initial velocity reference, and an initial acceleration reference of the one or more subsequent trajectory segments in the sequence.
28. 28. The method of any one of claims 1 to 27, wherein the trajectory is S-shaped such that the one or more trajectory segments are generated using a mathematical formula for an S-curve.
29. 29. The method of any one of claims 1 to 28, wherein generating the one or more trajectory segments by applying the at least one jerk constraint generates a trajectory that defines an acceleration reference that varies continuously as a function of time for the one or more trajectory segments in the sequence of trajectory segments.
30. A trajectory generator for generating a trajectory for controlling a movement of a motion device, comprising: i) a memory; and ii) a processor configured to execute computer executable instructions stored on a memory, wherein the processor is configured to generate a motion control profile according to the method of any one of claims 1 to 29 upon receiving a command position. A trajectory generator comprising:
31. 30. A load handling apparatus comprising: a wheel assembly having a first pair of wheels and a second pair of wheels; a drive mechanism for driving the wheel assembly; and a controller connected to the drive mechanism instructed to drive the wheel assembly according to a motion control profile generated by the method of any one of claims 1 to 29.
32. 30. A non-transitory computer medium having stored thereon computer-executable instructions that, upon execution, cause a computer system to perform the operations recited in any one of claims 1 to 29.
Citation Information
Patent Citations
Articulated robot, articulation harmonization control device and method
JP2013215827A
Robot controller, and robot control method
JP2014014876A
Method and apparatus for trajectory-planning
US20200384645A1
