买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院)

摘要：本发明涉及一种基于IMU的绳驱械臂臂段末端姿态等效控制方法及装置，其中方法包括对期望的臂段两个旋转方向的总角度进行五次多项式插值，得到期望关节角度，基于正运动学计算，得到当前期望的末端姿态；获得当前联动臂段末端的实际姿态，得到当前末端姿态差；计算联动臂段当前角度差；输入PID控制器，输出角度控制量；将角度控制量映射为驱动绳索的速度，并再次映射为驱动电机的转速；输入所述电机控制器，所述电机控制器控制驱动电机按期望的转速转动。本发明解决现有技术中，对绳驱分段联动机械臂的联动臂段进行关节角度取平均值的闭环控制无法精确控制末端姿态，以及编码器占据机械臂内部较多空间的问题。

主权项：1.基于IMU的绳驱械臂臂段末端姿态等效控制方法，应用在绳驱柔性机械臂上，所述绳驱柔性机械臂包括至少一个联动臂段、IMU测量装置（400）、PID控制器、驱动电机和电机控制器，每个联动臂段包括多个臂杆（100）和多个十字轴型万向节（200），在联动臂段内，单个臂杆（100）和单个十字轴型万向节（200）依次间隔连接，所述联动臂段由驱动绳索驱动，驱动绳索的一端与对应的驱动电机连接，所述IMU测量装置（400）安装在绳驱柔性机械臂末端的联动臂段中的末端臂杆（100）上，其特征在于，所述的基于IMU的绳驱械臂臂段末端姿态等效控制方法包括以下步骤：S100、对期望的臂段两个旋转方向的总角度进行五次多项式插值，得到当前时刻的期望关节角度；S200、得到当前期望关节角度后，基于正运动学计算，得到当前期望的联动臂段末端姿态；所述步骤S200包括：S210、建立整个联动臂段的DH坐标系，建立基坐标系B，所述基坐标系B的z轴方向与联动臂段的第一个关节轴方向重合，所述基坐标系B的x轴方向选取机械臂零位状态时连杆的伸展方向，即竖直向上的方向，所述基坐标系B的y轴方向由右手定则确定；S220、基于DH方法建立万向节坐标系组，所述万向节坐标系组包括基于联动臂段第一个万向节的第一自由度的起始第零坐标系、基于联动臂段第一个万向节的第二自由度的第一坐标系、基于联动臂段第二个万向节的第一自由度的第二坐标系、基于联动臂段第二个万向节的第二自由度的第三坐标系、基于联动臂段第三个万向节的第一自由度的第四坐标系、基于联动臂段第三个万向节的第二自由度的第五坐标系、基于联动臂段第四个万向节的第一自由度的第六坐标系、基于联动臂段第四个万向节的第二自由度的第七坐标系和基于下一个联动臂段或末端的末端坐标系E；S230、根据当前的期望关节角度值，使用DH方法构造各坐标系之间的变换矩阵；S240、通过累乘，得到末端坐标系E相对于起始第零坐标系的当前期望的联动臂段末端姿态，以旋转矩阵的形式表示；S250、对当前期望的联动臂段末端姿态使用直接求导法，得到当前期望的联动臂段末端姿态对于当前时刻的期望关节角度的雅可比矩阵；S300、通过IMU测量装置（400）的IMU传感器获得当前联动臂段末端的实际姿态，所述当前联动臂段末端的实际姿态的形式为四元数，将所述当前联动臂段末端的实际姿态变换为旋转矩阵形式，与当前期望的联动臂段末端姿态作差，得到当前末端姿态差；S400、基于当前末端姿态差，计算联动臂段yaw偏航控制方向和pitch俯仰控制方向的当前角度差；S500、将当前角度误差输入PID控制器，PID控制器输出yaw偏航控制方向和pitch俯仰控制方向的角度控制量，所述角度控制量为关节在yaw偏航控制方向和pitch俯仰控制方向的角速度；S600、将所述角度控制量映射为驱动绳索的速度，并将驱动绳索的速度再次映射为对应的驱动电机的转速；S700、将驱动电机的转速输入所述电机控制器，所述电机控制器控制驱动电机按期望的转速转动，完成联动臂段的运动，同时联动臂段末端的IMU测量装置（400）不间断反馈当前姿态。

全文数据：

权利要求：

百度查询： 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 基于IMU的绳驱械臂臂段末端姿态等效控制方法及装置