一种基于角度传感器的串并联机器人末端姿态闭环控制方法与流程

本发明涉及一种基于角度传感器的串并联机器人末端姿态闭环控制方法。

背景技术：

在现代的自动化生产中机器人占据着及其重要的地位。相比于传统的机床加工，机器人有着更好的灵活性和更大的运动空间。但是机器人的精度相对较低，在一些精密件的加工领域机器人还不能取代机床得到很好的应用。一些精度要求较高，运动空间要求较大的场合就需要在应用机器人的情况下尽可能提高其加工精度。串并联机器人结合了并联机器人和串联机器人的优点，加工精度较高，运动空间较大。在此基础上运用闭环控制方法，对机器人位姿进行误差补偿，可有效提高机器人精度，满足更多的应用需求。

目前机器人的控制方法主要有两种：半闭环控制和闭环控制。半闭环控制方法通过电机编码器进行误差补偿，只能补偿电机的转动误差，不能有效的提高末端位姿精度。在此基础上，在关节传动件的末端添加光电编码器进行误差补偿，可消除传动件的传动误差，提高末端位姿精度。但此方法需要多个光电编码器，而且只能补偿传动误差，对于一些结构参数的误差无法消除。本发明在末端法兰盘安装角度传感器，通过测量得到实际末端位姿并基于运动学进行误差补偿，从全局进行误差补偿可有效提高机器人加工精度。

技术实现要素：

本发明的目的在于针对已有技术存在的缺陷，提供一种基于角度传感器的串并联机器人末端姿态闭环控制方法，为达到上述目的，本发明采用全局进行误差补偿而有效提高机器人的加工精度。本发明的构思是：本发明将角度传感器安装在串并联机器人末端法兰盘的安装槽中，并用螺钉固定，测量得到机器人在大地坐标系下绕x轴和绕z轴的姿态角。通过坐标转化，大地坐标系下的姿态角转化为机器人基坐标系下的的位姿矩阵。然后根据串并联机器人的运动学模型，通过逆运动学求解得到当前位姿下机器人的实际关节变量，从而得到各关节的关节误差。对关节误差进行补偿，实现串并联机器人末端位姿的闭环控制。该方法基于角度传感器实现闭环控制，集成方便，不影响末端执行器的安装，能够提高机器人的末端姿态精度，适用于一些对末端姿态精度要求较高的场合。

如图1所示。给定机器人某一点位姿，通过逆运动学求解得到关节变量j。关节变量j传递给控制器，然后带动伺服系统和传动系统执行运动命令。其中，系统传函包括伺服系统和传动系统的传递函数。在机器人运动的过程中，角度传感器测得当前末端姿态a1,b1。然后通过坐标转化得到基于串并联机器人基坐标系的末端位姿，逆运动学求解得到当前关节变量，从而得到关节误差δj。数据处理单元包括坐标转化，逆运动学求解，各关节误差求解。求得的关节误差补偿到关节变量中，便可提高机器人的精度。(j＝[q1,q2,q3,β1,β2]^t,δj＝[δq1,δq2,δq3,δβ1,δβ2]^t)

根据上述发明构思，本发明采用下述技术方案：

一种基于角度传感器的串并联机器人末端姿态闭环控制方法，其特征在于采用如下闭环控制系统，包括逆运动学模型(4)、控制器(1)、系统传函(5)、角度传感器(2)和数据处理单元(7)；给定位姿xyzab(3)通过逆运动学模型(4)求解得到串并联机器人的关节变量j,然后传递到控制器(1)，控制器(1)的控制信号传递到传动机构的系统传函(3)得到串并联机器人实际位姿x1y1z1a1b1(6)；角度传感器(2)检测到串并联机器人末端姿态角a1b1，并传递到数据处理单元(7)得到关节变量误差δj，δj与j的差值作为输入值传递给控制器(1)，实现串并联机器人末端姿态的闭环控制；具体操作步骤如下：

1)将角度传感器(2)安装在串并联机器人末端法兰盘的安装槽(a)中，并用螺钉固定，在机器人运动过程中测得末端坐标系与大地坐标系x轴的夹角σ、与y轴的夹角θ；

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

3)建立运动学模型，通过逆运动学求解得到当前位姿下串并联机器人的实际关节变量q′1,q′2,q′3,β′1,β′2，其中q′1代表支链1(3′)的实际关节长度，q′2代表支链2(5′)的实际关节长度，q′3代表支链3(4′)的实际关节长度，β′1代表旋转关节1(7′)的实际关节角度，β′2代表旋转关节2(8′)的实际关节角度；

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

根据以上步骤所述，测量末端姿态需要用到角度传感器。该串并联机器人有五个自由度(两转三移)，为了测量末端执行器的姿态可选用高精度双轴倾角仪。通过该倾角仪可测得末端坐标系相对于大地坐标系x轴，y轴的夹角。

该倾角仪分辨率为0.0005°，精度0.002°，输入电压可选11-35v的直流电，输出频率可选5-100hz，产品输出可选rs232，rs485，和can总线。

