CN103213129B - 一种空间机械臂位置力混合控制方法 - Google Patents

Abstract

一种空间机械臂位置力混合控制方法，是一种笛卡尔空间和关节空间相结合的双回路控制方法。相比于基于整体模型的传统控制方法存在的过渡过程振荡和算法复杂性问题，本发明将位置/力混合控制问题分解为笛卡尔空间的规划问题和关节空间的控制问题，给出笛卡尔空间到关节空间的位置/力运动学规划和关节空间的动力学控制，组成内外回路控制系统，实现对机械臂末端自由空间的位置控制和受环境约束空间的接触力控制。对7自由度冗余机械臂的仿真结果证实了方法简单、有效、易于工程实现。

Description

一种空间机械臂位置力混合控制方法

技术领域

本发明涉及一种空间机械臂位置力混合控制方法，可为我国载人三期机械臂工程提供技术支持。

背景技术

空间机械臂研制是我国未来空间站项目成败的核心技术之一，其工程设计要求十分苛刻，涉及众多亟需攻关解决的关键技术。机械臂在空间站系统中发挥着重要作用，承担着舱段捕获与转移、仪器设备转移与安装、辅助航天员作业等多种功能。由于空间机械臂是一个复杂的动力学系统，呈现变质量、变结构、大柔性、强非线性、强耦合的复杂动力学特性，无论从动力学建模还是控制系统设计都给设计师带来很大的挑战。机械臂完成任务操作有两种基本模式：在自由空间的位置控制和被环境约束时的力控制。在实际工程中往往需要同时执行自然约束和人工约束控制这两种基本模式，此时，位置控制与力控制组成一个有机整体，这类位置控制和力控制融合在一起的控制问题即为位置/力混合控制问题。这类混合工作模式的控制相对于基本模式控制要困难得多，国内外的研究也不成熟。

机械臂单独的力控制方法包括刚度控制、阻抗控制和直接力控制等方法。Raibert和Craig首先提出位置/力混合反馈控制，代表性的有，Khabib提出了基于任务空间模型的控制方法，Yoshikawa提出了基于动力学模型和末端约束超平面描述的动态控制方法，McClamroch给出了机械臂受限动力学模型的镇定条件和闭环特性，关于混合控制的稳定性问题，Doulgeri讨论了运动学坐标变换引出的运动学失稳问题，An讨论了机械臂动力学与环境接触力动力学耦合引出的动力学失稳问题。邱志成和谈大龙提出利用加速度反馈，处理位置控制到力控制过渡时存在的冲击、震荡、不稳定等问题。机械臂力控制的主要难点在于笛卡尔空间的接触力模型与关节空间的动力学模型是非线性对应关系，特对是对于冗余自由度机械臂，是复射关系，造成难以将模型处理为从关节空间控制力矩输入到笛卡尔空间力输出的统一模型。然而，目前的控制方法大都是将运动学模型和动力学模型作为整体处理，给出基于物理概念的控制律，但没有补偿动力学特性，存在过渡过程振荡和稳定性问题；给出基于模型推导的控制律，但存在如雅克比矩阵导数项难以求解的算法问题，且计算量过大而难以应用。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种空间机械臂位置力混合控制方法，将位置/力混合控制问题分解为笛卡尔空间的规划问题和关节空间的控制问题，给出笛卡尔空间到关节空间的位置/力运动学规划和关节空间的动力学控制，组成内外回路控制系统，实现对机械臂末端自由空间的位置控制和受环境约束空间的接触力控制。针对7自由度冗余机械臂的仿真结果，验证了方法的有效性和实用性。

本发明的技术解决方案是：

一种空间机械臂位置力混合控制方法，步骤如下：

（1）建立机械臂动力学模型为其中，q∈Rⁿ为关节角，和分别为q的二阶导数和一阶导数，M(q)∈R^n×n为关节空间的惯性矩阵，为离心力和科氏力耦合效应，τ∈Rⁿ为关节驱动力矩，F∈R⁶为机械臂末端在笛卡儿空间的广义力，环境自由度n>6，冗余自由度为n-6，J(q)∈R^6×n为雅克比矩阵；

