CN104723341A - 基于连接和阻尼配置的柔性关节机械臂的位置控制方法 - Google Patents

Abstract

基于连接和阻尼配置的柔性关节机械臂的位置控制方法，属于机器人控制技术领域。本发明解决了现有的柔性关节机械臂，由于关节柔性较大，导致柔性关节机械臂带宽小、响应慢，机械臂残余振动大；动力学建模困难，致使柔性关节机械臂控制律较为复杂的问题。技术要点为：通过CAD三维模型得到柔性关节机械臂动力学和运动学参数；通过参数辨识得到柔性关节的关键参数；建立柔性关节机械臂的动力学方程；机械臂重力补偿值的求取；电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；求解期望连接矩阵和阻尼矩阵；柔性关节位置控制律的获取。本发明可应用于服务机器人，医疗机器人，和空间机器人的控制。

Description

基于连接和阻尼配置的柔性关节机械臂的位置控制方法

技术领域

本发明涉及机械臂的控制方法，尤其涉及一种基于连接和阻尼配置的柔性关节机械臂的位置控制方法，属于机器人控制技术领域。

背景技术

随着人口老龄化和劳动力资源匮乏等现象日益严重，迫切的需要机械臂代替人类实现部分操作。近些年来，随着机械臂技术的快速发展，小型化、轻量化机械臂已经逐渐地应用于先进服务领域，如空间探索、医疗和服务等行业。与工业机械臂比较，该类机械臂对灵巧性和安全性提出了更高的要求，主要体现在：轻质量、高负载/自重比、丰富的感知系统等。谐波减速器由于其结构紧凑、无齿侧间隙、高负载自重比等优点被广泛应用于轻型机械臂关节设计中。除此之外，为了实现更好的人机协调，力矩传感器也被引入到轻型机械臂关节中，然而，谐波减速器和力矩传感器导致了轻型机械臂的关节柔性，如何克服关节柔性对控制性能的影响成为当前研究的热点问题。相对于传统的工业机械臂，柔性关节机械臂的控制相对于传统的以工业机器人为代表的刚性机械臂控制的难点主要在于：控制力矩无法直接作用到关节端，而是需要通过弹簧阻尼系统传递到关节端，柔性使得轻型机械臂关节存在残余振动，导致控制困难。针对柔性关节机械臂的控制方法，大量的学者进行了广泛地研究，归纳起来主要包括以下几种方法：奇异摄动法，解耦控制和积分反步法，基于无源性理论的控制方法等。奇异摄动方法只适用于较高刚度的机械臂控制；解耦控制和积分反步法计算量过大，并且关节惯性矩阵的求逆的过程可能会导致病态矩阵；在轻型机械臂振动抑制方面，能量整形等方法使得控制存在较大的延迟，影响了轻型机械臂的响应速度。基于无源性理论的关节控制器具有算法简单、便于工程应用、跟踪性能好等优点，更适合多自由度柔性关节机械臂的应用。基于无源性理论的控制器已经在DLR的轻型机械臂中得到了应用，并且取得了优秀的控制效果。传统的基于无源性理论方法是建立在Lyapunov稳定性理论的基础上的，该方法虽然实现了系统的能量整形，但缺少从系统结构配置层面去思考各参数的作用。

发明内容

本发明的目的是提出一种基于连接和阻尼配置的柔性关节机械臂的位置控制方法，以解决针对现有的柔性关节机械臂，由于关节柔性较大，导致柔性关节机械臂带宽小、响应慢，机械臂残余振动大；动力学建模困难，致使柔性关节机械臂控制律较为复杂的问题。

本发明为解决上述技术问题所采用的技术方案是：

本发明所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，是按照以下步骤实现的：

步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数；

步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、阻尼矩阵D、摩擦力矩τ^f；

步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian状态方程；

步骤四、基于柔性关节电机位置信息的机械臂重力补偿值的求取；

步骤五、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；

步骤六、根据匹配方程求解得到期望连接矩阵和阻尼矩阵；

步骤七、基于连接矩阵和阻尼矩阵的柔性关节位置控制律的获取。

本发明的有益效果是：

1、本发明方法在柔性关节机械臂位置控制中，控制器运算量小，适合在多自由度机

械臂上应用。 

2、本发明可以实现对柔性关节机械臂闭环系统的能量整形和结构配置，从系统自身物理性质出发推导出合适的控制律，使得控制律设计得到简化，方便调节控制参数。

3、本发明通过注入耦合阻尼，机械臂抑制抖动的能力提高了30％～50％。

附图说明

图1是本发明的流程图；

图2(a)是传统的柔性关节控制模型示意图，其中K为关节刚度，D为关节阻尼，B为电机惯量，1为连杆，2为电机，3为连杆；

图2(b)是本发明所述的柔性关节控制模型示意图，其中K为关节刚度，D为关节阻尼，B为电机惯量，K_θ为比例增益，r₄为电机端注入的自阻尼，r₅为电机端和关节端之间注入的耦合阻尼，γ为电机转动惯量较小的尺度，1为连杆，2为电机，3为连杆，图中虚线圈标记出了本图相对于图2(a)添加的内容；

图3是柔性关节位置控制的关节角度误差比较曲线图，其中PD为比例-微分控制，PBC为无源性控制，IDA-PBC1为基于D_s(r₅)＝-2.8时的连接和阻尼配置的无源性控制，IDA-PBC2为基于D_s(r₅)＝2.8时的连接和阻尼配置的无源性控制，其具体控制参数如表4 所示；

图4是柔性关节位置控制的关节力矩比较曲线图，其中PD为比例-微分控制，PBC为无源性控制，IDA-PBC1为基于D_s(r₅)＝-2.8时的连接和阻尼配置的无源性控制，IDA-PBC2为基于D_s(r₅)＝2.8时的连接和阻尼配置的无源性控制，其具体控制参数如表4所示。

具体实施方式

结合附图进一步详细说明本发明的具体实施方式。

具体实施方式一：本实施方式所述的一种基于连接和阻尼配置的柔性关节机械臂的位置控制方法，包括以下步骤：

步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数；

步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、阻尼矩阵D、摩擦力矩τ_f；

