下肢康复训练机器人协作控制与S型规划算法研究

龚兰芳,曹成涛,许伦辉

(1.广东水利电力职业技术学院 自动化工程系，广州 510925; 2.广东交通职业技术学院 智能交通工程技术应用中心,广州 510650; 3.华南理工大学 土木与交通学院,广州 510640)

一般来说,下肢康复训练机器人对指数积(Product of Exponentials,POE)运动控制、协作控制、轨迹规划和超高速运动性能具有较高的要求[1-4].因此,对下肢康复机器人的运动学、动力学和轨迹规划进行研究，是提升康复类下肢康复训练机器人灵活性和灵敏度的重要保证[5].下肢康复机器人步态训练有两种主要方法:跑步机训练和可编程末端执行器训练.

本文首先研究了下肢康复训练机器人的训练原理,然后建立了POE运动学模型和动力学辨识模型[5].针对简化后的模型参数，采用最小二乘法辨识了动力学参数,从而实现了协作控制.最后在前面研究的基础上,提出了一种加加速度受限的非对称S型轨迹规划方法,给出了一种S型轨迹插补形状的完整算法,对提升下肢康复系统启动和停止速度、确保轨迹平滑等具有积极作用.与许多其他轨迹规划技术不同,保证了机器人运动的运动学约束,表示为所有机器人关节的速度、加速度和抖动的绝对值的上限.通过系统实验,验证协作控制算法和位置伺服系统下S型轨迹插补规划控制结果的正确性.

1 下肢康复机器人运动控制

图1为所研究的下肢康复机器人,它在肢体修复过程中能够通过编程方式加以控制,并且实现可变的预定步进姿势和力感测[6-8].下肢康复机器人可以帮助患者模拟正常人的脚步,锻炼腿部肌肉,逐渐恢复神经控制,从而帮助患者完成行走动作,最终以正常方式完全行走.

图1 柔性下肢康复机器人Fig.1 Flexible lower limb rehabilitation robot

1.1 POE正运动学

手臂运动学正解:串联开链机器人的正向运动学公式,对于n个转动/移动关节的串联机器人来说,设base0为基坐标系,为Tooln+1为工具坐标系,则应用POE公式计算机器人的正向运动学模型步骤如下:

步骤1计算机器人末端初始位姿

(1)

式中:Rst为姿态矩阵；Pst为刚体的位置矢量.

根据欧拉定理,任意一个三维空内的旋转运动都可以表示为围绕着某一个轴ω的转动,假设转动角度表示为θ,那么可以将该旋转矩阵表示为矩阵指数的形式,即

(2)

对于转动关节来说，q为转轴上任意一点的坐标,关节转动对应的运动旋量坐标表示为

则

对于移动关节来说,运动旋量中的ω对应分量为0,即

步骤3代入POE公式

(3)

1.2 机器人动力学建模

对于任何机械系统,拉格朗日函数L定义为系统的动能Ek和势能Ep之差:

系统动力学方程(称第二类拉格朗日方程)为

(4)

利用拉格朗日函数L,系统动力学方程可以表示为

由于势能Ek不显含速度项,因此，动力学方程也可以写成

(5)

1.2.1拉格朗日动力学

拉格朗日(动能与位能法):m1,m2,m3分别表示3个杆件质量,l1,l2,l3分别表示3个杆件长度,d1,d2,d3分别表示3个杆件质心到关节的距离.

设关节1上升高度为h,3个旋转关节旋转角度分别为θ1,θ2,θ3,q1=θ1,q2=θ2,q3=θ3,则康复机器人在运动平面内位置如图2所示.

图2 康复机器人在运动平面内位置Fig.2 The position of a rehabilitative robot in the motion plane

1.2.2机器人总势能和动能

康复机器人杆件1的势能P1为

康复机器人杆件2的势能P2为

康复机器人杆件3的势能P3

则康复机器人总位能为

(6)

康复机器人连杆1的动能K1为

康复机器人连杆2的动能K2为

康复机器人连杆3的动能K3为

