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

权利要求书

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

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

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

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

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

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

2.一种根据权利要求1所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述方法包括以下步骤：

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

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

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

S4对绳索拉力和末端位姿的同步优化：基于驱动绳索拉力最小条件与位姿误差最小条件的非线性二次规划优化模型，并引入允许绳索拉力范围、位姿误差允许范围和关节角阈值限制条件，实时求解出合适的PD控制器增益及期望驱动绳索拉力；

S5结合多重运动学转换模块、动力学计算模块、PD控制器、二次规划优化模块，偏差数据获取模块获得柔性机器人轨迹跟随。

3.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述步骤S1中，所述D-H坐标系根据柔性机器人各关节及关节之间的实际几何参数得到D-H参数表。

4.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述步骤S1中，根据节之间绳索分布模型，可以推导出关节角变换量与绳索长度变换量之间的关系，建立起绳索驱动空间到关节空间再到末端笛卡尔空间的映射关系。

5.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述PD控制器参数Kp与Kv是动态变化；所述非线性二次规划优化模型获得动态参数Kp、Kv，实现绳索拉力与末端位姿协同优化。

6.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述柔性机器为离散式刚性连杆与分段联动构型的机器人。