Algorithm 3 Get the value of (Q, W).
1: functionGet Q($w_{goal}$,$C_w$,$R_w$,$P_C$,$L_v$,$c,v,n$)
2:     $ad{d}_{obs}\left[v\right]=0$,$add\left[n\right]=0,j,k,i$	▹ Temporal variables.
3:     for $(j=0;j<c;j++)$ do
4:         for $(k=0;k<v;k++)$ do
5:            for $(i=0;i<n;i++)$ do
6:                $x_k\left[i\right]=cos\left(w_{goal}\left[k\right]\right)\left(L\left[k\right]\right)\left(\frac{i+1}{n}\right)$
7:                $y_k\left[i\right]=sin\left(w_{goal}\left[k\right]\right)\left(L\left[k\right]\right)\left(\frac{i+1}{n}\right)$	▹$(x_k,y_k)$ are the coordinates of each singular point for a given link
8:                $ad{d}_{obs}\left[k\right]+=\frac{P_C\left[j\right]}{{(x_k\left[i\right]-x\left[j\right])}^2+{(y_k\left[i\right]-y\left[j\right])}^2-{\left(r\left[j\right]\right)}^{{}^2}}$	▹ The equation of the circular obstacle $C\left(w\right)$ can be replaced by the equation of ellipsoidal obstacle $R\left(w\right)$
9:            end for
10:            $add\left[j\right]+=ad{d}_{obs}\left[k\right]$
11:         end for
12:         $ad{d}_{obs}\left[v\right]=0$	▹ Temporal variable is cleared.
13:         $Q+=add\left[j\right]$
14:     end for
15:     return $\left(Q\right)$	▹ The value of Q is obtained.
16: end function