一种多自由度实体位姿的模糊控制方法

本发明涉及机器人控制，具体为一种多自由度实体位姿的模糊控制方法。

背景技术：

1、在智能制造装备产业中，工业机器人、特殊作业机器人及服务消费机器人等多自由度机构广泛应用于工业分拣、精密装配、医疗操作等领域，其末端执行器的位姿控制是实现高精度作业的核心问题，也是工业控制系统中运动控制子系统的关键技术环节。位姿控制的本质是在任务空间中计算末端执行器相对于期望位姿的误差，并将误差映射为各关节的驱动指令，驱动机构向期望位姿收敛。在此过程中，雅可比矩阵承担着任务空间速度与关节空间速度之间的映射职责，是多自由度机构运动控制的核心数学工具。当机构处于或趋近奇异构型时，雅可比矩阵的秩发生退化，其逆或伪逆的数值条件急剧恶化，导致关节速度指令出现奇异放大。

2、针对该问题，现有主流方法包括阻尼最小二乘法与奇异值截断法，通过在奇异值矩阵中引入固定阻尼因子或直接置零小奇异值来抑制发散。在智能控制领域，模糊控制也被引入机器人控制中，与pid结合以提升轨迹跟踪精度。

3、然而，现有方法存在以下不足：其一，阻尼最小二乘法和奇异值截断法采用固定参数处理奇异问题，固定参数的抑制策略无法根据工业自动控制系统装置制造中机构构型的实时变化自适应调整，在多方向同时退化时易出现过度抑制或抑制不足；其二，现有模糊控制方案主要针对关节空间的误差调节，未将奇异性信息引入模糊推理过程，缺乏对任务空间各方向运动能力退化的感知与响应机制；其三，多自由度机构在高速运动时不可避免地产生振动，振动信息对于调节控制增益至关重要，但现有方法普遍缺乏对关节振动状态的实时感知手段，导致控制系统在振动加剧时无法自适应收紧控制指令，进一步恶化控制性能。

技术实现思路

1、本发明提供一种多自由度实体位姿的模糊控制方法，面向工业控制系统中多自由度机构的运动控制需求，解决多自由度机构趋近奇异构型时控制失稳及振动感知缺失的问题。

2、为实现上述目的，本发明提供了如下的技术方案：

3、本发明一种多自由度实体位姿的模糊控制方法，包括：

4、s100：获取多自由度机构的实际位姿与期望位姿，计算实际位姿相对于期望位姿的位姿误差，得到位姿误差向量；

5、s200：计算所述实际位姿对应的雅可比矩阵，对所述雅可比矩阵进行奇异值分解，得到左奇异向量矩阵、奇异值对角矩阵和右奇异向量矩阵；

6、s300：实时采集多自由度机构各关节的运动信号，在滑动时间窗口内计算所述运动信号的模糊熵，得到振动感知指标；

7、s400：将所述位姿误差向量经控制增益映射后投影至所述左奇异向量矩阵的各列向量方向，得到左奇异向量方向投影分量；以所述奇异值对角矩阵中每一奇异值与所述奇异值对角矩阵中最大奇异值之比及所述振动感知指标为输入，经与每一左奇异向量方向一一对应的独立模糊推理系统分别计算对应左奇异向量方向的衰减系数，以所述衰减系数对对应左奇异向量方向投影分量施加比例衰减，将比例衰减后的左奇异向量方向投影分量以对应左奇异向量为基向量加权求和，得到任务空间指令向量；

8、s500：将所述任务空间指令向量依次经所述左奇异向量矩阵的转置、所述奇异值对角矩阵的伪逆和所述右奇异向量矩阵映射，合成关节速度指令并输出至各关节。

9、作为本发明的一种优选技术方案，在s100中，所述计算实际位姿相对于期望位姿的位姿误差的具体步骤包括：

10、计算期望位姿在李群se(3)中的逆元素，将所述逆元素与实际位姿在李群se(3)上相乘，得到位姿误差元素；

11、提取所述位姿误差元素中的位置误差向量，作为位置误差分量；

12、对所述位姿误差元素中的旋转误差矩阵取李群so(3)上的对数映射，得到旋转误差角度与旋转误差轴，以轴角形式表示旋转误差分量；

13、将所述位置误差分量与所述旋转误差分量拼接为六维误差向量，作为所述位姿误差向量。

14、作为本发明的一种优选技术方案，在s200中，还包括基于所述雅可比矩阵动态调整独立模糊推理系统隶属函数宽度参数的步骤，所述步骤具体包括：

15、以所述雅可比矩阵与所述雅可比矩阵的转置的乘积构造局部度量张量；

16、对所述局部度量张量进行特征值分解，以所述局部度量张量的每一特征值的平方根作为对应误差分量方向的度量缩放系数；

17、以所述度量缩放系数对所述位姿误差向量中沿对应左奇异向量方向的误差分量的隶属函数宽度参数进行独立缩放，当所述雅可比矩阵更新时同步触发所述隶属函数宽度参数更新。

18、作为本发明的一种优选技术方案，在s300中，所述计算所述运动信号的模糊熵的具体步骤包括：

19、在所述滑动时间窗口内对多自由度机构每一关节的运动信号进行模糊化处理，以指数型模糊隶属度函数替代精确匹配计算样本间相似度；

