Motion control of robotic loading and unloading equipment

Abstract

1. A load handling apparatus for lifting and moving one or more containers stacked in a storage system, the storage system comprising a grid framework structure supporting a plurality of tracks arranged in a grid pattern to define a grid structure over said stack of one or more containers, the grid pattern comprising a plurality of grid cells, each of said stacks of one or more containers located within the footprint of one grid cell, the load handling apparatus comprising: i) a wheel assembly comprising a first pair of wheels and a second pair of wheels, the wheel assembly being driven by a drive mechanism; ii) a lifting device for lifting a storage container from one of the stacks of containers into a container receiving space of the load handling apparatus; iii) position detection means for measuring a position of the load handling apparatus relative to the grid structure; and iv) a controller in communication with the position detection means and the drive mechanism, the controller being configured to: a) determine a feedforward torque demand of the wheel assembly at a given time from a predetermined motion control profile; and b) control movement of the load handling apparatus from a current position on the grid structure to a command position by compensating for the feedforward torque demand from a measured position of the load handling apparatus on the grid structure at said given time to calculate a torque demand of the wheel assembly.

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 provides a materials handling apparatus for lifting and moving one or more stacked containers in a storage system, the storage system comprising a grid framework structure supporting a plurality of tracks arranged in a grid pattern to define a grid structure over said stack of one or more containers, the grid pattern comprising a plurality of grid cells, each of the stack of one or more containers being located within the footprint of one grid cell, the materials handling apparatus comprising: i) a wheel assembly comprising a first pair of wheels and a second pair of wheels, the wheel assembly being driven by a drive mechanism; ii) a lifting device for lifting a storage container from one of the stacks of containers into a container receiving space of the loading device; iii) position detection means for determining the position of the loading equipment relative to the grid structure; iv) a controller in communication with the position sensing means and the drive mechanism, the controller comprising: a) determining a feedforward torque demand for a wheel assembly at a given time from a predetermined motion control profile; b) compensating for feedforward torque demand from the measured position of the loading device on the grid structure from the position sensing means at said given time to calculate torque demand of the wheel assembly. and configured to control movement of the load handling equipment from a current position on the grid structure to a command position by The present invention provides a loading device comprising:

[0018] According to the invention, a control device in communication with the position detection means and the drive mechanism comprises a motion control system for controlling the position and kinematic state of the load handling equipment on the grid structure under the guidance of a predetermined motion control profile or trajectory. When the control device determines that the load handling equipment must move to a new position or change speed, the control device calculates a position or velocity trajectory, called the predetermined motion control profile, for transitioning the load handling equipment from a current position or velocity trajectory to a target position or velocity. The predetermined motion control profile defines the kinematic state of the load handling equipment on the grid structure, including but not limited to the velocity, acceleration, and / or position over time as the load handling equipment moves from the current position to the target state. The predetermined motion control profile comprises a sequence of one or more trajectory segments, each of which is a portion of the trajectory or motion control profile that defines at least one of a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time.

[0019] Once this predetermined 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. The predetermined motion control profile can be optionally based on two preferred motion control profiles: a trapezoidal profile or an S-curve profile. In both the trapezoidal and S-curve profiles, the various trajectory segments of the motion control profile are generated based on predefined constraints, i.e., maximum speed, maximum acceleration, etc., where the defined constraints may correspond to mechanical limitations of the drive mechanism of the load handling equipment. Given these constraints and the desired command position, the controller generates the predetermined motion control profile that is used to execute the desired movement. In a trapezoidal profile, the controller calculates the motion control profile according to three different stages, i.e., an acceleration stage, a constant velocity stage, and a deceleration stage. Such a profile results in a trapezoidal velocity curve. In the case of a trapezoidal profile, it is the acceleration that creates the discontinuities, and therefore the manipulated variable (defined as the variable that is controlled and thus allowed to change discontinuously) is the acceleration. Thus, the trapezoidal profile always accelerates or decelerates at a defined maximum acceleration rate. This results in a "jerkier" movement of the loading equipment on the grid structure.

[0020] The S-curve profile is a modification of the trapezoidal profile that mitigates this "jerky" by adding an additional stage that allows for a gradual transition between the constant (or zero) velocity stage and the constant acceleration / deceleration stage, resulting in smoother motion. In comparison to the trapezoidal profile, the manipulated variable that is allowed to vary discontinuously is the "jerk". Thus, in addition to the given constraints of maximum velocity and maximum acceleration, the S-curve profile requires an additional "jerk" constraint.