为了保证倾角仪测得的角度为末端坐标系相对于大地坐标系x轴，y轴的夹角，在该串并联机器人上把倾角仪安装在五轴与末端执行器连接的法兰盘上，并保证倾角仪在零度角的情况下与大地水平。

倾角仪尺寸为103.8*55.4*26mm，安装孔直径4mm*3。在连接法兰上开一个110*60*30mm的槽，并用螺钉固定安装在槽中。如图3所示，a为角度传感器的安装槽，b为角度传感器的安装孔，c为角度传感器工作原理示意。

根据步骤2)所述，角度传感器测得的基于大地坐标系的两个角度参数(σ、θ)需要通过坐标转化转换为末端执行器在串并联机器人基坐标系下的位姿矩阵。如图4所示，a1a2a3为定平台，b1b2b3为动平台，q1,q2,q3为三根直动链关节变量，beta1，beta2为两个转动关节变量。混联机器人的基坐标系为o-x1y1z1,大地坐标系为o-x2y2z2,角度传感器本身的坐标系为o-x3y3z3,其坐标系方向与大地坐标系方向相同。

末端执行器在大地坐标系下的位置坐标为(xw,yw,zw)，转换方法如下：

①大地坐标系下的位姿矩阵t^w:

②转换矩阵

该转换矩阵在不同情况下有不同的使用情况。当机器人基坐标系与大地坐标系方向一致时为单位矩阵；当机器人基坐标系与大地坐标系不一致时，根据基坐标系的改变而改变，表示大地坐标系转化为基坐标系的变化矩阵。

③基坐标系下的位姿矩阵t^o：

根据步骤3)所述，串并联机器人基坐标系下的当前位姿需要通过逆运动学求解得到当前位姿下的关节变量。本发明中运动学求解采用模块化方法，并联部分作为一个模块，可用一个变换矩阵t0表示其末端位姿。然后就可按照串联机器人运动学建模方法对串并联机器人进行运动学建模。通过建立dh模型，串并联机器人串联1关节的变化矩阵t1，串联2关节变化矩阵t2，末端位姿矩阵th：

th＝t0*t1*t2

公式两段各右乘以t2的逆矩阵，可求得两个串联关节得关节变量β′1，β′2，并联结构的末端参数变量b1(动平台绕y轴的转角)，c1(动平台绕x轴的转角)，z1(动平台沿z轴的移动)。

然后采用中间参数法求解并联机构的三个直动关节变量q′1,q′2,q′3。

本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著技术进步：

1)本发明采用全局进行误差补偿，实现串并联机器人末端姿态的闭环控制，有效提高了机器人的加工精度。

2)本发明基于角度传感器实现串并联机器人末端姿态的闭环控制，集成方便，不影响末端执行器的安装。

3)本发明主要对串并联机器人末端姿态进行误差补偿，能够提高串并联机器人的末端姿态精度，对于一些对末端姿态精度要求较高的场合有一定的优势。

附图说明

图1是串并联机器人闭环控制框图

图2是闭环控制方法流程示意图

图3是角度传感器安装示意图

图4是串并联机器人坐标转换示意图

具体实施方式

本发明的优选实施例结合附图详述如下：

实施例一：

本基于角度传感器的串并联机器人末端姿态闭环控制方法，其特征在于采用如下闭环控制系统，包括逆运动学模型(4)、控制器(1)、系统传函(5)、角度传感器(2)和数据处理单元(7)；给定位姿xyzab(3)通过逆运动学模型(4)求解得到串并联机器人的关节变量j,然后传递到控制器(1)，控制器(1)的控制信号传递到传动机构的系统传函(3)得到串并联机器人实际位姿x1y1z1a1b1(6)；角度传感器(2)检测到串并联机器人末端姿态角a1b1，并传递到数据处理单元(7)得到关节变量误差δj，δj与j的差值作为输入值传递给控制器(1)，实现串并联机器人末端姿态的闭环控制；具体操作步骤如下：

1)将角度传感器(2)安装在串并联机器人末端法兰盘的安装槽(a)中，并用螺钉固定，在机器人运动过程中测得末端坐标系与大地坐标系x轴的夹角σ、与y轴的夹角θ；

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

3)建立运动学模型，通过逆运动学求解得到当前位姿下串并联机器人的实际关节变量q′1,q′2,q′3,β′1,β′2，其中q′1代表支链1(3′)的实际关节长度，q′2代表支链2(5′)的实际关节长度，q′3代表支链3(4′)的实际关节长度，β′1代表旋转关节1(7′)的实际关节角度，β′2代表旋转关节2(8′)的实际关节角度；

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

实施例二：本实施例与实施例一基本相同，特别之处如下：

所述角度传感器(2)选用高精度双轴倾角仪，用该双轴倾角仪测得末端坐标系相对于大地坐标系绕x轴，y轴的转角；为保证倾角仪测得的角度为末端坐标系相对于大地坐标系绕x轴，y轴的转角，把该倾角仪安装在有五个自由度—两转三移的串并联机器人五轴与末端执行器连接的法兰盘的安装槽中，用螺钉固定，保证其安装精度，并保证倾角仪在零度角的情况下与大地水平。

