foresightful acceleration and decelerations

I am programming a CNC machine (using matlab). In order to generate a surface with a "high" shape accuracy (order of $$1\mu m$$) and "small" micro roughness (order of few nm) I need to make sure that no large acceleration/decelerations takes place, because this would induce vibrations. If the amplitude of these vibrations exceeds approx. $$50nm$$ they would generate unwanted structures on the machined surface.

Since most people are not familiar with the CNC context, I like to transfer the problem to a car, which is driving down a street in 2D. As input to the program I have a 2D point cloud $$(x,y)$$, which represent the ideal path. The goal is to move along this ideal path as quickly as possible. However, not only should the change in position $$(x, y)$$ be smooth, but also the change of the accelerations $$(a_x, a_y)$$ -- we have to avoid vibrations.

The main problem I am currently facing is the deceleration. Since I like to move along the path as fast as possible, I accelerate the car till I reach it's maximal velocity. However, my code does not work well, if I reach a sharp edge. In order to stay within the given path the car has to "look ahead" and decelerate way before reaching the edge. Hence, I need to implement a foresightful driver. Are there simple methods available for this task? How would you implement this?

Inputs: The input is a dense point-cloud $$(x,y)$$ with a step side of $$\Delta x = 1\mu m = \Delta y$$. This raster is only used to define the surface.

Outputs: In order to achieve the high accuracy I have to use an external reference, which define a fixed time interval $$\Delta t$$. The path is defined as point-cloud $$(x,y)$$ in such a way, that the machine (car) gets one point per time interval:

$$x_1 y_1$$ $$x_2 y_2$$ $$x_3 y_3$$

Hence, the velocity and the accelerations are defined by the point-cloud itself. Each point must be accurate to the $$1nm$$ level.

===

There are robotic communities, which did these kind of calculations. Here is one example: Link.

• Can you please give some background on the topics? Like how do you generate the surface, why large accelerations even occur, why vibrations are undesired, etc. Since I don't really understand what's going on, I can't really help; but did you take a look at splines?
– user114966
Jul 27 '20 at 22:50
• I don't understand the algorithmic task you are trying to solve. What is the input to the algorithm, and what do you want it to output? You tell us you're using a moving average, but it's not clear what you are using it for or how you are using it. What does it mean to "generate a surface"? Can you phrase that in terms a computer scientist could understand, without needing to have any prior knowledge about CNC machines?
– D.W.
Jul 28 '20 at 1:17
• @Dmitry: I changed my text. Hope that it becomes much clearer now, that I transferred my problem it into an everyday context. Jul 28 '20 at 18:46
• @D.W.: Thank you for the comment. Hope my new context helps to make the task understandable. Jul 28 '20 at 18:48

I think an approach that will work is to "work backwards" through the path, keeping track of the maximum velocity that can be used at that point (subject to the restrictions on acceleration), then "work forwards" along the path, assigning a velocity at each point (subject to the restrictions on acceleration).

Represent the path as a sequence of evenly spaced points $$P_1,\dots,P_n$$, i.e., $$P_1$$ is the start of the path, $$P_n$$ is the end of the path, and $$\|P_{i+1}-P_i\|_2$$ is the same for all $$i$$. At each point $$P_i$$, you know the direction $$D_i$$ the car will be heading: $$D_i$$ is a unit-length vector that is tangent to the path at that point, so we can approximate $$D_i = (P_{i+1}-P_{i-1})/\|P_{i+1}-P_{i-1}\|_2$$, using vector arithmetic. I'll describe an algorithm to compute speeds $$s_1,\dots,s_n$$ so that $$s_i$$ represents the speed of the car at point $$P_i$$, and so that the acceleration constraints are satisfied. Then the car's velocity vector at point $$V_i$$ will be $$V_i = s_i D_i$$.

What constraints do the acceleration constraints impose on the $$s_i$$? Well, I'll assume your goal is to impose an upper bound on the maximum acceleration allowed: in particular, I'll assume we have a constant threshold $$c$$ and we require that the acceleration vector $$A_i$$ satisfies $$\|A_i\|_2 \le c$$ at every point. Notice $$A_i = {V_{i+1} - V_i \over \|V_i\|_2/\|P_{i+1}-P_i\|},$$ so the constraint $$\|A_i\|_2 \le c$$ becomes

$$\|s_{i+1} D_{i+1} - s_i D_i\|_2 \le c s_i / \|P_{i+1}-P_i\|.$$

Given $$s_{i+1},D_i,D_{i+1}$$, we can obtain an upper bound on the maximum allowable $$s_i$$ that is consistent with this constraint, as the constraint is equivalent to

$$\|s_{i+1} D_{i+1} - s_i D_i\|_2^2 - c' s_i^2 \le 0,$$

where $$c' = c \|P_{i+1}-P_i\|$$ is a known constant. The left-hand side of that is a quadratic function in $$s_i$$, so you can efficiently compute an upper bound on $$s_i$$ by solving a quadratic equation. Moreover, given an upper bound on $$s_{i+1}$$, we can obtain an upper bound on $$s_i$$ by replacing $$s_{i+1}$$ in the equation above with the upper bound and solving for the upper bound on $$s_i$$.

So, our strategy during the backwards pass will be: at each point $$P_i$$, we will compute an upper bound $$u_i$$ on the maximum speed $$_i$$ that the car can be permitted to have at that point. In particular, we'll choose $$u_i$$ so we're assured that if the car has speed $$u_i$$ or less at $$P_i$$, then there is a way to complete the path so that from $$P_i \ldots P_n$$ the car satisfies all the constraints. We've already described how to do this: given $$u_{i+1}$$ (an upper bound on $$s_{i+1}$$), we described above how to compute $$u_i$$ (the upper bound on $$s_i$$). Thus, during the backwards pass, we will iterate through $$i:=n,n-1,\dots,1$$, setting at each step $$u_i$$ based on $$u_{i+1}$$. At the end of the backwards pass, we have computed all of the $$u_1,\dots,u_n$$.

Next, we will do the forwards pass, to assign a speed at each point. Iterate through the points in the order $$i:=1,2,\dots,n$$. At each step, we'll compute $$s_i$$ based on $$s_{i-1}$$ and $$u_i$$. In particular, the acceleration constraints impose a constraint on $$s_i$$: $$\|s_{i} D_{i} - s_{i-1} D_{i-1}\|_2^2 \le c' s_i$$. We also have the constraint $$s_i \le u_i$$. You can solve this to find the maximum possible value of $$s_i$$ (it involves solving a quadratic equation), and assigning $$s_i$$ this value. Continue forward along the path, until you have assigned speeds $$s_1,\dots,s_n$$ for the entire path.

The result will be a valid plan for how to traverse the path. In particular, at point $$P_i$$, the car will be travelling at speed $$s_i$$. While at point $$P_i$$, you will accelerate or decelerate the car so that by point $$P_{i+1}$$ it is travelling at speed $$s_{i+1}$$. This can be done by using the gas pedal or brake pedal to accelerate by $$A_i \cdot D_i$$, where $$A_i = s_{i+1} D_{i+1} - s_i D_i$$ (this will ensure acceleration in the $$D_i$$ direction of the acceleration vector $$A_i$$), using the steering to stay on the road (this will ensure acceleration in the direction perpendicular to $$D_i$$ to stay on the road).

This solution assigns the maximum possible speed at each point, and thereby traverses through the path in the fastest way possible, while respecting the limits on acceleration at every point along the path.