[发明专利]一种五轴治疗床的逆解工程及轨迹规划算法

权利要求书

1.一种五轴治疗床的逆解工程及轨迹规划算法，其特征在于，包括以下步骤：

(1)、病灶空间坐标定位，其过程如下：

(1.1)、通过医学影像设备确定肿瘤坐标O_C与病患头顶坐标O_H的相对位置

(1.2)、在五轴治疗床躺板上的头部一端安装有激光测距传感器，利用激光测距传感器确定病患头顶坐标O_H与治疗床躺板固定坐标O_B的相对位置

(1.3)、肿瘤抵达靶区，肿瘤坐标O_C与靶区中央坐标O_G重合，即得到治疗床躺板固定坐标O_B与靶区中央坐标O_G的相对位置

(1.4)、靶区中央坐标O_G与五轴治疗床基座O₀的相对位置为固定参数，得到：其中是五轴治疗床基座O₀相对于治疗床躺板固定坐标O_B的三维空间向量；

(2)、五轴治疗床运动学逆解运算，其过程如下：

(2.1)、根据Denavit–Hartenberg法，在各个关节轴上建立坐标系，得到1-5号坐标系，基于构型及目标运动过程限制，需保证治疗床躺板固定坐标系与五轴治疗床基座坐标系的Z轴方向一致，此时，第四关节转角需满足θ₄＝π-θ₃；其中θ₃指的是第三个转动关节的转动角度，θ₄指的是第四个转动关节的转动角度；

(2.2)、求解得到治疗床躺板固定坐标O_B相对五轴治疗床基座O₀的位置变换矩阵：

变换矩阵中：

o＝-cos(θ₁+θ₂+θ₅)；r＝-sin(θ₁+θ₂+θ₅)；u＝0

p＝sin(θ₁+θ₂+θ₅)；s＝-cos(θ₁+θ₂+θ₅)；v＝0

q＝0；b＝0；w＝1

p_x＝a1*cos(θ₁)-a2*cos(θ₁+θ₂)+a3*cos(θ₁+θ₂)*cos(θ₃)

p_y＝a1*sin(θ₁)-a2*sin(θ₁+θ₂)+a3*sin(θ₁+θ₂)*cos(θ₃)

p_z＝d0+d1+d4+d5+a3*sin(θ₃)

其中：o、r、u、p、s、v、q、b、w、p_x、p_y、p_z，是治疗床躺板固定坐标系相对五轴治疗床基座全局坐标系的位置姿态及空间位置参数值，θ₁、θ₂、θ₅分别指的是第一个、第二个、第五个转动关节的转动角度；

(2.3)、根据步骤(2.2)位置变换矩阵得到各个关节的解析解：

2.3.1)根据矩阵中第三行第四列的元素(3，4)求得：

2.3.2)根据矩阵中第一行第四列的元素(1，4)与矩阵中第二行第四列的元素(2，4)求得：

cos(θ₁)＝(p_x+a2*cos(θ₁+θ₂)-a3*cos(θ₁+θ₂)*cos(θ₃))/a1

sin(θ₁)＝(p_y+a2*sin(θ₁+θ₂)-a3*sin(θ₁+θ₂)*cos(θ₃))/a1

其中：cos(θ₁)²+sin(θ₁)²＝1，上述两方程联立得到θ₁+θ₂；

2.3.3)根据矩阵中第一行第一列的元素(1，1)求得：

θ₁+θ₂+θ₅＝arc cos(-o)

其中：结合上述步骤求得的θ₁+θ₂，得到θ₅；

2.3.4)根据矩阵中第一行第四列的元素(1，4)，将上述步骤得到的θ₃、θ₅、θ₁+θ₂代入：θ₁＝arcos[(p_x+a2*cos(θ₁+θ₂)-a3*cos(θ₁+θ₂)*cos(θ₃))/a1]；

代入方程后可以求解得到θ₁，由于已知θ₁+θ₂，所以可以求解得到θ₂；

(3)、高效轨迹规划运算，其过程如下：

(3.1)、确定目标运动路径一—直线运动；

(3.2)、根据可达空间的中心缺陷，进行路径分割，路径分割为可达空间缺陷处的空间曲线运动、外围空间的直线运动，确定状态转接点P_T，进行运动方式的切换：

3.2.1)轨迹过程中间点：

P_mid(t)＝L(t)*(P_B-P_I)+P_I，t＝0.01*i，i∈[0，100]

其中，代表时间参数，

P_I指的是治疗床躺板的初始位置向量，

P_B指的是治疗床躺板的目标位置向量；

3.2.2)由于构型限制，五轴治疗床在起始点后的一段可能无法进行直线轨迹规划，通过从i＝0遍历，判断逆解的存在性，找到第一个逆解存在的点P_mid(t_T)，i＝i_T；

(3.3)、对于可达空间缺陷处的空间曲线运动：采用五次多项式进行插值，限定其速度、加速度由静止状态开始，并以静止状态结束：

θ(t)＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

公式中a₀到a₅指的是五次多项式的插值参数，由MATLAB robotic stoolbox自动得到，对于每个关节转角都会重新生成一组新的多项式插值参数，t指的是时间；

(3.4)、对于外围空间的直线运动：限定其速度、加速度由静止状态开始，并以静止状态结束，满足上述条件的直线运动方程的驱动变换函数：

L(t)＝(-sin(2*π*t-π)/(4*π²)-(1/(2*π))*t)*(-2*π)

dL(t)＝(-cos(2*π*t-π)/(2*π)-(1/(2*π)))*(-2*π)

ddL(t)＝sin(2*π*t-π)*(-2*π)

直线轨迹规划过程中间点：

P_mol(t)＝L(t)*(P_B-P_T)+P_T，t＝0.01*i，i∈[0，100]。