本发明涉及一种针对控制体系封闭机器人的动态力/位置混合控制方法，是一种适用于存在强非线性和不确定性且无开放力矩接口的机器人系统的非线性力/位置混合控制方法，能够同时适应系统内环控制器参数的不确定性和机器人系统的惯量、质量、质心位置等不确定性，属于机器人控制技术领域。
背景技术：
力/位置混合控制方法是处理机器人同环境柔顺交互的一种重要手段，提出于上个世纪80年代，主要针对关节力矩开放的机器人系统。但是在工业和工程应用场合，动态力/位置混合控制的实施却受到机器人控制体系封闭的约束，比如控制力矩不开放，只能设计关节位置或者速度指令。在工业和商业场合，大部分公司出产的机器人也都具有类似的特点。那么自然提出一个基本问题：如何在有限的设计自由度(只开放关节位置或者速度指令)下，实现机器人动态力/位置混合控制？
针对此问题的传统做法是施加运动学控制，忽略机器人动力学的影响。这种做法对于大减速比工业机器人有一定的适用性，因为安装有大减速比减速器的工业机器人的动力学耦合大大降低。但是在精度、安全性和合作性要求更多的场合，特别是目前蓬勃发展的协作机器人和轻型机器人，运动学控制所忽略的机器人非线性耦合的影响占有很大的比重，对于系统的稳定性和动态精度都有不可忽视的影响。因此需要一种能够充分考虑机器人系统动力学影响的力/位置混合控制方法，以保证操作的精度、安全性和动态性能。
技术实现要素：
本发明的技术解决问题是：克服现有技术的不足，提出控制体系封闭机器人自适应动态力/位置混合控制方法，该方法针对存在较强非线性和参数不确定性的控制体系封闭机器人，为一种基于动态反馈的自适应力/位置混合控制方法。
本发明的技术解决方案是：
控制体系封闭机器人自适应动态力/位置混合控制方法，包括下列步骤：
(1)建立控制体系封闭机器人的参数化模型
考虑接触柔顺环境的n自由度机器人。用x∈rn表示机械臂末端位置，q∈rn表示机械臂关节位置，那么有经典关系式：
x＝σ(q)(1)
其中σ:rn→rn为非线性函数，rn表示n维实空间。用j(q)∈rn×n表示机械臂的雅克比矩阵，那么机械臂末端速度和机械臂关节速度的关系可表述为如下的经典形式：
用δx表示环境轮廓由于同机械臂末端的交互作用而产生的变形量，那么有经典关系式：
h(x)＝δx(3)
其中h:rn→r为非线性函数，那么根据目前已有的结果，机械臂末端与环境轮廓的接触力f可表达为
其中γ＝kδx为接触力的幅值大小，k为环境刚度。对于力/位置混合控制，任务空间变量通常定义为
其中ξ(x)为n-1维位置矢量。根据公式(5)和公式(2)、(3)得到：
其中j*(q,x)为雅克比矩阵。
根据已有研究结果，控制体系封闭机器人的动力学模型可表达为：
其中，m(q)∈rn×n为惯量矩阵，
其中w∈rn为n维常值矢量，wp∈rn为n维常值矢量，显然
其中，θ为未知参数矢量，ζ为一个可微分的矢量，
(2)设计基于动态反馈的自适应控制律：
控制目标为使得机械臂末端对环境施加期望作用力γd，机械臂末端到达期望位置xd。此处考虑调节问题，因此γd和xd均为常值。
定义矢量z∈rn为
其中α1，α2和α3为正设计常数，δγ＝γ-γd，δξ(x)＝ξ(x)-ξ(xd)。方程(10)给出的滤波形式的动态系统可看作经典的无源滤波器的推广。定义矢量
关节控制律(关节控制律给定关节位置指令或关节速度指令)定义为
其中
其中，t为当前时刻，zi(0)为zi的初值，zi(0)可选为任意常数。
用于更新估计参数
其中γ为对称正定矩阵，λ和λp为对角正定矩阵。
(3)根据步骤(2)得到的公式(12)控制律更新关节位置指令或速度指令，根据步骤(2)得到的公式(13)自适应律更新估计参数
(4)循环执行步骤(3)，当循环次数趋于无穷时，机械臂末端力调节误差δγ和位置调节误差δξ(x)趋于零。
本发明与现有技术相比的有益效果是：
(1)本发明针对控制体系封闭机器人的力/位置混合控制问题，在系统存在非线性和不确定性的情况下，通过一种动态反馈方法生成关节位置或者速度指令的方式，对系统的非线性和不确定性予以恰当的补偿，目前的文献中只有关于控制体系封闭机器人自由空间运动的相关内容，但没有针对接触空间运动的相关内容；
(2)本发明同时考虑了机器人底层控制器参数的不确定性和底层驱动模型的不确定性，并设计了相应的参数自适应规律；
(3)本发明提出一种基于动态反馈的力/位置混合控制方法，在机器人系统力矩接口不开放的情况下，对系统的不确定性和非线性进行了有效的补偿，保证了系统的稳定性和收敛性。
(4)本发明提出的基于动态反馈的非线性控制方法充分考虑机器人的动力学耦合和非线性。所提出的控制方法体现为机械臂关节位置或者速度指令，该指令信息中包含基于力的测量信息和机械臂状态信息的动态补偿，用于实现机器人系统的动态力/位置混合控制。所提出的控制方法把目前已有的基于动态反馈的自由空间控制方法推广至考虑机械臂末端同环境接触的情形。
(5)控制体系封闭机器人的自适应动态力/位置混合控制方法，包括下列步骤：(1)建立控制体系封闭机器人的参数化模型；(2)设计基于动态反馈的自适应控制律；(3)根据自适应控制律更新关节位置指令或速度指令和估计参数；(4)循环执行步骤(3)。本发明针对控制体系封闭机器人动态力/位置混合控制问题，在存在不确定性和非线性的情况下，通过基于动态反馈的自适应控制方法设计关节位置或者速度指令以保证实现力和位置的调节目标。
附图说明
图1为本发明控制体系封闭机器人的自适应动态力/位置混合控制方法的流程框图；
图2为本发明仿真例子中两自由度机械臂接触环境的示意图；
图3为本发明仿真例子中力调节误差示意图；
图4为本发明仿真例子中位置调节误差示意图。
具体实施方式
控制体系封闭机器人自适应动态力/位置混合控制方法，包括下列步骤：
(1)建立控制体系封闭机器人的参数化模型
如图2所示，考虑接触柔顺环境的n自由度机器人(n＝2)。用x∈rn表示机械臂末端位置，q∈rn表示机械臂关节位置，那么有经典关系式：
x＝σ(q)(1)
其中σ:rn→rn为非线性函数，rn表示n维实空间。用j(q)∈rn×n表示机械臂的雅克比矩阵，那么机械臂末端速度和机械臂关节速度的关系可表述为如下的经典形式：
用δx表示环境轮廓由于同机械臂末端的交互作用而产生的变形量，那么有经典关系式：
h(x)＝δx(3)
其中h:rn→r为非线性函数，那么根据目前已有的结果，机械臂末端与环境轮廓的接触力f可表达为
其中γ＝kδx为接触力的幅值大小，k为环境刚度。对于力/位置混合控制，任务空间变量通常定义为
其中ξ(x)为n-1维位置矢量。根据公式(5)和公式(2)、(3)得到：
控制体系封闭机器人的动力学模型为：
其中，m(q)∈rn×n为惯量矩阵，
其中w∈rn为n维常值矢量，wp∈rn为n维常值矢量，显然
其中，θ为未知参数矢量，ζ为一个可微分的矢量，
(2)设计基于动态反馈的自适应控制律：
控制目标为使得机械臂末端对环境施加期望作用力γd，机械臂末端到达期望位置xd。此处考虑调节问题，因此γd和xd均为常值。
定义矢量z∈rn为
其中α1，α2和α3为正设计常数，δγ＝γ-γd，δξ(x)＝ξ(x)-ξ(xd)。定义矢量
关节控制律(关节控制律给定关节位置指令或关节速度指令)定义为
其中
其中，t为当前时刻，zi(0)为zi的初值，zi(0)可选为任意常数。
用于更新估计参数
其中γ为对称正定矩阵，λ和λp为对角正定矩阵。
(3)根据步骤(2)得到的公式(12)控制律更新关节位置指令或速度指令，根据步骤(2)得到的公式(13)自适应律更新估计参数
(4)循环执行步骤(3)，当循环次数趋于无穷时，机械臂末端力调节误差δγ和位置调节误差δξ(x)趋于零。
本发明以两自由度机械臂接触环境时的力/位置控制为例进行阐述。在系统控制体系封闭的情况下，所提出的基于动态反馈的自适应力/位置混合控制方法能够实现对系统非线性和不确定性的恰当补偿，从而保证了机械臂末端力和位置误差的收敛性。
如图1所示，本发明提供了一种自适应动态力/位置混合控制方法，包括下列步骤：
(1)建立控制体系封闭机器人的参数化模型；
柔性环境表面给定为
其中，q＝[q1,q2]t为关节位置矢量，τ＝[τ1,τ2]t为关节控制力矩矢量，参数矢量
内环等效pd控制器参数给定为kd＝diag[1200,600]，kp＝diag[1800,900]。这样得到考虑机器人系统同环境接触时的参数化模型
其中，
(2)设计基于动态反馈的自适应力/位置混合控制律
定义一个矢量z∈r2为：
其中α1，α2和α3为正设计常数，δγ＝γ-γd，δξ(x)＝ξ(x)-ξ(xd)。定义矢量
关节控制律定义为
其中
其中，t为当前时刻，zi(0)为zi的初值，zi(0)可选为任意常数。
用于更新估计参数
其中γ为对称正定矩阵，λ和λp为对角正定矩阵。
在仿真中，设计参数设定为：α1＝0.08，α2＝8，α3＝36，γ＝100i5，λ＝0.02i2，λp＝1000i2，
采用本发明给出的自适应控制方法的仿真结果如图3、图4。
图3给出的是力的误差曲线，图4给出的是位置的误差曲线。
根据图3和图4可以看出：本发明给出的自适应力/位置混合控制方法可有效保证力和位置调节误差的渐近收敛性。
本发明说明书中未作详细描述的内容属本领域专业技术人员的公知技术。