[0021] In the present invention, a controller in communication with the position sensing means and the drive mechanism controls the movement of the load handling equipment from a current position on the grid structure to a commanded position by determining a feedforward torque demand of the wheel assembly at a given time from a predetermined motion control profile. Knowing the mass of the load handling equipment and the radius of each of the wheels of the wheel assembly, the feedforward torque control is calculated from the product of the mass and acceleration from the predetermined motion control profile, where the acceleration is the derivative of the velocity of the predetermined motion control profile, i.e. the track acceleration reference. The controller preferably compensates the feedforward torque demand with an "actual" measurement of the position of the load handling equipment on the grid structure from the position sensing means to calculate an "actual" torque demand of the wheel assembly. Preferably, the feedback torque demand is calculated from the derivative with respect to time of the position measurement. More preferably, the controller: i) determining a feedback torque demand on the wheel assembly from the measured position of the load handling device; ii) combining the feedforward torque demand with the feedback torque demand to calculate the torque demand of the wheel assembly; The feedforward torque demand is thereby compensated for.

[0022] Optionally, the feedback torque demand is determined from a comparison between a magnitude of a position reference derived from a predefined motion profile and a measured position of the load equipment on the grid structure at said given time. According to an embodiment of the invention, the controller compares the track position reference derived from the predefined motion control profile with the measured position of the load equipment on the grid structure. Preferably, the controller is configured to perform a PI or PID calculation on the difference between the magnitude of the track position reference and the measured position. This difference provides a position error that is fed to the PI or PID controller to calculate a compensating torque demand, the track position reference derived from the motion control profile being the target position and the signal from the position sensor being the measured position. The track position reference and the measured position are combined in the PI or PID controller to provide the compensated torque demand. The compensated or feedback torque demand is combined with a feedforward torque demand to provide a torque demand of the wheels for driving the load equipment. This compensating torque demand allows the load equipment to follow the "theoretical" motion control profile provided by the predefined motion control profile.

[0023] Preferably, the feedforward torque demand is determined from the magnitude of an acceleration reference and / or the magnitude of a velocity reference.

[0024] Further preferably, any one of the jerk reference, and / or the acceleration reference, and / or the velocity reference, and / or the position reference is stored in a look-up table.

[0025] Preferably, the feedforward torque demand is obtained by summing three torque terms: an acceleration feedforward torque demand, a velocity feedforward torque demand, and a zeroth order feedforward torque demand, where the acceleration feedforward torque demand is proportional to an acceleration reference, the velocity feedforward torque demand is proportional to a velocity reference, and the zeroth order feedforward torque demand is a constant with a fixed value and sign of the trajectory velocity reference.

[0026] Once the torque demand is calculated, the controller converts the torque demand into an appropriate control signal for moving the load handling equipment on the grid structure through a trajectory defined by a predetermined motion control profile. Preferably, each wheel of the first and second wheel pairs is configured to be driven independently by a drive mechanism. More preferably, each wheel of the wheel assembly comprises a hub motor. Thus, a control signal for moving the load handling equipment is sent to the hub motor. However, the present invention is not limited to a drive mechanism comprising a hub motor driving the wheels of the wheel assembly, and any drive mechanism for driving wheels known in the art is applicable in the present invention. For example, the wheels may be driven by one or more drive motors by suitable pulley and / or gear arrangements. However, an advantage of using a hub motor to drive each of the wheels of the load handling equipment is that the torque of each wheel of the wheel assembly can be controlled by controlling the control signal sent to each hub motor.

[0027] Preferably, the drive means is configured to at least partly drive at least one of the wheels of the wheel assembly to meet the calculated torque demand. Preferably, the drive means is configured to at least partly brake at least one of the wheels of the wheel assembly to meet the calculated torque demand.

[0028] Preferably, the controller is configured to share the calculated torque demand between each wheel of the wheel assembly. In the present invention, the term "share" also encompasses distribution in that the calculated torque demand is distributed between each wheel of the wheel assembly. Optionally, the controller is configured to share the calculated torque demand between a first wheel pair and a second wheel pair of the wheel assembly. In one example of the present invention, the torque demand is divided between a front wheel pair and a rear wheel pair depending on the direction of travel of the load handling equipment on the grid structure. The front wheel pair can be imagined as being driven synchronously and driven about a front "axle". Similarly, the rear wheel pair can be imagined as being driven synchronously and driven about a rear "axle". In a particular embodiment of the present invention, the torque demand is divided between a front "axle" and a rear "axle" to account for any weight transfer of the load handling equipment on the grid structure.

[0029] Optionally, the controller is configured to vary the proportion of the calculated torque demand provided by the first and second wheel pairs. Preferably, the controller is configured to transmit a greater proportion of the calculated torque demand to the first wheel pair when the first wheel pair is further aft of the second wheel pair in the direction of travel of the load handling equipment on the grid structure. Preferably, the controller is configured to transmit a greater proportion of the calculated torque demand to the second wheel pair when the second wheel pair is further forward of the first wheel pair in the direction of travel of the load handling equipment on the grid structure. For example, the amount of torque shared between the front and rear wheel pairs can be varied and can depend on whether the load handling equipment is accelerating forward on the grid structure or decelerating on the grid structure. The first wheel pair represents the front wheel pair and the second wheel pair represents the rear wheel pair. When the load handling equipment is accelerating on the grid structure, a greater proportion of the torque is distributed to the front wheel pair such that the drive mechanism of the front wheel pair is responsible for a greater proportion of the torque demand. Conversely, during deceleration of the load handling device on the grid structure, torque demand is switched to the rear wheel pair.

