Cubic Hermite Curves

Given

Starting and Ending Points and Tangents

P_0, u_0, P_1, u_1

General Form

P (t) = a_0 + a_1t + a_2t^2 + a_3t^3

/tP (t) = P^′ (t) = a_1 + 2a_2t + 3a_3t^2

Boundary Conditions

P (0) = P_0

P^′ (0) = u_0

P (1) = P_1

P^′ (1) = u_1

Solving for the a_i

a_0 = P_0

a_1 = u_0

a_2 = -3 a_0 - 2 a_1 + 3 P_1 - u_1 = 3 (P_1 - P_0) - 2 u_0 - u_1

a_3 = 2 a_0 + a_1 - 2 P_1 + u_1 = u_0 + u_1 - 2 (P_1 - P_0)

Solve[{P_1 == a_0 + a_1 + a_2 + a_3, u_1 == a_1 + 2a_2 + 3a_3}, {a_2, a_3}]

{{a_2→ -3 a_0 - 2 a_1 + 3 P_1 - u_1, a_3→2 a_0 + a_1 - 2 P_1 + u_1}}

{{a_2→ -3 a_0 - 2 a_1 + 3 P_1 - u_1, a_3→2 a_0 + a_1 - 2 P_1 + u_1}}

Substitutiing these a_iinto the "general form" above

P (t) = a_0 + a_1t + a_2t^2 + a_3t^3 = P_0 + u_0t + (3 (P_1 - P_0) - 2 u_0 - u_1) t^2 + (u_0 + u_1 - 2 (P_1 - P_0)) t^3

Rewriting

P (t) = ({{t^3, t^2, t, 1}}) ({{2, -2, 1, 1}, {-3, 3, -2, -1}, {0, 0, 1, 0}, {1, 0, 0, 0}}) ({{P_0}, {P_1}, {u_0}, {u_1}})

Multiplying with right association leads directly to the previous expression. Multiplying with left association leads to an equicalent but slightly different form

P (t) = (1 - 3 t^2 + 2 t^3) P_0 + (3 t^2 - 2 t^3) P_1 + (t - 2 t^2 + t^3) u_0 + (-t^2 + t^3) u_1

This is the Hermite Basis form.

Calculations Useful for Robotics

Hermit Curves

[Graphics:HTMLFiles/index_21.gif]

x_t = A_x (t - t_0)^3 + B_x (t - t_0)^2 + C_x (t - t_0) + D_x

y_t = A_y (t - t_0)^3 + B_y (t - t_0)^2 + C_y (t - t_0) + D_y

Specified positions of robot

(x_0, y_0)

(x_1, y_1)

(x_2, y_2)

Specified orientation of robot at the initial and final points of its first segment

θ_0, θ_1

Specified speed of robot initial and final points of its first segment

s_0, s_1

Specified time that robot reaches initial and final points of its first segment

t_0, t_1

Time variable

t

Derived, third-degree polynomials giving x and y-coordinates of the robot's position as a function of time

x_t , y_t

Derived,robot's speed as a function of time

s_t

Derived, robot's acceleration as a function of time

a_t

Derived,robot's orientation as a function of time

θ_t

Derived, robot's rotational velocity as a function of time

ω_t

Derivation

^2x/t^2 = a_x (t - t_0) + b_x

x/t = ν_ (xt =) 1/2a_x (t - t_0)^2 + b_x (t - t_0) + K_x1

RowBox[{x (t), =, RowBox[{x_t, =, RowBox[{RowBox[{Cell[], 1/6, a_x, (t - t_0)^3}], +, 1/2b_x (t - t_0)^2, +, K_1 (t - t_0), +, K_x2}]}]}]

^2y/t^2 = a_y (t - t_0) + b_y

y/t = ν_yt = 1/2a_y (t - t_0)^2 + b_y (t - t_0) + K_y1

RowBox[{y (t), =, RowBox[{y_t, =, RowBox[{RowBox[{Cell[], 1/6, a_y, (t - t_0)^3}], +, 1/2b_y (t - t_0)^2, +, K_y1 (t - t_0), +, K_y2}]}]}]

Velocity

(ν_x0, ν_y0) = s_0 (Cos (θ_0), Sin (θ_0))

(ν_x1, ν_y1) = s_1 (Cos (θ_1), Sin (θ_1))

Point Start

ν_x0 = x/t (t_0) = S_0Cos[θ_0] = Dx_0 - x_0 = K_x1

ν_y0 = y/t (t_0) = S_0Sin[θ_0] = Dy_0 - y_0 = K_y1

RowBox[{x_0, =, RowBox[{K_x2, Cell[]}]}]

