Calculate the displacement coordinates of a semi-articulated truck

Question

As shown in the image below, I'm creating a program that will make a 2D animation of a truck that is made up of two articulated parts.

The truck pulls the trailer.

The trailer moves according to the docking axis on the truck.

Then, when the truck turns, the trailer should gradually align itself with the new angle of the truck, as it does in real life.

I would like to know if there is any formula or algorithm that does this calculation in an easy way.

I've already seen inverse kinematics equations, but I think for just 2 parts it would not be so complex.

Can anybody help me?

Answer 1

Let A be the midpoint under the front axle, B be the midpoint under the middle axle, and C be the midpoint under the rear axle. For simplicity assume that the hitch is at point B . These are all functions of time t , for example A(t) = (a_x(t), a_y(t) .

The trick is this. B is moving directly towards A with the component of A 's velocity in that direction. Or in symbols, dB/dt = (dA/dt).(AB)/||AB|| And similarly, dC/dt = (dB/dt).(BC)/||BC|| where . is the dot product.

This turns into a non-linear first-order system in 6 variables. This can be solved with normal techniques, such as https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods .

UPDATE: Added code

Here is a Python implementation. You can replace it with https://rosettacode.org/wiki/Runge-Kutta_method for your favorite language and your favorite linear algebra library. Or even hand-roll that.

For my example I started with A at (1, 1) , B at (2, 1) and C at (2, 2) . Then pulled A to the origin in steps of size 0.01 . That can be altered to anything that you want.

#! /usr/bin/env python
import numpy

# Runga Kutta method.
def RK4(f):
    return lambda t, y, dt: (
            lambda dy1: (
            lambda dy2: (
            lambda dy3: (
            lambda dy4: (dy1 + 2*dy2 + 2*dy3 + dy4)/6
            )( dt * f( t + dt  , y + dy3   ) )
            )( dt * f( t + dt/2, y + dy2/2 ) )
            )( dt * f( t + dt/2, y + dy1/2 ) )
            )( dt * f( t       , y         ) )


# da is a function giving velocity of a at a time t.
# The other three are the positions of the three points.
def calculate_dy (da, A0, B0, C0):
    l_ab = float(numpy.linalg.norm(A0 - B0))
    l_bc = float(numpy.linalg.norm(B0 - C0))

    # t is time, y = [A, B, C]
    def update (t, y):
        (A, B, C) = y
        dA = da(t)

        ab_unit = (A - B) / float(numpy.linalg.norm(A-B))
        # The first term is the force.  The second is a correction to
        # cause roundoff errors in length to be selfcorrecting.
        dB = (dA.dot(ab_unit) + float(numpy.linalg.norm(A-B))/l_ab - l_ab) * ab_unit

        bc_unit = (B - C) / float(numpy.linalg.norm(B-C))
        # The first term is the force.  The second is a correction to
        # cause roundoff errors in length to be selfcorrecting.
        dC = (dB.dot(bc_unit) + float(numpy.linalg.norm(B-C))/l_bc - l_bc) * bc_unit

        return numpy.array([dA, dB, dC])

    return RK4(update)

A0 = numpy.array([1.0, 1.0])
B0 = numpy.array([2.0, 1.0])
C0 = numpy.array([2.0, 2.0])
dy = calculate_dy(lambda t: numpy.array([-1.0, -1.0]), A0, B0, C0)

t, y, dt = 0., numpy.array([A0, B0, C0]), .02
while t <= 1.01:
    print( (t, y) )
    t, y = t + dt, y + dy( t, y, dt )

Answer 2

By the answers I saw, I realized that the solution is not really simple and will have to be solved by an Inverse Kinematics algorithm.

This site is an example and it is a just a start, although it still does not solve everything, since the point C is fixed and in the case of the truck it should move.

Answer 3

基于2D解析二骨IK ，我在Geogebra中建立了一个全功能模型 ，其中核由两个简单的数学方程组成。