CN110385718B - 一种六自由度治疗床的运动控制方法 - Google Patents

Abstract

本发明公开了一种六自由度治疗床的运动控制方法，包括：治疗床的关节构型及数学模型、治疗床运动学的逆解算法、运动控制算法三个部分。所述的治疗床的关节构型及数学模型，给出了治疗床特殊的六个串联转动关节的构型，同时建立关节坐标系的数学模型；所述的治疗床运动学的逆解算法得到了治疗床终端空间位置坐标与六个转动关节角度之间的联系；所述运动控制算法给出了对应终端的空间运动的关节转动的控制方法。本发明以六轴串联机械臂结构为基础设计治疗床，并建立稳定完善的运动控制方法，满足了在治疗过程中的患者位姿的调整，保证了治疗计划的实现。

Description

一种六自由度治疗床的运动控制方法

技术领域

本发明涉及机械臂运动学技术领域，尤其涉及一种六自由度治疗床的运动控制方法。

背景技术

在癌症患者的射线放射治疗及离子束流轰击治疗中，常常需要调整患者的位姿与放射源治疗头相配合来完成治疗计划，以保证治疗的有效性及对患者正常组织的伤害最小。为实现患者位姿的实时调整，需要一种六自由治疗床来承载患者实现延三个坐标轴移动及转动的六自由度运动，同时为实现治疗床的精确位姿调整控制，相应的运动控制方法也是必不可少的。

发明内容

本发明目的就是为了弥补已有技术的缺陷，提供一种六自由度治疗床的运动控制方法。

本发明是通过以下技术方案实现的：

针对上述需求，本发明基于六轴串联机械臂的应用提出一种六自由度治疗床的运动控制算法，独特的关节构型实现终端六自由度的运动，采用D-H方法建立其数学模型，求得其运动学的正向及逆向的解。以此为基础，针对终端坐标系点到点的运动采用5次多项式实现转角关节空间的轨迹规划；对于终端轨迹的多点连续控制，采用多段三次样条曲线规划的方法实现关节转角的驱动，并采用遗传算法实现整体运动时间最短优化，保证了治疗床运动的快速响应。

一种六自由度治疗床的运动控制方法，具体包括以下内容：

(1)构建六自由度治疗床的关节构型及数学模型；

(2)基于治疗床的关节构型和数学模型，采用治疗床运动学的逆解算法，得到治疗床各个关节的转角；

(3)通过运动控制算法对治疗床进行运动控制。

所述的六自由度治疗床的关节构型为：包括有六个旋转关节，旋转关节一是由连接模块一分别连接基座和第一机械臂的一端构成的，旋转关节二是由连接模块二分别连接第一机械臂的另一端和第二机械臂的一端构成的，旋转关节三是由连接模块三分别连接第二机械臂的另一端和第三机械臂的一端构成的，旋转关节四是由连接模块四分别连接第三机械臂的另一端和第四机械臂的一端构成的，旋转关节五是由连接模块五分别连接第四机械臂的另一端和第五机械臂的一端构成的，旋转关节六是由连接模块六分别连接第五机械臂的另一端和承载平台的下端构成的，所述的连接模块一、连接模块二、连接模块三、连接模块四、连接模块五和连接模块六均是由减速器和驱动电机构成的，相邻的两机械臂通过与减速器上的法兰盘固定而依次串联起来，所述的基座与地面固定连接，所述的承载平台用于承载患者；

所述的数学模型的建立首先设定基座的坐标系，然后采用Denavit–Hartenberg方法依次建立各个转动关节的关节坐标系，同时定义描述坐标系j相对于坐标系j-1的位置和方向的四个参数如下：

θ_j：两轴x_j-1、轴x_j间的夹角，当绕轴z_j-1逆时针转动时取正，该参数为旋转关节的转动变量；

d_j：两坐标系延公垂线方向的距离，即轴x_j-1到轴x_j延轴z_j-1的距离；

α_j：两轴z_j-1、轴z_j间的夹角，当绕轴x_j逆时针转动时取正，该值是根据关节的设计构型确定的，当结构确定后即为定值；

a_j：两坐标系原点的距离，也是轴z_j-1到轴z_j沿x_j的距离。

所述的治疗床运动学的逆解算法，具体内容如下：