[0030] The ratio of torque provided by the first and second wheel pairs can be dependent on the magnitude of the track acceleration reference derived from the predetermined motion control profile. Optionally, the controller is configured to share the calculated torque demand between the first and second wheel pairs of the wheel assembly in a ratio based on the magnitude of the track acceleration reference derived from the predetermined motion control profile. This ratio of torque demand distribution between the front and rear wheels can be proportional to the magnitude of the acceleration reference of the loading device on the grid structure. For example, when the magnitude of the track acceleration reference according to the predetermined motion control profile is high, more torque is distributed to the front wheel pair, and conversely, when the acceleration magnitude is decreasing or decelerating, more torque is distributed to the rear wheel pair. In this manner, the distribution of torque demand between the first and second wheel sets varies in a ratio based on data related to the track acceleration reference derived from the predetermined motion control profile.

[0031] Preferably, the ratio of the calculated torque demand between the first wheel pair and the second wheel pair of the wheel assembly is a function of the magnitude of the track acceleration reference.

[0032] During operation of the load handling equipment on the grid structure the predetermined motion control profile is periodically updated from position measurements from the position detection means. Preferably the control device is configured to determine a kinematic state of the load handling equipment on the grid structure from the position measurement means. More preferably the kinematic state is a velocity. Preferably the velocity of the load handling equipment relative to the grid structure is determined from a first derivative with respect to time of the measurement of the position of the load handling equipment relative to the grid structure. Preferably the load handling equipment comprises a wheel encoder coupled to each of the wheels of the wheel assembly for measuring the speed of each of the wheels of the wheel assembly. A comparison of the kinematic state of the load handling equipment relative to the grid structure with the speed of at least one of the wheels of the wheel assembly will determine whether any one of the wheels of the wheel assembly is slipping on the grid structure or track, i.e. whether it has lost traction with the grid structure.

[0033] More preferably, a controller in communication with the drive wheel encoder further comprises: i) comparing the measured speed of each of the wheels of the wheel assembly with the kinematic conditions of the loading device; ii) in response to the measured speed of any one of the wheels of the wheel assembly being substantially different from the kinematic condition of the load handling device on the grid structure; a) adjusting the shared calculated torque demand to the other wheels so that the kinematics of the load handling equipment remain substantially unchanged; and / or b) redistributing the calculated torque demand to each of the wheels of the wheel assembly. The present invention is configured to:

[0034] Further preferably, the controller in communication with the drive wheel encoder is configured, in response to the measured speed of any one of the wheels of the wheel assembly being substantially greater than the kinematic condition of the load handling device, to remove the calculated torque demand shared from any one of the wheels of the wheel assembly until the speed of said wheel is substantially equal to the kinematic condition of the load handling device.

[0035] There are two possible cases when the speed of at least one wheel is different from the kinematic condition of the load handling equipment on the grid structure. The first case may be that at least one of the wheels of the wheel assembly has lost traction and is slipping on the grid structure. The second case may be that the distribution of torque demand between at least one of the wheels of the wheel assembly is not sufficiently uniform, and at least one wheel may rotate at a different speed than the other wheels that are primarily driving the load handling equipment on the grid structure. In either case, the controller is configured to adjust the shared calculated torque demand to any one of the wheels of the wheel assembly. Preferably, in response to the measured speed of any one of the wheels of the wheel assembly being substantially less than the kinematic condition of the load handling equipment on the grid structure, the controller adjusts the shared calculated torque demand to the other wheels and / or redistributes the calculated torque demand to each of the wheels of the wheel assembly such that the kinematic condition of the load handling equipment remains substantially unchanged. Alternatively, in response to the measured speed of any one of the wheels of the wheel assembly being substantially greater than the kinematic conditions of the load handling device on the grid structure, the controller removes the calculated shared torque demand from any one of the wheels of the wheel assembly until the speed of that wheel is substantially equal to the kinematic conditions of the load handling device on the grid structure. In this example, if any one of the wheels is slipping, torque is removed from that wheel until the speed of that wheel catches up with the kinematic conditions of the load handling device on the grid structure. The "remove" function can include stopping or turning off the control signal sent to the slipping wheel.

[0036] As an alternative to the torque demand being a driving torque for driving the load handling equipment on the grid structure, the torque demand may also be a braking or resisting torque to any one of the wheels of the wheel assembly, e.g., during deceleration of the load handling equipment on the grid structure. Thus, instead of driving the rotation of the wheels, the torque demand may also provide a resisting or braking torque, i.e., the controller converts the predetermined motion control profile into a control signal commanding the load handling equipment to decelerate on the grid structure according to the predetermined motion control profile.

[0037] The wheel assembly of the present invention is configured such that the wheel assembly comprises a first wheel set arranged to move the load handling equipment in a first direction on the grid structure and a second wheel set arranged to move the load handling equipment in a second direction on the grid structure, the first wheel set being oriented transverse to the second wheel set. The first wheel set comprises a pair of wheels at the front and rear of the load handling equipment body. The second wheel set comprises a pair of wheels on either side of the load handling equipment body.