y_0 = K_y2

K_x1 = ν_x0, K_y1 = ν_y0

K_x2 = x_0, K_y2 = y_0

θ_0 = ArcTan[Dx_0 - x_0, Dy_0 - y_0] = ArcTan[ν_x0, ν_y0]

S_0 = ((Dx_0 - x_0)^2 + (Dy_0 - y_0)^2)^(1/2)

S_0 = (Dx_0 - x_0) Cos[θ_0] + (Dy_0 - y_0) Sin[θ_0]

S_0 = ν_x0Cos[θ_0] + ν_y0Sin[θ_0]

Point End

ν_x1 = x/t (t_1) = S_1Cos[θ_1] = Dx_1 - x_1 = 1/2a_x (t_1 - t_0)^2 + b_x (t_1 - t_0) + K_x1

ν_y1 = y/t (t_1) = S_1Sin[θ_1] = Dy_1 - y_1 = 1/2a_y (t_1 - t_0)^2 + b_y (t_1 - t_0) + K_y1

RowBox[{x_1, =, RowBox[{RowBox[{Cell[], 1/6, a_x, (t_1 - t_0)^3}], +, 1/2b_x (t_1 - t_0)^2, +, K_1 (t_1 - t_0), +, K_x2}]}]

RowBox[{y_1, =, RowBox[{RowBox[{Cell[], 1/6, a_y, (t_1 - t_0)^3}], +, 1/2b_y (t_1 - t_0)^2, +, K_y1 (t_1 - t_0), +, K_y2}]}]

θ_1 = ArcTan[Dx_1 - x_1, Dy_1 - y_1] = ArcTan[ν_x1, ν_y1]

S_1 = ((Dx_1 - x_1)^2 + (Dy_1 - y_1)^2)^(1/2)

S_1 = (Dx_1 - x_1) Cos[θ_1] + (Dy_1 - y_1) Sin[θ_1]

S_1 = ν_x1Cos[θ_1] + ν_y1Sin[θ_1]

Get Calculate

ν_x1 = 1/2a_x (t_1 - t_0)^2 + b_x (t_1 - t_0) + ν_x0

x_1 = 1/6a_x (t_1 - t_0)^3 + 1/2b_x (t_1 - t_0)^2 + ν_x0 (t_1 - t_0) + x_0

Solve[{ν_x1 == 1/2a_x (t_1 - t_0)^2 + b_x (t_1 - t_0) + ν_x0, x_1 == 1/6a_x (t_1 - t_0)^3 + 1/2b_x (t_1 - t_0)^2 + ν_x0 (t_1 - t_0) + x_0}, {a_x, b_x}]//Simplify

a_x = (6 (-2 (x_0 - x_1) + (t_0 - t_1) (ν_x0 + ν_x1)))/(t_0 - t_1)^3

b_x = (-6 (x_0 - x_1) + 2 (t_0 - t_1) (2 ν_x0 + ν_x1))/(t_0 - t_1)^2

Results

a_x = 6 ((ν_x1 + ν_x0) (t_1 - t_0) - 2 (x_1 - x_0))/(t_1 - t_0)^3

b_x = -2 ((ν_x1 + 2 ν_x0) (t_1 - t_0) - 3 (x_1 - x_0))/(t_1 - t_0)^2

a_y = 6 ((ν_y1 + ν_y0) (t_1 - t_0) - 2 (y_1 - y_0))/(t_1 - t_0)^3

b_y = -2 ((ν_y1 + 2 ν_y0) (t_1 - t_0) - 3 (y_1 - y_0))/(t_1 - t_0)^2

x_t = 1/6a_x (t - t_0)^3 + 1/2b_x (t - t_0)^2 + ν_x0 (t - t_0) + x_0

y_t = 1/6a_y (t - t_0)^3 + 1/2b_y (t - t_0)^2 + ν_y0 (t - t_0) + y_0

ν_xt = 1/2a_x (t - t_0)^2 + b_x (t - t_0) + ν_x0

ν_yt = 1/2a_y (t - t_0)^2 + b_y (t - t_0) + ν_y0

t0=0

A_x = (12 (x_0 - x_1))/T^3 + (6 (ν_x0 + ν_x1))/T^2

B_x = -(6 (x_0 - x_1))/T^2 - (2 (2 ν_x0 + ν_x1))/T

x_t = 1/6A_xt^3 + 1/2B_xt^2 + ν_x0t + x_0

y_t = 1/6A_yt^3 + 1/2B_yt^2 + ν_y0t + y_0

ν_xt = 1/2A_xt^2 + B_xt + ν_x0

ν_yt = 1/2A_yt^2 + B_yt + ν_y0