步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian(port control Hamiltonian,PCH)状态方程；

步骤四、基于柔性关节电机位置信息的机械臂重力补偿值的求取；

步骤五、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；

步骤六、根据匹配方程求解得到期望连接矩阵和阻尼矩阵；

步骤七、基于连接矩阵和阻尼矩阵的柔性关节位置控制律的获取。结合图1理解本实施方式。

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤三所述的柔性关节机械臂的动力学方程的建立过程为：

基于弹簧阻尼模型的柔性关节机械臂的动力学方程为：

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})+g\left(q\right)=\mathrm{\τ}+D{K}^{-1}\stackrel{\·}{\mathrm{\τ}}+{\mathrm{\τ}}_{\mathrm{ext}}\\ B\stackrel{\·\·}{\mathrm{\θ}}+\mathrm{\τ}+D{K}^{-1}\stackrel{\·}{\mathrm{\τ}}={\mathrm{\τ}}_m-{\mathrm{\τ}}_f\\ \mathrm{\τ}=K(\mathrm{\θ}-q)\end{array}\right.---\left(1\right)$

式中——关节角度(rad)；

——关节角速度(rad/s)；

——关节角加速度(rad/s²)；

——电机角度(rad)；

——电机角加速度(rad/s²)；

n——柔性关节机械臂的自由度数目；

——关节刚度矩阵(Nm/rad)，k_i为第i个柔性关节的刚度，i∈[1,n]；

——关节阻尼矩阵(Nm·s/rad)，d_i为第i个柔性关节的阻尼，i∈[1,n]；

B、——分别为电机和连杆的惯性矩阵(kg·m²)；

——离心力哥氏力矩阵；

——关节重力矩阵(Nm)；

τ、τ_m、τ_f、——分别为关节力矩、电机输出力矩、摩擦力矩和外力矩(Nm)；

选择柔性关节机械臂的Hamiltonian函数为：

$H(s,p)=\frac{1}{2}{p}^{T}M{\left(s\right)}^{-1}p+V\left(s\right)---\left(2\right)$

式中——系统转动惯量， $M\left(s\right)=\left[\begin{array}{cc}M\left(q\right)& 0_n\\ 0_n& B\end{array}\right];$

M(q)——连杆的惯性矩阵(kg·m²)；

B——电机的惯性矩阵(kg·m²)；

0_n——n阶0矩阵；

——系统广义位置坐标；

——系统广义动量；

V(s)——系统势能函数，包含了重力势能函数V_g(q)和弹性势能函数V_k(q,θ)，即：

$V\left(s\right)=V_g\left(q\right)+\frac{1}{2}{(\mathrm{\θ}-q)}^{T}K(\mathrm{\θ}-q)=V_g\left(q\right)+V_k(q,\mathrm{\θ})---\left(3\right)$

为了得到柔性关节机械臂的PCHD模型，对动力学方程(1)改写如下：忽略外力和摩擦力的影响，令ψ＝[s,p]^T为状态变量，则将式(1)改写为端口受控的耗散Hamiltonian系统PCHD(Port-controlled Hamiltonian Systems with Dissipation,PCHD)方程的形式：

$\stackrel{\·}{\mathrm{\ψ}}=[J\left(\mathrm{\ψ}\right)-R\left(\mathrm{\ψ}\right)]\frac{\∂H}{\∂\mathrm{\ψ}}\left(\mathrm{\ψ}\right)+\left[\begin{array}{c}{0}_{2n}\\ G\end{array}\right]u---\left(4\right)$

式中J(ψ)——柔性关节机械臂的连接矩阵， $J\left(\mathrm{\ψ}\right)=\left[\begin{array}{cc}{0}_{2n}& I_{2n}\\ -I_{2n}& 0_{2n}\end{array}\right];$

R(ψ)——柔性关节机械臂的阻尼矩阵， $R\left(\mathrm{\ψ}\right)=\left[\begin{array}{cc}{0}_{2n}& 0_{2n}\\ 0_{2n}& \left[\begin{array}{cc}D& -D\\ -D& D\end{array}\right]\end{array}\right];$

u——柔性关节机器人系统的输入变量，在本发明里代表电机输入力矩；

κ(ψ)——输入力矩矩阵， $\mathrm{\κ}\left(\mathrm{\ψ}\right)=\left[\begin{array}{c}{0}_{2n}\\ G\left(\mathrm{\ψ}\right)\end{array}\right],G\left(\mathrm{\ψ}\right)=\left[\begin{array}{c}{0}_n\\ I_n\end{array}\right];$

——H(ψ)的偏微分向量， $\frac{\∂H}{\∂\mathrm{\ψ}}\left(\mathrm{\ψ}\right)=\left[\begin{array}{c}-\frac{1}{2}{\stackrel{\·}{q}}^T\frac{\∂M\left(q\right)}{\∂q}\stackrel{\·}{q}+g\left(q\right)-K(\mathrm{\θ}-q)\\ K(\mathrm{\θ}-q)\\ \stackrel{\·}{q}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right];$

0_2n——2n阶零矩阵；

I_2n——2n阶单位矩阵；

D——关节阻尼矩阵(Nm·s/rad)；

从G(ψ)可以看出，系统广义坐标下自由度个数为2n，而系统输入变量个数为n，所以柔性关节机械臂属于具有n个自由度的欠驱动系统。通过分析式(4)中的连接矩阵和阻尼矩阵的形式可以看出：连接矩阵中次对角线上的元素为单位矩阵；而阻尼矩阵为关于关节阻尼D的半正定对称矩阵。连接矩阵和阻尼矩阵的特点可以用于指导闭环系统的期望连接矩阵和阻尼矩阵的选取。为了提高柔性关节位置控制的精度，控制器中常常引入复杂的摩擦模型，而复杂的摩擦模型可能造成系统很难表述成PCHD形式。因此，

将式(1)改写为端口受控的Hamiltonian状态方程： 

ψ＝f(ψ)+κ(ψ)^Tu   (5) 