1)采用theta-alpha-d-a的齐次坐标转换顺序进行坐标系的转换,得到第j-1个坐标系到第j个坐标系时的齐次变换矩阵为：

A_j＝Rot(z,θ_j)Rot(x,α_j)Trans(0,0,d_j)Trans(a_j,0,0)

将上式中的旋转及平移矩阵依次相乘，得到单个齐次变换矩阵为：

将各个关节坐标系间的齐次变换矩阵依次右乘，得到终端坐标系

在基准坐标系中的位姿转换矩阵为：

其中[n o w]为终端坐标系的姿态向量，[p_x p_y p_z]为终端坐标系的位置坐标，得到正向运动学的解的各项表达式为：

上述式中，C12＝cos(θ₁+θ₂)，S12＝sin(θ₁+θ₂)，C1＝cosθ₁，S1＝sinθ₁，其它表达类似；

2)当终端坐标系的姿态向量[n o w]及位置坐标[p_x p_y p_z]已知时，根据步骤1)得到的转换矩阵中的各项等式方程组推得各个关节角度的解析解，根据终端坐标系的位置向量[p_x p_y p_z]，求得六个关节旋转角分别如下：

a)根据等式p_x，求得

θ₃＝-acos[(d₁-p_z)/d₃]

b)从等式p_x、p_y中提取出cos(θ₁)和sin(θ₂)并求和，得

k₁S12+k₂C12＝k₃

其中定义参数

k₁＝2p_y(d₃S3-a₃)

k₂＝2p_x(d₃S3-a₂)

则旋转关节角一和二的和为

c)将θ₁+θ₂代入p_x、p_y，定义参数

k₄＝sin(θ₁)＝(p_y-a₂S12+d₃S12S3)/a₁

k₅＝cos(θ₁)＝(p_x-a₂C12+d₃C12S3)/a₁

得到旋转关节一、二两转角为

θ₁＝atan2(k₄,k₅)

θ₂＝(θ₁+θ₂)-θ₁

定义矩阵R＝A₄A₅A₆及L＝inv(A₃)inv(A₂)inv(A₁)T_table，inv为求矩阵的逆矩阵，两矩阵对应项相等，有：

d)基于L₃₃＝R₃₃，定义参数

k₆＝-C5＝w_xS1S2S3-w_xC1C2S3-w_yC1S2S3-w_yC2S1S3-w_zC3

得到旋转关节五转角

θ₅＝acos(-k₆)

e)基于L₃₁+L₃₂＝R₃₁+R₃₂，有

k₇S6-k₇C6＝k₈

其中定义参数

k₇＝S5

k₈＝n_xS1S2S3-n_xC1C2S3-n_yC1S2S3-n_yC2S1S3+o_xS1S2S3-o_xC1C2S3-o_yC1S2S3-o_yC2S1S3-o_zC3-n_zC3

得到旋转关节六转角为

f)基于L₁₃+L₂₃＝R₁₃+R₂₃，有

k₇S4+k₇C4＝k₉

其中定义参数

k₉＝w_xC1C2C3-w_zS3+w_yC1C3S2+w_yC2C3S1-w_xC3S1S2+w_xS12-w_yC12

得到旋转关节四转角为

由此，根据治疗床终端坐标系的位姿得到对应的各关节的转角，逆向运动学的解即得出。

所述的运动控制算法具体内容如下：

1)通过逆解算法得到转动关节对应的离散点角度值时，采用5次多项式的插值方法实现空间点到点运动的关节角度运动规划：

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

上式中θ(t)为t时刻关节的转角值，b₀至b₅为待求系数；

根据运动的始末两点的位置θ_begin、θ_end，速度

加速度

要求及整体运动时长t_total，可以求得五次多项式的系数为

2)对于治疗床的空间路径有严格要求的运动(如直线、圆弧等)，将治疗床终端坐标系依次遍历的空间位姿点通过运动逆解算法求解出对应的关节空间中的多个离散的型值点，对于多个离散点通过连续的多段三次样条曲线插值来实现关节转角的连续规划：

S_i(t)＝c_i+e_i(t-t_i)+f_i(t-t_i)²+m_i(t-t_i)³

上式为第i段三次样条曲线的函数，其中t_i为第i段样条函数开始的时间，c_i、e_i、f_i、m_i为待求系数；