[0038] Each of the wheel sets allows the vehicle to move along the rails in the X and Y directions respectively. By lifting each wheel set off its respective rail, one or both wheel sets can be moved vertically so that the vehicle can be moved in the desired direction on the grid.

[0039] Preferably, the position detection means comprises a first position detection means and a second position detection means, the first position detection means configured to detect the position of the loading equipment in a first direction on the grid structure and the second position detection means configured to detect the position of the loading equipment in a second direction on the grid structure, the first direction intersecting the second direction.

[0040] Preferably, the position detection means comprises an encoder coupled to a position wheel rotatably mounted on an arm such that rotation of the position wheel is detected by the encoder, said arm being biased downwards to bias the position wheel into engagement with the grid structure. The position detection means is also referred to as a "fifth wheel" due to the fact that it is simply an additional wheel between the first or second set of wheels. The position wheel is biased and pressed against the grid structure such that movement of the loading equipment on the grid structure causes the position wheel to rotate. An encoder coupled to the position wheel measures the rotational speed of the position wheel, which is an indication of the kinematic state of the loading equipment on the grid structure.

[0041] The present invention relates to a first set of tracks in a substantially horizontal plane and a second set of tracks running intersecting the first set of tracks, the first set of tracks defining a grid pattern having a plurality of grid spaces or grid cells; a stack of containers positioned beneath the first set of tracks and the second set of tracks, wherein each of the stacks of containers occupies one grid space or grid cell; a loading device according to the invention arranged to travel along a first set of tracks and a second set of tracks over said plurality of grid spaces or grid cells such that when positioned over a stack of containers occupying one grid space or grid cell, the lifting device is configured to lift at least one container from said stack of containers; A storage system comprising:

[0042] 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]

[0043] [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

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

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

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

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

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

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

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

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

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

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

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

[0055] 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 separately stored within the controller of the load handling device 130 .

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

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

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

[0059] 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:

[0060]

number

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

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

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

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

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

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

[0067] 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:

[0068]

number

[0069] 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-τ).

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

[0071] In this particular embodiment of the invention, the generation of the motion control profile is described with reference to an S-curve profile.

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

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

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

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

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

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

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

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

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

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

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

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

[0084] In one example, the total torque demand is calculated as follows:

[0085] 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:

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

[0087] The velocity feedforward gain also depends on the estimated total mass of the loading device.

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

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

[0090] Alternatively, values ​​for the acceleration feedforward gain, velocity feedforward gain, and zero order torque demand may be calculated rather than empirically determined.

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

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

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

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

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

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

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

[0098] In some examples, the velocity, acceleration, and / or jerk constraints can be modified depending on the mass of the storage container and contents.

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

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

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

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

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

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

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

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

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

[0108] 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".

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

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

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

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

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

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

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

[0116] 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:

[0117] Acceleration Phase Jerk 1 - Increase the magnitude of acceleration when you want to increase the velocity.

[0118] Jerk 2 - Decreasing the magnitude of acceleration when you want to increase the velocity.

[0119] Deceleration Phase Jerk 3- Increasing the magnitude of acceleration when you want to decrease the velocity.

[0120] Jerk 4 - Decrease the magnitude of acceleration when you want to decrease the velocity.

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

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

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

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

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

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

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

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

[0129] 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:

[0130] 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 asked 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 of the trajectory or the current status.

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

[0132] 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

[0133] The manipulated variable (jerk) value used is J=J2 (reduce the orbital acceleration reference).

[0134] The duration of the orbital segment is

[0135]

number

[0136] is given by:

[0137] ·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.

[0138] If the situation is such that the current trajectory acceleration does not exceed the commanded acceleration, the control system proceeds to the next question.

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

[0140] 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

[0141] The manipulated variable (jerk) value used is J=J2 (reduce the orbital acceleration reference).

[0142] The duration of the orbital segment is

[0143]

number

[0144] is given by:

[0145] ·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.

[0146] 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 depends on 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. 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.

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

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

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

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

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

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

[0153] 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:

[0154] (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.

[0155] (2) After time T, the command speed changes to zero speed (i.e., v f =0), the orbit must undergo a velocity transition, which causes the orbit position to increase by an additional Δp decel Only one step forward will be taken.

[0156] (3) Therefore, remaining distance = Δp cruise +Δp decel .

[0157] If the trajectory is at the current position, the remaining distance is calculated from the command position minus the current position (p0).

[0158] Δp cruise , Δp decel, and the values ​​of T are calculated by the control system as follows:

[0159] Δ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

[0160] By (3), the control system is cruise = remaining distance – Δp decel can be calculated.

[0161] Finally, the duration T of the trajectory segment in the constant velocity phase is given by

[0162]

number

[0163] is determined from.

[0164] Where: Δp cruise is the distance traveled while at a constant commanded velocity.

[0165] v max is the command speed.

[0166] 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).