其中， $f\left(\mathrm{\ψ}\right)=\left[\begin{array}{c}\stackrel{\·}{q}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{M}\left(q\right)\stackrel{\·}{q}-C(q,\stackrel{\·}{q})\stackrel{\·}{q}-g\left(q\right)+K(\mathrm{\θ}-q)+D(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{q})\\ -K(\mathrm{\θ}-q)-D(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{q})-{\mathrm{\τ}}_f\end{array}\right].$

具体实施方式三：本实施方式与具体实施方式一或二不同的是：步骤四所述的机械臂 重力补偿值的求取过程为：

为了降低关节的重量和体积，柔性关节内部仅集成了电机端的位置传感器和关节输出端的力矩传感器。由于无法直接检测到关节端精确的位置信息，而重力只与关节位置q相关。

基于电机位置信息的重力补偿，是利用检测到的电机位置信息在线计算得到准稳态下的关节位置，然后由该关节位置计算得到重力补偿值。具体方法如下：

性质1保证了机械臂自身关节刚度可以克服重力矩而不产生失效。当外力矩为0时，柔性关节机械臂稳态下的电机位置和关节位置满足集合：Ω:＝{(q,θ)|K(θ-q)＝g(q)}。可以看出在集合Ω中，对于给定的电机位置θ存在唯一的关节位置q与之对应。

证明：假设存在和同时满足集合Ω，即和定义映射Tq:＝θ-K^-1g(q)，则根据性质1，在映射Tq中，和的距离满足下述不等式：

$\left|\right|T{\stackrel{\&OverBar;}{q}}_1-T{\stackrel{\&OverBar;}{q}}_2\left|\right|=\left|\right|K^{-1}g\left({\stackrel{\&OverBar;}{q}}_1\right)-K^{-1}g\left({\stackrel{\&OverBar;}{q}}_2\right)\left|\right|<\mathrm{\&alpha;}/\left|\right|K\left|\right|\left|\right|{\stackrel{\&OverBar;}{q}}_1-{\stackrel{\&OverBar;}{q}}_2\left|\right|<\left|\right|{\stackrel{\&OverBar;}{q}}_1-{\stackrel{\&OverBar;}{q}}_2\left|\right|---\left(6\right)$

根据压缩映射的原理，对于给定的电机位置θ，在集合Ω中存在唯一的不动点使得 此外，在集合Ω中，当(q₀,θ₀)∈Ω时，电机位置θ₀也可以由关节位置q₀计算得到：

θ₀＝q₀+K^-1g(q₀)＝:h_g(q₀)                (7) 

即在集合Ω中θ₀和q₀存在一一对应的关系。公式(7)的逆解为： 

$q_0=h_g^{-1}\left({\mathrm{\&theta;}}_0\right)=:\stackrel{\&OverBar;}{q}\left({\mathrm{\&theta;}}_0\right)---\left(8\right)$

对于任意给定的关节角度值通过迭代的方法得到关节角度值接近真实的逆解迭代公式为：

${\hat{q}}_{i+1}=T{\hat{q}}_i=\mathrm{\&theta;}-K^{-1}g\left({\hat{q}}_i\right)---\left(9\right)$

式中为第i次迭代值，在实际应用中，考虑到实时性要求，将迭代初始值设置为 则经过1-2次迭代就可以得到满意的结果。将基于电机位置信息的重力补偿值记作则：

$\begin{array}{cc}\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)=g\left(q\right)& \&ForAll;(q,\mathrm{\&theta;})\&Element;\mathrm{\&Omega;}---\left(10\right).\end{array}$

在下面的分析中，我们假设已知。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：步骤五所述的最小Hamiltonian函数值的求取过程为：

当电机的转动惯量B越小，关节刚度矩阵K越大，柔性关节模型越接近于刚性关节模型。对于柔性关节机械臂位置控制，

选择期望的Hamiltonian函数为:

$H_d(s,p)=\frac{1}{2}{p}^T{M}_d^{-1}\left(s\right)p+V_d\left(s\right)---\left(11\right)$

式中M_d(s)——闭环系统期望的转动惯量， $M_d\left(s\right)=\left[\begin{array}{cc}M\left(q\right)& 0_{2n}\\ 0_{2n}& \mathrm{\&gamma;B}\end{array}\right];$

γ——电机惯量整形系数，

期望势能函数为：

$V_d\left(s\right)=V_g\left(q\right)+V_k\left(s\right)+\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}-V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)---\left(12\right)$

式中为电机位置误差，期望电机位置为θ_d＝q_d-K^-1g(q_d)，V_g(θ)为基于电机位置信息的重力势能函数，满足： $\&PartialD;V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)/\&PartialD;\mathrm{\&theta;}=\stackrel{\&OverBar;}{g}{\left(\mathrm{\&theta;}\right)}^T,V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)=V_g\left(\stackrel{\&OverBar;}{q}\right)+\frac{1}{2}{(\mathrm{\&theta;}-\stackrel{\&OverBar;}{q})}^{T}K(\mathrm{\&theta;}-\stackrel{\&OverBar;}{q}),$ 其中根据柔性关节机械臂性质，下述不等式成立：

$|V_g\left(q_2\right)-V_g\left(q_1\right)-{(q_2-q_1)}^{T}g\left(q_1\right)|\&le;\frac{1}{2}\mathrm{\&alpha;}{\left|\right|q_2-q_1\left|\right|}_2^2---\left(13\right)$

对V_d(q,θ)项经过转换得到：

$\begin{array}{c}{V}_d(q,\mathrm{\&theta;})=V_g\left(q\right)+\frac{1}{2}{(\mathrm{\&theta;}-q)}^{T}K(\mathrm{\&theta;}-q)+\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}-V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)\\ \&GreaterEqual;\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}+\frac{1}{2}{(\stackrel{\&OverBar;}{q}-q)}^T(K-\mathrm{\&alpha;I})(\stackrel{\&OverBar;}{q}-q)\&GreaterEqual;0\end{array}---\left(14\right)$

公式(14)中，当且仅当θ＝θ_d和时等号成立，由(q_d,θ_d)∈Ω以及θ＝θ_d，在关于电机位置信息与关节位置信息的集合Ω中电机位置信息与关节位置信息的一一对应关系得出s_d＝(q_d,θ_d)＝arg minV_d(s)，则ψ_d＝(s_d,0)＝arg minH_d(s)，H_d在平衡位置ψ_d处取得最小值，其中，s_d为期望的广义坐标函数，V_d(s)为期望势能函数。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤六所述的根 据匹配方程求解得到期望连接矩阵和阻尼矩阵具体如下：