所述步骤2)中，角度传感器(2)测得的基于大地坐标系的两个角度参数σ、θ，需要通过坐标转化转换为末端执行器在串并联机器人基坐标系下的位姿矩阵；末端执行器在大地坐标系下的位置坐标为(xw,yw,zw)，转换方法如下：

①大地坐标系下的位姿矩阵t^w:

②转换矩阵

当机器人基坐标系与大地坐标系方向一致时为单位矩阵；当机器人基坐标系与大地坐标系不一致时根据基坐标系的改变而改变，表示大地坐标系转化为基坐标系的变化矩阵；

③基坐标系下的位姿矩阵t^o：

实施例三：

本实施例的具体实施方案如下：如图1所示，给定机器人某一点位姿，通过逆运动学求解得到关节变量j。关节变量j传递给控制器，然后带动伺服系统和传动系统执行运动命令。其中，系统传函包括伺服系统和传动系统的传递函数。在机器人运动的过程中，角度传感器测得当前末端姿态a1,b1。然后通过坐标转化得到基于串并联机器人基坐标系的末端位姿，逆运动学求解得到当前关节变量，从而得到关节误差δj。数据处理单元包括坐标转化，逆运动学求解，关节误差求解。求得的关节误差补偿到关节变量中，便可提高机器人的精度。(j＝[q1,q2,q3,β1,β2]^t,δj＝[δq1,δq2,δq3,δβ1,δβ2]^t)

具体实施步骤如下所述：

第一步：将角度传感器安装在串并联机器人末端法兰盘上，并用螺钉固定，在机器人运动过程中可测得末端坐标系与大地坐标系x轴地夹角(σ)、与y轴的夹角(θ)。

如图3所示，串并联机器人由基座(1′)、定平台(2′)、支链1(3′)、支链2(5′)、支链3(4′)、动平台(6′)、旋转关节1(7′)和旋转关节2(8′)组成。其中a为角度传感器的安装槽，b为角度传感器的3个安装孔，c为角度传感器工作原理示意。

该串并联机器人有五个自由度(两转三移)，为了测量末端执行器的姿态选用高精度双轴倾角仪。通过该倾角仪测得机器人末端坐标系相对于大地坐标系x轴，y轴的夹角。

该倾角仪分辨率为0.0005°，精度0.002°，输入电压可选11-35v的直流电，输出频率可选5-100hz，产品输出可选rs232，rs485，和can总线。

为了保证倾角仪测得的角度为执行器相对于大地坐标系x轴，y轴的夹角，在该串并联机器人上把倾角仪安装在五轴与末端执行器连接的法兰盘的安装槽中，并保证倾角仪在零度角的情况下与大地水平。

倾角仪尺寸为103.8*55.4*26mm，安装孔直径4mm*3。在连接法兰上开一个110*60*30mm的槽a，并用螺钉固定安装在槽中。

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

如图4所示，a1a2a3为定平台，b1b2b3为动平台，q1,q2,q3为三根直动链关节变量，beta1，beta2为两个转动关节变量。串并联机器人的基坐标系为o-x1y1z1,大地坐标系为o-x2y2z2,角度传感器本身的坐标系为o-x3y3z3,其坐标系方向与大地坐标系方向相同。

末端执行器在大地坐标系下的位置坐标为(xw,yw,zw)，转换方法如下：

④大地坐标系下的位姿矩阵t^w:

①转换矩阵

该转换矩阵在不同情况下有不同的使用情况。当串并联机器人基坐标系与大地坐标系方向一致时为单位矩阵；当机器人基坐标系与大地坐标系不一致时根据基坐标系的改变而改变，表示大地坐标系转化为基坐标系的变化矩阵。

②基坐标系下的位姿矩阵t^o：

第三步：建立运动学模型，通过逆运动学求解可得到当前位姿下混联机器人的实际关节变量q′1,q′2,q′3,β′1,β′2。

本发明中运动学求解采用模块化方法，并联部分作为一个模块，可用一个变换矩阵t0表示其末端位姿。然后就可按照串联机器人运动学建模方法对串并联机器人进行运动学建模。通过建立dh模型，串并联机器人串联1关节的变化矩阵t1，串联2关节变化矩阵t2，末端位姿矩阵th：

th＝t0*t1*t2

公式两段各右乘以t2的逆矩阵，可求得两个串联关节得关节变量β′1，β′2，并联结构的末端参数变量b1(动平台绕y轴的转角)，c1(动平台绕x轴的转角)，z1(动平台沿z轴的移动)。

然后采用中间参数法求解并联机构的三个直动关节变量q′1,q′2,q′3。

第四步：已知当前串并联机器人的关节控制变量q1,q2,q3,β1,β2，可求出当前位姿下的关节变量误差δq1,δq2,δq3,δβ1,δβ2，对关节变量进行误差补偿，从而提高机器人的末端姿态精度。