[0167] If questions (A) through (E) above are all false, we can conclude that the orbital velocity can be increased.

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

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

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

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

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

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

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

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

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

[0177] 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 speed or delta V Δv can vary depending on where in the trajectory the speed transition algorithm of the present invention is applied. This is because the speed reference v(t) depends on where in the trajectory the speed reference is taken, i.e., the value of the speed trajectory at the trajectory point. Preferably, delta V is based on the difference between the desired final speed reference and either the initial or terminal point speed reference of one or more trajectory segments.

[0178] The process begins by initializing the output list of trajectory segments and determining if conditions are right to start the velocity transition algorithm.

[0179] 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:

[0180] a) The trajectory starts with zero trajectory acceleration a0 and already reaches the desired final velocity v f Mathematically, this is given by the condition

[0181]

number

[0182] 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

[0183]

number

[0184] 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

[0185]

number

[0186] 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:

[0187] 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:

[0188]

number

[0189] 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 additional trajectory segments can be generated to return to the original point. Mathematically, this null condition is given by:

[0190]

number

[0191] 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:

[0192]

number

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

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

[0195] [Table 1]

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

[0197] 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."

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

[0199] -Trajectory segment duration is T1=|a max -|a0|| / J red is given by:

[0200] By applying the manipulated variable jerk J, the terminal point trajectory acceleration a E1 is a E1 =sign(a0)×a max is given by:

[0201] 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

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

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

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

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

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

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

[0208] -Trajectory segment duration is

[0209]

number

[0210] is given by:

[0211] 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

[0212] 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).

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

[0214] 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

[0215] 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 it may overshoot v (if the third check determines that it does), does not mean that the trajectory 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.

[0216] 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

[0217] 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

[0218] 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

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

[0220] - Orbital segment duration is T3=|a0| / J dec is given by:

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

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

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

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

[0225] Example 1 Consider the following example input value:

[0226] Initial orbital acceleration a0=1.0 Command acceleration a max =2.0 Initial speed v0=1.0 Desired final velocity v f =1.1

[0227]

number

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

[0229] 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

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

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

[0232] 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).

[0233] 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

[0234]

number

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

[0236] →The end point of this segment is a E (Terminal point acceleration)=-2.0, v E (Terminal velocity)=0.5.

[0237] →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.

[0238] →Δv=v f -v E Recalculate = 3.0 - (-0.5) = 2.5.

[0239] →Reassign a0=-2.0 and v0=0.5.

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

[0241] →The end point of this segment has an acceleration of a E =0, v E =-1.5.

[0242] →Δv=v f -v E Recalculate =3.0-(-1.5)=4.5.

[0243] →Reassign a0=0 and v0=-1.5.

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

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

[0246] 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).

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

[0248] [Table 2]

[0249] 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

[0250]

number

[0251] 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:

[0252]

number

[0253] It is.

[0254]

number

[0255] When we solve for

[0256]

number

[0257] It becomes.

[0258] Therefore, the magnitude of the peak acceleration

[0259]

number

[0260] can be obtained by taking the square root of

[0261]

number

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

[0263] A) The magnitude of the initial acceleration a0 is the peak acceleration a peak corresponds to the size of

[0264] 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).

[0265] 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

[0266]

number

[0267] 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).

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

[0269] 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

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

[0271] 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

[0272] The duration of the first orbital segment is T=|a peak -a0| / J inc It is.

[0273] 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).

[0274] 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 )

[0275] For the second trajectory segment 530b: The value of the manipulated variable (jerk) is J=S×J dec It is.

[0276] The duration of the second orbital segment is T=|a peak | / J dec It is.

[0277] 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).

[0278] C) The magnitude of the peak acceleration is the maximum acceleration a max is larger than the size of

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

[0280] a max The value of the change in velocity due to the decrease in acceleration from

[0281]

number

[0282] To obtain a in Equation 21, peak =a0=a max It is calculated by substituting

[0283] This is shown in Figure 30. const The length (in time) of the constant acceleration phase, denoted as , can also be calculated as follows:

[0284]

number

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

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

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

[0288] The duration of the orbital segment is

[0289]

number

[0290] is given by:

[0291] 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).

[0292] ​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 )

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

[0294] The duration of the segment is T2 = T const is given by:

[0295] 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

[0296] 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 )

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

[0298] The duration of the orbital segment is

[0299]

number

[0300] is given by:

[0301] 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).

[0302] 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).

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

[0304] Duration T const For the first trajectory segment 540 spanning The value of the manipulated variable (jerk) used is zero.

[0305] The duration of the segment is T2=T const is given by:

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

[0307] For a second trajectory segment 542 of duration T3: The value of the manipulated variable (jerk) used is j=-S×j dec It is.

[0308] The duration of the orbital segment is

[0309]

number

[0310] is given by:

[0311] 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).

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

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

[0314] 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

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

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

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

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