根据柔性关节机械臂PCHD方程(4)中连接矩阵和阻尼矩阵的结构，假设期望的连接矩阵和阻尼矩阵如下：

$J_d=\left[\begin{array}{cccc}{0}_n& J_{12}& J_{13}& J_{14}\\ -J_{12}^T& 0_n& J_{23}& J_{24}\\ -J_{13}^T& -J_{23}^T& 0_n& J_{34}\\ -J_{14}^T& -J_{24}^T& -J_{34}^T& 0_n\end{array}\right],R_d=\left[\begin{array}{cccc}{r}_1& 0_n& 0_n& 0_n\\ 0_n& r_2& 0_n& 0_n\\ 0_n& 0_n& r_3& -r_5\\ 0_n& 0_n& -r_5& r_4\end{array}\right]\&GreaterEqual;0;$

式中J₁₂、J₁₃、J₁₄、J₂₃、J₂₄、J₃₄、r₁、r₂、r₃、r₄、r₅为待求解的连接矩阵和阻尼矩阵的参数，

选择矩阵κ(ψ)的左零化子空间为： ${\mathrm{\&kappa;}}^{\&perp;}\left(\mathrm{\&psi;}\right)=\left[\begin{array}{cccc}{I}_n& 0_n& 0_n& 0_n\\ 0_n& I_n& 0_n& 0_n\\ 0_n& 0_n& I_n& 0_n\end{array}\right].$

H_d(ψ)对各状态变量的偏微分得到：

$\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)=\left[\begin{array}{c}-\frac{1}{2}{\stackrel{\&CenterDot;}{q}}^T\frac{\&PartialD;M\left(q\right)}{\&PartialD;q}\stackrel{\&CenterDot;}{q}+g\left(\stackrel{\&CenterDot;}{q}\right)-K(\mathrm{\&theta;}-\stackrel{\&CenterDot;}{q})\\ K(\mathrm{\&theta;}-q)+K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)-\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)\\ \stackrel{\&CenterDot;}{q}\\ B^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]$

根据状态方程(5)和上述假设求解匹配方程：

${\mathrm{\&kappa;}}^{\&perp;}\left(\mathrm{\&psi;}\right)f\left(\mathrm{\&psi;}\right)={\mathrm{\&kappa;}}^{\&perp;}\left(\mathrm{\&psi;}\right)[J_d\left(\mathrm{\&psi;}\right)-R_d\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}---\left(15\right)$

得到各参数值为：

$\begin{array}{c}{r}_1=0_n,J_{12}=0_n,J_{13}=I_n,J_{14}=0_n;\\ r_2=0_n,J_{23}=0_n,J_{24}=B_{\mathrm{\&theta;}}^{-1}B;\\ r_3=D,J_{34}+r_5=B_{\mathrm{\&theta;}}^{-1}\mathrm{BD};\\ \&ForAll;r_4\end{array}---\left(16\right)$

将公式(16)代入J_d、R_d中，得到期望的连接矩阵和阻尼矩阵的表达式为：

$J_d=\left[\begin{array}{cccc}{0}_n& 0_n& I_n& 0_n\\ 0_n& 0_n& 0_n& B_{\mathrm{\&theta;}}^{-1}B\\ -I_n& 0_n& 0_n& B_{\mathrm{\&theta;}}^{-1}\mathrm{BD}-r_5\\ 0& -{\left[B_{\mathrm{\&theta;}}^{-1}B\right]}^T& -{[B_{\mathrm{\&theta;}}^{-1}\mathrm{BD}-r_5]}^T& 0_n\end{array}\right],R_d=\left[\begin{array}{cccc}{0}_n& 0_n& 0_n& 0_n\\ 0_n& 0_n& 0_n& 0_n\\ 0_n& 0_n& D& -r_5\\ 0_n& 0_n& -r_5^T& r_4\end{array}\right].$

由R_d正定的条件可以推导出闭环系统稳定的充分必要条件为此时系统的能量平衡方程变为：

$\begin{array}{c}{\stackrel{\&CenterDot;}{H}}_d\left(\mathrm{\&psi;}\right)=\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\frac{{\&PartialD;}^T{H}_d}{\&PartialD;\mathrm{\&psi;}}[J_d\left(\mathrm{\&psi;}\right)-R_d\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}=-\frac{{\&PartialD;}^T{H}_d}{\&PartialD;\mathrm{\&psi;}}{R}_d\left(\mathrm{\&psi;}\right)\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}\\ =-{\stackrel{\&CenterDot;}{q}}^{T}D\stackrel{\&CenterDot;}{q}-{\left(B^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right)}^T{r}_4\left(B^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right)+2{\stackrel{\&CenterDot;}{q}}^T{r}_5\left(B^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right)\&le;0\end{array}---\left(17\right).$

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤七所述的柔性关节位置控制律的获取过程为：将步骤六得到的连接矩阵和阻尼矩阵代入控制律表达式：

$u=\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)={\left[{\mathrm{\&kappa;}}^T\left(\mathrm{\&psi;}\right)\mathrm{\&kappa;}\left(\mathrm{\&psi;}\right)\right]}^{-1}{\mathrm{\&kappa;}}^T\left(\mathrm{\&psi;}\right)\&times;\left\{\right[J_d\left(\mathrm{\&psi;}\right)-R_d\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)-f\left(\mathrm{\&psi;}\right)\}---\left(18\right)$

得到柔性关节机械臂位置控制律为：

$\begin{array}{c}\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)=(I-B_{\mathrm{\&theta;}}^{-1}B)K(\mathrm{\&theta;}-q)+B_{\mathrm{\&theta;}}^{-1}B(-K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right))+{\mathrm{\&tau;}}_f+D(\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\stackrel{\&CenterDot;}{q})\\ -B_{\mathrm{\&theta;}}^{-1}B[(D-B^{-1}{B}_{\mathrm{\&theta;}}2r_5)\stackrel{\&CenterDot;}{q}+B^{-1}{B}_{\mathrm{\&theta;}}{r}_4{B}^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}]\end{array}---\left(19\right)$

