本发明涉及机器人的控制，尤其涉及一种力/位协调的多臂机器人柔顺控制方法。
背景技术：
机器人在多机械臂力耦合情况下操作完成一个操作任务时，为了保证机械臂和操作物体的安全性，需要采用柔顺控制或者力控制的方式实现力协调，但是由于力闭环的响应周期较长，而实际上机械臂的内环大都采用位置控制的模式，因此，机械臂的响应时间会受到影响，因此，如果能够同时保证多机械臂协调操作过程中力的同步性，同时也保证位姿的同步性，这将会提高机械臂的协调操作能力。
近年来，随着力控制的需求越来越大，机器人的操作任务需求也从单一的位置控制需求，逐渐发展为位置和力的同时需求的阶段。尤其是在多臂力耦合完成操作任务的情况下，多臂力/位协调控制变得更加重要。
多臂力/位协调控制可以有效保证多机械臂力的协调关系和位，但是仅有力的协调关系，机械臂的位姿精度将无法保证，降低了多机械臂的协调控制性能。而机械臂保证力控制的有效方式是采用阻抗控制的方法，建立机械臂位姿与力之间关系的函数方程，从而实现力和位姿之间的动态协调平衡。单个机械臂中，阻抗方程的输出与输入关系只跟机械臂自身的位姿误差、力误差有关，当多机械臂协调时，为了保证系统的安全性，采用力同步的方式保证多机械臂的力协调关系，但是实际应用中，由于力传感器的物理响应和闭环周期都较慢，而机械臂的位姿闭环周期较短，因此，仅有力的同步闭环控制，会对机械臂的位姿跟踪产生影响。
技术实现要素：
本发明的目的是提供一种力/位协调的多臂机器人柔顺控制方法，该方法在确定不同机械臂之间的力协调的同时，保证了位置的同步协调关系。
本发明为解决上述技术问题采用以下技术方案：
一种力/位协调的多臂机器人柔顺控制方法，机器人有n个机械臂t={t1，t2，…，ti，…，tn}，1<i<n，ti表示第i个机械臂，所述控制方法具体包括以下步骤：
步骤s10，由机械臂的操作任务计算机械臂末端期望接触力；
步骤s11，通过安装的机械臂末端六维力矩传感器测量机械臂末端实际接触力；
步骤s12，由机械臂的操作任务计算机械臂末端期望位姿；
步骤s13，通过机械臂关节角度传感器测量机械臂关节绝对角度；
步骤s14，以步骤s10得到的所述机械臂末端期望接触力、步骤s11得到的所述机械臂末端实际接触力、步骤s12得到的所述机械臂末端期望位姿以及步骤s13得到的所述机械臂关节绝对角度作为条件输入基于力/位协调的多机械臂同步阻抗控制器；
步骤s15，根据步骤s14所述基于力/位协调的多机械臂同步阻抗控制器的输出结果实现机械臂闭环控制。
进一步的，所述步骤s140的具体过程为：
步骤s140，根据步骤s10得到的所述机械臂ti-1，机械臂ti，机械臂ti+1末端期望接触力和步骤s11得到的所述机械臂ti-1，机械臂ti，机械臂ti+1末端实际接触力以及同步控制思想得到机械臂ti末端力误差、同步力误差、同步力耦合误差；
步骤s141，根据步骤s140得到的所述机械臂ti末端力误差、同步力误差以及同步力耦合误差得到机械臂ti末端力补偿量；
步骤s142，设计机械臂末端同步阻抗控制器，所述机械臂末端同步阻抗控制器以步骤s141得到的所述机械臂ti末端力补偿量作为输入条件，从而建立多机械臂同步力与运动学之间的关系；
步骤s143，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望加速度；
步骤s144，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望速度；
步骤s145，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望位姿；
步骤s146，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望加速度；
步骤s147，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望速度；
步骤s148，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望位姿；
步骤s149，根据步骤s13得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的关节绝对角度信息和机械臂的正运动学得到机械臂ti末端实际位姿；
步骤s1410，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿和步骤s149得到的所述机械臂ti末端实际位姿得到械臂ti末端位姿误差、同步位姿误差、同步位姿耦合误差；
步骤s1411，根据步骤s1410得到的所述械臂ti末端位姿误差、同步位姿误差、同步位姿耦合误差得到机械臂ti末端同步位姿补偿量；
步骤s1412，根据步骤s143得到的所述机械臂ti阻抗期望加速度和步骤s146得到的所述机械臂ti的运动期望加速度得到机械臂ti同步期望加速度；
步骤s1413，根据步骤s144得到的所述机械臂ti阻抗期望速度和步骤s147得到的所述机械臂ti的运动期望速度得到机械臂ti同步期望速度；
步骤s1414，根据步骤s145得到的所述机械臂ti阻抗期望位姿、步骤s148得到的所述机械臂ti的运动期望位姿和步骤s1411得到的所述机械臂ti末端同步位姿补偿量得到机械臂ti同步期望位姿。
进一步的，所述步骤s15的具体过程为：
步骤s150，通过步骤s13得到的所述机械臂关节绝对角度和建立关节的速度观测器，计算得到机械臂的关节角速度；
步骤s151，根据机械臂速度雅克比关系和步骤s150得到的所述机械臂的关节角速度计算求得机械臂末端实际速度；
步骤s152，根据步骤s13得到的所述机械臂关节绝对角度计算求得机械臂末端实际位姿；
步骤s153，根据步骤s151得到的所述机械臂末端实际速度和步骤s152得到的所述机械臂末端实际位姿信息计算得到机械臂末端实际加速度；
步骤154，根据步骤s151得到的所述机械臂末端实际速度和步骤s1413得到的所述机械臂ti同步期望速度之间的差值，求得机械臂速度补偿量；
步骤s155，根据雅克比关系以及步骤s154求得的所述机械臂速度补偿量求得瞬间时刻的机械臂关节角速度补偿量，在短时间段内，该补偿量等效为关节速度补偿量；
步骤s156，根据步骤s153得到的所述机械臂末端实际加速度和步骤s1412得到的所述机械臂ti同步期望加速度之间的差值得到机械臂加速度补偿量；
步骤s157，根据步骤s156得到的所述机械臂末端加速度补偿量得到关节加速度补偿量；
步骤s158，根据步骤s1414得到的所述机械臂同步期望位姿和步骤s152求得的所述机械臂末端实际位姿之间的差得到机械臂位姿补偿量；
步骤s159，根据步骤s158得到的所述机械臂位姿补偿量和逆运动学关系得到机械臂的关节角度补偿量；
步骤s1510，根据步骤s155得到的所述关节速度补偿量、步骤s157得到的所述关节加速度补偿量和步骤s159得到的所述机械臂的关节角度补偿量得到机械臂的关节控制补偿量从而实现机械臂关节闭环控制。
进一步的，机械臂的阻抗控制系统采用笛卡尔阻抗控制系统，笛卡尔阻抗控制系统的内环采用笛卡尔位置控制，外环采用阻抗控制器。
与现有技术相比，本发明有以下有益效果：
（1）通过在机械臂的大闭环周期中采用力同步协调控制方法，在小闭环周期中采用位姿同步协调控制的方法，从而实现多机械臂的力/位同步协调控制；（2）利用阻抗控制方法建立了多臂之间力和位姿的协调控制关系，通过采用力同步控制的思想，建立多机械臂之间的力同步控制器，保证了力协调的精确性；（3）通过在机械臂控制的小闭环内采用位姿同步控制的思想，提高了系统的响应频率，缩短了响应周期，提高了位姿协调控制精度，通过在多机械臂之间建立位姿同步控制器，从而实现基于力/位协调的多机械臂柔顺控制方法。
附图说明
图1为多臂机器人同步阻抗控制流程图；
图2为基于力/位的同步阻抗控制器结构框图；
图3为机械臂闭环控制框图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合附图及具体实施例对本发明的技术方案做进一步的详细说明：
如图1所示，一种力/位协调的多臂机器人柔顺控制方法，机器人有n个机械臂t={t1，t2，…，ti，…，tn}，1<i<n，ti表示第i个机械臂，该控制方法具体包括以下步骤：
步骤s10，由机械臂的操作任务计算机械臂末端期望接触力；
步骤s11，通过安装的机械臂末端六维力矩传感器测量机械臂末端实际接触力；
步骤s12，由机械臂的操作任务计算机械臂末端期望位姿；
步骤s13，通过机械臂关节角度传感器测量机械臂关节绝对角度；
步骤s14，以步骤s10得到的所述机械臂末端期望接触力、步骤s11得到的所述机械臂末端实际接触力、步骤s12得到的所述机械臂末端期望位姿以及步骤s13得到的所述机械臂关节绝对角度作为条件输入基于力/位协调的多机械臂同步阻抗控制器；
步骤s15，根据步骤s14所述基于力/位协调的多机械臂同步阻抗控制器的输出结果实现机械臂闭环控制。
如图2所示，步骤s140的具体过程为：
步骤s140，根据步骤s10得到的所述机械臂ti-1，机械臂ti，机械臂ti+1末端期望接触力和步骤s11得到的所述机械臂ti-1，机械臂ti，机械臂ti+1末端实际接触力以及同步控制思想得到机械臂ti末端力误差、同步力误差、同步力耦合误差；由机械臂ti-1和机械臂ti得到机械臂ti同步力误差，通过机械臂ti-1和机械臂ti+1可以计算得到机械臂ti的同步力耦合力误差。
步骤s141，根据步骤s140得到的所述机械臂ti末端力误差、同步力误差以及同步力耦合误差得到机械臂ti末端力补偿量；
步骤s142，设计机械臂末端同步阻抗控制器，所述机械臂末端同步阻抗控制器以步骤s141得到的所述机械臂ti末端力补偿量作为输入条件，从而建立多机械臂同步力与运动学之间的关系；
步骤s143，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望加速度；
步骤s144，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望速度；
步骤s145，根据所述机械臂末端同步阻抗控制器得到机械臂ti阻抗期望位姿；
步骤s146，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望加速度；
步骤s147，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望速度；
步骤s148，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿得到机械臂ti的运动期望位姿；
步骤s149，根据步骤s13得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的关节绝对角度信息和机械臂的正运动学得到机械臂ti末端实际位姿；
步骤s1410，根据步骤s12得到的所述机械臂ti-1，机械臂ti，机械臂ti+1的末端期望位姿和步骤s149得到的所述机械臂ti末端实际位姿得到械臂ti末端位姿误差、同步位姿误差、同步位姿耦合误差；
步骤s1411，根据步骤s1410得到的所述械臂ti末端位姿误差、同步位姿误差、同步位姿耦合误差得到机械臂ti末端同步位姿补偿量；
步骤s1412，根据步骤s143得到的所述机械臂ti阻抗期望加速度和步骤s146得到的所述机械臂ti的运动期望加速度得到机械臂ti同步期望加速度；
步骤s1413，根据步骤s144得到的所述机械臂ti阻抗期望速度和步骤s147得到的所述机械臂ti的运动期望速度得到机械臂ti同步期望速度；
步骤s1414，根据步骤s145得到的所述机械臂ti阻抗期望位姿、步骤s148得到的所述机械臂ti的运动期望位姿和步骤s1411得到的所述机械臂ti末端同步位姿补偿量得到机械臂ti同步期望位姿。
如图3所示，步骤s15的具体过程为：
步骤s150，通过步骤s13得到的所述机械臂关节绝对角度和建立关节的速度观测器，计算得到机械臂的关节角速度；
步骤s151，根据机械臂速度雅克比关系和步骤s150得到的所述机械臂的关节角速度计算求得机械臂末端实际速度；
步骤s152，根据步骤s13得到的所述机械臂关节绝对角度计算求得机械臂末端实际位姿；
步骤s153，根据步骤s151得到的所述机械臂末端实际速度和步骤s152得到的所述机械臂末端实际位姿信息计算得到机械臂末端实际加速度；
步骤154，根据步骤s151得到的所述机械臂末端实际速度和步骤s1413得到的所述机械臂ti同步期望速度之间的差值，求得机械臂速度补偿量；
步骤s155，根据雅克比关系以及步骤s154求得的所述机械臂速度补偿量求得瞬间时刻的机械臂关节角速度补偿量，在短时间段内，该补偿量等效为关节速度补偿量；
步骤s156，根据步骤s153得到的所述机械臂末端实际加速度和步骤s1412得到的所述机械臂ti同步期望加速度之间的差值得到机械臂加速度补偿量；
步骤s157，根据步骤s156得到的所述机械臂末端加速度补偿量得到关节加速度补偿量；
步骤s158，根据步骤s1414得到的所述机械臂同步期望位姿和步骤s152求得的所述机械臂末端实际位姿之间的差得到机械臂位姿补偿量；
步骤s159，根据步骤s158得到的所述机械臂位姿补偿量和逆运动学关系得到机械臂的关节角度补偿量；
步骤s1510，根据步骤s155得到的所述关节速度补偿量、步骤s157得到的所述关节加速度补偿量和步骤s159得到的所述机械臂的关节角度补偿量得到机械臂的关节控制补偿量从而实现机械臂关节闭环控制。
机械臂的阻抗控制系统采用笛卡尔阻抗控制系统，笛卡尔阻抗控制系统的内环采用笛卡尔位置控制，外环采用阻抗控制器。
以上所述实施方式仅是本发明的具体和详细描述，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。