Cinemática inversa de manipuladores seriales
BE3027 - Robótica Médica
¿Qué tenemos hasta ahora?
q
\mathbf{q}
K(q)⊆BTE(q)
\mathcal{K}\left(\mathbf{q}\right) \subseteq {^B}\mathbf{T}_E(\mathbf{q})
K:C→T
\mathcal{K}:\mathcal{C}\to \mathcal{T}
cinemática directa
q˙
\dot{\mathbf{q}}
BVE=[BvEBωBE]
{^B}\mathcal{V}_E=\begin{bmatrix} {^B}\mathbf{v}_E \\ {^B}\boldsymbol{\omega}_{BE} \end{bmatrix}
BVE=J(q)q˙
{^B}\mathcal{V}_E=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}
cinemática diferencial
q˙
\dot{\mathbf{q}}
BVE=[BvEBωBE]
{^B}\mathcal{V}_E=\begin{bmatrix} {^B}\mathbf{v}_E \\ {^B}\boldsymbol{\omega}_{BE} \end{bmatrix}
BVE=J(q)q˙
{^B}\mathcal{V}_E=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}
cinemática diferencial
¿A qué queremos llegar?
q
\mathbf{q}
⊆BTE(q)
\subseteq {^B}\mathbf{T}_E(\mathbf{q})
K−1:T→C
\mathcal{K}^{-1}:\mathcal{T}\to \mathcal{C}
cinemática inversa
especificación de la tarea
(se tiene)
referencias para los servos
(se quiere)
A pesar que el planteamiento es claro, se tiene un problema considerable
K−1(K(q))=q
\mathcal{K}^{-1}\left( \mathcal{K}(\mathbf{q})\right)=\mathbf{q}
A pesar que el planteamiento es claro, se tiene un problema considerable
K−1(K(q))=q
\mathcal{K}^{-1}\left( \mathcal{K}(\mathbf{q})\right)=\mathbf{q}
K(q)
\mathcal{K}(\mathbf{q})
(en general) función extremadamente no lineal de la configuración
A pesar que el planteamiento es claro, se tiene un problema considerable
K−1(K(q))=q
\mathcal{K}^{-1}\left( \mathcal{K}(\mathbf{q})\right)=\mathbf{q}
K−1
\mathcal{K}^{-1}
encontrar este mapeo resulta muy difícil y en muchos casos NO existe solución analítica
Ejemplo: cinemática inversa analítica
q=[q1q2]∈R2
\mathbf{q}=\begin{bmatrix} q_1 \\ q_2 \end{bmatrix} \in \mathbb{R}^2
T∼(x,y)⊂SE(2)
\mathcal{T} \sim (x,y) \subset SE(2)
espacio de configuración
espacio de tarea
Ejemplo: cinemática inversa analítica
θjq1q2dj00aja1a2αj00
\begin{array}{c|c|c|c}
\theta_j & d_j & a_j & \alpha_j \\ \hline
q_1 & 0 & a_1 & 0 \\ \hline
q_2 & 0 & a_2 & 0 \\
\end{array}
R
R
manipulador RR
BTE(q)=cos(q1+q2)sin(q1+q2)00−sin(q1+q2)cos(q1+q2)000010a2cos(q1+q2)+a1cos(q1)a2sin(q1+q2)+a1sin(q1)01
{^B}\mathbf{T}_E(\mathbf{q})=\begin{bmatrix} \cos(q_1+q_2) & -\sin(q_1+q_2) & 0 & a_2\cos(q_1+q_2)+a_1\cos(q_1) \\ \sin(q_1+q_2) & \cos(q_1+q_2) & 0 & a_2\sin(q_1+q_2)+a_1\sin(q_1) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
J(q)=−a1sin(q1)−a2sin(q1+q2)a1cos(q1)+a2cos(q1+q2)0001−a2sin(q1+q2)a2cos(q1+q2)0001
\mathbf{J}(\mathbf{q})=
\begin{bmatrix} -a_1\sin(q_1)-a_2\sin(q_1 + q_2) & -a_2\sin(q_1 + q_2) \\ a_1\cos(q_1)+a_2\cos(q_1 + q_2) & a_2\cos(q_1 + q_2) \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{bmatrix}
BTE(q)=cos(q1+q2)sin(q1+q2)00−sin(q1+q2)cos(q1+q2)000010a2cos(q1+q2)+a1cos(q1)a2sin(q1+q2)+a1sin(q1)01
{^B}\mathbf{T}_E(\mathbf{q})=\begin{bmatrix} \cos(q_1+q_2) & -\sin(q_1+q_2) & 0 & a_2\cos(q_1+q_2)+a_1\cos(q_1) \\ \sin(q_1+q_2) & \cos(q_1+q_2) & 0 & a_2\sin(q_1+q_2)+a_1\sin(q_1) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
K(q)=B[xE(q)yE(q)]=[a1cos(q1)+a2cos(q1+q2)a1sin(q1)+a2sin(q1+q2)]
\mathcal{K}\left(\mathbf{q}\right)=
{^B}\begin{bmatrix} x_E(\mathbf{q}) \\ y_E(\mathbf{q}) \end{bmatrix}=
\begin{bmatrix} a_1\cos(q_1)+a_2\cos(q_1+q_2) \\ a_1\sin(q_1)+a_2\sin(q_1+q_2) \end{bmatrix}
⇒q=K−1(B[xE(q)yE(q)])
\Rightarrow \mathbf{q}=
\mathcal{K}^{-1}\left({^B}\begin{bmatrix} x_E(\mathbf{q}) \\ y_E(\mathbf{q}) \end{bmatrix}\right)
empleando trigonometría y métodos algebráicos
q1=atan2(xEyE)−atan2(a1+a2cos(q2)a2sin(q2))
q_1=\mathrm{atan2}\left(\dfrac{y_E}{x_E}\right)-\mathrm{atan2}\left(\dfrac{a_2\sin(q_2)}{a_1+a_2\cos(q_2)}\right)
q2=arccos(2a1a2xE2+yE2−a12−a22)
q_2=\arccos\left( \dfrac{x_E^2+y_E^2-a_1^2-a_2^2}{2 a_1a_2}\right)
empleando trigonometría y métodos algebráicos
q1=atan2(xEyE)−atan2(a1+a2cos(q2)a2sin(q2))
q_1=\mathrm{atan2}\left(\dfrac{y_E}{x_E}\right)-\mathrm{atan2}\left(\dfrac{a_2\sin(q_2)}{a_1+a_2\cos(q_2)}\right)
q2=arccos(2a1a2xE2+yE2−a12−a22)
q_2=\arccos\left( \dfrac{x_E^2+y_E^2-a_1^2-a_2^2}{2 a_1a_2}\right)
arctan pero válida en (−π,π] (programación)
las expresiones trigonométricas muestran múltiples soluciones al problema
si bien hay casos en donde podemos encontrar la solución analítica, hacerlo no es tan buena idea
→ expresiones complejas e individuales, incluso para manipuladores triviales
→ solución dada en lazo abierto (no corrige errores)
¿Qué hacemos entonces?
emplear algoritmos basados en control (lazo cerrado) para encontrar la cinemática inversa de forma numérica
>> be3027_clase7_robotRR_ik.mlx
¿Cómo funciona el ikine de la Robotics Toolbox?
q0
\mathbf{q}_0
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
Td=BTE(qd)=[Rd0od1]
\mathbf{T}_d={^B}\mathbf{T}_E(\mathbf{q}_d)\\
=\begin{bmatrix} \mathbf{R}_d & \mathbf{o}_d \\ \mathbf{0} & 1 \end{bmatrix}
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
qf
\mathbf{q}_f
Td=BTE(qd)=[Rd0od1]
\mathbf{T}_d={^B}\mathbf{T}_E(\mathbf{q}_d)\\
=\begin{bmatrix} \mathbf{R}_d & \mathbf{o}_d \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
qf
\mathbf{q}_f
ikine
BTE→Td
{^B}\mathbf{T}_E \to \mathbf{T}_d
(en general)
q→qd
\mathbf{q} \to \mathbf{q}_d
NO necesariamente
Td=BTE(qd)=[Rd0od1]
\mathbf{T}_d={^B}\mathbf{T}_E(\mathbf{q}_d)\\
=\begin{bmatrix} \mathbf{R}_d & \mathbf{o}_d \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
qf
\mathbf{q}_f
ikine
BTE→Td
{^B}\mathbf{T}_E \to \mathbf{T}_d
(en general)
q→qd
\mathbf{q} \to \mathbf{q}_d
NO necesariamente
Para que el problema tenga solución y se acople a los GDL del robot, la Robotics Toolbox emplea la noción de máscaras
Td=BTE(qd)=[Rd0od1]
\mathbf{T}_d={^B}\mathbf{T}_E(\mathbf{q}_d)\\
=\begin{bmatrix} \mathbf{R}_d & \mathbf{o}_d \\ \mathbf{0} & 1 \end{bmatrix}
q0
\mathbf{q}_0
T0=BTE(q0)=[BRE(q0)0BoE(q0)1]
\mathbf{T}_0={^B}\mathbf{T}_E(\mathbf{q}_0)=
\begin{bmatrix} {^B}\mathbf{R}_E(\mathbf{q}_0) & {^B}\mathbf{o}_E(\mathbf{q}_0) \\ \mathbf{0} & 1 \end{bmatrix}
qf
\mathbf{q}_f
ikine
BTE→Td
{^B}\mathbf{T}_E \to \mathbf{T}_d
(en general)
q→qd
\mathbf{q} \to \mathbf{q}_d
NO necesariamente
Td=BTE(qd)=[Rd0od1]
\mathbf{T}_d={^B}\mathbf{T}_E(\mathbf{q}_d)\\
=\begin{bmatrix} \mathbf{R}_d & \mathbf{o}_d \\ \mathbf{0} & 1 \end{bmatrix}
>> be3027_clase7_p560ik.m
Obteniendo intuición detrás de los algoritmos
idea fundamental (prestando atención sólo a la posición)
BvE=Jv(q)q˙
{^B}\mathbf{v}_E=\mathbf{J}_v(\mathbf{q})\dot{\mathbf{q}}
ΔtΔBoE=Jv(q)ΔtΔq
\dfrac{\Delta {^B}\mathbf{o}_E}{\Delta t}=\mathbf{J}_v(\mathbf{q}) \dfrac{\Delta \mathbf{q}}{\Delta t}
idea fundamental (prestando atención sólo a la posición)
BvE=Jv(q)q˙
{^B}\mathbf{v}_E=\mathbf{J}_v(\mathbf{q})\dot{\mathbf{q}}
ΔtΔBoE=Jv(q)ΔtΔq
\dfrac{\Delta {^B}\mathbf{o}_E}{\Delta t}=\mathbf{J}_v(\mathbf{q}) \dfrac{\Delta \mathbf{q}}{\Delta t}
ΔBoE=Jv(q)Δq
\Delta {^B}\mathbf{o}_E=\mathbf{J}_v(\mathbf{q}) \Delta \mathbf{q}
⇒Δq=Jv†(q)ΔBoE
\Rightarrow \Delta \mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}) \Delta {^B}\mathbf{o}_E
idea fundamental (prestando atención sólo a la posición)
BvE=Jv(q)q˙
{^B}\mathbf{v}_E=\mathbf{J}_v(\mathbf{q})\dot{\mathbf{q}}
ΔtΔBoE=Jv(q)ΔtΔq
\dfrac{\Delta {^B}\mathbf{o}_E}{\Delta t}=\mathbf{J}_v(\mathbf{q}) \dfrac{\Delta \mathbf{q}}{\Delta t}
ΔBoE=Jv(q)Δq
\Delta {^B}\mathbf{o}_E=\mathbf{J}_v(\mathbf{q}) \Delta \mathbf{q}
⇒Δq=Jv†(q)ΔBoE
\Rightarrow \Delta \mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}) \Delta {^B}\mathbf{o}_E
pseudo-inversa del jacobiano >> pinv(Jv)
Td≡ meta
BTE(q)
efector final
el algoritmo debe llevar al efector final a la pose deseada o "meta"
Td≡ meta
BTE(q)
efector final
Nota 1: el robot sólo puede cambiar su configuración
Nota 2: por el momento nos interesa sólo la posición
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
¿Cuál es la discrepancia entre la posición deseada y la posición actual?
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
¿Cuál es la discrepancia entre la posición deseada y la posición actual?
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
ep=od−BoE(q1)
\mathbf{e}_p=\mathbf{o}_d-{^B}\mathbf{o}_E(\mathbf{q}_1)
¿Cuál es la discrepancia entre la posición deseada y la posición actual?
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
ep=od−BoE(q1)∼ΔBoE
\mathbf{e}_p=\mathbf{o}_d-{^B}\mathbf{o}_E(\mathbf{q}_1) \sim \Delta {^B}\mathbf{o}_E
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
¿Hacia dónde, potencialmente, puede moverse el robot?
¿Hacia dónde, potencialmente, puede moverse el robot?
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
configuración actual: q1
Av−1(q1)=(Jv(q1)Jv⊤(q1))−1
\mathbf{A}_v^{-1}(\mathbf{q}_1)=\left(\mathbf{J}_v(\mathbf{q}_1)\mathbf{J}^\top_v(\mathbf{q}_1)\right)^{-1}
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
Av−1(q1)=(Jv(q1)Jv⊤(q1))−1
\mathbf{A}_v^{-1}(\mathbf{q}_1)=\left(\mathbf{J}_v(\mathbf{q}_1)\mathbf{J}^\top_v(\mathbf{q}_1)\right)^{-1}
entonces el robot hace un movimiento pequeño tomando todo en consideración
configuración actual: q2
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
Av−1(q1)=(Jv(q1)Jv⊤(q1))−1
\mathbf{A}_v^{-1}(\mathbf{q}_1)=\left(\mathbf{J}_v(\mathbf{q}_1)\mathbf{J}^\top_v(\mathbf{q}_1)\right)^{-1}
entonces el robot hace un movimiento pequeño tomando todo en consideración
configuración actual: q2
q2=q1+Δq
\mathbf{q}_2=\mathbf{q}_1+\Delta\mathbf{q}
Δq=Jv†(q1)ΔBoEΔq=Jv†(q1)ep
\Delta\mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}_1) \Delta {^B}\mathbf{o}_E \\
\Delta\mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}_1) \mathbf{e}_p
BoE(q1)
{^B}\mathbf{o}_E(\mathbf{q}_1)
od
\mathbf{o}_d
Av−1(q1)=(Jv(q1)Jv⊤(q1))−1
\mathbf{A}_v^{-1}(\mathbf{q}_1)=\left(\mathbf{J}_v(\mathbf{q}_1)\mathbf{J}^\top_v(\mathbf{q}_1)\right)^{-1}
entonces el robot hace un movimiento pequeño tomando todo en consideración
configuración actual: q2
q2=q1+Δq
\mathbf{q}_2=\mathbf{q}_1+\Delta\mathbf{q}
Δq=Jv†(q1)ΔBoEΔq=Jv†(q1)ep
\Delta\mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}_1) \Delta {^B}\mathbf{o}_E \\
\Delta\mathbf{q}=\mathbf{J}^\dagger_v(\mathbf{q}_1) \mathbf{e}_p
q2=q1+Jv†(q1)ep
\mathbf{q}_2=\mathbf{q}_1+\mathbf{J}^\dagger_v(\mathbf{q}_1)\mathbf{e}_p
y se repite el proceso hasta que el error/discrepancia sea lo suficientemente pequeño
pueden obtenerse algoritmos similares pero para la orientación o la pose completa
qk+1=qk+J†(qk)ek
\mathbf{q}_{k+1}=\mathbf{q}_k+\mathbf{J}^\dagger\left(\mathbf{q}_k\right)\mathbf{e}_k
Todo algoritmo de cinemática inversa cumple con la estructura
donde qk+1→qd conforme incrementa el número de iteraciones
Cinemática inversa de manipuladores seriales BE3027 - Robótica Médica
BE3027 - Lecture 7 (2024)
By Miguel Enrique Zea Arenales
BE3027 - Lecture 7 (2024)
- 184