（2）通过位置和力运动规划算法得到机械臂末端从笛卡尔空间到关节空间的逆向运动学结果，该逆向运动学结果即为关节角指令；

（3）通过关节角运动控制算法进行机械臂动力学特性的补偿，实现对所述关节角指令的跟踪控制；

（4）由机械臂动力学模型、位置和力运动规划算法以及关节角运动控制算法组成内外回路位置/力混合控制系统，实现对所述空间机械臂位置力混合控制。

所述位置和力运动规划算法具体为：

（2.1）令将设置为被控对象；

（2.2）假设雅克比矩阵J(q)是行满秩的，即机械臂处于非奇异位形，令u_x=J⁺(q)K_x(x_des-x)，其中K_x∈R^6×6是正定对角增益矩阵，J⁺(q)为雅克比矩阵的广义逆，J⁺(q)=J^T(q)(J(q)J^T(q))^-1，x_des∈R⁶为期望末端位姿，x为机械臂末端在笛卡尔空间的位姿，有x＝[x，y，z，φ，θ，ψ]^T，其中[x,y,z]^T为位置，[φ，θ，ψ]^T为姿态；

（2.3）由公式可以得到位置规划方程

（2.4）根据机械臂运动的低动态特性，忽略公式中的小量动态项，简化为F=K_tE_x；其中，M_t,B_t,K_t∈R^6×6为正定对角矩阵，分别代表环境惯性、阻尼和刚度，E_x=x-x_r其中x_r为接触点标称轨迹，包括机械臂末端位置和姿态；

（2.5）对式F=K_tE_x两边取导，得将雅克比矩阵关系式 $\stackrel{\·}{x}=J\left(q\right)\stackrel{\·}{q}$ 代入得 $\stackrel{\·}{F}=K_t{\stackrel{\·}{E}}_x=K_{t}J\left(q\right)\stackrel{\·}{q},$ 重新定义控制量 $\stackrel{\·}{q}=J^{+}\left(q\right)u_f,$ 得到 $\stackrel{\·}{F}=K_{t}J\left(q\right)\stackrel{\·}{q}=K_t{u}_f;$

（2.6）针对线性系统令u_f=K_f(F_des-F)，将其代入得到力规划方程其中K_f∈R^6×6是正定对角增益矩阵，F_des∈R⁶是期望接触力；

（2.7）位置规划方程 $\stackrel{\·}{q}=J^{+}\left(q\right)K_x(x_{\mathrm{des}}-x)$ 和力规划方程 $\stackrel{\·}{q}=J^{+}\left(q\right)K_f(F_{\mathrm{des}}-F)$ 即为机械臂末端从笛卡尔空间到关节空间的逆向运动学结果。

所述关节角运动控制算法具体为：

（3.1）针对机械臂关节空间的动力学模型定义新的控制输入u_q∈Rⁿ，对其非线性状态反馈补偿得到控制方程

（3.2）针对式 $\stackrel{\·\·}{q}=u_q,$ 设计PD控制律 $u_q={\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q),$ 其中q_des∈Rⁿ为运动规划关节角，为运动规划关节角速度，为运动规划关节角加速度，K_p∈R^n×n为比例增益矩阵，K_d∈R^n×n为微分增益矩阵；

