基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，属于机器人控制技术领域。本发明解决了传统的机械臂控制方法在柔性关节机械臂控制中，残余振动大，无法达到稳定控制的目的问题。技术要点为：通过CAD三维模型得到柔性关节机械臂动力学和运动学参数；通过参数辨识得到柔性关节的关键参数；建立柔性关节机械臂的动力学方程；建立基于电机位置的重力和外力补偿算法；机械臂重力和外力补偿值的求取；电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；求解阻抗控制中的期望连接矩阵和阻尼矩阵；基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取。本发明可应用于服务机器人，医疗机器人，和空间机器人的控制。
1.一种基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于所述方法包括以下步骤：步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数，包括几何参数、质量、转动惯量；
步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、关节阻尼矩阵D、关节摩擦力矩τf；
步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian状态方程；
步骤四、基于步骤二获得的关节刚度矩阵K和关节阻尼矩阵D，建立基于电机位置的重力和外力补偿算法；
步骤五、基于柔性关节电机位置信息的机械臂重力和外力补偿值的求取；
步骤六、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；
步骤七、根据匹配方程求解得到阻抗控制中的期望连接矩阵和阻尼矩阵；
步骤八、基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取。
2.根据权利要求1所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤三所述的建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian状态方程的具体过程为：基于弹簧阻尼模型的柔性关节机械臂的动力学方程为：式中 ——关节角度；
τ、τm、τf、 ——分别为关节力矩、电机输出力矩、摩擦力矩和外力矩；
选择柔性关节机械臂的Hamiltonian函数为：式中 ——系统转动惯量，
M(q)——连杆的惯性矩阵(kg·m )；B——电机的惯性矩阵(kg·m )；
V(s)——系统势能函数，包含了重力势能函数Vg(q)和弹性势能函数Vk(q,θ)，即：对动力学方程(1)改写如下：忽略外力和摩擦力的影响，且令ψ＝[s,p]T为状态变量，则将式(1)改写为端口受控的耗散Hamiltonian系统方程的形式：式中J(ψ)——柔性关节机械臂的连接矩阵，R(ψ)——柔性关节机械臂的阻尼矩阵，u——代表电机输入力矩；
将式(1)改写为端口受控的Hamiltonian状态方程：其中，
3.根据权利要求2所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤四所述的基于步骤二获得的关节刚度矩阵K和关节阻尼矩阵D，建立基于电机位置的重力和外力补偿算法，具体过程如下：通过设定期望的Λx＝Λ(x)消去式(6)中的最后一项，期望的质量-弹簧-阻尼的模型简化为：
4.根据权利要求3所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤五所述的基于柔性关节电机位置信息的机械臂重力和外力补偿值的求取过程为：保证机械臂在平衡状态下，外力Fext与位置偏差 满足如下关系：将τext＝J(q)TFext代入式(1)后，得到机器人稳态方程为：K(θ-q)＝g(q)-J(q)TKx(x-xd) (9)其中，x表示在关节位置x处的刚度矩阵；
通过迭代的方法计算得到稳态时电机位置θ对应的关节位置 迭代方程为：迭代的初始值选择为电机角度，即 经过1～2次迭代达到近似的稳态位置
5.根据权利要求4所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤六所述的电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取过程为：对于在外力作用下的柔性关节机器人阻抗控制，选择期望Hamiltonian函数为:式中Md(s)——闭环系统的转动惯量，γ——电机惯量整形系数，
满足 将式(13)代入式12)中得到Vd(s)满足下述不等式：当且仅当 且x＝xd时等式成立，Vd(s)取得最小值0，当ψd＝(sd,0)时期望能量函数Hd(ψ)达到最小值，即ψd＝(sd,0)＝arg min Hd(ψ)，其中ψd为期望的平衡位置，sd为期望的广义坐标函数，Vd(s)为期望势能函数，Vg(q)为重力势能函数。
6.根据权利要求5所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤七所述的根据匹配方程求解得到阻抗控制中的期望连接矩阵和阻尼矩阵的具体过程为：假设期望的连接矩阵和阻尼矩阵如下：式中J12、J13、J14、J23、J24、J34、r1、r2、r3、r4、r5为待求解期望的连接矩阵和阻尼矩阵参数；
根据状态方程(5)和上述假设求解匹配方程：得到各参数值为：
将公式(13)代入Jd、Rd中，得到期望的连接矩阵和阻尼矩阵的表达式为：
7.根据权利要求6所述的基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，其特征在于步骤八所述的基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取过程为：将步骤七得到的连接矩阵和阻尼矩阵代入控制律表达式：得到柔性关节机器人阻抗控制律关于位置和力矩的全状态反馈形式为：式中，Ks＝2r5/γ-D、Kv＝r4/γ2+D-2r5/γ分别为力矩微分的反馈系数和闭环系统阻尼；
的正定矩阵，令τm＝β(ψ)并带入式(1)得到闭环系统方程：
基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法
技术领域
[0001] 本发明涉及机械臂控制方法，尤其涉及一种基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，属于机器人控制技术领域。
背景技术
[0002] 随着人口老龄化和劳动力资源匮乏等现象日益严重，迫切的需要机械臂代替人类实现部分操作。近些年来，随着机械臂技术的快速发展，小型化、轻量化机械臂已经逐渐地应用于先进服务领域，如空间探索、医疗和服务等行业。与工业机械臂比较，该类机械臂对灵巧性和安全性提出了更高的要求，主要体现在：轻质量、高负载/自重比、丰富的感知系统等。谐波减速器由于其结构紧凑、无齿侧间隙、高负载自重比等优点被广泛应用于轻型机械臂关节设计中。除此之外，为了实现更好的人及协调，保证人在人机协调中的安全，轻型机械臂需要感知外界对其施加的力和力矩，力矩传感器也被引入到轻型机械臂关节中，然而，谐波减速器和力矩传感器导致了轻型机械臂的关节柔性，如何克服关节柔性对控制性能的影响成为当前研究的热点问题。
[0003] 相对于传统的工业机械臂，柔性关节机械臂的控制相对于传统的以工业机器人为代表的刚性机械臂控制的难点主要在于：控制力矩无法直接作用到关节端，而是需要通过弹簧阻尼系统传递到关节端，柔性使得轻型机械臂关节存在残余振动，导致控制困难。针对柔性关节机械臂的控制方法，大量的学者进行了广泛地研究，归纳起来主要包括以下几种方法：奇异摄动法，解耦控制和积分反步法，基于无源性理论的控制方法等。奇异摄动方法只适用于较高刚度的机械臂控制；解耦控制和积分反步法计算量过大，并且关节惯性矩阵的求逆的过程可能会导致病态矩阵；在轻型机械臂振动抑制方面，能量整形等方法使得控制存在较大的延迟，影响了轻型机械臂的响应速度。基于无源性理论的关节控制器具有算法简单、便于工程应用、跟踪性能好等优点，更适合多自由度柔性关节机械臂的应用。基于无源性理论的控制器已经在DLR的轻型机械臂中得到了应用，并且取得了优秀的控制效果。
[0004] 传统的基于无源性理论方法是建立在Lyapunov稳定性理论的基础上的，该方法虽然实现了系统的能量整形，但缺少从系统结构配置层面去思考各参数的作用。现有的柔性关节机械臂，由于关节柔性较大导致柔性关节机械臂带宽小、响应慢，机械臂残余振动大；而且动力学建模困难，致使柔性关节机械臂控制律较为复杂，导致控制器运算量大，控制律设计复杂，不易调节控制参数，抖动大等问题。
发明内容
[0005] 本发明的目的是提出一种基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，以解决传统的机械臂控制方法在柔性关节机械臂控制中，残余振动大，无法达到稳定控制的目的问题。
[0006] 本发明为解决上述技术问题所采用的技术方案是：
[0007] 本发明所述的一种基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，是按照以下步骤实现的：
[0008] 步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数，包括几何参数、质量、转动惯量；
[0009] 步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、关节阻尼矩阵D、关节摩擦力矩τf；
[0010] 步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian状态方程；
[0011] 步骤四、基于步骤二获得的关节刚度矩阵K和关节阻尼矩阵D，建立基于电机位置的重力和外力补偿算法；
[0012] 步骤五、基于柔性关节电机位置信息的机械臂重力和外力补偿值的求取；
[0013] 步骤六、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；
[0014] 步骤七、根据匹配方程求解得到阻抗控制中的期望连接矩阵和阻尼矩阵；
[0015] 步骤八、基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取。
[0016] 与现有方法相比，本发明的有益效果为：
[0017] 1、本发明方法在柔性关节机械臂阻抗控制中，控制器运算量小，适合在多自由度机
[0018] 械臂上应用。
[0019] 2、本发明可以实现对柔性关节机械臂闭环系统的能量整形和结构配置，从系统自身物理性质出发推导出合适的控制律，使得控制律设计得到简化，方便调节控制参数。
[0020] 3、本发明通过注入耦合阻尼，提高了机械臂抑制抖动的能力。
[0021] 4、本发明对阻抗控制刚度没有限制，应用中可以任意的改变期望刚度以提高环境的适应能力和人机交互的安全性。
[0022] 5、本发明采用的基于电机位置的重力和外力补偿，适用于低刚度的阻抗控制，对机器人硬件要求较低。
附图说明
[0023] 图1为本发明的流程图；
[0024] 图2(a)是传统的柔性关节控制模型示意图，其中K为关节刚度，D为关节阻尼，B为电机惯量，1为连杆，2为电机，3为连杆，；
[0025] 图2(b)是本发明所述的柔性关节控制模型示意图，其中K为关节刚度，D为关节阻尼，B为电机惯量，Kθ为比例增益，r4为电机端注入的自阻尼，r5为电机端和关节端之间注入的耦合阻尼，γ为电机转动惯量较小的尺度，1为连杆，2为电机，3为连杆，图中虚线圈标记出了本图相对于图2(a)添加的内容；
[0026] 图3是6自由度柔性关节机械臂在不同位置施力的实验图，从左至右的顺序为：第一幅图代表腕部施力，第二幅图代表前臂施力，第三幅图代表肘部施力，第四幅图代表上臂施力；
[0027] 图4是6自由度柔性关节机械臂阻抗控制中，在笛卡尔空间坐标系下，机械臂位置误差随时间变化曲线图，图中(a)代表腕部受力，(b)代表前臂受力，(c)代表肘部受力，(d)代表上臂受力，x——代表在笛卡尔空间坐标系下x方向的位置误差；y——代表在笛卡尔空间坐标系下y方向的位置误差；z——代表在笛卡尔空间坐标系下y方向的位置误差；
[0028] 图5是6自由度柔性关节机械臂阻抗控制中，在笛卡尔空间坐标系下，机械臂受到的接触力随时间变化曲线图，图中(a)代表腕部受到的接触力，(b)代表前臂受到的接触力，(c)代表肘部受到的接触力，(d)代表上臂受到的接触力，Fx——代表在笛卡尔空间坐标系下x方向受到的接触力的变化曲线，Fy——代表在笛卡尔空间坐标系下y方向受到的接触力的变化曲线，Fz——代表在笛卡尔空间坐标系下z方向受到的接触力的变化曲线。
具体实施方式
[0029] 结合附图进一步详细说明本发明的具体实施方式。
[0030] 具体实施方式一：本实施方式所述的一种基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法，包括以下步骤：
[0031] 步骤一、通过CAD三维模型得到柔性关节机械臂动力学和运动学参数，包括几何参数、质量、转动惯量；
[0032] 步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度矩阵K、关节阻尼矩阵D、关节摩擦力矩τf；
[0033] 步骤三、建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian(port control Hamiltonian,PCH)状态方程；
[0034] 步骤四、基于步骤二获得的关节刚度矩阵K和关节阻尼矩阵D，建立基于电机位置的重力和外力补偿算法；
[0035] 步骤五、基于柔性关节电机位置信息的机械臂重力和外力补偿值的求取；
[0036] 步骤六、电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取；
[0037] 步骤七、根据匹配方程求解得到阻抗控制中的期望连接矩阵和阻尼矩阵；
[0038] 步骤八、基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取。结合图1理解本实施方式。
[0039] 具体实施方式二：本实施方式与具体实施方式一不同的是：步骤三所述的建立柔性关节机械臂的动力学方程，并将其改写为端口受控的Hamiltonian(port control Hamiltonian,PCH)状态方程的具体过程为：
[0040] 基于弹簧阻尼模型的柔性关节机械臂的动力学方程为：
[0041]
[0042] 式中 ——关节角度(rad)；
[0043] ——关节角速度(rad/s)；
[0044] ——关节角加速度(rad/s2)；
[0045] ——电机角度(rad)；
[0046] ——电机角加速度(rad/s2)；
[0047] n——机器人的自由度数目；
[0048] ——关节刚度矩阵(Nm/rad)；
[0049] ——关节阻尼矩阵(Nm·s/rad)；
[0050] ——分别为电机和连杆的惯性矩阵(kg·m2)；
[0051] ——离心力哥氏力矩阵；
[0052] ——关节重力矩阵(Nm)；
[0053] ——分别为关节力矩、电机输出力矩、摩擦力矩和外力矩(Nm)；
[0054] 选择柔性关节机械臂的Hamiltonian函数为：
[0055]
[0056] 式中 ——系统转动惯量，
[0057] M(q)——连杆的惯性矩阵(kg·m2)；B——电机的惯性矩阵(kg·m2)；
[0058] 0n——n阶0矩阵；
[0059] ——系统广义位置坐标；
[0060] ——系统广义动量；
[0061] V(s)——系统势能函数，包含了重力势能函数Vg(q)和弹性势能函数Vk(q,θ)，即：
[0062]
[0063] 为了得到柔性关节机械臂的PCHD模型，对动力学方程(1)改写如下：忽略外力和摩擦力的影响，且令ψ＝[s,p]T为状态变量，则将式(1)改写为端口受控的耗散Hamiltonian系统PCHD(Port-controlled Hamiltonian Systems with Dissipation,PCHD)方程的形式：
[0064]
[0065] 式中J(ψ)——柔性关节机械臂的连接矩阵，
[0066] R(ψ)——柔性关节机械臂的阻尼矩阵，
[0067] u——柔性关节机器人系统的输入变量，在本发明里代表电机输入力矩；
[0068] κ(ψ)——输入力矩矩阵，
[0069] ——H(ψ)的偏微分向量，
[0070] 02n——2n阶零矩阵；
[0071] I2n——2n阶单位矩阵；
[0072] D——关节阻尼矩阵(Nm·s/rad)；
[0073] 从G(ψ)可以看出，系统广义坐标下自由度个数为2n，而系统输入变量个数为n，所以柔性关节机械臂属于具有n个自由度的欠驱动系统。通过分析式(4)中的连接矩阵和阻尼矩阵的形式可以看出：连接矩阵中次对角线上的元素为单位矩阵；而阻尼矩阵为关于关节阻尼D的半正定对称矩阵。连接矩阵和阻尼矩阵的特点可以用于指导闭环系统的期望连接矩阵和阻尼矩阵的选取。为了提高柔性关节阻抗控制的精度，控制器中常常引入复杂的摩擦模型，而复杂的摩擦模型可能造成系统很难表述成PCHD形式。因此，
[0074] 将式(1)改写为端口受控的Hamiltonian状态方程：
[0075]
[0076] 其中， 结合图2(a)，图2(b)理解本实施方式。
[0077] 具体实施方式三：本实施方式与具体实施方式一或二不同的是：步骤四所述的基于步骤二获得的关节刚度矩阵K和关节阻尼矩阵D，建立基于电机位置的重力和外力补偿算法，具体过程如下：
[0078]
[0079] 上式表明，为了得到式(5)所示的阻抗性能，机械臂必须具备检测外力Fext的能力。现阶段的机械臂经常在末端或腕部安装6维力矩传感器来检测外力。然而，对于仿人形机器人来说，碰撞可能发生在机械臂的任意位置，限制了上述控制方法的应用。然而，通过设定期望的Λx＝Λ(x)消去式(6)中的最后一项，期望的质量-弹簧-阻尼的模型简化为：
[0080] 其它步骤与具体实施方式一或二相同。
[0081] 具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：步骤五所述的基于柔性关节电机位置信息的机械臂重力和外力补偿值的求取过程为：给定期望的刚度和阻尼，以及期望位置，并建立基于电机位置的重力和外力补偿算法；在阻抗控制过程中，刚度矩阵Kx一般是任务给定的，保证机械臂在平衡状态下，外力Fext与位置偏差 满足如下关系：
[0082]
[0083] 将τext＝J(q)TFext代入式(1)后，得到机器人稳态方程为：
[0084] K(θ-q)＝g(q)-J(q)TKx(x-xd) (9)
[0085] 其中，x表示在关节位置x处的刚度矩阵，定义l(q)＝g(q)-J(q)TKx(x-xd)为柔性关节机器人系统受到的外力，以及势能函数为 则成立。采用与重力补偿类似的方法，在集合Ωl:＝{(q,θ)|K(θ-q)＝l(q)}
中，集合Ωl中的电机位置θ和关节位置q存在一一对应的关系。则通过电机端位置得到稳态下的机器人关节位置q，进而实现机器人所受重力和外力的补偿。
[0086] 通过迭代的方法计算得到稳态时电机位置θ对应的关节位置 迭代方程为：
[0087]
[0088] 对于任意的初始值 随着迭代次数的增加，获得的稳态的关节角度 越精确。考虑到实时性要求，迭代的初始值选择为电机角度，即 经过1～2次迭代达到近似的稳态位置
[0089] 具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤六所述的电机位置信息在期望平衡位置处的最小Hamiltonian函数值的求取过程为：
[0090] 当电机的转动惯量B越小，弹簧的刚度系数K越大，柔性关节模型越接近于刚性关节模型。对于在外力作用下的柔性关节机器人阻抗控制，选择期望Hamiltonian函数为:
[0091]
[0092] 式中Md(s)——闭环系统的转动惯量，
[0093] γ——电机惯量整形系数，
[0094] 期望势能函数为：
[0095]
[0096]
[0097] 满足 将式(13)代入式12)中得到Vd(s)满足下述不等式：
[0098]
[0099] 当且仅当 且x＝xd时等式成立，Vd(s)取得最小值0。考虑到笛卡尔位置x＝xd决定了关节位置q＝qd，根据稳态时 可以唯一确定电机的位置：θ＝θd＝qd+K-1l(qd)，则sd＝(qd,θd)＝arg minVd(s)。因此，当ψd＝(sd,0)时期望能量函数Hd(ψ)达到最小值，即ψd＝(sd,0)＝arg minHd(ψ)，其中ψd为期望的平衡位置，sd为期望的广义坐标函数，Vd(s)为期望势能函数，Vg(q)为重力势能函数。
[0100] 具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤七所述的根据匹配方程求解得到阻抗控制中的期望连接矩阵和阻尼矩阵的具体过程为：根据柔性关节机械臂PCHD方程(4)中连接矩阵和阻尼矩阵的结构，假设期望的连接矩阵和阻尼矩阵如下：
[0101]
[0102] 式中J12、J13、J14、J23、J24、J34、r1、r2、r3、r4、r5为待求解期望的连接矩阵和阻尼矩阵参数，
[0103] 选择矩阵κ(ψ)的左零化子空间为：
[0104] Hd(ψ)对各状态变量的偏微分得到：
[0105]
[0106] 根据状态方程(5)和上述假设求解匹配方程：
[0107]
[0108] 得到各参数值为：
[0109] r1＝0n,J12＝0n,J13＝In,J14＝0n；
[0110]
[0111] 将公式(13)带入Jd、Rd中，得到期望的连接矩阵和阻尼矩阵的表达式为：
[0112]
[0113] 由Rd正定的条件可以推导出闭环系统稳定的充分必要条件为 此时系统的能量平衡方程变为：
[0114]
[0115] 具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：步骤八所述的基于连接矩阵和阻尼矩阵的柔性关节阻抗控制律的获取过程为：将步骤七得到的连接矩阵和阻尼矩阵代入控制律表达式：
[0116]
[0117] 得到柔性关节机器人阻抗控制律关于位置和力矩的全状态反馈形式为：
[0118]
[0119] 式中，Ks＝2r5/γ-D、Kv＝r4/γ2+D-2r5/γ分别为力矩微分的反馈系数和闭环系统阻尼；
[0120] γ——电机惯量整形系数；
[0121] ——关节角度(rad)；
[0122] ——关节角速度(rad/s)；
[0123] ——电机角度(rad)；
[0124] ——电机角速度(rad/s)；
[0125] K——关节刚度矩阵(Nm/rad)；
[0126] ——关节阻尼矩阵(Nm·s/rad)；
[0127] B——分别为电机惯性矩阵(kg·m2)；
[0128] τf——摩擦力矩(Nm)；
[0129] Bθ——闭环系统电机转动惯量；
[0130] r4——电机端注入的自阻尼；
[0131] r5——电机端和关节端之间注入的耦合阻尼；
[0132] Kd——位置误差的比例增益；
[0133] 为了与期望阻尼Dx匹配，Dθ(θ)为满足额外的约束：
[0134]
[0135] 的正定矩阵，令τm＝β(ψ)并带入式(1)得到闭环系统方程：
[0136]
[0137] 公式(20)证明了本发明的阻抗控制律对系统是稳定的。
[0138] 在 条件下， 成立的充要条件为 将 带入闭环方程式(20)，根据稳态下 得到 则 由于在
平衡状态下(q,θ)∈Ωl，根据集合Ωl的性质，存在唯一的θd与qd对应。因此集合的最大不变集为 从而证明了在平衡点
处柔性关节阻抗控制系统渐进稳定。
[0139] 本发明的实验验证如下：
[0140] 以6自由度仿人形机械臂为例，该机械臂具有6个自由度，其中肩部和肘部的4个模块化关节采用谐波减速器加上无刷直流电机的结构；手腕关节采用盘式电机、谐波减速器、同步齿形皮带和4个伞齿轮组成的差动机构等的驱动、传动方案。肩部和肘部关节的每个关节都安装有电位计和磁编码器用于检测关节和电机位置，关节内具有电流传感器和力矩传感器分别检测电机输出电流和关节输出力矩，直流无刷电机采用矢量控制的方法保证电机输出力矩与输出电流的比例关系。
[0141] 具体步骤如下：
[0142] 步骤一、通过CAD建模得到准确的机械臂动力学和运动学参数，以6自由度仿人形机械臂为例，其中D-H参数和机械臂连杆参数分别如表1和表2所示：
[0143] 表1仿人形机械臂D-H参数表
[0144]
[0145] 表2连杆参数名义值
[0146]
[0147]
[0148] 步骤二、通过参数辨识得到柔性关节的关键参数，包括关节刚度、阻尼等；
[0149] 根据矢量控制检测的电流信息和力矩信息，得到肩部和肘部4个关节的刚度和阻尼分别如表3所示：
[0150] 表3连杆参数名义值
[0151]
[0152] 步骤三、建立原始柔性关节机器人系统的动力学方程(1)，并将其改写为端口受控的Hamiltonian(port control Hamiltonian,PCH)方程的形式(2)；最后得到状态方程的形式(4)。
[0153] 步骤四、给定期望的刚度、阻尼和轨迹，并建立基于电机位置的重力和外力补偿算法；
[0154] 笛卡尔空间期望的刚度和阻尼分别如表4所示：
[0155] 表4动态碰撞实验参数
[0156]
[0157] 根据式(10)迭代方法得到基于电机位置的重力和外力补偿
[0158] 步骤五、给定期望的Hamiltonian能量函数，使其在期望的平衡位置处取得最小值；按式(11)选择能量函数；
[0159] 步骤六、假设期望的连接和阻尼矩阵形式，并根据匹配方程求解得到期望连接矩阵和阻尼矩阵；
[0160] 步骤七、得到基于IDA-PBC方法的柔性关节阻抗控制律，参数取值。
[0161] 最后，将上述结果带入控制律表达式：得到柔性关节机器人阻抗控制关于位置和力矩的全状态反馈形式为：
[0162]
[0163] 式中Ks＝2r5/γ-D。为了与期望阻尼Dx匹配，Dθ(θ)为满足额外的约束：
[0164]
[0165] 的正定矩阵。
[0166] 选择γ＝3，r5＝-4Dγ，笛卡尔空间期望刚度由外力决定。按照图3所示的加载力顺序对轻型机械臂施加外力，得到笛卡尔空间位置误差如图4所示，并由关节力矩得到笛卡尔空间外力曲线如图5所示。可以看出，在加载过程中，机械臂阻抗控制基本上可以跟踪外力的变化。然而，在外力撤销后，机械臂并没有回到指定的位置，而是在x、y、z方向都存在较大的静态误差。x、y、z方向静态误差的最大值分别为：exmax＝6.2mm、exmax＝6.2mm、exmax＝6.2mm。
