Spline Derivative and Error Linearization

[PranjalBiswas]

Hi @alexliniger,

I was working out the math to go from Eq (13a) to (14a). In that while linearizing the errors it requires to calculate derivatives of the arc-length parametrized reference path w.r.t the arc-length. Also, the reference path is parametrized using splines that are piecewise polynomials. So. if linearizing at points away from the knots of splines it is possible to select a specific polynomial, although if we linearize at points very close to the knot derivative of which polynomial should be taken? Because it might happen that $\thata_{A}$ may lie in next polynomial of the spline while $\theta_{P}$ in the previous polynomial. Will this not result in wrong approximation of the error? Or am I understanding it incorrectly?

Kind Regards
Pranjal Biswas

[alexliniger]

Depends on how fast the path is changing and how ill-behaved it is. Here I assume that the path is changing slowly enough such that such effects can be neglected. However, if the original path would be so ill-behaved, first splines would be a bad approximation and even you would need to be very careful with the SQP approach and use a significantly more advanced approach than I do here.

Best,
Alex

[PranjalBiswas]

When you mention how fast the path is changing, do you mean the change of heading and the curvature w.r.t. the arc-length or how fast the vehicle moves, cause in your simulation I understand that the heading and curvature of paths change pretty fast while the RC car also moves at high-speeds? Also, how do you say that a specific path is changing slowly enough or quantify that? One more thing will be what does it actually mean for a path to be ill-behaved?

[PranjalBiswas]

1 more thing to add will be given the reference path satisfies conditions mentioned by you, how is it possible to include a error linearization jacobian, which is parametrized by the piecewise polynomial (pp) coefficients in an MPC formulation. Cause 1 prediction horizon can include multiple polynomials which will change the error jacobian for each pp. Ideally according to my knowledge jacobian in such a case shall only be parametrized by the states and the inputs.

[alexliniger]

1. I mean the change of heading and the curvature w.r.t. the arc-length. To be honest the concatenation of straights and curves as in my case is already not optimal as the curvature is not smooth anymore. However, this also shows that this is not so important for the method as it still works.

2. With ill-behaved I mainly mean the orientation is not continuous (however even in this case the method does work in practice). Ideally, the path should have a smooth heading and curvature, so if you can design the reference path this should be considered.

3. In summary, if the track is drivable by the car and you have a smooth curvature your path should be good. My case is not optimal, since, the track to be drivable but the heading is only continuous and the curvature jumps, but it still works.

4. The state contains the arc length and this is the linearization point for the error, thus for every time step the jacobians is clearly defined (with the exception of points with discontinuities heading or curvature).

I am not sure that I understood your last question correctly.

Best,
Alex

[PranjalBiswas]

Thanks for the clarification w.r.t. first three points.

With respect to the 4th point, if you see the image below, I try to linearize the non-linear error using Taylor series. While doing that you can see that we need to evaluate partial derivative of the error w.r.t states "x,y,\theta" (i.e. term 1 in the image) resulting in the Jacobians. Given the fact that non-linear errors (as in 11a and 11b) are dependent on the reference path, and since this reference path in turn is parametrized by arc-length (\theta), the Jacobians will have coefficients of the arc-length parametrized refrence path.

Considering the predictions horizon consists of only "pp1" segment of the spline, the coefficient of the reference path do not change and thus jacobians are not parametrized by the coefficients of reference path. Although if prediction horizon consists of multiple segments of the splines (lets say pp1, pp2 and pp3), the Jacobians now are parametrized by the coefficients of the reference path as well depending on at what point the errors are being linearized. (relating to which pp to choose depending upon the linearization point)

So, I was referring to such parametrization of the Jacobian and how does this fit withing the MPC formulation. If I understand correctly the jacobians shall not have such parametrization, else it will lead to something like piecewise jacobians or something like that.

I hope it is more clear now. Please let me know your views.

[alexliniger]

If you look at the code you will see that each step in the horizon uses a different linearization point (for the cost, dynamics and track constraints). Thus, each point in the horizon potentially uses a different spline segment, which should solve your concern.

[PranjalBiswas]

So if I understand correctly, the linearization of the errors is not done at each time step, but at each prediction step for a given time step? Thus at each prediction step we can choose the appropriate "pp" and then linearize the errors. Am I correct in that case?

[alexliniger]

yes, but important to notice is that this is not only true for the errors, but also the dynamics and the constraints. Compared to linear MPC, where all the matrices are the same, in this implementation all matrices are different, and you use the shifted previous solution (the whole horizon) as linearization points.

[PranjalBiswas]

Ahh, thanks a lot for the clarification, although in such a case I will have to check how the math will work out. But anyway thanks for taking out the time to reply to my queries. I will close this issue for now.

[alexliniger]

The good part is that the math doesn't change, at least on how the linearizations work. You just have 40 different linearization points