式中：——关节角度(rad)；

——关节角速度(rad/s)；

——电机角度(rad)；

——电机角速度(rad/s)；

K——关节刚度矩阵(Nm/rad)；

——关节阻尼矩阵(Nm·s/rad)；

B——分别为电机惯性矩阵(kg·m²)；

τ_f——摩擦力矩(Nm)；

B_θ——闭环系统电机转动惯量；

r₄——电机端注入的自阻尼；

r₅——电机端和关节端之间注入的耦合阻尼；

K_d——位置误差的比例增益；

对式(19)简化，进一步改写成关于位置和力矩的全状态反馈形式为：

$\begin{array}{c}\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)=\mathrm{\&gamma;}(-K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)-K_v\stackrel{\&CenterDot;}{\mathrm{\&theta;}})+\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)+{\mathrm{\&tau;}}_f\\ +(I-\mathrm{\&gamma;})(\mathrm{\&tau;}-\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right))+D{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}-\mathrm{\&gamma;}{K}_s{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\end{array}---\left(20\right)$

式中K_s＝2r₅/γ-D、K_v＝r₄/γ²+D-2r₅/γ分别为力矩微分的反馈系数和闭环系统的 速度反馈系数，γ——电机惯量整形系数，将式(20)带入公式(1)得到柔性关节机械臂位置控制的动力学方程为：

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+g\left(q\right)=\mathrm{\&tau;}+D{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\\ B_{\mathrm{\&theta;}}\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+K_v\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+\mathrm{\&tau;}+K_s{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}=\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)\\ \mathrm{\&tau;}=K(\mathrm{\&theta;}-q)\end{array}\right.---\left(21\right).$

通过公式(21)即可证明本发明的控制律能够保证系统的稳定性。

在 ${\mathrm{Dr}}_4\&GreaterEqual;r_5^2$ 条件下， ${\stackrel{\&CenterDot;}{H}}_d\left(\mathrm{\&psi;}\right)=0$ 成立的条件当且仅当 ${\stackrel{\&CenterDot;}{s}}^T={\left[\begin{array}{cc}{\stackrel{\&CenterDot;}{q}}^T& {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}^T\end{array}\right]}^T=0,$ 将 ${\stackrel{\&CenterDot;}{s}}^T=0$ 带入式(20)得到θ＝θ_d。由于在平衡状态下(q,θ)∈Ω，根据集合Ω的性质，存在唯一的q_d与θ_d对应。因此集合的最大不变集为 $\mathrm{\&psi;}={\left[\begin{array}{cc}{s}_d^T& 0\end{array}\right]}^T,$ 从而证明了在平衡点 $\mathrm{\&psi;}={\left[\begin{array}{cc}{s}_d^T& 0\end{array}\right]}^T$ 处渐进稳定。

本发明的实验验证如下：

结合图2(a)，图2(b)，图3及图4，以6自由度仿人形机械臂为例，该机械臂具有6个自由度，其中肩部和肘部的4个模块化关节采用谐波减速器与无刷直流电机的结构；手腕关节采用盘式电机、谐波减速器、同步齿形皮带和4个伞齿轮组成的差动机构等的驱动、传动方案。肩部和肘部关节的每个关节都安装有电位计和磁编码器用于检测关节和电机位置，关节内具有电流传感器和力矩传感器分别检测电机输出电流和关节输出力矩，无刷直流电机采用矢量控制的方法保证电机输出力矩与输出电流的比例关系。

具体步骤如下：

步骤一、通过CAD建模得到准确的机械臂动力学和运动学参数，以6自由度仿人形机械臂为例，其中D-H参数和机械臂连杆参数分别如表1和表2所示：

表1仿人形机械臂D-H参数表

表2连杆参数名义值

步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度、阻尼等；

根据矢量控制检测的电流信息和力矩信息，得到肩部和肘部4个关节的刚度和阻尼分别如表3所示：

表3连杆参数名义值

步骤三、建立原始柔性关节机器人系统的动力学方程(1)，并将其改写为端口受控的Hamiltonian(port control Hamiltonian,PCH)方程的形式(4)；最后得到状态方程的形式(5)。

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})+g\left(q\right)=\mathrm{\&tau;}+D{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}+{\mathrm{\&tau;}}_{\mathrm{ext}}\\ B\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+\mathrm{\&tau;}+D{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}={\mathrm{\&tau;}}_m-{\mathrm{\&tau;}}_f\\ \mathrm{\&tau;}=K(\mathrm{\&theta;}-q)\end{array}\right.---\left(22\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=[J\left(\mathrm{\&psi;}\right)-R\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)+\left[\begin{array}{c}{0}_{2n}\\ G\end{array}\right]u---\left(23\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=f\left(\mathrm{\&psi;}\right)+\mathrm{\&kappa;}{\left(\mathrm{\&psi;}\right)}^{T}u---\left(24\right)$

步骤四、根据式(9)迭代方法和公式(10)得到基于电机位置的重力补偿。

${\hat{q}}_{i+1}=T{\hat{q}}_i=\mathrm{\&theta;}-K^{-1}g\left({\hat{q}}_i\right)---\left(25\right)$

$\begin{array}{cc}\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)=g\left(q\right)& \&ForAll;(q,\mathrm{\&theta;})\&Element;\mathrm{\&Omega;}---\left(26\right)\end{array}$

步骤五、给定期望的Hamiltonian能量函数，使其在期望的平衡位置处取得最小值；按式(11)选择能量函数。

$H_d(s,p)=\frac{1}{2}{p}^T{M}_d^{-1}\left(s\right)p+V_d\left(s\right)---\left(27\right)$

步骤六、假设期望的连接和阻尼矩阵形式，并根据匹配方程求解得到期望连接矩阵和阻尼矩阵；

步骤七：得到基于IDA-PBC方法的柔性关节位置控制律，参数取值；

步骤八：关节位置控制实验及结果。

选择γ＝4，r₅＝-4Dγ，从0.2s开始关节以0.785rad/s匀速运动到水平位置，PD、PBC和IDA-PBC控制参数见表4，括号内的参数代表IDA-PBC控制律式(28)中参数。PD代表比例-微分控制，PBC代表一般的无源性控制，IDA-PBC代表基于连接和阻尼配置的无源性控制方法。

$\begin{array}{c}\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)=\mathrm{\&gamma;}(-K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)-K_v\stackrel{\&CenterDot;}{\mathrm{\&theta;}})+\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)+{\mathrm{\&tau;}}_f\\ +(I-\mathrm{\&gamma;})(\mathrm{\&tau;}-\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right))+D{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}-\mathrm{\&gamma;}{K}_s{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\end{array}---\left(28\right).$

表4位置控制器的参数增益表

结果显示：PD控制的抖动幅度和持续时间都是上述4种方法最大的一个，而其他3种方法都可以一定程度上抑制抖动；IDA-PBC1方法与PBC方法具有相似的控制性能，进一步验证两种控制的等效关系；此外，通过增加耦合阻尼r₅，可以使闭环系统更快地达到稳定状态，如实验中r₅＝4Dγ的抖动抑制效果最好。然而，从位置误差曲线可以看出，上述方法忽略了摩擦对柔性关节机械臂控制性能的影响，导致存在较大的跟踪误差和静态误差。本发明可应用于服务机器人，医疗机器人，和空间机器人的控制。

Claims (6)

1.一种基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于所述方法包括以下步骤：

步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数；

步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、阻尼矩阵D、摩擦力矩τ_f；

步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian状态方程；

步骤四、基于柔性关节电机位置信息的机械臂重力补偿值的求取；

步骤五、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；

步骤六、根据匹配方程求解得到期望连接矩阵和阻尼矩阵；

步骤七、基于连接矩阵和阻尼矩阵的柔性关节位置控制律的获取。

2.根据权利要求1所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于步骤三所述的柔性关节机械臂的动力学方程的建立过程为：

基于弹簧阻尼模型的柔性关节机械臂的动力学方程为：

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})+g\left(q\right)=\mathrm{\&tau;}+{\mathrm{DK}}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}+{\mathrm{\&tau;}}_{\mathrm{ext}}\\ B\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+\mathrm{\&tau;}+{\mathrm{DK}}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}={\mathrm{\&tau;}}_m-{\mathrm{\&tau;}}_f\\ \mathrm{\&tau;}=K(\mathrm{\&theta;}-q)\end{array}\right.---\left(1\right)$