根据第i段曲线的始末两点在曲线上、任一中间点两边的两条曲线的一阶及二阶导数连续，可以得到曲线的系数为：

上述式中，θ_i和θ_i+1分别为第i段的始末两点坐标值，h_i为第i段曲线的整体时长，

及

为曲线始末两点处的二阶导数；

为了使治疗床的运动时间在满足转动速度及加速度约束的情况下最短，采用遗传算法来求解第i段的运动时长h_i，有运动时间最短求解模型：

s.t.max|S′_i(t)|≤v_max；max|S″_i(t)|≤a_max

其中v_max＝6°·s^-1；a_max＝18°·s^-2，s.t.是subject to的缩写，受约束于,公式里s是秒，t是时间。

所述的遗传算法，具体内容如下：

5.1)编码；采用浮点数编码；

5.2)初始种群，以最大允许转速经过两型值点间的转角值的时间h_imin为基准，给定每两个型值点间的时间步长范围，以此0.7～2.7倍的h_imin作为变量的随机取值范围，随机产生一定数量的个体组成种群，

h_i＝(0.7+rand*2)*h_imin

rand为产生的一个0～1之间的随机数。

5.3)个体适应度；种群中每个个体的适应度fitvalue_i由下式计算，其中以P为惩罚因子，当个体满足运动学要求时，P取值为0，当不满足要求时，P取值为10000；

popsize为种群中个体数量；

5.4)选择复制；采用赌轮盘的方法进行选择，且选择只在适应度排序在前一半的个体中进行，后一半个体直接淘汰，个体被选择的概率如下式

5.5)交叉；选用算术交叉算子，将种群中的个体随机两两配对，则两父代个体q₁、q₂与子代个体q`<sub>1</sub>、q`₂之间有如下关系，其中r为(0,1]之间的随机数

q′₁＝rq₁+(1-r)q₂；q′₂＝(1-r)q₁+rq₂

