[发明专利]一种基于角度传感器的串并联机器人末端姿态闭环控制方法

权利要求书

1.一种基于角度传感器的串并联机器人末端姿态闭环控制方法，用于提高一种有两转三移五个自由度的串并联机器人的加工精度，其特征在于采用如下闭环控制系统，包括逆运动学模型(4)、控制器(1)、系统传函(5)、角度传感器(2)和数据处理单元(7)；给定位姿XYZAB(3)通过逆运动学模型(4)求解得到串并联机器人的关节变量J，然后传递到控制器(1)，控制器(1)的控制信号传递到传动机构的系统传函(5)得到串并联机器人实际位姿X1Y1Z1A1B1(6)；角度传感器(2)检测到串并联机器人末端姿态角A1B1，并传递到数据处理单元(7)得到关节变量误差ΔJ，ΔJ与J的差值作为输入值传递给控制器(1)，实现串并联机器人末端姿态的闭环控制；具体操作步骤如下：

1)为保证角度传感器测得的角度为末端坐标系相对于大地坐标系绕x轴，y轴的转角同时不影响末端执行器的安装，把该角度传感器安装在五个自由度串并联机器人第五轴与末端执行器连接法兰盘的安装槽中，在机器人运动过程中测得末端坐标系与大地坐标系x轴的夹角σ，与y轴的夹角θ；

2)角度传感器(2)测得的基于大地坐标系的两个角度参数，以矩阵形式描述当前末端执行器在大地坐标系下的位姿T^w，通过坐标转换得到末端执行器在串并联机器人基坐标系下的位姿矩阵T^o；

3)采用模块化方法，将并联部分作为一个模块，串并联机器人按照串联机器人建模方法建立运动学模型，通过串并联机器人逆运动学求解得到当前位姿下串并联机器人的实际关节变量q′₁，q′₂，q′₃，β′₁，β′₂，其中q′₁代表支链1(3′)的实际关节长度，q′₂代表支链2(5′)的实际关节长度，q′₃代表支链3(4′)的实际关节长度，β′₁代表旋转关节1(7′)的实际关节角度，β′₂代表旋转关节2(8′)的实际关节角度；

4)已知当前串并联机器人的关节控制变量q₁，q₂，q₃，β₁，β₂，求出当前位姿下的关节变量误差Δq₁，Δq₂，Δq₃，Δβ₁，Δβ₂，对各关节变量进行误差补偿，从而提高机器人的末端姿态精度，其中q₁代表支链1(3′)的理论关节长度，q₂代表支链2(5′)的理论关节长度，q₃代表支链3(4′)的理论关节长度，β₁代表旋转关节1(7′)的理论关节角度，β₂代表旋转关节2(8′)的理论关节角度，Δq_i＝q_i-q′_i，i＝1，2，3；Δβ_i＝β_i-β′_i，i＝1，2。