式中——关节角度；

——关节角速度；

——关节角加速度；

——电机角度；

——电机角加速度；

n——柔性关节机械臂的自由度数目；

——关节刚度矩阵，k_i为第i个柔性关节的刚度，i∈[1,n]；

——关节阻尼矩阵，d_i为第i个柔性关节的阻尼，i∈[1,n]；

——分别为电机和连杆的惯性矩阵；

——离心力哥氏力矩阵；

——关节重力矩阵；

τ、τ_m、τ_f、——分别为关节力矩、电机输出力矩、摩擦力矩和外力矩；

选择柔性关节机械臂的Hamiltonian函数为：

$H(s,p)=\frac{1}{2}{p}^{T}M{\left(s\right)}^{-1}p+V\left(s\right)---\left(2\right)$

式中——系统转动惯量， $M\left(s\right)=\left[\begin{array}{cc}M\left(q\right)& 0_n\\ 0_n& B\end{array}\right];$

M(q)——连杆的惯性矩阵；

B——电机的惯性矩阵；

0_n——n阶0矩阵；

——系统广义位置坐标；

——系统广义动量；

V(s)——系统势能函数，包含了重力势能函数V_g(q)和弹性势能函数V_k(q,θ)，即：

$V\left(s\right)=V_g\left(q\right)+\frac{1}{2}{(\mathrm{\&theta;}-q)}^{T}K(\mathrm{\&theta;}-q)=V_g\left(q\right)+V_k(q,\mathrm{\&theta;})---\left(3\right)$

对动力学方程(1)改写如下：忽略外力和摩擦力的影响，令ψ＝[s,p]^T为状态变量，则将式(1)改写为端口受控的耗散Hamiltonian系统PCHD方程的形式：

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=[J\left(\mathrm{\&psi;}\right)-R\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)+\left[\begin{array}{c}{0}_{2n}\\ G\end{array}\right]u---\left(4\right)$

式中J(ψ)——柔性关节机械臂的连接矩阵， $J\left(\mathrm{\&psi;}\right)=\left[\begin{array}{cc}{0}_{2n}& I_{2n}\\ -I_{2n}& 0_{2n}\end{array}\right];$

R(ψ)——柔性关节机械臂的阻尼矩阵， $R\left(\mathrm{\&psi;}\right)=\left[\begin{array}{cc}{0}_{2n}& 0_{2n}\\ 0_{2n}& \left[\begin{array}{cc}D& -D\\ -D& D\end{array}\right]\end{array}\right];$

u——代表电机输入力矩；

κ(ψ)——输入力矩矩阵， $\mathrm{\&kappa;}\left(\mathrm{\&psi;}\right)=\left[\begin{array}{c}{0}_{2n}\\ G\left(\mathrm{\&psi;}\right)\end{array}\right],G\left(\mathrm{\&psi;}\right)=\left[\begin{array}{c}{0}_n\\ I_n\end{array}\right];$

——H(ψ)的偏微分向量， $\frac{\&PartialD;H}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)=\left[\begin{array}{c}-\frac{1}{2}{\stackrel{\&CenterDot;}{q}}^T\frac{\&PartialD;M\left(q\right)}{\&PartialD;q}\stackrel{\&CenterDot;}{q}+g\left(q\right)-K(\mathrm{\&theta;}-q)\\ K(\mathrm{\&theta;}-q)\\ \stackrel{\&CenterDot;}{q}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right];$

0_2n——2n阶零矩阵；

I_2n——2n阶单位矩阵；

D——关节阻尼矩阵；

将式(1)改写为端口受控的Hamiltonian状态方程：

