Seja $C$ o conjunto de pontos na superfície de um elo do robô e $D$ o conjunto de pontos na superfície de um obstáculo, pode-se definir a função de distância $\Psi_{CD}(q)$ e sua jacobiana $J_{\Psi}\equiv J_{\Psi_{CD}}(q):$

Controle com restrições (MARINHO et al., 2019):

$$\begin{align*} \min_{\dot{q}}{}&{\| J_{r}\dot{q} + K_{r}r\|_2^2 + \|\epsilon\dot{q}}\|_2^2\\ \text{sujeito a} &\ W\dot{q}\leq w , \end{align*}$$

onde $q\equiv q(t)\in \mathbb{R}^n$ é o vetor de configurações do robô, $r\equiv r(q):\mathbb{R}^n\mapsto\mathbb{R}^m$ é uma função de tarefa, $J_r\equiv J_r(q)=\frac{\partial r(q)}{\partial q}\in \mathbb{R}^{m\times n}$ é a matriz jacobiana de tarefa e $K_r \in (0, \infty)$ é um ganho. $W\equiv W(q)\in \mathbb{R}^{\ell \times n}$ e $w\equiv w(q)\in \mathbb{R}^{\ell}$ definem as restrições de desigualdade.

A tarefa corresponde a atingir uma pose alvo, portanto pode ser definida por (GONÇALVES, 2022a):

$$\begin{align*} r(q) = \begin{pmatrix}p_{e}(q) - p_{tg}\\1-x_{tg}^Tx_{e}(q)\\1-y_{tg}^Ty_{e}(q)\\1-z_{tg}^Tz_{e}(q)\end{pmatrix}\in\mathbb{R}^6, \end{align*}$$

onde $p_{e}(q) \in \mathbb{R}^{3}$ é a posição do centro do referencial do efetuador, $p_{tg}\in \mathbb{R}^{3}$ é a posição do centro do alvo, $x_e(q), y_e(q), z_e(q)\in\mathbb{R}^3$ são os eixos $x,y, z$ do referencial do efetuador e $x_{tg}, y_{tg}, z_{tg}\in\mathbb{R}^3$ são os eixos $x,y, z$ do alvo. Todos os elementos são medidos no referencial do mundo.

$$\begin{align*} r(q) = \begin{pmatrix}p_{e}(q) - p_{tg}\\1-x_{tg}^Tx_{e}(q)\\1-y_{tg}^Ty_{e}(q)\\1-z_{tg}^Tz_{e}(q)\end{pmatrix}\in\mathbb{R}^6, \end{align*}$$

$$\begin{align*} J_r(q) = \frac{\partial r(q)}{\partial q}=\begin{bmatrix} J_p(q) \\ x_{tg}^TS\left(x_e\left(q\right)\right)J_\omega(q) \\ y_{tg}^TS(y_e(q))J_\omega(q) \\ z_{tg}^TS(z_e(q))J_\omega(q) \end{bmatrix} \in\mathbb{R}^{6\times n}, \end{align*}$$

onde $J_p(q)\in\mathbb{R}^{3\times n}$ é a jacobiana de posição do efetuador, que são as três primeiras linhas de sua jacobiana geométrica $J_{geo}\in\mathbb{R}^{6\times n}$, $J_\omega(q)\in\mathbb{R}^{3\times n}$ é a jacobiana de orientação do efetuador, que são as três últimas linhas de $J_{geo}$. $S$ é um operador que mapeia um vetor em sua respectiva matriz antissimétrica

Pode ser definida pela seguinte inequação diferencial, adotando-se uma distância de segurança $d_\text{safe}$ e um coeficiente de amortecimento $\eta$ (KANOUN; LAMIRAUX; WIEBER, 2011).

$$\begin{align*} \frac{d}{dt} \Psi_{C_iD_j} &\ge -\eta \left(\Psi_{C_iD_j}-d_\text{safe}\right)\\ \implies J_{\Psi_{C_iD_j}}^T\dot{q} &\ge -\eta \left(\Psi_{C_iD_j}-d_\text{safe}\right) \end{align*}$$

Dado

$$ \begin{align*} b= \begin{bmatrix} \Psi_{C_{1}D_{1}} \\ \Psi_{C_{2}D_{1}} \\ \vdots \\ \Psi_{C_{p}D_{1}} \\ \Psi_{C_{1}D_{2}} \\ \vdots \\ \Psi_{C_{p}D_{l}} \end{bmatrix} \in \mathbb{R}^{p\cdot l},\quad A= \begin{bmatrix} J_{\Psi_{C_{1}D_{1}}}^{T} \\ J_{\Psi_{C_{2}D_{1}}}^{T} \\ \vdots \\ J_{\Psi_{C_{p}D_{1}}}^{T} \\ J_{\Psi_{C_{1}D_{2}}}^{T} \\ \vdots \\ J_{\Psi_{C_{p}D_{l}}}^{T} \end{bmatrix} \in \mathbb{R}^{(p\cdot l)\times n} \end{align*} $$

A restrição é expressa por $$-A \dot{q} \leq \eta \left(b - d_\text{safe}\right) $$

O custo computacional para se calcular todas as distâncias e jacobianas é caro, portanto basta tomar as $k$ distâncias mais críticas (isso é feito internamente pelo simulador via hierarquia de primitivas).