20、统计相邻两个模板维度下模板对模糊相似度均值的对数差，得到每一关节的模糊熵；

21、取每一关节的模糊熵中的最大值作为所述振动感知指标。

22、作为本发明的一种优选技术方案，在s300中，所述滑动时间窗口的长度根据多自由度机构每一关节运动信号的变化率自适应调整：

23、当每一关节运动信号的变化率大于预设变化率阈值时，缩短所述滑动时间窗口的长度；

24、当每一关节运动信号的变化率不大于所述预设变化率阈值时，延长所述滑动时间窗口的长度。

25、作为本发明的一种优选技术方案，在s400中，与每一左奇异向量方向一一对应的独立模糊推理系统的隶属函数设计具体包括：

26、对所述奇异值对角矩阵中每一奇异值与所述奇异值对角矩阵中最大奇异值之比分别定义正常、过渡、退化三个模糊语言值及对应隶属函数；

27、对所述振动感知指标分别定义低、中、高三个模糊语言值及对应隶属函数；

28、所述奇异值与所述最大奇异值之比越小则退化模糊语言值的隶属度越大，所述振动感知指标越大则高模糊语言值的隶属度越大，所述奇异值与所述最大奇异值之比及所述振动感知指标共同决定对应左奇异向量方向的衰减系数；

29、当某一左奇异向量方向对应奇异值与所述最大奇异值之比的退化模糊语言值隶属度超过过渡模糊语言值对应的隶属度时，无论所述振动感知指标大小，该左奇异向量方向的衰减系数取预设下限值。

30、作为本发明的一种优选技术方案，当多个左奇异向量方向对应奇异值与所述最大奇异值之比的退化模糊语言值隶属度同时超过过渡模糊语言值对应的隶属度时，所述比例衰减的执行步骤具体包括：

31、识别所有退化模糊语言值隶属度超过过渡模糊语言值对应隶属度的左奇异向量方向，对所述左奇异向量方向的左奇异向量方向投影分量施加与所述左奇异向量方向一一对应的独立模糊推理系统计算所得衰减系数；

32、当所有退化模糊语言值隶属度超过过渡模糊语言值对应隶属度的左奇异向量方向的衰减系数均达到所述预设下限值时，以所有退化模糊语言值隶属度超过过渡模糊语言值对应隶属度的左奇异向量方向的退化模糊语言值隶属度的算术均值为缩减系数，对其余左奇异向量方向的左奇异向量方向投影分量同步施加比例缩减，得到缩减后指令分量；

33、将比例衰减后的左奇异向量方向投影分量与所述缩减后指令分量以对应左奇异向量为基向量加权求和，得到所述任务空间指令向量。

34、作为本发明的一种优选技术方案，在s400中，还包括将所述位姿误差向量的误差变化率向量同步投影至所述左奇异向量矩阵各列向量方向的步骤，所述步骤具体包括：

35、将所述位姿误差向量投影至所述左奇异向量矩阵的各列向量方向，得到误差投影分量；

36、计算所述位姿误差向量在相邻控制周期间的差值，得到误差变化率向量；

37、将所述误差变化率向量经控制增益映射后投影至所述左奇异向量矩阵的各列向量方向，得到左奇异向量方向误差变化率投影分量；

38、以所述误差投影分量、所述左奇异向量方向误差变化率投影分量、所述奇异值对角矩阵中对应奇异值与所述奇异值对角矩阵中最大奇异值之比及所述振动感知指标为输入，经与对应左奇异向量方向一一对应的独立模糊推理系统计算对应左奇异向量方向的衰减系数。

39、作为本发明的一种优选技术方案，在s400中，与每一左奇异向量方向一一对应的独立模糊推理系统采用区间二型模糊逻辑系统，所述区间二型模糊逻辑系统的推理结果采用karnik-mendel迭代算法进行降型计算，以相邻两次迭代结果之差作为收敛判据，满足所述收敛判据时终止迭代并解模糊，得到对应左奇异向量方向的衰减系数。

40、作为本发明的一种优选技术方案，所述多自由度机构为具有整周转动能力的三平动一转动四自由度并联机器人，多自由度机构各关节的运动信号为驱动所述并联机器人四个主动转动副的伺服电机编码器信号。

41、本发明的有益效果是：

42、1.本发明通过对雅可比矩阵进行奇异值分解，将位姿误差向量投影至各左奇异向量方向，并以奇异值与最大奇异值之比为输入经独立模糊推理系统计算各方向衰减系数，实现了对任务空间各方向运动能力退化的精细感知与自适应抑制，克服了现有固定参数奇异处理方法在多方向同时退化时过度抑制或抑制不足的问题。

43、2.本发明实时采集各关节运动信号，在自适应滑动时间窗口内计算模糊熵得到振动感知指标，并将振动感知指标与奇异值比共同作为独立模糊推理系统的输入，使控制系统在关节振动加剧时能够同步收紧控制指令，填补了现有多自由度机构位姿控制方法中振动状态实时感知与控制增益协同调节的空白。

44、3.本发明采用区间二型模糊逻辑系统结合karnik-mendel迭代算法，并基于雅可比矩阵构造局部度量张量动态调整隶属函数宽度参数，使模糊推理系统对机构构型变化和隶属函数参数不确定性均具有较强的鲁棒性，实现了奇异抑制与振动感知的协同优化。