基于动态逆模型的机器人轨迹跟随纠偏控制∗

林荣霞

（广东工业大学华立学院 广州 511325）

1 引言

随着人工智能技术和工业自动化技术的发展，机器人不断取代传统的机械作业模式，实现人工智能控制，提高了作业的精度，变刚度关节驱动机器人作为一种重要的仿生机器人，在执行复杂环境下的搜救、探测以及开采等工作中发挥重要作用。变刚度关节驱动机器人在进行行走和驱动过程中，受到环境扰动以及机械结构部件的影响，导致变刚度关节驱动机器人的轨迹和末端位姿容易出现偏移，需要对变刚度关节驱动机器人进行纠偏控制，提高变刚度关节驱动机器人的稳定性，研究变刚度关节驱动机器人的稳定控制模型，在整个机器人的稳定控制过程中具有很好的应用价值［1］。

变刚度关节驱动机器人控制系统采用集中式控制方法，即变刚度关节驱动机器人的全部控制由一台微型计算机完成，另一种是分散（级）式控制，即采用多台微机联合总线控制方法，进行轨迹跟随纠偏控制，传统方法中，主要的纠偏控制方法有自适应传感滤波控制方法、稳态积分控制方法、三轴陀螺调节控制方法等［2～3］，通过控制指令实现对机器人的陀螺、继电器、电磁阀及执行电机的自适应控制，提高对机器人的轨迹跟随纠偏控制能力。文献［4］中提出一种基于运动学解析模型的变刚度关节驱动机器人轨迹跟随纠偏控制方法，解决变刚度关节驱动机器人在行走、姿态变换过程中的轨迹偏移问题，提高控制过程的抗扰动能力，但该方法的控制计算开销较大，控制的实时性不好；文献［5］中提出一种基于三维空间末端位姿调节的机器人轨迹跟随纠偏控制方法，根据变刚度关节驱动机器人的运动部件对应的质心求得稳定性运动惯量参数，实现机器人的位姿和轨迹的自动调整，但该方法在控制过程中的抗扰动性和鲁棒性不好。针对上述问题，本文提出一种基于动态逆模型的机器人轨迹跟随纠偏方法。首先构建变刚度关节驱动机器人的D-H 参数和运动学模型，采用扩展卡尔曼滤波方法进行的机器人轨迹跟随参量采样和自适应融合处理，然后采用动态逆控制模型计算机器人的轨迹跟随误差，以机器人的几何空间位姿参数的调节对象，进行轨迹跟随纠偏控制，利用关节轴线间的几何关系求出模型中运动学参数，获得的连杆模型参数能及时反馈给变刚度关节驱动机器人的控制系统，实现控制律优化设计。最后进行仿真实验分析，展示了本文方法在实现机器人轨迹跟随纠偏控制方面的优化性能。

2 变刚度关节驱动机器人运动学分析和参量解算

2.1 变刚度关节驱动机器人运动学模型

在三维空间中构建变刚度关节驱动机器人的运动学解析模型，采用三轴加速度计、磁力计和三轴陀螺仪进行变刚度关节驱动机器人的运动参数采集，在笛卡尔空间中进行机器人的行为参数和动力学参数分析，在惯性参考系ΣI下，构建变刚度关节驱动机器人的运动解析模型，用4×4 的齐次坐标矩阵IT0(α0,β0,γ0)表示变刚度关节驱动机器人的运动部件的力学参数，为（≡IT0(θ1,θ2,θ3)），包含3方向的平移常量以及绕3 个轴的旋转，在机器人的动态逆分布模型中，分别用机器人的位姿偏移角α0，俯仰角β0和转向角γ0表示机器人的行为空间参数，其中，机器人的位姿偏移角α0是绕z 轴的旋转角，得到机器人轨迹跟随控制的伴随跟踪矩阵Rz(α0)表示为

设变刚度关节驱动机器人的末端效应器在笛卡尔空间的位姿、速度的自适应调节模态函数分别为pe、p˙e∈R6×1，确定线性补偿，得到机器人的轨迹跟随控制的角位置、角速度分别为θ、θ˙∈R10×1，β0是绕y 轴的旋转角，控制律导引曲线偏差矩阵Ry(β0)表示为

其中，γ0是绕x 轴的旋转角，引入机器人控制的滑模误差偏移角度，得到旋转矩阵Rx(γ0)表示为

在机器人的多状态随机分布特征空间中，采用多通道加权控制方法进行机器人的动态逆反馈调节，可得末端效应器坐标系Σe(≡Σ7)相对于惯性坐标系ΣI的齐次变换矩阵IT7(θ)为

构建变刚度关节驱动机器人的D-H 参数和运动学模型，得到变刚度关节驱动机器人从腰部到手臂末端效应器的位形θ 和末端效应器位姿pe的动态逆运动学关系式:

采用扩展卡尔曼滤波方法进行的机器人轨迹跟随参量采样和自适应融合处理［6］，由此构建变刚度关节驱动机器人轨迹跟随纠偏控制的误差反馈调节系统模型如图1所示。

2.2 机器人的轨迹分布位置参量解算

采用扩展卡尔曼滤波方法进行的机器人轨迹跟随参量采样和自适应融合处理，结合谐振比率控制方法进行机器人轨迹跟随的误差反馈调节［7］，构造最佳专家补偿函数:

在机器人的行为轨迹空间点的相机坐标系中，采用自适应参数调节方法，进行机器人轨迹的线性空间规划［8］，得到机器人轨迹跟随纠偏的等价非线性时变反馈系统为

图1 机器人轨迹跟随纠偏控制的误差反馈调节模型

