本发明涉及scara机器人关节力矩估计方法,特别是一种针对不使用力矩传感器情况下，以带噪声的电机电流信号为基本信息，基于卡尔曼滤波方法的提高关节力矩估计精度的方法。
背景技术：
scara机器人高速、高精度特性的实现依赖于有效的控制算法，而控制算法对关节力矩等控制信息的精确度有较高的要求。目前，关节力矩有两种常用的获取方式，通过力矩传感器获取或通过关节电机电流换算。
1)通过力矩传感器获取关节力矩。该方法的主要问题在于成本高以及安装维护困难。尤其是对于一些成品机器人，若其设计阶段并未考虑在关节处加装传感器，则其安装空间可能并不足够，同时也可能不具备相应的电气接口。
2)通过关节电机电流换算关节力矩。该方法的主要问题在于电流是模拟量，存在噪声，换算得到的关节力矩波动较大。目前常用的方法是通过硬件滤波或软件滤波对读取到的数据进行处理。但一些滤波方法的实时性并不能得到保证，从而影响控制算法的实现。
技术实现要素：
为了在不使用力矩传感器的情况下提高scara机器人控制算法中所需关节力矩信息的精度，本发明提供一种针对scara机器人关节力矩的卡尔曼估计方法。其技术方案是：
一种scara机器人关节力矩的卡尔曼估计方法，包括以下步骤：
步骤1：根据牛顿欧拉方法建立scara机器人的动力学模型，获得各关节的连续非线性力矩表达式；
步骤2：利用多元函数的一阶泰勒展开公式将连续非线性的关节力矩表达式转化为递推的线性离散关节力矩表达式；
步骤3：以线性离散化后的各关节力矩表达式为过程方程，并添加相应的量测方程，将过程方程和量测方程组合整理为各关节力矩的离散时间状态空间模型；
步骤4：按照基本卡尔曼滤波方法，结合关节力矩的离散时间状态空间模型，确定一个步进循环中需要用到的公式，利用读取到的关节电机电流信号和电机编码器信号实时估计scara机器人的关节力矩。
进一步地，步骤1中，在建立scara机器人的动力学模型时，将所述scara机器人的第三关节和第四关节合并，形成组合连杆，组合后的关节类型为移动关节，从而使scara机器人的动力学模型包括第一关节、第二关节、第三关节。
进一步地，步骤1中，所述各关节的连续非线性力矩表达式为：
式中，a1，a2，a3，a4是由scara机器人动力学可辨识参数集中的参数所组成的系数项，所述scara机器人动力学可辨识参数集由前期参数辨识工作获得；f11，f12，f21，f22，f31，f32，p5，jm3为可辨识参数集中的部分参数，其中，f11，f21，f31分别表示第一关节、第二关节、第三关节的库伦摩擦力系数，f12，f22，f32分别表示第一关节、第二关节、第三关节的粘滞摩擦力系数，p5为组合参数，jm3为第三关节电机的转动惯量；g为重力加速度，这里只表示大小，为-9.81m·s-2；
进一步地，所述步骤2具体包括：
步骤21：对所述各关节的连续非线性力矩表达式在k时刻分别进行多元函数一阶泰勒展开，可得到线性化的关节力矩递推公式：
式中，τ1,k，τ2,k，τ3,k分别表示k时刻第一关节，第二关节，第三关节的关节力估计矩值；τ1,k+1，τ2,k+1，τ3,k+1分别表示k+1时刻第一关节，第二关节，第三关节的关节力矩估计值；
步骤22：进一步将以上线性表达式改写可得：
τ1,k+1＝τ1,k+bu1,ku1,k+w1,k
τ2,k+1＝τ2,k+bu2,ku2,k+w2,k
τ3,k+1＝τ3,k+bu3,ku3,k+w3,k
式中，
w1,k，w2,k，w3,k为过程噪声。
进一步地，步骤3中，将过程方程和量测方程组合整理为各关节力矩的离散时间状态空间模型具体包括：
第一关节对应的状态空间模型：
τ1,k+1＝τ1,k+bu1,ku1,k+w1,k
y1,k＝τ1,k+v1,k
第二关节对应的状态空间模型：
τ2,k+1＝τ2,k+bu2,ku2,k+w2,k
y2,k＝τ2,k+v2,k
第三关节对应的状态空间模型：
τ3,k+1＝τ3,k+bu3,ku3,k+w3,k
y3,k＝τ3,k+v3,k
式中，v1,k，v2,k，v3,k为量测噪声。
进一步地，步骤4中，所述按照基本卡尔曼滤波方法，结合关节力矩的离散时间状态空间模型，确定一个步进循环中需要用到的公式具体包括：
步骤41：设线性系统的离散时间状态空间模型为：
xk+1＝fkxk+bukuk+bwkwk
yk＝ckxk+vk
式中，xk为状态向量；yk为量测向量；uk为输入向量；wk为过程噪声；vk为量测噪声；fk，buk，bwk，ck为对应的系数矩阵；下标中k表示k时刻；
步骤42：对于形如上式所示的状态空间模型，可按照如下方程完成一个步进循环的卡尔曼滤波估计：
式中，wk和vk分别是过程噪声和量测噪声的协方差矩阵，
相比现有技术，本发明益处在于：该方法考虑了机器人一侧的特性，对不使用传感器的scara机器人进行关节力矩信息估计时，将电机电流信息和机器人自身的动力学因素综合考虑，将电机电流信息与动力学模型结合起来对关节力矩进行估计，比起单纯应用硬件滤波或软件滤波更深入考虑了实际的应用背景，达到了提高力矩估计精度的目的，同时由于使用的是一种递推的估计方法，保证了机器人控制的实时性要求。
附图说明
图1为本实施例所涉及的scara机器人示意图。
图中所示：1-基座；2-第一关节臂；3-第二关节臂；4-第三关节和第四关节的组合关节臂。
具体实施方式
下面将结合附图和具体实施例对本发明做进一步的说明。
一种scara机器人关节力矩的卡尔曼估计方法，包括以下步骤：
步骤1：根据牛顿欧拉方法建立scara机器人的动力学模型，获得各关节的连续非线性力矩表达式；
步骤2：利用多元函数的一阶泰勒展开公式将连续非线性的关节力矩表达式转化为递推的线性离散关节力矩表达式；
步骤3：以线性离散化后的各关节力矩表达式为过程方程，并添加相应的量测方程，将过程方程和量测方程组合整理为各关节力矩的离散时间状态空间模型；
步骤4：按照基本卡尔曼滤波方法，结合关节力矩的离散时间状态空间模型，确定一个步进循环中需要用到的公式，利用读取到的关节电机电流信号和电机编码器信号实时估计scara机器人的关节力矩。
具体而言，步骤1中，在建立scara机器人的动力学模型时，如图1所示，所述的scara机器人包括基座1、第一关节臂2、第二关节臂3、第三关节和第四关节的组合关节臂4。第四关节的旋转运动会对第三关节的移动造成一定量的同方向耦合，考虑到第四关节的旋转运动对scara机器人主要的水平面定位运动不产生影响，为合理简化动力学方程，将所述scara机器人的第三关节和第四关节合并，形成组合连杆，组合后的关节类型为移动关节，从而使scara机器人的动力学模型包括第一关节、第二关节、第三关节。
具体而言，利用牛顿欧拉方法计算关节力矩的算法由两部分组成。第一部分是对每个连杆应用牛顿-欧拉方程，从第一连杆1到第n连杆向外迭代计算连杆的速度和加速度。第二部分是从第n连杆到第1连杆向内迭代计算出连杆间的相互作用力和力矩以及关节驱动力矩。此外，还需对考虑电机转子所需的力矩以及克服摩擦的力矩。最终总的关节力矩表达式包含驱动力矩，电机转子力矩和摩擦力矩三部分，因此，步骤1中，所述各关节的连续非线性力矩表达式为：
式中，a1，a2，a3，a4是由scara机器人动力学可辨识参数集中的参数所组成的系数项，所述scara机器人动力学可辨识参数集由前期参数辨识工作获得；f11，f12，f21，f22，f31，f32，p5，jm3为可辨识参数集中的部分参数，其中，f11，f21，f31分别表示第一关节、第二关节、第三关节的库伦摩擦力系数，f12，f22，f32分别表示第一关节、第二关节、第三关节的粘滞摩擦力系数，p5为组合参数，jm3为第三关节电机的转动惯量；g为重力加速度，这里只表示大小，为-9.81m·s-2；
具体而言，所述步骤2具体包括：
步骤21：对所述各关节的连续非线性力矩表达式在k时刻分别进行多元函数一阶泰勒展开，可得到线性化的关节力矩递推公式：
式中，τ1,k，τ2,k，τ3,k分别表示k时刻第一关节，第二关节，第三关节的关节力估计矩值；τ1,k+1，τ2,k+1，τ3,k+1分别表示k+1时刻第一关节，第二关节，第三关节的关节力矩估计值；
步骤22：进一步将以上线性表达式改写可得：
τ1,k+1＝τ1,k+bu1,ku1,k+w1,k
τ2,k+1＝τ2,k+bu2,ku2,k+w2,k
τ3,k+1＝τ3,k+bu3,ku3,k+w3,k
式中，
w1,k，w2,k，w3,k为过程噪声。
具体而言，步骤3中，将过程方程和量测方程组合整理为各关节力矩的离散时间状态空间模型具体包括：
第一关节对应的状态空间模型：
τ1,k+1＝τ1,k+bu1,ku1,k+w1,k
y1,k＝τ1,k+v1,k
第二关节对应的状态空间模型：
τ2,k+1＝τ2,k+bu2,ku2,k+w2,k
y2,k＝τ2,k+v2,k
第三关节对应的状态空间模型：
τ3,k+1＝τ3,k+bu3,ku3,k+w3,k
y3,k＝τ3,k+v3,k
式中，v1,k，v2,k，v3,k为量测噪声。
具体而言，步骤4中，所述按照基本卡尔曼滤波方法，结合关节力矩的离散时间状态空间模型，确定一个步进循环中需要用到的公式具体包括：
步骤41：设线性系统的离散时间状态空间模型为：
xk+1＝fkxk+bukuk+bwkwk
yk＝ckxk+vk
式中，xk为状态向量；yk为量测向量；uk为输入向量；wk为过程噪声；vk为量测噪声；fk，buk，bwk，ck为对应的系数矩阵；下标中k表示k时刻；
步骤42：对于形如上式所示的状态空间模型，可按照如下方程完成一个步进循环的卡尔曼滤波估计：
式中，wk和vk分别是过程噪声和量测噪声的协方差矩阵，
综上所述，本发明提供的估计方法首先根据牛顿欧拉方法建立scara机器人的动力学模型，获得一组关节力矩的连续非线性表达式；然后利用多元函数的一阶泰勒展开，将关节力矩表达式离散线性化；接着将关节力矩的离散线性表达式作为过程方程，并添加相应的量测方程，每个关节对应的两个方程组合并整理后可得到该关节力矩的离散时间状态空间模型；最后结合状态空间模型和卡尔曼滤波方法，利用每个控制周期读取到的电机电流信号和电机编码器信号，可以实时地对关节力矩进行估计，且实时性和估计精度都能满足机器人实际控制的要求。