[发明专利]一种基于模糊ADRC的连续体机械臂多环控制方法

权利要求书

1.一种基于模糊ADRC的连续体机械臂多环控制方法，其特征在于，包括以下步骤：

S1、初始化机械臂位姿状态和控制器所有参数；

S2、将期望的末端位姿输入到单片机内以逆解出机械臂中每个关节的位姿矩阵；

S3、将每个关节的位姿矩阵转化为电机转动信号并输入到位姿环ADRC以得到控制信号；

S4、将控制信号输入到速度环ADRC以得到电机控制信号；

S5、将电机控制信号输入到驱动器中以通过电机和驱动线带动机械臂运动。

2.如权利要求1所述的一种基于模糊ADRC的连续体机械臂多环控制方法，其特征在于，步骤S2包括：将期望的末端位姿输入到单片机内，利用数值法将期望的末端点相对于极坐标系的位姿矩阵通过相邻关节的转换矩阵逆解得出每个关节的位姿矩阵；其中，相邻关节的转换矩阵表示为：

其中，表示相邻关节的转换矩阵，θ_i表示极坐标系中以Z_i-1轴方向看X_i-1轴与X_i轴的夹角，i表示第i个关节，a_i表示极坐标系中以Z_i-1轴方向看X_i-1轴与X_i轴之间的距离，α_i表示极坐标系中以X_i-1轴方向看Z_i-1轴与Z_i轴的夹角。

3.如权利要求2所述的一种基于模糊ADRC的连续体机械臂多环控制方法，其特征在于，所述位姿矩阵包括旋转角度和关节坐标位置向量。

4.如权利要求1所述的一种基于模糊ADRC的连续体机械臂多环控制方法，其特征在于，所述将每个关节的位姿矩阵转化为电机转动信号的步骤包括：

将每个关节的位姿矩阵通过关节角度与驱动线线长关系公式，转化成线长变化数值；

通过线长变化数值转化为电机旋转圈数信息以得到电机转动信号。

5.如权利要求1所述的一种基于模糊ADRC的连续体机械臂多环控制方法，其特征在于，步骤S5还包括：

通过机械臂末端陀螺仪采集机械臂运动时的末端位姿，判断机械臂运动的末端位姿是否达到移动和旋转要求，若未达到，则重复步骤S3-S5，若达到，则停止电机运动。

6.一种电子设备，其特征在于，包括：

存储器，用于存储一个或多个程序；

处理器；

当所述一个或多个程序被所述处理器执行时，实现如权利要求1-5中任一项所述的一种基于模糊ADRC的连续体机械臂多环控制方法。

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1-5中任一项所述的一种基于模糊ADRC的连续体机械臂多环控制方法。