orientation

θ_t = ArcTan[ν_xt, ν_yt]

speed

S_t = ν_xtCos[θ_t] + ν_ytSin[θ_t]

x, y velocity

ν_xt = S_tCos[θ_t]

ν_yt = S_tSin[θ_t]

arc length

L = ∫_t_0^t_1S_tt = ∫_t_0^t_1 (ν_xt^2 + ν_yt^2)^(1/2) t

robot speed

s_t = ((x/t)^2 + (y/t)^2)^(1/2)

s_t = √ ((1/2a_x (t - t_0)^2 + b_x (t - t_0) + ν_x0)^2 + (1/2a_y (t - t_0)^2 + b_y (t - t_0) + ν_y0)^2)

s_t = √ ((1/2a_xt^2 + b_xt + ν_x0)^2 + (1/2a_yt^2 + b_yt + ν_y0)^2)

s_t = √ ((a_x^2 + a_y^2)/4t^4 + (a_x b_x + a_y b_y) t^3 + (b_x^2 + b_y^2 + a_x ν_x0 + a_y ν_y0) t^2 + 2 (b_x ν_x0 + b_y ν_y0) t + (ν_x0^2 + ν_y0^2))

({{p_0}, {p_1}, {p_2}, {p_3}, {p_4}}) = ({{ν_x0^2 + ν_y0^2}, {2 (b_x ν_x0 + b_y ν_y0)}, {b_x^2 + b_y^2 + a_x ν_x0 + a_y ν_y0}, {a_x b_x + a_y b_y}, {1/4 (a_x^2 + a_y^2)}})

(∵t_0 = 0)

s_t = (p_4t^4 + p_3t^3 + p_2t^2 + p_1t^1 + p_0)^(1/2)

Length

L = ∫_t_0^t_1S_tt = ∫_t_0^t_1 (ν_xt^2 + ν_yt^2)^(1/2) t

∫_0^T ((1/2A_xt^2 + B_xt + ν_x0)^2 + (1/2A_yt^2 + B_yt + ν_y0)^2)^(1/2) t

∫_0^T √ ((A_x^2 + A_y^2)/4t^4 + (A_x B_x + A_y B_y) t^3 + (B_x^2 + B_y^2 + A_x ν_x0 + A_y ν_y0) t^2 + 2 (B_x ν_x0 + B_y ν_y0) t + (ν_x0^2 + ν_y0^2)) t

∫_0^T (P_4t^4 + P_3t^3 + P_2t^2 + P_1t + P_0)^(1/2) t

acceleration

a_t = s/t = ∂_t (p_4t^4 + p_3t^3 + p_2t^2 + p_1t^1 + p_0)^(1/2) = (p_1 + 2 t p_2 + 3 t^2 p_3 + 4 t^3 p_4)/(2 (p_0 + t p_1 + t^2 p_2 + t^3 p_3 + t^4 p_4)^(1/2))

orientation

θ_t = ArcTan[ν_yt/ν_xt] = ArcTan[(1/2a_yt^2 + b_yt + ν_y0)/(1/2a_xt^2 + b_xt + ν_x0)]

rotational velocity

ω_t = θ/t = ∂_t ArcTan[(1/2a_yt^2 + b_yt + ν_y0)/(1/2a_xt^2 + b_xt + ν_x0)]

ω_t = ((a_yt + b_y) (1/2a_xt^2 + b_xt + ν_x0) - (a_xt + b_x) (1/2a_yt^2 + b_yt + ν_y0))/((1/2a_xt^2 + b_xt + ν_x0)^2 + (1/2a_yt^2 + b_yt + ν_y0)^2)

ω_t = ((a_yt + b_y) ν_xt - (a_xt + b_x) ν_yt)/(ν_xt^2 + ν_yt^2)

Robot 2 Wheel

LinearVelocity = (LeftSpeed + RightSpeed)/2

AngularVelocity = (RightSpeed - LeftSpeed)/WheelBase

LeftSpeed = LinearVelocity - WheelBase/2 * AngularVelocity

RightSpeed = LinearVelocity + WheelBase/2 * AngularVelocity

left wheel position

{x_t + WheelBase/2 * Cos[θ_t + π/2], y_t + WheelBase/2 * Sin[θ_t + π/2]}

right wheel position

{x_t + WheelBase/2 * Cos[θ_t - π/2], y_t + WheelBase/2 * Sin[θ_t - π/2]}

left wheel speed

Speed_t - WheelBase/2 * ω_t

right wheel speed

Speed_t + WheelBase/2 * ω_t

Vector

waypoints