（3.3）将 $u_q={\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q)$ 代入式 $\mathrm{\τ}=M\left(q\right)u_q+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F,$ 即得到控制律为 $\mathrm{\τ}=M\left(q\right)({\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F.$

位置/力混合控制系统具体为：

（4.1）将位置规划方程 $\stackrel{\·}{q}=J^{+}\left(q\right)K_x(x_{\mathrm{des}}-x)$ 和力规划方程 $\stackrel{\·}{q}=J^{+}\left(q\right)K_f(F_{\mathrm{des}}-F)$ 组成位置/力规划外回路，利用所述位置规划方程和力规划方程将自由空间的期望末端位姿x_des和期望接触力F_des统一规划为关节角命令，利用位姿x和接触力F，计算得到关节角q_des、关节角速度和关节角加速度作为关节角控制内回路指令；

（4.2）将关节角控制律 $\mathrm{\τ}=M\left(q\right)({\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F$ 组成关节角内回路，将外回路计算得到的关节角信息q_des、和作为输入指令，利用机械臂模型得到的关节角q和角速度作为反馈量，实现对外回路指令的跟踪控制。

本发明与现有技术相比的优点在于：

1）将位置/力混合控制问题分解为笛卡尔空间的规划问题和关节空间的控制问题，给出笛卡尔空间到关节空间的位置/力运动学规划和关节空间的动力学控制，组成内外回路位置力混合控制系统。

2）相比于基于整体模型的传统控制方法存在的过渡过程振荡和方法复杂性问题，此位置力混合控制方法物理意义清晰，能够实现从位置约束到力约束平缓稳定的过渡，且位置/力混合控制跟踪精确，计算简单，便于工程实现。

附图说明

图1为本发明方法流程图；

图2为本发明位置/力混合控制原理图；

图3为本发明机械臂Matlab模型图；

图4为机械臂末端位置x曲线图；

图5为机械臂末端位置y曲线图；

图6为机械臂末端位置Z曲线图；

图7为机械臂末端接触力Fx曲线图；

图8为机械臂末端接触力Fy曲线图；

图9为机械臂末端接触力Fz曲线图；

图10为机械臂末端姿态滚动角曲线图；

图11为机械臂末端姿态俯仰角曲线图；

图12为机械臂末端姿态偏航角曲线图；

图13为机械臂末端接触力矩Mx曲线图；

图14为机械臂末端接触力矩My曲线图；

图15为机械臂末端接触力矩Mz曲线图。

具体实施方式

整个发明的实施流程见图1。具体实施方式详述如下：

(1)建立机械臂动力学模型

考虑一个n自由度冗余空间机械臂，其多刚体动力学方程在关节空间描述为

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}---\left(1\right)$

其中M(q)∈R^n×n为关节空间的惯性矩阵，为离心力和科氏力耦合效应，τ∈Rⁿ为关节驱动力矩，自由度n>6，冗余自由度为n-6。当机械臂末端与环境接触时，其受限动力学方程描述为

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}=\mathrm{\τ}-{\mathrm{\τ}}_e---\left(2\right)$

其中τ_e为机械臂在笛卡尔空间的末端接触力作用在关节处的等效力矩。

定义机械臂末端在笛卡尔空间的位姿x＝[x，y，z，φ，θ，ψ]^T其中[x,y,z]^T为位置，[φ，θ，ψ]^T为姿态。由运动学关系，末端位姿与关节角之间满足以下代数方程

x=x(q)    (3)

对式(3)两边取导，得机械臂末端笛卡儿空间运动速度与关节空间运动速度的关系

$\stackrel{\·}{x}=\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}\frac{{\∂x}_i}{\∂q_j}{\stackrel{\·}{q}}_j=J\left(q\right)\stackrel{\·}{q}---\left(4\right)$

其中J(q)∈R^6×n为雅克比矩阵，代表关节空间速度向笛卡儿空间速度的传动比。

建立笛卡儿空间的环境力与关节空间的驱动力矩之间的静力变换，根据虚功等价原理，得

F^Tδx=τ^Tδq    (5)

其中F∈R⁶为机械臂末端在笛卡儿空间的广义力，包括力和力矩，由式(4)得

τ=J^T(q)F    (6)

那么，式(2)中的等效力矩τ_e=J^TF。机械臂末端与环境之间的接触力具有如下动态特性

$M_t{\stackrel{\·\·}{E}}_x+B_t{\stackrel{\·}{E}}_x+K_t{E}_x=F---\left(7\right)$

其中M_t,B_t,K_t∈R^6×6为正定对角矩阵，分别代表环境惯性、阻尼和刚度，E_x定义如下

E_x=x-x_r    (8)

其中x_r为接触点标称轨迹，包括机械臂末端位置和姿态。E_x描述了机械臂末端与环境物体之间的嵌入深度，式(7)描述了机械臂末端位姿与环境作用力之间的动态关系。

(2)设计位置和力运动规划方法

运动规划解决机械臂末端从笛卡尔空间到关节空间的逆向运动学求解问题，即给定末端位置和力控制要求，求解一组关节角和角速度，用于驱动关节空间控制系统。文本选择在速度水平上，利用雅克比矩阵，将运动规划问题转换为控制问题，给出闭环运动规划方法。

考虑位置规划，令考虑方程(4)为被控对象

$\stackrel{\·}{x}=J\left(q\right)u_x---\left(9\right)$

假设雅克比矩阵J(q)是行满秩的，即机械臂处于非奇异位形，令

u_x=J⁺(q)K_x(x_des-x)    (10)

其中K_x∈R^6×6是正定对角增益矩阵，J⁺(q)为雅克比矩阵的广义逆，当J(q)行满秩时，J⁺(q)=J^T(q)(J(q)J^T(q))^-1，是线性方程组(4)的最小范数解，x_des∈R⁶为期望末端位姿。代入式(9)，得一阶闭环动态特性

$\stackrel{\·}{x}=K_x(x_{\mathrm{des}}-x)---\left(11\right)$

最终，得到位置规划方程

$\stackrel{\·}{q}=J^{+}\left(q\right)K_x(x_{\mathrm{des}}-x)---\left(12\right)$

考虑力规划，由于机械臂运动的低动态特性，忽略式(7)中的小量动态项，简化为

F=K_tE_x    (13)

对式(13)两边取导，得

$\stackrel{\·}{F}=K_t{\stackrel{\·}{E}}_x=K_t\stackrel{\·}{x}---\left(14\right)$

利用雅克比矩阵关系式(4)，代入式(14)得

$\stackrel{\·}{F}=K_t{\stackrel{\·}{E}}_x=K_{t}J\left(q\right)\stackrel{\·}{q}---\left(15\right)$

重新定义控制量得

$\stackrel{\·}{F}=K_t{\stackrel{\·}{E}}_x=K_t{u}_f---\left(16\right)$

针对线性系统(16)，令

u_f=K_f(F_des-F)    (17)

其中K_f∈R^6×6是正定对角增益矩阵，F_des∈R⁶是期望接触力。代入式(16)，得一阶闭环动态特性

$\stackrel{\·}{F}+K_t{K}_{f}F=K_t{K}_f{F}_{\mathrm{des}}---\left(18\right)$

最终，得到力规划方程

$\stackrel{\·}{q}=J^{+}\left(q\right)K_f(F_{\mathrm{des}}-F)---\left(19\right)$

(3)设计关节角运动控制方法

运动控制解决机械臂动力学特性的补偿问题，提供良好的动态特性和稳定性，实现对位置和力规划给出的关节角指令的快速、精确跟踪控制。本文选择在关节空间，将机械臂耦合项和非线性项考虑在控制律中，采用非线性控制加以补偿和解耦，给出运动控制方法。

考虑机械臂关节空间的动力学模型(2)，定义新的控制输入u_q∈Rⁿ，对其非线性状态反馈补偿

$\mathrm{\τ}=M\left(q\right)u_q+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F---\left(20\right)$

将式(2)中的惯性矩阵、离心力和科氏力项、接触力等效力矩补偿掉，得到如下控制方程

$\stackrel{\·\·}{q}=u_q---\left(21\right)$

方程(21)为非线性补偿后的线性解耦系统。针对式(21)，设计如下PD控制律

$u_q={\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q)---\left(22\right)$

其中q_des∈Rⁿ为运动规划关节角，为运动规划关节角速度、为运动规划关节角加速度，K_p∈R^n×n为比例增益矩阵，K_d∈R^n×n为微分增益矩阵。通常按照典型二阶环节选择K_p和K_d为如下形式

$K_p=\mathrm{diag}({\mathrm{\ω}}_1^2,{\mathrm{\ω}}_2^2,...,{\mathrm{\ω}}_n^2)---\left(23\right)$

K_d=diag(2ξ₁ω₁,2ξ₂ω₂,…,2ξ_nω_n)    (24)

其中ω_i为第i个通道的带宽，ξ_i为第i个通道的阻尼比。定义跟踪误差

e=q_des-q    (25)

则由式(2)，式(20)和式(22)二阶闭环动态特性

$\stackrel{\·\·}{e}+K_d\stackrel{\·}{e}+K_{p}e=0---\left(26\right)$

最终获得的控制律为

$\mathrm{\τ}=M\left(q\right)({\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F---\left(27\right)$

(4)组成位置力混合控制方法

由（1）给出的机械臂动力学模型，（2）所给出的运动规划方法、和（3）给出的运动控制方法，组成内外回路位置/力混合控制系统，如图2所示。具体为：位置/力混合控制系统具体为：

（4.1）将位置规划方程和力规划方程组成位置/力规划外回路，利用所述位置规划方程和力规划方程将自由空间的期望末端位姿x_des和期望接触力F_des统一规划为关节角命令，利用位姿x和接触力F，计算得到关节角q_des、关节角速度和关节角加速度，作为关节角控制内回路指令；

（4.2）将关节角控制律 $\mathrm{\τ}=M\left(q\right)({\stackrel{\·\·}{q}}_{\mathrm{des}}+K_d({\stackrel{\·}{q}}_{\mathrm{des}}-\stackrel{\·}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+J^T\left(q\right)F$ 组成关节角内回路，将外回路计算得到的关节角信息q_des、和作为输入指令，利用机械臂模型得到的关节角q和角速度作为反馈量，实现对外回路指令的跟踪控制。

工作过程描述：机械臂开始工作时常为位置控制，即机械臂末端按照指令要求沿目标轨迹和给定速度运动。当末端与环境接触时，安装在机械臂末端上的力传感器探测到接触的发生，图2中的规划回路将受环境约束自由度由位置规划切换为力规划，即在环境约束方向进行接触力柔顺控制，在自由方向进行人为的位姿控制，实现位置/力混合控制。应用中，图2规划回路中的切换器后可以串联滤波器，保证切换信号的连续性和光滑性。

(5)仿真算例

为了验证所给出的位置/力混合控制方法的合理性和有效性，首先利用Matlab的Robotics Toolbox建立7自由度冗余机械臂的多刚体动力学模型，如图3所示。机械臂初始关节角和关节角速度均为零，所对应的机械臂末端初始位姿为x₀=[8.14m,1.42m,1.605m,0°,0°,0°]^T。

仿真时间为100s，机械臂处于自由运动状态，首先控制机械臂末端从初始位姿运动到期望位姿x_des=[5m,2m,3m,10°,-10°,10°]^T，假设在末端坐标系x，y轴平动自由度和z轴转动自由度存在环境约束，即在x<7.64m，y>1.92m，ψ>5°时产生接触力和力矩，期望接触力和力矩为F_x=-5N，F_y=5N，M_z=5N·m，当产生接触力时，在环境约束方向位置规划转换为力规划，在自由方向仍为位置规划，如图1所示。仿真结果如图4-15所示，可以看出，在受环境约束自由度方向的接触力产生时，位置控制平缓稳定过渡到力控制，且稳定在期望值附近，在自由方向继续进行位置控制，实现对参考轨迹的精确跟踪。

Claims (2)

1.一种空间机械臂位置力混合控制方法，其特征在于步骤如下：

(1)建立机械臂动力学模型为其中，q∈Rⁿ为关节角，和分别为q的二阶导数和一阶导数，M(q)∈R^n×n为关节空间的惯性矩阵，为离心力和科氏力耦合效应，τ∈Rⁿ为关节驱动力矩，F∈R⁶为机械臂末端在笛卡儿空间的广义力，环境自由度n>6，冗余自由度为n-6，J(q)∈R^6×n为雅克比矩阵；

(2)通过位置和力运动规划算法得到机械臂末端从笛卡尔空间到关节空间的逆向运动学结果，该逆向运动学结果即为关节角指令；

(3)通过关节角运动控制算法进行机械臂动力学特性的补偿，实现对所述关节角指令的跟踪控制；

(4)由机械臂动力学模型、位置和力运动规划算法以及关节角运动控制算法组成内外回路位置/力混合控制系统，实现对所述空间机械臂位置力混合控制；

所述位置和力运动规划算法具体为：

(2.1)令将设置为被控对象；

(2.2)假设雅克比矩阵J(q)是行满秩的，即机械臂处于非奇异位形，令u_x＝J⁺(q)K_x(x_des-x)，其中K_x∈R^6×6是正定对角增益矩阵，J⁺(q)为雅克比矩阵的广义逆，J⁺(q)＝J^T(q)(J(q)J^T(q))^-1，x_des∈R⁶为期望末端位姿，x为机械臂末端在笛卡尔空间的位姿，有其中[x,y,z]^T为位置，为姿态；

(2.3)由公式得到位置规划方程

(2.4)根据机械臂运动的低动态特性，忽略公式中的小量动态项，简化为F＝K_tE_x；其中，M_t,B_t,K_t∈R^6×6为正定对角矩阵，分别代表环境惯性、阻尼和刚度，E_x＝x-x_r，其中x_r为接触点标称轨迹，包括机械臂末端位置和姿态；

(2.5)对式F＝K_tE_x两边取导，得将雅克比矩阵关系式代入得 $\stackrel{.}{F}=K_t{\stackrel{.}{E}}_x=K_{t}J\left(q\right)\stackrel{.}{q},$ 重新定义控制量 $\stackrel{.}{q}=J^{+}\left(q\right)u_f,$ 得到 $\stackrel{.}{F}=K_{t}J\left(q\right)\stackrel{.}{q}=K_t{u}_f;$

(2.6)针对线性系统令u_f＝K_f(F_des-F)，将其代入得到力规划方程其中K_f∈R^6×6是正定对角增益矩阵，F_des∈R⁶是期望接触力；

(2.7)位置规划方程和力规划方程即为机械臂末端从笛卡尔空间到关节空间的逆向运动学结果；

所述关节角运动控制算法具体为：

(3.1)针对机械臂关节空间的动力学模型定义新的控制输入u_q∈Rⁿ，对其非线性状态反馈补偿得到控制方程

(3.2)针对式设计PD控制律其中q_des∈Rⁿ为运动规划关节角，为运动规划关节角速度，为运动规划关节角加速度，K_p∈R^n×n为比例增益矩阵，K_d∈R^n×n为微分增益矩阵；

(3.3)将 $u_q={\stackrel{..}{q}}_{\mathrm{des}}+K_d({\stackrel{.}{q}}_{\mathrm{des}}-\stackrel{.}{q})+K_p(q_{\mathrm{des}}-q)$ 代入式 $\mathrm{\&tau;}=M\left(q\right)u_q+C(q,\stackrel{.}{q})\stackrel{.}{q}+J^T\left(q\right)F,$ 即得到控制律为 $\mathrm{\&tau;}=M\left(q\right)({\stackrel{..}{q}}_{\mathrm{des}}+K_d({\stackrel{.}{q}}_{\mathrm{des}}-\stackrel{.}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{.}{q})\stackrel{.}{q}+J^T\left(q\right)F.$

2.根据权利要求1所述的一种空间机械臂位置力混合控制方法，其特征在于：位置/力混合控制系统具体为：

(4.1)将位置规划方程和力规划方程组成位置/力规划外回路，利用所述位置规划方程和力规划方程将自由空间的期望末端位姿x_des和期望接触力F_des统一规划为关节角命令，利用位姿x和接触力F，计算得到关节角q_des、关节角速度和关节角加速度作为关节角控制内回路指令；

(4.2)将关节角控制律 $\mathrm{\&tau;}=M\left(q\right)({\stackrel{..}{q}}_{\mathrm{des}}+K_d({\stackrel{.}{q}}_{\mathrm{des}}-\stackrel{.}{q})+K_p(q_{\mathrm{des}}-q))+C(q,\stackrel{.}{q})\stackrel{.}{q}+J^T\left(q\right)F$ 组成关节角内回路，将外回路计算得到的关节角信息q_des、和作为输入指令，利用机械臂模型得到的关节角q和角速度作为反馈量，实现对外回路指令的跟踪控制。