引入质心运动状态方程，采用欠驱动步行稳定控制方法，进行机器人的前馈控制，采用动态逆模型进行机器人的轨迹分布位置自适应解算，在控制律S域中，得到:

在刚性均质杆控制下，机器人的轨迹纠偏误差状态转移概率函数为

根据变刚度关节驱动机器人的运动部件对应的质心求得稳定性运动惯量参数，当变刚度关节驱动机器人处于非加速运动状态时，将陀螺仪采集的原始数据进行滤波处理［9］。确定补偿器控制系数矩阵D，得到系统的稳态误差限定在:

当变刚度关节驱动机器人处于任意姿态时，通过惯性参量调节，使得H(s)严格正实，从而得到机器人的轨迹跟随误差调节的过程是全局渐进稳定的［10］，即

综上分析，采用动态逆控制模型计算机器人的轨迹跟随误差，以机器人的几何空间位姿参数的调节对象［11］，得到机器人轨迹跟随控制模糊约束方程为

式中，Km为变刚度关节驱动机器人的实际D-H 控制偏差，δ 为惯性扰动系数，θ 为滑模误差偏移角度，其中sin θp=θp，cos θp=1。

3 轨迹跟随纠偏控制优化

3.1 动态逆模型控制

在构建变刚度关节驱动机器人的D-H 参数和运动学模型，采用扩展卡尔曼滤波方法进行的机器人轨迹跟随参量采样和自适应融合处理的基础上，进行机器人的轨迹跟随纠偏控制优化，本文提出一种基于动态逆模型的机器人轨迹跟随纠偏方法，在变刚度关节驱动机器人初始零位状态下，以第一关节平面的法线作为基坐标系的Z 轴方向，第二关节平面的法线作为基坐标系的X 轴方向，得到变刚度关节驱动机器人末端位姿跟踪控制过程的动态逆调节的协方差矩阵为:

运用变刚度关节驱动机器人单关节独立运动控制的方法，取方差的上界作为调节系数，采用修正LESLIE-GOWER模型进行机器人的轨迹偏离修正，得到解向量的子空间w(k)与V(k)具有相关性，计算关节圆的拟合误差为

形成实际变刚度关节驱动机器人空间D-H 参数模型，得到变刚度关节驱动机器人的惯性力矩系数，引入了辅助矩阵，采用自适应误差调节方法进行反馈跟随识别，得到轨迹偏差修正的周期解为

运用动态跟踪原理，控制变刚度关节驱动机器人运行至空间任意位置［12］，得到动态逆模型控制的对偶问题可表达为

3.2 控制律优化

以机器人的几何空间位姿参数的调节对象，进行轨迹跟随纠偏控制，利用关节轴线间的几何关系求出模型中运动学参数为

获得的连杆模型参数，变刚度关节驱动机器人的控制模糊核函数为

获得的连杆模型参数及时反馈给变刚度关节驱动机器人，得到机器人的轨迹跟随纠偏二项式核为

根据k-1 时刻的姿态角预测，在变刚度关节驱动机器人运动学模型的基础上，本文采用圆周点法来测量机器人的轨迹跟踪的姿态偏移，得到偏移距离可以转化为

由此获得连杆模型参数，采用自适应参数调整方法将误差信息反馈给变刚度关节驱动机器人的控制系统，实现控制律优化设计。

4 仿真实验与结果分析

为了测试本文方法在实现机器人轨迹跟随纠偏控制中的应用性能，进行仿真实验，实验采用自主研发的Motoman UP6 型变刚度关节驱动机器人，控制算法设计采用Matlab 7 设计，设置机器人的行为轨迹偏差为50mm，在相机坐标系中的初始位置为（0，0.2，2），目标位置为（1，2.4，6），轨迹的最大偏差为1.5 mm，机器人的轨迹跟踪速度为12 mm/s，控制器的采样时间为0.01s。根据上述仿真环境和参数设定，进行机器人的控制仿真，得到机器人的动态参量采集结果如图2所示。

图2 参数采集输出

以图2 采集的数据作为机器人的轨迹跟随控制，采用动态逆控制模型计算机器人的轨迹跟随误差，以机器人的几何空间位姿参数的调节对象，进行轨迹跟随纠偏控制，得到轨迹纠偏仿真结果如图3所示。

图3 机器人轨迹纠偏控制仿真结果

图4 控制性能对比

分析图3 得知，采用本文方法进行机器人轨迹跟随的纠偏性能较好。测试不同方法进行机器人轨迹纠偏控制性能，得到控制性能曲线对比如图4所示。

分析图4 得知，采用本文方法进行机器人轨迹跟随纠偏的误差修正能力较好，纠偏控制能力较强，误差比传统方法降低12.7%，展示了较好的轨迹跟随纠偏控制性能。

5 结语

对变刚度关节驱动机器人进行纠偏控制，提高变刚度关节驱动机器人的稳定性，本文提出一种基于动态逆模型的机器人轨迹跟随纠偏方法。构建变刚度关节驱动机器人的D-H 参数和运动学模型，采用扩展卡尔曼滤波方法进行的机器人轨迹跟随参量采样和自适应融合处理，结合谐振比率控制方法进行机器人轨迹跟随的误差反馈调节，采用动态逆控制模型计算机器人的轨迹跟随误差，以机器人的几何空间位姿参数的调节对象，进行轨迹跟随纠偏控制，利用关节轴线间的几何关系求出模型中运动学参数，获得的连杆模型参数能及时反馈给变刚度关节驱动机器人的控制系统，实现控制律优化设计。研究得知，本文方法进行机器人控制的轨迹纠偏性能较好，误差较低，控制鲁棒性较高。