Overscript[P, →] _ (i - 1), Overscript[P, →] _i, Overscript[P, →] _ (i + 1)

Overscript[a, →] _i = Overscript[P, →] _i - Overscript[P, →] _ (i - 1)

Overscript[b, →] _i = Overscript[P, →] _ (i + 1) - Overscript[P, →] _i

Overscript[p, →] ·Overscript[u, →] = 0

∵Overscript[p, →] ·Overscript[u, →] = 0

Overscript[p, →] · (Overscript[b, →] | Overscript[a, →] | +Overscript[a, →] | Overscript[b, →] |) = 0

∴Overscript[u, →] = Overscript[a, →] | Overscript[b, →] | +Overscript[b, →] | Overscript[a, →] |

Cos (θ_i) = (Overscript[a, →] _i·Overscript[b, →] _i)/(| Overscript[a, →] _i | | Overscript[b, →] _i |)

Overscript[a, →] _i· (a_iy, -a_ix) = a_ixa_iy - a_iya_ix = 0

Overscript[m, →] _i = (a_iy, -a_ix)/(| Overscript[a, →] _i |)

Sin (θ_uω) = (u_xω_y - u_yω_x)/(| Overscript[u, →] | | Overscript[ω, →] |)

Trajectory

φ_ (i, 1) = ArcCos ((Overscript[a, →] _ (i + 1) ·Overscript[ν, →] _i)/(| Overscript[a, →] _ (i + 1) | | Overscript[ν, →] _i |))

L_ (i, 1) = | (| Overscript[a, →] _ (i + 1) | φ_ (i, 1))/(Sin (φ_ (i, 1))) |

φ_ (i, 2) = ArcCos ((Overscript[a, →] _ (i + 1) ·Overscript[ν, →] _ (i + 1))/(| Overscript[a, →] _ (i + 1) | | Overscript[ν, →] _ (i + 1) |))

L_ (i, 2) = | (| Overscript[a, →] _ (i + 1) | φ_ (i, 2))/(Sin (φ_ (i, 2))) |

Δr = (L_ (i, 1) + L_ (i, 2))/(s_i + s_ (i + 1))

Circular Arc

x = r Cos (φ)

y = r Sin (φ)

r = (x^2 + y^2)^(1/2)

φ = ArcTan (y/x)

Sin (a±b) = Sin (a) Cos (b) ±Cos (a) Sin (b)

Sin (2a) = 2 Sin (a) Cos (a)

Sin (a + π/2) = Cos (a)

Sin (a - π/2) = -Cos (a)

Cos (a + π/2) = -Sin (a)

Cos (a - π/2) = Sin (a)

Trajectory based on an arc of a circle

[Graphics:HTMLFiles/index_141.gif]

(Sin (θ))/r_G = (Sin (R))/r_C

φ_G + R = π/2

θ + 2R = π

θ + 2 (π/2 - φ_G) = π

θ = 2φ_G

(Sin (2φ_G))/r_G = (Sin (π/2 - φ_G))/r_c

r_c = r_G (Sin (π/2 - φ_G))/(Sin (2φ_G))

r_c = r_G (Cos (φ_G))/(2Sin (φ_G) Cos (φ_G))

r_c = r_G/(2Sin (φ_G))

L = r_Cθ

L = r_C (2φ_G)

L = (r_G·φ_G)/(Sin (φ_G))

Geometry in Motion

t_1 - t_0 = L/s

ω = θ/(t_1 - t_0)

θ (t) = ω (t - t_0) + θ_0

Trigonometry at Work

Relation between initial point and circle.

[Graphics:HTMLFiles/index_157.gif]

x_C = x_0 + r_C·Cos (θ_0 + π/2)

y_C = y_0 + r_C·Sin (θ_0 + π/2)

x_C = x_0 -| r_C | Cos (θ_0 - π/2)

y_C = y_0 -| r_C | Sin (θ_0 - π/2)

x_C = x_0 - r_C·Sin (θ_0)

y_C = y_0 + r_C·Cos (θ_0)

x (t) = x_C + r_C·Cos (ω (t - t_0) + θ_0 - π/2)

y (t) = y_0 + r_C·Sin (ω (t - t_0) + θ_0 - π/2)

x (t) = x_C - r_C·Sin (θ_0) + r_C·Sin (ω (t - t_0) + θ_0)

y (t) = y_0 + r_C·Cos (θ_0) - r_C·Cos (ω (t - t_0) + θ_0)

Results

if φ_G>π/2then θ = 2 (φ_G - π)

if φ_G< -π/2then θ = 2 (φ_G + π)

L = -r_C·θ

t_1 - t_0 = L/s

ω = θ/(t_1 - t_0)