[发明专利]一种柔性机器人动力学建模及轨迹跟随控制方法及装置

说明书

本发明公开了一种柔性机器人动力学建模及轨迹跟随控制方法及装置，根据“离散式刚性连杆+联动机构”构型绳索驱动串联机器人的机构几何参数，借助D‑H参数法建立起关节‑末端的运动学模型；再根据关节间绳索传动的关系，建立了“绳索‑关节‑末端”的多重运动学耦合模型。进一步地，提出了分段联动式串‑并联混合的动力学求解方法，利用主从被动式的传动特点，通过对驱动绳索与联动绳索作用力进行求解，推导并建立了绳索驱动串联机器人的串‑并联混合的动力学模型。并且将所得动力学模型作为控制器的前馈控制输入，补偿运动误差，同时设计非线性二次规划模块，对绳索拉力和操作空间精度指标的进行加权优化，实现了较高精度的末端操作空间位姿的跟随运动，并使得绳索拉力和末端位姿的同步优化。

技术领域

本发明涉及机器人控制领域，尤其是一种柔性机器人动力学建模及轨迹跟随控制方法及装置。

技术背景

如今对智能机器人的环境适应性以及环境限制的克服能力有着越来越高的要求，由于传统工业机器人工作空间小、运动灵活性能力不足，特别是对一些非结构化的环境适应能力很弱，并且面临自由度数限制以及刚性臂杆难以完成狭小环境中各类障碍物的穿越问题。绳驱超冗余机器人具有自由度数多、臂杆细小、运动灵活性强以及环境适应性好、但刚度差等特点。采用离散式刚性连杆与分段联动构型的绳驱超冗余机器人在保持灵活性的同时大大提升了机械臂的刚度，能满足很多狭小空间的穿越任务，例如在核电站冷却管道的维修、油气管道维护以及核反应堆管道检查等超常规作业区域。

然而，由于该类型机构自由度比较多，主从被动式分段联动机械臂的多层级动力学耦合的存在，使得此类机器人的动力学控制与轨迹规划变得更加复杂。因此需要提出一种能够提高轨迹规划效率的同时兼顾控制精度的柔性机器人轨迹规划方法。

发明内容

鉴于现有技术的缺陷，本发明旨在于提供一种柔性机器人动力学建模及轨迹跟随控制方法及装置，能够更好地研究机器人的运动特性并提高机器人末端轨迹跟随的精确度。

为了实现上述目的，本发明采用的技术方案如下：

一种柔性机器人动力学建模及轨迹跟随的控制装置，具有柔性机器人，以及所述控制装置，所述控制装置包括：

偏差数据获取模块：用于获取相对偏差数据并构建所述柔性机器人空间模型；

计算模块：用于实时计算期望轨迹的笛卡尔空间样条插值、运动学、动力学以及求解非线性二次规划优化模型；

阈值判断模块：用于根据所述相对偏差数据与非线性二次规划优化模型的阈值条件，判断末端位姿、关节角度、绳索拉力是否在设定目标范围内；

执行模块：用于当满足阈值判断条件，将非线性二次规划优化模型的计算结果输入至PD控制器。控制器输出信号到执行模块并作用到所控制柔性机器人；

其中，所述相对偏差数据包括所述柔性机器人末端点和设定的期望点的相对位位移与位姿的偏差、实际绳索长度和期望绳索长度的偏差以及绳索实际拉力与期望拉力的偏差。

本发明还提供一种实现柔性机器人动力学建模及轨迹跟随的控制的方法，所述方法包括以下步骤：

S1建立“绳索-关节-末端”多重传动运动学模型,对目标柔性机器人各关节建立D-H坐标系，推导得柔性机器人末端位姿、速度、加速度与关节角、角速度、角加速度之间的关系，建立起关节空间到末端笛卡尔空间的映射关系；

S2建立了串-并联混合的动力学模型：基于牛顿—欧拉方程对柔性机器人整体惯性力、重力进行求解；求解所述驱动力与摩擦力；求解整个柔性机器人的联动绳索力/力矩；并进一步地推导出柔性机器人串-并联混合动力学模型；

S3建立基于串-并联混合的动力学模型前馈的PD控制器：设计柔性机器人运动的PD控制器，并将步骤S2中的串-并联混合的动力学模型作为控制前馈输入，补偿柔性机器人末端的运动误差；