$\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=f\left(\mathrm{\&psi;}\right)+\mathrm{\&kappa;}{\left(\mathrm{\&psi;}\right)}^{T}u---\left(5\right)$

其中， $f\left(\mathrm{\&psi;}\right)=\left[\begin{array}{c}\stackrel{\&CenterDot;}{q}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \stackrel{\&CenterDot;}{M}\left(q\right)\stackrel{\&CenterDot;}{q}-C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}-g\left(q\right)+K(\mathrm{\&theta;}-q)+D(\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\stackrel{\&CenterDot;}{q})\\ -K(\mathrm{\&theta;}-q)-D(\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\stackrel{\&CenterDot;}{q})-{\mathrm{\&tau;}}_f\end{array}\right].$

3.根据权利要求2所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于步骤四所述的机械臂重力补偿值的求取过程为：

对于任意给定的关节角度值通过迭代的方法得到关节角度值迭代公式为：

${\hat{q}}_{i+1}=T{\hat{q}}_i=\mathrm{\&theta;}-K^{-1}g\left({\hat{q}}_i\right)---\left(6\right)$

式中为第i次迭代值，将基于电机位置信息的重力补偿值记作则：

$\begin{array}{cc}\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)=g\left(q\right)& \&ForAll;(q,\mathrm{\&theta;})\&Element;\mathrm{\&Omega;}\end{array}----\left(7\right).$

4.根据权利要求3所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于步骤五所述的最小Hamiltonian函数值的求取过程为：

选择期望的Hamiltonian函数为:

$H_d(s,p)=\frac{1}{2}{p}^T{M}_d^{-1}\left(s\right)p+V_d\left(s\right)---\left(8\right)$

式中M_d(s)——闭环系统期望的转动惯量， $M_d\left(s\right)=\left[\begin{array}{cc}M\left(q\right)& 0_{2n}\\ 0_{2n}& \mathrm{\&gamma;B}\end{array}\right];$

γ——电机惯量整形系数，

期望势能函数为：

$V_d\left(s\right)=V_g\left(q\right)+V_k\left(s\right)+\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}-V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)---\left(9\right)$

式中为电机位置误差，期望电机位置为θ_d＝q_d-K^-1g(q_d)，V_g(θ)为基于电机位置信息的重力势能函数，满足： $\&PartialD;V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)/\&PartialD;\mathrm{\&theta;}=\stackrel{\&OverBar;}{g}{\left(\mathrm{\&theta;}\right)}^T,V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)=V_g\left(\stackrel{\&OverBar;}{q}\right)+\frac{1}{2}{(\mathrm{\&theta;}-\stackrel{\&OverBar;}{q})}^{T}K(\mathrm{\&theta;}-\stackrel{\&OverBar;}{q}),$ 其中 $(\stackrel{\&OverBar;}{q},\mathrm{\&theta;})\&Element;\mathrm{\&Omega;};$

根据柔性关节机械臂性质，下述不等式成立：

$|V_g\left(q_2\right)-V_g\left(q_1\right)-{(q_2-q_1)}^{T}g\left(q_1\right)|\&le;\frac{1}{2}\mathrm{\&alpha;}{\left|\right|q_2-q_1\left|\right|}_2^2---\left(10\right)$

对V_d(q,θ)项经过转换得到：

$\begin{array}{c}{V}_d(q,\mathrm{\&theta;})=V_g\left(q\right)+\frac{1}{2}{(\mathrm{\&theta;}-q)}^{T}K(\mathrm{\&theta;}-q)+\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}-V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)\\ =\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}+\frac{1}{2}{(\stackrel{\&OverBar;}{q}-q)}^{T}K(\stackrel{\&OverBar;}{q}-q)+{(\mathrm{\&theta;}-\stackrel{\&OverBar;}{q})}^{T}K(\stackrel{\&OverBar;}{q}-q)+V_g\left(q\right)-V_g\left(\stackrel{\&OverBar;}{q}\right)\\ \&GreaterEqual;\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}+\frac{1}{2}{(\stackrel{\&OverBar;}{q}-q)}^{T}K(\stackrel{\&OverBar;}{q}-q)-|V_g\left(q\right)-V_g\left(\stackrel{\&OverBar;}{q}\right)-{(q-\stackrel{\&OverBar;}{q})}^{T}g\left(\stackrel{\&OverBar;}{q}\right)|\\ \&GreaterEqual;\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}+\frac{1}{2}{(\stackrel{\&OverBar;}{q}-q)}^T(K-\mathrm{\&alpha;I})(\stackrel{\&OverBar;}{q}-q)\&GreaterEqual;0\end{array}$

$\begin{array}{c}{V}_d(q,\mathrm{\&theta;})=V_g\left(q\right)+\frac{1}{2}{(\mathrm{\&theta;}-q)}^{T}K(\mathrm{\&theta;}-q)+\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}-V_{\stackrel{\&OverBar;}{g}}\left(\mathrm{\&theta;}\right)\\ \&GreaterEqual;\frac{1}{2}{\widetilde{\mathrm{\&theta;}}}^T{K}_d\widetilde{\mathrm{\&theta;}}+\frac{1}{2}{(\stackrel{\&OverBar;}{q}-q)}^T(K-\mathrm{\&alpha;I})(\stackrel{\&OverBar;}{q}-q)\&GreaterEqual;0\end{array}---\left(11\right)$

公式(14)中，当且仅当θ＝θ_d和时等号成立，由(q_d,θ_d)∈Ω以及θ＝θ_d，在关于电机位置信息与关节位置信息的集合Ω中电机位置信息与关节位置信息的一一对应关系得出s_d＝(q_d,θ_d)＝arg minV_d(s)，则ψ_d＝(s_d,0)＝arg minH_d(s)，H_d在平衡位置ψ_d处取得最小值，其中，s_d为期望的广义坐标函数，V_d(s)为期望势能函数。

5.根据权利要求4所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于步骤六所述的根据匹配方程求解得到期望连接矩阵和阻尼矩阵具体如下：

根据柔性关节机械臂PCHD方程(4)中连接矩阵和阻尼矩阵的结构，假设期望的连接矩阵和阻尼矩阵如下：