5.6)变异，变异采用非一致变异方式，变异后的个体q`_v可以由下式计算

其中，

和

分别为个体取值范围的上下界，g为当前迭代次数，变异量Δ(g,y)由下式计算，其中r为(0,1]区间的随机数，G为最大迭代次数，b为形状系数，b取值为2，s为个体与上下取值边界之间的差值；

Δ(g,s)＝s·r·(1-g/G)^b

种群数量为N＝40，迭代次数选定为G＝300，交叉概率Pc＝0.7，变异概率Pm＝0.1，U、L是上下限的缩写。

本发明的优点是：本发明通过六轴串联机械臂构型的治疗床承载患者并充当了患者在整体空间坐标系中定位的桥梁，同时其数学建模及正向运动学解实现了终端坐标系的精确定位，其逆向运动学解及运动规划方法实现了治疗床终端坐标系的实时高效的运动控制，完全满足了患者治疗过程中的位姿精确调整的功能要求。

附图说明

图1为本发明中提到的六自由度治疗床的关节构型。

图2为本发明中提到的六自由度治疗床的部件分解示意图。

图3为采用D-H方法建立的治疗床各个关节轴的坐标系示意图。

图4为本发明的治疗床采用theta-alpha-d-a的齐次坐标转换顺序得到的D-H参数图。

图5为本发明提及的运动控制方法的规划流程。

具体实施方式

一种六自由度治疗床的运动控制方法，具体包括以下内容：

(1)构建六自由度治疗床的关节构型及数学模型；

(2)基于治疗床的关节构型和数学模型，采用治疗床运动学的逆解算法，得到治疗床各个关节的转角；

(3)通过运动控制算法对治疗床进行运动控制。

如图1、2所示，所述的六自由度治疗床的关节构型为：包括有六个旋转关节，旋转关节一14是由连接模块一2分别连接基座1和第一机械臂3的一端构成的，旋转关节二15是由连接模块二4分别连接第一机械臂3的另一端和第二机械臂5的一端构成的，旋转关节三16是由连接模块三6分别连接第二机械臂5的另一端和第三机械臂7的一端构成的，旋转关节四17是由连接模块四8分别连接第三机械臂7的另一端和第四机械臂9的一端构成的，旋转关节五18是由连接模块五10分别连接第四机械臂9的另一端和第五机械臂11的一端构成的，旋转关节六19是由连接模块六12分别连接第五机械臂11的另一端和承载平台13的下端构成的，所述的连接模块一2、连接模块二4、连接模块三6、连接模块四8、连接模块五10和连接模块六12均是由减速器和驱动电机构成的，相邻的两机械臂通过与减速器上的法兰盘固定而依次串联起来，所述的基座1与地面固定连接；承载平台13用于承载患者。

如图3所示，所述的数学模型的建立首先设定基座的坐标系，然后采用Denavit–Hartenberg方法依次建立各个转动关节的关节坐标系，同时定义描述坐标系j相对于坐标系j-1的位置和方向的四个参数如下：

θ_j：两轴x_j-1、轴x_j间的夹角，当绕轴z_j-1逆时针转动时取正，该参数为旋转关节的转动变量；

d_j：两坐标系延公垂线方向的距离，即轴x_j-1到轴x_j延轴z_j-1的距离；

α_j：两轴z_j-1、轴z_j间的夹角，当绕轴x_j逆时针转动时取正，该值是根据关节的设计构型确定的，当结构确定后即为定值；

a_j：两坐标系原点的距离，也是轴z_j-1到轴z_j沿x_j的距离。

所述的治疗床运动学的逆解算法，具体内容如下：

如图4所示，1)采用theta-alpha-d-a的齐次坐标转换顺序进行坐标系的转换,得到第j-1个坐标系到第j个坐标系时的齐次变换矩阵为：

A_j＝Rot(z,θ_j)Rot(x,α_j)Trans(0,0,d_j)Trans(a_j,0,0)

将上式中的旋转及平移矩阵依次相乘，得到单个齐次变换矩阵为：

将各个关节坐标系间的齐次变换矩阵依次右乘，得到终端坐标系

在基准坐标系中的位姿转换矩阵为：

其中[n o w]为终端坐标系的姿态向量，[p_x p_y p_z]为终端坐标系的位置坐标，得到正向运动学的解的各项表达式为：

上述式中，C12＝cos(θ₁+θ₂)，S12＝sin(θ₁+θ₂)，C1＝cosθ₁，S1＝sinθ₁，其它表达类似；

2)当终端坐标系的姿态向量[n o w]及位置坐标[p_x p_y p_z]已知时，根据步骤1)得到的转换矩阵中的各项等式方程组推得各个关节角度的解析解，根据终端坐标系的位置向量[p_x p_y p_z]，求得旋转关节转角一、二、三如下：

a)根据等式p_x，求得

θ₃＝-acos[(d₁-p_z)/d₃]

b)从等式p_x、p_y中提取出cos(θ₁)和sin(θ₂)并求和，得

k₁S12+k₂C12＝k₃

其中定义参数

k₁＝2p_y(d₃S3-a₃)

k₂＝2p_x(d₃S3-a₂)

则旋转关节角一和二的和为

c)将θ₁+θ₂代入p_x、p_y，定义参数

k₄＝sin(θ₁)＝(p_y-a₂S12+d₃S12S3)/a₁

k₅＝cos(θ₁)＝(p_x-a₂C12+d₃C12S3)/a₁

得到旋转关节一、二两转角为

θ₁＝atan2(k₄,k₅)

θ₂＝(θ₁+θ₂)-θ₁

定义矩阵R＝A₄A₅A₆及L＝inv(A₃)inv(A₂)inv(A₁)T_table，inv为求矩阵的逆矩阵，两矩阵对应项相等，有：

d)基于L₃₃＝R₃₃，定义参数

k₆＝-C5＝w_xS1S2S3-w_xC1C2S3-w_yC1S2S3-w_yC2S1S3-w_zC3

得到旋转关节五转角

θ₅＝acos(-k₆)

e)基于L₃₁+L₃₂＝R₃₁+R₃₂，有

k₇S6-k₇C6＝k₈

其中定义参数

k₇＝S5

k₈＝n_xS1S2S3-n_xC1C2S3-n_yC1S2S3-n_yC2S1S3+o_xS1S2S3-o_xC1C2S3-o_yC1S2S3-o_yC2S1S3-o_zC3-n_zC3

得到旋转关节六转角为

f)基于L₁₃+L₂₃＝R₁₃+R₂₃，有

k₇S4+k₇C4＝k₉

其中定义参数

k₉＝w_xC1C2C3-w_zS3+w_yC1C3S2+w_yC2C3S1-w_xC3S1S2+w_xS12-w_yC12

得到旋转关节四转角为

由此，根据治疗床终端坐标系的位姿得到对应的各关节的转角，逆向运动学的解即得出。

如图5所示，所述的运动控制算法具体内容如下：

1)通过逆解算法得到转动关节对应的离散点角度值时，采用5次多项式的插值方法实现空间点到点运动的关节角度运动规划：

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

上式中θ(t)为t时刻关节的转角值，b₀至b₅为待求系数；

根据运动的始末两点的位置θ_begin、θ_end，速度

加速度

要求及整体运动时长t_total，可以求得五次多项式的系数为

2)对于治疗床的空间路径有严格要求的运动(如直线、圆弧等)，将治疗床终端坐标系依次遍历的空间位姿点通过运动逆解算法求解出对应的关节空间的中的多个离散的型值点，对于多个离散点通过连续的多段三次样条曲线插值来实现关节转角的连续规划：

S_i(t)＝c_i+e_i(t-t_i)+f_i(t-t_i)²+m_i(t-t_i)³

上式为第i段三次样条曲线的函数，其中t_i为第i段样条函数开始的时间，c_i、e_i、f_i、m_i为待求系数；

根据第i段曲线的始末两点在曲线上、任一中间点两边的两条曲线的一阶及二阶导数连续，可以得到曲线的系数为：

上述式中，θ_i和θ_i+1分别为第i段的始末两点坐标值，h_i为第i段曲线的整体时长，

及

为曲线始末两点处的二阶导数；

为了使治疗床的运动时间在满足转动速度及加速度约束的情况下最短，采用遗传算法来求解第i段的运动时长h_i，有运动时间最短求解模型：

s.t.max|S′_i(t)|≤v_max；max|S″_i(t)|≤a_max

其中v_max＝6°·s^-1；a_max＝18°·s^-2，s.t.是subject to的缩写，受约束于，公式里s是秒，t是时间。

所述的遗传算法，具体内容如下：

5.1)编码；采用浮点数编码；

5.2)初始种群，以最大允许转速经过两型值点间的转角值的时间h_imin为基准，给定每两个型值点间的时间步长范围，以此0.7～2.7倍的h_imin作为变量的随机取值范围，随机产生一定数量的个体组成种群，

h_i＝(0.7+rand*2)*h_imin

rand为产生的一个0～1之间的随机数。

5.3)个体适应度；种群中每个个体的适应度fitvalue_i由下式计算，其中以P为惩罚因子，当个体满足运动学要求时，P取值为0，当不满足要求时，P取值为10000；

popsize为种群中个体数量；

5.4)选择复制；采用赌轮盘的方法进行选择，且选择只在适应度排序在前一半的个体中进行，后一半个体直接淘汰，个体被选择的概率如下式

5.5)交叉；选用算术交叉算子，将种群中的个体随机两两配对，则两父代个体q₁、q₂与子代个体q`<sub>1</sub>、q`₂之间有如下关系，其中r为(0,1]之间的随机数

q′₁＝rq₁+(1-r)q₂；q'₂＝(1-r)q₁+rq₂

5.6)变异，变异采用非一致变异方式，变异后的个体q`_v可以由下式计算

