本发明涉及水下机器人运动控制，尤其是涉及一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法及系统。
背景技术：
1、水下双臂机器人凭借其灵活的操作性和更强的作业能力，已经成为水下作业的重要工具，在海洋资源开发、科学探索和水下工程领域得到了广泛应用。水下机器人的运动控制技术基本原理与陆地机器人相似，需要通过控制运动轨迹、速度、力等参数来控制其运动状态，不同的是，水下机器人受到水流、水压等水下环境因素的影响比较大，往往需要根据环境条件进行相应调整。
2、现有的水下双臂机器人运动规划方法通常基于各单臂独立运动的控制方式，难以确保双臂间的协调性，特别是在复杂任务场景下效率较低。当水下双臂机器人下潜深度的增加，受到复杂多变的深海环境的影响也就越大。水下的噪声通常由波浪、海流等自然因素产生，它们往往是以谐波形式呈现。这些噪声干扰会降低水下双臂机器人运动的精度，尤其是在协调操作时，会导致任务难以准确完成。因此，消除谐波噪声干扰成为了水下双臂机器人协调运动规划亟需解决的难题。
技术实现思路
1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法及系统，能够有效提高水下双臂机器人运动控制的抗噪性能，提升协调规划任务的准确性和稳定性。
2、本发明的目的可以通过以下技术方案来实现：一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，包括以下步骤：
3、s1、根据相对雅可比矩阵建模方法，建立水下双臂机器人的运动学方程；
4、s2、考虑谐波噪声的影响，设计补偿谐波噪声的自适应抗噪动态系统；
5、s3、根据水下双臂机器人协调运动规划的需求，在运动学方程中引入自适应抗噪动态系统和水下双臂机器人末端执行器的位姿误差后求解该运动学方程，得到自适应抗扰协调规划计算结果；
6、s4、根据自适应抗扰协调规划计算结果，相应驱动水下双臂机器人完成协调规划任务。
7、进一步地，所述步骤s1中水下双臂机器人具体是一个由右臂r和左臂l组成的水下双臂机器人，其双臂末端执行器期望的速度向量等于相对雅克比矩阵乘以水下双臂机器人的关节速度向量。
8、进一步地，所述步骤s1中水下双臂机器人的运动学方程具体为：
9、
10、其中，ψx(qrl(t))为相对雅克比矩阵，qrl(t)为水下双臂机器人的关节角度向量，为水下双臂机器人的关节速度向量，y(t)为水下双臂机器人末端执行器期望的位姿向量，为水下双臂机器人末端执行器期望的速度向量、由y(t)对时间求导可得，t表示时间。
11、进一步地，所述步骤s2中自适应抗噪动态系统具体是基于内模原理，通过检测谐波噪声的频率来模拟谐波噪声，从而达到抑制噪声的作用。
12、进一步地，所述自适应抗噪动态系统具体为：
13、
14、
15、其中，m(t)表示包含多个频率的谐波噪声，q表示谐波噪声的个数，mp(t)表示第p个谐波噪声，ap表示第p个谐波噪声的幅值，fp表示第p个谐波噪声的频率，φp表示第p个谐波噪声的相位，由m(t)对t求导可得，wp(t)为用于消除mp(t)干扰的信号，hp(t)由wp(t)对t求导可得，由hp(t)对t求导可得。
16、进一步地，所述步骤s3具体是在水下双臂机械人运动学方程中引入自适应抗噪动态系统和双臂末端执行器的位姿误差，得到基于伪逆描述的自适应抗噪运动控制方案，用于抵消谐波噪声对水下双臂机器人执行任务时的干扰。
17、进一步地，所述基于伪逆描述的自适应抗噪运动控制方案具体为：
18、
19、ε(t)＝φ(qrl(t))-y(t)
20、其中，为相对雅克比矩阵ψx的伪逆矩阵，为ψx的转置矩阵；ε(t)为水下双臂机器人末端执行器的位姿误差，φ(·)为水下双臂机器人中关节角度与末端执行器位姿关系的非线性映射函数；增益参数满足δ>0，τ>0。
21、一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划系统，包括上位机及其连接的下位机控制器，所述上位机用于建立水下双臂机器人的运动学方程、设计补偿谐波噪声的自适应抗噪动态系统，以及构建基于伪逆描述的自适应抗噪运动控制方案，并求解得到协调规划计算结果；
22、所述下位机控制器根据协调规划计算结果，相应驱动水下双臂机器人完成协调规划任务。
23、进一步地，所述上位机连接有用于测量谐波噪声信号频率的测量装置，所述上位机基于测量装置输出的频率数据，利用内膜原理，以设计补偿谐波信号的自适应抗噪动态系统。
24、进一步地，所述下位机控制器内置有驱动算法及多个驱动模块，用于将协调规划计算结果转换为驱动指令，以控制水下双臂机器人的各个关节发生相应动作，完成协调规划任务。
25、与现有技术相比，本发明具有以下优点：
26、本发明通过相对雅可比矩阵建模方法建立水下双臂机器人运动学方程；并考虑谐波噪声的影响，设计补偿谐波噪声的自适应抗噪动态系统；之后根据水下双臂机器人的协调运动规划需求，引入自适应抗噪动态系统和水下双臂机器人末端执行器的位姿误差，构建水下双臂机器人自适应抗扰协调运动规划控制方案。由此通过建立双臂间的相对运动关系，同时借助自动计算补偿信号的能力，有效地抑制谐波噪声的干扰，使协调规划任务的准确性和稳定性显著提升，提高了水下双臂机器人在复杂环境中的适应性和可靠性，为高精度和高稳定性的协调规划任务提供了强有力的支持，即使在谐波噪声干扰下，也能确保水下双臂机器人高效、准确地执行指定的协调运动规划任务。
27、本发明在设计自适应抗噪动态系统时，利用内模原理，在谐波噪声部分信息未知的情况下，只需测得谐波频率便能自适应模拟谐波噪声，以起到抑制谐波噪声的作用，并在在水下双臂机械人运动学方程中，引入该自适应抗噪动态系统和双臂末端执行器的位姿误差，使得构建的协调运动规划方案能够有效抵消谐波噪声对水下双臂机器人执行任务时的干扰。
1.一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述步骤s1中水下双臂机器人具体是一个由右臂r和左臂l组成的水下双臂机器人，其双臂末端执行器期望的速度向量等于相对雅克比矩阵乘以水下双臂机器人的关节速度向量。
3.根据权利要求2所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述步骤s1中水下双臂机器人的运动学方程具体为：
4.根据权利要求1所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述步骤s2中自适应抗噪动态系统具体是基于内模原理，通过检测谐波噪声的频率来模拟谐波噪声，从而达到抑制噪声的作用。
5.根据权利要求4所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述自适应抗噪动态系统具体为：
6.根据权利要求5所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述步骤s3具体是在水下双臂机械人运动学方程中引入自适应抗噪动态系统和双臂末端执行器的位姿误差，得到基于伪逆描述的自适应抗噪运动控制方案，用于抵消谐波噪声对水下双臂机器人执行任务时的干扰。
7.根据权利要求6所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，所述基于伪逆描述的自适应抗噪运动控制方案具体为：
8.一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划系统，用于实现如权利要求1所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划方法，其特征在于，包括上位机及其连接的下位机控制器，所述上位机用于建立水下双臂机器人的运动学方程、设计补偿谐波噪声的自适应抗噪动态系统，以及构建基于伪逆描述的自适应抗噪运动控制方案，并求解得到协调规划计算结果；
9.根据权利要求8所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划系统，其特征在于，所述上位机连接有用于测量谐波噪声信号频率的测量装置，所述上位机基于测量装置输出的频率数据，利用内膜原理，以设计补偿谐波信号的自适应抗噪动态系统。
10.根据权利要求8所述的一种基于相对雅可比的水下双臂机器人自适应抗扰协调规划系统，其特征在于，所述下位机控制器内置有驱动算法及多个驱动模块，用于将协调规划计算结果转换为驱动指令，以控制水下双臂机器人的各个关节发生相应动作，完成协调规划任务。