[0319] 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 The root-finding algorithm calculates the commanded velocity v such that the trajectory completes a point-to-point move from a given initial starting point to a given end point without overshooting or undershooting. 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:

[0320] 1) From the initial velocity v0 to the desired peak velocity (say v A ) first speed transition.

[0321] 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

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

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

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

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

[0326] The inputs to the forced deceleration algorithm are shown in Table 3 below.

[0327] [Table 3]

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

[0329] 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:

[0330] The value of the manipulated variable (jerk) used is J=J4.

[0331] Segment duration is

[0332]

number

[0333] is given by:

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

[0335] 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 ).

[0336] 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

[0337] For example, one trajectory segment is generated as follows:

[0338] The value of the manipulated variable (jerk) used is J=J4.

[0339] Segment duration is

[0340]

number

[0341] is given by:

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

[0343] 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).

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

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

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

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

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

[0349] 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

[0350] 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:

[0351] A) First question: Is the acceleration excessive? No.

[0352] B) Second question: The orbit is v f Are you accelerating away from it? Yes.

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

[0354] 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).

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

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

[0357] 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 needs to be reduced to zero to satisfy. In other words, this portion of the orbit covered by the three orbital segments 1, 2, and 3 looks like a trapezoid.

[0358] 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 )

[0359] 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 orbit 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 )

[0360] The third, 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 materials handling device for lifting and moving one or more stacked containers in a storage system, said storage system comprising a grid framework structure supporting a plurality of tracks arranged in a grid pattern to define a grid structure over said stack of one or more containers, said grid pattern comprising a plurality of grid cells, each of said stacks of one or more containers located within the footprint of one grid cell, said materials handling device comprising: i) a wheel assembly including a first pair of wheels and a second pair of wheels, the wheel assembly being driven by a drive mechanism; ii) a lifting device for lifting a storage container from one of said stacks of containers into a container receiving space of said loading device; iii) position detection means for determining the position of the loading equipment relative to the grid structure; iv) a controller in communication with said position sensing means and said drive mechanism, a) determining a feedforward torque demand for said wheel assembly at a given time from a predetermined motion control profile; b) compensating said feedforward torque demand from the measured position of said loading device on said grid structure at said given time to calculate a torque demand of said wheel assembly; a control device configured to control movement of the loading equipment from a current position on the grid structure to a command position by A loading and unloading device comprising: 2. The control device comprises: i) determining a feedback torque demand for the wheel assembly from the measured position of the loading device; ii) combining the feedforward torque demand with the feedback torque demand to calculate the torque demand of the wheel assembly. Load handling apparatus according to claim 1 , configured to compensate for the feedforward torque demand by 3. A loading apparatus as described in claim 1 or 2, wherein the predetermined motion control profile comprises a series of one or more trajectory segments, each of the one or more trajectory segments being a portion of the motion control profile that defines at least one of a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time. 4. A loading apparatus as claimed in claim 3, wherein the feedforward torque demand is determined from the magnitude of the acceleration reference and / or the magnitude of the velocity reference. 5. A loading apparatus as claimed in claim 3 or 4, wherein any one of the jerk reference, and / or the acceleration reference, and / or the velocity reference, and / or the position reference is stored in a look-up table. 6. A loading apparatus as claimed in any one of claims 3 to 5, wherein the feedback torque demand is determined from a comparison of the magnitude of the position reference derived from the predetermined motion control profile and the measured position of the loading apparatus on the grid structure at the given time. 7. The loading apparatus of claim 6, wherein the controller is configured to perform a PI or PID calculation on the difference between the magnitude of a track position reference and the measured position. 8. A loading and unloading device as described in any one of claims 1 to 7, wherein each wheel of the first wheel pair and the second wheel pair is configured to be independently driven by the drive mechanism. 9. The load handling apparatus of claim 8, wherein each wheel of the wheel assembly is provided with a hub motor. 10. Load handling apparatus according to any one of claims 1 to 9, wherein the drive means is configured to at least partially drive at least one of the wheels of the wheel assembly to meet the calculated torque demand. 11. Load handling apparatus according to any one of claims 1 to 10, wherein the drive means is configured to at least partially brake at least one of the wheels of the wheel assembly to meet the calculated torque demand. 12. Load handling apparatus according to any one of claims 8 to 11, wherein the controller is configured to share the calculated torque demand between each wheel of the wheel assembly. 13. The load handling apparatus of claim 12, wherein the controller is configured to share the calculated torque demand between the first wheel pair and the second wheel pair of the wheel assembly. 14. The load handling apparatus of claim 13, wherein the controller is configured to vary a proportion of the calculated torque demand provided by the first pair of wheels and the second pair of wheels. 15. A loading device as described in claim 13 or 14, wherein the control device is configured to transmit a greater proportion of the calculated torque demand to the first pair of wheels when the first pair of wheels is rearward of the second pair of wheels in the direction of travel of the loading device on the grid structure. 16. A loading device as claimed in claim 14 or 15, wherein the control device is configured to transmit a greater proportion of the calculated torque demand to the second pair of wheels when the second pair of wheels is forward of the first pair of wheels in the direction of travel of the loading device on the grid structure. 17. The load handling apparatus of any one of claims 14 to 16, wherein the controller is configured to share the calculated torque demand between the first wheel pair and the second wheel pair of the wheel assembly in a ratio based on a magnitude of a track acceleration reference derived from the predetermined motion control profile. 18. A loading device as claimed in any one of claims 12 to 17, further comprising means for measuring the kinematic dynamics of the loading device on the grid structure. 19. A load handling apparatus as claimed in claim 18, wherein the control device is configured to determine the kinematic dynamic state of the load handling apparatus on the grid structure from position measurement means. 20. A loading device as claimed in claim 18 or 19, wherein the kinetic state is velocity. 21. A load handling apparatus as claimed in any one of claims 18 to 20, further comprising a drive wheel encoder coupled to each of the wheels of the wheel assembly for measuring the speed of each of the wheels of the wheel assembly. 22. The controller in communication with the drive wheel encoder comprises: i) comparing the measured speed of each of the wheels of the wheel assembly with the kinematic conditions of the load handling apparatus; ii) in response to the measured speed of any one of the wheels of the wheel assembly being substantially different from the kinematic condition of the load handling apparatus on the grid structure; a) adjusting the calculated shared torque demand for other wheels such that the kinematic state of the loading device remains substantially unchanged; and / or 22. Load handling apparatus according to claim 21, configured to: b) redistribute the calculated torque demand to each of the wheels of the wheel assembly. 23. The load handling apparatus of claim 21 or 22, wherein the controller in communication with the drive wheel encoder is configured to, in response to a measured speed of any one of the wheels of the wheel assembly being substantially greater than the kinematic dynamic condition of the load handling apparatus, remove the calculated shared torque demand from any one of the wheels of the wheel assembly until the speed of the wheel is substantially equal to the kinematic dynamic condition of the load handling apparatus. 24. A loading and unloading device as claimed in any one of claims 1 to 23, wherein the position detection means comprises a first position detection means and a second position detection means, the first position detection means configured to detect a position of the loading and unloading device in a first direction on the grid structure, and the second position detection means configured to detect a position of the loading and unloading device in a second direction on the grid structure, the first direction intersecting the second direction. 25. A loading apparatus as claimed in any one of claims 1 to 24, wherein the position detection means comprises an encoder coupled to a position wheel rotatably mounted on an arm such that rotation of the position wheel is detected by the encoder, the arm being biased downwardly to bias the position wheel into engagement with the grid structure. 26. A method for manufacturing a grid pattern having a plurality of grid spaces or cells, the method comprising: forming a grid pattern including a first set of tracks in a substantially horizontal plane and a second set of tracks running intersecting the first set of tracks; a plurality of stacks of containers positioned beneath the first set of tracks and the second set of tracks, each stack of containers occupying one grid space or grid cell; 26. A loading apparatus according to any one of claims 1 to 25, arranged to travel along the first set of tracks and the second set of tracks over the grid spaces or grid cells such that when positioned over a stack of containers occupying a grid space or grid cell, the lifting device is configured to lift at least one container from the stack of containers. A storage system comprising:

