note
Forward kinematic
Inverse kinematic
x 1 α 1 β 1 = T x 2 + T y 2 = arctan ( T x − T y ) − 1 = arccos ( 2 ⋅ x 1 ⋅ L 1 x 1 2 + L 1 2 − ( L 3 + d ) 2 )
Calculate P x P y α 2 β 2
{ Q x Q y = − L 1 ⋅ cos ( 18 0 ∘ − α 1 − β 1 ) = − L 1 ⋅ sin ( 18 0 ∘ − α 1 − β 1 ) ⇒ { Q x Q y = L 1 ⋅ cos ( α 1 + β 1 ) = − L 1 ⋅ sin ( α 1 + β 1 ) { P x P y = d + L 3 Q x ⋅ d + T x ⋅ L 3 = d + L 3 Q y ⋅ d + T y ⋅ L 3
x 2 α 2 β 2 = ( D − P x ) 2 + P y 2 = arctan ( D − P x − P y ) = arccos ( 2 ⋅ x 2 ⋅ L 1 x 2 2 + L 1 2 − L 2 2 ) Summary
θ 1 = α 1 + β 1 θ 2 = α 2 + β 2 = arctan T x 2 T y 2 + arccos 16 1 T x 2 + T y 2 T x 2 + T y 2 − 260 = arctan 8.16 − 9 32 cos ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 ))) − 9 5 T x 18 − 64 sin ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 ))) + 10 T y + arccos ( 8.16 − 9 32 cos ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 ))) − 9 5 T x ) 2 + 324 − 64 sin ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 )) + 10 T y ) 2 16 1 ( ( 8.16 − 9 32 cos ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 ))) − 9 5 T x ) 2 + 324 − 64 sin ( arctan ( T y 2 / T x 2 ) + arccos ( 16 1 ( T x 2 + T y 2 T x 2 + T y 2 − 260 )) + 10 T y ) 2 − 105
Kind of scary. This function is only for demonstration, I’m not going to use it in c++ programming, but break down the steps and store the values into variables instead.
Route control
PID
Stanley Controller
Pure Pursuit Algorithm
MPC (Model Predictive Control)
LQR (Linear Quadratic Regulator)
DWA (Dynamic Window Approach)