其中，

和

分别为个体取值范围的上下界，g为当前迭代次数，变异量Δ(g,y)由下式计算，其中r为(0,1]区间的随机数，G为最大迭代次数，b为形状系数，b取值为2，s为个体与上下取值边界之间的差值；

Δ(g,s)＝s·r·(1-g/G)^b

种群数量为N＝40，迭代次数选定为G＝300，交叉概率Pc＝0.7，变异概率Pm＝0.1，U、L是上下限的缩写。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (3)

1.一种六自由度治疗床的运动控制方法，其特征在于：具体包括以下内容：

(1)构建六自由度治疗床的关节构型及数学模型；

(2)基于治疗床的关节构型和数学模型，采用治疗床运动学的逆解算法，得到治疗床各个关节的转角；

(3)通过运动控制算法对治疗床进行运动控制；

所述的六自由度治疗床的关节构型为：包括有六个旋转关节，旋转关节一是由连接模块一分别连接基座和第一机械臂的一端构成的，旋转关节二是由连接模块二分别连接第一机械臂的另一端和第二机械臂的一端构成的，旋转关节三是由连接模块三分别连接第二机械臂的另一端和第三机械臂的一端构成的，旋转关节四是由连接模块四分别连接第三机械臂的另一端和第四机械臂的一端构成的，旋转关节五是由连接模块五分别连接第四机械臂的另一端和第五机械臂的一端构成的，旋转关节六是由连接模块六分别连接第五机械臂的另一端和承载平台的下端构成的，所述的连接模块一、连接模块二、连接模块三、连接模块四、连接模块五和连接模块六均是由减速器和驱动电机构成的，相邻的两机械臂通过与减速器上的法兰盘固定而依次串联起来，所述的基座与地面固定连接；