式中：J1，J2，J3分别为连杆1，2，3的转动惯量.则康复机器人总动能为

(7)

1.2.3拉格朗日函数

拉格朗日函数L为

则康复机器人各关节驱动力矩为

式中:C1,C2,C3分别为3根连杆驱动电机的电势系数；S1，S2，S3分别为第1，2，3个关节施转角度的正弦值.

1.3 摩擦力矩拟合方法

根据对机器人的摩擦力矩Tf的测试,可以看出摩擦力矩的大小变化与机器人的重力矩、加速转矩和转动角速度的大小变化相关.为了简单的线性化拟合,试将摩擦力矩拟合公式确定如下:

(8)

计算角速度影响因数b,利用测量软件测得Tf,消除将Tf中的重力矩,则

此时,Tf只有角速度和加速转矩的影响,选取机器人平稳运行的时间段,在角速度平稳时进行影响因数b的拟合.

计算加速转矩影响因数c,在重力矩和角速度影响因数确定完成之后,简单运算即可求出加速转矩影响因数.

2 S型轨迹规划

机器人技术的一个基本问题在于轨迹规划,它可以这样定义:找出沿给定几何路径的时间运动规律,轨迹差补规划专用于为机器人的控制系统生成驱动器的参考输入,以便电动机能够执行该运动.在轨迹差补中,运动学和动力学约束是轨迹规划算法的输入,而关节(或末端效应器)的轨迹表示为位置、速度和加速度值的时间序列,是输出.

步骤1V为S型速度曲线的速度,J为S型速度曲线的加加速度,P为所走的路程,t为系统获得的机器人状态的相应时间序列.对于t0～t1

步骤2对于t1～t2

步骤3对于t2～t3

步骤4对于t3～t4

步骤5对于t4～t5

步骤6对于t5～t6

步骤7对于t6～t7

3 实验结果及分析

3.1 协作控制算法验证

本文在研发了柔性下肢康复机器人的动力学控制框架的基础上,验证软件编写的拉格朗日法的正确性.图3是使用机器人平台控制器展示协作控制算法,从图3(a)和图3(b)对比分析数据图可知,在机器人零力控制即不受重力和摩擦力的条件下,写作控制算法具有良好的效果,确实有效,从侧面验证了动力学算法和辨识出参数的准确性,可以有效地用到协作功能中去.

图3 柔性下肢康复机器人负载零力控制Fig.3 Load zero force control of flexible lower limb rehabilitation robot

3.2 轨迹规划实验

从图4可以看出S型轨迹规划的正确性.在关节空间中而不是在操作空间中规划轨迹具有一个主要优点,即控制系统作用于操纵器而不是作用于末端作用器,所以在联合工作空间内更容易实现运动轨迹的调整.此外,关节空间的轨迹规划可以避免由运动奇点和操纵引起的问题.但是,在关节空间中，进行轨迹规划中,在关节与末端作用器连接过程中,引入的非线性操作将会影响末端效应器动作执行的可预见性.S型轨迹规划要求伺服驱动通常采用位置控制,提高了电动机的速度和电流响应速度,进而改善了位置控制的效果.

图4 S型轨迹规划Fig.4 S-shaped trajectory planning diagram

4 结语

本文以下肢康复机器人为研究对象,进行了分析.针对下肢康复机器人的结构参数特点,首先运用POE运动控制求解了运动学问题,然后构建了拉格朗日动力学方程,同时提出了一种摩擦力矩拟合的方法来构建协作控制算法模型,最后讨论了S型轨迹规划的数学问题.规划方法可以获得较小的位置误差和较低的系统振动,可以通过S型规划插补产生平滑的轨迹,即具有良好的连续性特征的轨迹来实现.经过实验证明运动学、动力学与S规划算法对下肢康复机器人性能有很大的提升,同时对于机器人的精确运动控制有着重要的意义.下肢康复机器人初步实施被动者的被动动作和辅助动作,除了考虑运动模式,还要补充其他功能,下肢康复训练机器人将是一个更复杂的系统.