$J_d=\left[\begin{array}{cccc}{0}_n& J_{12}& J_{13}& J_{14}\\ -J_{12}^T& 0_n& J_{23}& J_{24}\\ -J_{13}^T& -J_{23}^T& 0_n& J_{34}\\ -J_{14}^T& -J_{24}^T& -J_{34}^T& 0_n\end{array}\right],R_d=\left[\begin{array}{cccc}{r}_1& 0_n& 0_n& 0_n\\ 0_n& r_2& 0_n& 0_n\\ 0_n& 0_n& r_3& -r_5\\ 0_n& 0_n& -r_5& r_4\end{array}\right]\&GreaterEqual;0;$

式中J₁₂、J₁₃、J₁₄、J₂₃、J₂₄、J₃₄、r₁、r₂、r₃、r₄、r₅为待求解的连接矩阵和阻尼矩阵的参数，

根据状态方程(5)和上述假设求解匹配方程：

${\mathrm{\&kappa;}}^{\&perp;}\left(\mathrm{\&psi;}\right)f\left(\mathrm{\&psi;}\right)={\mathrm{\&kappa;}}^{\&perp;}\left(\mathrm{\&psi;}\right)[J_d\left(\mathrm{\&psi;}\right)-R_d\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}---\left(12\right)$

得到各参数值为：

$\begin{array}{c}{r}_1=0_n,J_{12}=0_n,J_{13}=I_n,J_{14}=0_n;\\ r_2=0_n,J_{23}=0_n,J_{24}=B_{\mathrm{\&theta;}}^{-1}B;\\ r_3=D,J_{34}+r_5=B_{\mathrm{\&theta;}}^{-1}\mathrm{BD};\\ \&ForAll;r_4\end{array}---\left(13\right)$

将公式(16)代入J_d、R_d中，得到期望的连接矩阵和阻尼矩阵的表达式为：

$J_d=\left[\begin{array}{cccc}{0}_n& 0_n& I_n& 0_n\\ 0_n& 0_n& 0_n& B_{\mathrm{\&theta;}}^{-1}B\\ -I_n& 0_n& 0_n& B_{\mathrm{\&theta;}}^{-1}\mathrm{BD}-r_5\\ 0& -{\left[B_{\mathrm{\&theta;}}^{-1}B\right]}^T& -{[B_{\mathrm{\&theta;}}^{-1}\mathrm{BD}-r_5]}^T& 0_n\end{array}\right],R_d=\left[\begin{array}{cccc}{0}_n& 0_n& 0_n& 0_n\\ 0_n& 0_n& 0_n& 0_n\\ 0_n& 0_n& D& -r_5\\ 0_n& 0_n& -r_5^T& r_4\end{array}\right].$

6.根据权利要求5所述的基于连接和阻尼配置的柔性关节机械臂的位置控制方法，其特征在于步骤七所述的柔性关节位置控制律的获取过程为：将步骤六得到的连接矩阵和阻尼矩阵代入控制律表达式：

$u=\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)={\left[{\mathrm{\&kappa;}}^T\left(\mathrm{\&psi;}\right)\mathrm{\&kappa;}\left(\mathrm{\&psi;}\right)\right]}^{-1}{\mathrm{\&kappa;}}^T\left(\mathrm{\&psi;}\right)\&times;\left\{\right[J_d\left(\mathrm{\&psi;}\right)-R_d\left(\mathrm{\&psi;}\right)]\frac{\&PartialD;H_d}{\&PartialD;\mathrm{\&psi;}}\left(\mathrm{\&psi;}\right)-f\left(\mathrm{\&psi;}\right)\}---\left(14\right)$

得到柔性关节机械臂位置控制律为：

$\begin{array}{c}\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)=(I-B_{\mathrm{\&theta;}}^{-1}B)K(\mathrm{\&theta;}-q)+B_{\mathrm{\&theta;}}^{-1}B(-K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right))+{\mathrm{\&tau;}}_f+D(\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-\stackrel{\&CenterDot;}{q})\\ -B_{\mathrm{\&theta;}}^{-1}B[(D-B^{-1}{B}_{\mathrm{\&theta;}}2r_5)\stackrel{\&CenterDot;}{q}+B^{-1}{B}_{\mathrm{\&theta;}}{r}_4{B}^{-1}{B}_{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}]\end{array}---\left(15\right)$

式中：——关节角度；

——关节角速度；

——电机角度；

——电机角速度；

K——关节刚度矩阵；

——关节阻尼矩阵；

B——分别为电机惯性矩阵；

τ_f——摩擦力矩；

B_θ——闭环系统电机转动惯量；

r₄——电机端注入的自阻尼；

r₅——电机端和关节端之间注入的耦合阻尼；

K_d——位置误差的比例增益；

对式(19)简化，进一步改写成关于位置和力矩的全状态反馈形式为：

$\begin{array}{c}\mathrm{\&beta;}\left(\mathrm{\&psi;}\right)=\mathrm{\&gamma;}(-K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)-K_v\stackrel{\&CenterDot;}{\mathrm{\&theta;}})+\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)+{\mathrm{\&tau;}}_f\\ +(I-\mathrm{\&gamma;})(\mathrm{\&tau;}-\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right))+{\mathrm{DK}}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}-\mathrm{\&gamma;}{K}_s{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\end{array}---\left(16\right)$

式中K_s＝2r₅/γ-D、K_v＝^r ₄/γ²+D-2r₅/γ分别为力矩微分的反馈系数和闭环系统的速度反馈系数，γ——电机惯量整形系数，将式(20)带入公式(1)得到柔性关节机械臂位置控制的动力学方程为：

$\left\{\begin{array}{c}M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+g\left(q\right)=\mathrm{\&tau;}+{\mathrm{DK}}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\\ B_{\mathrm{\&theta;}}\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+K_v\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+K_d(\mathrm{\&theta;}-{\mathrm{\&theta;}}_d)+\mathrm{\&tau;}+K_s{K}^{-1}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}=\stackrel{\&OverBar;}{g}\left(\mathrm{\&theta;}\right)\\ \mathrm{\&tau;}=K(\mathrm{\&theta;}-q)\end{array}\right.---\left(17\right).$