所述的数学模型的建立首先设定基座的坐标系，然后采用Denavit–Hartenberg方法依次建立各个转动关节的关节坐标系，同时定义描述坐标系j相对于坐标系j-1的位置和方向的四个参数如下：

θ_j：两轴x_j-1、轴x_j间的夹角，当绕轴z_j-1逆时针转动时取正，该参数为旋转关节的转动变量；

d_j：两坐标系延公垂线方向的距离，即轴x_j-1到轴x_j延轴z_j-1的距离；

α_j：两轴z_j-1、轴z_j间的夹角，当绕轴x_j逆时针转动时取正，该值是根据关节的设计构型确定的，当结构确定后即为定值；

a_j：两坐标系原点的距离，也是轴z_j-1到轴z_j沿x_j的距离；

所述的治疗床运动学的逆解算法，具体内容如下：

1)采用theta-alpha-d-a的齐次坐标转换顺序进行坐标系的转换,得到第j-1个坐标系到第j个坐标系时的齐次变换矩阵为：

A_j＝Rot(z,θ_j)Rot(x,α_j)Trans(0,0,d_j)Trans(a_j,0,0)

将上式中的旋转及平移矩阵依次相乘，得到单个齐次变换矩阵为：

将各个关节坐标系间的齐次变换矩阵依次右乘，得到终端坐标系在基准坐标系中的位姿转换矩阵为：

其中[n o w]为终端坐标系的姿态向量，[p_x p_y p_z]为终端坐标系的位置坐标，得到正向运动学的解的各项表达式为：

p_z＝d₁-d₃C3

上述式中，C12＝cos(θ₁+θ₂)，S12＝sin(θ₁+θ₂)，C1＝cosθ₁，S1＝sinθ₁，其它表达类似；

2)当终端坐标系的姿态向量[n o w]及位置坐标[p_x p_y p_z]已知时，根据步骤1)得到的转换矩阵中的各项等式方程组推得各个关节角度的解析解，根据终端坐标系的位置向量[p_x p_y p_z]，求得六个关节旋转角分别如下：

a)根据等式p_x，求得

θ₃＝-acos[(d₁-p_z)/d₃]

b)从等式p_x、p_y中提取出cos(θ₁)和sin(θ₂)并求和，得

k₁S12+k₂C12＝k₃

其中定义参数

k₁＝2p_y(d₃S3-a₃)

k₂＝2p_x(d₃S3-a₂)

则旋转关节角一和二的和为

c)将θ₁+θ₂代入p_x、p_y，定义参数

k₄＝sin(θ₁)＝(p_y-a₂S12+d₃S12S3)/a₁

k₅＝cos(θ₁)＝(p_x-a₂C12+d₃C12S3)/a₁

得到旋转关节一、二两转角为

θ₁＝atan2(k₄,k₅)

θ₂＝(θ₁+θ₂)-θ₁

定义矩阵R＝A₄A₅A₆及L＝inv(A₃)inv(A₂)inv(A₁)T_table，inv为求矩阵的逆矩阵，两矩阵对应项相等，有：

d)基于L₃₃＝R₃₃，定义参数

k₆＝-C5＝w_xS1S2S3-w_xC1C2S3-w_yC1S2S3-w_yC2S1S3-w_zC3

得到旋转关节五转角

θ₅＝acos(-k₆)

e)基于L₃₁+L₃₂＝R₃₁+R₃₂，有

k₇S6-k₇C6＝k₈

其中定义参数

k₇＝S5

