Question regarding Virtual Tractor steering logic in the Reverse-Capable Motion Planning example

I am currently exploring the tractor-trailer reversing control logic used in the Navigation Toolbox example titled "Reverse-Capable Motion Planning for Tractor-Trailer Model Using plannerControlRRT." While analyzing the helper files, I came across the virtualToActualSteering function inside exampleHelperPurePursuitGetSteering.m. The code comments explicitly state that this function was generated by the Symbolic Math Toolbox version 8.6.

Here is the generated code block:

function alphaDes = virtualToActualSteering(beta, L1, L2, M)
%virtualToActualSteering Convert trailer's virtual steer command to truck steering angle
%
%    This function was generated by the Symbolic Math Toolbox version 8.6.
%    13-Aug-2020 15:29:19

    t2 = sign(beta);
    t3 = L2.^2;
    t4 = beta.*t2;
    t5 = tan(t4);
    t6 = t5.^2;
    t7 = t6+1.0;
    alphaDes = t2.*atan(-(L1.*M.*t5-sqrt(t7).*abs(t5).*sqrt(L1.^2.*t3))./(t3.*t7-M.^2));
end

I would like to deeply understand how this mathematical relationship is derived. Specifically, I want to know which base kinematic equations, differential constraints, and geometric relationships of the truck-trailer system were fed into the Symbolic Math Toolbox to yield this exact analytical solution. Could anyone share the underlying theoretical background, the differential equations involved, or the symbolic derivation steps used to achieve this exact formulation?

Thank you in advance for your help.

[Walter Roberson]

Experimenting, making a semi-reasonable assumption that the parameters are real-valued:

syms beta L1 L2 M real
result = virtualToActualSteering(beta, L1, L2, M)

result = 

result = subs(result, beta*sign(beta), abs(beta))

result = 

sympref('heavisideatorigin', 1);
result = rewrite(result, 'heaviside')

result = 

simplify(result, Steps=1000)

ans = 

function alphaDes = virtualToActualSteering(beta, L1, L2, M)
%virtualToActualSteering Convert trailer's virtual steer command to truck steering angle
%
%    This function was generated by the Symbolic Math Toolbox version 8.6.
%    13-Aug-2020 15:29:19

    t2 = sign(beta);
    t3 = L2.^2;
    t4 = beta.*t2;
    t5 = tan(t4);
    t6 = t5.^2;
    t7 = t6+1.0;
    alphaDes = t2.*atan(-(L1.*M.*t5-sqrt(t7).*abs(t5).*sqrt(L1.^2.*t3))./(t3.*t7-M.^2));
end

Not sure this is useful at all.

[Hakan Kosebas]

Is this more simplified version? I couldn't understand.

[Walter Roberson]

The version with is the simplified version. It has managed to convert the tan() calls into sin() and cos() calls. Unfortunately I personally do not see either version as being informative about what is really going on.

[Walter Roberson]

At first I was thinking that it is likely that L1 and L2 are positive, which would simplify the expressions. However, looking again, if they are assumed to be positive, it would be a waste in the original code to write sqrt(L1^2*L2^2), so now I am thinking that there is a decent chance that at least one of them might be negative.

[Hakan Kosebas]

I asked gemini and glm-2 and they says it is derived from;

which is the function computeEquilibriumInternalAngle.

but I can't get same results with solve to find . And I wonder who wrote the code, I really want to see original code that generate .

Answer 1

[Cameron Stabile]

Hi Hakan,

If memory serves, this helper was derived from the paper listed in the reference section of the corresponding example: [1] Holmer, Olov. “Motion Planning for a Reversing Full-Scale Truck and Trailer System”. M.S. thesis, Linköping University, 2016.

The specific helper is derived from the following diagram, which presents the form of an N-body truck-trailer system locked into a steady-state configuration. To maintain the angles between each pair of bodies as a velocity is applied along the longitudinal axis of a body, the perpendicular line extending from each axle needs to intersect at a single point. For a 3-body system, you end up with the following figure:

To simplify the diagram above, we'll ignore the last body and use the following simplified notations:

• alpha,e -> A
• beta2,e -> B
• M1 -> M
• L1 -> L1
• L2 -> L2

You ultimately want to solve for A, which means working backwards through your knowns:

(1) A = atan(L1 / R1)

(2) let R1' = R1 + (M/cos(B))*sin(B) = R1 + M*tan(B)

(3) also, R1' = R2/cos(B)

Set (2)/(3) equal and solve for R1:

(4) R1 = R2/cos(B) - M*tan(B)

(5) R2 = (L2 + M/cos(B)) / sin(B) * cos(B) = (L2*cos(B) + M)/sin(B)

Sub (5) into (4), and the result into (1):

R1 = (L2*cos(B) + M) / sin(B) / cos(B) - M*tan(B) = L2/sin(B) + M/(sin(B)*cos(B)) - M*tan(B)

(6) A = atan(L1 / (L2/sin(B) + M/(sin(B)*cos(B)) - M*tan(B)))

This can also be done using the pythagorean theorem rather than trig-math, which the Symbolic Toolbox may have opted to use for efficiency (hard to say), but hopefully this provides some intuition into the geometric relationship. Just for good measure we can validate our trig-derived version against the example helper:

Note that this function only serves to derive the steering angle (A) needed to keep a constant interior angle (B) that has been given to it. You can envision this as each axle continuing to point towards O as the rear axle of each body moves along the concentric circles defined by R1, R2, R3, etc...

This helper does not account for transient dynamics, drift, boundary constraints, etc - it serves only as a way to convert the desired interior angle (obtained via a pure-pursuit scheme which aims to guide the rear-axle of the second body along a reference-path) to the steering angle capable of maintaining a steady-state motion (i.e. moving each axle along those concentric circles).

I believe the reference paper might offer additional details related to the proof/requirements of steady-state motion for these kind of systems, but hopefully the above is sufficient.

Hope this helps,

Cameron

[Hakan Kosebas]

Hi Cameron,

Thank you a lot for the answer, this helps me a lot. Very clearly explained.

Do you know how could original code be that using symbolic toolbox too? I couldn't make it solve by any way.