Claims

1. 1. A material handling apparatus for lifting and moving one or more stacked containers in a storage system, the storage system comprising a grid framework structure supporting a plurality of tracks arranged in a grid pattern to define a grid structure over the stack of one or more containers, the grid pattern comprising a plurality of grid cells, each of the stacks of one or more containers located within the footprint of one grid cell, the material handling apparatus comprising: i) a wheel assembly including a first pair of wheels and a second pair of wheels, the wheel assembly being driven by a drive mechanism; ii) a lifting device for lifting a storage container from one of said stacks of containers into a container receiving space of said loading device; iii) position sensing means for determining the position of the loading equipment relative to the grid structure; iv) a controller in communication with said position sensing means and said drive mechanism, a) determining a feedforward torque demand for said wheel assembly at a given time from a predetermined motion control profile; b) compensating said feedforward torque demand from the measured position of said loading device on said grid structure at said given time to calculate a torque demand of said wheel assembly. a control device configured to control movement of the loading equipment from a current position on the grid structure to a command position by Equipped with each wheel of the first pair of wheels and the second pair of wheels is configured to be independently driven by the drive mechanism; the controller is configured to share the calculated torque demand between each wheel of the wheel assembly; the controller is configured to share the calculated torque demand between the first pair of wheels and the second pair of wheels of the wheel assembly; The controller is configured to vary a proportion of the calculated torque demand provided by the first pair of wheels and the second pair of wheels; the controller is configured to share the calculated torque demand between the first wheel pair and the second wheel pair of the wheel assembly in a ratio of torque demand based on a magnitude of a trajectory acceleration reference derived from the predetermined motion control profile. Loading equipment.