k₈＝n_xS1S2S3-n_xC1C2S3-n_yC1S2S3-n_yC2S1S3+o_xS1S2S3-o_xC1C2S3-o_yC1S2S3-o_yC2S1S3-o_zC3-n_zC3

得到旋转关节六转角为

f)基于L₁₃+L₂₃＝R₁₃+R₂₃，有

k₇S4+k₇C4＝k₉

其中定义参数

k₉＝w_xC1C2C3-w_zS3+w_yC1C3S2+w_yC2C3S1-w_xC3S1S2+w_xS12-w_yC12

得到旋转关节四转角为

由此，根据治疗床终端坐标系的位姿得到对应的各关节的转角，逆向运动学的解即得出。

2.根据权利要求1所述的一种六自由度治疗床的运动控制方法，其特征在于：所述的运动控制算法具体内容如下：

1)通过逆解算法得到转动关节对应的离散点角度值时，采用5次多项式的插值方法实现空间点到点运动的关节角度运动规划：

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

上式中θ(t)为t时刻关节的转角值，b₀至b₅为待求系数；

根据运动的始末两点的位置θ_begin、θ_end，速度

加速度

要求及整体运动时长t_total，可以求得五次多项式的系数为

2)对于治疗床的空间路径有严格要求的运动，将治疗床终端坐标系依次遍历的空间位姿点,通过运动逆解算法，求解出其对应的关节空间中的多个离散的角度值，，对于多个离散点通过连续的多段三次样条曲线插值来实现关节转角的连续规划：

S_i(t)＝c_i+e_i(t-t_i)+f_i(t-t_i)²+m_i(t-t_i)³

上式为第i段三次样条曲线的函数，其中t_i为第i段样条函数开始的时间，c_i、e_i、f_i、m_i为待求系数；

根据第i段曲线的始末两点在曲线上、任一中间点两边的两条曲线的一阶及二阶导数连续，可以得到曲线的系数为：

上述式中，θ_i和θ_i+1分别为第i段的始末两点坐标值，h_i为第i段曲线的整体时长，

及

为曲线始末两点处的二阶导数；

为了使治疗床的运动时间在满足转动速度及加速度约束的情况下最短，采用遗传算法来求解第i段的运动时长h_i，有运动时间最短求解模型：

s.t.max|S′_i(t)|≤v_max；max|S″_i(t)|≤a_max

其中v_max＝6°·s^-1；a_max＝18°·s^-2。

3.根据权利要求2所述的一种六自由度治疗床的运动控制方法，其特征在于：所述的遗传算法，具体内容如下：

3.1)编码；采用浮点数编码；

3.2)初始种群，以最大允许转速经过两型值点间的转角值的时间h_imin为基准，给定每两个型值点间的时间步长范围，以此0.7～2.7倍的h_imin作为变量的随机取值范围，随机产生一定数量的个体组成种群，

h_i＝(0.7+rand*2)*h_imin

rand为产生的一个0～1之间的随机数；

3.3)个体适应度；种群中每个个体的适应度fitvalue_i由下式计算，其中以P为惩罚因子，当个体满足运动学要求时，P取值为0，当不满足要求时，P取值为10000；

popsize为种群中个体数量；

3.4)选择复制；采用赌轮盘的方法进行选择，且选择只在适应度排序在前一半的个体中进行，后一半个体直接淘汰，个体被选择的概率如下式

3.5)交叉；选用算术交叉算子，将种群中的个体随机两两配对，则两父代个体q₁、q₂与子代个体q`<sub>1</sub>、q`₂之间有如下关系，其中r为(0,1]之间的随机数

q′₁＝rq₁+(1-r)q₂；q′₂＝(1-r)q₁+rq₂

3.6)变异，变异采用非一致变异方式，变异后的个体q`_v可以由下式计算

其中，

和

分别为个体取值范围的上下界，g为当前迭代次数，变异量Δ(g,y)由下式计算，其中r为(0,1]区间的随机数，G为最大迭代次数，b为形状系数，b取值为2，s为个体与上下取值边界之间的差值；

Δ(g,s)＝s·r·(1-g/G)^b

种群数量为N＝40，迭代次数选定为G＝300，交叉概率Pc＝0.7，变异概率Pm＝0.1，U、L是上下限的缩写。

Images