Dessa forma, $\bar{b}\in\mathbb{R}^k$ que contém apenas as linhas de $b$ que apresentam as $k$ menores distâncias $\Psi_{C_iD_j}$. Suas respectivas jacobianas são armazenadas em uma matriz $\bar{A}\in\mathbb{R}^{k\times n}$. As restrições se tornam

$$-\bar{A} \dot{q} \leq \eta \left(\bar{b} - d_\text{safe}\right)$$

Análoga às restrições de colisão externa, mas cada $D_j$ representa um outro elo não subsequente do robô. As restrições podem ser definidas por

$$-\bar{A}_\text{auto} \dot{q} \leq \eta_\text{auto} \left(\bar{b}_\text{auto} - d_\text{safe}\right),$$

onde, devido a hierarquia de primitivas, $\bar{A}_\text{auto}\in\mathbb{R}^{g\times n}, \bar{b}_\text{auto}\in\mathbb{R}^{g}$.

Basta impor que a velocidade de junta $\dot{q}$ satisfaça

$$ \begin{align*}&\xi\left(q_{\text{min}} - q\right) \leq I\dot{q} \leq \xi\left(q_{\text{max}} - q\right)\\ &\implies \begin{cases} I\dot{q} \leq \xi\left(q_{\text{max}} - q\right)\\ - I\dot{q} \leq -\xi\left(q_{\text{min}} - q\right)\end{cases},\end{align*}$$

onde $q_\text{min}, q_\text{max}\in\mathbb{R}^n$ são, respectivamente, vetores que contém os valores mínimos e máximos de cada junta, $\xi$ um fator de amortecimento e $I$ a matriz identidade de tamanho $n \times n$.

Basta garantir que a configuração das juntas $q_7$ e $q_8$ mantenham uma diferença constante com a configuração $q_6$, sendo o manipulador representado pela configuração $(q_1, q_2, \dots, q_9)$, ou seja

$$ \begin{align*} \dot{q}_7 - \dot{q}_6 &= -\sigma\left(q_7 - q_6 - \delta_{76}\right) \\ \dot{q}_8 - \dot{q}_6 &= -\sigma\left(q_8 - q_6 - \delta_{86}\right) , \end{align*}$$

onde $\sigma$ é um fator de amortecimento, $\delta_{76} \equiv q_{7,0} - q_{6,0}$ e $\delta_{86} \equiv q_{8,0} - q_{6,0}$, sendo $q_{6,0}, q_{7,0}\ \text{e}\ q_{8,0}$ as configurações iniciais das juntas $q_6, q_7$ e $q_8$, respectivamente.

Tomando-se as definições matriciais

$$ \begin{align*} \widetilde{q}_{76} &\equiv \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & -1 & 1 & 0 & 0 \end{bmatrix}\\ \widetilde{q}_{86} &\equiv \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & -1 & 0 & 1 & 0 \end{bmatrix} \end{align*}$$

pode-se definir as restrições por meio da seguinte inequação.

$$ \begin{equation*} \begin{bmatrix} \widetilde{q}_{76}\\\widetilde{q}_{86} \\ -\widetilde{q}_{76}\\ -\widetilde{q}_{86} \end{bmatrix}\dot{q} \le \begin{bmatrix} -\sigma\left(q_7-q_6-\delta_{76}\right) \\ -\sigma\left(q_8-q_6-\delta_{86}\right)\\ \sigma\left(q_7-q_6-\delta_{76}\right) \\ \sigma\left(q_8-q_6-\delta_{86}\right) \end{bmatrix} \end{equation*} $$

Concatenando-se todas as restrições, tem-se

$$ \begin{equation*} W = \begin{bmatrix} -\bar{A}\\-\bar{A}_\text{auto}\\ I_{n\times n} \\ -I_{n\times n} \\ \widetilde{q}_{76}\\\widetilde{q}_{86} \\ -\widetilde{q}_{76}\\ -\widetilde{q}_{86} \end{bmatrix} \in \mathbb{R}^{(k+g+2n+4)\times n} ,\quad w=\begin{bmatrix} \eta\left(\bar{b} - d_\text{safe}\right) \\ \eta_\text{auto}\left(\bar{b}_\text{auto} - d_\text{safe}\right) \\ \xi\left(q_\text{max} - q\right) \\ -\xi\left(q_\text{min} - q\right) \\ -\sigma\left(q_7-q_6-\delta_{76}\right) \\ -\sigma\left(q_8-q_6-\delta_{86}\right) \\ \sigma\left(q_7-q_6-\delta_{76}\right) \\ \sigma\left(q_8-q_6-\delta_{86}\right) \end{bmatrix} \in \mathbb{R}^{(k+g+2n+4)} \end{equation*}$$

Obtendo-se por fim a representação

$$ \begin{equation*} W \dot{q} \le w \end{equation*} $$

Função de tarefa ao longo do tempo de execução

Distâncias obtidas pela estimativa de colisões externas e auto-colisão ao longo do tempo de execução

Configuração das juntas ao longo do tempo de execução

Velocidade das juntas ao longo do tempo de execução

Tempo gasto para a construção do problema de otimização e para a sua respectiva solução em cada iteração

Função de tarefa ao longo do tempo de execução

Distâncias obtidas pela estimativa de colisões externas e auto-colisão ao longo do tempo de execução

Configuração das juntas ao longo do tempo de execução

Velocidade das juntas ao longo do tempo de execução

Número de restrições, ou número de linhas de $W$, por iteração

Tempo gasto para a construção do problema de otimização e para a sua respectiva solução em cada iteração