2. The control device includes: i) determining a feedback torque demand for the wheel assembly from the measured position of the loading device; ii) combining said feedforward torque demand with said feedback torque demand to calculate said torque demand of said wheel assembly. Load handling apparatus according to claim 1 , configured to compensate for the feedforward torque demand by

3. 3. Load handling apparatus according to claim 1 or 2, wherein the predetermined motion control profile comprises a series of one or more trajectory segments, each of the one or more trajectory segments being a portion of the motion control profile defining at least one of a jerk reference, an acceleration reference, a velocity reference, and a position reference as a function of time.

4. Load handling apparatus according to claim 3 , wherein the feedforward torque demand is determined from the magnitude of the acceleration reference and / or the magnitude of the velocity reference.

5. 5. Load handling apparatus according to claim 3 or 4, wherein any one of the jerk reference, and / or the acceleration reference, and / or the velocity reference, and / or the position reference is stored in a look-up table.

6. 6. A load handling apparatus as claimed in any one of claims 3 to 5 dependent on claim 2, wherein the feedback torque demand is determined from a comparison of the magnitude of the position reference derived from the predetermined motion control profile and the measured position of the load handling apparatus on the grid structure at the given time.

7. 7. Load handling apparatus according to claim 6, wherein the controller is configured to perform a PI or PID calculation on the difference between a magnitude of a track position reference and the measured position.

8. 2. The load handling apparatus of claim 1, wherein each wheel of the wheel assembly includes a hub motor.

9. 9. Load handling apparatus according to claim 1, wherein the drive mechanism is configured to at least partially drive at least one of the wheels of the wheel assembly to meet the calculated torque demand.

10. 10. Load handling apparatus according to any one of claims 1 to 9, wherein the drive mechanism is configured to at least partially brake at least one of the wheels of the wheel assembly to meet the calculated torque demand.

11. 2. The load handling device of claim 1, wherein the controller is configured to transmit a greater proportion of the calculated torque demand to the first pair of wheels when the first pair of wheels is rearward of the second pair of wheels in a direction of travel of the load handling device on the grid structure.

12. 12. A loading device as claimed in claim 1 or 11, wherein the controller is configured to transmit a greater proportion of the calculated torque demand to the second pair of wheels when the second pair of wheels is forward of the first pair of wheels in a direction of travel of the loading device on the grid structure.

13. 13. A load handling apparatus as claimed in claim 1, 11 or 12, further comprising means for measuring the kinematic dynamics of the load handling apparatus on the grid structure.

14. 14. A load handling apparatus according to claim 13, wherein the controller is configured to determine the kinematic dynamic state of the load handling apparatus on the grid structure from position measurement means.

15. 15. Load handling apparatus according to claim 13 or 14, wherein the kinetic state is velocity.

16. 16. Load handling apparatus according to any one of claims 13 to 15, further comprising a drive wheel encoder coupled to each of the wheels of the wheel assembly for measuring the speed of each of the wheels of the wheel assembly.

17. The controller in communication with the drive wheel encoder, i) comparing the measured speed of each of the wheels of the wheel assembly with the kinematic conditions of the load handling apparatus; ii) in response to the measured speed of any one of the wheels of the wheel assembly being substantially different from the kinematic condition of the load handling apparatus on the grid structure; a) adjusting the calculated shared torque demand for other wheels such that the kinematic state of the load handling device remains substantially unchanged; and / or b) redistributing the calculated torque demand to each of the wheels of the wheel assembly.

17. Load handling apparatus according to claim 16, configured to:

18. 18. The load handling apparatus of claim 16 or 17, wherein the controller in communication with the drive wheel encoder is configured to, in response to a measured speed of any one of the wheels of the wheel assembly being substantially greater than the kinematic condition of the load handling apparatus, remove the calculated shared torque demand from any one of the wheels of the wheel assembly until the speed of the wheel is substantially equal to the kinematic condition of the load handling apparatus.

19. 19. A loading device as claimed in any one of claims 1 to 18, wherein the position detection means comprises a first position detection means and a second position detection means, the first position detection means configured to detect a position of the loading device in a first direction on the grid structure and the second position detection means configured to detect a position of the loading device in a second direction on the grid structure, the first direction intersecting the second direction.

20. 20. Load handling apparatus according to any one of claims 1 to 19, wherein the position detection means comprises an encoder coupled to a position wheel rotatably mounted on an arm such that rotation of the position wheel is detected by the encoder, the arm being biased downwardly to bias the position wheel into engagement with the grid structure.

21. a first set of tracks in a substantially horizontal plane and a second set of tracks running intersecting the first set of tracks, the first set of tracks defining a grid pattern having a plurality of grid spaces or grid cells; a plurality of stacks of containers positioned beneath the first set of tracks and the second set of tracks, each stack of containers occupying one grid space or grid cell; 21. A loading apparatus according to any one of claims 1 to 20, arranged to travel along the first set of tracks and the second set of tracks over the plurality of grid spaces or grid cells such that when positioned over a stack of containers occupying a grid space or grid cell, the lifting device is configured to lift at least one container from the stack of containers. A storage system comprising:

Images