[MR]RCM约束下的手术机器人

---

RCM(Remote Center of Motion)约束，通俗讲就是做微创手术时，机械臂插进身体，机械臂受身体的插入点的约束，不能乱动。

相关参考论文《 Task Control with Remote Center of Motion Constraint for Minimally Invasive Robotic Surgery》

---

RCM例题如下

下图显示了用机器人执行内窥镜检查问题的平面表示。

平面2R机械臂的第二个（也是最后一个）连杆的内窥镜摄像头必须穿过套管针（红色圆柱体）与垂直壁相交， 通过套管针中心（红点）

机械臂的两个连杆的长度都等于1。

求$P_E$点的所有容许的笛卡尔速度

解题如下

首先写出$P_1$和$P_2$ (即 $P_E$) 的坐标如下：

$$P_1=\left(\begin{array}{c}\cos \left(q_1\right)\\ \sin \left(q_1\right)\end{array}\right)$$

$$P_2=\left(\begin{array}{c}\cos \left(q_1\right)+\cos \left(q_2\right)\\ \sin \left(q_1\right)+\sin \left(q_2\right)\end{array}\right)$$

以及角度$q$如下：

$$q=\left(\begin{array}{c}{q}_1\\ q_2\end{array}\right)$$

然后 $P_1$和$P_2$ 对 $q$ 微分求 雅可比矩阵 $J_1$ 和 $J_2$ 如下：

$$J_1=\frac{\partial P_1}{\partial q}=\left(\begin{array}{cc}-\sin \left(q_1\right)& 0\\ \cos \left(q_1\right)& 0\end{array}\right)$$

$$J_2=\frac{\partial P_2}{\partial q}=\left(\begin{array}{cc}-\sin \left(q_1\right)& -\sin \left(q_2\right)\\ \cos \left(q_1\right)& \cos \left(q_2\right)\end{array}\right)$$

然后根据论文提到的公式，可以求出 $P_{RCM}$ 处的雅可比矩阵 $J_{RCM}(q,\lambda )$ 如下：

$$J_{RCM}(q,\lambda )={\left(\begin{array}{c}{J}_2-\lambda (J_2-J_1)\\ P2-P1\end{array}\right)}^T=\left(\begin{array}{ccc}-\sin \left(q_1\right)& -\lambda \sin \left(q_2\right)& \cos \left(q_2\right)\\ \cos \left(q_1\right)& \lambda \cos \left(q_2\right)& \sin \left(q_2\right)\end{array}\right)$$

接着，根据下面这个公式，可以求得角速度 $\dot{q}$如下：

$$\dot{P_{RCM}}=J_{RCM}(q,\lambda )\left(\begin{array}{c}\dot{q}\\ \dot{\lambda }\end{array}\right)=0$$

$$\dot{q}=\left(\begin{array}{c}\frac{\dot{\lambda }}{\sin \left(q_1-q_2\right)}\\ -\frac{\dot{\lambda }\cos \left(q_1-q_2\right)}{\lambda \sin \left(q_1-q_2\right)}\end{array}\right)$$

求得角速度后，就可以计算出 $\dot{P_2}=\dot{P_E}$ 如下：

$$\dot{P_E}=J_2\dot{q}=\left(\begin{array}{c}-\frac{\dot{\lambda }\left(\sin \left(q_1-2q_2\right)-\sin \left(q_1\right)+2\lambda \sin \left(q_1\right)\right)}{2\lambda \sin \left(q_1-q_2\right)}\\ -\frac{\dot{\lambda }\left(\cos \left(q_1-2q_2\right)+\cos \left(q_1\right)-2\lambda \cos \left(q_1\right)\right)}{2\lambda \sin \left(q_1-q_2\right)}\end{array}\right)$$

Matlab代码实现如下

clear all
close all
clc

定义各种要用的符号
syms q1 q2 real     %角
syms lambda lambdav

q = [q1; q2];
qv = [qv1; qv2];

P1 = [[cos(q1)]; [sin(q1)]]
P2 = [[cos(q1)+cos(q2)]; [sin(q1)+sin(q2)]];

J1 = jacobian(P1, q);
J2 = jacobian(P2, q);

Jrcm = [[J1+lambda*(J2-J1)] P2-P1]
v = [qv; lambdav];

qv = simplify((inv(Jrcm(:, 1:2)))*(Jrcm(:, 3)*(-lambdav)))

Pe = simplify(J2*qv)