[发明专利]一种可运动解耦的连续体机器人控制方法

权利要求书

1.一种可运动解耦的连续体机器人控制方法，其特征在于，包括以下步骤：

建立机器人正运动学末端位姿矩阵；

所述机器人正运动学末端位姿矩阵如下：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z均为为齐次变换矩阵参数；其中，p_x、p_y、p_z表示机器人末端位置；n_x、n_y、n_z表示末端x轴的单位矢量；o_x、o_y、o_z表示末端y轴的单位矢量；a_x、a_y、a_z表示末端z轴的单位矢量；

通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量；

建立机器人整体运动学关系模型，具体如下：

其中，表示第i+1个坐标系O_i+1在第i个坐标系O_i的位姿转换矩阵，i＝0、1、…、4；Z_j表示第j个坐标系O_j的z轴，j＝0、1、…、4；d表示第0个坐标系沿其z轴方向运动的距离；L₂、L₄分别表示近端段与中间段之间的连接段长度、中间段与远端段之间的连接段长度；α₁、R₁、θ₁分别表示近端段的旋转角、近端段的弯曲半径和近端段的弯曲角，α₂、R₂、θ₂分别表示远端段的旋转角、远端段的弯曲半径和远端段的弯曲角，α₃表示末端执行器的旋转角；X₁₁、Y₁₁、Z₁₁分别表示第1个坐标系O₁绕其z轴旋转α₁之后得到新坐标系O₁₁的x轴、y轴和z轴，Z₁₂表示坐标系O₁₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₁₂的z轴；X₂₁、Y₂₁、Z₂₁分别表示第2个坐标系O₂绕其z轴旋转π+α₁-π/6之后得到新坐标系O₂₁的x轴、y轴和z轴，Z₂₂表示坐标系O₂₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₂₂的z轴；X₃₁、Y₃₁、Z₃₁分别表示第3个坐标系O₃绕其z轴旋转α₂之后得到新坐标系O₃₁的x轴、y轴和z轴，Z₃₂表示坐标系O₃₁沿其z轴运动R₂sinθ₂，再沿其x轴运动R₂(1-cosθ₂)得到的新坐标O₃₂的z轴；

将关节向量对驱动关节的驱动变量进行映射；

根据驱动变量对机器人进行控制。

2.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于：通过机器人整体运动学关系模型中的与机器人正运动学末端位姿矩阵T^E的相等关系，得到齐次变换矩阵参数n_z、o_x、o_y、o_z、a_z：