CN117043702A - Motion control of robotic load handling device - Google Patents
Motion control of robotic load handling device Download PDFInfo
- Publication number
- CN117043702A CN117043702A CN202180094510.8A CN202180094510A CN117043702A CN 117043702 A CN117043702 A CN 117043702A CN 202180094510 A CN202180094510 A CN 202180094510A CN 117043702 A CN117043702 A CN 117043702A
- Authority
- CN
- China
- Prior art keywords
- load handling
- speed
- acceleration
- track
- handling apparatus
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000003860 storage Methods 0.000 claims abstract description 77
- 230000007246 mechanism Effects 0.000 claims abstract description 61
- 238000001514 detection method Methods 0.000 claims abstract description 20
- 238000004891 communication Methods 0.000 claims abstract description 17
- 230000001133 acceleration Effects 0.000 claims description 445
- 230000036461 convulsion Effects 0.000 claims description 148
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000004044 response Effects 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 7
- 238000004422 calculation algorithm Methods 0.000 description 178
- 230000009466 transformation Effects 0.000 description 127
- 238000000034 method Methods 0.000 description 51
- 230000006870 function Effects 0.000 description 38
- 230000009467 reduction Effects 0.000 description 34
- 230000008859 change Effects 0.000 description 29
- 230000008569 process Effects 0.000 description 26
- 230000007423 decrease Effects 0.000 description 24
- 230000007704 transition Effects 0.000 description 20
- 230000036772 blood pressure Effects 0.000 description 19
- 230000002829 reductive effect Effects 0.000 description 17
- 238000010586 diagram Methods 0.000 description 16
- 230000000246 remedial effect Effects 0.000 description 9
- 238000006243 chemical reaction Methods 0.000 description 8
- 230000003247 decreasing effect Effects 0.000 description 7
- 230000000694 effects Effects 0.000 description 7
- 230000001131 transforming effect Effects 0.000 description 7
- 230000009471 action Effects 0.000 description 6
- 238000005096 rolling process Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 235000009508 confectionery Nutrition 0.000 description 4
- 230000001419 dependent effect Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000007689 inspection Methods 0.000 description 4
- 230000002441 reversible effect Effects 0.000 description 4
- 238000009826 distribution Methods 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000010355 oscillation Effects 0.000 description 3
- 230000007717 exclusion Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000001141 propulsive effect Effects 0.000 description 2
- 230000008929 regeneration Effects 0.000 description 2
- 238000011069 regeneration method Methods 0.000 description 2
- 230000000284 resting effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 239000003990 capacitor Substances 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000000151 deposition Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 235000013305 food Nutrition 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 229910052751 metal Inorganic materials 0.000 description 1
- 239000002184 metal Substances 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000013021 overheating Methods 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
- 230000003595 spectral effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Landscapes
- Numerical Control (AREA)
- Control Of Position Or Direction (AREA)
- Warehouses Or Storage Devices (AREA)
Abstract
A load handling apparatus for lifting and moving one or more containers stacked in a storage system comprising a grid framework structure supporting a plurality of rails arranged in a grid pattern to define a grid structure over a stack of one or more containers, the grid pattern comprising a plurality of grid cells and each of the stack of one or more containers being located within a footprint of only a single grid cell, the load handling apparatus comprising: i) A wheel assembly including first and second pairs of wheels, the wheel assembly being driven by a drive mechanism; ii) lifting means for lifting the storage container from one of the stacks of containers to a container receiving space of the load handling apparatus; iii) Position detection means for measuring the position of the load handling device relative to the grid structure; iv) a controller in communication with the position detection tool and the drive mechanism, the controller configured to control movement of the load handling apparatus over the grid structure from a current position to a commanded position: a) Determining a feed-forward torque demand on the wheel assembly at a given time from a preset motion control profile; b) The feed forward torque demand is compensated with the measured position of the load handling device on the grid structure at a given time to calculate the torque demand on the wheel assembly.
Description
Technical Field
The present invention relates to the field of storage or fulfillment systems in which stacks of bins or containers are placed within a grid framework structure, and more particularly to the generation and control of motion of load handling devices that operate to move one or more containers stored in the storage or fulfillment system.
Background
Storage systems comprise a three-dimensional storage grid structure in which storage containers/bins are stacked on top of each other, as is well known. PCT patent publication No. WO2015/185628A (Ocado) describes a known storage and fulfillment system in which stacks of boxes or containers are placed within a grid framework structure. The boxes or containers are accessible by load handling devices (also referred to as "robots") operating on rails located at the top of the grid framework. A system of this type is schematically illustrated by figures 1 to 3 of the accompanying drawings.
As shown in fig. 1 and 2, stackable containers, referred to as storage containers or boxes 10, are stacked upon one another to form a stack 12. The stacks 12 are placed in a grid framework structure 14 in a warehouse or production environment. The grid framework structure is composed of a plurality of storage columns or grid columns. Each grid in the grid framework structure has at least one grid column for storing a stack of containers. Fig. 1 is a schematic perspective view of a grid framework structure 14, and fig. 2 is a top view of a stack 12 of boxes 10 placed in the framework structure 14. Each bin 10 typically holds a plurality of product items (not shown), which may be the same or different product types within the bin 10, depending on the application.
The grid framework structure 14 includes a plurality of upright members 16 that support horizontal members 18, 20. The first set of parallel horizontal grid members 18 is arranged perpendicular to the second set of parallel horizontal members 20 to form a plurality of horizontal grid structures 15 supported by the upright members 16 in a grid pattern. The members 16, 18, 20 are typically made of metal. The bins 10 are stacked between the members 16, 18, 20 of the grid framework structure 14, such that the grid framework structure 14 prevents the stacks 12 of bins 10 from moving horizontally and directs the bins 10 to move vertically.
The top layer of the grid framework structure 14 includes a grid or grid structure 15 including rails 22 arranged in a grid pattern on top of the stack 12. Further, referring to fig. 3, a track or rail 22 guides a plurality of load handling devices 30. The first set 22a of parallel tracks 22 directs the robotic load handling device 30 to move in a first direction (e.g., X-direction) at the top of the grid frame structure 14, while the second set 22b of parallel tracks 22 arranged perpendicular to the first set 22a directs the load handling device 30 to move in a second direction (e.g., Y-direction) perpendicular to the first direction. In this manner, the rails 22 allow the robotic load handling apparatus 30 to move laterally in two dimensions in a horizontal X-Y plane so that the load handling apparatus 30 can be moved to an upper position of either stack 12.
PCT patent publication No. WO2015/019055 (Ocado) (incorporated herein by reference) describes that the known load handling apparatus 30 shown in fig. 4 and 5 comprises a carrier body 32, wherein each load handling apparatus 30 covers only one grid space of the grid framework structure 14. Here, the load handling device 30 includes a wheel assembly including a first set of wheels 34 and a second set of wheels 36, the first set of wheels 34 being comprised of pairs of wheels at the front of the carrier body 32 and pairs of wheels 34 at the rear of the carrier body 32 for engaging a first set of tracks or rails to guide the movement of the device in a first direction, the second set of wheels 36 being comprised of pairs of wheels 36 on each side of the carrier body 32 for engaging a second set of tracks or rails to guide the movement of the device in a second direction. Each set of wheels is driven to enable the carrier to move along the track in X and Y directions, respectively. One or both sets of wheels may be moved vertically to lift each set of wheels off of its respective track, allowing the carrier to move in a desired direction on the grid.
The load handling device 30 is equipped with a lifting device or lifting mechanism to lift the storage container from above. The hoisting mechanism comprises a winch rope or cable 38 wound on a reel or spool (not shown) and a gripping device 39. The lifting means comprises a set of lifting lines 38 extending in a vertical direction and connected at or near the four corners of the lifting frame 39, also called gripping means (one line near each of the four corners of the gripping means) for releasable connection to the storage container 10. The gripping means 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 push the lifting device.
The wheels 34, 36 are arranged around the periphery of a cavity or recess in the lower part, also referred to as a container receiving recess or container receiving space 40. As shown in fig. 5 (a and b), the recess is sized to accommodate the container 10 when the container 10 is lifted by the lifting mechanism. While in the recess, the container is lifted off the underlying track so that the carrier can be moved laterally to different positions. Upon reaching a target location (e.g., another stack, access point in a storage system, or conveyor belt), the bins or containers may be moved down from the container receiving section and released from the gripping device.
Typically, one or more load handling devices operating remotely on the grid structure are configured to receive instructions from the master controller to retrieve storage containers from specific storage locations within the grid framework structure. Wireless communications and networks may be used to provide a communication infrastructure from a master controller to one or more load handling devices operating on a grid structure via one or more base stations. A controller in the load handling apparatus responsive to receiving the instructions is configured to control the various drive mechanisms to control movement of the load handling apparatus. For example, the load handling device may be instructed to retrieve containers from the storage columns at specific locations on the grid structure. The instructions may include various movements in the X-Y direction on the grid structure. Once in the storage column, the lifting mechanism operates to grasp and lift the storage container to a container receiving space within the body of the load handling apparatus, where the container is then transported to another location on the grid structure, commonly referred to as a discharge. The storage container is lowered to a suitable pick-up station to allow removal of the items from the storage container. The movement of the load handling device over the grid structure also includes the load handling device being instructed to move to a charging station generally located at the periphery of the grid structure.
To enable the controller to determine the position of the load handling apparatus relative to the grid structure, each load handling apparatus may be provided with one or more Radio Frequency Identification (RFID) tag readers or scanners, and a plurality of RFID tags may be provided on 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 load handling device moves across the grid structure, each RFID tag reader of the load handling device reads signals from one or more RFID tags fixed at different locations on the grid structure. Typically, RFID tags will be affixed at the rail intersections or cross-gates of each grid cell.
In order to maneuver the load handling devices from one destination to another on a grid structure, each of the load handling devices is equipped with a motor for driving the wheels of the load handling device. The rotation of the wheel may be driven by one or more belts attached to the wheel or by a motor incorporated into the wheel alone. Typically, for a single unit load handling apparatus where the footprint of the load handling apparatus occupies a single grid unit, the motor for the drive wheel would be incorporated into the wheel due to the limited space available for the carrier body. The wheels of the single unit load handling device would be driven by the in-wheel motor. Each in-wheel motor includes an outer gear including a plurality of permanent magnets arranged to rotate about a hub including coils forming an inner stator.
One of the main drawbacks of single unit load handling devices is the stability of the load handling device as it moves on the track, which creates challenges, especially when the load handling device accelerates/decelerates on the track. High accelerations or decelerations on the rails may cause the load handling apparatus to oscillate or even tip over the grid structure. Typically, the rotation of the wheels of the load handling apparatus will be controlled individually so that the load handling apparatus is driven in a straight line on the track without yawing or deviating from its intended path.
Because of the similar size of the wheels, the load handling devices typically rotate at the same speed as they move on the track. However, there are several factors that may affect the smoothness of the movement of the load handling apparatus on the track, making it possible for it to yaw or be driven at an angle deviating from the intended one. These factors may include one or more wheels slipping, sliding, and/or loading, all of which may cause one or more wheels to rotate at different speeds. In addition, sudden changes in acceleration of the load handling apparatus on the track, also known as jerk (jerk), can affect the smoothness of the movement of the load handling apparatus on the track. When carrying fragile or damaged items, such as food items, abrupt movement of such load handling devices upon acceleration or deceleration may cause the items to be thrown in the storage container.
European patent No. 3535633 (Autostore Technology AS) teaches a method and controller for controlling the movement of a robot from a start position to a stop position, the robot having a pair of wheels with a drive means controlled by a local controller. Here, the master controller is configured to set an individual speed drive sequence 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 sequence is transmitted to a local controller controlling the pairs of wheels, which in turn controls the acceleration and deceleration of each wheel by means of the respective drive means. In European patent No. 3535633 (Autostore Technology AS), the local controller ensures that the speeds of the paired wheels remain synchronized. Thus, the teaching of European patent No. 3535633 (Autostore Technology AS) focuses on controlling the respective speeds of the wheels and ensuring the speed synchronisation of the pairs of wheels.
It is therefore an object of the present invention to provide a method and apparatus for controlling the movement of a load handling apparatus from a start position to a stop position to ensure smooth movement of the load handling apparatus when changing different speeds on a grid structure.
The present application claims priority from uk patent application nos. GB2020680.1, GB2020668.6, GB2020681.9, GB2020684.3, all filed on 12 months 24 2020, the contents of which are incorporated herein by reference.
Disclosure of Invention
The present application provides a load handling apparatus for lifting and moving one or more containers stacked in a storage system, the storage system including a grid framework structure supporting a plurality of rails arranged in a grid pattern to define a grid structure over a stack of one or more containers, the grid pattern including a plurality of grid cells, and each of the stack of one or more containers being located within a footprint of only a single grid cell, the load handling apparatus comprising:
i) A wheel assembly including first and second pairs of wheels; the wheel assembly is driven by a drive mechanism;
ii) lifting means for lifting the storage container from one of the stacks of containers to the container receiving space of the load handling apparatus;
iii) Position detection means for measuring the position of the load handling device relative to the grid structure;
iv) a controller in communication with the position detection tool and the drive mechanism, the controller configured to control movement of the load handling apparatus over the grid structure from the current position to the commanded position by:
a) Determining a feed-forward torque demand on the wheel assembly at a given time according to a preset motion control profile;
b) The feed forward torque demand is compensated with measured positions of the load handling device on the grid structure from the position detection tool at a given time to calculate the torque demand on the wheel assembly.
According to the invention, the controller in communication with the position detection means and the drive mechanism comprises a motion control system to control the position and motion of the load handling device on the grid structure under the guidance of a preset motion control profile or trajectory. When the controller determines that the load handling apparatus must move to a new position or change its speed, the controller will calculate a position or speed trajectory, referred to as a preset motion control profile, for use in transitioning the load handling apparatus from the current position or speed trajectory to the target position or speed. The preset motion control profile defines the motion of the load handling apparatus on the grid structure including, but not limited to, the speed, acceleration and/or position over time as the load handling apparatus moves from the current position to the target state. The preset motion control profile includes one or more track segments in a sequence, the one or more track segments being portions of a track or 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.
Once this preset motion control profile is generated, the controller translates the motion control profile into appropriate control signals to cause the load handling device to move along the trajectory defined by the motion control profile. The preset motion control profile may optionally be based on two preferred motion control profiles, namely a trapezoidal or S-curve profile. In the trapezoidal and S-curve profiles, the individual track segments of the motion control profile are generated according to preset defined constraints, i.e. maximum speed, maximum acceleration, etc., wherein the defined constraints may correspond to mechanical limitations of the drive mechanism of the load handling device. Given these constraints and the desired command positions, the controller will generate a preset motion control profile for performing the desired movement. For a trapezoidal trajectory, the controller will calculate the motion control profile from three different phases, namely an acceleration phase, a constant velocity phase and a deceleration phase. Such a profile will produce a trapezoidal velocity profile. In the case of a trapezoidal profile, the acceleration may exhibit a discontinuity, so the manipulated variable (defined as the variable that is controlled, and therefore allowed to vary in a discontinuous manner) is the acceleration. Thus, the trapezoidal profile always accelerates or decelerates at the maximum defined acceleration rate. This results in a more "jerky" movement of the load handling apparatus over the grid structure.
The S-curve profile alters the trapezoidal profile to mitigate this "jerk" by adding an additional stage that allows a gentle transition between a constant (or zero) speed stage and a constant acceleration/deceleration stage, thereby providing a smoother motion. The manipulated variable (in the S-curve profile) that is allowed to vary in a discontinuous manner is "jerk" as compared to the trapezoidal profile. Thus, in addition to the preset constraints of maximum speed and maximum acceleration, the S-curve profile also requires additional "jerk" constraints.
In the present invention, a controller in communication with the position detection tool and the drive mechanism controls movement of the load handling apparatus from a current position to a commanded position on the grid structure by determining a feed forward torque demand on the wheel assembly from a preset motion control profile at a given time. Knowing the mass of the load handling device and the radius of each wheel of the wheel assembly, the feed-forward torque control can be calculated from the product of the mass and the acceleration, which is the derivative of the speed of the preset motion control profile, i.e. the track acceleration reference, from the preset motion control profile. The controller preferably compensates the feed forward torque demand by "real time" measurement of the position of the load handling device on the grid structure from the position detection tool to calculate the "real time" torque demand on the wheel assembly. Preferably, the feedback torque demand is calculated from the derivative of the position measurement as a function of time. More preferably, the controller is configured to compensate for the feedforward torque demand by:
i) Determining a feedback torque demand on the wheel assembly based on the measured position of the load handling device;
ii) calculating a torque demand on the wheel assembly in combination with the feedforward torque demand and the feedback torque demand.
Alternatively, the feedback torque demand is determined by comparing the magnitude of a position reference derived from a preset motion profile to a measured position of the load handling device on the grid structure at a given time. According to a specific embodiment of the invention, the controller compares the trajectory position reference from the preset motion control profile with the measured position of the load handling device on the grid structure. Preferably, the controller is configured to perform PI or PID calculations on the difference between the magnitude of the track position reference and the measured position. The difference provides a position error that is input to a PI or PID controller to calculate the compensating torque demand; the trajectory position reference derived from the motion control profile is the target position, while the signal from the position sensor is the measured position. The track position reference and the measured position are both combined in a PI or PID controller to provide the compensating torque demand. The compensation torque demand or the feedback torque demand is combined with the feedforward torque demand to provide a torque demand of the wheels to drive the load handling device. The compensating torque demand allows the load handling device to follow the "theoretical" motion control profile provided by the preset motion control profile.
Preferably, the feed forward torque demand is determined by the magnitude of the acceleration reference and/or the magnitude of the speed reference.
More preferably, any one of the jerk reference and/or the acceleration reference and/or the speed reference and/or the position reference is stored in a look-up table.
Preferably, the feed forward torque demand is obtained by summing three torque terms: acceleration feedforward torque demand, speed feedforward torque demand, and zero order feedforward torque demand. The acceleration feedforward torque demand is proportional to the acceleration reference, the speed feedforward torque demand is proportional to the speed reference, the zero order feedforward torque demand is a constant, with a fixed value and sign of the track speed reference.
Once the torque demand is calculated, the controller converts the torque demand into an appropriate control signal to cause the load handling device to move on the grid structure via a trajectory defined by the preset motion control profile. Preferably, each of the first and second pairs of wheels is configured to be independently driven by a drive mechanism. More preferably, each wheel in the wheel assembly comprises an in-wheel motor. Accordingly, a control signal for moving the load handling apparatus is transmitted to the in-wheel motor. However, the present invention is not limited to a drive mechanism comprising an in-wheel motor for driving a wheel of a wheel assembly, any drive mechanism known in the art for driving a wheel is suitable for use in the present invention. For example, the wheels may be driven by one or more drive motors via a suitable pulley and/or gear arrangement. However, the use of the in-wheel motors to drive each wheel of the load handling apparatus has the advantage that the torque of each wheel of the load handling apparatus can be controlled by controlling the control signals sent to each in-wheel motor.
Preferably, the driving tool is configured to at least partially drive at least one wheel of the wheel assembly to meet the calculated torque demand. Preferably, the driving tool is configured to at least partially brake at least one wheel of the wheel assembly to meet the calculated torque demand.
Preferably, the controller is configured to share the calculated torque demand between each wheel of the wheel assembly. For the purposes of the present invention, the term "sharing" also includes allocation, that is, the calculated torque demand is allocated to each wheel of the wheel assembly. Optionally, the controller is configured to share the calculated torque demand between the first and second pairs of wheels of the wheel assembly. In one embodiment of the invention, torque demand is divided into front and rear pairs of wheels depending on the direction of travel of the load handling device on the grid structure. The front pair of wheels may be driven synchronously and are assumed to be driven about a front "axle". Likewise, the rear pair of wheels may be driven synchronously and are assumed to be driven about the rear "axle". In certain embodiments of the present invention, torque demand is divided into front and rear "axles" to account for any weight transfer of the load handling device on the grid structure.
Optionally, the controller is configured to vary the ratio of the calculated torque demands provided by the first and second pairs of wheels. Preferably, the controller is configured to transfer a greater proportion of the calculated torque demand onto the first pair of wheels when the first pair of wheels is behind the second pair of wheels in the direction of travel of the load handling device on the grid structure. Preferably, the controller is configured to transfer a greater proportion of the calculated torque demand onto the second pair of wheels when the second pair of wheels is in front of the first pair of wheels in the direction of travel of the load handling device on the grid structure. For example, the amount of torque shared between the front and rear pairs of wheels may be varied and may depend on whether the load handling apparatus accelerates or decelerates on the grid structure in the forward direction. The first pair of wheels represents the front pair of wheels and the second pair of wheels represents the rear pair of wheels. When the load handling device accelerates across the grid structure, a greater proportion of the torque is distributed to the front pair of wheels, such that the front pair wheel drive mechanism has a greater proportion of the torque demand. Conversely, the torque demand is exchanged to the rear pair of wheels during deceleration of the load handling device on the grid structure.
The ratio of torque provided by the first and second pairs of wheels may be formulated based on the extent of the track acceleration reference derived from the preset motion control profile. Optionally, the controller is configured to share the calculated torque demand between the first and second pairs of wheels of the wheel assembly, the sharing ratio being based on a magnitude of the track acceleration reference derived from the preset motion control profile. The proportion of torque demand between the front and rear wheels may be proportional to the magnitude of the acceleration reference of the load handling device on the grid structure. For example, when the magnitude of the track acceleration reference according to the preset motion control profile is larger, more torque is allocated to the front pair of wheels, and conversely, when the magnitude of the acceleration is reduced or decelerated, more torque is allocated to the rear pair of wheels. Thus, the distribution of torque demand between the first and second sets of wheels varies in proportion, which is based on data associated with a track acceleration reference derived from a preset motion control profile.
Preferably, the ratio of the calculated torque demand between the first and second pairs of wheels of the wheel assembly is a function of the magnitude of the track acceleration reference.
The preset motion control profile is periodically updated during operation of the load handling device on the grid structure based on position measurements from the position detection tool. Preferably, the controller is configured to determine a movement state of the load handling apparatus on the grid structure from the position measurement tool. More preferably, the movement state is speed. Preferably, the speed of the load handling device relative to the grid structure is determined from a first derivative of a position measurement of the load handling device relative to the grid structure, the position measurement being a function of time. Preferably, the load handling device includes a wheel encoder coupled to each wheel of the wheel assembly for measuring the speed of each wheel of the wheel assembly. Comparing the movement state of the load handling device relative to the grid structure with the speed of at least one wheel of the wheel assembly, it can be determined whether any of the wheels of the wheel assembly is slipping on the grid structure or track, i.e. losing traction with the grid structure.
More preferably, the controller in communication with the drive wheel encoder is configured to:
i) Comparing the measured speed of each wheel of the wheel assembly with the motion state of the load handling apparatus; and
ii) in response to the measured speed of any one wheel of the wheel assembly being substantially different from the state of motion of the load handling apparatus on the grid structure, or:
a) Adjusting the calculated torque demand shared to the other wheels to maintain the motion state of the load handling apparatus substantially unchanged; and/or
b) The calculated torque demand is redistributed to each wheel of the wheel assembly.
More preferably, the controller in communication with the drive wheel encoders is configured to remove the shared calculated torque demand from any one of the wheels of the wheel assembly in response to the measured speed of any one of the wheels of the wheel assembly being substantially greater than the state of motion of the load handling device until the speed of that wheel is substantially equal to the state of motion of the load handling device on the grid structure.
There may be two situations in which the speed of at least one wheel is different from the state of motion of the load handling apparatus on the grid structure. The first is that at least one wheel of the wheel assembly loses traction and slips on the grid structure. The second case is an uneven distribution of torque demand between at least one wheel of the wheel assembly, resulting in at least one wheel rotating at a different speed than the other wheels, which mainly drive the load handling devices on the grid structure. In both cases, the controller is configured to adjust the calculated torque demand shared to any one of the wheels of the wheel assembly. Preferably, in response to the measured speed of any one wheel of the wheel assembly being substantially less than the state of motion of the load handling apparatus on the grid structure, the controller adjusts the calculated torque demand shared to the other wheels so that the state of motion of the load handling apparatus remains substantially unchanged and/or redistributes the calculated torque demand to each wheel of the wheel assembly. Alternatively, in response to the measured speed of any one of the wheels of the wheel assembly being substantially greater than the state of motion of the load handling device on the grid structure, the controller removes the shared calculated torque demand from any one of the wheels of the wheel assembly until the speed of that wheel is substantially equal to the state of motion of the load handling device on the grid structure. In this case, if any one wheel slips, torque will be removed from that wheel until the speed of that wheel catches up with the motion of the load handling device on the grid structure. The "remove" function may include stopping or switching control signals sent to the beater wheel.
As an alternative to the torque demand being a driving torque for driving the load handling device on the grid structure, the torque demand may be a braking or resistive torque applied to any one of the wheels of the wheel assembly, for example when the load handling device is decelerating on the grid structure. Thus, the torque demand may also provide a resistive or braking torque compared to the rotation of the drive wheel, i.e. the controller converts the preset motion control profile into a control signal instructing the load handling means to slow down the grid structure according to the preset motion control profile.
The wheel assembly of the present invention is arranged such that the wheel assembly comprises a first set of wheels arranged to move the load handling device in a first direction on the grid structure and a second set of wheels arranged to move the load handling device in a second direction on the grid structure, the first set of wheels being transverse to the second set of wheels. The first set of wheels includes pairs of wheels located forward and aft of the body of the load handling apparatus. The second set of wheels includes pairs of wheels on either side of the body of the load handling apparatus.
Each set of wheels is driven to move the carrier along the track in X and Y directions, respectively. One or both of the cluster wheels may be moved vertically to lift each cluster wheel off its respective track, thereby allowing the carrier to move in a desired direction on the grid.
Preferably, the position detection means comprises first position detection means configured to detect a position of the load handling apparatus on the grid structure in a first direction and second position detection means configured to detect a position of the load handling apparatus on the grid structure in a second direction, the first direction being transverse to the second direction.
Preferably, the position detection tool comprises an encoder coupled to a position wheel rotatably mounted on an arm so as to detect rotation of the position wheel by the encoder, the arm being biased downwardly so that the position wheel is biased into engagement with the grid structure. The position detection tool is also referred to as the "fifth" wheel simply because it is an additional wheel in the first or second set of wheels. The position wheel is biased toward the grid structure or track such that the load handling apparatus rotates the position wheel as it moves over the grid structure. An encoder coupled to the position wheel measures the rotational speed of the position wheel, which indicates the state of motion of the load handling apparatus on the grid structure.
The present invention provides a storage system comprising:
a first set of tracks and a second set of tracks transverse to the first set of tracks on a substantially horizontal plane to form a grid pattern comprising a plurality of grid spaces or grid cells;
A stack of a plurality of containers located below the first track set and the second track set, wherein each stack of containers occupies a single grid space or grid cell;
the load handling apparatus according to the invention is arranged to traverse a plurality of grid spaces or grid cells along a first and a second set of tracks such that when positioned over a stack of containers occupying the grid spaces or grid cells, the lifting means is configured to lift at least one container from said stack of containers.
Drawings
Further features and aspects of the present invention will become apparent from the following detailed description of illustrative embodiments thereof, which is to be read in connection with the accompanying drawings.
Fig. 1 is a schematic diagram of a grid framework structure according to a known system.
Fig. 2 is a schematic diagram in top view showing a stack of bins disposed within the frame structure of fig. 1.
FIG. 3 is a schematic diagram of a known storage system showing a load handling device operating on a grid framework structure.
Fig. 4 is a schematic perspective view of the load handling apparatus, showing the lifting apparatus gripping the container from above.
Fig. 5 (a) and 5 (b) are schematic cross-sectional views of the load handling apparatus of fig. 4, showing (a) a container received in a container receiving space of the load handling apparatus, and (b) a container receiving space of the load handling apparatus.
Fig. 6 is a schematic perspective view of a load handling apparatus on a portion of a grid structure according to an embodiment of the invention.
Fig. 7 is a schematic perspective view of a wheel of a load handling apparatus, showing a central rotational axis according to an embodiment of the invention.
Fig. 8 is an exploded view of an in-wheel motor for driving a wheel about a central rotational axis according to an embodiment of the present invention.
Fig. 9 is a wheel assembly control and interface architecture according to an embodiment of the present invention.
FIG. 10 is a schematic diagram of communication between a load handling device and a host controller via a network in accordance with an embodiment of the present invention.
Fig. 11 (a to c) are schematic diagrams of trapezoidal track profiles.
Fig. 12 (a to d) are schematic diagrams of S-curve track profiles.
FIG. 13 is a block diagram of a motion control system including a trajectory or motion profile generator, according to an embodiment of the invention.
Fig. 14 is a perspective view of a robotic load handling device including a position sensor according to an embodiment of the invention.
FIG. 15 is a flowchart of an embodiment of a method for generating a trajectory profile, according to an embodiment of the invention.
FIG. 16 is a block diagram illustrating the input and output of an exemplary motion profile generator according to an embodiment of the present invention.
FIG. 17 is a block diagram illustrating the input and output of an exemplary position controller according to an embodiment of the present invention.
FIG. 18 is a flowchart of an embodiment of a method for generating a trajectory profile, according to an embodiment of the invention.
Fig. 19 is a step of the forced deceleration in fig. 18.
Fig. 20 is a remedial step of gradually reducing the magnitude of the acceleration to the commanded acceleration in fig. 18.
FIG. 21 is a remedial step of FIG. 18 of gradually reducing the magnitude of the velocity to substantially zero.
Fig. 22 is a remedial step of fig. 18 of applying a return from the overshoot.
Fig. 23 is a remedial step in fig. 18 of gradually reducing the magnitude of acceleration to substantially zero and exercising return from overshoot.
FIG. 24 is a remedial step when the magnitude of the trace acceleration is greater than the commanded speed.
FIG. 25 is a remedial step when the magnitude of the current track speed is substantially equal to the commanded speed.
Fig. 26 is a remedial step when the magnitude of the current track speed is increased to the commanded speed.
Fig. 27 is a flowchart of an example method of a first speed change sub-algorithm in accordance with a specific embodiment of the present invention.
FIG. 28 is a flowchart of an example method of a second speed transformation sub-algorithm according to an embodiment of the present invention.
FIG. 29 is a schematic imaginary triangle depicting the magnitude of trace acceleration for successive velocity transitions, according to an embodiment of the invention.
FIG. 30 is a schematic hypothetical trapezoid depicting the magnitude of trace accelerations for a plurality of speed transforms, according to an embodiment of the present invention.
FIGS. 31 (a) - (d) are graphs showing the application of a speed transformation algorithm for different desired speeds according to embodiments of the present inventionv f A trace profile when the vehicle is traveling.
FIGS. 32 (a through c) are graphs for different peak speeds v when a root algorithm is applied in combination with a speed transformation algorithm, according to embodiments of the present invention Peak to peak A trace profile when the vehicle is traveling.
FIG. 33 is a plot of the objective function of the root algorithm versus the different peak velocities of FIG. 32, according to an embodiment of the present invention.
Fig. 34 is a flow chart of an example method of forcing a deceleration algorithm according to an embodiment of the invention.
Fig. 35 (a-c) are schematic diagrams of embodiments of trapezoidal profiles.
The line graph of fig. 36 shows how the average current of the hoist motor varies with the mass of the contents of the storage vessel lifted by the motor.
Detailed Description
The present invention has been devised for known features of storage systems, such as the grid framework structure and load handling apparatus described above with reference to fig. 1 to 5. The present invention is defined by a method and system for controlling the movement of a load handling device or "robot" over a grid structure.
The load handling apparatus 130 includes a carrier body 132 equipped with a lifting mechanism (not shown) including a winch or lifting mechanism to lift a storage container or case, also referred to as a case, from below. The hoisting mechanism comprises a winch tether wound on a reel or spool and a gripping device. The gripping means is configured to grip the top of the container to lift it from a stack of containers in a storage system of the type shown in figures 4 and 5.
The carrier body 132 includes an upper portion and a lower portion. The lower section includes a wheel assembly that includes two sets of wheels 134, 136 that run on rails at the top of the grid framework structure of the storage system. For purposes of explaining the present invention, the two sets of wheels are defined as a first set of wheels 134 and a second set of wheels 136. The first 134 and second 136 sets of wheels are disposed about the periphery of the load handling apparatus 130. Each of the first 134 and second 136 sets of wheels is disposed on opposite sides in the lower portion of the carrier body 132, including pairs of wheels on opposite sides of the carrier body, i.e., each of the first and second sets of wheels includes four wheels in total. In the particular embodiment of the invention shown in fig. 6, each of the first 134 and second 136 sets of wheels is rotatably mounted to a carrier frame in the lower portion of the carrier body 132 so that the load handling apparatus can move along the track or track in the X and Y directions, respectively. While particular embodiments of the present invention show paired wheels mounted to opposite sides of the carrier body, the present invention is not limited to load handling apparatus mounted to paired wheels on either side of the carrier body. Stability of the load handling apparatus may be achieved by at least one wheel disposed diagonally opposite each other on opposite sides of the carrier body as compared to a pair of wheels mounted on opposite sides of the carrier. Thus, the first and second sets of wheels each comprise at least one wheel mounted to opposite sides of the carrier body such that they are diagonally opposite each other.
In a particular embodiment of the invention, the first 134 and second 136 sets of wheels are arranged around a cavity or recess in the lower portion, referred to as the periphery of the container receiving recess (see fig. 4 and 5). As shown in fig. 5 (a and b), the recess is sized to receive a storage container or container 10 when the container 10 is lifted by the lifting mechanism. Although the specific embodiments describe a container receiving space arranged within the carrier body, as described for example in WO 2015/019055 (Ocado Innovation Limited), the carrier body may comprise a cantilever arm as taught in WO2019/238702 (Autostore Technology AS), in which case the container receiving space is located below the cantilever arm of the load handling apparatus. In this case, the gripping means are lifted by the cantilever arm so that the gripping means can engage the containers and lift them from the stack into the container receiving space below the cantilever arm.
The upper portion of the carrier body 132 may house most of the bulk components of the load handling apparatus. Typically, the upper portion of the vehicle body houses a drive mechanism for driving the lifting mechanism and an onboard rechargeable power source for providing power to 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 even a capacitor.
As shown in fig. 6, the grid structure 115 includes a first set of parallel grid members extending in a first direction and a second set of grid members extending in a second direction, which are arranged in a grid pattern including a plurality of cells. For the convenience of explanation of the present invention, the movement of the load handling apparatus in the first direction in the horizontal plane represents the movement in the X direction, and the movement in the second direction on the grid structure represents the movement in the Y direction. To allow one or more load handling devices to travel on the grid structure, a first set of grid members includes a first set of rails 122a and a second set of grid members includes a second rail 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). Alternatively, the first set of rails 122a is snap-fit to the first set of rail supports and the second set of rails 122b is snap-fit to the second set of rail supports. It is equally reasonable in the present invention that a plurality of rails 122a, 122b may be fused into the first and second sets of rail supports such that the grid members of the grid structure comprise rails and rail supports.
To change direction on the grid structure, thereby allowing the carrier or load handling apparatus to move in an orthogonal direction, each of the first 134 and second 136 sets of wheels is arranged to move vertically to lift off its respective track or rail. For example, to change direction on the grid structure 115 (e.g., move in the X-direction instead of the Y-direction), the first set of wheels 134 are lifted off the first set of grid members or rails 122a, while the second set of wheels 136 engage the second set of grid members or rails 122 b. A drive mechanism (not shown), such as a motor, drives the first 134 or second 136 set of wheels in either the X-direction or the Y-direction on the grid structure 115. In certain embodiments of the present invention, each of the first 134 and second 136 sets of wheels in the lower portion of the carrier body 132 are individually driven by an in-wheel motor to provide four-wheel drive capability of the load handling apparatus 130 on the grid structure 115. In other words, all wheels of the first and second sets of wheels are driven by a single in-wheel motor. This is to allow the load handling apparatus to travel along the rails or tracks 122a, 122b on the grid structure 115 as either of the set of wheels 134, 136 slides on the rails or tracks. Fig. 7 and 8 illustrate perspective views of the wheel 150 of the load handling apparatus 130 according to an embodiment of the present invention.
In detail, the in-wheel motor 160 shown in fig. 7 and 8 includes an outer rotor 162 including an outer surface arranged to engage with a grid structure (e.g., a track) and an inner surface including annular permanent magnets 164 arranged to rotate about a hub or inner hub 166 including a stator of the in-wheel motor 160. Typically, the stator includes coils of an in-wheel motor. To drive each wheel 150 of the first or second set of wheels and thereby move the load handling apparatus in the first or second direction on the grid structure, the outer rotor 162 of the in-wheel motor 160 is arranged to rotate about a rotational axis A-A corresponding to the central axis of the respective wheel. The outer surface of the rotor 162 may optionally include tires 168 for engagement with a track or rail. In the particular embodiment shown in fig. 8, outer rotor 162 rotates about a rotational axis on bearings (not shown) and includes outer rotor 162 with permanent magnets 164 bonded to the inner surface of outer rotor 162. By coupling an inner hub or hub 166, including the stator of the in-wheel motor, to the carrier body, each of the wheels 150 is coupled to the carrier body of the load handling device, thereby allowing the outer rotor 162 to rotate relative to the hub 166. While the particular embodiment describes the drive mechanism for each wheel 150 of the first 134 and second 136 sets of wheels as including an in-wheel motor, other rotatable drive wheel arrangements are suitable for use with the present invention. For example, pairs of wheels on the front and rear and both sides of the load handling apparatus may be driven by one or more motors connected to suitable pulleys or gear mechanisms.
In the block diagram of the wheel assembly shown in fig. 9, each of the pairs of wheels in the first 134 and second 136 sets of wheels is instructed by a control module or controller 170 to rotate about their respective axes of rotation. The control module 170 may comprise a computer system including one or more processors and memory storing instructions that, when executed by the one or more processors, instruct the first or second set of wheels to rotate synchronously about their respective axes of rotation. The memory may be any storage device known in the art including, but not limited to, RAM, computer readable media, magnetic storage media, optical storage media, or other electronic storage media that can be used to store data and that can be accessed by the processor. The one or more processing devices may be any processing device known in the art. Exemplary embodiments include, but are not limited to, microprocessors. The drive mechanism and wheel positioning of each of the first 134 and second 136 sets of wheels are communicatively coupled to the control module via any suitable communication interface unit 172. These include, but are not limited to, any wired or wireless communication known in the art.
The controller 170 sends control instructions to the driving mechanism, and the driving mechanism converts the control instructions into suitable driving current output signals according to the control instructions and sends the driving current output signals to the wheel motor so as to control the movement of the load processing device on the track. The control signal may be provided directly to the wheel motor or to a motor drive that controls the speed and direction of the wheel motor by varying the power output to the wheel motor. When the controller 170 determines that the drive mechanism must move the load handling apparatus from the current position to a new position or change its speed, the controller 170 generates a trajectory, also referred to as a motion control profile, which is converted into an appropriate control signal to drive the load handling apparatus from the current position or speed to the target position or command a stop position via the drive mechanism. The motion control profile defines a state of motion of the load handling apparatus, such as a time-varying speed, acceleration and/or position, as the load handling apparatus moves from a current position to a commanded stop position. The current position may be stationary or resting on the grid structure, in which case the initial speed and acceleration of the load handling apparatus is substantially zero, or in motion, in which case the load handling apparatus has an initial speed greater than zero. The command location may be an intended storage location or grid cell on the grid structure, for example, when retrieving or depositing storage containers. Alternatively, the commanded position may be a position on the grid structure near one or more charging points or grid cells, for example, in the event that the load handling device is instructed to recharge the rechargeable power source. Typically, the charging points are located at the periphery of the grid structure.
Once the motion control profile is generated, the controller will move the motion control profileWhich translates into appropriate control signals for moving the load handling apparatus through the trajectory defined by the motion control profile. The different segments (or phases) of the motion control profile are calculated based on one or more preset constraints corresponding to the mechanical limitations of the drive mechanism. The one or more preset constraints include, but are not limited to, maximum speed, maximum acceleration, and maximum deceleration. For example, a load handling device according to an embodiment of the present invention is expected to travel at speeds of up to 4m/s and at speeds of 2m/s 2 Is accelerated by the acceleration of the vehicle. Given these constraints and the desired commanded position, the controller will calculate a motion control profile to perform the desired movement. The desired movement may include a plurality of positions on the grid structure along the X and Y directions that may be performed together or sequentially to bring the load handling apparatus to the desired storage column. Each of the plurality of positions may include a single movement in either the X-direction or the Y-direction on the grid structure. For the purposes of the present invention, the terms "commanded speed" and "speed constraint" are used interchangeably to refer to the same function. Likewise, the terms "commanded acceleration" and "acceleration constraint" are used interchangeably to refer to the same function.
The present invention relates to the generation of constraint-based, time-optimal motion control profiles for point-to-point movement of load handling devices on a grid structure. In the context of the present invention, point-to-point movement refers to movement from a start position to a commanded stop position. When the load handling device reaches the planned destination or commanded stop position, the final acceleration and speed are moved to zero. In certain embodiments of the present invention, the controller input is received through the main controller 174. As shown in fig. 10, the controller may be coupled to a master controller 174 via a network 176. As shown in fig. 10, the controller is incorporated into the load handling apparatus 130. The network 176 may be any of a variety of types of networks including a LAN (local area network), a WAN (wide area network), the internet, an intranet, or the like. One or more base stations (not shown) may transmit the input information to the controller. In particular embodiments of the present invention, the input may include one or more constraints and a commanded or target location on the grid structure. One or more constraints may be received independent of the command location. For example, the master controller 174 may communicate the commanded position and one or more constraints individually to a motion control profile or trajectory generator stored in a controller of the load handling device 130.
Based on the received one or more constraints and the commanded position corresponding to the desired position on the grid structure, which may be the desired storage location or charging point on the grid structure, a motion control profile is generated or calculated (e.g., automatically by the controller). According to the invention, the motion control profile or trajectory comprises a series of position signals, velocity signals, acceleration signals and/or jerk signals. When the commanded position is reached and the remaining signals (velocity, acceleration, jerk) are exactly zero, the trajectory is terminated (or an endpoint is reached).
The generated motion control profile is a function of the type of profile generated by the controller configuration. This includes the S-curve profile and its simplified version, the trapezoidal profile.
1.0 trapezoidal and S Curve track Profile
In the context of point-to-point movement, a trapezoidal profile typically includes 3 distinct phases of motion in a triangular motion profile. The trapezoidal profile, in its simplest form, is shown in fig. 11 (a to c), which includes a period from constant acceleration to maximum speed followed by a period from constant deceleration back to zero speed. The period of positive acceleration is referred to as the "acceleration phase", and the period of negative acceleration is referred to as the "deceleration phase". As shown by the acceleration section in fig. 11c, the acceleration and deceleration increase and decrease almost instantaneously. At these points, the acceleration rate of change (da/dt) is not limited and is considered to be equal to infinity. The rate of change of acceleration with respect to time is known as Jerk (Jerk), j=da/dt, and in high throughput applications, undefined Jerk can result in shock loading, oscillations of the load handling device on the grid structure, and possibly audible noise. The generated motion control profile includes a series of (i) acceleration references (fig. 11 c), (ii) velocity references (fig. 11 b), and (iii) position references (fig. 11 a) as a function of time. Thus, the trapezoidal track shown in fig. 11 (a to c) includes signals related to position, velocity, and acceleration. Based on the trapezoidal profile, the main inputs for the generation of the motion control profile are: (i) Command position (position that should be reached at the end of the track); (ii) command speed (maximum allowed speed); and (iii) command acceleration (maximum acceleration allowed). The command velocity and command acceleration represent one or more constraints in the generation of a motion control profile based on the trapezoidal profile. Thus, the speed reference is controlled in the trapezoidal profile by manipulating the acceleration reference.
Given time t=0, velocity v (t) =v 0 And position p (t) =p 0 And keeps the manipulated variable acceleration reference a constant (t.gtoreq.t (0) in time), then for a trapezoidal trajectory profile, the velocity reference and the position reference as a function of time can be derived from the following equations:
v(t)＝+v 0 +at (1)
at any point in time, when the value of the manipulated variable (acceleration) changes, it is possible to simply set the value of the locus point at time τ to the initial condition, e.g., v 0 ←v(τ),p 0 And re-setting time to (t- τ), equations (1) and (2) can be applied again. The trajectory point refers to the value of a point on the characteristic time acceleration reference, the velocity reference and the position reference, which may be the start point or the end point of the trajectory segment, as will be explained later.
"manipulated variable" herein refers to a variable whose value is set by the control system. For a trapezoidal profile, the manipulated variable is acceleration, so the value of acceleration is set by the control system. Other variables (position and velocity) than the manipulated variables are calculated from the values of the manipulated variables according to the above equation.
As shown in fig. 12 (a-d), the S-curve acceleration profile is an alternative to a typical trapezoidal profile, typically consisting of seven distinct phases of motion. The acceleration profile may be generally divided into three sections: acceleration phase (phases I, II and III), constant speed phase (phase IV) and deceleration phase (phases V, VI and VII). Referring to fig. 11 (a to c), the acceleration phase, the constant speed phase, and the deceleration phase in the S-curve profile correspond to the acceleration phase, the constant speed phase, and the deceleration phase in the above-described trapezoidal profile, but the acceleration phase and the deceleration phase need to be subdivided to fully describe the trajectory due to the application of jerk to make the curve more complicated. In the acceleration phase, phase I starts to move the load handling device from the rest or start position with a positive jerk value until it reaches a maximum acceleration. In phase II, the profile accelerates at a maximum acceleration rate until it must start decreasing as it approaches maximum speed. In the trace profile shown in fig. 12c, the trace transitions to zero jerk in phase II. In phase III, the acceleration decreases until zero, i.e. the jerk shifts to a negative jerk value, until zero acceleration is reached at the desired speed. In phase IV, the constant speed phase, the expected speed remains constant, with both acceleration and jerk being zero. Stages V, VI and VII are deceleration stages in which the profile decelerates in a symmetrical fashion with stages I, II and III. In the deceleration phase, the S-curve profile starts with applying a negative jerk value in phase V, zero jerk in phase VI, and finally increases jerk to a positive value in phase VII to decelerate the trajectory until zero speed is reached.
Compared with the generation of the motion control profile based on the trapezoidal profile, the main inputs of the generation of the motion control profile based on the S-curve profile are as follows: (i) Command position (position to be reached at the end of the track); (ii) Command speed (maximum speed allowed, v) max ) The method comprises the steps of carrying out a first treatment on the surface of the (iii) Command acceleration (maximum acceleration allowed, a) max ) The method comprises the steps of carrying out a first treatment on the surface of the And (iv) commanding jerk. Thus, the one or more constraints that generate the motion control profile include a commanded speed, a commanded acceleration, and a commanded jerk. The commanded acceleration may include a maximum positive acceleration during an acceleration phase and a maximum negative acceleration during a deceleration phase. Alternatively, different terms may be used to decompose the acceleration-related constraint into a commanded acceleration and a commanded deceleration to distinguish between acceleration and deceleration phases of acceleration. For purposes of explaining the present invention, the terms command speed and speed constraint are used interchangeably to refer to the same function, the term command accelerationThe degree and acceleration constraints also represent the same function. The generated motion control profile includes a series of (i) jerk references as a function of time; (ii) an acceleration reference; (iii) a speed reference; and (iv) a position reference. Thus, the S-curve trace shown in fig. 12 includes signals related to position, velocity, acceleration, and jerk.
The edge or "angle" of the velocity profile provided by the one or more commanded jerks in the S-curve motion profile is smoothed compared to a trapezoidal profile where the transition between constant velocity and acceleration is abrupt, which reduces abrupt changes in acceleration, thereby allowing smoother movement of the load handling device over the grid structure, i.e., a significant reduction in vibrational energy injected by the S-curve profile over the load of the connection mechanism and the load handling device. This is especially the case when the drive mechanism for the drive wheel assembly is a servo motor or a stepper motor. The S-curve profile provides a control mechanism, compared to the trapezoidal profile, that eliminates oscillations in the load handling apparatus by adjusting the ratio of the transition phase to the constant acceleration phase of the profile. In the case of jerk, the higher the jerk, the greater the unnecessary vibrational energy generated and the broader the spectral range of the vibrational energy. This means that the faster the acceleration changes, the stronger the vibration. By reducing jerk, abrupt changes in acceleration within the region when the acceleration transitions to constant acceleration at the edges or "corners" of the velocity profile can be reduced. This will be apparent by being the edge or "corner" of the velocity profile flat.
The position, velocity and acceleration in the S-curve profile are all controlled by manipulating one or more jerk values, as compared to the trapezoidal profile where acceleration is the manipulated variable. The transformation time (or transformation) is defined as the time at which the manipulated variable changes its value, then there are at least four transformation points at which the manipulated variable changes its value. In brief, the manipulated variable remains constant between each pair of successive transition points. In the case of a trapezoidal track, the manipulated variable is acceleration, and in the case of an S-curve track, the manipulated variable is jerk. The trajectory value at the transformation is called transformation point. For the purpose of explaining the terminology in trapezoidal profile and S-curve profile, railsTrace section refers to the portion of the profile when the manipulated variables (acceleration for trapezoidal profile and jerk for S-curve profile) remain constant. In other words, the track segment starts from the transition point and ends at the transition point. Referring to fig. 11 and 12, in the above description of the trapezoidal and S-curve trajectories, three phases of the trapezoidal trajectory and seven phases of the S-curve trajectory may also be described as trajectory segments. For the purpose of explaining the invention, the track segment has a track segment consisting of track points (p 0 、v 0 、a 0 ) A given start point and end point (p E 、v E 、a E ) Wherein p is 0 、v 0 、a 0 Respectively, initial position reference a 0 Initial speed reference v 0 And initial acceleration reference a 0 And p is E 、v E 、a E Respectively, the end point position references p E Endpoint speed reference v E And endpoint acceleration reference a E . A motion control profile or trajectory is a series or sequence of such trajectory segments resulting from a change in a manipulated variable.
Given time t=0 and start acceleration a (t) =a 0 Start speed v (t) =v 0 Start position p (t) =p 0 And keeps the manipulated variable jerk j constant for t.gtoreq.0, then for an S-curve trajectory profile the relationship between jerk reference, acceleration reference, velocity reference and position reference as a function of time can be derived from the following equation:
a(t)＝a 0 +jt (3)
in the case of a trapezoidal profile, at any point in time τ, when the value of the manipulated variable changes, it is possible to simply set the value of the trace point at time τ to the initial condition, e.g., a 0 ←a(τ)、v 0 ←v(τ)、p 0 And re-apply equations (3) to (5) again.
The trace profiles in fig. 11 and 12 show that in the case of a trapezoidal trace, there is a discontinuity in the second derivative of the position, i.e. acceleration, and in the case of an S-curve trace, there is a discontinuity in the third derivative of the position, i.e. jerk. A trapezoidal track is typically used instead of an S-curve track because it provides a good enough approximation to the motion of the system and is more lightweight in mathematical complexity (and thus also in software implementation). However, due to the stability of the load handling apparatus, the motion control profile generated from the trapezoidal profile is not ideal in controlling the motion of the load handling apparatus on the grid structure. In order to achieve the generation of trajectories, for trapezoidal trajectories the manipulated variable, acceleration, is allowed to vary in a discontinuous manner. For the S-curve trajectory, the manipulated variable, jerk, is allowed to vary in a discontinuous manner compared to the trapezoidal trajectory. The time at which the manipulated variable changes its value, also known as the transition time, can affect the smoothness of the load handling device on the grid structure. Since the change in acceleration affects the smoothness of the stroke of the load handling apparatus over the grid structure, the transition time of the manipulated variables in the trapezoidal profile is too abrupt, resulting in oscillations of the load handling apparatus over the grid structure, i.e. the acceleration changes in a discontinuous manner. In the S-curve trajectory, the manipulated variable, i.e., jerk, is allowed to vary in a discontinuous manner, allowing the acceleration to vary in a more continuous manner, thus allowing the stroke to be smoother.
In certain embodiments of the present invention, the generation of the motion control profile will be described with reference to an S-curve trajectory.
2.0 parts of the track Generator
As described above, the generation or calculation of the motion control profile for controlling the drive mechanism of the wheel assembly of the load handling apparatus on the grid structure is accomplished primarily by a controller in the load handling apparatus that is responsive to one or more constraints received from the controller and an expected specification (commanded position) of the trajectory 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 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 a motion control signal, and a position controller 184 that converts the motion control signal into an appropriate torque signal to drive a drive mechanism 186 in accordance with one or more embodiments of the present invention. In this specification, the terms "motion profile generator" and "trajectory generator" may be used interchangeably.
The motion profile generator 182 may be, for example, a Programmable Logic Controller (PLC) or other controller that monitors and controls the drive mechanism of the load handling device. In this embodiment, the motion profile generator may be a functional component of the operating system of the controller and/or control software executed by one or more processors located in the controller. The motion profile generator 182 may also be a hardware component located within the controller, such as a circuit board or integrated circuit that exchanges data with other functional elements of the controller. Other suitable embodiments of the motion control profile are within the scope of the present invention. Although the motion profile generator 182 is shown in fig. 13 as an integrated component of a controller or control system, the motion profile generator 182 may be a separate component from the controller. In this configuration, the motion profile generator may exchange data with the controller or other component of the motion control generation system via any suitable communication means, including but not limited to a wired or wireless network, a hardwired data link, or other suitable communication link. In a specific embodiment of the invention, the controller and thus also the motion profile generator is located in the load handling device.
The generated motion profile defines a trajectory or motion control profile for controlling the motion of the load handling device from a current state (first position) or speed to a commanded position (second position), wherein the motion control profile is defined in terms of one or more of a position reference, a speed reference, an acceleration reference, and jerk (in the case of an S-curve trajectory) as a function of time. When in the commanded position, the load handling device may be a preset threshold distance from the commanded position. Preferably, the preset threshold is substantially less than 5 mm, more preferably less than 1 mm, more preferably less than 0.5 mm, so that the magnitudes of the end acceleration reference and the end velocity reference of one or more track segments are substantially zero at the commanded position. Preferably, the preset threshold is substantially zero, i.e. at the commanded position.
Fig. 15 is a flow chart 200 showing the main phases of adjusting the motion constraint 188 for movement based position in the motion profile generator 182 and translating the motion control profile into control signals sent to the drive mechanism to effect the transformation of the load handling device. This operation begins with the track generator receiving 204 an instruction to travel the load handling device 130 from a current location to a commanded location, in this case a particular storage column or grid cell. Given the defined constraints and command positions, the trajectory generator 182 generates 206 a trajectory or motion control profile. In certain embodiments of the present invention, the trajectory generator 182 periodically follows the motion profile 208 to calculate the wheel's motion torque demand to drive the load handling device 130.
Returning to FIG. 13, in operation, the destination location 190 is provided by the master controller 174 to the motion profile generator 182, optionally through the movement manager 192. The destination location 190 contains commands for the load handling device to travel from a current location or state to a commanded location on the grid structure. The current state of the load handling apparatus may be a resting position of the load handling apparatus on the grid structure or a current movement state (e.g. speed) of the load handling apparatus on the grid structure. Based on the current state of the load handling device on the grid structure, the motion profile generator 182 calculates a motion control profile as a function of one or more motion constraints, which may represent mechanical constraints of the drive mechanism 186 or motion constraints 188, or user preferences for operation of the drive mechanism. As shown in fig. 16, motion profile generator 182 receives input of a set of constraints 188. These constraints include upper limits on speed 188a, acceleration 188b, deceleration 188c, and jerk 188 d. These constraints 188 may be set during mobilization of the motion profile generator or reconfigured for each movement of the load handling device on the grid structure. For example, the motion profile generator 182 may be updated upon receipt of a new command position or upon receipt of a different value for one of the other constraints. The motion profile generator allows individual configuration of acceleration and deceleration limits to accommodate motion control profiles with asymmetric acceleration and deceleration. Upon receipt of the commanded position, the motion profile generator responds to and calculates a constraint-based, time-optimal motion control profile 194 that defines a trajectory for moving the load handling device from the current position to the commanded position.
As shown in fig. 15, the generation of the motion control profile involves: the manipulated variable 206, in the case of an S-curve profile, is applied to increase and/or decrease acceleration in a continuous manner during the acceleration and deceleration phases of the motion control profile. As shown in fig. 12 and 16, the motion control profile includes one or more jerk references, acceleration references, velocity references, and position references. These references are interrelated and are mathematical derivatives as a function of time. Jerk reference is the derivative of acceleration, acceleration is the derivative of speed, and speed is the derivative of position. The motion profile generator periodically advances the motion control profile, calculating one or more jerk reference, acceleration reference, velocity reference, and position reference for each of the different trajectory segments of the motion control profile. Typically, there are up to seven different phases of the S-curve motion profile described above with reference to fig. 12. The motion control generator defines these references as a function of time for each of a defined set of motion profile phases of the motion control profile. The invention is not limited to seven different phases of the S-curve motion profile, but may be any number of phases from a minimum of four phases to more than seven phases. For example, in the case of four phases, there may not be a constant speed phase in phase III and the load handling apparatus cannot reach the maximum speed constraint. Also, the number of stages may be greater than seven if at least one change in the trajectory occurs due to a change in one or more constraints or a change in commanded position. Further details of the generation of motion control profiles according to embodiments of the present invention will be discussed in the next section (section 3.0). For the purposes of the present invention, the functions "motion control profile", "motion profile" and "trajectory" may be used interchangeably to denote the same features.
As shown in fig. 14, the position of the load handling device relative to the grid structure is measured by one or more position sensors 198a, b. In certain embodiments of the invention, the position sensors 198a, b comprise so-called "fifth" wheels, that is, in this embodiment, each of the first and second sets of wheels is comprised of four wheels, one pair at the front of the load handling apparatus and one pair at the rear of the load handling apparatus, so the position sensor wheels are "fifth" wheels. Additional "fifth" wheels are present in the first and second sets of wheels for monitoring the position of the load handling apparatus on the grid structure in the first and second directions, respectively. As shown in fig. 14, a first "fifth" wheel 198a is mounted beside one wheel of the first set of wheels and a second "fifth" wheel 198b is mounted beside one wheel of the second set of wheels. The first "fifth" wheel 198a, corresponding to the first position sensor, is configured to engage the track when the load handling apparatus is traveling in the first direction such that rotation of the first "fifth" wheel may indicate the position of the load handling apparatus in the first direction relative to time and the direction of travel. Likewise, a second "fifth" wheel 198b corresponding to the second position sensor is configured to engage the track when the load handling apparatus is traveling in the second direction such that rotation of the second "fifth" wheel may indicate the position and direction of travel of the load handling apparatus in the second direction relative to time. The first and second directions may be X and Y directions along the track, respectively. In certain embodiments of the invention, the one or more position sensors comprise an incremental encoder comprising a rotary electromechanical device that generates pulses upon rotation of the "fifth" wheel, the pulses indicating the position and direction of rotation and being translatable into displacement of the load handling device relative to the grid structure. A "fifth" wheel may be mounted on the arm and biased downwardly to engage the track. In addition to measurements from one or more position sensors (e.g., a "fifth" wheel), the desired commanded position on the grid structure defined in the master controller 174 is also passed to the movement manager 192, which movement manager 192 converts to the relevant unit of position, which is then passed to the motion profile generator to effect generation of the motion control profile. For example, the movement manager 192 translates the commanded position from the controller, represented as the number of grid cells the load handling device is instructed to move over the grid structure, into "position units" that the controller or motion profile generator can understand and correspond to the readings of one or more position sensors 198 ("fifth" wheel). Subsequently, the controller or motion profile generator 182 can correlate the requested commanded position from the controller with a position reading from the "fifth" wheel or one or more position sensors or a position reference provided by the motion control profile. Further details regarding the generation of motion control profiles or trajectories for point-to-point movement of load handling devices on a grid structure will be discussed below.
Once the motion control profile is generated, the position controller 184 translates data associated with the motion control profile 194 into control signals that may be sent directly to the drive mechanism 186 to effect a transition of the load handling device to the commanded position. As described above, the position controller 184 may form part of the controller or control system 170 of the load handling apparatus, that is to say the controller or control system comprises functional components related to the trajectory generator 182 and the position control 184. FIG. 13 is a block diagram illustrating the input and output of an exemplary position controller according to an embodiment of the present invention. The motion control profile data generated by the trajectory generator is transmitted to a position controller 184 which converts it into control signals to drive the wheels of the wheel assembly. Based on the motion control profile, the position controller 184 varies the power of the drive mechanism to vary the speed of the load handling apparatus relative to the grid structure. As with the motion control generator, the position controller 184 may be a functional component that resides with the controller or, alternatively, a component separate from the motion profile generator that has its own control system. For example, pairs of wheels of the wheel assembly may be connected to a local controller and configured to translate motion control signals from a motion profile generator. The conversion of the motion control signal includes, but is not limited to, converting motion control data into a suitable torque control signal for driving a motor of a drive mechanism or wheel.
As described above and shown in fig. 16, the motion control profile defines a trajectory of point-to-point movement of the load handling device relative to the grid structure over time in 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). These references represent the functional calculations of the trajectory generator, defining how the individual motion properties of the drive mechanism are controlled as a function of time to achieve a given point-to-point movement. These reference values are mathematically related as derivatives. Converting the motion control profile into a control signal that instructs the load handling device to perform the desired point-to-point movement in accordance with the motion control profile involves the position controller generating a feed-forward signal based on the motion control profile, more specifically an acceleration reference derived from the motion control profile.
Fig. 17 is an exemplary position controller 184 showing the architecture of the position controller for converting signals from a trajectory or motion profile generator to a drive mechanism of a load handling device. Knowing the mass of the load handling device, the feedforward signal 210 (denoted FFs in fig. 17) represents the calculated torque demand, which is proportional to the acceleration reference, more specifically the derivative of the speed reference, derived from the motion control profile. The feed forward signal 210 may be converted to an appropriate signal to drive the wheels of the wheel assembly. However, to ensure that the point-to-point movement of the load handling device on the grid structure is aligned with the trajectory segments of the motion control profile, the calculated feedforward signal 210 is compensated by a feedback signal 212 (denoted FBs in fig. 17) indicating the actual motion state of the load handling device relative to the grid structure. The motion state of the load handling apparatus on the grid structure includes, but is not limited to, the position of the load handling apparatus relative to the grid structure and the speed of the load handling apparatus. The speed of the load handling device may be derived from the first derivative of the position measurements with respect to time from one or more position sensors 198.
Based on the feedback signal 212, the position controller 184 will adjust or compensate for the control signals from the one or more position sensors 198, which is necessary to ensure that the load handling device moves as closely as possible in accordance with the motion control profile. In certain embodiments of the present invention, feedback signals 212 from one or more position sensors are fed back to the position controller to reflect the actual state of the load handling device relative to the grid structure. Upon receiving data signals from one or more position sensors, the position controller, in response, can be instructed to regenerate one or more track segments of the motion control profile by changing the manipulated variable jerk to ensure that the load handling device reaches the commanded position without undershooting or overshooting, i.e., the acceleration and velocity are zero at the commanded position.
In certain embodiments of the present invention, feedback signals 212 from one or more position sensors are combined with the feed forward signal 210 to generate a total torque demand for the wheel or drive mechanism of the drive wheel. Track positions derived from the motion control profile are compared to measured positions from one or more position sensors. This comparison represents the position error, i.e. the difference between the track position reference and the measured position as a function of time. The position error is input to a PID (proportional, integral and derivative) controller or PI controller 214, and the position is corrected according to the PID or PI to the set point indicated by the motion control profile. The output 214 from the PID or PI is a correction torque that, when applied, will correct the position error and align the actual trajectory of the load handling device with the motion control profile. The feed forward signal 210 represents the torque required to meet the speed/acceleration requirements of the trajectory generator. The corrected torque 214 from the PID or PI is combined with the torque demand of the feed forward signal 210 to generate a total torque demand. As shown by the communication path in fig. 17, the "acceleration feedforward" signal and the "velocity feedforward" signal are combined in a feedforward calculation and may be stored in a look-up table. In practice, the acceleration and velocity feedforward torque demand terms are added to the Zero order torque demand (Zero-order Torque Demand) of the constant torque demand term, i.e., a fixed value, which is signed (track velocity). The magnitudes of the acceleration and velocity feedforward torque demand terms are determined by the acceleration and velocity of the generated motion control profile or trajectory, respectively.
In one embodiment, the total torque demand may be calculated as follows:
total torque request = feedforward torque request + feedback torque request
Wherein:
feedforward torque demand = acceleration feedforward torque demand + speed feedforward torque demand + zero order torque demand value
Wherein:
acceleration feedforward torque demand = track acceleration x acceleration feedforward gain
Speed feedforward torque demand = track speed x speed feedforward gain
Zero order feedforward torque demand = sign (track speed) x zero order torque demand value
Wherein the values of the following three are empirically determined:
the acceleration feedforward gain is a value that depends on the estimated total mass of the load handling apparatus including the container and its contents, which results in a higher gain value when the load handling apparatus is carrying a heavier container.
The speed feed forward gain is also dependent on the estimated total mass of the load handling device.
The zero order torque demand value is the value of the total torque demand required to only keep the load handling apparatus moving at a constant, near zero speed, i.e., it is a scale factor.
These three components, the acceleration feedforward torque demand term, the speed feedforward torque demand term, and the zero order torque demand value, are summed to form a feedforward torque demand and stored in a look-up table. When compensating for the calculated feedforward torque demand with the feedback torque signal, the controller retrieves the feedforward torque demand from the look-up table to be combined with the feedback torque measurement.
Alternatively, the values of the acceleration feedforward gain, the speed feedforward gain, and the zero order torque demand may be determined by calculation rather than empirically.
The acceleration feed forward torque demand term is a component of the total torque demand that provides torque to enable the load handling device to match the desired acceleration of the track. The propulsive force required to meet the required acceleration may be calculated by multiplying the required acceleration by the total mass of the load handling apparatus including the storage container and the contents. The propulsive force may be converted to a torque demand (torque = force x radius) by multiplying by the wheel radius. In embodiments where the total mass of the load handling device is known, the acceleration feed forward gain may be calculated rather than empirically determined:
acceleration feedforward gain = total mass of load handling device x wheel radius.
The speed feed forward torque demand term and the zero order torque demand term are components of the total torque demand that provides torque so that the load handling device can overcome the drag. Various models of calculating drag are known in the art, among which there may be a component proportional to speed, a component proportional to the square of speed, and a constant component. The drag may be caused by air drag, rolling resistance of the wheels on the track, and/or grade drag when the track is not perfectly level. In current applications, the square term of the speed of the resistance may be less important and may be ignored in order to make the calculation of the torque demand faster, with less computational load.
The propulsion force required to overcome the rolling resistance can be calculated by multiplying the normal force (total weight of the load handling device including the storage container and the contents) by the rolling resistance coefficient, which is an attribute of the wheels and the track. The propulsion term may be converted to a torque demand (torque = force x radius) by multiplying by the wheel radius. In an embodiment where the total mass of the load handling device is known, and the coefficient of rolling resistance of the wheel on the track, the speed feedforward gain may be calculated instead of empirically determined:
speed feed forward gain = total mass of load handling device x gravitational acceleration x rolling resistance coefficient x wheel radius.
In practice, it may be easier to determine the speed feedforward gain and the zero-order torque coefficient empirically than by calculation.
Determining the total mass of the load handling apparatus can be difficult because the mass of any storage container and contents must be taken into account. Typically, the load handling device is configured to receive instructions from the host controller to retrieve storage containers from specific storage locations within the grid framework structure. In some embodiments, the master controller will use a database to store data about the location and contents of all storage containers in the grid framework structure, and thus already contain quality information for each storage container. When the load handling apparatus picks up a storage container from the grid framework structure, the master controller communicates the mass of the storage container to a controller on the load handling apparatus so that the mass can be used to calculate the total mass of the load handling apparatus. Similarly, when the load handling device places the storage container back into the grid framework, the master controller will communicate information to the controller on the load handling device that does not take into account the quality of the storage container. The master controller must track the mass of each storage container and contents and update them continuously (e.g., as items are removed from the storage containers to fulfill customer orders). The mass of any storage containers and contents carried by the load handling apparatus may then be incorporated into the mass of the load handling apparatus itself to calculate the total mass.
An alternative method of determining the mass of the storage container and its contents is to measure the current consumption of the motor used to operate the lifting device and hoist the storage container to the container receiving space of the load handling device. The current consumption varies approximately linearly with the mass of the storage containers, thus providing a simple and convenient method of determining mass and the benefit is that the main control system is not required to track the mass of each storage container and communicate this information to the controller on the load handling apparatus. The graph of fig. 36 plots the measured average current consumption of the hoist motor versus the mass of the contents of the storage vessel. From which the data points are visible falling on a straight line. The intercept represents the current required to lift off the storage vessel (about 6 kA). Since the data points lie on a straight line, a linear fit can be used to capture the relationship between current consumption and the mass of the storage container, so that the measured current can be used to easily and accurately estimate the mass of the container and its contents.
The above calculation of the torque demand is one example of how the torque demand is calculated. The skilled artisan will appreciate that other methods of calculating, predicting or measuring acceleration forces and resistances may be employed in the present invention.
In some embodiments, the speed constraint, the acceleration constraint, and/or the jerk constraint may vary depending on the mass of the storage container and the contents.
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 load handling device (including any storage containers and content), a change in the total mass of the load handling device and its cargo will result in a change in the acceleration feedforward torque demand. In some embodiments, the acceleration feed forward requirement is the most important component of the total torque requirement. Because the wheel motor is rated to provide a given maximum torque, carrying a particularly heavy load handling device may require the wheel motor to provide more torque than it can provide, either the load handling device cannot maintain its commanded acceleration or the wheel motor runs at risk of overheating because of exceeding its rated torque. Or alternatively, if the rated torque of the wheel motor is increased in order to solve this problem, the maximum torque capacity of the wheel motor is only used occasionally, and as such, the wheel motor is actually over-designed in application and thus may be heavier and more expensive than necessary.
To address this issue, in some embodiments, acceleration constraints may be formulated based on the total mass of the load handling apparatus and its cargo. For example, if the value of the acceleration constraint is inversely proportional to the total mass of the load handling apparatus and its cargo, the acceleration feedforward torque demand will remain substantially constant and substantially independent of the mass of any storage container and contents carried by the load handling apparatus. This has the advantage that the torque rating of the wheel motor can be selected to match the desired acceleration feed forward torque demand and that the torque demand does not substantially change due to mass changes.
Similarly, the speed feedforward torque demand is the product of the track speed and the speed feedforward gain, and the speed feedforward gain is proportional to the total mass of the load handling device (including any storage containers and contents), and a change in the total mass of the load handling device and its cargo will result in a change in the speed feedforward torque demand. Similar to the acceleration constraint, if the speed constraint is inversely proportional to the total mass of the load handling apparatus and its cargo, for example, the speed feedforward torque demand will remain substantially constant and substantially independent of the mass of any storage containers and contents carried by the load handling apparatus. Having both acceleration and speed constraints dependent (e.g., inversely proportional) on the total mass of the load handling apparatus means that none of the three terms of the feed-forward torque demand equation are dependent on mass, so that the torque demand remains substantially constant relative to the mass of the load handling apparatus and its cargo.
In some embodiments, the value of the jerk constraint may depend on the total mass of the load handling apparatus and any storage containers and contents located within the container receiving space of the load handling apparatus. For example, a load handling apparatus carrying a heavy storage container may have a lower center of gravity and be more stable, and thus may accept higher jerk values without the risk of the load handling apparatus tipping over the grid. The absolute value of the jerk constraint may be defined as a function of the total load mass of the load handling apparatus, and may be a continuous function or a set of discrete values.
In other embodiments, the value of the jerk constraint may depend on the contents of the storage container. For example, a lower jerk value may be used when the load handling apparatus is carrying a storage container containing delicate or fragile items in its container receiving space. Whereas higher jerk values may be used when the load handling device is carrying stronger items in its container receiving space or items that are less likely to roll and strike the container walls.
One method that can be used to determine the value of the jerk constraint is to place the load handling apparatus under a trapezoidal profile (theoretically infinite jerk), measure the time required to achieve the commanded position, and measure the jerk of the actual motion of the load handling apparatus. The S-curve profile can then be run with a series of jerk values around the measured jerk and evaluate the effect on the position error and total time required to reach the commanded position relative to the trapezoidal profile.
Although specific embodiments of how the jerk, acceleration, and speed constraints are formulated to vary with respect to the mass of the load handling vehicle have been described herein, it should be understood that the scope of the invention encompasses other variations and that other values or amounts of jerk, acceleration, and/or speed constraints may be applied.
The calculated total torque demand is converted into a suitable control signal to drive the wheels. In certain embodiments of the present invention, the total torque demand is assigned to either the first or second set of wheels depending on whether the first set of wheels is engaged with the grid structure or the track or the second set of wheels is engaged with the grid structure. In certain embodiments of the present invention, each of the first and second sets of wheels is individually driven by in-wheel motor 220, as described above. The first set of wheels includes a pair of wheels at a front of the load handling apparatus body and a pair of wheels at a rear of the load handling apparatus body. The second set of wheels presents similar pairs of wheels, namely a first pair and a second pair on either side of the body of the load handling apparatus. In operation, the paired wheels are driven synchronously as if they were driven on a "virtual or imaginary" axis. For example, the front pair of wheels are driven synchronously as if they were driven on the same axis, and the rear pair of wheels are driven synchronously as if they were driven on the same axis. Although the second set of wheels is driven by a separate in-wheel motor, the same principle applies to the second set of wheels. The same applies to the case of the invention: the four wheels of the first set of wheels are all synchronously driven. Likewise, all four wheels of the second set of wheels may also be driven in synchronization. The advantage of driving the wheels of the wheel assembly by means of the in-wheel motor alone is that the torque to the wheels can be differently distributed to each of the wheels, which is particularly important when the wheels slip.
Torque from the motor control profile or the compensated total torque demand is distributed to 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 and rear "shafts" to account for the weight transfer of the load handling device on the grid structure. For example, as the load handling device accelerates across the grid structure, more of the calculated total torque demand is transferred to the front "axle". Conversely, when the load handling device decelerates on the grid structure, the total torque demand is transferred more to the rear "axle". Thus, the biasing mechanism will vary the amount of torque demand imparted to the front and rear "axles" to shift the weight of the load handling apparatus between the front and rear of the load handling apparatus, respectively, as the load handling apparatus accelerates or decelerates across the grid structure.
The biasing mechanism 216 applies a differential torque (differential torque) demand to the wheels, and more specifically to the motors of the drive wheels, in accordance with the trajectory acceleration reference of the motion control profile. The trajectory acceleration reference and/or the trajectory speed reference of the motion control profile provide an indication or reference point of whether the load handling device is accelerating or decelerating on the grid structure. In other words, the differential torque applied to the wheel follows the track acceleration reference of the motion control profile, i.e. there is a proportional relationship between the torque distribution to the wheel and the track acceleration reference of the motion control profile. In response to a signal from the motion control profile generator indicating acceleration, the biasing mechanism will shift more of the calculated total torque demand onto the front axle. Conversely, when deceleration is indicated, more of the total torque demand is transferred to the rear axle, or the total torque demand is reduced to provide a "braking" force. Likewise, when traveling smoothly on a grid structure, the total torque demand is split substantially evenly between the front and rear "axes".
In addition to the differential torque applied to the front and rear "axles", the torque demand of the wheel motors used on either side of the body of the load handling apparatus may also be controlled to control the yaw or steering of the load handling apparatus on the grid structure. For example, depending on the direction of travel of the load handling apparatus on the grid structure, the torque demand may be used to control the speed of the left or right set of wheels, and thus the yaw or steering angle of the load handling apparatus on the grid structure. Ideally, the grid members are arranged to extend in a lateral or X-Y direction, and the differential torque of the left and right wheels is controlled to prevent the load handling device from yawing or steering, i.e. traveling substantially straight, on the track.
In addition to translating the motion control profile into control signals for driving the drive mechanism, the biasing mechanism 216 may be configured to control wheel slip on the track and periodically compensate for the motion control profile in response to position signals received from one or more position sensors and/or one or more track position sensors mounted on the load handling device. One or more position sensors may also be used to determine the slip of either of the first and second sets of wheels on the track. In fig. 17, this is shown as a separate skid member 218 in the position controller. When the rotational speed of any one wheel exceeds the state of motion of the load handling apparatus on the grid structure, the wheel may slip. The state of motion of the load handling apparatus on the grid structure may be determined or derived by a first derivative of one or more position sensors as a function of time, e.g. depending on the rotational speed of the first or second "fifth" wheel of the load handling apparatus travelling in the first or second direction. The individual rotational speeds of each wheel of the first and second sets of wheels are determined by one or more wheel encoders placed adjacent to each wheel. The one or more wheel encoders may be incremental encoders that include a rotary electromechanical device that generates pulses indicative of the rotational speed of the wheel. In certain embodiments of the present invention, the slip control manager manages the slip of each wheel of the wheel assembly by comparing the rotational speed of the wheel to the movement of the load handling device on the grid structure alone. The state of motion of the load handling device on the grid structure is determined by a first "fifth" wheel or a second "fifth" wheel depending on the direction of travel of the load handling device on the grid structure. In certain embodiments of the present 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.
If the rotational speed of any one wheel exceeds the state of motion of the load handling apparatus on the grid structure, the slip control manager removes or reduces the torque of that wheel by controlling the bias to the torque demand of that wheel until the rotational speed of the wheel matches the state of motion of the load handling apparatus. For example, by biasing the mechanism, the position controller may change the torque demand on the beater wheel, i.e., reduce the torque demand or even completely eliminate the torque demand on the wheel to reduce the rotational speed of the beater wheel, i.e., make the rotation of the wheel more passive. Alternatively, the slip control manager may also redistribute torque on the beater wheel to other wheels to balance the wheel speeds of the wheel assembly to match the motion state of the load handling device on the grid structure.
It should be understood that the architecture shown in fig. 17 is merely an example, and that other arrangements of the components of the position control are within the scope of the present invention. For example, the position controllers may be incorporated into the controller that generates the motion control profile or trajectory, or alternatively they may be separate controllers.
3.0 Generation of tracks
The generation of a trajectory or motion control profile involves controlling the number and duration of the trajectory segments such that when the trajectory segments are "stitched together" the acceleration and velocity are substantially zero at the commanded position. According to the invention, the generation of the track segments depends on the application of one or more jerk references based on the identification of one or more conditions in the track or the satisfaction of one or more conditions of the track. These conditions may be changes in one or more of the constraints and/or command positions. In generating the track, the track generator should be able to decide which track segments are needed to get the track from the current point to its end point. In identifying these cases, the generator must be able to perform this decision process (i.e., generate a trajectory) regardless of the current trajectory point, as its input (especially, but not exclusively, command position) may change over time. For example, the command position will typically be modified based on feedback from the fifth wheel and/or grid sensor when passing through the grid cell.
Whether the track is terminated or still in progress, in its simplest form, requiring the generator to regenerate the result of the track (i.e., by modifying its input) involves generating one or more track segments, each track segment comprising:
the values of the manipulated variables applied from this point on,
the length of time the manipulation value is applied (i.e. the duration of the track segment—the transition time), and
the end of the segment (as it is useful information that can be cached).
Thus, an already ongoing track segment will be replaced by a regenerated track segment or, if the track is terminated, an ongoing track segment.
The values of the manipulated variables may take any value (within the rules described below) regardless of the value prior to the request to regenerate the trajectory. In the case of a finite jerk trajectory (S-curve trajectory), the number of trajectory segments is controlled by applying one or more jerk constraints to increase or decrease the magnitude of acceleration during different phases of the motion control profile. For the convenience of explanation of the present invention, the jerk constraint is applied to the acceleration phase (positive acceleration) and the deceleration phase (negative acceleration). The acceleration phase corresponds to a trajectory in which the trajectory speed reference increases. Instead, the deceleration phase corresponds to a trajectory in which the trajectory speed reference decreases. Thus, there is a set of jerk constraints to change acceleration during the acceleration phase and a set of jerk constraints during the deceleration phase.
The summary is as follows:
acceleration stage
Jerk 1-increase the magnitude of acceleration when it is desired to increase speed.
Jerk 2-the magnitude of the acceleration is reduced when an increase in speed is desired.
Deceleration stage
Jerk 3-increase the magnitude of acceleration when it is desired to reduce speed.
Jerk 4-the magnitude of the acceleration is reduced when a reduction in speed is desired.
Jerk 1, jerk 2, jerk 3, and jerk 4 may also be referred to as J, respectively 1 、J 2 、J 3 And J 4 . In certain embodiments of the present invention, four non-zero jerk constraints are used. Jerk 1 and jerk 2 are used to increase and decrease the acceleration during the acceleration phase, respectively, while jerk 3 and jerk 4 are used to increase and decrease the deceleration step, respectivelyThe magnitude of the acceleration (deceleration) of the segment. In a particular embodiment of the invention, the motion control generator is configured to switch between jerk 1 and jerk 2 in one or more switching modes to control the number and duration of track segments in the acceleration phase and between jerk 3 and jerk 4 to control the number and duration of track segments in the motion control profile, with the aim that the track reaches the commanded position in the shortest time, i.e. the achieved acceleration and velocity are substantially zero at the desired commanded position. A zero value of the jerk constraint may be used when it is desired to maintain the desired acceleration during the acceleration phase or the desired deceleration during the deceleration phase.
In a 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 the magnitude of the acceleration is intended to be increased or decreased during the acceleration phase. The positive sign "+" is used when the magnitude of the acceleration is to be increased, and the negative sign "-" is used when the magnitude of the acceleration is to be decreased. The absolute values of the accelerations 1, 2, 3 and 4 selected depend on the ability of the load handling apparatus to withstand sudden changes in acceleration without tilting or toppling over the grid structure, which depends on the stability of the load handling apparatus over the grid structure. For a load handling device with a relatively low center of gravity, which is very stable, the jerk that such a load handling device can withstand is much higher than for a load handling device that is relatively less stable. In the case of a load handling apparatus having a footprint of a single grid cell, the absolute value of jerk is selected so that the load handling apparatus can accelerate at a maximum speed defined by constraints without tilting or tipping. In a particular embodiment of the invention, the absolute value of the jerk is 20m/s3. Thus, jerk 1 and jerk 2 will have values of +20m/s3 and-20 m/s3, respectively. Likewise, jerk 3 and jerk 4 will have values of +20m/s3 and-20 m/s3, respectively. However, jerks 1, 2, 3 and 4 do not have to have the same value to control the number of track segments. Jerk constraints may have different absolute values. Alternatively, the jerk constraint may be two values used to increase or decrease the magnitude of acceleration in the acceleration phase and the deceleration phase, respectively.
As previously described, in some embodiments, the value of the acceleration constraint may depend on the total mass of the load handling apparatus and any storage containers and contents located within the container receiving space of the load handling apparatus.
In the generation of the motion control profile, the value of jerk may be switched between any of the jerk constraints described above, regardless of the value prior to its request to regenerate the trajectory. For example, if the track is in an acceleration phase (i.e., gradually accelerating toward the commanded speed) and the commanded position is modified back to the starting position, the generator must immediately generate new track segment(s) to slow down the track (i.e., gradually decelerating to zero). The fact that the trajectory is in the acceleration phase has no effect at all on the decision to generate a new trajectory segment to slow down the trajectory.
Another embodiment: if the track speed is already at the command speed limit and the track position is still far from the command position, then if the (absolute value of the) command speed or the constraint speed is increased and all other constraints input by the track generator remain unchanged, the track generator has to start increasing the track speed in order to meet the "shortest time" requirement, possibly all the way up to a new command speed value, depending on the new value of the command position. The fact that the trajectory is in a constant velocity phase before the instruction to increase the velocity constraint or command the velocity has no effect on this decision.
Generating a track involves establishing a state or condition of the track at a given time, generating one or more track segments based on the condition of the track at that time so that the track can start from a current point and reach an intended destination (which may be a commanded position). The track may comprise a plurality of track segments, each track segment having a respective track segment end point, which together represent a track or motion control profile. For the purposes of the present invention, the term "endpoint" reflects the point of the track at the end of a given track segment.
The process of establishing the state of the trajectory at a given time is accomplished by a decision making process through an exclusionary process, wherein a controller or control system is asked or queried with a series of questions in a particular order until a certain answer clearly indicates what action scheme should be taken next to accommodate the new generator input value. In the first embodiment described above, the obvious course of action is to start gradually decreasing the speed to zero by generating one or more track segments, because the longer this process takes, the farther the track moves from the intended position. In a second embodiment, the obvious action scenario is to increase the speed, since not doing so (i.e. keeping the same speed) would allow the trajectory to be longer to reach the desired position.
In generating the track, the generator generates a plurality of track segments, wherein it uses a first track segment and is able to cache other track segments for subsequent use. Each of the plurality of track segments may represent one or more speed transforms. Briefly, a trajectory is moved from a starting point (p 0 、v 0 、a 0 ) Brought to the end point (p) e 、v e 、a e ) It may not be sufficient because the track may accelerate away from the command position, so the track is needed to redirect this back to the command position, and/or the track may accelerate too much to let the track drop at the command position without overshooting (overshowing). At the transition to the desired final speed value v f All these conditions have to be taken into account when, and this may involve generating a plurality of track segments. The expected final speed value v f The trajectory may be substantially zero, that is, the point-to-point movement to the commanded position, and the control system is instructed to generate the trajectory for the point-to-point movement such that the final speed value at the commanded position is substantially zero. Whenever a new request occurs to regenerate a track (again, due to a change in the generator input), the generator must clear all cached track segments because they may be invalid at the new generator input. The act of re-generating the track itself will re-compute a new track segment.
The decision process associated with generating a finite jerk trajectory in accordance with an embodiment of the present invention is summarized in the following section in connection with the flowchart 300 shown in fig. 18. Each part is organized in the form of a series of questions (bold), which represent a logical check to be made to produce an answer, and an action scheme that must be taken according to the answer to the question. Each of the movement schemes is represented by the flowcharts shown in fig. 19 to 26. The purpose of each question is to establish the state in which the current trajectory is prior to applying the action plan. The series of problems for generating the trajectory are as follows:
a) Is a forced deceleration 302 in progress (or just requested?
In some cases a forced deceleration may be applied, for example when the commanded position changes and a different track segment needs to be generated to bring the track to the updated commanded position, i.e. the track is required to terminate prematurely. This process uses a forced deceleration algorithm, discussed further below, to find the deceleration trajectory segment to reach the updated commanded position, i.e., acceleration and velocity are substantially zero at the updated commanded position. Selecting a deceleration trajectory such that the trajectory reaches an updated commanded position without overshooting and exceeding the commanded deceleration may require the control system to generate new inputs, including but not limited to new constraints in deceleration, to allow the trajectory to decelerate more strongly. The method of forced deceleration described below involves applying a forced deceleration constraint to the trajectory. In some cases, an algorithm, more specifically a Root-finding algorithm (Root-Finding Algorithm), may be applied to determine the appropriate deceleration constraints for the trajectory. The objective function is the degree of overshoot experienced by the trajectory between when the deceleration is performed under the selected deceleration constraint and the updated commanded position, while the root is the deceleration at which the objective function is substantially zero. The algorithm steps to a more accurate approximation of the root, which corresponds to the deceleration value that enables the trajectory to fall at the updated commanded position. For example, the trajectory of the deceleration phase may be requested to reach a new command position that is closer than the previously requested command position. The speed transformation algorithm according to the invention can be executed, when applying the forced deceleration algorithm, to transform the speed reference from an initial starting point to an expected final speed v having a magnitude of substantially zero f . Transforming the magnitude of the velocity from an initial value to an expected final velocity v f The speed of substantially zero in this case may involve generating a oneOne or more track segments, depending on the state of the starting point or the current state of the track.
If the answer to question A is 303, the desired track segment is generated to bring the track to the commanded position as described in the following section forced deceleration and shown in FIG. 34. Otherwise, if the trajectory does not exist (i.e., forced deceleration is not in progress and not requested), the control system will go to the next problem.
B) Is (the absolute value of) the current trajectory acceleration exceeded the commanded acceleration 304?
In this case, the absolute value of the current track acceleration is compared with the command acceleration (acceleration constraint). If the absolute value of the current track acceleration is greater than the commanded acceleration, then a new track segment will need to be calculated so that the magnitude of the track acceleration is reduced to the commanded acceleration. In this case, one or more jerk values will be applied to the trajectory to reduce the trajectory acceleration (see fig. 20), i.e. the end point of the acceleration reference has an end point acceleration equal to the reduction of the commanded acceleration. Each time jerk is applied to the track, the magnitude of the track acceleration is compared with the commanded acceleration until the track acceleration is reduced to the commanded acceleration. This is achieved by having a starting point (p 0 、v 0 、a 0 ) And endpoint (p) e 、v e 、a e ) To illustrate:
the manipulated variable (jerk) value used is j=j 2 (reducing the trajectory acceleration reference),
track segment duration is defined byIt is given that,
by directly applying the trajectory equations 3, 4 and 5, the starting point of use (p 0 、v 0 、a 0 )、J 2 And a period of time duration T, the end acceleration a can be obtained e End point velocity v e And end position p e 。
If it is the case that the current track acceleration does not exceed the commanded acceleration, the control system will go to the next problem.
C) If the track now starts to be brought to a fixed position (zero velocity and zero acceleration), its fixed position:
a) Is substantially equal to (i.e., within 0.5 positional units) command position 306?
b) On the opposite side 308 of the current position of the track relative to the commanded position (i.e., overshoot)?
c) Farther 310 from the commanded position than the current position of the track (i.e. we are moving away from the commanded position)?
In case it is known that the magnitude of the track acceleration does not exceed the command acceleration, the control system checks whether the track is in the command position when stopping the track. This will also indicate how far the track is from the commanded position or whether the commanded position will be exceeded. By presenting a series of problems to the control system and excluding the process, the state of the track in the current state can be established, as well as the required remedial action applied to the track, to ensure that the track is within the plan of point-to-point movement.
If the answer to question (a) is yes, the control system will perform the necessary steps to gradually slow down to zero speed in order to generate a signal from the current trajectory point (p 0 、v 0 、a 0 ) One or more track segments to the end point allow the track to reach a fixed position at the commanded position, where the end point acceleration a e And end point velocity v e At the commanded position it is substantially zero (see fig. 21). This is accomplished by generating the following having a starting point (p 0 、v 0 、a 0 ) And endpoint (p) e 、v e 、a e ) To illustrate:
the manipulated variable (jerk) value used is j=j 2 (decrease the track acceleration reference),
track segment duration is defined byIt is given that,
end point acceleration a e And end point velocity v e Substantially zero, endpoint p e The starting point (p) can be used by directly applying the trajectory equations 3, 4 and 5 0 、v 0 、a 0 )、J 2 And a segment duration T.
However, if the answer to question (a) is no, but the answer to any one of the last two questions (b) and (c) 308, 310 (see fig. 22 and 23) is yes, it is the case that the trajectory exceeds the commanded position. And the remedy is to adjust the track speed to a "reverse" value so that the track can be redirected toward the commanded position by "returning". As with the forward trajectory toward the commanded position, one or more reverse trajectory segments are provided by applying one or more jerk values to cause the reversely operated load handling device to travel toward the commanded position. The duration or length of the reverse track segment 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 speed at the commanded position before gradually decelerating to zero speed. Conversely, if the degree of overshoot is small, the track speed may not reach the commanded speed, and thus the desired peak speed v is required Peak to peak This speed will allow the reverse trajectory to reach the commanded position in the shortest time. In both cases, a "speed transformation algorithm" will be applied to transform the magnitude of the speed from an initial or current value to a final speed v f Values. The speed transformation algorithm will be discussed in further detail below. Essentially, a first speed transformation algorithm is applied to move the trajectory from an initial (current) state or trajectory point to a peak speed v that is equal to the expected peak speed v Peak to peak End speed v of corresponding given track segment E The locus point reached. It is given an initial trajectory point (p 0 、v 0 、a 0 ) End point speed (v) E ) Command acceleration (a) max ) And a specific value of jerk constraint to increase or decrease the magnitude of the trace acceleration reference. By shifting the end point (p E 、v E 、a E ) Assigned to a starting point (p 0 、v 0 、a 0 ) The second speed transformation algorithm reduces the magnitude of the speed reference from the expected peak speed v Peak to peak Corresponding initial velocity v 0 Move to and expect final velocity v f Corresponding new end point velocity v E And has a basic structureZero value and end acceleration a corresponding to substantially zero E . Further details of the speed transformation algorithm will be discussed below. If the trajectory moves away from the commanded position, then the desired final velocity v at the commanded position is applied to the velocity conversion algorithm f Previously, the magnitude of the current trajectory acceleration had to be gradually reduced to substantially zero. As shown in the flow chart shown in fig. 23, the return response is applied to the expected final velocity v, which is substantially zero at the commanded position f Previously, the magnitude of the track acceleration was gradually reduced to substantially zero.
However, if the answer to all three questions is "no", then the case must be: the gradual decrease to zero speed will now result in the track not reaching the commanded position. In this case the control system will enter the next question to establish the next phase of trajectory generation.
D) Is the track speed exceeded the command speed 312?
According to a specific embodiment of the invention, the generation of the motion control profile depends on the maximum possible speed at which the load handling means can be realized during the acceleration phase on the grid structure without causing the load handling means to exceed the commanded position. Conversely, too low a speed will result in the load handling apparatus not reaching the commanded position. However, the peak or maximum speed at which the load handling device can travel is largely limited by either the maximum speed limit constraint or the command speed limit. Peak speed v capable of reaching a commanded speed close to the drive mechanism Peak to peak Depending on the run length on the grid structure. If the stroke length is long, the speed has enough time to reach the commanded speed before entering the deceleration phase. If the stroke length is short, the speed will not have enough time to reach the commanded speed without exceeding the commanded position before entering the deceleration stage.
The goal of the controller is always to reach the commanded speed, defined by the constraints, in order to provide the minimum time required to complete the point-to-point movement. When the stroke length is too short, it may not be feasible to reach the commanded speed. In case the load handling device reaches the command speed, the load handling device will go beyond the command position.Therefore, it is necessary to operate at the command speed and the initial speed v 0 The load processing device is selected to achieve the optimal peak value velocity v Peak to peak To allow the load handling device to complete the trace in a minimum amount of time.
In this particular case, the control system determines whether the track speed exceeds the commanded speed. If the answer to question D) is 313 (see FIG. 24), the control system executes a speed transformation algorithm to reduce the size of the track speed reference to the commanded speed by generating one or more track segments such that the end of the track segment corresponds to the commanded speed; otherwise, the control system will go to the next problem. The principle of the speed transformation method, which will be discussed further below, is to generate one or more track segments required to move the track from the start point to the magnitude of the speed to the final speed v f And the end point acceleration is substantially zero.
A root algorithm can be used to find the expected peak velocity v at which the trajectory can reach and will not exceed or reach the commanded position Peak to peak . The velocity transformation algorithm according to embodiments of the present invention generates the necessary track segments to transform the magnitude of the velocity to the peak velocity v Peak to peak And then back down to substantially zero. The objective function is the degree of overshoot or undershoot, and the root is the expected peak velocity, where the objective function is substantially zero.
E) Whether the current track speed is the same 314 as the commanded speed?
If the answer to question E) is no, go to the next question F. If the answer is 315 (see fig. 25), i.e. the current track speed is the same as the commanded speed, the control system will calculate one or more track segments for the remainder of the constant speed phase. To calculate one or more track segments of the constant velocity phase or the remainder of the constant velocity phase, it is necessary to calculate the distance traveled by the track from the start to gradually slow down to zero velocity. Typically, this will be calculated as a result of answering one of the previous questions, especially question C above.
The duration of one or more track segments of the remainder of the constant speed phase, and the distance that the track may still cover during the constant speed phase, is determined by subtracting the distance traveled by the track from the start to gradually slow down to zero speed from the current remaining distance. This can be explained as follows:
(1) The track may be maintained at the commanded speed for a period of time T, whereby the track position will advance by Δp At a uniform speed A kind of electronic device
(2) After a time T, the trajectory must be completed from the commanded speed to zero speed (i.e., v f =0), whereby the track position will be further advanced by Δp Deceleration of ，
(3) Thus: residual distance = Δp At a uniform speed +Δp Deceleration of From the current position of the track, the remaining distance is calculated by the command position-current position (p 0 ) And (5) calculating to obtain the product.
Δp At a uniform speed Δp Deceleration of The value of (2) is calculated by the control system as follows:
delta p is determined Deceleration of : the travel distance of the track when the speed conversion (2) is performed. This is done by a speed transformation algorithm that produces its usual output: track segment + travel distance. This distance corresponds to the distance travelled by the track in the deceleration phase (from the commanded speed to zero speed), i.e. Δp Deceleration of 。
According to (3), the control system can now calculate Δp At a uniform speed Residual distance- Δp Deceleration of 。
Finally, the duration T of the track segment in the constant velocity phase is determined as follows:
wherein:
Δp at a uniform speed Is the distance travelled at a constant commanded speed;
v max is the commanded speed.
The duration of the constant speed phase track segment is determined by the distance deltap At a uniform speed Absolute value of divided by command velocity v max Is obtained. In the track segment of the constant velocity phase, the value of the manipulated variable jerk is zero. Due to being in segments of constant speed phaseThe track speed remains constant, and the control system should therefore conclude that the magnitude of the track acceleration is zero at this point; if it is not zero, the processing of the last problem (C) ("speed overshoot") is not performed correctly, and this calculation can be used as a check on the calculation in problem (C).
If all of the above problems (a) to (E) are negative, it can be concluded that the track speed can be increased.
By the process of exclusion, the situation of the trajectory can be concluded: if the track moves to a fixed position will not overshoot, it does not currently exceed the commanded speed nor the commanded acceleration, nor does it travel at the commanded speed. Thus, it is confirmed that the magnitude of the speed can be increased. By organizing the questions in this order, the shortest time for point-to-point movement can be achieved.
F) Whether the magnitude of the track speed can be increased to the command speed 316?
In this case, the size of the track speed is in a condition that can be increased to the command speed through the process of exclusion. Here, the control system applies a first speed transformation algorithm to move the magnitude of the track speed immediately to the commanded speed, and then immediately applies a second speed transformation algorithm to move the track to a fixed position, i.e., when the magnitude of the speed is substantially zero. If the track position resulting from these two consecutive speed transitions does not exceed the commanded position, then the answer to this question is 316 (see FIG. 26), in which case the track segment generated as part of the first speed transition calculation is employed as an immediate solution.
However, if the track speed cannot be increased to the commanded speed without exceeding the commanded position, then the peak speed v is required Peak to peak Corresponding expected final speed v f Wherein the track speed may be increased such that after the second speed change the track speed reaches a fixed position at the commanded position. This will be done by applying a root algorithm in the target problem to search for the optimal peak velocity v according to the invention Peak to peak To solve, wherein the candidate peak speed is on the abscissa and the degree of overshoot distance is on the ordinate. Thus, the objective functionf (t) is the degree of overshoot or undershoot, i.e. deviation from the commanded position. Root is at command speed v max And an initial velocity v 0 Peak velocity v between Peak to peak Wherein the deviation from the commanded position is substantially zero, i.e. f (t) =0. Root algorithm from final peak velocity v Peak to peak Starting at one or more initial guesses of (c) and determining the achievement to the final peak velocity v by applying a velocity transformation algorithm Peak to peak Track segments required for the speed transformation of (a). The deviation or overshoot from the commanded position is determined by applying the trajectory equations (3 to 5). For each peak velocity v selected when applying the root algorithm Peak to peak This process is repeated. Each iteration of the root algorithm produces a progressively more accurate approximation of the root. This iteration is repeated until the digital sequence, in this case the peak velocity selection, converges towards the root, i.e. the objective function of the root is substantially equal to zero. Peak velocity v Peak to peak Obvious candidates are:
the magnitude of the initial track velocity,
the speed of the command is given by,
a weighted combination of the former two.
Further details regarding the application of root-finding algorithms are discussed below in section 5.0. The root-finding algorithm can be any one of Newton root-finding method, secant root-finding method, binary root-finding method, interpolation-based root-finding method, inverse interpolation-based root-finding method, brint's root-finding method, fourier-Booth-based root-finding method and Shi Tum chain-based root-finding method. The track segment resulting from the speed transformation (current speed to the highest speed of the scheme) will be preserved.
If the magnitude of the velocity does not need to be increased to the commanded velocity or velocity constraint at the final stage of generating the trajectory or motion control profile, the velocity transformation algorithm 318 will be applied to bring the trajectory to zero velocity at the commanded position.
4.0 speed transformation algorithm
The process of generating a finite jerk (S-curve) trajectory is very dependent on solving the so-called velocity transformation algorithm of the present invention. The speed transformation algorithm focuses on determiningFrom position p 0 Velocity v 0 And acceleration a 0 With an expected final velocity v of substantially zero magnitude from the initial trajectory point to the final acceleration f The desired track segment. Final speed v f Any value that is contemplated. For example, when it is desired to instruct the load handling apparatus to stop, v may be set f Is set to substantially zero and the speed transformation algorithm will execute the desired track segment to stop the track from the current track point. This is subjected to a commanded acceleration (a max ) Command speed (v) max ) And one or more jerk constraints J 1 、J 2 、J 3 、J 4 Is a constraint of (a). For ease of explanation and understanding of the speed transformation algorithm, one or more jerk constraints that increase the magnitude of acceleration are referred to as j Increase the number of One or more jerk constraints that reduce the magnitude of acceleration are then referred to as j Reduction of 。
The speed transformation algorithm is decomposed into sub-algorithms: the first speed transforming sub-algorithm and the second speed transforming sub-algorithm, both of which need to be executed before the speed transforming algorithm according to the present invention is completed. The first speed transformation sub-algorithm checks the validity or condition of the track and whether it is necessary for one or more track segments to properly apply the second speed transformation sub-algorithm. The second speed transformation sub-algorithm is the one that actually performs the transformation of (speed) to the desired final speed v f And regardless of v f An algorithm of what the value of (c) is. For the avoidance of doubt, the terms "speed" and "speed reference" are used interchangeably to denote the same function. Also, the terms "acceleration" and "acceleration reference" are used interchangeably to refer to the same function.
In applying the second speed transformation sub-algorithm, it must be ensured that: i) The acceleration of the track is not excessive, ii) the processed track is far from the final velocity v f Acceleration, iii) processing the final velocity v f Is set in the above-described range). If any of these conditions are not met, the control system will apply the first sub-speed transformation algorithm to generate one or more track segments to handle these anomalies or conditions, and then proceed to the second speed transformation sub-algorithm. In other words, in order to deal with the general case of a speed transformation algorithmIn the situation, the control system first needs to check these three terms: i.e. excessive acceleration, misdirected acceleration and track speed exceeding the expected final speed v f . By providing these remedial steps or generating track segments to address these sub-problems that may occur in the track, the problem is reduced to a simple problem that the second speed transformation sub-algorithm can address.
Fig. 28 is a flow chart illustrating in detail the application of the second speed transformation sub-algorithm and the checking that the second speed transformation sub-algorithm needs to perform according to a specific embodiment of the present invention.
In generating one or more track segments, the output variables are the jerk value J and the time duration T of the track segment. For the purpose of illustrating the speed transformation algorithm, the speed increment Deltav is defined as the expected final speed v f And velocity v at a given point in time 0 The difference between the two Δv=v f -v 0 Will be used to evaluate the state of the track. The value of the velocity increment Δv or the velocity may vary depending on the position in the trajectory at which the velocity conversion algorithm according to the invention is applied. This is because the velocity reference v (t) depends on the value of the velocity reference taken from the position in the track, i.e. the velocity track at the track point. Preferably, the speed delta is based on a difference between the expected final speed reference and an initial speed reference or an end speed reference of one or more track segments.
The process starts with initializing an output list of track segments and determining if the conditions for starting the speed transformation algorithm are correct.
4.1 application of first speed Change sub-Algorithm
However, the second speed transformation sub-algorithm cannot start unless certain conditions are met or processing is required whether there is an anomaly or situation in the track. The condition for effective start of the second speed transformation sub-algorithm is one of:
a) Track from zero track acceleration a 0 Initially, already at the desired final speed v f . Mathematically, this is given by the following condition:
Δv=0 and a 0 ＝0
b) The trajectory is not already at the expected final velocity v f And (2) andthe locus is facing v f Acceleration, but the track acceleration does not exceed the constraint of the commanded acceleration or the initial acceleration is zero. Mathematically, this is given by the following condition:
(Deltav. Noteq.0) and (Deltav. Times.a) 0 Not less than 0) and (|a) 0 |≤a max )
c) The trajectory is not already at the expected final velocity v f And initial acceleration a 0 Zero. Mathematically, this is given by the following condition:
(Deltav. Noteq.0) and (a) 0 =0) and (|a) 0 |≤a max )
These conditions are shown in fig. 28 as decision blocks 504, 506, 508, and 510. In contrast to the condition that the second speed transformation sub-algorithm is satisfied, the second speed transformation sub-algorithm cannot be executed unless it is necessary to handle some anomaly or condition in the trajectory, such as excessive trajectory acceleration. These sub-problems are solved by a first speed transformation sub-algorithm. If these anomalies or conditions are not addressed, the second speed transformation sub-algorithm will not be able to transform the speed to the desired final speed v f . These start conditions, referred to as invalid, are shown in the flow chart of fig. 28 as blocks labeled "invalid start conditions" 512, include:
a) The trajectory is already at the desired final velocity v f But initial acceleration a 0 Is not zero. In other words, the trajectory is accelerating away from the expected final velocity v f . To correct this anomaly, three or four track segments are required: one moves the magnitude of the track acceleration to zero, and then the velocity transformation sub-algorithm may generate two or three additional track segments to bring the magnitude of the track velocity back to the desired final velocity v f . Mathematically, the invalid start condition is given by:
Δv=0 and a 0 ≠0
b) The trajectory is accelerating away from the expected final velocity v f . In this case, deltav and a 0 Opposite sign (i.e. Deltav is positive and a 0 Is negative, or Deltav is negative and a 0 Positive) and requires three or four track segments: one to zero the magnitude of the track acceleration, then the velocity transformation sub-algorithm may generate two or three additional track segmentsAllowing the magnitude of the track velocity to return to the desired final velocity v f . Mathematically, the invalid condition is given by:
Δv×a 0 <0
c) Initial acceleration a 0 Exceeding a maximum constraint or commanded acceleration a max . One or more track segments are required to move the track acceleration to the commanded acceleration a max . Mathematically, the invalid condition is given by:
|a 0 |>a max
before the speed transformation algorithm is completed, an abnormality in the trajectory that does not satisfy the condition or necessary condition for applying the second speed transformation sub-algorithm will be discussed first in order to let the trajectory be in the condition for applying the second speed transformation sub-algorithm. These cases are addressed by the first speed translation sub-algorithm and are described as sub-problems that need to be addressed in the particular order noted in the flow chart 400 shown in fig. 27. Solving these anomalies will result in a sequence of one to five track segments that together constitute a solution to the speed transformation algorithm.
In solving the sub-problem and generating one or more track segments, in addition to J Increase the number of (J inc ) And J Reduction of (J dec ) The preempted jerk value J is also in some embodiments Lowering blood pressure Input 402 is introduced as shown in table 1.
Table 1: the input and output of the first speed transformation sub-algorithm is applied.
J Lowering blood pressure And J Reduction of Different because of preemptive jerk J Lowering blood pressure Needed to be in contact with J Reduction of Different ways reduce the magnitude of jerk to substantially zero. In other words, J Lowering blood pressure To be equal to J Reduction of The different rates reduce the magnitude of the acceleration and are required when handling the following two initial checks (excessive acceleration and acceleration away from the final speed). Thus, when the acceleration is too great or the trajectory is accelerating away from the desired final velocity v f When the control system will apply the preempted J Lowering blood pressure To reduce the acceleration to substantially zero. The above explanation applies to J Lowering blood pressure And J Reduction of Embodiments with different values. In other embodiments, J Lowering blood pressure And J Reduction of The values of (c) may be the same.
The flow chart 400 shown in fig. 27 and described below details the solution of the sub-problem of the first speed transforming sub-algorithm and their order of resolution before the application of the second speed transforming sub-algorithm to generate a trajectory according to the present invention. The sub-problem is decomposed into one or more blocks, each of the one or more blocks representing a solution to the sub-problem addressed by the first speed transformation sub-algorithm. Each solution to the sub-problem generates one or more track segments that are input into the output list. Initially, the output list of track segments is empty. For the purpose of explaining the speed transformation algorithm, the term "integration" in the context means "apply track equations 3, 4 and 5 (using the starting point, track segment jerk and segment duration) to reach the segment end point".
A) Handling excessive accelerations
During the decision process and prior to applying the second speed transformation sub-algorithm of the present invention, the control system determines the initial acceleration |a 0 Whether the magnitude of i is greater than the constraint command acceleration a max . This is represented by decision block 404 in fig. 27. If the answer is yes, the control system adds track segment 406 to the output list. The new track segment reduces track acceleration to a max Then try again to solve the second velocity transformation sub-algorithm-in this case a track segment must be generated, wherein:
the manipulated variable jerk values used are: j= -sign (a 0 )×J Lowering blood pressure . The negative sign is to change the acceleration from the initial acceleration a 0 Necessary for reduction.
Track segment duration is defined by T 1 ＝|a max -|a 0 ||/J Lowering blood pressure Given.
By applying the manipulated variable jerk J, the end point trajectory acceleration a E1 Will be represented by a E1 ＝sign(a 0 )×a max Given.
By directly applying the trajectory equations 3, 4 and 5, the starting point of use (a 0 、v 0 、p 0 ) Jerk J and track segment duration T 1 Initial position p 0 Initial velocity v 0 End point velocity v E1 And end position p E1 Can be obtained thereby. In the flow chart, this is shown as going from the initial point (v 0 、a 0 ) Integrating 408 the trajectory, calculating the generation speed v using all segments in the output list E1 And acceleration a E1 。
If a track segment is generated in this step, then its end point will become the initial track point for the next sub-problem, i.e. (a) 0 、v 0 、p 0 )←(a E1 、v E1 、p E1 ) And according to Deltav≡v f –v E1 The speed increment 412 is recalculated. In other words, the end point of the generated track segment is assigned as the start point of the subsequent track segment. This is represented as block 410 in fig. 27.
However, if the initial acceleration a 0 Is less than or equal to a in size max And the control system transitions to the next sub-problem, then no new track segment needs to be generated to reduce the excessive acceleration to a max Is carried out by a method comprising the steps of.
Regardless of the outcome of decision block 404 (i.e., acceleration a 0 Whether or not the size of (a) is larger than a max ) The algorithm continues to steps 408 and 410. Step 408 integrates the entire track from the starting point, including outputting all track segments in the list. In case the acceleration is too great and a new track segment has been generated in step 406, the segment is integrated. In the case that the acceleration is not too great and a new track segment has not been generated, the output list of track segments is empty, the integration step 408 will still continue to be performed but will not have any effect, thus generating the velocity v E1 And acceleration a E1 Will be equal to the initial velocity v 0 And initial acceleration a 0 The same applies.
Similar to step 408, the subsequent steps of integrating the trajectory in steps 420 and 436 will still be applied, regardless of the result of the previous decision block. The integration step is applied to the entire track from the starting point, whether or not a new track segment has been added to the output list in the previous step.
B) The process accelerates away from the expected final speed v f Is a trajectory of (a).
This is represented in fig. 27 by decision blocks 414 and 416. The control system applies a decision process to cope with a speed of magnitude v f But accelerate away from v f Is not at v, and/or the speed is not at v f And accelerate away from v f Is an exception (condition) 416. In the former anomaly 414, the control system determines whether the velocity delta Δv is equal to zero, i.e., Δv=0, and the initial acceleration a 0 Is not equal to zero in size, i.e. a 0 Not equal to 0. If both conditions exist, it is indicated that the trajectory is accelerating away from the expected final velocity v f . If neither condition exists, control passes to decision block 416 and determines whether the trajectory accelerates away from v f . To determine if this is the case, the control system determines an initial trajectory acceleration a 0 Whether the product of the magnitude of (a) and the velocity increment Δv is negative (Δv×a) 0 <0) 416. If the product is negative, it indicates that the trajectory is accelerating away from the expected final velocity v f I.e. in opposite directions. If acceleration a 0 Is opposite in sign to the velocity increment Deltav, deltav x a 0 The product of (2) will be negative, i.e. the trajectory is accelerating in the opposite direction to that required to reach the final speed.
If either of decision blocks 414 and 416 is affirmative, i.e., the trajectory is accelerating away from the desired final velocity v f Track segment 418 must be generated to correct the anomaly by reducing track acceleration to zero before the second velocity conversion sub-algorithm can be applied. For the new track segment 418 to be used,
the manipulated variable jerk used is: j= -sign (a 0 )×J Lowering blood pressure . The negative sign is necessary to reduce the acceleration to substantially zero;
track segment duration is defined byIs given;
the magnitude of the end point acceleration is zero (a E2 =0). By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point (a 0 、v 0 、p 0 ) Jerk J and track segment duration T 2 Initial position p 0 Initial velocity v 0 End point velocity v E2 And end position p E2 Can be obtained thereby. In the flow chart, this is shown as being from the starting point (v 0 、a 0 ) Integrating 420 the trajectory, calculating the endpoint velocity v using all segments on the output list E2 And end point acceleration a E2 。
If such a track segment is generated in this step, its end point becomes the initial track point for the next sub-problem, i.e. (a) 0 、v 0 、p 0 )←(a E2 、v E2 、p E2 ) And according to Deltav≡v f –v E2 The speed increment is recalculated, i.e. the values 420, 422 are reassigned.
However, if in either of decision blocks 414 and 416 both conditions are negative, then either the trace is indicated as being at v f And acceleration is substantially zero, or the trajectory is facing v f Acceleration, then the control system maintains the initial trajectory point (a 0 、v 0 、p 0 ) And goes to the next sub-problem.
C) Processing trajectories beyond final speed
The third check is represented by decision block 424 in fig. 27. The first check in step a) determines that the trajectory acceleration is not too great, while the second check in step B) determines that the trajectory is not accelerating in the "wrong" direction. Thus, by then, the track acceleration reference a will either be zero (in which case the third check will not perform any operation, since the track is not actually accelerating at that time), or the track will accelerate to the desired final velocity v f (and possibly beyond it). In this case and prior to applying the second speed transformation sub-algorithm, the control system will determine as a final check whether the trajectory exceeds the expected final speed v f . Decision makingThe process begins with a check decision block 424: initial track acceleration a 0 Whether the size of (c) is substantially equal to zero. If the initial track acceleration a 0 Is substantially equal to zero, indicating that the condition for applying the second-speed transform sub-algorithm is valid. However, if the track acceleration is not substantially equal to zero, then there will be a track speed that is still on the rising track and will exceed the expected track final speed v f Risk of (2). When a third check is applied, this check helps to capture this and applies a track segment that reduces the magnitude of the acceleration to zero, which is only used to do so the track speed will go beyond and continue away from the expected final speed v f And (3) moving.
If the track speed crosses and exceeds the expected final speed v f The next corrective action is taken before the second speed transformation sub-algorithm is applied. At first meeting, a heuristic trajectory segment is calculated, wherein the applied acceleration is j=j Reduction of With J used in the previous two examinations Lowering blood pressure Unlike, but the heuristic track segments are not added to the output list. Due to J in the second sub-algorithm Reduction of Will then be applied to transform the speed to the desired final speed v f The same jerk value J is continued to be used in the third inspection Reduction of Is significant because the trajectory is also facing the expected final velocity v f Advancing, and possibly exceeding it. Track speed just exceeding v f The possibility of (if the third check determines that the track speed does exceed) does not prevent use and if the track can be used at a speed not exceeding v f Falls under v f The same jerk value is used in the case of (a). In the specific case of excessive track acceleration and/or tracks accelerating in the wrong direction and needing to return, preemptive acceleration j=j will be used Lowering blood pressure 。
In step 426, the trail is integrated using the tentative trail segment to calculate the generation speed v of the load handling device at the end of the tentative trail segment aux . This speed is needed in the next step to calculate if the trajectory exceeds the final speed v f 。
Determining that the track speed is in decision block 432Whether the final speed v is exceeded f In the time-course of which the first and second contact surfaces,
the speed increment Deltav, 428 will be calculated, the speed increment representing the initial speed v from the last check 0 To the expected final speed v f Is a speed transformation of (a). Thus, deltav represents the initial velocity v 0 Relative to the expected final speed v f Overshoot degree of (2);
speed increment Deltav aux And 430 represents the end velocity v of the heuristic trajectory section calculated in step 426 aux To the expected final speed v f Is a speed transformation of (a). Thus, deltav aux Representing the v calculated at present aux Relative to the expected final speed v f Is not exceeded.
If the track segment overshoots during the final inspection, v x v aux The product of (a) will be less than zero or negative (Deltav x Deltav aux <0) I.e. speed increment Dv or Dv aux One is negative (see decision block 432 in fig. 27). If (Dv×Deltav) aux <0) It is clearly shown that the track segment exceeds the expected final velocity v f . In this case, in step 434, the control system adds the previously calculated heuristic track segments to the output list to make the magnitude of the acceleration substantially zero. For new track segments (now no longer heuristic):
the manipulated variable jerk value used is J Reduction of Rather than preemptive jerk J Lowering blood pressure . The jerk applied is therefore j= -sign (a 0 )×J Reduction of . A negative sign is necessary to reduce the acceleration to substantially zero, i.e. to represent the deceleration.
Track segment duration is defined by T 3 ＝|a 0 |/J Reduction of Given.
End point acceleration of zero (a E3 =0). By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point (a 0 、v 0 、p 0 ) Jerk J and track segment duration T 3 Initial position p 0 Initial velocity v 0 End point velocity v E3 And end position p E3 Can be obtained thereby. In the flow chart, this is shown asFrom the initial point (v 0 、a 0 ) Integrating 436 the trajectory, calculating the generation speed v using all segments in the output list E3 And acceleration a E3 . End point velocity v E3 Equal to v aux 。
Thus, the third check ensures that the acceleration reference a gradually decreases to substantially zero and is prevented from further moving away from the expected final speed v f Accelerating. If the track period exceeds the expected final velocity v while the acceleration reference a is gradually reduced to zero f This condition is accepted. The purpose of the third check is to prevent the trajectory from accelerating further away from the expected final speed v f It is therefore necessary to gradually reduce the acceleration reference to substantially zero. Again, the endpoint becomes the initial trajectory point for the speed transformation algorithm, i.e. (a) 0 、v 0 、p 0 )←(a E3 、v E3 、p E3 ) 438, and according to dv=v f –v E3 The speed or speed increment is recalculated. The updated trajectory point then becomes the initial trajectory point for the next sub-problem.
If the track segment does not exceed the expected final velocity v f The track segment is completely ignored, i.e. no modification of the initial track point or speed increment av is required.
Once the trajectory condition is set (e.g., the magnitude of the acceleration is moved to substantially zero), which involves generating one or more trajectory segments in a first speed transformation sub-algorithm, the control system will enter the next stage of the speed transformation algorithm, the second speed transformation sub-algorithm 440, to transform the magnitude of the speed to the desired final speed. Finally, by integrating 442 all of the generated track segments, the total distance Δp resulting from the application of the first speed transformation sub-algorithm may be determined. The application of the second speed transformation sub-algorithm will be discussed further in section 4.2 below.
The following is an embodiment of applying the first speed transformation sub-algorithm in different scenarios of track segments. All of these examples use the international system of units: distance in m, time in s, speed in m/s, acceleration in m/s 2 In units of jerk in m/s 3 Is a single sheetBits.
Example 1
In the following embodiments, the input values are:
initial track acceleration a 0 ＝1.0
Command acceleration a max ＝2.0
Initial velocity v 0 ＝1.0
The expected final speed v f ＝1.1
Dv＝v f –v 0 ＝0.1
Jerk value J Lowering blood pressure ＝J Increase the number of ＝J Reduction of ＝0.1
A) The first problem is: is the acceleration excessive? No, |a 0 |=1.0 and |a max |=2.0, so the acceleration a is initiated 0 And not too large.
B) A second problem: whether the track accelerates away from v f ? No, dv=0.1 is positive and a 0 Is positive, so the track is facing v f Accelerating.
C) Third problem: even if the acceleration immediately starts to decrease to zero, the trajectory will exceed v f Is? Is the result.
Using J Reduction of Jerk value of 0.1, will be a 0 Gradually decrease to zero: t= |a 0 I/j=1.0/0.1=10 time units. When the acceleration gradually decreases to zero, the track velocity increases by 10 x 1.0/2=5.0 (right triangle area, bottom 10 time units, height a 0 =1.0). Thus, even if the control system attempts to gradually reduce the track acceleration to zero, the track velocity will still be from v 0 =1.0 increases to 6.0, further exceeding v f ＝1.1。
In this embodiment, the first two checks do not result in any operation (no operation is required), but still a third check exists to handle v f Otherwise the second sub-speed transformation algorithm (start condition invalid) cannot be invoked.
Example 2
Command acceleration a max ＝2.0
Initial acceleration a 0 ＝-3.0
Initial initiationVelocity v 0 ＝1.0
The expected final speed v f ＝3.0
Dv＝v f –v 0 ＝3.0-1.0＝2.0
J Lowering blood pressure ＝1.0
J Increase the number of ＝J Reduction of ＝0.5
A) The first problem is: is the acceleration excessive? Is the result. I a 0 |=3.0 is greater than a max The control system must therefore create j= +j Lowering blood pressure Track segment of = +1.0, duration t= |a max -|a 0 ||/J Lowering blood pressure ＝1.0/1.0＝1.0
--->The end point of the segment is a E (end acceleration) = -2.0, v E (end speed) =0.5
--->Note that the acceleration is still negative, since this step only works by reducing the magnitude of the acceleration so that it is no longer too large (i.e. higher than a max )
--->Recalculate dv=v f -v E ＝3.0-(0.5)＝2.5
--->Reassignment a 0 = -2.0 and v 0 ＝0.5
B) A second problem: whether the track accelerates away from v f ? Is the result. Dv=2.5 (positive value) and a 0 = -2.0 (negative value), so the trajectory is accelerating positively away from v f . Therefore, we have to create j= +j Lowering blood pressure Track segment of = +1.0 and its duration is t= |a 0 |/J Lowering blood pressure
＝2.0/1.0＝2.0
--->The end point of the segment is the acceleration a E ＝0,v E ＝-1.5
--->Recalculate dv=v f -v E ＝3.0-(-1.5)＝4.5
--->Reassignment a 0 =0 and v 0 ＝-1.5
C) Third problem: even if the acceleration immediately starts to gradually decrease to zero, whether the trajectory exceeds v f ? No, because the current acceleration is already zero.
In example 2, the first two inspections produced the operation (each inspection produced one track segment), but the firstThe three checks did not result in any operation (no operation was required). From the previous three queries, the trajectory now has valid conditions and can continue to execute a second-speed transformation sub-algorithm with a 0 ＝0、v 0 ＝-1.5、v f The initial trajectory point, i.e. the acceleration, of =3.0 has been gradually reduced to substantially zero. Applying the second velocity transform sub-algorithm will create two more track segments (because a Peak to peak As a result, it was 1.5, lower than a max ) To move the track speed to v f (and move the acceleration to zero). a, a Peak to peak Is critical for the application of the second sub-speed transformation algorithm.
4.2 application of the second-speed transformation sub-algorithm
The inputs and outputs of the second speed transformation sub-algorithm are shown in table 2 below. Once the previous sub-problem has been solved by the first speed transformation sub-algorithm as described above, the control system can invoke the second speed transformation sub-algorithm. The second speed transformation sub-algorithm is to actually perform the transformation of (speed) to v f Irrespective of v f How the value of (c) is. It will only be invoked if the specific start condition described above has been resolved. This includes, but is not limited to, the trajectory having excessive acceleration, and/or the trajectory accelerating away from the desired final speed. The checking is performed by blocks 504, 506, 508, 510 in fig. 28 and is described above. The purpose of the speed transformation algorithm is to generate track segments to achieve a selected speed v f . Final speed v f Any speed selection subject to speed constraints may be used. However, if the velocity or velocity delta Dv is substantially zero-i.e., the track velocity v 0 Has been at the desired final speed v f And acceleration a 0 In which case the trajectory does not accelerate away from the desired final velocity v f Then the conditions to reach the desired final speed have matured. This is illustrated by the path from decision block 506 to the end of the flow chart (fig. 28) of the second speed transformation sub-algorithm.
According to the invention, a useful concept found to be useful for the purpose of the second speed transformation sub-algorithm is the speed increment Deltav, which is defined as the expected final speed v f And at the beginningInitial velocity v 0 The difference between them. Initial velocity v 0 The value of (2) depends on the value assigned to the initial velocity v in the first velocity transformation sub-algorithm 0 End point velocity reference v of (2) E . Referring to the flowchart 500 shown in fig. 28, the peak trajectory acceleration a is calculated Peak to peak Is represented by the acceleration value at the vertex of an imaginary triangle, the area of which is given by Dv, and the edges by the rate of increase of acceleration (away from zero acceleration) J Increase the number of And rate of decrease (towards zero acceleration) J Reduction of Definition (see fig. 29). In other words, a Peak to peak Acceleration values at the vertices of a triangle-like shape may be used at a rate J that increases the acceleration magnitude Increase the number of And a rate J at which the acceleration is reduced in magnitude Reduction of Is drawn so that the area of this shape corresponds to or corresponds to Dv. Reaching block 514 depends on the conditions in decision blocks 504, 508 and 510—the valid starting conditions, i.e., (i) dv+.0; (ii) Deltav×a 0 Not less than 0; and (iii) A 0 |≤A max . These conditions should be fulfilled because the track has completed the first speed transformation sub-algorithm, so any excessive acceleration or anomalies in the track accelerating away from the final speed should have been resolved by adding track segments to the output list.
Table 2: and applying the input and output of the second speed transformation sub-algorithm.
In detail, given an initial acceleration a 0 Initial velocity v 0 (defined by the output of the first speed transformation sub-algorithm), the desired final speed v f Speed increment Δv=v f -v 0 And for increasing (J Increase the number of ) And decrease (J) Reduction of ) Specific values of jerk for the magnitude of the trace acceleration, it is possible to draw an imaginary "triangle acceleration trace" (see fig. 29) So that a 0 The area on the right is equal to Δv. In other words, performing such a change in the triangular shaped acceleration signal will give v 0 Becomes v f (regardless of the number of track segments) to solve the second speed transformation sub-problem. According to a first principle, assuming an area equal to the delta speed Δv under an imaginary triangular acceleration trajectory, then:
A 0 +A 1 +A 2 ＝Δv (7)
wherein A is 0 、A 1 And A 2 Is the area shown in fig. 29. Let a Peak to peak The value of acceleration at the vertex of the virtual triangle. The portion of the total area under the triangle corresponding to Δv can be expressed as the sum of three smaller areas, i.e., Δv=a 0 +A 1 +A 2 Wherein:
A 0 ＝L 1 a 0 (8)
solving for
Δv＝A 0 +A 1 +A 2
Magnitude of peak accelerationThis can be obtained by taking the square root of the equation:
once the peak acceleration a is calculated by equation 21 Peak to peak One of three mutually exclusive possibilities may be true, each of which is described in detail below:
0 peak to peak A) The initial acceleration a is matched with the peak acceleration a
If the magnitude of the initial acceleration is equal to the peak acceleration 516, i.e., |a 0 |＝|a Peak to peak I, the trajectory has reached peak acceleration, only the acceleration is gradually reduced (deceleration) to zero to reach the desired final velocity v f 。
In this case, a single track segment 518 is generated, wherein:
i) The manipulated variable (jerk) has a value j= -sign (a) 0 )×j Reduction of In order to reduce the acceleration, the acceleration is reduced,
ii) track segment duration is defined byGiven, sum
iii) The end point acceleration is zero (a E4 =0), the end point speed v E4 ＝v f And the end position is obtained by directly applying the trajectory equations (equations 3, 4 and 5), using the start point (a 0 、v 0 、p 0 ) Acceleration j Reduction of And a segment duration T.
Peak to peak max B) The peak acceleration a is less than or equal to the maximum acceleration a (command acceleration)
If the magnitude of the peak acceleration is less than the magnitude 526 of the maximum acceleration, i.e., |a Peak to peak |<a max The magnitude of the track acceleration can still be increased to |a Peak to peak I, then it is required to gradually decrease to zero, at which point the desired final speed v will be reached f 。
Thus, in this case, two track segments 528, 530 need to be generated, and can be illustrated by the phantom triangle in fig. 29, fig. 29 showing a first track segment 528b to peak acceleration and a second track segment 530b tapering to substantially zero.
For the first track segment 528b,
the manipulated variable (jerk) value used is j=s×j Increase the number of . The sign S depends on the initial acceleration a 0 Whether 0 is positive or negative. As shown in the flow chart, if the initial acceleration a 0 Almost equal to zero, the sign S is equal to the initial velocity v 0 With the same sign, because the track segments need to step up or down the track acceleration depending on the direction of the track velocity to reach the final velocity v f . However, if the initial acceleration a 0 If not zero, the sign S is the sign of the initial acceleration so as to gradually increase or decrease the track acceleration to bring the velocity to the final velocity v f 。
The duration of the first track segment is t= |a Peak to peak -a 0 |/J Increase the number of 。
The end point acceleration is the peak acceleration (a E5 ＝a Peak to peak ) End point velocity v E5 And end position p E5 By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point a 0 、v 0 、p 0 Jerk j=j Increase the number of And a segment duration T.
Reassigning its end point to the initial track point of the second track segment, namely: (a) 0 、v 0 、p 0 )←(a E5 、v E5 、p E5 )。
For the second track segment 530b,
manipulated variable (jerk) value j=s×j Reduction of ；
The second track segment duration is t= |a Peak to peak |/j Reduction of ；
The end point acceleration is substantially zero (a E6 =0), end point velocity v E6 And end position p E6 By directly applying the trajectory equations (equations 3, 4 and 5), the starting trajectory point a is used E5 、v E5 、p E5 Jerk j=j Reduction of And a segment duration T.
max C) The peak acceleration is greater than the maximum acceleration a
Next, if not |a Peak to peak |<a max Then |a Peak to peak I must be greater than a max . If the magnitude of the peak acceleration is greater than the magnitude of the maximum acceleration, i.e. a Peak to peak >a max The magnitude of the track acceleration must be increased to a all the time max Then must be at a max For a period of time, eventually it is necessary to gradually reduce it to zero in time to reach the desired final speed v f . In other words, the acceleration trajectory will take on a trapezoidal shape. Thus, in this case, two or three track segments may be generated, depending on the magnitude of the track acceleration, which is already equal to a max Or still be smaller than a max 。
First by combining a Peak to peak ＝a 0 ＝a max Substituting into equation 21 to calculate the velocity of the acceleration from a max The change in speed from gradually decreasing to zero is then calculated to give:
wherein, by T in FIG. 30 Constant-force device The (time) length of the represented constant acceleration phase can also be calculated as:
wherein:
the |Deltav| is the initial velocity v at the beginning of the constant acceleration phase 0 To the expected final speed v f Is used for the speed change of the speed sensor. As shown in fig. 30, the constant acceleration phase and acceleration gradually reduce the total area a under the track segments 540 and 542 2 +A 3 For speed increment Δv=v 0 -v f (wherein v 0 Is the speed at which the constant acceleration phase begins). Area A under acceleration taper section 542 3 Dv calculated for equation 22 Reduction of . Area A 2 By subtracting area A from the total area 3 Calculated, thus A 2 ＝Dv-Dv Reduction of . According to area A 2 Duration T Constant-force device This area can be divided by the acceleration a in equation 23 max To calculate.
If |a 0 |<a max Then have toThree track segments have to be generated. This is represented by decision block 532 in fig. 28 and additional track segments 534 generated over track segments 540 and 542, and is illustrated by the imaginary trapezoidal shape in fig. 30.
For the duration T in fig. 30 1 A first track segment 534 within:
the manipulated variable (jerk) value used is j=s×j Increase the number of ，
Track segment duration is defined byGiven, sum
The end point acceleration is the maximum acceleration (a E7 ＝sign(a 0 )×|a max I), end point velocity v E7 And position p E7 By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point a 0 、v 0 、p 0 Acceleration j Increase the number of Sum of segment duration T 1 To obtain the product,
reassigning its end point to the initial track point of the second track segment, namely: (a) 0 、v 0 、p 0 )←(a E7 、v E7 、p E7 )。
For duration T 2 Shown as T in fig. 30 Constant-force device Second track segment 540 in:
the manipulated variable (jerk) value used is zero,
t for a segment duration given by equation 23 2 ＝T Constant-force device Give and give
The end point acceleration is the maximum acceleration (a E8 ＝a 0 ) (note: a, a 0 ＝a E7 From previous calculations), and an end point velocity v E8 And position p E8 By directly applying the trajectory equation, using the starting point a E7 、v E7 、p E7 Zero jerk and segment duration T 2 ＝T Constant-force device The process is carried out in a manner that,
reassigning its end point to the initial track point for the second track segment generation, namely: (a) 0 、v 0 、p 0 )←(a E8 ,v E8 ,p E8 )。
For the duration T in fig. 30 3 Third track section 542 in:
the manipulated variable (jerk) value used is j= -S x J Reduction of ，
Track segment duration is defined byGiven, sum
End point acceleration of zero (a E9 =0), the end point speed v E9 ＝v f End position p E9 By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point a E8 、v E8 、p E8 Acceleration j Reduction of And a segment duration T.
However, if a is from the first speed transformation sub-algorithm 0 ＝a max Only two track segments 540, 542 have to be generated: first track segment at time T Constant-force device Internal acceleration is a max The second track segment transfers the acceleration from a max Gradually decreasing to zero. This is passed in the trace through decision block 532 sub a of the flow chart 0 |<a max The no decision branch of (c) is displayed and the two track segments added are represented by blocks 540 and 542 in fig. 28 and 30. This is because if |a 0 |<a max If "no", then the obvious choice is |a 0 |＝a max Because one of the three checks is whether the magnitude of the initial acceleration is greater than a when the speed transformation algorithm is applied max 、|a 0 |>a max I.e. too high acceleration, and a is to be taken 0 Move to a max (see block 406 in fig. 27).
For |a 0 |＝a max Gradually increase to the maximum acceleration a max Is omitted in fig. 30, and the magnitude of the track acceleration has reached the maximum acceleration a max 。
For duration T Constant-force device First track segment 540 in:
the manipulated variable (jerk) value used is zero,
the duration of the segment is defined by T 2 ＝T Constant-force device Given, sum
The end point acceleration is the peak acceleration (a E8 ＝a 0 ) End point velocity v E8 And end position p 8 By directly applying the trajectory equation, using the starting point a 0 、v 0 、p 0 Zero jerk and segment duration T 2 Is obtained.
For duration T 3 Second track segment 542 in:
the manipulated variable (jerk) value used is j= -S x J Reduction of ，
Track segment duration is defined byGiven, sum
End point acceleration of zero (a E9 =0), the end point speed v E9 ＝v f End position p E11 By directly applying the trajectory equations (equations 3, 4 and 5), using the starting point a E8 、v E8 、p E8 Acceleration j Reduction of Sum of segment duration T 3 Is obtained.
When the expected final speed v is reached f When the second speed transformation sub-algorithm according to the invention is terminated. When commanded to rest in the commanded position, a final velocity v is expected f May have any value, for example substantially zero.
Examples
FIGS. 31 (a through d) illustrate the speed change algorithm according to the present invention when applied to a different final speed v f —
-different trajectories or motion control profiles generated when ranging from-34.052 to 18.167. All the generated profiles have the same starting point (p 0 、v 0 、a 0 ). As is clear from fig. 31 (a), the transition to a different final speed v f Different travel lengths result and therefore different position references in turn may deviate differently from the commanded position. Track segment when generating trackThe amount depends on the final speed v f At most four track segments can be generated (see fig. 31 d). The controller uses the trajectory or motion control profile to generate or calculate the feed forward torque demand discussed in section 2.0 above.
5.0 root-finding algorithm
In generating a trajectory or motion control profile, it is critical to control the trajectory speed in such a way that the trajectory falls at the commanded position in a minimum amount of time without undershoot or overshoot. For example, the load handling apparatus is provided with a command speed v when the track distance is relatively short and there is little time for the load handling apparatus to reach the command speed v max In the case of a track that is required to undergo multiple speed transitions all the way from the initial starting position to accurately reach the commanded position without undershoot or overshoot. This involves the trajectory at peak velocity v Peak to peak Travel and then gradually decelerate back to the desired final speed v f 。
Seeking to travel without exceeding the commanded speed v max Is the expected peak velocity v of (2) Peak to peak So that it is challenging for the load handling device to complete the trace in the shortest time. According to an embodiment of the invention, a root algorithm is used to predict the initial velocity v of the trajectory 0 And maximum speed constraint v max The "sweet spot" velocity (root) in between, wherein the objective function represents the deviation of the trajectory from the commanded position, i.e., the trajectory point at which the end-point trajectory acceleration and end-point velocity are substantially zero. The root algorithm is not limited to use only where the trajectory is at the beginning of the motion control profile. Based on one or more conditions of the trajectory, such as one or more constraints or commanded positions, the motion profile generator may regenerate the motion control profile in response to one or more conditions different from the initial one or more conditions. The regeneration of the motion control profile may be instructed to take place during an acceleration phase and/or a deceleration phase of the motion control profile. In this case, a root algorithm may be applied to the regeneration phase of the motion control profile to predict the "sweet spot" velocity for reaching the distance left by the commanded position.
The use of the root algorithm is not limited to the track speed, but it can also be applied to different parameters of the track. For example, a root algorithm may be applied to find the desired trajectory acceleration or deceleration to reach the commanded position in the shortest time and/or not over-shoot.
In the case where the parameter is track velocity, the root algorithm is applied to explore different peak velocities v along with the velocity transformation algorithm described above Peak to peak To generate track segments that arrive at different track speeds. The root algorithm will use the results of the track segments generated by the velocity transformation algorithm to determine the degree of overshoot or undershoot relative to the commanded position. Thus, as in the "peak speed search" problem, this means that the root algorithm is trying to "expected peak speed" v Peak to peak And searching v with substantially zero overshoot in conjunction with a speed transformation algorithm Peak to peak Values. According to the invention, when the final speed v is expected f At a command speed v max When the command speed is within the range and can be increased, the root algorithm will in this case call the speed transformation algorithm.
The root algorithm generates one or more track segments to move the magnitude of the velocity to the desired peak velocity v each time the velocity transformation algorithm is invoked Peak to peak And back to the desired final speed v f 。
Take the embodiment shown in fig. 32 as an example, where the expected peak velocity v is required Peak to peak To be from the initial position p 0 Travel to final speed v f And a commanded position where acceleration is substantially zero. The top profile (fig. 32 a) represents the magnitude of the position reference as a function of time, the middle profile (fig. 32 b) represents the magnitude of the velocity reference as a function of time, and the bottom profile (fig. 32 c) represents the magnitude of the acceleration reference as a function of time. FIG. 32 shows a plurality of velocity profiles at a relatively low peak velocity v A To a relatively large peak velocity v C A plurality of different peak speeds "selected" therebetween. The root algorithm is used to find the ideal peak velocity v between the upper and lower limits corresponding to the commanded velocity Peak to peak So that the trajectory completes a point-to-point movement from a given initial point to a given end point without overshooting or undershooting (see fig. 32 b). For the purposes of explaining the present invention, the term "deviation" from the commanded position is to be construed broadly to includeThe track does not reach or exceed the commanded position. In the embodiment shown in fig. 32, for each candidate peak velocity v Peak to peak Two speed changes will occur:
1) From the initial velocity v 0 To the desired peak speed (say v A ) And (2) first speed change of
2) From the initial velocity v A (peak velocity v will be A Reassigned to the initial speed) to an expected final speed v corresponding to substantially zero f Is a second speed transformation of (a).
In each iteration of the root algorithm, a track segment is generated by applying the first and second velocity transforms to determine the degree of deviation from the commanded position, i.e., the track segment is integrated by applying the track equations (3, 4, and 5) to generate the accumulated endpoint position p E . The deviation of the cumulative end point for the continuous value of the speed peak from the commanded position indicates the degree of undershoot or overshoot from the commanded position, i.e., the deviation. Thus, each iteration of the root-finding algorithm in combination with the application of the velocity transformation algorithm produces a progressively more accurate approximation of the root, in this case the expected peak velocity v Peak to peak . Another way to look at this process is that one or more speed transformation algorithms are nested in each iteration of the root algorithm. For each iteration of the root algorithm, at least one velocity transformation algorithm is used to determine a calculated transformation to peak velocity v Peak to peak The necessary track segments and thus the degree of deviation from the commanded position are determined. The "sweet spot" velocity shown in the graph between the deviation and the velocity in FIG. 33 shows the expected peak velocity v between the magnitude of the initial velocity and the commanded velocity at substantially zero deviation Peak to peak . In the particular embodiment shown in fig. 33, the "sweet spot" velocity is 29.63 when the objective function is almost zero.
When changing to the "root" speed so that the final speed v f At a commanded position of substantially zero, the trajectory or motion control profile that constitutes the trajectory segment generated by the speed transformation algorithm may be referred to as an optimal motion control profile or optimal trajectory. The application of the root-finding algorithm is not limited to the finding of the optimal peak speed, but the root-finding algorithm can be implemented in the embodiment of the inventionIn a manner called for optimal deceleration constraints so that the trajectory can be effectively decelerated to fall at the commanded position when the speed transformation algorithm described above is applied. See section 7.0 below, where there are more details of applying a root algorithm to find the optimal deceleration constraint.
6.0 return
There may be situations where the track exceeds the commanded position. This may be because the commanded position suddenly changes or is closer to the load handling device, or the travel distance to the commanded position is too short, so that the duration of the track segment is short, so that the desired final speed v is achieved f It takes too long to cause overshoot. The generation of the trajectory for returning to the commanded position is very similar to the generation of the trajectory moving towards the commanded position, that is, the root-finding algorithm is used in combination with the speed transformation algorithm to determine the expected peak speed v Peak to peak In order to shift to the desired peak velocity v Peak to peak From the expected peak velocity v Peak to peak Conversion to a final speed v having a magnitude substantially equal to zero f Track segments are generated, i.e. the track returns to the commanded position. If the commanded position is exceeded longer, the magnitude of the track velocity may be increased all the way to the maximum velocity constraint v before gradually decelerating to the commanded position max . In both overshoot cases, a speed transformation algorithm is applied to transform back to the commanded position.
7.0 forced deceleration
The input of the trajectory may be accessed again by the control system periodically and one or more constraints may be updated depending on whether the trajectory changes, e.g., accesses a different destination or grid cell, or the presence of an obstacle on the grid structure. The one or more changes may include a command location and may cause the track generator to regenerate the track based on the updated command location. The updated command position may cause the track to terminate more closely than originally intended. Therefore, it is necessary to slow down the track so that the track speed and acceleration are substantially zero at the updated commanded position. Deceleration enables the controller or control system to apply the necessary torque demand to slow the load handling apparatus to an updated commanded position, i.e., braking force. In a particular embodiment of the invention A forced deceleration algorithm is applied to the trajectory involving transforming the velocity from a current or initial velocity to a final velocity v of substantially zero using the velocity transformation algorithm described above f . This involves generating one or more track segments using a Forced Deceleration (FD) constraint, rather than using a commanded acceleration to force the track to decelerate so that the track acceleration and velocity are substantially zero at the updated commanded position.
The inputs to the forced deceleration algorithm are shown in table 3 below:
table 3: forced deceleration algorithm input and output
Upper limit of FD max The value of (2) may be different or substantially the same as the commanded acceleration and depends on the tolerance of the motor. In the case of a load handling device, FD max Depending on tolerances of the drive mechanism of the wheel assembly and/or stability of the load handling device on the grid structure. The control system will pass the calculated end position p E To determine whether the trajectory exceeds or does not reach the updated commanded position, depending on whether the upper-limit forced deceleration FD is used when applying the speed-transformation algorithm max Or lower limit forced deceleration FD min Additional track segments are generated.
The application of forced deceleration is summarized in the flow chart 600 shown in fig. 34. After entering the constraints 602, this process begins to perform one or more decision processes to determine whether the track can use FD before computing the appropriate deceleration constraints by applying a root algorithm max Or FD min Is forced to slow down. The first two decision processes of the forced deceleration algorithm require that the trajectory be placed in conditions where a velocity transformation algorithm is applied to transform the magnitude of the trajectory velocity to substantially zero, i.e. v f =0. In the order shown in the flow chart, a first check 604 relates to whether the speed is increasing or notDecision making process. If the track speed is increasing, the control system applies the appropriate jerk constraint J 4 Generating a single track segment 606, the acceleration a 0 Gradually increasing to substantially zero to prevent further increases in speed. For example, a single track segment is generated as follows:
the manipulated variable (jerk) value used is j=j 4 ，
The duration of the segment is defined byGiven, sum
The end point acceleration is the peak acceleration (a E1 =0), end point velocity v E1 And end position p E1 By directly applying the trajectory equations (3, 4 and 5), the starting point a is used 0 、v 0 、p 0 Jerk j=j 4 And a segment duration T;
the end of the track segment is reassigned 608 as the start of the next track segment (a 0 、v 0 、p 0 ←a E1 、v E1 、p E1 )。
If the speed is not increasing, the control system proceeds to a next decision process 610 by determining if the magnitude of the trajectory acceleration is greater than the forced deceleration maximum constraint, i.e., |a 0 |>FD max To determine whether the initial deceleration is excessive. If the magnitude of the acceleration is too great, the control system generates a single track segment 612 to gradually reduce the magnitude of the acceleration to substantially match the forced deceleration constraint FD max Matching.
For example, a single track segment is generated as follows:
the manipulated variable (jerk) value used is j=j 4 ，
The duration of the segment is defined byGiven, sum
The end point acceleration is the peak acceleration (a E2 ＝FD max ) End point velocity v E2 And end position p E2 By passing throughDirectly apply the trajectory equation, use the starting point a 0 、v 0 、p 0 、J＝J 4 And a segment duration T;
the end of the track segment is reassigned 614 to the start of the next track segment (a 0 、v 0 、p 0 ←a E2 、v E2 、p E2 )。
Since the trajectory acceleration is neither increased nor excessive, the three subsequent decision processes 616, 620, 624 involve the application of a speed transformation algorithm to transform the speed to the desired final speed v f I.e. having a final speed v which is equal to the desired speed v f End point speeds of substantially equal magnitude. Decision process continues to judge application of speed transformation algorithm using forced deceleration constraint FD max Whether the track will be dropped at the commanded position 616. If so, then the velocity is converted from the initial velocity v by using the velocity conversion algorithm 618 0 The magnitude of the converted speed is converted to an expected final speed v substantially equal to zero f (wherein the acceleration constraint is equal to FD max ) To determine av and thereby generate one or more track segments. If not, forced deceleration FD max May not be sufficient or too aggressive. If forced deceleration FD max When applied to a track, is deemed insufficient and would result in the track still exceeding 620 the updated command position, the only option is that the control system needs to apply the "return" 622 discussed in section 6.0 to recover from the overshoot and thus fall on the updated command position. Track segments are generated to reduce the magnitude of track acceleration to FD max . One or more track segments are then generated to return from the overshoot by applying a continuous speed transformation algorithm to determine av. This involves applying a first speed transformation algorithm to the primary speed v 0 The magnitude of the converted speed is converted to the expected peak speed v Peak to peak End position p E . A second speed transformation algorithm is then applied to transform the peak speed v Peak to peak Gradually decreasing so that the end position p when the second speed conversion algorithm is applied E In the commanded position.
However, if the maximum forcing is used in applying the speed transformation algorithmDeceleration FD max When the track position overshoots, the maximum forced deceleration FD max May be considered too aggressive. In this case, it is necessary to restrict FD with a milder deceleration min Applying to trajectories, i.e. applying a speed transformation algorithm, using FD min As a commanded acceleration. If a speed transformation algorithm is applied, a lower limit forced deceleration FD is used min Causing the track position to undershoot or fall at the updated command position 624, FD is used min (wherein the magnitude of the track acceleration is reduced to FD min ) A speed transformation algorithm 626 is applied. If FD is in use min Under-shoot of the track, the limit that the magnitude of the track velocity has reached is too low, beyond the use of FD min The ability to fall at an updated commanded position, i.e. the desired final velocity v f Substantially close to zero at the end point before the commanded position.
If the upper limit forced deceleration FD is when the speed transformation algorithm is applied max Resulting in the trajectory not reaching the commanded position, but applying a lower limit forced deceleration FD min And would result in the track exceeding the commanded position, then it would be at FD max And FD (color filter) min A forced deceleration constraint FD in between, which can be applied to bring the trajectory to the commanded position. In this case, the control system will execute the root algorithm 628 to determine the forced deceleration constraint FD to allow the trajectory to fall in the final position, i.e., when v f Near zero. Root algorithm at forced deceleration FD as upper bound max And FD as a lower limit min Application is performed therebetween. The objective function is the degree of positional deviation from the final position when a forced deceleration constraint selected between the upper limit and the lower limit of the forced deceleration constraint is applied. In each successive iteration of the root algorithm, a speed transformation algorithm is applied to transform the trajectory from an initial starting point to the desired final speed v f -wherein v f Substantially zero and using selected forced deceleration constraints to determine the accumulated end point position p E I.e. from the initial position p 0 The trajectory is integrated and the total travel distance deltap of the solution is calculated using all segments on the output list. End position p E Will provide a "command position"Indication of deviation ", i.e. when the final speed v f Substantially zero. Each iteration of the root-finding algorithm produces a progressively accurate approximation of the root-in this case the forced deceleration FD. In combination with applying root algorithm and speed transformation algorithm to transform track speed to final speed v f When the control system has the task of determining the correct forced deceleration FD using a root-finding algorithm. Once the root is determined, a forced deceleration FD will be applied to the trajectory to transform the velocity from an initial starting point to v f Substantially equal to the endpoint at which zero is located.
While the specific embodiments described above relate to robotic load handling devices operable on a grid structure, it should be appreciated that the generation of trajectories or motion control profiles may be applicable to other moving devices or robotic devices requiring movement. For example, the generated trajectory may be used to control the point-to-point motion of the robotic arm.
It is to be understood that the methods illustrated and described above are not limited by the order of the blocks, as some blocks may occur in different orders and/or concurrently with other blocks from what is depicted and described herein. It is to be understood that various other branches, flow paths, and orders of blocks may be employed to achieve the same results.
Embodiments applying a speed transformation algorithm
The application of the speed transformation algorithm according to the present invention can be demonstrated by the trace graphs shown in fig. 35 (a to c). The trajectory is expressed from an initial velocity reference v 0 To the expected final speed reference v f Is a transformation of (a). In a particular embodiment of the invention shown in the velocity profile of fig. 35c, the velocity transition is from the initial velocity reference v 0 (+18) to the desired final speed v f (-16). For ease of explanation, the trajectory reaches the desired final velocity v at the commanded position f . These graphs are divided into stages 0, 1, 2 and 3, representing the transformation of the speed reference to the desired final speed v f Different track segments are required.
Initially, the magnitude of the acceleration is positive, and therefore the trajectory is far from the expected final velocity v f I.e. acceleration in the opposite direction (note that the expected final speed is negative). Application No. 1A speed transformation sub-algorithm: -a) first problem: is the acceleration excessive? And (3) if not.
B) A second problem: whether or not the track accelerates away from v f ? Is the result.
To solve the second problem, the first phase of applying the first speed translation sub-algorithm, represented by phase 0 in the trace-plot, is: generating the trajectory gradually reduces the magnitude of the trajectory acceleration to zero to prevent the trajectory acceleration from moving further away from the expected final velocity v f . This is illustrated by the acceleration diagram shown in fig. 35 a. The initial velocity delta is Δv shown by the velocity diagram in fig. 35c 0 Given.
At this stage, a track segment is generated, and the end point of the track segment thus generated becomes the initial track point for the next sub-problem, i.e. (a) 0 、v 0 、p 0 )←(a E 、v E 、p E ) Then, the corresponding calculation is carried out on the speed increment: deltav 1 ←(v f –v E )。
C) Third problem: even if the acceleration immediately starts to gradually decrease to zero, whether the trajectory exceeds v f ? No, because the acceleration has been gradually reduced to zero by the track segment applied to the previous problem.
Once the first speed transformation sub-algorithm has been applied, the trajectory is subjected to the application of the second speed sub-algorithm. Stages (1), (2) and (3) shown in the trace-graph represent the generation of trace segments when the second velocity transform sub-algorithm is applied.
The application of the second speed transformation sub-algorithm involves: given an initial velocity reference, peak acceleration a is calculated according to equation (21) Peak to peak . In this particular embodiment, the peak acceleration a Peak to peak A magnitude greater than the maximum acceleration. Thus, application of the second velocity transformation sub-algorithm involves increasing acceleration all the way to a max And the acceleration must be kept at a max For a period of time, then eventually it needs to gradually decrease to zero in time to reach the desired final speed v f . In other words, the portion of the track covered by the three track segments 1, 2 and 3 will take on a trapezoidal shape.
The first track segment represented by (1) in fig. 35a involves: increasing acceleration through the application of jerk values to a maximum commanded acceleration a max . Speed increment Deltav 2 ←(v f –v E ) Thereby, and the end point of the track segment is assigned to the initial track point for the next sub-problem, i.e. (a) 0 、v 0 、p 0 )←(a E 、v E 、p E )。
The second track segment denoted by (2) relates to: by letting the jerk value equal to zero, the acceleration is kept at a maximum acceleration a max For a period of time. Therefore, when the acceleration is at the maximum acceleration a max The velocity will vary linearly over a period of time, when the magnitude of the acceleration decreases as the trajectory enters the final stage (3). Speed increment Deltav 3 ←(v f –v E ) Thereby, and the end point of the track segment is assigned to the initial track point for the next sub-problem, i.e. (a) 0 、v 0 、p 0 )←(a E 、v E 、p E )。
The third and final track segment, denoted by (3), involves applying jerk values to reduce the magnitude of the acceleration in time to achieve the desired final velocity v f . The final track segment decreases the speed reference until the desired final speed v is reached f . In fig. 35c, the final speed v is expected f And has a gentle slope.
Claims (26)
1. Load handling apparatus for lifting and moving one or more containers stacked in a storage system comprising a grid framework supporting a plurality of rails arranged in a grid pattern to define a grid structure over a stack of the one or more containers, the grid pattern comprising a plurality of grid cells, and each of the stack of one or more containers being located within a footprint of only a single grid cell, the load handling apparatus comprising:
i) A wheel assembly comprising first and second pairs of wheels, the wheel assembly being driven by a drive mechanism;
ii) lifting means for lifting a storage container from one of said stacks of containers to a container receiving space of said load handling means;
iii) Position detection means for measuring the position of the load handling apparatus relative to the grid structure;
iv) a controller in communication with the position detection tool and the drive mechanism, the controller configured to control movement of the load handling apparatus across the grid structure from a current position to a commanded position:
a) Determining a feed-forward torque demand on the wheel assembly at a given time according to a preset motion control profile;
b) The feed forward torque demand is compensated by the measured position of the load handling device on the grid structure at the given time to calculate a torque demand on the wheel assembly.
2. The load handling device of claim 1, wherein the controller is configured to compensate for the feedforward torque demand by:
i) Determining a feed forward 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 on the wheel assembly.
3. The load handling device of claim 1 or 2, wherein the preset motion control profile comprises one or more track segments in a sequence, the one or more track segments being portions of the motion control profile, 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. A load handling device according to claim 3 wherein the feed forward torque demand is determined by the magnitude of the acceleration reference and/or the magnitude of the speed reference.
5. Load handling device according to claim 3 or 4, characterized in that any one of the jerk reference and/or the acceleration reference and/or the speed reference and/or the position reference is stored in a look-up table.
6. A load handling device according to any of claims 3 to 5 wherein the feed forward torque demand is determined from a comparison between the magnitude of the position reference derived from the preset motion control profile and the measured position of the load handling device on the grid structure at the given time.
7. The load processing device of claim 6, wherein the controller is configured to perform PI or PID calculations on a difference between a magnitude of the track position reference and the measured position.
8. A load handling apparatus according to any preceding claim, wherein each wheel of the first and second pairs of wheels is configured to be independently driven by the drive mechanism.
9. The load handling device of claim 8, wherein each wheel of the wheel assembly comprises an in-wheel motor.
10. A load handling apparatus according to any preceding claim, 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. A load handling apparatus according to any preceding claim, 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. The load handling device of any 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 device of claim 12, wherein the controller is configured to share the calculated torque demand between the first and second pairs of wheels of the wheel assembly.
14. The load handling device of claim 13, wherein the controller is configured to vary a ratio of the calculated torque demands provided by the first and second pairs of wheels.
15. The load handling apparatus of claim 13 or 14, wherein the controller is configured to transfer a greater proportion of the calculated torque demand to the first pair of wheels when the first pair of wheels is behind the second pair of wheels in the direction of travel of the load handling apparatus on the grid structure.
16. The load handling apparatus of claim 14 or 15, wherein the controller is configured to transfer a greater proportion of the calculated torque demand to the second pair of wheels when the second pair of wheels is in front of the first pair of wheels in the direction of travel of the load handling apparatus on the grid structure.
17. The load handling apparatus of claim 14 or 16, wherein the controller is configured to share the calculated torque demand between the first and second pairs of wheels of the wheel assembly in proportion to the magnitude based on the trace acceleration reference derived from the preset motion control profile.
18. A load handling apparatus according to any of claims 12 to 17, further comprising means for measuring the state of motion of the load handling apparatus on the grid structure.
19. The load handling apparatus of claim 18, wherein the controller is configured to determine the motion state of the load handling apparatus on the grid structure from the position measurement tool.
20. A load handling device according to claim 18 or 19 wherein the movement state is speed.
21. The load handling apparatus of any 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 load handling device of claim 21, wherein the controller in communication with the drive wheel encoder is configured to:
i) Comparing the measured speed of each of the wheels of the wheel assembly with the motion state of the load handling apparatus; and
ii) in response to the measured speed of any of the wheels of the wheel assembly being substantially different from the state of motion of the load handling apparatus on the grid structure, either:
a) Adjusting the shared calculated torque demand to other wheels so that the state of motion of the load handling apparatus remains substantially unchanged; and/or
b) Reassigning 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 remove the shared calculated torque demand from any of the wheels of the wheel assembly until the speed of the wheel is substantially equal to the movement state of the load handling apparatus in response to the measured speed of any of the wheels of the wheel assembly being substantially greater than the movement state of the load handling apparatus.
24. A load handling apparatus according to any preceding claim, wherein the position detection means comprises first position detection means configured to detect a position of the load handling apparatus on the grid structure in a first direction and second position detection means configured to detect a position of the load handling apparatus on the grid structure in a second direction, the first direction being transverse to the second direction.
25. A load handling apparatus according to any preceding claim 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 such that the position wheel is biased into engagement with the grid structure.
26. The storage system includes:
a first set of tracks and a second set of tracks traversing the first set of tracks laterally in a substantially horizontal plane to form a grid pattern comprising a plurality of grid spaces or grid cells;
a plurality of stacks of containers located below the first and second sets of tracks, wherein each of the stacks of containers occupies a single grid space or grid cell;
A load handling apparatus as claimed in any one of claims 1 to 25 arranged to traverse over the plurality of grid spaces or grid cells along the first and second sets of tracks such that when positioned over a stack of containers occupying the grid spaces or grid cells, the lifting apparatus is configured to lift at least one container from the stack of containers.
Applications Claiming Priority (6)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| GB2020684.3 | 2020-12-24 | ||
| GB2020680.1 | 2020-12-24 | ||
| GBGB2020684.3A GB202020684D0 (en) | 2020-12-24 | 2020-12-24 | Motion control of a load handling device operative on |
| GB2020681.9 | 2020-12-24 | ||
| GB2020668.6 | 2020-12-24 | ||
| PCT/EP2021/087168 WO2022136480A1 (en) | 2020-12-24 | 2021-12-21 | Motion control of a robotic load handling device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117043702A true CN117043702A (en) | 2023-11-10 |
Family
ID=74532166
Family Applications (4)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202180094514.6A Pending CN116888551A (en) | 2020-12-24 | 2021-12-21 | Motion control of a motion device |
| CN202180094510.8A Pending CN117043702A (en) | 2020-12-24 | 2021-12-21 | Motion control of robotic load handling device |
| CN202180094518.4A Pending CN116888552A (en) | 2020-12-24 | 2021-12-21 | Motion control of a motion device |
| CN202180094512.7A Pending CN116888550A (en) | 2020-12-24 | 2021-12-21 | Motion control of robotic load handling device |
Family Applications Before (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202180094514.6A Pending CN116888551A (en) | 2020-12-24 | 2021-12-21 | Motion control of a motion device |
Family Applications After (2)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202180094518.4A Pending CN116888552A (en) | 2020-12-24 | 2021-12-21 | Motion control of a motion device |
| CN202180094512.7A Pending CN116888550A (en) | 2020-12-24 | 2021-12-21 | Motion control of robotic load handling device |
Country Status (2)
| Country | Link |
|---|---|
| CN (4) | CN116888551A (en) |
| GB (1) | GB202020684D0 (en) |
-
2020
- 2020-12-24 GB GBGB2020684.3A patent/GB202020684D0/en not_active Ceased
-
2021
- 2021-12-21 CN CN202180094514.6A patent/CN116888551A/en active Pending
- 2021-12-21 CN CN202180094510.8A patent/CN117043702A/en active Pending
- 2021-12-21 CN CN202180094518.4A patent/CN116888552A/en active Pending
- 2021-12-21 CN CN202180094512.7A patent/CN116888550A/en active Pending
Also Published As
| Publication number | Publication date |
|---|---|
| GB202020684D0 (en) | 2021-02-10 |
| CN116888550A (en) | 2023-10-13 |
| CN116888552A (en) | 2023-10-13 |
| CN116888551A (en) | 2023-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2021405506B2 (en) | Motion control of a robotic load handling device | |
| WO2012111193A1 (en) | Transport carriage system and method for controlling running of transport carriage | |
| CN117043702A (en) | Motion control of robotic load handling device | |
| CN118055894A (en) | Determining kinematic states of load handling devices in a storage system | |
| JP2008222424A (en) | Travel control device of stacker crane | |
| CA3229742A1 (en) | Determining a kinematic state of a load handling device in a storage system | |
| JP2002169616A (en) | Movable body |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |