[发明专利]一种主从式遥操作手术机器人多臂协同控制系统

权利要求书

1.一种主从式遥操作手术机器人多臂协同控制系统，用于对多臂手术机器人进行控制，多臂手术机器人包括主手操作端和从手执行端，主手操作端为7DOF串联式机器人，从手执行端由两个7DOF串联式机械臂和两个6DOF串联式机械臂组成，每个机械臂末端均安装多维力传感器和运动控制器，其特征在于：该控制系统包括工业PC系统和底层控制系统，所述工业PC系统包括工业PC、以太网、CAN总线适配卡；所述底层控制系统包括数字量与模拟量采集卡、伺服驱动器；工业PC利用以太网接受来自其他系统的信号，利用CAN总线适配卡连接伺服驱动器控制机械臂运动，利用数字量/模拟量采集卡采集和处理来自多维力传感器的信号；各机械臂接收主手操作端传递过来的位置控制指令，通过逆运动学解算得到各关节期望位置；通过多维力传感器感知与环境间的接触力及机械臂之间的接触力，经过逆动力学求解得到各关节力和力矩信息，并将接触力反馈至手术机器人主手；通过绝对值位置传感器获取各关节实际位置；由工业PC协调各运动控制器的数据传递及信号传递，完成多机械臂协同控制功能；该手术机器人多臂协同控制系统包括滑模控制器、自适应模糊逻辑控制器和非线性观测器；所述滑模控制器，用于精确获得系统的动力学参数；所述自适应模糊逻辑控制器，用于提高控制精度；所述非线性观测器，用于补偿外部环境对系统稳定新和精度的影响；

所述滑模控制器的控制律为τ＝τ_eq+τ_sm，其中τ_sm为滑模控制项，为机器人的动力学等效模型，M(q)为惯量矩阵，J^-1(q)为逆雅可比矩阵，为机器人末端笛卡尔空间加速度，为雅可比矩阵的导数，为关节空间速度，为惯性项和科氏项，G(q)为重力项；选择滑模面其中s为6×1维向量，c为正定对角矩阵，e＝q_id-q_i，表示e的导数，q_id为关节期望位置，q_i为关节实际位置；所述滑模控制项为k₁为6×6的正定对角矩阵；M(q)为惯性矩阵；J^T(q)为雅可比矩阵的转秩矩阵，s＝[s₁,s₂,…,s_i]为模糊逻辑输入量，γ＝[γ₁,γ₂,…,γ_i]为模糊逻辑输出量；

所述非线性观测器的模型为：其中K₁与K₂是正定矩阵，表示外部转矩扰动，表示外部转矩扰动的估计，为关节速度误差向量，为对关节速度误差的估计量，表示关节加速度误差；M^-1表示逆惯量矩阵，C表示惯性项和科氏项，表示关节空间速度，G表示重力项，为关节输入加速度，τ∈R⁶为关节输入力矩。