4 Motion Control with Machine Models
4.1 Introduction
In this chapter we will depart from the world of hardware and systems architecture and move towards applications in motion control and planning. Before we get started with anything else, a quick disambiguation of terms. Motion control can mean many things. Depending on who you ask, it may be the practice of:
- Tuning a servo’s PID1 gains to generate smooth and precise response to target positions or velocities.
- Generating single-segment acceleration “ramps” aka trapezoids to smooth motion of an individual axis.
- Optimizing motor commands in a bipedal, legged, or 6-dof robotic system.
- Generating target curves for a robot or machine to follow, navigating through space.
The field of robotics (of which machine control is a subset) is a mixed bag of all of these activities (and more!). In this thesis, “path planning” is the step where path geometries are defined. That is the purview of slicers and CNC CAM tools; I do not work on this layer explicitly. For “motion control” I mean two things:2 first is the task of “velocity planning” where we already know the path we want our machine to take (our “target trajectory”) but must pick the velocities that it should use along that path, generating a “planned trajectory.” Given those trajectories, we must actually follow them: this means motor control. Of course there is a natural connection between all of these layers; constraints from motor control and machine kinematics relate directly to velocity planning, but they should also be reflected into path planning algorithms. That remains out of the scope of this thesis, but I do discuss how the work here could be extended into that layer in Section 7.2.2.2 and 7.7.1.
So, about those constraints. Motors produce limited torque, structures deflect with load, polymer extrusion is limited by rheological (and thermal!) dynamics, cutting forces are limited by tool and part stiffness, etc. Implicit to CAM programming is that users should set parameters such that their GCodes don’t require the machine to violate steady-state constraints (1.3.1). But what about the dynamic limits? GCodes describe target trajectories (see Figure 1.6 in the earlier discussion of the hidden optimization 1.3.3) and completely faithful execution of those trajectories would mean making instantaneous changes in velocity. In real-time machine control (where those GCodes are turned into actual motion) we use velocity planners to avoid exceeding dynamic constraints, for example constraining instantaneous changes in velocity by imposing an acceleration constraint. This is the hidden optimization that I spoke about in the introduction, and it takes place beneath the GCode layer, e.g. in a firmware on the machine’s control board.
Perhaps the critical word here is planner; these account for machine dynamics that must be anticipated, i.e. their main concern is to ensure that a machine starts slowing down well ahead of a corner in order to avoid the sudden change in direction that would over-extend motors and excite the machine’s structure. They are often called “look-ahead planners.” To disambiguate time scales, planners typically operate over a horizon of tens of seconds3 whereas a motor controller’s main loop may run ten times in every millisecond; recall Figure 1.8 for a map between time scales and control components.
In Chapter 2, I showed how we can bring these planners up out of machine firmwares and make them into software objects. That roughly solves the problem of the hidden optimization simply by relocating the algorithm into a higher level system where it can be more easily modified and inspected / observed. But how should we actually describe and operate those planners?
4.1.1 Key Limits to Classical Velocity Planning
MAXL includes a trapezoid-based planning block (2.5.4.4) which implements a common style of velocity planner that I explain in more detail later on in this chapter (4.2.4). These form the basis of the state-of-the-art in machine control, but they suffer from the same similar structural issue that is present in CAM workflows: they are configured using parameters that describe their outputs rather than with models that describe their constraints. For example trapezoid planners are configured using parameters for maximum velocities and maximum accelerations4 \((a_{\text{max}}, v_{\text{max}})\) for each of the system’s degrees of freedom. Those parameters are related to physical models for motion, but alignment between those parameters and the real underlying physics must be done by hand. To say it again in one more way: these types of planner require that machine builders work the problem forwards (from models or intuition to limits) rather than backwards (from constraints to planners and solutions).
For a first order mapping between machine physics and these motion parameters: maximal motor RPM and machine friction constrain speed, motor torque and machine mass constrain acceleration, and jerk is limited by the permissible rise time for current in a motor’s stator coils (see Section 4.3.1). That gives us about one physical constraint per motion parameter, but there are other considerations: backlash, stiffnesses, vibrations, and externalities like temperature (see 4.2.2 for more notes on each). Really, each of these aspects need to be considered together to understand our machine’s total motion constraints. Collapsing them all into a single set of parameters is limiting: we may reduce velocity overall to avoid vibrations that only appear under very particular conditions (i.e. where our machine is going fast and exerting high forces on the work piece), or limit acceleration and jerk overall where we really want to decrease low-end motor loading.
Defining these limits outright (one maximum acceleration at all times) is also restrictive because the underlying constraints change based on many of the other (dynamically changing) states. For example if you take a look at a motor’s torque curve (e.g. Figure 4.4) you can see that they can exert far more force (bigger accelerations) at low speeds than at high speeds. To maximize performance we would want our planners to be able to exploit that low-end torque!
Motion control is also coupled into process constraints, but trapezoid planners do not account for these naturally. Relying on CAM configurations to encode those limits works for steady-state operation (besides the configuration burden) but the couplings can impact dynamic operation too. Because trapezoid planners are based at their core on the kinematic equations it can be difficult to modify them to accommodate for other physics. This means that we either need to make structural changes to trapezoid planners when we deploy them on new processes (in Section 5.2.3 I show how this is done for 3D printers) or build compensations and modifications to their outputs that work “on top” of them, rather than at their core (in Section 4.2.6 I cover a few examples of this strategy).
But what’s the actual difference there? Even though these modifications enable trapezoid planners to better accommodate process physics, they cannot anticipate them or reflect their constraints back into the motion optimization. Instead, process control must follow along with whichever planned trajectories were chosen based on kinematic constraints alone. For two clear examples of this: realtime CNC controllers do not model cutting force, and so cannot anticipate that they may need to slow down before they start a new cut (we use feed rate selections in CAM for that), and 3D printer controllers do not model polymer melt flows, and so they do not know where they may need to slow down in order to avoid overpressure conditions in the nozzle (again, we use slicers for that).
4.1.2 Chapter Overview
4.1.2.1 Key Questions and Goals
So, this chapter answers one of the leading questions from the introduction: how can we re-frame machine control explicitly as a constrained optimization problem? Trapezoidal planners are rudimentary constrained optimizers, but they don’t operate directly on model-based representations of those constraints, so a more accurate framing of the question at this point would be to say “how can we re-frame velocity planning as an explicit optimization over model-based constraints?“
The key goal then is to learn how to do this: existing frameworks are limited in their ability to describe arbitrary physics primarily due to computational constraints. So, can we leverage new computational technologies to make a planner that removes these constraints with brute force? That means parallelization, so how exactly do we formulate the planner in a manner where the computations are matched to the compute hardware, learning from other state-of-the-art parallel formulations? What are the limits and tradeoffs; do we trade one set of machine-related parameters for another set of planner-related hyperparameters, and how much determinism and observability do we give up for the new capabilities that it unlocks?
We will of course need some models. Amongst the many modelling techniques for kinematic systems and actuators, we need to select the formulations that suit the planner best. We would also like to use the same set of models for parameter fitting and for operation — this would let us share computational representations across multiple parts of the same complete system. We also have the goal of fitting those models to data from the hardware that the controller will operate, and fitting them in a way that would allow us to update them with new data from regular machine operation. While none of these modelling techniques are new research, understanding how each of these concerns informs those model choices is an interesting trade study.
It is difficult to capture all the relevant machine physics, especially when we are just beginning to fit them on new hardware. So, where models fail to capture important dynamics, we will want to develop handles into the planner that let us encode heuristics that can still modify behaviour of the system appropriately.
Finally, if we can pull this off, can we also formulate the planner so that its internal states (which constitute as complete a simulation of the hardware as our models would allow) can be used to learn about our machines and processes themselves? In that case, what might we learn, and how do we render and distill those outputs, or combine them with other measurements to make new estimates of values that would have been difficult to measure directly?
4.1.2.2 Background Sections
I look closer at state-of-the-art motion control in Section 4.2.4 (which explains in detail how trapezoid planners work) and then 4.2.6 covers how these are extended and improved by leading industrial motion control firms.5
It is fairly well known that model-based control has direct advantages over the state-of-the-art; much progress (particularly in robotics) has been made in advancing these techniques recently. I cover key methods and insights from that field in Section 4.2.7, also discussing how the planner that I develop here differs from common formulations.
There is less research on model based control for machines. One reason may be that machine control typically requires higher bandwidth than robotic control (machines must locate tools more precisely than a robot dog must position its feet). Optimization based controllers are also less inspectable and predictable than heuristics based planners — this means that engineers who care primarily about their systems’ reliability tend to prefer the latter, as I discuss in 7.2.4.
Based on the contributions that do apply these techniques to digital fabrication equipment (covered in Section 4.2.8), researchers seem to be limited by GCode interpreters and state-of-the-art machine architecture more generally. This is interesting in itself with regard to the earlier discussion on the partitioning problem (1.4), but also provides some more motivation for the work from Chapter 2 on systems integration: eliminating architectural barriers seems like an important step towards helping these researchers develop smarter controllers.
I also cover background on fundamental and then more sophisticated machine physics (Section 4.2.1 and 4.2.2), and a brief note on reinforcement learning (RL, 4.2.7.2), which I do not rely on or implement directly but which remains relevant primarily as we look ahead to the future of control in general and its application to machine control.
4.1.2.3 Key Methods and Contributions
Rather that inventing a litany of sideband corrections, compensations, and parameterizations of these underlying physics and couplings, we can try to develop a velocity planner that uses models more fundamentally as an input to describe constraints. That challenge makes up the contents of this chapter.
This requires a few key methods. Firstly, we need to develop and fit models can will articulate motion constraints. I cover models for electric motors and for kinematics (transmissions, inertias, and friction) in Section 4.3. To fit these models on the hardware where they will be used (4.5), I develop a closed-loop stepper driver and a method for learning some of its key parameters automatically (4.4). I assess the quality of our models in Section 4.7.2 and show how they can be iteratively improved using data from real-world machine operation (4.7.3).
In Section 4.6 I develop a motion planner in an optimization framework that uses these models directly. It has the simple task of maximizing speeds while respecting actuator and kinematic constraints. Doing so with high fidelity models for motion is computationally expensive; I lean on a set of tools that have been recently developed for parallel computing on the GPU, and carefully frame the computation such that it remains parallel in structure (4.6.1).
This planner only uses models of actuators and inertial / friction dynamics, I still rely on heuristics to modify the controller such that constraints which are not described explicitly are not violated, but those heuristics are encoded in a way where they work “through” models, as I discuss in 4.8.5.2.
However, the formulation of this controller differs from the state-of-the-art then in a few important ways. The first is that it is carefully set up such that it can run in parallel on a GPU, which greatly improves its compute performance and allows us to describe more process physics alongside motion optimization alone, which I show in Section 5.9.
The second is that it uses models for motion more directly than other formulations; normally these are compiled and run in hardware, whereas our formulation runs in the same software where the models are fit: this lets us use the same computational representation for both tasks and also for high level planning steps (as in Section 5.8) and for machine design (6.4.2).
Using models to describe constraints is helpful because we can build those automatically from physical data: e.g. to set motion parameters optimally we would first want to build models anyway, why not use those directly? This is not just a convenience: in Section 4.7.1 I evaluate the planner against a trapezoid-based equivalent and show how it increases the dynamic range used by our actuators to improve overall speed.
The planner’s formulation has the benefit that we can use internal states from the planner to understand which constraint limited performance at any point in a planned trajectory, relating models to operation. In Section 5.11.1 I show how this can help us to understand machine design choices more directly. In Section 5.11.2, I expand on the same method to discuss how planner outputs (alongside process models and model-based heuristics) enable a more direct tuning relationship between user-selected values and fabrication outcomes. These values can also be used in combination with realtime measurements to compute new data like machining cut force estimates (6.3.2).
Section 4.8 summarizes the major lessons learned in this exercise and 4.9 discusses some of its major shortcomings and how it may be improved in the future.
4.2 Background in Motion Control
4.2.1 Essential Physics for Motion Control
At the first layer, the physics of motion control is just inertial; to move our machine’s masses we must apply forces that accelerate them — changes to acceleration update velocity, and then position: \(F=MA.\) We do so with motors (aka actuators, which have inertias of their own), whose relationship to our machine’s moving parts is described with kinematic models. Of course there is also friction in each of these systems. I cover those basic physics and the models that I use in this piece of work to describe them in Section 4.3.2.
The actuators also introduce constraints: electric motors cannot instantaneously change the amount of force that they generate because they are essentially electromagnets, and it is impossible to instantaneously change the amount of current that is generated in motor windings. This introduces a constraint on our motion systems’ jerk, which is the third derivative of position (the next three are snap, crackle, and pop). I cover motor models in Section 4.3.1, and discuss how these are connected to one another in 4.3.3 such that we can estimate the current requirements — and limits — for our motors’ actuators given any velocity and acceleration at the machine’s tool tip.
4.2.2 Sophisticated Physics for Motion Control
I will show how kinematic and motor models can be used to optimize motion control directly in Section 4.6, but I also want to describe here some finer grained physics that are relevant for motion. I was not able to manage all these in detail in this thesis (I discuss the value of heuristics to overcome that loss in Section 7.2.4), but they are still relevant.
They also motivate our formulation of a planner that would enable us to more rapidly incorporate new physical models into a framework where models could be fit to data, simulated, and controlled all using the same core representations. This requires that we apply big computing to the problem because models for these phenomena are more complex, time-dependent, and may change over time, aspects of which I demonstrate primarily in Section 5.9. I discuss some future work on the topic in 4.9.3.
These phenomena are also worth understanding so that we get a finer sense of why in-situ metrology is valuable: some of these physics change not only because of the machine’s own construction (which a manufacturer may be able to control), but due to their particular site of use, and vary also day to day. In many cases — as I discuss in Section 1.3.3 — GCode interpreters prevent us from studying these physics “as they happen” on our machines, motivating us also to build our systems such that controller states themselves can be used as we develop and discover these models.
4.2.2.1 Backlash
Backlash (sometimes just called lash) happens where actuators are coupled into machine axes in a manner where there is some dead band between changes in their position and changes in the axes position.6
For a simple explanation, this happens when there are even slightly loose connections between the actuator and the moving component. The most common example emerges from gear trains, where a driving gear’s tooth is not completely meshed with the driven gear; when the driving gear changes direction, it must first move to close the gap from one tooth to the next before it can actually drive the output gear. The same is true of some belt driven systems and some lead screws, rack-and-pinions, etc.
Backlash is exceptionally common but difficult to model because it is dependent on motion profiles’ history and on external loads like gravity, forces from acceleration, and forces from process phenomenology. It is also nonlinear, as we can see.
4.2.2.2 Nonlinear Kinematics
The machines that I deploy using my own dynamics planner (in Chapter 5 and 6) both have linear kinematics, meaning that their tool-tip position is just a linear combination of their actuator positions (and vise versa). The CNC machine is the most straightforward: each actuator is attached only to one axis, whereas the 3D printer uses Core XY kinematics [2], where two motors (a and b) work in parallel to control the machine’s x and y axes (see Section 2.5.4.6). Linear kinematics are simpler to model because their dynamics don’t change significantly when their position changes, whereas in a nonlinear machine (like a robot arm, for example) they do: the robot has more rotational inertia when it is outstretched than when it is not, and also requires more static torque at the base to balance against gravity.
Machines with linear kinematics also have the property that their associated mass and friction terms are simply mapped onto the machine’s acceleration and velocity in cartesian (tool-tip) space, whereas defining the kinematics for a machine with nonlinear kinematics would also require that we define motion of intermediate masses and frictions. Take for example the little guy, in Figure 4.2, a machine that I developed partially to understand these types of system in more detail.
little guy machine, and its nonlinear kinematics where motion of the tool-tip in cartesian space is composed of arcs traversed by the machine’s two arms, which are connected to one another with rotary joints.
It is a drawing machine that uses a common kinematic design pattern for robot dog legs, using two rotary actuators linked in an arm, each controlled by a motor that is rigidly mounted to the chassis. Kinematic arrangements like this can have serious advantages: they reduce total moving mass, and they package well (keeping the robot small while its work volume remains large). In this case the motion of the two masses in the system are not in the tool-tip’s cartesian space, nor are they in the actuator space — each traces its own path through space. The relevant friction terms (related to rotational speeds at each joint) are also intermediate.
I did use MAXL to control this machine (see Section 2.5.4.7), but that layer only requires that we define the inverse kinematics. I did not extend the dynamics planner in this chapter to account for the nonlinear dynamics of those moving masses and frictions. Instead, I did what many machine controllers for these systems do: apply maximum accelerations and velocities for the tool tip using a trapezoidal planner and tune those such that the underlying dynamics seem OK. In Section 4.9.3.1, I explore how the kinematic definitions that I do use in this chapter could be extended to also return these intermediate values and incorporated into the planner.
4.2.2.3 Stiffness and Resonant Excitation
Beyond actuator limits themselves (which are effectively force limits), machines also flex when we exert those forces. For example one of my old mentors once told me “there’s no such thing as a zero backlash drive.” He was referring actually to machine stiffness: even with a perfect coupling between our drive system and machine axis, the transmission elements will transition from positive, to zero, to negative preload as we change direction — and that preload deflects machine hardware.
So, ideally we would also develop models for machine stiffness that would tell us how much they would deflect under these loads because we want our machines to be precise: if the forces we exert in order to move them around cause them to deflect significantly, we will miss the point! Stiffnesses can be more challenging to model than actuators, inertias, and frictions alone because they can change as the machine’s configuration in space changes, for example a machine with a cantilevered beam will deflect more when it is extended than when it is retracted. They also require that we measure the actual tool-tip position rather than measuring it indirectly with our motor’s position estimates (broadcasting those through kinematic models); the kinematic chain between our actuator and the tool tip is what is flexing, so we need some way to measure that displacement directly.
Many machine tools can be quite stiff, for example linear rails (a common motion component) have stiffnesses that are often measured in Newtons per micrometer of deflection \((N / \mu m)\). This is one of the reasons (the other being that direct measurement of tool-tip position adds the complexity and expense of adding another set of encoders to the system) that it is common to measure machine stiffness indirectly using accelerometers.
This is done by exciting machines with sinusoidal or step inputs and fitting parameters for stiffness, mass and damping in the frequency domain. I developed MAXL’s chirp block (2.5.4.3) roughly for this purpose, but ultimately ended up using it to fit friction parameters alone, as I will show later on in this chapter (4.5.2). In any case we are interested in stiffness largely to cancel out vibrational effects7 on our machine, and so analysis in the frequency domain can be applied directly.
The frequency response of a machine can change significantly due to apparently small or unseen physical changes, e.g. when timing belts becoming slightly looser or tighter, or when masses change (which happens also as printers deposit material, milling machines remove material, or we modify components — for example adding new sensors to a tool head). Even temperature — a longer discussion of which comes up shortly — can change the stiffness of a belt or the preload (friction and stiffness) of a linear rail.
4.2.2.4 Excitations from Actuators and Transmissions Themselves
Besides exciting resonant modes of our machine structures by moving it around, there are a few other noteworthy ways that machines can be structurally excited.
Firstly, timing belt teeth mesh and unmesh from timing belt pulleys at a frequency that is exactly related to the pulley’s rotational velocity, diameter, and the pitch of the belt. While subtle, those meshing events can cause belts to vibrate [3]. The same is true for gears in general, where meshing of teeth happens periodically, although it is particularly important for belt drives because the belts can act like plucked strings on an instrument [4], and their resonances change as their lengths and tensions change [5], complete models of these phenomena can require complete FEA analysis [6].
Ballscrews and leadscrews are another common type of machine transmission,8 these can “whip,” buckle or vibrate under high speed or high load conditions [7], models have been developed to analyze these properties [8], and predict or analyze their failures using analysis of their vibrations [9].
So, elements of the transmission can cause excitations — but so can the motors themselves. Stepper motors in particular are driven with pulses.9 Where motor speeds, pulse frequency, and structural resonances line up, this can excite machine structures [10], [11]. Brushless servos of all types produce cogging torque, which is a similar phenomenon: internal motor construction is non-uniform, and so particular phases of the motor’s rotation (where rotor and stator irons are better aligned) can produce more torque than others. This too can excite systems if it is not corrected for, as in [12] and [13].
Finally, fabrications processes can excite machines structurally — this is most apparent in CNC machine chatter, which I discuss in Section 6.1.4.2.
4.2.2.5 Temperatures and External Disturbances
To understand how minute the concerns here can become: national scale manufacturing of extremely precise optics (used mostly for spy satellites and then occasionally for science) is done on machines that can hold position down to nanometers across meter-scale parts [14], [15]. Even small changes to their chassis’ temperatures can cause deviations of this size, and so they are built within multiple nested thermally controlled rooms. High end CNC mills often have active thermal control on each of their axes: Kern (a company that specializes in ultra-precision machines for small parts in e.g. medical device manufacturing and radar waveguides) elects to build their machine components mostly in aluminum (rather than steel or iron, which is more often chosen for its stiffness, damping and mass properties) because its conductivity provides better thermal stability [16], [17]. In machines with timing belt drives, ambient temperature (and temperature rise due to belt friction) can change those belts’ spring rates nontrivially.
National scale metrology equipment like the device that measures the kilogram’s mass according to universal physical constants (called a kibble balance) are similar [18]. I was lucky enough to visit this machine: to get there you traverse about eight flights of stairs underground (elevators cause too much disturbance), and then into a room, and then into a second room that is structurally separate from the rest of the building (suspended in its own mass of dirt and concrete). You get the point: when extreme precision becomes key, lots of physical gremlins appear.
To back out to more relevant scales, even the structure of the desk on which our machine is placed can affect its frequency response: forces that the machine exerts on its masses must be reflected ultimately into the earth’s own mass — if they do so through a flexible desk, the machine system changes! Consider a print farm’s shelf with nine 3D printers on it — each of those are now interacting a-la the famous metronome synchronization demonstrations.
4.2.2.6 Interaction with Process Physics
The coupling between process physics and motion control is perhaps the most complex of this set. There are many manufacturing processes and each is internally heterogeneous; tools, materials and other configurations change between jobs. Modelling these physics properly can also require that we model the geometries involved and computational representations of variable geometry can be heavy.
I discuss these physics for 3D printing in more detail in Section 5.2.1: those physics are largely rheological and couple into motion control because nozzle pressures must be dynamically changed alongside machine velocity in order to precisely deposit polymers at target track widths. Printer rheology is actually time-dependent because it is also coupled into the nozzle’s thermodynamics; completely optimal motion control for 3D printers requires look-ahead over much longer time scales than what would normally be used for motion alone, see 5.11.5 for some discussion on that topic.
CNC machining physics are briefly discussed in Section 6.1.1, and I have made some mention of them in this chapter, mostly about the vibrations that the machining process imparts on milling machines. Cutting force is also a factor: where motion control is fundamentally constrained by the forces we can exert on our machine, sudden changes to those forces (e.g. when a milling machine enters or exits a cut) can of course matter a great deal. Here the modelling challenge is geometric: to account for these step changes dynamically we would need to represent part and tool geometries and also update them as they are machined. In any case, I do not engage in these experiments in this thesis, but I do show how motion data can be combined with motor measurements to estimate cut forces in Section 6.3.2 (this is not a new invention here, I develop it as an additional demonstration for other methods in the thesis).
4.2.3 The Role of PIDs and Hierarchical Control
Controllers of any complexity are normally formulated hierarchically, where lower levels (like current control) are handled with simple PI controllers10 and more complexity is added as we move up in systems scope and down in response time, see [19] for an overview of this approach and [21] for more modern hierarchical systems.
The rule of thumb is that the bandwidth of the controller below the one you are currently designing should be fast enough that the delay it introduces can be ignored when compared to the current level. A good example of this is shown here in Section 4.4, where I develop a motor controller that uses two levels of control internally: microseconds to close current control loops and milliseconds to close position control loops. The position controller’s target trajectory is a basis spline whose control points are output by the velocity planner from this chapter, which covers longer time spans using look-ahead planning.
While hierarchical control is a useful partitioning of the problem they can still be somewhat lossy because the minutia of lower levels are ignored by upper levels. However, this is also the point: considering all the required physics simultaneously is difficult and can get messy. For example trapezoid planners that do not smooth jerk will output trajectories that are not physically realizable by motors that cannot instantaneously change current in their coils. In practice this is OK for most systems because lower levels of the controls stack work well enough to correct for it; we introduce some error, but it is negligible.
Partitions here are set up by hand / according to heuristics: arranging these partitions is a design exercise that involves the normal trade-offs between complexity, utility, and performance.
4.2.4 Trapezoidal Planners
Trapezoidal planners (or s-curve generators) are the most common and practical type of motion controllers that we find in practice, although the exact configuration of most commercially available machines is difficult to study because their controllers are proprietary, especially CNC machine tools [22]. In digital fabrication research, where machines are often controlled using open source hardware, trapezoid planners are the rule. They are very well understood in texts as single-segment planners [23, Sec. 9.2.2.2] but their extension into multi-segment trajectories involves one or two additional steps that I should explain.
Firstly the basics: they work by generating trapezoids in the velocity-time plot, which are s-curves in the position-time plot.
Trapezoid segments assume fixed acceleration over each component, meaning the kinematic equations can be used, those are below.
\[ \begin{aligned} d = v_it + \frac{1}{2}at^2 \\ d = \frac{v_i + v_f}{2} t \\ v_f = v_i + at \\ v_f^2 = v_i^2 + 2ad \end{aligned} \tag{4.1}\]
Trapezoids planners are configured by setting a maximum acceleration (slope in \(v(t)\)) and velocity (maximum value in \(v(t)\)) for each axis of motion. Some also specify maximum jerk. Using these values along with initial and final velocities for each segment \(v_i, v_f\) and a distance for the segment \(d\), trapezoid planners use the kinematic equations to determine where acceleration and deceleration should be applied in each. Since motion parameters are set on a per-axis basis but line segments can point in any direction, trapezoid planners also choose per-segment maximum acceleration and velocity normals to use based on their unit vectors.
We must also choose a maximum velocity for the corners that sit in each junction between segments. In reality, we cannot move through a sharp corner at any velocity without an infinite impulse of acceleration. Most trapezoid planners use an heuristic algorithm called junction deviation at this point that approximates maximum cornering velocity using an imaginary arc segment between the two lines. The arc radius is chosen such that were it inserted at the corner the arc would not deviate from the actual corner by more than the defined junction deviation parameter (a distance), and the acceptable cornering velocity is set according to the radius of that arc. However, the arc is not actually interpolated, instead the cornering velocity is simply applied to the incoming segment’s maximum \(v_f\) and to the next segment’s maximum \(v_i.\) The original blog post on the topic is at [24] for the firmware GRBL [25], and also explained in [26].
So, junction deviation provides an initial maximum \(v_i, v_f\) for each segment and per-axis maximum acceleration and velocity are combined with unit vectors along each segment to choose the segment’s maximum values. We then need to link multiple segments together and plan across them. This is important because we often have cases where we need to start decelerating for a corner that is ten or twenty segments away. In this step we use a form of dynamic programming [27]; we compute a forwards pass (ensuring each segments’ \(v_f\) is not so large that it is not achievable with the segment’s \(a, v_i, d\)) and a reverse pass, which is the same computation but from \(v_f \rightarrow v_{i,max}\).
Trapezoid planners re-compute junction deviations and linking velocities whenever a new segment is added, and they always plan to come to a full stop at the end of the segments in this queue. The queue size used to be constrained by microcontroller memory footprints and compute power because each segment consumes some RAM and re-computing over each requires a few clock cycles. This is less of a constraint with modern embedded computing, but the queue imposes some other limits for e.g. updating plans in real-time that I discuss in Section 7.4.2.
Each segment in the queue is a single trapezoid; one end of the queue can be extended but the other is constantly being interpolated as the machine operates. Trapezoids are useful in this case as well because they can be rapidly evaluated in firmware to e.g. generate stepper motor pulses or servo target values. When the planner interpolates past the end of each segment, it removes it from memory to free space for the next segment to be added.
It is worth noting in this point is that trapezoid planners are based on fixed maximum accelerations and velocities, whereas motor dynamics show us that they are capable of more: larger accelerations at low speeds, and higher speeds where low accelerations are required. Where our machines have nonlinear kinematics, minimum and maximum velocities and accelerations change through space as well; a robot arm requires more torque to move its base joint when it is outstretched because the rotational inertia of the arm is larger when it is in a longer configuration. Because trapezoidal planners are fundamentally based a set of kinematic equations for segments with fixed acceleration over their length, they are unable to exploit these dynamics. They are practical for the same reason: entire linear (and arc) segments can be rapidly evaluated in only a few clock cycles. See Section 4.7.1 for the full comparison between this type of planner and the model-based optimization that I develop.
4.2.5 Geometry Smoothing and Filter-Based Methods
The junction deviation step in trapezoidal planners is lossy because it approximates smooth junctions (virtual arcs) between line segments. When machines traverse those junctions (which remain sharp despite having nonzero velocity), they actually perform some instantaneous acceleration. These impulses are normally taken up by motors and other springs in the machine system, but it would clearly be preferable to avoid them entirely.
For this purpose, actually smoothing the trajectory such that it does not contain any corners with zero radii is best. This is the topic of many contributions in machine control research. For example [28] intelligently modifies GCode geometries according to geometric heuristics where (1) it is possible to do so within specified refitting tolerances and (2) it is advantageous to do so for motion control, i.e. smoothing small corners into arcs.
Most other methods in this family are similar: geometric tolerances are combined with varying geometric representations to refit input geometry such that maximum smoothness is attained while tolerances are respected, called “chord deviations”. For example Lavernhe [29] does this for surfaces, considering also kinematics of a five-axis machining center. A simpler version of the same idea is implemented into almost every velocity planner: incoming GCodes encode segments which are smaller than the spatial tolerance of the actual system are ignored, simplifying computation over these segments. This is mentioned explicitly in the Delta Tau manual [30] and in Ward’s thesis [31], and is implemented in interpreters like GRBL [25] and its many descendants.
This geometric smoothing can be combined with velocity planning itself by articulating the whole problem in terms of filter design; some of these methods apply filters on top of trapezoid planner trajectories to smooth sharp corners [32], [33]. They can also be used to reduce excitations generated by the planned trajectory at a machine’s known resonant frequencies. This is known as input shaping, [34] [35] [36], and it has become common in even consumer grade 3D printer controllers like Klipper [37], which I covered in the systems chapter (Section 2.2.4.4). Input shapers are designed, they are configured to reduce energy at selected frequencies and a number of different styles exist. Understanding which frequencies should be filtered is normally done against frequency response plots generated using accelerometers mounted to machines.
Finite Impulse Response (FIR) filter-based methods are the basis of another family of velocity planning algorithms that can supplant (rather than simply extend) trapezoidal planners. The method was originally developed in [38]: rather than planning with trapezoids and then filtering corner geometries or modifying velocity profiles to remove certain frequencies, filter parameters are derived from maximum jerk, acceleration, and velocity parameters. Applying a finite impulse response filter thrice to a target trajectory (of line segments) results in a planned trajectory that is continuous in jerk. These have been extended to solve also for time optimality while suppressing other vibrations in [39]. [40] improved the FIR approach for use in human-facing robotics where online replanning of trajectories is important, and [41] [42] made further improvements for five axis machining and compute performance.
FIR-style velocity planners have a few advantages over trapezoid planners: they can smooth geometries and velocities in one step (avoiding the junction deviation abstraction) and since they are already formulated in the frequency domain they can be easily modified to suppress other vibrations. They are also a much simpler way to generate trajectories that are continuous in the jerk derivative when compared to methods that extend trapezoid planners to do the same. However, like trapezoid planners, they are configured using direct kinematic parameters (maximum velocity, acceleration, and jerk — and geometric tolerance), rather than using models of these systems. While those parameters can be extracted from good models of a kinematic system, this means that they have many of the same limit that I mentioned in Section 4.1.1.
4.2.6 The State-of-the-Art in Industrial Motion Control
It is hard to know for sure what the internals of state-of-the-art industrial motion controllers do because these firms guard their algorithms closely [22]. To investigate, I read some technical literature that is available publicly from a few purveyors of motion control. From Heidenhain (a leading provider of machine controllers in the machining industry) I found trade literature [43] and a manual that is made available for machine tool builders [44] which was particularly useful because it is written for a knowledgeable audience. I did the same for Siemens Sinumerik ONE controllers [45] but didn’t find any equivalent “manual for integrators,” presumably one has to enter into a trade agreement with them before they will explain their technology. I also read (not entirely, it is 832 pages) the manual for Delta Tau’s PMAC (for Programmable Multi Axis Control) controllers [30]. These are more general purpose controllers as their name suggests, marketed for use in everything from packaging equipment to multi-axis synchronized machine control with velocity planning over multiple segments. As a result, their documentation is the most complete: they cannot really hide their algorithms from their users since the configuration of their equipment across multiple domains requires that it is understood by those users.11 I also discuss these controllers’ system architecture more broadly in Section 2.2.
4.2.6.1 Fixed Kinematic Parameters, not Models
Based on the velocity-time plots available in each, and on notes in literature for machine tool builders on how they are configured, each example is fundamentally based on kinematic parameters (maximum velocity, acceleration and jerk) for each axis. This means that they must be either trapezoidal or FIR-style velocity planners. For example in the Delta Tau manual [30] around page 738, we see trapezoidal profiles for look-ahead planning through multiple linear segments. There, motion is planned according to kinematic parameters defined for each motor in the system. They also note that if the user wants to constrain motion based on limits applied to the end effector position (i.e. motion on the other side of the kinematic chain), they should apply “virtual motors” onto these axes, noting a key limitation across these methods: real physical constraints emerge from motor space and cartesian space, but here we can normally only write parameters for one or the other. In Chapter 2 of Ward’s thesis [31], he fits measured velocity data from a DMG Mori to an FIR-style velocity controller, showing that their velocity planner is likely based on the same method.
Siemens Sinumerik ONE marketing material claims that it uses a digital twin representation of the machine for control, which could mean model-based control that is more similar to the velocity planner that I develop here. The most comprehensive documentation they provide publicly are manuals for machine interface configuration, these detail steps to attach 3D models to GCode axes definitions, but say nothing about supplying physical model parameters like mass, motor data, or damping terms. This indicates that by “digital twin” they may only mean that their planner is attached to a 3D representation, but not more fundamental models for motion. That said, 3D models do provide immense value for collision detection; Heidenhain controllers seem to have similar capability.
4.2.6.2 Compensations
Heidenhain literature [43] mentions a few different controls technologies that compensate for errors that arise during operation. These all seem to be methods that are applied on top of either trapezoid or FIR-style planners. There are multiple compensations mentioned there, each of which gets a small paragraph below:
CTC for “cross talk compensation” is compensation of acceleration-dependent position errors at the tool center point, adjusts trajectories based on machine deflections that arise from acceleration forces. They mention that parameters for this compensation can be found using machine measurements, but no other information is available on the topic. They relay that this “makes it possible to increase jerk by a factor of 2” (in one example), indicating that these stiffness models are not automatically incorporated or fed back into the actual acceleration control step.
PAD for position-dependent adaptation of control parameters adapts control for various kinematic poses (i.e. a beam gantry which has lower stiffness than when it is contracted) through “position-dependent filter settings and control factors,” again indicating that the method is likely applied using tabulated values or interpolated parameters through space, rather than on position-varying kinematic models themselves.
LAC for load-dependent adaptation of control parameters seems to be the most advanced, it enables the controller to “automatically ascertain the current mass with linear axes and the mass moment of inertia with rotary axes as well as the friction forces,” which is very similar to the methods that I describe in this section. It is unclear if these updates are transmitted to the velocity planner itself, based on their language for “feedforward parameters” it seems more likely that they are updating lower-level control gains for axes servos to reduce following errors.
AVD for active vibration damping: despite sounding like an online control method (“active”) on closer reading sounds like FIR-style filters that are parameterized around vibrations from the machine’s structure and from motion components (i.e. torque ripple), not adapted online based on disturbances from the process (cutting, i.e. chatter). Again, these updates are not reflected back into the velocity planning constraints themselves. Rather, their inclusion “allows jerk limits to be increased.”
Besides these four compensations, the Heidenhain literature also mentions compensations for temperature and kinematic models. These apply spatial offsets to target trajectories, for example moving machine axes away from other machine components that have expanded due to heat, or correcting motion on axes that are slightly out of square. Each of these is also configured using a set of parameters rather than by describing e.g. models for thermal expansion.
Overall, these controllers are certainly sophisticated equipment; they represent the pinnacle of what is available “on the factory floor” today. However, they are still fundamentally based on directly written motion parameters and their various compensations are added on top of trapezoid or FIR-style velocity planners. With more fundamentally model-based control, there is an opportunity to express these operations in a more straightforward manner.
4.2.6.3 Process Coupling and Active Control
Heidenhain also details a few online control strategies that manage interactions between process physics and velocity planning; ACT for active chatter control and AFT adaptive feed control both appear in [44].
AFT is likely based on a patent from 2008 [47] where feed rate targets are adapted based on spindle load measurements. A older Boeing patent from 1987 [48] is related; there feed rate overrides are modified using data from strain gauges mounted to a milling machine’s spindle. ACT can be traced to a more recent 2018 patent for monitoring spindle load during machining [49] and one from 2023 [50] for adjusting feed parameters accordingly.
Both approaches connect to the velocity controller’s feed rate override, so they are similar to research literature that I will cover in Section 4.2.8; they do not connect these concerns to the velocity planning step, they run in an outer loop around that step using feedback rather than predictive planning. Presumably though the active chatter control here is often used in conjunction with the active vibration damping modification described in the prior section, but again both are filter design methods rather than being model-based.
These methods extend well beyond the topics covered directly in this thesis. It is interesting however to see the partitioning problem appear here again: chatter is largely geometry dependent and because planners of all nature lack good representations for changing part geometry, so most methods are based on active control rather than predictive. As we move more control into higher performance computing environments, it will become easier to manage these physics via computation directly against simulateable models.
4.2.7 Model-Based Control in Robotics
So, the industrial state-of-the-art seems to prefer control methods that are more classical in nature, which makes sense in a risk-averse industry with a long history; CNC machines entered service on the factory floor in the early 1960s [51]. It also makes sense where determinism and predictability is of primary importance.
The story is similar for industrial robots e.g. the 6-DOF12 arms that populate automotive manufacturing lines, welding lines, or material handling lines.13 They have been on the factory floor since about the 1970’s [52] and their controllers are similar in nature to those present in CNC machines. Many of them are even tasked using low level instructions that look similar to GCodes: KUKA robots are programmed in KRL (KUKA Robot Language) and ABB robots are programmed in a language called RAPID. A longer discussion on that topic would be interesting, but it is outside the scope of the present focus.
Here I want to discuss model-based methods that are quite distinct from industrial motion control, so we want to look controllers from the more sci-fi instantiation of the word robotics i.e. things that have legs and arms. Modern approaches to control these systems are much younger than CNC, and they are often based around some form of Model Predictive Control (MPC).
MPC controllers work in much the same way as you and I do: we use a simulation of the future to optimize future and current control outputs. The classic example comes from driving: when we are entering a corner, we know that we need to slow down before the turn. To pick a braking point, we use a mental model of our car’s dynamics (how long it takes to slow down) to simulate (imagine) how long it will take to come to the appropriate entry speed for the corner. With computational MPC, we simulate and optimize control outputs over a horizon of up to a few seconds at each time step, but only issue the immediate next control output to our system. These are common in practice to solve controls problems for legged and bipedal robots [53] [54], and other dynamical systems like quadcopters [55]. The best practical overviews of their implementation I have found are in Tedrake’s course notes [56] and Brunton’s textbook [57].
These solvers are normally constructed symbolically and are subject to the constraint that the optimizations they solve must be quadratic programs, i.e. highly convex with one global minima. Two common tools for this practice are CasADi [58] and acados [59]. CasADi lets developers author symbolic representations of their system dynamics and cost functions, and then generates C code for evaluating derivatives and integrating dynamics models. Acados is a solver that can directly use those C codes to minimize cost functions in real-time. So for a programming workflow example: we write down our systems dynamics mathematically in a symbolic programming language, write a cost function for that system, and then compile that into a quadratic program (QP, [60]); this step is possible only where the mathematic definition of the problem meets quadratic programming constraints. That program is then copied into the controller’s firmware and recompiled to run. So these are akin to programming languages and compilers: we write systems and constraints at a high level (mathematically), and then automatically generate programs (solvers).
MPC typically uses the canonical state space representation [61] or [62], which I will include here for clarity:
\[ \begin{aligned} \dot{x}(t) &= A(t)x(t) + B(t)u(t) \\ y(t) &= C(t)x(t) + D(t)u(t) \end{aligned} \tag{4.2}\]
Where \(x\) is the state vector (what is happening), \(y\) is the output (what happens), and \(u\) are the control inputs, i.e. the knobs that our controller can set; normally motor voltages, currents, or velocities depending on the controller’s hierarchical design. The matrices describe various aspects of system evolution. The \(\dot{x}(t)\) function describes how the system evolves from any given state to the next state, with inputs \(u(t)\) from the controller. Using this formulation to simulate the system is easy, because that function is already an integrator. MPCs work on this representation and are typically either formulated using a “direct shooting” method or a “direct transcription” method. [56]
Direct shooting integrates states forwards in time. The solver begins with initial state \(x(t)\) and picks control outputs over time, some \(u(t).\) So the control inputs are the solver’s decision variables. It then calculates forwards how the system evolves over time given those inputs, compares points on that planned trajectory to an optimal outcome and computes a cost for the solution. The gradient between the decision variables and the cost is used to update the \(u(t)\) towards an optimal set. This is perhaps the most comprehensible formulation because it mirrors how we naturally think about dynamical systems. However, it requires serial computation of the system’s evolving states which makes it difficult to compute, and gradients can vanish over long integrals.
In direct transcription, the solver picks the resulting trajectory \(y(t)\) and each step’s internal states \(x(t)\) directly, and also the control inputs \(u(t)\). This breaks the time dependence and allows us to parallelize much of the computation. But it also requires that the solver implicitly link each time step such that control states at each time \(u(t)\) for each system state \(x(t)\) generates the next system state \(x(t + 1)\). That is, the cost function compares the planned trajectory to optimal outcomes, but also penalizes steps where the time series of states doesn’t make sense according to the system’s dynamics. Because the solver is now choosing outcomes as well as inputs, the dimensionality of the problem increases significantly. But, this has advantages for parallelization and also means that the solver doesn’t have to compute gradients through as many time steps.
4.2.7.1 New Tools for Constrained Optimization and Differentiable Simulation
I am enabled to complete the work in this chapter largely because of a new availability of automatic differentiation (aka autograd) tools, that automatically generate gradients for any given program (gradients being a key ingredient for optimization, most performant optimizers are based on gradient descent). In particular, I am using JAX [63]. With JAX, any system that can be articulated as a purely functional Python code can be automatically differentiated. We can then use the function’s gradient to optimize the function’s inputs, and JAX conveniently pairs with optimizers from the OPTAX library [64]. The performance of this system is only viable because JAX also includes a just-in-time compiler that turns the whole system into code that can run in parallel on high performance hardware (the GPU).
4.2.7.2 Reinforcement Learning and Policy Control
MPCs are typically used for short-order optimizations over time horizons around one to five seconds, and they usually run somewhere between 50Hz and 500Hz. For longer horizons of control and to solve problems that are not differentiable the current best practice uses simulated systems to train policy controllers that can issue higher-level control commands. Policy controllers are normally coupled with lower level, faster feedback controllers and classical controls components like Kalman filters and PIDs — recall the note on hierarchical control above (4.2.3). [65] is a particularly clear example of how all of these components come together to produce performant controllers, it includes classical simulations, policy networks, Kalman filters, and PIDs. Policy controllers are often trained without gradients because it is hard to differentiate through simulations that involve rigid body collisions and that happen over long time spans, but recent work deploys differentiable simulation to overcome this issue [66].
Policy controllers are one component of Reinforcement Learning (RL) for robotics: are the “brain” of RL systems. They compute optimal actions \((a)\) based on observed states \((s)\) from their environment and are trained with rewards \((r)\) that are also computed from their environments.
RL is making huge progress recently that extends from control into Large Language Models (LLMs) because it is possible where differentiation of the environment itself is not possible — i.e. they can be trained to manage large, messy tasks that don’t have straightforward or analytical mathematical representations. In robotics, RL it is mostly applied to legged or wheeled robots (but again, combined hierarchically with lower level controllers).
Overall the topic is interesting here because progress in applying RL to machines could be enabled by some contributions in this thesis; namely the systems that I develop to more easily connect high- and low-level computing and make sense of hierarchical control systems. RL policies are normally trained offline using countless simulations of their environment in what researchers call “infinite fun time,”14 the operation of which requires good ground-truth simulations of those environments. The models developed in this thesis for machine systems would be appropriate in these contexts as well. I discuss how RL could help to connect work in this thesis to path planning in Section 7.7.1 and expand on this thesis’ connection to RL and AI more broadly in Section 7.7.
4.2.8 Integrating Model-Based Control with Machines
Model-based approaches are less common in machine control than in robotics for perhaps a few reasons. The first is the preference in machine control for deterministic, fast controllers that I mentioned in Section 4.2.6: optimizers are less inspectable than classical control approaches and can sometimes generate unexpected behaviour. The second is related to the partitioning problem: bandwidth requirements for machine control are much greater than in robotic contexts, and so even more computing power is required to run optimization based solvers here.
Although it is impossible to know how common model-based control of machines really is in industry [22], there are a few good examples in the literature that I will look at here.
4.2.8.1 Integration Around Interpolators
In many of these examples, model-based planners are added to existing velocity planners. [68] proposes MPC to maximize feed velocity within dynamical limits involving feed rate and cutting forces. [69], [70] and [71] all make excellent progress for online estimation of cutting forces, detection of chatter, and modify machine velocity control to optimize or correct these physics. But in all three cases the control signal from the model-based controller plugs into an existing machine controller’s feed override input. This sets the target feed rate for the machine, which its own controller is in charge of; in [68] the authors note: “however in such a scenario, an accurate modeling and control of the feed dynamics become more crucial, as the built-in routines within the machining center induce unknown nonlinearities.” This adds complexity to the author’s work and requires that they also model the hidden planner itself in order to anticipate its internal dynamics15. The theme appears also in [29], where the authors are concerned with optimizing toolpath geometry against a five-axis milling machine’s motion constraints; “… The actual feed rate is thus locally lower than the programmed on due to the interpolation cycle time.”
In fact, Rob Ward of [69] (whose work was also covered in Section 4.2.5) includes in his PhD methods for the prediction and modelling of GCode interpreters [31] for the purposes of improving the development of these types of “piggyback” control systems for CNC machines.
Using feed rate override is not necessarily a bad approach, it is an important feature that allows machine operators (and these researchers) to quickly change the most critical parameter for machine operation. It is also exactly the kind of separation of scales that we would expect in a hierarchical controller, and these researchers show excellent results. It does introduce some loss and as I mentioned adds complexity; model-based control should enable even more fundamental control of machine dynamics; for example the inclusion of motor dynamics themselves — and direct access to motor torques — could significantly improve each of these controllers’ force bandwidths by bypassing their velocity planners’ imposed bandwidths (recall that they are essentially filters!). For discussion on how these researchers managed control integration from a systems architecture viewpoint, see Section 2.6.2.
In FFF, Pinyi Wu uses feed-forward modelling of extrusion dynamics to improve control of material extrudate [72], [73]. These methods benefit the extruder system, but the extruder system’s dynamics are not fed back into the velocity controller in any manner. That is, the two systems are controlled in separate control paths; Wu’s model-based controller tracks a signal that is generated from an off-the-shelf interpreter. Also in FFF, [74] integrates some melt flow modelling with a 6-axis robot controller — their systems architecture also tracks outputs from the robot’s hidden velocity planner.
4.2.8.2 Other Optimization-Based Interpolators
[75] presents an optimization based controller for CNC machines whose formulation is distinct from methods that I have covered so far, and it is solved using the research group’s own optimizer from [76], which was originally developed for automobiles. In [77] they combine this optimizer with earlier work on chatter control for milling that I mentioned above [71] where the chatter control was implemented using feed override of existing machine controllers.
Their implementation performs the optimization by selecting a time series trajectory directly against a reference trajectory made up of linear segments. This allows them to minimize time for the path against geometric constraints (deviations from those linear segments) as well as motion constraints. But those constraints are formulated as maximum per-axis jerk, acceleration, and velocity values that are chosen ahead of time, rather than being themselves selected by the controller or expressed directly as models for motion.
4.2.9 Comparisons to This Work
As I will discuss, the planner that I develop here is somewhat similar to a direct transcription style MPC in that it is parallelizable and that it selects the solution rather than the control values. But really it is somewhere in between MPC style control and classical velocity planners. It is described entirely in Section 4.6.1 and then discussed in 4.8.2. To generalize I would say that it represents the problem in a manner that is most similar to velocity planner, but it solves the problem in a way that is more akin to MPC, or to the planners from Section 4.2.8.2.
It is structurally different from each of these in that it does not use direct parameters to constrain motion profiles themselves (maximum jerk, accelerations, and velocities) and analytical representations (as in most velocity planners) nor does it use quadratic programming (as in MPC). It uses a more generic first-order gradient descent method that is borrowed directly from a set of tools developed for large machine learning problems, and it represents constraints not directly on the planned trajectory itself but by broadcasting the planned trajectory back through kinematic, inertial, and motor models in order to arrive at those constraints.
This allows it to exploit more of the underlying system’s dynamics because those constraints vary alongside other system states. This is particularly relevant at low speeds where motors can produce more torque and where friction is lower. It can also exploit friction within our systems that actually helps us to decelerate; planners that use fixed values for these parameters apply the maximum acceleration value also during deceleration.
There is also the matter of kinematic transforms: planners that use direct parameters must define them either in the space of the motion path itself (which is most common) or in the space of actuators (which is the default in e.g. the Delta Tau controllers [30]). In systems with trivial kinematics (one motor per axis) this is not so much of an issue, but many machine systems use parallel kinematics where motion of the machine tool is coupled to multiple motors — CoreXY is the most common of these arrangements. In these cases we have distinct inertias and damping terms in both spaces and so mapping limits from each space into one set of direct parameters is not straightforward. This matters for inertial and damping terms but also for motor physics. For example when a CoreXY-based machine moves at a 45-degree angle, only one of its motors is used to move both x and y masses, but when it moves in x or in y alone, both motors are used — this means that the real underlying motion constraints are anisotropic!
Using models directly also simplifies controller authorship and allows us to add dynamics to the system that may be cumbersome to describe symbolically, we can “pile into the cost function”. It also enables us to use the same component descriptions in the model fitting and model-based control workflows, and avoid the step where we might re-compile and update a firmware when we generate new models (as in with CasADi and acados).
This is significantly less performant than the state-of-the-art in MPC for realtime control in terms of the FLOPs16 required. I make up for the increased FLOPs requirement by formulating the computation so that it parallelizes well on the GPU, and developing the low-level motion system description and representation in Section 2.5 and Chapter 2 more broadly, which helps to connect the large compute required to run this planner with our hardware.
All of that said, the planner that I develop here is clearly missing the geometric consideration: the abstraction that I use to compute required accelerations along the path (in 4.6.1.1) is somewhat lossy and computed solutions are transmitted using basis splines that introduce some interpolation error. Properly, these errors should be accounted for in the optimization loop itself. I discuss this in Section 4.8.5.3, and possible improvements to the lossy acceleration calculations in 4.9.1.
It is also less deterministic, which may be a real issue to larger scale adoption of the technique. That is partially due to the nature of optimization based control and partially due to the location of the planner (which runs within a non-realtime OS). However, the same questions on determinism are being answered by researchers who deploy these types of optimizers for human-robotics interaction. It may also be possible to modify lower levels of the controller to enforce safety boundaries in cases where the planner itself fails, as I discuss briefly in Section 2.9.4.3.
4.3 Models for Motion Control
This section covers background on key models for motion, for motors (below) and for kinematics and damping (4.3.2). I then use a closed-loop stepper driver (4.4) to fit those models to data in Section 4.5.
4.3.1 Motor Models
Motor physics are the most foundational for motion planning: as discussed, we need to understand these physics to know what our motion constraints are going to look like. This means electric motors in their many forms: classically we have Brushed DC, more modern control typically happens on Brushless DC (BLDC or PMSM: Permanent Magnet Synchronous Motors) which both rely on permanent magnetic fields interacting with changing magnetic fields, and Induction or Reluctance motors, which operate based on the field’s propensity to “want” to move through magnetic circuits that have low reluctance. Luckily for us, the models are all very similar and basically involve generating current with voltage, and then figuring out how current (which is a close proxy for magnetic field strength) relates to the motor’s torque and speed.
Because they are extremely common in machine control, I take particular interest in Stepper Motors, which are known technically as Hybrid Permanent Magnet Reluctance Motors. They work partially like a BLDC and partially like Reluctance Motors.
Our motor has a stator (the static part) with electromagnetic windings that can generate a magnetic field, and a rotor that has a permanent magnetic field. Motors work because these two fields (in the stator and rotor) want to line up with one another (which is a way of saying that they are in a lower energy state when they are aligned). The teeth in a stepper motor are arranged such that these aligned states are closer together: given some stator field, the closest alignment for the rotor is never more than \(7.2^\circ\) away from the rotor’s current position. That is, for every electrical rotation, when we rotate the magnetic field through \(0, 2 \pi\) only \(1/50^{th}\) of a mechanical rotation occurs.
When the motor is operating, we switch current in the stator to motivate the rotor to rotate continuously. In brushed DC motors, this happens mechanically: the brushes are aligned such that any current passing through them switches onto the windings that will generate current out of phase with the rotor. In brushless motors (steppers are one type of brushless motors), we do this using digital switches and some computing. This process is called commutation in all cases.
The motor physics for any electric motor breaks down into a few constitutive equations, which are below: the electrical half of the motor in 4.3, the mechanical equations in 4.4, and a table of each symbol, its units, and meaning in 4.1.
\[ \begin{aligned} Z &= \sqrt{R^2 + (\omega_e L)^2} \\ \omega_e &= \omega_m p \\ V_p &= V_a-k_e\omega_m \\ \dot{I} &= (V_p-RI) / L \\ I_{ss} &= V_a / Z \end{aligned} \tag{4.3}\]
The electrical half of the model describes how the voltage that we apply to the windings \(V_a\) becomes the rotor current \(I\) which will generate torque. The first equation above defines the motor’s impedance: \(Z, \ ohms\) — this is important especially in stepper motors where inductance \(L\) can be quite large, and is one half of the reason that generating current at high RPM’s is difficult: with very many pole pairs \((p)\) in a stepper motor (50), we quickly end up with large impedance in the stator when we get going at high speeds. The second equation shows how electrical and mechanical speeds are defined by pole pairs.
The third, fourth, and fifth lines above describe stator current. We have that the voltage potential across the stator windings — \(V_p\) — is the applied voltage less back-EMF \((k_e \omega_m).\) Back-EMF is the voltage that the rotor generates as its magnetic field moves through the stator’s windings.
The dynamic value of the stator current \(I\) is defined by its rate of change (fourth statement): \(\dot{I}\) is potential voltage less voltage already contained in the stator \((RI)\), divided by its inductance. This is the term we want to use if we want a time-varying estimate of current, or to estimate the maximum \(\dot{I}\) which becomes common later on in this chapter.
The mechanical half of the model (below) shows that torque generated by the motor \(\tau\) is a simple multiplication of stator current with the magical \(k_t\) parameter (which we will fit to data later on). The rotor acceleration \(\dot{\omega}_m\) is torque, less the torque produced by rotor friction \((k_d\omega_m)\) divided by the rotor inertia \(J,\) and the rotor’s angular position is just the integral of mechanical speed.
\[ \begin{aligned} \tau &= k_tI \\ \dot{\omega}_m &= (\tau-k_d\omega_m) / J \\ \Theta &= \int \omega_m \, dt \end{aligned} \tag{4.4}\]
| Symbol | Units | Value | Source |
|---|---|---|---|
| \(V_a\) | \(Volts\) | Voltage applied to the stator coils | Output(t) |
| \(V_p\) | \(Volts\) | Voltage potential at the stator coils | Product(t) |
| \(A\) | \(Amps\) | Current in the stator coils | Measurement(t) |
| \(R\) | \(Ohms\) | Stator’s DC Resistance | Measurement |
| \(L\) | \(Henries\) | Stator’s DC Inductance | Measurement |
| \(Z\) | \(Ohms\) | Stator’s Impedance | Product(t) |
| \(\tau\) (tau) | \(Nm\) | Torque | Measurement(t) |
| \(p\) | n/a | Number of Pole Pairs | Measurement |
| \(\omega_e\) (omega) | \(radians/s\) | Rotor Electrical Velocity | Product(t) |
| \(J\) | \(kg \cdot m^2\) | Rotor Inertia | Measurement |
| \(\Theta\) (Theta) | \(radians\) | Rotor Angle | Measurement(t) |
| \(\omega_m\) (omega) | \(radians/s\) | Rotor Mechanical Velocity | Product(t) |
| \(\dot{\omega}\) | \(radians/s^2\) | Rotor Acceleration (derivative of omega) | Product(t) |
| \(k_t\) | \(Nm/Amp\) | Torque Constant | Model Fit |
| \(k_e\) | \(Volts/rad/s\) | Back-EMF Constant | Model Fit |
| \(k_d\) | \(Nm/rad/s\) | Rotor Damping (friction) | Model Fit |
Two more notes on this, which are not required reading:
- The sign of \(I\) can easily be opposite to the sign of \(V_a\), i.e. when the motor is generating current in one direction while our controller is driving it in the opposite direction. This is where electric motors enter regeneration. This is very cool and useful when we are in a battery powered system (regeneration simply shows up as a lift in voltage on the supply, which charges the battery) but can cause issues where we use wall-supplied switching power supplies: they shutdown when the voltage at their output terminals rises substantially past their target output voltage. To ameliorate this, I simply mount a big bleed resistor to my power supplies to perpetually drain excess power (one is seen in Figure 3.20). Done properly, we use a switch across this resistor that only opens when the voltage rises above some threshold — that prevents us from spending energy to make the resistor hot.
- By the magic of SI units, the values of \(k_t\) and \(k_e\) are equivalent (although they have different physical meanings and units). One way to intuit this is that they both describe the interaction between the rotor and stator’s magnetic fields: \(k_t\) tells us ~ how much the stator’s field pulls on the rotor’s field, and \(k_e\) tells us ~ how much the rotor’s field drives voltage in the stator’s windings. For a better explainer on that topic, see [78].
Most of these values can be easily measured in the real world: \(R\) is a simple multimeter test, and \(L\) can be measured using an LCR probe. \(V\) and \(A\) are measured in the motor controller’s firmware, \(\Theta\) (and its derivatives for velocity \(\omega_m\) and acceleration \(\dot{\omega_m}\)) are also measured in firmware (using an encoder and Kalman filter). Rotor Inertia \(J\) is typically included in a motor datasheet, but I have at times taken motors apart to weigh and measure this when datasheets seem untrustworthy. That leaves \(k_t, k_e\) and \(k_d\) to fit computationally, which we will see later.
4.3.2 Kinematic Models
Except in some rare cases, motors don’t drive machine components directly. They are connected via belt drives, leadscrews and ballscrews, gearboxes (etc) or combinations of each. Understanding the machine’s kinematics enables us to translate between the machine motion we are interested in controlling and the motor models that constrain that motion.
Kinematic models for machines and robots are typically described as rigid bodies connected in kinematic chains using Homogeneous Transformation Matrices (HTMs). These encode translations and rotations. Their effects can be combined by multiplying matrices along the kinematic chain, and to invert a transform we simply take that HTM’s inverse, kind of tautological. Parallel kinematics can be more difficult to describe with HTMs, but we can normally simply pick one side of the parallel mechanism to model.
4.3.2.1 Cartesian to Actuator Coordinates
In this thesis, I wanted to use simpler kinematic descriptions that could easily be articulated as code. For machines, these are also often all we need. In fact for linear kinematic systems we can mostly get by with only the inverse kinematics. I describe these as simple Python functions, an approach that is shared by Gestalt [79] and is common also in open source machine control firmwares (where functions are instead written in C [25] [80]).
So, to start with the model formulations I use here, we have a vector of actuator positions \(\vec{x}_{\text{actu}}\) and positions in cartesian space, \(\vec{x}_{\text{cart}}\). For any given machine system, we have a function that describes there inverse kinematics - from the cartesian position to the actuator position. Generically, this is just:
\[ \vec{x}_{\text{actu}} = f_{IK}(\vec{x}_{\text{cart}}) \]
Importantly for our models, \(\vec{x}_{\text{actu}}\) units are in \(radians\) and \(\vec{x}_{\text{cart}}\) units are in \(meters.\) We will be looking at two systems in this chapter and the next two, one is a CoreXY based FFF 3D printer and the other is a (kinematically) simple milling machine, their kinematic functions are below.
# a, b: 32mm travel per rotation
# (and inverted direction)
# z: 8mm per rotation
rads_per_m = np.array([
-1.0 * (2.0 * np.pi) / 0.032,
-1.0 * (2.0 * np.pi) / 0.032,
(2.0 * np.pi) / 0.008,
])
def cart_2_actu_corexy(xyz):
abz = np.array([
(xyz[:, 0]-xyz[:, 1]) * rads_per_m[0],
(xyz[:, 0]+xyz[:, 1]) * rads_per_m[1],
(xyz[:, 2]) * rads_per_m[2],
])
return abz # x, y: 8mm travel per rotation
# z: 4mm per rotation
rads_per_m = np.array([
(2.0 * np.pi) / 0.008,
(2.0 * np.pi) / 0.008,
(2.0 * np.pi) / 0.004,
])
def cart_2_actu_mill(xyz_cart):
xyz_actu = np.array([
xyz_cart[:, 0] * rads_per_m[0],
xyz_cart[:, 1] * rads_per_m[1],
xyz_cart[:, 2] * rads_per_m[2]
])
return xyz_actu4.3.2.2 Applying Coulomb Friction
We need to add friction to these models. I use an approximation for Coulomb friction that combines a viscous term \(\mu_{f,v}\) that is multiplied by speed, with an offset term \(\mu_{f,static}\). The output from this model (defined with 4.3) is shown below in 4.5. In the rest of the section, I use the symbol \(F_c(v)\) to define this friction function.
def coulomb_friction_force(offset, slope, speed, smooth_interval=0.001):
return np.tanh(speed * (1.0 / smooth_interval)) * offset + slope * speed I had initially tried to fit models with viscous damping alone, but found that most machines have significant static friction. The function that describes Coulomb friction is simple but nonlinear. It is also sharp; its derivative briefly goes to infinity at the origin. These are two properties that make classical controls methods twinge, the jump from linear to nonlinear control representing a substantial step in complexity. Our motor models’ saturations (we can easily increase voltage applied to the motor, until we hit the maximum available from our power supply) are also simple but nonlinear and have sharp derivatives (dropping suddenly to zero at the saturation).
In the velocity planner that I develop in Section 4.6 the nonlinear property is easily managed numerically, but the sharp derivatives are not OK because the optimizer relies on gradients to make choices about how to update the solution. This is why \(tanh\) appears in my implementation of coulomb friction; it approximates static friction but smoothes it near the origin. I apply a similar trick with motor model saturations (and in a few other places in the planner, see Section 4.6.3).
4.3.2.3 Inertias
We also need inertias / masses. I measure these in \(kg\) for axes \((M_{\text{cart}})\) and \(kg \cdot m^2\) for rotary bodies e.g. motor rotors, leadscrews and couplers \((J_{\text{actu}}).\) Measurements are easy enough to make when we are assembling a new machine (a kitchen scale suffices, alongside radii of parts for rotary inertias) and given the uncertainty in system components that are more difficult to measure (like friction) I adopted this practice instead of trying to fit mass estimates alongside other kinematic parameters — but see Section 8.2 for some discussion on how we might estimate all of these parameters in a more “end-to-end” manner, which would be important for retrofitting cumbersome or difficult to disassemble equipment.
4.3.3 Combining Models for Constraint-Based Solving
To calculate actuator constraints in the planner, I combine these models such that we can predict the current required to drive our machine given any velocity and acceleration state at its tool tip, and our actuators’ current limits in those configurations. I show these steps in Equation 4.5 (for the CNC machine) and 4.6 (for the 3D printer) below.
4.3.3.1 Required Actuator Current from Cartesian Velocities and Accelerations
The equation below shows how I combine machine kinematics \(f_{IK}\) with known system states \(\vec{a}_{\text{cart}}, \vec{v}_{\text{cart}}\) and a damping model \(Fc(v)\) to estimate required current for the CNC milling machine. In this model we have damping terms on the cartesian system, but not on the actuator system, because those parameters would be degenerate if we tried to fit both sets. Instead, we just fit one set of damping terms, which is sufficient since damping of the motors is directly proportional to damping in the cartesian system: the one term effectively captures both.
\[ \begin{aligned} \vec{F}_{\text{req,cart}} &= \vec{a}_{\text{cart}} \cdot M_{\text{cart}} + Fc(\vec{v}_{\text{cart}}) + 9.8 \cdot M_z \\ \vec{a}_{\text{actu}} &= f_{IK}(\vec{a}_{\text{cart}}) \\ \vec{\tau}_{\text{req,actu}} &= f_{IK}(\vec{F}_{\text{req,cart}}) + \vec{a}_{\text{actu}} \cdot J_{\text{actu}} \\ \vec{I}_{\text{req,actu}} &= \vec{\tau}_{\text{req,actu}} / a_{kt} \end{aligned} \tag{4.5}\]
Below is the same but for the 3D printer. They both work in a similar manner. First we calculate the force required to move the cartesian system around, which is just \(F=ma + F_f\) we then take cartesian states and transform them to actuator states: for linear kinematics, we can use the same \(f_{IK}()\) to transform velocity, accelerations, or forces (so long as we use SI units). We add to the required torque \((\vec{\tau}_{\text{req,actu}})\) the requirement for spinning the motor rotor and (in the case of the 3D printer, where rotor velocity is not a 1:1 combination of one axis’ cartesian velocity), we fit a set of damping terms in the actuator space as well. Current estimation is then simple, dividing required torque by the actuators’ \(k_t\).
\[ \begin{aligned} \vec{F}_{\text{req,cart}} &= \vec{a}_{\text{cart}} \cdot M_{\text{cart}} + Fc(\vec{v}_{\text{cart}}) \\ \vec{a}_{\text{actu}} &= f_{IK}(\vec{a}_{\text{cart}}) \\ \vec{v}_{\text{actu}} &= f_{IK}(\vec{v}_{\text{cart}}) \\ \vec{\tau}_{\text{req,actu}} &= f_{IK}(\vec{F}_{\text{req,cart}}) + \vec{a}_{\text{actu}} \cdot J_{\text{actu}} + Fc(\vec{a}_{\text{actu}}) \\ \vec{I}_{\text{req,actu}} &= \vec{\tau}_{\text{req,actu}} / a_{kt} \end{aligned} \tag{4.6}\]
This step uses machine parameters that have been loaded in the planner’s configuration: kinematic functions (authored as Python functions), along with damping and mass parameters and motor model parameters. I use the same formulations also to fit models to data because we can easily collect time-series velocity and acceleration states of the machine using our motors and planners. This is a useful property of the planner’s formulation. Especially because the planner is authored in Python (which has an excellent set of function fitting tools) we can actually use the exact same block of code for both tasks.
4.3.3.2 Actuator Current Limits from Actuator Velocities
The step above includes calculation of the actuator velocities \(\vec{v}_{\text{actu}}\) which we can use to calculate each actuator’s real current limits \(I_{\text{lim}}, \ \dot{I}_{\text{lim}}\) for each state. In the motion planning step, calculating these limits is important because they represent the actuator constraints.
The power supply voltage and driver current limits17 are both saturations not described by the motor models themselves. Supply and driver saturations are one of the most common sources of nonlinearity in real systems because they pop up even in where the dynamics are otherwise linear — nobody has a power supply with infinite available voltage! To apply these saturations without causing gradients of the models to vanish (see Section 4.6.3), I use \(\tanh\) to clip values. This also leads overall more conservative use of actuators than a simple clip would.
| Value | In Motor Models | For Limit Calculation |
|---|---|---|
| Applied Voltage, at supply limit | \(V_a\) | \(V_{\text{drv,max}}\) |
| Driver current limit | n/a | \(I_{\text{drv,max}}\) |
| Limits to Potential Voltage | \(V_p\) | \(V_{\text{lim}^+}, V_{\text{lim}^-}\) |
| Limits to Motor Current | \(I\) | \(I_{\text{lim}^+}, I_{\text{lim}^-}\) |
So, combining some parts of the electrical motor equations 4.3 with those saturations, we drive the motor models with maximum supply voltage to find the current limit. These functions are also applied with negative-signed supply voltages to produce lower bounds for each, \(V_{\text{lim}^-}\) and \(I_{\text{lim}^-}\).
\[ \begin{aligned} Z &= \sqrt{R^2 + (L \cdot \omega_e)^2} \\ V_{\text{lim}^+} &= \tanh \left( \frac{(V_{\text{drv,max}} - k_e \omega_m)}{V_{\text{drv,max}}} \right) \cdot V_{\text{drv,max}} \\ I_{\text{lim}^+} &= \tanh \left( \frac{V_{\text{max}} / Z}{I_{\text{drv,max}}} \right) \cdot I_{\text{drv,max}} \end{aligned} \tag{4.7}\]
We also need limits for the rate of change of current \(\dot{I}_{\text{lim}}.\) In steady state, this is just the applied voltage divided by the speed-varying impedance (fifth line of Equation 4.3 from 4.3.1). This is the value that I use in the planner (4.6.1), again with positive and negative limits.
\[ \dot{I}_{\text{lim}^+} = \frac{V_{\text{drv,max}}}{L} \tag{4.8}\]
This is technically only true when the motor is at a standstill. Looking back to the third line of Equation 4.3 shows that our real limit actually changes with speed (via the change above to \(V_{\text{lim}}\)):
\[ \dot{I}_{\text{lim}^+} = (V_{\text{lim}^+} - RI) / L \tag{4.9}\]
There is nothing structural in the planner that prevents me from using the second, more complete definition for \(\dot{I}_{\text{lim}}\) — I simply did not implement this because the steady-state approximation is good enough for our purposes.
4.4 Field-Oriented Control on (almost) Any Stepper
Developing this Field-Oriented Control (FOC) motor controller was a key enabler of the research in this thesis because it turns motors into equal parts actuator and sensor; the controller can both drive and measure stator currents, and measures rotor position for commutation18 and for position / velocity control. When we combine those readings with motor models, the current measurements become torque measurements via \(\tau = k_tI\) from Equation 4.4. This is known as proprioception and is a common method in the field of robotics [81].
Most proprioceptive actuators are based on BLDC (Brushless DC) motors, but I elected to develop this controller for stepper motors. In Section 4.3.1 I gave a brief overview of these motors: they are ubiquitous in all types of machines and especially common in simpler equipment because they do not require closed-loop control. Because they have very many electrical rotations for every mechanical rotation (i.e. they have many “pole pairs”), a controller that sequentially “steps” through those electrical rotations can position the motor using dead reckoning. However, when stepper motors are loaded beyond their limits they “miss steps” and higher level controllers loose track of the machine’s position. Although some modern drivers can detect skipped steps, for many other reasons it is advantageous to drive them under “full” closed-loop control: this can reduce errors, improve precision, and above all provide us with dynamical feedback; open-loop controllers do not track the internal states (like current and rotor angle) that are most useful in higher level controllers.
So, to drive steppers as closed-loop motors I am using an architecture called FOC (Field Oriented Control), which is also more commonly used for BLDCs but which can be deployed on almost any motor architecture. To implement the FOC algorithm I relied heavily on Dave Wilson’s excellent blog posts and video seminars on the topic [82]. It is not common to drive stepper motors in this way, but there are a few examples of other implementations in open source hardware communities but none that couple the FOC algorithm to motor modelling methods and motion control etc. The main motivation to develop this type of control for steppers arises from their ubiquity and cost, but in Section 7.5 I share some more thoughts on why this type of motor architecture may actually be better overall for machine control.
Driving steppers with FOC is especially difficult because their high pole pair count means that commutation must be performed very quickly even at relatively low rotor velocities. But, high performance embedded computing is becoming cheaper. This circuit uses a SAMD51 microcontroller, two DRV8251A H-Bridges (which integrate current sensing hardware) and an AS5047P magnetic encoder with 14 bits of resolution. The encoder is contactless and reads field direction from a diametrically magnetized disk that can be attached to the rear shaft of a stepper motor. Despite the SAMD51’s relatively high performance (at \(120\text{MHz}\) and including an FPU), its cycles are nearly entirely used up to operate the control interrupts in the firmware that I developed here. The circuit is also limited in total drive power by the DRV8251A’s maximum current rating (at \(4.5A\) each) and the board’s small footprint, which limits heat flow out of the driver.
The controller’s layout is diagrammed in Figure 4.7. FOC manages the lowest level: current control and commutation. At the heart of FOC are two PI current controllers for the D and Q axes. These are aligned to the rotor’s reference frame such that D is direct (aligned to rotor north) and Q is quadrature to the rotor (\(90\,^\circ\) from D). Only the current that is aligned with Q produces torque, so typically the D controller works to drive current there to zero and the Q controller works to drive whichever current is requested by any higher level controller. Key to the FOC algorithm are the AB/DQ and DQ/AB transforms: current is measured by the H-Bridges in the AB frame, which is static. By transforming these readings into the DQ frame, the lower level current controllers don’t have to track rapidly oscillating sinusoidal commands which greatly reduces their bandwidth requirements. They generate voltage signals in DQ which are then transformed back into the AB frame before being applied to the H-Bridges. There are a handful of other implementation-specific details that would be important to reproduce this work, but they are not novel research. I included them in an earlier blog post [83] but here I will stick with the details that are immediately relevant to the rest of the thesis.
One layer up, the controller implements either a second PI controller for velocity control, or a PID controller for position control. The velocity and position controllers can be controlled directly or configured to track the output of a MAXL spline interpolator. The spline additionally produces target velocities and accelerations, which can be combined with feed-forward terms for either to improve performance. For example in a system with known dynamics, the amount of current required to generate a target acceleration or overcome friction at a certain speed is known. To improve estimates of the rotor position, velocity and acceleration the controller implements a Kalman filter [84] [85].
4.4.0.1 Encoder Calibration
The magnetic encoder used here has some nonlinearity because the magnet is never perfectly aligned with the hall sensors in the encoder. Especially because the motor’s pole pairs are so close together, calibrating these nonlinearities away is important. Luckily the encoder provides absolute readings, so one calibration can be performed for each motor. This produces two look-up tables (LUTs); one relates encoder readings to rotor electrical angles, and the second relates encoder readings to rotor mechanical angles. Both apply the same offset, but the electrical LUT has 50 times as many rotations (for the 50 pole pairs) and outputs \(16\text{bit}\) values whereas the mechanical LUT outputs floating point values in \(0 \rightarrow 2\pi\) — angular measurements in radians. I use integer values for the electrical LUT because the AB/DQ and DQ/AB transforms themselves use look-up tables to perform their trigonometric calculations. This is faster than relying on sin and cos functions, which can take many cycles each; the LUTs trade memory for computation. Equally, the encoder calibrations could be applied using computation but the LUT is faster here too.
This is another place where reconfigurable access to the motor is valuable. To fit the encoder LUTs I configure a MAXL / PIPES script to drive the motor, attaching the spline to the motor’s position target but driving the motor using open loop current control, and use a MAXL OneDOF block to rotate the motor at a set velocity while reading raw encoder, current, and voltage signals back through a Pipes Function that transmits low level motor states. I can then analyze these readings in a script that fits a set of correcting function parameters to produce the interpolations in Figure 4.8. This then writes look-up tables as .h files that are loaded into the motor’s firmware. This last step could presumably be automated as well, but the simpler approach works well for the time being. Low-level control of the motor is then also used to drive quadrature current requests directly in order to fit motor models in Section 4.5.1.
4.4.0.2 Current, Velocity and Position Control
In the motor’s lower level control, gains are tuned by hand. There are actually quite a few parameters here, in Section 4.9.3.2 I discuss how this is one of the key “missing components” to the motion control architecture overall, but tuning these using models is well understood and so would presumably be a simple addition.
Firstly there are PI gains in the current controllers. Luckily they can be set directly via the motor’s \(R\) and \(L\) using the method developed by Dave Wilson [82] (\(R, L\) themselves are easily measured, see Section 4.5.1).
\[ \begin{aligned} k_{p,PI} &= R \\ k_{i,PI} &= 2\pi f_c \cdot L \end{aligned} \]
The PI / PID gains for the velocity or position controllers are a different story; tuning them properly requires a complete model not only of the motor’s electrical half but also how the rotor interacts with the rest of the machine. I do collect and fit all of those models in this chapter, but I did not connect them to automatic PID tuning — instead I simply tune these by hand. To do so I use a PIPES / MAXL configuration using a chirp generator that drives machines across a range of velocities and accelerations and cycle through tests, analysis and gains updates; this is described in more detail in Section 4.5.2.
These controllers have bandwidth limits because they rely on error signals to produce outputs. The feed-forward terms that I mentioned above can help, but the bandwidth limits do impose additional constraints on control overall. Luckily here they are quite fast; the current controllers run at \(30\text{kHz}\) which is well above the electrical bandwidth of most stepper motors and the velocity / position controller runs at \(15\text{kHz}\).
One of the key limits for the velocity controller is that it uses a filtered velocity estimate that has a time-constant of about one millisecond. Velocity control for the extruder system poses some problems in the 3D printer’s extruder system (see Sections 5.9.1 and 5.11.7.5)
4.4.1 The Motor’s Software and Data Interfaces
| Value | Units | Data Type | Name |
|---|---|---|---|
| System time at sample generation | \(\mu\mathrm{s}\) | uint64 |
timestamp |
| Position from the interpolated MAXL spline | \(rads\) | float |
maxl_position |
| Velocity | \(rads/s\) | float |
maxl_velocity |
| Acceleration | \(rads/s^2\) | float |
maxl_accel |
| Target current from internal pos / vel controller | \(Amps\) | float |
cur_cmd |
| Measured current on the Q axis | \(Amps\) | float |
cur_q |
| Kalman estimate of unwrapped angular position | \(rads\) | float |
obsv_position |
| Kalman estimate of angular rate | \(rads/s\) | float |
obsv_velocity |
| Kalman estimate of angular acceleration | \(rads/s^2\) | float |
obsv_accel |
| Controller error flags | \(\text{bitmask}\) | uint8 |
ctrl_flags |
| MAXL error flags | \(\text{bitmask}\) | uint8 |
maxl_flags |
| Value | Units | Data Type | Name |
|---|---|---|---|
| System time at sample generation | \(\mu\mathrm{s}\) | uint64 |
timestamp |
| Raw encoder reading \(\in [0, 16383]\) | \(\text{quantized}\) | uint16 |
encoder_raw |
| Position from the interpolated MAXL spline | \(rads\) | float |
maxl_pos |
| Velocity from the spline | \(rads/s\) | float |
maxl_vel |
| Acceleration from the spline | \(rads/s^2\) | float |
maxl_accl |
| Kalman estimate of unwrapped angular position | \(rads\) | float |
obsv_pos |
| Kalman estimate of angular rate | \(rads/s\) | float |
obsv_vel |
| Kalman estimate of angular acceleration | \(rads/s^2\) | float |
obsv_accl |
| Measured voltage on the supply lines | \(Volts\) | float |
vcc_rails |
| Voltage applied on the D axis | \(Volts\) | float |
vcc_d |
| Voltage applied on the Q axis | \(Volts\) | float |
vcc_q |
| Target current from internal pos / vel controller | \(Amps\) | float |
cur_cmd |
| Contribution of the P term to the current command | \(Amps\) | float |
pid_p |
| Contribution of the I term | \(Amps\) | float |
pid_i |
| Contribution of the D term | \(Amps\) | float |
pid_d |
| Measured current on the D axis | \(Amps\) | float |
cur_d |
| Measured current on the Q axis | \(Amps\) | float |
cur_q |
| Controller error flags | \(\text{bitmask}\) | uint8 |
ctrl_flags |
| MAXL error flags | \(\text{bitmask}\) | uint8 |
maxl_flags |
| Index of last-inserted control point in ring buffer | \(\text{index}\) | uint8 |
maxl_insert |
| Timestamp of closest (in the past) control point | \(\mu\mathrm{s}\) | uint64 |
last_eval |
| Timestamp of last control point wiped from buffer | \(\mu\mathrm{s}\) | uint64 |
last_tail |
The motor is attached to higher level controllers using PIPES / MAXL via the spline interpolator, and a set of Pipes Functions are used to configure its control modes (i.e. whether the spline outputs are used to drive the velocity controller, position controller, or other lower levels).
The driver also streams internal control states back to data collectors and controllers using PIPES. I normally do this by configuring one of these functions to run at a fixed interval to produce streams of data in time-series. A short list of the most important states in one of those streams are listed in 4.3 below. States are collected in each loop of the controller’s main step (at \(15\text{kHz}\)) and the bundle of them is timestamped against the OSAP system time so that the exact measurement time can be reconciled with other system states.
The actual data rate that these streams run at is configurable using PIPES, and the maximum possible rate is a function of underlying network performance and traffic; I typically set this at \(500\text{HZ}\) which is above most machine’s physical bandwidth — see i.e. Figures 4.19 or 4.24. In Section 2.6.2 I compare two other state-of-the-art machine control efforts; they collect smaller data streams (just motor position, velocity, and current) at lower data rates (\(250\) and \(100\text{Hz}\)). I write a more complete systems-level timing analysis in Section 2.7.9.
I also include here Table 4.4, a more extensive set of values that I use during motor modelling, PID tuning, and motor firmware debugging.
Finally, the driver’s control parameters and configurations can all be read or written remotely using Pipes Functions (normally as RPCs). Besides the encoder calibration, all of these values are remotely updated when systems are initialized and so motors do not need to themselves store or “remember” any of these values in their own memory; configurations are tracked at a systems level.
4.5 Fitting Motion Models
Given the models from 4.3, and our motor above, this section describes how I fit free parameters (and which ones I simply measure directly).
4.5.1 Fitting Motor Models
Table 4.1 renders the motor model parameters, and denotes which of these are measured directly (labelled Measurement) or measured by the controller and available as time-series measurements or easily calculated products of other measurements (labelled Measurement(t), Product(t)).
Measurements of static values are often straightforward. I measure \(R\) and \(L\) with an off-the-shelf LCR probe, the pole-pair count \(p\) is always \(50\) for stepper motors (and easy to count otherwise). The rotor inertia \(J\) can be measured, but only by disassembling the motor, taking dimensions of the rotor, and weighing it. This is cumbersome, and so I mostly rely on motor datasheets to include this value. In 4.9.3.2, I discuss how it is possible to add estimation of these parameters to this step.
However, that is for later. Here, we make them manually. Then we are left with the three “key” model parameters, \(k_t, k_e, k_d\). To estimate those, I developed a simple program that runs the motor in a direct current mode at a number of pre-determined current targets. It runs each for a period that is long enough to get the rotor up to its steady-state velocity (where damping and back-emf overcome the motor/driver’s ability to generate current). I run this program with the motor fixed on a desk, and mounted to a flywheel whose inertia has been measured.
Test data stores all the values output by the motor in Table 4.3 in time-series, as well as the test parameters. We can then reconstruct this time-series and fit our model against it.
I do this in two steps, using Python and scipy’s optimize routine [86], [87]. First, we can fit the mechanical parameters \(k_t, k_d\) using the motor’s measurements of its velocity, acceleration, and stator current:
\[ A = (k_tI-k_d\omega_m) / J \]
Parameters are minimized against a cost function that compares parameters’ acceleration estimates (from above) against acceleration measurements from the motor data, using sum squared error. The motor/actuator’s rotor inertia \(J_a\) was manually measured previously, and is combined with the inertia of the test flywheel for this step. The flywheel does two things that are valuable here. First it increases the time that the motor spends accelerating and decelerating, increasing data density in high acceleration, high current, and low velocity states. It also has much more inertia than the motor rotor, which reduces overall model error that may come from a rotor inertia estimate miss because its contribution to inertia is so large compared to the motor rotor itself.
To improve the fit, I also weight measurements by their distribution in velocity. This is useful because most measurements are taken when the motor is at it’s steady-state (terminal) velocity, but we want to ensure the fit is equally biased across velocity states.
def model_kt_kd(params, q_current, vel):
kt, kd = params
accel = (kt * q_current - kd * vel) / J
return accel
def loss_kt_kd(params, cur_data, vel_data, acc_data):
acc_guess = model_kt_kd(params, cur_data, vel_data)
return np.sum(np.square(acc_guess - acc_data) * weights) I estimate \(k_e\) separately, despite my note earlier (4.3.1) that the values for \(k_t\) and \(k_e\) should be the same. This is a bit of a hack, but is a valuable step because we can use this parameter to encode some more information about the motor and controller - that is, that the controller’s commutation is not perfect and introduces a small amount of phase lag. When it sets voltage on the HBridge’s PWMs, those voltages take some microseconds to be actualized in the drive. During those microseconds, the rotor continues spinning. This means that at high RPMs, it seems as though we have more back-EMF, that is, applied voltage becomes less effective.
Fitting this is similar, the model estimates the current measurement given the rotor velocity, some estimated \(k_e,\) and the voltage applied on the \(q\) axis. The estimated current is compared to the measured current, making a sum-squared error, which is used to fit the parameter using scipy’s minimize routine.
To verify that models explain the motor system, we can plot time-series data against a simulated system, where the recorded control input (a target current) is used to drive the simulation.
Models can also be used to generate torque curves across a range of driver currents, as below:
Limit to torque generation in these models comes from two places. Firstly, the driver’s current limits (or current set points) saturate. Above, you can see that the motor’s torque is limited to \(0.8N\) at zero velocity, simply because we cannot drive more than (in this case) \(2.5A\) through the driver’s H-Bridges. In practice this limit can be set also due to thermal limits in the motor. As the motor comes up to speed, current generation is limited by back-EMF and the motor’s impedance, which grows as speed increases.
Torque curves are an excellent analysis tool, and they are commonly used by machine designers to select actuators. For example if we know that an axis of our machine will mostly be responsible for fast accelerations at low speeds, we will pick a motor with more torque “in the low end” whereas if we have a large gear ratio and need our motor to work at large RPMs, we will look for motors where the torque does not fall off drastically as speed increases. In Section 6.2.1, I show how we can combine motor and kinematic models to virtually test multiple combinations of each to pick motors using data from our hardware (rather than datasheets), and select velocity controller parameters (for trapezoidal planners) using the same.
4.5.2 Fitting Kinematic Models
Fitting kinematic parameters follows a similar pattern. Before we start,
- Models for each of the machine’s actuators are fit using the method above.
- System inertias \(J, m\) are measured manually.
- Inverse kinematics \(f_{IK}()\) are written down as Python functions.
- An initial set of PID gains are written for the actuators’ position tracking low-level loop. These do not have to be well tuned, in fact I used the same starter gains for each actuator in all the systems that I developed in this thesis.
We then have a controllable system, and need to excite it in order to generate some test data. I use MAXL and PIPES to connect a chirp generator (2.5.4.3) to the axes that we are interested in fitting, using it to drive their positions. Chirp signals are time-varying sinusoids that slew from an initial to a final frequency (typically low to high) over a fixed time. Parameters for the chirp are configured manually. The system for one of these tests was shown in the systems chapter (Fig. 2.30). We can then collect time-series data for the test, an example of which is figured below in Figure 4.10.
I first use this data to tune the motors’ PID gains, which I do iteratively. This is a somewhat involved process, because improvements to gains allow us to drive the system at higher chirp frequencies — so during this process I am heuristically learning about the systems’ overall limits. However, it is a well understood process, so I will not elaborate except to note that Section 4.9.3.2 discusses how this could be automated in the future.
Even with data from poorly tuned gains, we can update our models. The only free parameters at this point in the kinematic models are the damping terms. As in the motor models, I set up a function that estimates the motor current using the motion states (acceleration and velocity) for each axis under test and a loss that compares that estimate to measurements from the dataset. This is the same system of equations as in Equation 4.5 or in Equation 4.6 when we are sampling data from a CoreXY based machine.
Fits can be visually verified by comparing their current estimates to measurements across time series data, as shown below. We can also extract global metrics from these time-series like average model estimate error (in \(Amps\)), which I expand on in Section 4.7.2.
4.6 Optimization Based Velocity Planning
With models for our machines, we can move on to velocity planning. The goal here is to develop an optimizer that will maximize velocities along a target trajectory without exceeding actuator constraints. The planner that I develop here differs from trapezoid planners in a few key ways.
First, it uses machine models to describe constraints — this makes the planner configuration more direct: we describe the system, rather than interpreting the system’s constraints as a set of parameters for maximum acceleration and maximum velocity. This also allows the planner to use more of the machine’s underlying dynamic range, and lets us describe constraints that emerge in the actuator coordinate space and those that emerge in the machine’s tool-tip (cartesian) coordinate space — this is valuable in kinematic systems where the two spaces are not identical, e.g. CoreXY. I discuss those improvements in Section 4.7.1. To tune the planner within those constraints, I use actuator current deployment scalars that de-tune the system as a percentage of total available actuator power19, rather than as direct parameters.
Second, it can be more easily extended to capture constraints that emerge from our process descriptions, something that I explore at length in Chapter 5, in particular 5.2.3 (which discusses the state-of-the-art in this regard) and 5.9 (which discusses how this planner is extended for 3D printing processes). Section 4.7.1 shows how the same idea is applied (albeit in a much simpler manner) to the CNC machining process.
Optimization-based controllers are themselves not new to the world. They are popular in robotics, where MPC (Model Predictive Control, see Section 4.2.7) has become quite common over the last decade, and have been applied to CNC machines and one polymer extrusion system. More recently, RL (Reinforcement Learning, 4.2.7.2) is emerging as a dominant practice. RL is more appropriate for more complex tasks, and typically run at much lower bandwidths and resolutions than are required for machine control.
In this section I describe the motion control component of the planner, in Section 5.9, I describe how it can be extended to include process dynamics.
4.6.1 Optimization Formulation
This formulation is similar to the direct transcription method that I described in 4.2.7, but different in two key ways. The first is that the planner picks velocities in space20 rather than at fixed time intervals. I take input trajectories and break them up into very many, very small line segments (as \(\vec{x}(s)\) below, with a distance of \(50 \mu m\) between each, although the value is tune-able, see Section 4.8.3).
Then, rather than choosing control inputs at each step as well as those velocities (and having the planner align those implicitly), we work backwards to calculate the control inputs that would be required to achieve those velocities, and compare those to actuator limits in our cost function: violating limits is much more expensive than going slowly.
This is also not really Model Predictive Control, although it does use models to predict optimal control outputs. Normally MPC feeds measured system states back into the start of new plans at each cycle, but here we are purely feed-forward. At a higher level, we fit models to hardware — so we could say that it is configured with feedback. Lower levels of the controller are feedback systems: the motors servo to position targets that the planner outputs. I discuss the fact that “feed-forward” does not mean “bad” in Section 7.5.
The planner is designed so that each step can be completed as a parallel computation over the whole trajectory. This structure is what enables the planner to be deployed on parallel computing hardware which greatly improves performance. Below (and diagrammed in Equation 4.10) are the basic steps.
First we provide a target trajectory as input that includes the 3D positions \(\vec{x}(s)\) where \(s\) is the length along the trajectory at any given point and then trajectory settings: maximum velocities \(v_{\text{max}}(s);\) in many cases we want our planner to travel at some set velocity, that goes here. The target trajectory also includes the deployment scalars for current and the rate of change of current \(I_{\text{prop}}(s), \ \dot{I}_{\text{prop}}(s)\). In an important first step this trajectory is segmented from its incoming representation into many small line segments each of which is \(0.05mm\) in length (configurable).
The planner selects \(v(s),\) a candidate set of velocities along the trajectory for each point. We then calculate the accelerations required (\(\vec{a}(s)\) in cartesian space) to accomplish these selections, see Equation 4.13 in 4.6.1.1 for some details on that calculation.
We then have the key states for acceleration and velocity in cartesian space \((\vec{a}(s), \ \vec{v}(s)).\) Other planner constrain motion based maximum values for those states alone. In this planner, I broadcast those back through the models that I described in Section 4.3.3 to generate the planner’s intermediate values and then compare those to actuator constraints. Those values are: the current required in each actuator, current generation limits for each (including nonlinear motor model saturations and velocity dependencies) and then (using a simple discrete derivative) requirements for the step change in current at each step \(\dot{I}_{\text{req}}\) and limits for the same from Equation 4.8 in the motor modelling section 4.3.1.
\[ \overbrace{ \begin{bmatrix} \vec{x} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} v_{\text{max}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} I_{\text{prop}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \dot{I}_{\text{prop}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} }^{\text{Input Trajectory}} \rightarrow \overbrace{ \begin{bmatrix} v \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} }^{\text{Select}} \rightarrow \overbrace{ \begin{bmatrix} \vec{a} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \Delta t \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} }^{f_a()} \rightarrow \overbrace{ \begin{bmatrix} \vec{I}_{\text{req}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{I}_{\text{lim}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{\dot{I}}_{\text{req}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{\dot{I}}_{\text{lim}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} }^{\text{Intermediate Values}} \tag{4.10}\]
Those intermediate values are sent into the cost function (see details in 4.6.1.3), where exceedances of actuator constraints \(I_{2big}\) are penalized. The cost function also receives all other values: the input trajectory, a copy of the planner’s selections, accelerations, and velocities in the actuator space, etc. The total sum over \(\Delta t\) is added to the cost as well to encode time optimality (small number fast, good). If we like, it is straightforward to also add terms to the cost function that constrain any of these other states directly.
The cost function is subject to minimization via updates to the planner’s selection of \(v(s)\) using a generic gradient descent optimizer; I explain the actual update step and solving framework in Section 4.6.1.4.
\[ \begin{aligned} f_{\text{cost}} = \sum_{s} & \Bigl[\, I_{2big}(s) \Bigr] + \sum_{s} \Bigl[\, \Delta t(s) \Bigr] \\ & \min_{v(s)} \; \text{cost} \end{aligned} \tag{4.11}\]
The planner iteratively improves the solution until a fixed number of cycles are completed (again, see 4.6.1.4 for details), after which time the planner needs to send new values to hardware and intermediate values to a data store for later analysis. That is diagrammed below and explained in some more detail in Section 4.6.1.5 on converting from the spatial representation to a time encoded representation. Integration of the planner with the remainder of the machine system is via PIPES/MAXL (Chapter 2), explained in 4.6.2.
\[ \overbrace{ \begin{bmatrix} \vec{x} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} v \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{a} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{I}_{\text{req}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{I}_{\text{lim}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{\dot{I}}_{\text{req}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} \begin{bmatrix} \vec{\dot{I}}_{\text{lim}} \\ \vdots \\ \vdots \\ \vdots \\ s \end{bmatrix} }^{\text{Spatial Trajectory and Solution}} \rightarrow \overbrace{ \begin{bmatrix} \Delta t \\ \vdots \\ s \end{bmatrix} }^{\forall s \in t_{\text{cycle}}} \rightarrow \overbrace{ \begin{bmatrix} \vec{x} \\ \vdots \\ t \end{bmatrix} \begin{bmatrix} \vec{a} \\ \vdots \\ t \end{bmatrix} \begin{bmatrix} \vec{I}_{\text{req}} \\ \vdots \\ t \end{bmatrix} \dots \begin{bmatrix} \vec{\dot{I}}_{\text{lim}} \\ \vdots \\ t \end{bmatrix} }^{\text{Time-Based Solution Outputs}} \tag{4.12}\]
I show samples of the planner’s visualizer in two figures below, these include 2D trajectory visualizations and spatial- and time-series outputs of some intermediate values, as well as a planner convergence plot.
4.6.1.1 Calculating Required Accelerations from Velocities
The planner’s velocity selections are for motion along the trajectory at fixed spatial steps. To calculate other system states, we need to transform those into accelerations at each point in all axes that would be required to achieve the selected velocities \(\vec{a}_{\text{req}}\) and time steps \(\Delta t\) for each spatial step.
This is a somewhat lossy abstraction because we are approximating cornering through a representation (line segments) that in reality has sharp corners. Properly described, maintaining any velocity through a sharp corner generates an infinite acceleration pulse on axes that are not aligned with the initial line segment — this is the same issue that junction deviation (used in trapezoid planners, 4.2.4) addresses.
So, this step is \(f_a()\) from Equation 4.10:
\[ \begin{aligned} \Delta t, \ \vec{a}_{\text{req}} &= f_a(v_{(s)}, \hat{v}_{(s)}, \Delta d) \end{aligned} \tag{4.13}\]
There are two representations that we care about: velocities along the trajectory \(v\) and in cartesian space \(\vec{v}, \ \vec{a}.\) First we compute initial and final velocities in cartesian space, projecting \(v(s)\) along unit vectors (from the trajectory’s \(\vec{x}(s)\)) to make a \(\vec{v}_i, \vec{v}_f\) for each linear segment:
\[ \begin{aligned} \vec{v}_i &= v_{(s)} \cdot \hat{v}_{(s)} \\ \vec{v}_f &= v_{(s + 1)} \cdot \hat{v}_{(s+1)} \\ \end{aligned} \tag{4.14}\]
We have a fixed \(\Delta d\) step between out two points that covers the distance along the trajectory, but these two velocities are in cartesian space. To compute the time step we want to look at the component of motion that is aligned with the trajectory only because the actual arc traversed between these two velocities has a variable arc length depending on the (changing each planner iteration) velocities. The initial velocity at this point is just the selected \(v(s)\) and the final velocity is the projection of \(\vec{v}_f\) onto the unit vector at the start.
\[ \begin{aligned} v_i &= v_{(s)} \\ v_f &= \text{proj}_{\vec{v}_f}\hat{v}_i \\ \end{aligned} \tag{4.15}\]
This has the important caveat that corners sharper than \(90 ^\circ\) have negative \(v_f\) along the trajectory, which complicates the next step (causing divisions-by-zero or negative time steps). I will note that I am working to develop an improvement to this step, see Sections 4.8.5.3 and 4.9.1. In the implementation that I used throughout the thesis, I instead assert that \(v_f == v_{\text{min}}\) for those acute corners, where \(v_{\text{min}}\) is a configurable planner parameter that ensures time steps do not become too wide.
Caveats aside, the time step for motion along the segment can be had now via the kinematic equations for line segments with fixed acceleration:
\[ \begin{aligned} \Delta t &= \Delta d / \left( \frac{1}{2} (v_f + v_i) \right) \\ \Delta t &= \text{softclamp}(\Delta t, t_{\text{min}}, t_{\text{max}}) \\ \end{aligned} \tag{4.16}\]
To ensure that time steps lie within acceptable boundaries, I clamp them after this computation using a smooth clamping function, discussed in Section 4.6.3. Once we have time steps, cartesian acceleration is just the discrete derivative of the two velocities.
\[ \begin{aligned} \vec{a}_{\text{req}} &= \left( \vec{v}_f-\vec{v}_i \right) / \Delta t \end{aligned} \tag{4.17}\]
That’s the end of this step. As I mentioned, it is a marginally lossy abstraction that is permissible because the line segments are so small. I work out an improvement to it in Section 4.9.1, using quadratic Béziers.
4.6.1.2 Calculating Required Actuator Currents
Given motion states along the trajectory, we need to broadcast these states back through the machine’s kinematics and into the motor models. I explained these formulations alongside their associated models in 4.3.3.
To recap, we have two steps. The first uses inverse kinematics and friction models to sum current required in the actuator along two paths, one of which accounts for forces required to move the cartesian system around and the other accounts for forces required to move the actuators’ own mass (rotor inertias) through friction present in the actuator space (rotor bearings and i.e. belts). This is described in full in 4.3.3.1 but also summarized in Equation 4.18 below.
\[ \begin{aligned} \vec{I}_{\text{req}} \leftarrow \vec{k_t} & \leftarrow \vec{\tau}_{\text{req,actu}} \leftarrow \text{Rotor} J, f_c \leftarrow \vec{v}_{\text{actu}}, \vec{a}_{\text{actu}} \leftarrow \text{IK} \leftarrow \vec{a}_{\text{cart}}, \vec{v}_{\text{cart}} \\ & \leftarrow \vec{\tau}_{\text{req,cart}} \leftarrow \text{IK} \leftarrow \vec{F}_{\text{req,cart}} \leftarrow \text{Axes} M, f_c \leftarrow \end{aligned} \tag{4.18}\]
The second step calculates actuator current limits based on their velocities, recall that our motor’s ability to generate current decays at high speeds and that they have important saturations for maximum voltage and drive current. Again this is explained in more detail in 4.3.3.2 but summarized below:
\[ \vec{I}_{\text{lim}^-}, \vec{I}_{\text{lim}^+}, \ \leftarrow \ \text{Electrical Models} \ \leftarrow \ \vec{v}_{\text{actu}} \ \leftarrow \ \text{IK} \ \leftarrow \ \vec{v}_{\text{cart}} \tag{4.19}\]
4.6.1.3 The Cost Function
The cost of the solution has two components. The first calculates exceedances of the actuator’s current limits — remember that \(I_{\text{lim}}\) includes positive and negative limits (for maximum and minimum applied voltages), so exceedances are cases where these criteria are not met:
\[ \begin{aligned} I_{\text{lim}^-} &< I_{\text{req}} < I_{\text{lim}^+} \\ \dot{I}_{\text{lim}^-} &< \dot{I}_{\text{req}} < \dot{I}_{\text{lim}^+} \end{aligned} \tag{4.20}\]
These costs are calculated using a smooth saturation (which also normalizes and scales them, see Section 4.6.3) so that gradients don’t change suddenly as exceedances are approached.
I mentioned in the introduction that even when we develop optimization-based control, we are likely to want to retain some heuristics-based knobs for our systems. Recall also that \(I, \ \dot{I}_{\text{lim}}\) have been calculated using motor models 4.3.1 and then scaled by \(I, \ \dot{I}_{\text{prop}}\) - values encoded into the trajectory (which can vary by segment) to scale deployment of each actuator’s total available power — this is where those knobs (which are in \(0, 1.0\)) are implemented here. In Section 5.8.2, I discuss how these scalars are set as part of the 3D printing workflow.
The second component encodes our preference for fast trajectories, it is much simpler: we sum all \(\Delta t\) and add that value to the cost. This provides a comprehendible basis for trade-offs that are expressed by cost function component normalization: when no other errors are present, the total cost represents the time that the machine will take to traverse the spatial horizon. In normalizing other error terms, we can then reason about how much error in this regard would I permit in order to reduce the trajectory time by \(1s\).
4.6.1.4 Updating the Solution
The planner is structurally organized around the cost function, which returns the cost for the planner’s current \(v(s),\) given a target trajectory and initial states along that trajectory. The cost function is wrapped in JAX’s value_and_grad function, which returns the cost value (to plot and debug) and the gradient. The gradient is passed to Optax’s implementation of the adam solver [64], [88], which generates and applies updates for \(v(s)\). The solver iterations are all wrapped into one jit-compiled21 function, which is below (4.5).
optimizer = optax.adam(params_solver_rate)
cost_func_wrapped = jax.value_and_grad(cost_func)
@ jax.jit
def outer_loop(params, params_i, traj_bundle):
# params = v(s),
# params_i = v(s=0), the initial state from the prior solution
# traj_bundle: x(s), v_max(s), i_prop(s), didt_prop(s), ...
def step(carry, _):
_params, opt_state = carry
cost, grads = cost_func_wrapped(_params, params_i, traj_bundle)
updates, opt_state = optimizer.update(grads, opt_state)
_params = optax.apply_updates(_params, updates)
return (_params, opt_state), (cost, grads)
# jax.lax.scan causes the entire cycle of optimizer iterations
# to be compiled together, i.e. this is just a jaxlike loop
(params_f, opt_state_f), (costs_f, grads_f) = jax.lax.scan(
step,
(params, optimizer.init(params)),
xs=None,
length=params_solver_steps
)
return params_f, costs_f, grads_f There are two important parameters here, params_solver_rate (which correlates to the size of the gradient descent step, but is further tuned by the adam solver) and params_solver_steps (the number of iterations to run). I found that tuning these across different systems was required, see Section 4.8.3.
4.6.1.5 From Space to Time
In the final step, we need to deliver part of the solution to hardware, and roll the trajectory horizon into the future; the hardware operates on a fixed time interval, whereas the planner’s internal states are all defined in fixed spatial steps. I.e. we need for each value \(x(s)\) to generate an \(x(t).\)
This is done with a set of interpolations. First we calculate the time at each point \(t(s)\) using a cumulative sum of our \(\Delta t(s),\) and then make a new time series \(t:\).
\[ t = 0 : \Delta t_{\text{spline}} : t_{\text{cycle}} \tag{4.21}\]
Where \(t_{\text{cycle}}\) (also appears in Equation 4.12) is the interval within which we generate new planner solutions, and \(\Delta t_{\text{spline}}\) is the interval between basis spline points consumed by the MAXL system within which the planner is wrapped (see the MAXL Section 2.5, splines in particular 2.5.3 and 4.6.2 for the integration).
We can then find each parameter for \(s\) that corresponds to each \(t\) in the new series, and interpolate the trajectory positions \(\vec{x}(s) \rightarrow \vec{x}(t)\) alongside all the other intermediate states that are selected to exit the planner.
Finally, we “roll” the trajectory along: segments that are within the transmitted time window are removed from the head of the trajectory, new points are added at the tail, and the planner begins work on the next cycle’s solution.
4.6.2 Pipelining the Planner
OK. As I mentioned earlier, the planner is quite compute intensive and we need to rely on parallel computing in order to operate it fast enough that new chunks of the solution emerge in time to keep up with the real machine. The planner itself runs on the GPU via JAX, but the script that interfaces with it blocks during cycles when the GPU is producing a new solution. To parallelize that as well, it is connected to a MAXL / PIPES system through an interface class (that lives in the system’s OSAP runtime) via an OS-level socket in Python.
In Figure 4.11, I show a PIPES system for the CNC machine system from 6 in a configuration where the planner is used to plan velocities.
The interface class also saves stores planner outputs directly for later evaluation. These are time synchronized with the rest of the system, which lets us align its internal states and outputs with data collected from sensors; this becomes important in Section 5.11.1 (and is used to analyze results from this chapter).
A note is that that while this planner runs in realtime, it does not receive realtime feedback from hardware. It produces new chunks of solution at \(10\text{Hz}\) only. The planner is a feed-forward trajectory optimization step, not what controls engineers call a Model Predictive Controller — although it does use models to predict optimal control outputs. The key difference is that MPCs update their initial states with sensor measurements, this planner sends position targets to actuators and relies on those actuators’ lower level PID-based position controllers (see notes on hierarchical control in 4.2.3) to track the targets. I can be caught calling it a feedback based controller because the models that it uses to pick optimal targets are based on feedback from the system itself, so — feedback but in much larger loop times.
I discuss the relative value of models for control (at various levels of the control stack) in some more detail in Section 7.2, and the perceived value of direct closed loop control in 7.5.
4.6.3 Soft Functions in the Planner
Ensuring that all the planner’s computations are smooth / soft is important because it has to compute a gradient of the whole cost function from \(v(s)\) all the way to the final output of the cost function. There are a lot of steps in between, and if gradients are sharp or vanishing in any of these steps the planner will likely fail, or will optimize poorly. This is not uncommon practice in machine learning and other optimization fields, but it was an important trick that I learned while writing the planner, so I include two of these functions here for the curious reader (I also showed the use of \(\tanh\) for smoothness in Section 4.3.3.2 and 4.3.2.2) - leaving out this detail would result in a planner that doesn’t optimize. I found it useful to define the size of the smoothing boundaries as a blend_width to make the configuration of the amount of smoothness more sensible — see Section 4.8.3 for some more notes on configuring these widths (a tuning step!) and on normalization of errors.
First is the soft_clamp that I use to assert that time steps are in a fixed range:
x within the lo, hi range.
def soft_clamp(x, lo=0.01, hi=0.1, blend_width=0.005):
"""
Smoothly clamp x to [lo, hi]. Linear in the interior,
soft-saturating within `blend_width` of each boundary.
"""
k = 6.0 / blend_width
x = lo + jax.nn.softplus((x - lo) * k) / k
x = hi - jax.nn.softplus((hi - x) * k) / k
return x
soft_clamp from Listing 4.6I use soft_err_over_under (below) to calculate exceedances of constraints in a manner where the error begins to increase slowly as the constraint is approached, rather than suddenly. I also include a normalizing scalar in this step.
def soft_err_over_under(val, min_val, max_val, blend_width=1.0, norm=1.0):
"""
Error for value over/under exceedances of min/max,
blend_width = size of softplus blend zone
norm = expected max_val to norm errors such that 2x max = 1.0
"""
hardness = 5.0 / blend_width
scalar = 1.0 / norm
umin = - (- min_val + val)
omax = - max_val + val
umin_soft = (jax.nn.softplus(umin * hardness) / hardness) * scalar
omax_soft = (jax.nn.softplus(omax * hardness) / hardness) * scalar
return umin_soft, omax_soft 
soft_err_over_under from Listing 4.74.7 Results
Here I share key results that are relevant to motion control on its own, but the methods that I develop here are extended in later chapters and form a basis for subsequent results.
4.7.1 Trapezoids vs. Model-Based Control
The planner allows us to use more of our motion systems’ dynamic range than trapezoidal planners because they use models that describe motion limits in the full range of acceleration and velocity states. Trapezoidal planners used fixed values: a maximum acceleration and a maximum velocity (per axis of cartesian motion).
In Section 6.2.1, I show that we can use models to select these parameters for trapezoidal planners. An example of one such selection is below. The shaded area marks the space where the trapezoidal planner can operate. However, the real boundary (marked by \(550N,\) a force margin) encloses a larger area, especially in the left-hand side of the plot.
The system modelled in these images has a large damping offset. When we are accelerating against the current velocity, that damping can actually work in our favor; this dynamic is not exploited by trapezoidal planners. We can see this clearly if we look at motion states traversed during a test part by a trapezoid planner using these parameters.
The boundary is still violated somewhat by the trapezoidal planner. This is an artefact from the junction deviation step, which causes the machine to execute accelerations that are actually somewhat larger than the specified maximum. See 4.2.4 for more detail.
If we run the same target trajectory with the planner, we can see that more states are used — again, especially in the left-hand side of the plot. There is some lossiness here, too — near the origin we can see a cloud of points that exceed the specified force margin. I believe that this is an artefact of the planner’s own lossy cornering step (4.6.1.1). I discuss how a more direct use of splines in the planner could reduce these errors in Section 4.8.5.3. However, this shows that model-based motion planners have potential to greatly improve machine performance.
The difference between the planner and trapezoid planners is evident also when we look at the velocity-time plots that they generate, a comparison of which is below.
We can see that the planner exploits the system’s damping dynamics: the front half of the speed curve (where it is working against friction) is much shallower than the tail, where it is working with friction. These plots are taken over the same trajectory segment: the planner also traverses it in less time \((0.245s \ vs \ 0.345s)\).
We can also see that the planner uses more acceleration more regularly, and that acceleration is variable with respect to the system speed: a result of damping and motor dynamics together.
In CNC machining, maintaining feed rates such that they match user’s cut parameters is of critical importance — when machine speeds decrease, chip sizes changes as well. When chips become too small, the cutting tool begins to rub instead of cut, which can cause chip welding. I discuss this in Section 6.1.1.
Because I developed a method to pick parameters for a trapezoidal planner using motion models (in Section 6.4.2), I was able to compare the model-based planner’s performance against a trapezoidal planner where we know that the trapezoidal planners’ motion parameters are not simply under-tuned. For this comparison, I use the trapezoid planner that I developed for MAXL 2.5.4.4. MAXL / PIPES also lets us swap between the two planners without making any other changes to the system.
We can see that velocity deviations are greatly reduced when the planner is used. This is observable also in histograms that show the distribution of deviations over the course of the entire trajectory.
4.7.2 Model-Fit Quality
In Section 5.10.1, results from each part include an error metric on motion model fits. This metric measures the average deviation of the planner’s motor current estimates from measured motor states. Those are calculated from time-series like those shown below, which renders a twelve-second slice of motion data from one of the prints. In it, we can see predicted motor currents (solid lines) vs. measured currents (dotted lines with some transparency).
On average, the printer models deviated from measurements by \(0.175 A,\) which is considerable. These errors emerge partially due to noise emitted by the motor’s PID controllers, which are tuned to be quite stiff. However, it is clear that the model fit is not excellent. The fits are initially performed on chirp data (per 4.5.2), but real-world machine control spans more states than those generated in the chirp test.
I also modified the printer between the chirp tests and operation, tightening its belts. This increases damping, which I forgot to account for by running new tests. However, noticing this leads us well into the next section here.
4.7.3 Continuous Improvement of Models
Because our hardware is also our testing equipment, we can re-fit motion models on data that we collect during regular operation, e.g., the 3D plot in Figure 4.18 shows current estimate errors in 3D from the FFF printer. In Section 4.5 I explained that friction values can be fit against any data where we know the systems’ acceleration, velocity and actuator current measurements. These are all available in the datasets that these machines generate, and the models used in the planner to predict actuator currents are the same that we fit against. Because these are not time-varying physics (at this order), we can apply exactly the same process as described in Section 4.5.2 with new data to update models.
| Parameter | Units | Value Before Refit | Value After Refit |
|---|---|---|---|
| X Mass | \(kg\) | \(0.523\) | n/a |
| y Mass | \(kg\) | \(0.880\) | n/a |
| X Damping Offset | \(N\) | \(1.147\) | \(0.647\) |
| Y Damping Offset | \(N\) | \(0.053\) | \(0.079\) |
| X Damping | \(N/m/s\) | \(0.398\) | \(0.199\) |
| Y Damping | \(N/m/s\) | \(0.117\) | \(0.073\) |
| A Damping Offset | \(Nm\) | \(0.0144\) | \(0.0206\) |
| B Damping Offset | \(Nm\) | \(0.0143\) | \(0.0213\) |
| A Damping | \(Nm/rads/s\) | \(0.000370\) | \(0.000340\) |
| B Damping | \(Nm/rads/s\) | \(0.000370\) | \(0.000360\) |
In Figure 4.19, I show motion states from a slice of time-series data generated on the FrankenPrusa (Section 5.4.2): solid lines are values collected from the model-based velocity planner in this section, dashed lines show current measurements sourced from the machine’s motors as it followed this trajectory, and traces in opposing color show current estimates that would be made on the same data with updated friction parameters based on the new data. The update does not entirely capture the data, but does move estimates closer to measurements. This is perhaps because the motor’s internal controllers add noise that the models don’t capture. Here we are also only updating friction parameters, whereas other dynamics may have changed slightly or e.g. rotor inertia estimates may be off. In Section 8.2 I discuss a key limit to this work, which is that we cannot fit through multiple models, for example learning updated motor \(k_t\) values or estimating changes to machine mass or kinematic parameters using end-to-end datasets.
We also have the problem that real-world data tends to over represent certain states: machines spend most of their time at target feedrates rather than in highly dynamical states. To alleviate this to some degree, I weight data according to velocities at each data point so that varying velocities are equally represented in the updated fit. Really the distribution of states has many more dimensions of this, so more advanced weighting methods should be applied. This is especially true for continuous updates to process models, which I discuss as a possibility in Section 5.12.2.
4.8 Discussion
This chapter answered one of the primary questions in the thesis, which was to learn how we can describe key machine control steps as using explicitly defined and numerically solved constrained optimization. To do so I developed a new type of velocity planner by combining relatively simple models for motion and actuators with a generic but powerful type of optimization framework. In contrast to state-of-the-art velocity planners it optimizes against models directly rather than being configured with simpler parameters for maximum jerk, acceleration, and velocities. This connects machine physics more concretely to machine control and allows us to e.g. update controllers by updating the models that represent the machine.
Many of the key results in later chapters are made possible by the work here, but I did show concretely that this planner out-performs an equivalent trapezoid planner; it can do so because it can leverage more of our machine’s underlying dynamic range as expressed by motion models. Whereas I initially developed these models and model fitting methods for the purpose of integrating them with the velocity planner, I was surprised to find that their value in other aspects of machine design and control is perhaps more immediately applicable. For example motor and motion models are also an excellent machine design tool, and can be used alongside simpler velocity planners to more precisely pick their motion parameters (Section 6.4.2). They can also be combined with motor readings to make measurements like cutting force measurements (Section 6.4.4).
I was also surprised to find the limits of these models: I did not include machine stiffness here but vibrations that emerge when velocity planners excite machine structures are a key constraint to overall machine velocity. In stiff machines this is less of an issue but for the 3D printers that I develop in Chapter 5 it is a primary limit. I discuss those in Section 4.8.5 below and also how the application of heuristics on top of models was a critical step to ameliorate those issues. I also discuss here some more results that compare trapezoid-based solving to the planner that I developed here, some thoughts on the planner’s formulation, and on motor models and machine component heterogeneity.
4.8.1 Machining with Trapezoidal vs. Model-Based Velocity Planning
I discussed a one-to-one comparison of a trapezoid planner to the optimization-based planner in Section 4.7.1. I was able to actually run these jobs on hardware. In these comparison runs, I saw two things of note.
First, it becomes clear that this planner is faster overall after the first few cuts. This is audible as well, there is a subtle difference to the sound that the machine makes as it enters a cut: it is more consistent (perhaps indicating uniform process parameters through the cut), whereas there is an audible ramp in tone in the trapezoid planner.
The premise is that maintaining chip load reduces the likelihood of chip welding, and (while this doesn’t constitute a rigorous evaluation), I could not get the trapezoidal planner to finish the cut without welding the endmill.
This shows how variable machining can be: the toolpath is the same as I had run a number of times previously, when I ran the experiments in Section 6.4.3. I tried the trapezoidal controller a total of three times, each with a fresh endmill. The updated controller run worked on the first attempt, and I tested it between one failed trapezoidal path and another. My guess is that in this case there was less time between jobs. In my prior runs of this part, I took a few minutes between each cut to adjust parameters in the toolpath. This time I was simply running paths one after another. The stock was warm to the touch during these experiments — perhaps the chip welding was more likely because of this increased temperature, however slight. This would also indicate that the toolpath parameters that I found in 6.4.3 were quite close to the limits.
4.8.2 Solving in Phase Space
Phase Space is a term I first saw while reading one of Ben Katz’ blog posts on trajectory optimization [89], [90]. It sounds like something to do with the frequency domain but in generalized controls terminology a phase space is just “the set of all possible physical states in the system” [91], so this makes sense for our planner: for any position, which velocity? I want to explain why I think this is an important part of the formulation of the planner, and what it taught me about planners like it, because I think it’s an important component of why the planner works well, and it was somewhat counterintuitive to me at first.
The background section on MPC (4.2.7) shows the canonical state space representation in Equation 4.2. That is common in robotics because it generically describes how systems evolve in time (dynamics!) and cleanly separates a system’s own evolution (the \(A\) matrix, subject to \(x(t)\) system states) from changes that are driven by our controllers (in the \(B\) matrix, subject to \(u(t)\) control inputs). This is perhaps the best way to intuit dynamical systems as well: if we are moving at some velocity (a state, \(x\)), and then apply some torque (a control knob, in \(u\)), we will have a new acceleration (goes in \(B\)), and also an acceleration due to damping on the existing velocity (goes in \(A\)). To integrate, we sum those changes to acceleration, update our new velocity, and we can get our next state.
In phase space position is the indexing variable, so we have variable time steps between each state, which can be a bit awkward for numerical integrators. However, it has advantage that the shape of the cost function doesn’t change when we update our solutions. For example, the planner’s main task is turning corners quickly: it needs to maximize speeds in between corners, but decelerate soon enough before the corner that it doesn’t break things. If we increase velocities in the lead-up to the corner, the corner’s position in time changes (i.e., it shows up faster), but its position in space stays the same. Thinking about the planner’s perspective, this means that it needs to decide where to begin slowing down for the corner (an optimal point that doesn’t move as its velocity selections change), rather than when to slow down (which does change).
If we formulated the planner such that the decision variables for the output trajectory were indexed in time, the shape of the problem that it is solving would change constantly as it updates its solution to that problem. The driving analogy works well here: imagine heading towards a corner going very quickly and then, as you apply some braking, the corner is suddenly much farther away. This would make the braking point a very difficult optimum to find, and is effectively what happens when we use time indexing.
MPCs that use the direct shooting formulation are set up in this way (fixed time steps), and can work because their solvers are mathematically more precise than the one that I used here: they use more derivatives of the system (and tighter definitions for which problems are solvable and which are not) to see through time, i.e. to connect how decision variables in the past affect outcomes in the future. Since a design goal in this step was to be able to more easily articulate problems (eschewing this mathematical precision in favor of simpler and more flexible systems descriptions), formulating the problem in a manner where the position (in the system representation) of the relevant decision variables doesn’t change over iterations was important.
The second reason that the phase space formulation works well in this planner is that it is what allows us to parallelize the planner; this is the same insight as in the direct transcription MPC formulation. Direct shooting formulations need to compute derivatives of their system through time through very many steps of the chain rule, that’s a similar idea as the one above: they need to mathematically describe how control inputs in the beginning of the trajectory will affect changes in the cost function near the end of the trajectory. They also need to compute their simulations serially: take initial states, and for-each selected \(u(t)\) iterate the solution one step in time, updating the next time step’s states, and so on. Trapezoidal planners have the same serial compute requirement in their forwards-and-backwards planning step.
While this works well for motion, it introduces some challenges for other dynamic systems like the 3D printing extruder as I discuss in Section 5.11.7.4.
4.8.3 About Performance and Hyperparameters
In the introduction to this chapter (Section 4.1.2.1) I mentioned that optimization-based planning runs the risk of trading one set of parameters for another set in the planner’s configuration. This turned out to be the case, as the planner has a few hyperparameters that sometimes need to be tuned in order to produce consistent results. There are three especially sensitive hyperparameters: the length of the trajectory that it solves over, the size of each segment that it will pick a velocity for, and its update rate (which defines how large of a step it will take in gradient descent). Together these set the size of the problem and the sensitivity that the planner will have to the system’s convexity. For example a long trajectory with very fine spacing requires more computation to plan for, and a high learning rate can cause the planner to become unstable when machine models are sensitive to small changes.
For motion control alone, setting these parameters up was never an issue because motion and actuator models on their own are quite simple and are highly convex. However, once I added the 3D printer’s flow system (see Section 5.9), the planner both required more compute cycles to generate optimal solutions and also more sensitive to subtle changes in the flow system — especially the time-dependence that is present there. As I added new physics to the planner in that work, t was common that I had to re-tune planner parameters (optimization rate, steps, size of the problem) to get it solving properly, and in time. For example with simpler systems I ran the planner for only 50 iterations per cycle at a relatively large update rate. Once the flow system was added in its entirety, I was running the planner for about 400 iterations per cycle at an update rate that was \(1/8^{th}\) the size.
Getting the planner to work well also required that components of the cost function were well normalized. This is common practice in machine learning and optimization. I will not expand on it at length, but normalizing each new set of cost function components was another sensitive step that indicates that the planner does not come without its own set of internal configurations. However, I found that the best way to articulate these normalizations was to connect them to real physical values; constraint exceedances in the cost function are articulated in a way where they are traded against time (in seconds). This way we can say e.g. “for every amp of current used in excess of defined limits, a solution must be at least fifty seconds faster than it would have been if those limits were not exceeded.” This connects the normalization back to the physical models that we are referencing.
4.8.4 Notes on Motor Models and Motor Heterogeneity
I wanted to briefly show that even two motors of the same make and model can have varying underlying parameters; no two parts are truly the same. For example Table 4.6 shows model-fit parameters for two individual motors that are both from the same manufacturer, same part number, etc. I found a \(7\%\) difference in their \(k_t\) values, which is the key parameter that defines how much torque each produces for each Amp of current.
This is likely due to the fact that how current couples into a motor’s magnetic system is a strong function of the quality of its windings, and winding stator coils is a somewhat imprecise process. This has real results for machine builders as well, who must normally select one set of motion control parameters for all the machines they will ship even though each has various underlying differences that are subtle but obviously present; model-based control deployed where models are fit against data generated on the specific hardware that will be controlled has clear benefits in light of these considerations.
Sourcing models from hardware also provides the benefit that it better informs machine designers choices as they trade off on other fronts e.g. between torque and speed. Controllers also factor in the tradeoff; low-end torque limits are as much a function of motor physics as they are of driver limits and high-end speed limits are a strong function of the voltage that we can supply to those drivers (and the speed of the controller’s commutation). So again we can see why developing methods that fit models through each of these components could be beneficial.
kt values, as shown in Figure 4.23 and Table 4.6
| Parameter for StepperOnline 23HS22-2804D Motor | dut_c Individual Motor |
rig_y Individual Motor |
% Difference |
|---|---|---|---|
| \(k_t\) | \(0.3234\) | \(0.3407\) | 7.3 |
| \(k_e\) | \(0.3312\) | \(0.3456\) | 4.3 |
| \(k_d\) | \(0.0009\) | \(0.0011\) | 22.2 |
dut_c (left) and rig_y (right), are the same manufacturer model, but have different model parameters, indicating that their internal construction is slightly different.
4.8.5 Key Limits
4.8.5.1 Stiffness and Vibration is Key
At the start of this project I had assumed actuators, friction and inertial physics would explain most of the limits to overall motion performance. In the past when I had designed machines, mostly I selected actuators based on this assumption and calculated machine stiffness for the purpose of modelling deflections, not vibrations. The coupling between vibration and motion control was not obvious to me, perhaps because the interaction between velocity planners, machine structures, and actuators and transmissions is much more difficult to model. Preliminary machine design is canonically a spreadsheet-based exercise [92] although obviously the practice is changing given new and more powerful modelling tools. At the design stage however it is difficult to tally all the aspects that will contribute to a machine’s frequency response.
In this work I found that vibration and stiffness is a primary concern for velocity planning. In hindsight this is clear to other researchers, especially those who articulate this step entirely in terms of filter-based methods (see Section 4.2.5). Actuators, friction, and inertias are of course still critical because those define the basis for control — vibration is a response to their inputs.
4.8.5.2 Heuristics In Lieu of Models
I developed the application of additional heuristics for motion control (see Section 4.6.1) largely in response to this shortcoming; when the planner is allowed to use all the power available in e.g. the FrankenPrusa’s actuators it’s frame deflects tens of millimeters. To calm the planner down we can provide \(I_{\text{lim}}, \dot{I}_{\text{lim}}\) values that scale the system’s underlying limits in the cost function. Applying these heuristics makes a significant improvement but doesn’t address the root of the problem. The application of these heuristics also make it difficult to tell what really limited machine performance in the plots that I produce in Section 5.11.1.
This points to another limit in the methods that I developed, which is that the planner does not model deviations from the toolpath that might arise due to these vibrations or due to steady-state structural loads. The application of any force to a machine will cause some deflection, and so the improvement over these methods would be articulate the speed-vs-precision trade-off against permissible geometric tolerances. I discuss this also in the 3D printing chapter, specifically in the section on Pareto frontier printing, 5.12.4.
That said, any attempt to model the entirety of a machine’s physics will eventually come up short. There are also moments when we are bootstrapping models where the application of simple heuristics is just easier, as I discuss in Section 7.2.4.
4.8.5.3 Lossy Abstractions in the Planner
In several places in this thesis I have articulated how other machine controllers are rife with lossy abstractions where the models used to operate certain components of machine workflows poorly capture important behaviors (or fail to reveal them to higher level algorithms). However, I introduce three of these abstractions in the planner’s formulation and in the way that the planner connects to hardware.
The first is in how cornering velocities are represented in the planner, which is described in Section 4.6.1.1. This step is central because it defines how machines move through curves and additionally calculates the time step for each spatial step; that is critical especially then the planner is connected to process physics later on. The loss emerges from the fact that the planner represents cornering using line segments, but the path traced by the machine is actually a curve. As I mentioned in Sections 4.2.4 and 4.2.5, any velocity carried through a sharp corner will in reality produce an infinite impulse in acceleration.
The second has to do with basis splines. The planner works by selecting velocities for each junction between a series of very small line segments, a spatial representation of the problem. But once solutions are generated, they are transmitted to motor controllers using basis splines. Although the lossiness of this handoff is minimal (see Section 7.4.1), it really seems as though we aught to be able to solve the spline outputs directly, rather than solving the trajectory in one representation and then transforming it into another. Because the splines are also a kind of filter, they would be an excellent basis for adding a geometric tolerance limit to the planner. At risk of stating the obvious, splines are also actually curves, not line segments that introduce the issues I just mentioned concerning cornering velocities, so using them directly in the planner could ameliorate that loss as well. Finally, they have other nice properties that emerge at a systems assembly level, which I will discuss at more length in Section 7.4. All of this said, representing the trajectory in phase space (with fixed spatial steps) was also extremely productive. In Section 4.9.1 I briefly discuss how we might resolve these conflicts.
The last concerns the way that the planner is connected to hardware through MAXL and PIPES. In those dataflow graphs, the machine’s kinematic arrangement is essentially mirrored again in the graph configuration. The planner emits points in the machine’s tool path space (i.e. XYZ axes) and those are then transformed again in the graph on their way to motors and other hardware. This is done even though the planner has its own internal representation of those kinematics, so misconfigurations can arise when the two representations do not match. Luckily both the planner and the MAXL graph are defined in software, so building a configuration step that aligns their representations would not be difficult whereas doing so across the GCode boundary is nigh impossible because there are no standard GCodes that let us query interpreters internal configurations.
4.8.5.4 Nonlinear Kinematics, End-to-End Fitting, and Models of Controllers
There are a few other clear limits to this system. I did not show how it might control systems with nonlinear kinematics even though doing so would have clear value because they are difficult to manage in other velocity planners. I also found that the bandwidths of lower level controllers is an important consideration in the velocity planning step, but did not include representations of those in the planner itself; I discuss these in the future work section here, 4.9.
4.9 Future Work
4.9.1 A Better Representation for \(\Delta t\) and \(\vec{a}_{\text{req}}\)
In Section 4.8.5.3 I explained how the planner’s representation for cornering velocities is somewhat lossy and started to connect that issue to other abstractions in the planner. This largely revolves around the use of basis splines; Section 7.4 connects those also to systems integration more broadly. An overarching goal of the thesis is to use similar systems representations throughout control layers, so the particular representation that I use for cornering velocities has stuck out to me as a failing in this regard. But it also works, mainly because it frames the planner’s work in phase space, as I discussed in Section 4.8.2.
So, a key component of the future work in developing this type of planner should revolve around resolving this tension. It seems likely then that an improved planner would use splines in two steps: first with fixed spatial control points over which velocities could be planned, and then with a time scaling applied according to those velocities to transmit plans to actuators.
4.9.2 Exploiting Time-Varying Motor Dynamics
In Section 5.11.5, I show that the planner developed here can be extended to solve over time-varying dynamics. In that example the planner exploits thermal history in a 3D printer’s melt flow system to briefly extrude at speeds beyond what steady-state limits alone would allow. There are similar dynamics at play in motion systems; actuator current is instantaneously limited by driver capability, but the motor’s own current limit is thermodynamic: if we heat the stator’s coils past the melting point of their insulating enamel, they short against one another and the motor fails.22 It takes time for coils to heat up, so if we could model these thermodynamics we may be able to use much larger drive currents and torques than we would otherwise at particular instants, e.g., where our machines are turning tight corners. In fact motor drivers have these dynamics as well — MOSFETs have instantaneous current carrying capabilities that are much larger than their steady-state limits, which are also based on their maximum temperatures.
4.9.3 The Missing Models for Motion
Section 4.8.5 discusses the most important aspects of motion control that are missing from the current formulation here, but I want to expand on how I think we should develop these components in the future.
The most promising aspect of the planner is that it allows us to describe machine physics explicitly and then do control declaratively rather than via intermediate parameters and approximations of those physics. In the frequency domain this should be no different. State-of-the-art methods for vibration repression in machine control are formulated using filters that are designed to suppress excitations are particular frequencies (see Section 4.2.5). While this is often done using machine-generated data that plots which frequencies are most valuable to suppress, this amounts to another parameter-based configuration step. The underlying issue is that vibrations cause machines to deviate from target paths, so we should be able to declare geometric constraints to the planner and then provide it with models that describe how those deviations emerge. For example frequency response is phenomenological not physical, the underlying terms are stiffness and damping terms. This means that frequency response tests could be used not to design filters but to fit these values across a range of machine states, and those could be used to “show” the planner how much a machine will deflect according to candidate velocities that it selects.
Frequency response is history dependent; machines oscillate because energy that has been loaded into e.g. belts subsequently unloads at a later time (the loading is stiffness related and the unloading is damping related). I show in Section 5.9 that the planner can manage history-dependent physics, but spring oscillations have much higher bandwidths than the thermal systems that I integrate there. Because the planner is discretized, there will be a Nyquist limit under which solving vibrational dynamics in this way is prohibitively expensive, but it is worth experimenting with for less rigid machines; belt driven hardware is definitely the most relevant here but even some larger gantry-based milling machines may have oscillations that can be managed in this way.
Modelling vibration in this way may also make it easier to couple those physics into processes. For example high performance machining using belt-driven systems is an absurd proposition because the interaction between chatter and belts is especially chaotic, but perhaps this would become possible if cutting force loads were tightly integrated with models for belt stiffness?
4.9.3.1 Nonlinear Kinematics
I also mentioned nonlinear kinematics, which cause inertial dynamics to change alongside not just machine velocity but also machine position. Describing these physics is difficult in the state-of-the-art but machine designers often experiment with novel kinematics to improve performance. To simplify how these kinematics are articulated to planner, in Section 8.1 I propose developing a specification for a machine’s 3D digital twin that could be derived from CAD models. I think that the planner could be extended to use this type of representation for varying inertial dynamics as well; it already tracks internal velocity and acceleration states for both motors and axes even where they are not directly coupled. Adding intermediate rigid bodies would require that kinematic functions like those in Section 4.3.2 also return positions and orientations for each of those bodies so that the forces required to move them around could be also reflected into actuator models.
4.9.3.2 Controller Models and Computational PIDs
I found that other controllers in the system should be modelled in the velocity planner because they have their own bandwidths. Motor physics alone do not describe how a motor will perform; to understand the whole picture we need also to understand things that normal hierarchical control considers external: encoder noise, phase lag from Kalman filters or other observers, and sample-and-hold delays introduced by discrete controllers.
Again, a benefit to the formulation here is that we can use the same computational representations of these systems across layers. It is already common for controls engineers to model these aspects of their hardware when they use optimization-based methods to write low level control gains. Here, modelling these finer grained details could have the extended benefits of connecting models of controller implementations to the planners that drive them.
Interdependence across layers is a theme in this thesis; try as we may to separate concerns, there are deep connections across layers that become hard to ignore at the edges of performance. In Section 8.2 I discuss a question that this all raises, which is to see whether we can fit hierarchical models end-to-end.
Finally, the implied goal of Section 4.4 is that our controller should be deployable, somewhat automatically, on many unique stepper motors. Measurements of motor \(R, L\) and pole pair counts are possible to do using the controller itself, rather than (in the case of \(L\)), the somewhat expensive LCR probe that I use. In this case the measurements are simpler and can be done without decoupling the motor from the machine. More difficult is estimating also the rotor inertia, as error in its estimation can easily slip into the motor’s \(k_t\) value (less \(k_t\) has a similar effect on acceleration as having larger \(J\) does); although it should not slip into the value for \(k_e\) and the two values are the same according to the motor model, so the difference can perhaps be distinguished form data. Overall on topics like this, I think that extending the modelling techniques used to quantify uncertainty about parameter fits is a clear next step for the work.
References
PID, for Proportional-Integral-Derivative controller, is a pattern that I will assume readers of this thesis have seen, built, or tuned. If not, never fear: there are excellent explainers about, my favourite of which is here: [1]. They are simple (ish) and surprisingly effective feedback controllers that are found all over the place - in simple and in complex systems, slow and fast, etc. You will also see i.e. the noun PIDs in referring to the controller’s gains.↩︎
Well, there is the third component to this problem, which is the systems-level implementation of these controllers that I already covered in Section 2.5.↩︎
Planners can alternately be configured to look ahead through some amount of space, which under acceleration and velocity limits can be pretty well correlated to time. For example automobile braking systems are defined by their stopping distance rather than stopping time. Some GCode interpreters though look ahead through a fixed number of linear segments, which is not necessarily correlated to either as I briefly touched on near the end of Section 2.2.1.↩︎
Sophisticated trapezoidal planners also limit jerk, acceleration’s derivative.↩︎
Even though it is difficult to ascertain really how these firms’ algorithms work because their secrets are kept quite close to the chest.↩︎
There are better technical descriptions of this, but any time you can wiggle a machine axis without changing the position of the actuator, you have backlash.↩︎
In 3D printing these produce what are called ringing artefacts on part surfaces, and in CNC machining they are related to chatter, some more on those later as well.↩︎
These are just what they sound like: the motor turns a screw, and the machine axis is affixed to a nut, rotating the screw translates rotary motion into linear. In the case of ball screws, the nut is in contact with the screw via recirculating ball bearings to reduce friction and increase precision. Many arrangements exist: the most common mounts the motor to the machine chassis and the nut to the moving axis, but we can also mount the screw rigidly and rotate the nut - actually this second arrangement is more common on longer drive spans where screw whip can become important, because it causes the screw to stay taught more often to prevent it from buckling.↩︎
Actually it is common to see frequency-dependent dips in a stepper motor’s torque curve; these are speeds when the pulse frequency resonates the rotor itself, which can lead to motors losing steps more often at those particular frequencies.↩︎
PI Controllers are Position, Integral, Derivative controllers (PIDs) without the derivative term. These are common for control of systems that don’t have any type of momentum.↩︎
A little bit off topic, but remember here the earlier discussion on how technical partitioning interacts with economic partitioning (Section 1.4). In this case, in order to sell motion control systems Delta Tau has to provide a kind of set of design rules [46] for systems integrators.↩︎
DOF: Degree of Freedom.↩︎
For a quick aside and motivation towards more unified control architectures, a common application of 6-DOF robot arms is in tending CNC machines: loading stock and unloading finished parts. It is common for the robot arm to physically press the CNC machine’s start cycle button because connecting their system logic together is much more cumbersome.↩︎
Infinite Fun Time is originally a reference to an excellent sci-fi series [67], where superintelligent AIs while away their time simulating universes in their imagination. Training RL policies is quite similar: numerous simulated environments are instantiated where policies are tested, generating rewards to improve those policies.↩︎
This style of control system is called a piggyback controller, which is pretty self explanatory. I first learned of these in the context of internal combustion Engine Control Units (ECUs) (thanks Dad !). Hot-rodders often want to unlock spare performance in their off-the-shelf automobile’s engine without spending much money. Replacing ECUs outright is possible but expensive and difficult to get right; the engineers at i.e. BMW definitely know a thing or two. Instead of a full refit, we can intercept some sensor signals (like an exhaust oxygen sensor, which signals to the ECU what the current fuel mixture is like), read them, and modify them before they go to the stock ECU, to coerce it into doing things (like adding more fuel to the mixture) that it might not be meant to do. This involves some reverse engineering of the computer that we would like to trick - and is certainly a hack - but i.e. here and in the case of GCode interpreters, is often the best solution.↩︎
Floating-point Operations: how much total math we need to do.↩︎
This saturation is just a system configuration that I set by hand per actuator based on my knowledge of that device’s amperage rating. In the fullness of time this could be defined in motor firmwares and read from their configurations on the fly (or saved alongside motor model definitions) to save a feed-forward configuration step.↩︎
Commutation is the process of rotating a motor stator’s magnetic field such that it is properly aligned to the rotor’s magnetic field.↩︎
I discuss this in more length in Section 5.11.1, where it relates also to process limits. But the gist is like this: on a guitar amp, we can “turn it up to 10,” this is straightforward: 10 is the maximum that the amp is capable of (11 would be cooler, but might break things). In trapezoidal planners, we write a maximum acceleration directly, e.g., setting \(1500mm/s^2\) for the X axis, \(1000mm/s^2\) for the Y axis etc. We then run the machine to check if this works well: if stepper motors skipped steps, the machine vibrated too much, etc. At least, this is the most common practice in open source machine design, we can also use models to find these parameters, as in Section 6.2.2, which is more common in professional engineering contexts. Nevertheless, expressing this tune as a scalar of the total available power is not a feature of any state-of-the-art motion controllers that I have surveyed or used.↩︎
The (position, velocity) plot is known as phase space for these types of problems, see Section 4.8.2 for some discussion on why this is a particularly useful approach for this type of optimization.↩︎
JIT: Just In Time compilation, where sections of a program are compiled just before they run (or in JAX’s case, after the first time they run).↩︎
Motors also have magnetic saturation limits that reduce magnetic field strength even when very large currents are applied.↩︎