CN105911995A - 一种基于位置速度控制的遥操作机器人碰撞预警方法 - Google Patents

Abstract

本发明提供一种基于位置速度控制的遥操作机器人碰撞预警方法，其步骤包括：首先将主手控制器视为积分器，通过位置积分计算出对机器人位置的期望值，实现机器人任务速度与手控器位置的有效对应，然后根据手控器位置信号主动预测出未来时刻机器人的位置，当机器人与环境临近碰撞状态时生成预警力，最后将反馈力与操作者手动控制力进行融合，引导操作者控制机器人避开障碍物，完成作业过程。本发明可将人类智能决策与机器智能相结合，有效提高机器人的作业精度与效率，减轻操作者的作业负担，降低对操作者技术熟练度的依赖，避免系统延时导致对机器人控制的盲目性。

Description

一种基于位置速度控制的遥操作机器人碰撞预警方法

技术领域

本发明属于机器人人机交互及机器人遥操作领域，特别涉及一种基于位置速度控制的遥操作机器人碰撞预警方法。

背景技术

在危及人身安全的极限环境或危险环境下作业，例地震灾后或核事故现场清理救援、海洋探索、宇宙空间开发和利用等，机器人作为代替人类进行作业的工具，发挥越来越重要的作用，因而遥操作技术得到充分发展与运用。但是，由于受机构、控制、人工智能、传感和材料技术等发展水平的制约，自主作业机器人的开发在短期内无法实现。机器人控制中通常所采用的操作方法是直接控制法，而机器人控制领域的操作性及安全性目前还存在诸多问题：

首先，遥操作系统存在着不可避免的延时问题，这极易导致操控不当而带来作业对象的损毁。现有的机器人控制方法中，机器人的位置控制方式属于被动方式，即机器人只是顺应跟随手控器的位置进行作业，操作者并不能主动预测出机器人未来某一时刻的姿态。由于数据传输量以及传输距离等问题，遥操作系统存在着不可避免的延时问题，这将导致作业过程中极易操控不当，造成无法弥补的损失。其次，在远程操作系统中，手控器的种类繁多，通用性差^[1-7]。不仅不同领域的手控器结构各不相同，即使同一领域的手控器结构也存在差异。再次，手控器有效活动空间同机器人的活动空间存在差异，这将导致高精度作业不易实现，操作者工作负担增加^[8]。当手控器的活动空间范围远远小于机器人的活动范围时，操作者对于手控器的控制精度难以掌握，机器人便难以进行高精度作业。最后，操作者的技能水平和熟练程度极大地影响着机器人作业效率的高低。手控器同机器人在结构、尺寸、有效活动空间范围等方面通常存在很大差异，操作者在操控机器人过程中，特别是精细作业任务中，往往需要反复调整机器人的姿态，才可以完成作业任务^[9-11]，这无疑会导致机器人作业效率不高。

[1]Adelstein B.D.,Rosen M.U.J..Design and implementation of a forcereflecting manipulandum for manual control research.Advances in Robotics,ASMEDSC,1992,5(3):1–12.

[2]Howe R.D.,Force-reflecting teleoperated hand system for the studyof tactile sensing in precision manipulation.Proceedings of the IEEEInternational Conference on Robotics and Automation,1992,1321–1326.

[3]Massie T,Salisbury K.The phantom haptic interface:a device forprobing virtual objects.ASME Journal of Dynamic Systems,Measurement,andControl,1994,116:295–299.

[4]Berkelman PJ.,Hollis RL.,Salcudean SE..Interacting with virtualenvironments using a magnetic levitation haptic interface.Proceedings of theIEEE/RSJ International Conference on Intelligent Robots and Systems,1995,117–122.

[5]Ellis RE,Ismaeil OM,Lipsett MG.Design and evaluation of high-performance haptic interface.Robotica,1996,14:321–327.

[6]Chiaverini S..Singularity-robust task-priority redundancyresolution for real-time kinematic control of robot manipulators.IEEETransaction on Robotic and Automation,1997,13(3):398–410.

[7]Jin B.D.,Ki-Young,Kwon D.-S..A 6DOF force-reflecting master handcontroller using the fivebar parallel mechanism.Journal of Control,Automationand System Engineers,1999,5(3):312–317.

[8]宁祎.手控器的性能和评价方法.机器人,2000,22(4)：282-288

[9]邓乐，赵丁选，倪涛等.基于Stewart平台的6自由度力反馈手控器.农业机械学报,2005,36(7)：118-121

[10]倪涛，赵丁选，张红彦.电液手控器型遥操作机器人力反馈控制策略.农业机械学报,2010,41(12)：190-194

[11]Huang Lingtao,Kawamura Takuya,Hirinao Yamada.Application of aposition-force control method in a master-slave teleoperation constructionrobot system.Applied Mechanics and Materials,2012,229-231：2243-2247.

发明内容

本发明针对遥操作机器人控制领域中的操作性及安全性所存在的问题，提供一种基于位置速度控制的遥操作机器人碰撞预警方法，具体步骤如下：

S1、基于手控器位置速度控制方法，以手控器的位置信号控制机器人运动；

S2、通过手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近目标对象的预警反力；

S3、将S1中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合，对手控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近目标对象，完成作业过程。

所述步骤S1包括以下步骤：

S1.1、搭建基于摇杆式手控器控制机器人运动的系统控制平台；

S1.2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动做准备；

S1.3、通过将手控器视作为积分器，根据公式

$\left\{\begin{array}{cc}{q}_{sd}^{t+l+1}=k_{ms}{t}_{sam}\underset{n=t}{\overset{t+l}{\Σ}}{q}_m^n+q_s^t& q_m^n\≠0\\ q_{sd}^{n+1}=q_s^n& q_m^n=0\end{array}\right.$

计算手控器期望的机器人关节角控制信号q_sd。式中，k_ms为手控器与机器人关节角比例因子，t_sam为系统的采样周期，为n时刻手控器关节角，为t+l+1时刻手控器期望的机器人关节角，k_ms为手控器按钮修改比例因子。

S1.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号为零时，设定为上一采样时刻机器人关节角

S1.5、按照公式

τ_m＝J^TF_m

计算在操纵力F_m下手控器的操作力矩τ_m.

S1.6、将手控器的运动简化为质量-阻尼-弹簧系统，手控器动力学模型表示为公式

$M_m{\stackrel{\·\·}{q}}_m+B_m{\stackrel{\·}{q}}_m+K_m{q}_m=k_1{\mathrm{\τ}}_m,$

式中，F_m为手控器操纵力，J为手控器的雅可比矩阵，M_m、B_m和K_m为手控器的惯性系数矩阵，阻尼系数矩阵和弹簧系数矩阵，k₁为操作者手动控制的权值系数。

所述步骤S2包括以下步骤：

S2.1、根据公式

$q_{st}^t=t\·k_{ms}{q}_m+q_s,(q_m\≠0)$

预测t秒后机器人关节角位置式中，q_m、q_s为手控器及机器人当前时刻关节角。

S2.2、通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境。

S2.3、结合环境相机测得的背景环境(或障碍物)的高度信息，及预测得到的机器人末端执行器位置，求解机器人末端沿速度方向与背景环境的距离d。

S2.4、依据公式

$F_t=\left\{\begin{array}{cc}{k}_t(d_0-d)\stackrel{\&RightArrow;}{v}& d<d_0\\ 0& d\&GreaterEqual;d_0\end{array}\right.$

计算机器人所受的碰撞预警力F_t，式中，k_t为比例因子，d₀为机器人末端与障碍物间的安全距离，为机器人末端运动速度的单位向量。

S2.5、依据公式

$\left[\begin{array}{c}{\mathrm{\&tau;}}_t\\ {\mathrm{\&tau;}}_r\end{array}\right]=J^T\left[\begin{array}{c}\left[\begin{array}{c}{\mathrm{RF}}_t\\ {\mathrm{RM}}_t\end{array}\right]\\ \left[\begin{array}{c}{\mathrm{RF}}_e\\ {\mathrm{RM}}_e\end{array}\right]\end{array}\right]$

计算机器人与环境间的接触反馈力矩τ_r和预警反馈力矩τ_t。式中，F_e为机器人末端与环境间的作用力，M_t、M_e为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋转矩阵。

所述步骤S3包括以下步骤：

S3.1、按照公式

$M_m{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{md}+B_m{\stackrel{\&CenterDot;}{q}}_{md}+K_m{q}_{md}=k_1{\mathrm{\&tau;}}_m-k_2({\mathrm{\&tau;}}_t+{\mathrm{\&tau;}}_r)$

设计手控器的位置预测控制器。式中，k₂为预警反馈力矩的权值系数。τ_m为手控器在操纵杆的操作力矩、τ_r为机器人与环境的接触力矩，τ_t为预警反馈力矩，q_md为手控器的运动趋势。

S3.2、按照公式

$u_m=k_{md}({\stackrel{\&CenterDot;}{q}}_{md}-{\stackrel{\&CenterDot;}{q}}_m)+k_{mp}(q_{md}-q_m)$

设计手控器的控制量u_m，使手控器跟随反馈力矩输出的运动趋势。式中，k_mp、k_md为手控器的比例增益和微分增益。

S3.3、依据公式

$M_d({\stackrel{\&CenterDot;\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;\&CenterDot;}{q}}_s)+B_d({\stackrel{\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;}{q}}_s)+K_d(q_d-q_s)=F_e$

设计机器人阻抗模型，式中，M_d、B_d和K_d分别为期望阻抗模型的惯性矩阵、阻尼矩阵和刚度矩阵，q_d和q_s分别为机器人参考关节角和实际关节角。

S3.4、机器人与环境间的作用力矩满足公式

$M_d\mathrm{\&Delta;}\stackrel{\&CenterDot;\&CenterDot;}{q}+B_d\mathrm{\&Delta;}\stackrel{\&CenterDot;}{q}+K_d\mathrm{\&Delta;}q=F_e.$

S3.5、按公式

$\mathrm{\&Delta;}Q\left(s\right)=\frac{F_e}{M_d\stackrel{\&CenterDot;\&CenterDot;}{s}+B_d\stackrel{\&CenterDot;}{s}+K_{d}s}$

计算作用力F_e经过阻抗模型后，机器人关节控制的轨迹修正量

S3.6、依据公式

Q_c＝Q_d-ΔQ

计算经过修正的机器人关节控制指令式中，

S3.7、根据公式

$u_s={\stackrel{\&CenterDot;\&CenterDot;}{q}}_c+k_{sd}({\stackrel{\&CenterDot;}{q}}_c-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_c-q_s)$

设计经阻抗修正的机器人控制量u_s，实现机器人对手控器的跟随。

S3.8、机器人与环境(或障碍物)无接触情况时，依据公式

$u_s={\stackrel{\&CenterDot;}{q}}_m+k_{sd}(q_m-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_{sd}-q_s)$

设计机器人的控制量u_s，实现机器人对手控器的跟随。式中，k_sp、k_sd为机器人控制器的比例增益和微分增益。

本发明相对于现有技术具有如下的优点及技术效果：

1.提出基于位置积分的位置速度控制策略，采用位置增量式的控制，改变传统手控器对机器人位置的点对点操作方式；

2.降低作业效率对操作者技术熟练度的依赖；

3.能够对机器人未来时刻的姿态进行预测，在机器人与周边环境发生碰撞前，生成防碰撞预警力，避免由延时引起的误操作，改善操作系统的安全性；

4.为手控器的开发提供一种新的思路，为通用型手控器的标准化设计奠定基础。

附图说明

图1为位置速度控制技术路线；

图2为碰撞预警力建模技术路线。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，图1为位置速度控制技术路线，图2为碰撞预警力建模技术路线。

本发明根据基于位置速度控制的遥操作机器人碰撞预警方法，包括如下步骤S1、S2、S3：

S1、基于手控器位置积分的方法，以手控器的位置信号控制机器人运动，所述步骤S1包括步骤S1.1、S1.2、S1.3、S1.4、S1.5、S1.6：

S1.1、搭建基于摇杆式手控器控制机器人运动的系统控制平台；

S1.2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动做准备；

S1.3、通过将手控器视作为积分器，根据公式

$\left\{\begin{array}{cc}{q}_{sd}^{t+l+1}=k_{ms}{t}_{sam}\underset{n=t}{\overset{t+l}{\&Sigma;}}{q}_m^n+q_s^t& q_m^n\&NotEqual;0\\ q_{sd}^{n+1}=q_s^n& q_m^n=0\end{array}\right.---\left(1\right)$

计算手控器期望的机器人关节角控制信号q_sd。式中，k_ms为手控器与机器人关节角比例因子，t_sam为系统的采样周期，为n时刻手控器关节角，为t+l+1时刻手控器期望的机器人关节角，k_ms为手控器按钮修改比例因子。

S1.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号为零时，设定为上一采样时刻机器人关节角

S1.5、按照公式

τ_m＝J^TF_m (2)

计算在操纵力F_m下手控器的操作力矩τ_m.

S1.6、将手控器的运动简化为质量-阻尼-弹簧系统，手控器动力学模型表示为公式

$M_m{\stackrel{\&CenterDot;\&CenterDot;}{q}}_m+B_m{\stackrel{\&CenterDot;}{q}}_m+K_m{q}_m=k_1{\mathrm{\&tau;}}_m,---\left(3\right)$

式中，F_m为手控器操纵力，J为手控器的雅可比矩阵，M_m、B_m和K_m为手控器的惯性系数矩阵，阻尼系数矩阵和弹簧系数矩阵，k₁为操作者手动控制的权值系数。

S2、根据手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近目标对象的预警反力。所述步骤S2包括步骤S2.1、S2.2、S2.3、S2.4、S2.5：

S2.1、根据公式

$q_{st}^t=t\&CenterDot;k_{ms}{q}_m+q_s,(q_m\&NotEqual;0)---\left(4\right)$

预测t秒后机器人关节角位置式中，q_m、q_s为手控器及机器人当前时刻关节角。

S2.2、通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境。

S2.3、结合环境相机测得的背景环境(或障碍物)的高度信息，及预测得到的机器人末端执行器位置，求解机器人末端沿速度方向与背景环境的距离d。

S2.4、依据公式

$F_t=\left\{\begin{array}{cc}{k}_t(d_0-d)\stackrel{\&RightArrow;}{v}& d<d_0\\ 0& d\&GreaterEqual;d_0\end{array}\right.---\left(5\right)$

计算机器人所受的碰撞预警力F_t，式中，k_t为比例因子，d₀为机器人末端与障碍物间的安全距离，为机器人末端运动速度的单位向量。

S2.5、依据公式

$\left[\begin{array}{c}{\mathrm{\&tau;}}_t\\ {\mathrm{\&tau;}}_r\end{array}\right]=J^T\left[\begin{array}{c}\left[\begin{array}{c}{\mathrm{RF}}_t\\ {\mathrm{RM}}_t\end{array}\right]\\ \left[\begin{array}{c}{\mathrm{RF}}_e\\ {\mathrm{RM}}_e\end{array}\right]\end{array}\right]---\left(6\right)$

计算机器人与环境间的接触反馈力矩τ_r和预警反馈力矩τ_t。式中，F_e为机器人末端与环境间的作用力，M_t、M_e为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋转矩阵。

S3、将S1中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合，对手控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近目标对象，完成作业过程。所述步骤S3包括步骤S3.1、S3.2、S3.3、S3.4、S3.5、S3.6、S3.7、S3.8：

S3.1、按照公式

$M_m{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{md}+B_m{\stackrel{\&CenterDot;}{q}}_{md}+K_m{q}_{md}=k_1{\mathrm{\&tau;}}_m-k_2({\mathrm{\&tau;}}_t+{\mathrm{\&tau;}}_r)---\left(7\right)$

设计手控器的位置预测控制器。式中，k₂为预警反馈力矩的权值系数。τ_m为手控器在操纵杆的操作力矩、τ_r为机器人与环境的接触力矩，τ_t为预警反馈力矩，q_md为手控器的运动趋势。

S3.2、按照公式

$u_m=k_{md}({\stackrel{\&CenterDot;}{q}}_{md}-{\stackrel{\&CenterDot;}{q}}_m)+k_{mp}(q_{md}-q_m)---\left(8\right)$

设计手控器的控制量u_m，使手控器跟随反馈力矩输出的运动趋势。式中，k_mp、k_md为手控器的比例增益和微分增益。

S3.3、依据公式

$M_d({\stackrel{\&CenterDot;\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;\&CenterDot;}{q}}_s)+B_d({\stackrel{\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;}{q}}_s)+K_d(q_d-q_s)=F_e---\left(9\right)$

设计机器人阻抗模型，式中，M_d、B_d和K_d分别为期望阻抗模型的惯性矩阵、阻尼矩阵和刚度矩阵，q_d和q_s分别为机器人参考关节角和实际关节角。

S3.4、机器人与环境间的作用力依照下列公式计算。

$M_d\mathrm{\&Delta;}\stackrel{\&CenterDot;\&CenterDot;}{q}+B_d\mathrm{\&Delta;}\stackrel{\&CenterDot;}{q}+K_d\mathrm{\&Delta;}q=F_e.---\left(10\right)$

S3.5、由阻抗控制理论，按公式

$\mathrm{\&Delta;}Q\left(s\right)=\frac{F_e}{M_d\stackrel{\&CenterDot;\&CenterDot;}{s}+B_d\stackrel{\&CenterDot;}{s}+K_{d}s}---\left(11\right)$

计算作用力F_e经过阻抗模型后，机器人关节控制的轨迹修正量

S3.6、依据公式

Q_c＝Q_d-ΔQ (12)

计算经过修正的机器人关节控制指令式中，

S3.7、根据公式

$u_s={\stackrel{\&CenterDot;\&CenterDot;}{q}}_c+k_{sd}({\stackrel{\&CenterDot;}{q}}_c-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_c-q_s)---\left(13\right)$

设计经阻抗修正的机器人控制量u_s，实现机器人对手控器的跟随。

S3.8、机器人与环境(或障碍物)无接触情况时，依据公式

$u_s={\stackrel{\&CenterDot;}{q}}_m+k_{sd}(q_m-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_{sd}-q_s)---\left(14\right)$

设计机器人的控制量u_s，实现机器人对手控器的跟随。式中，k_sp、k_sd为机器人控制器的比例增益和微分增益。

Claims (4)

1.一种基于位置速度控制的遥操作机器人碰撞预警方法，其特征在于，包括以下步骤：

S1、基于手控器位置速度控制方法，以手控器的位置信号控制机器人运动；

S2、通过手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近目标对象的预警反力；

S3、将S1中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合，对手控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近目标对象，完成作业过程。

2.根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在于，所述步骤S1包括以下步骤：

S1.1、搭建基于摇杆式手控器控制机器人运动的系统控制平台；

S1.2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动做准备；

S1.3、通过将手控器视作为积分器，根据公式

$\left\{\begin{array}{cc}{q}_{sd}^{t+l+1}=k_{ms}{t}_{sam}\underset{n=t}{\overset{t+l}{\&Sigma;}}{q}_m^n+q_s^t& q_m^n\&NotEqual;0\\ q_{sd}^{n+1}=q_s^n& q_m^n=0\end{array}\right.$

计算手控器期望的机器人关节角控制信号q_sd，式中，k_ms为手控器与机器人关节角比例因子，t_sam为系统的采样周期，为n时刻手控器关节角，为t+l+1时刻手控器期望的机器人关节角，k_ms为手控器按钮修改比例因子；

S1.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号为零时，设定为上一采样时刻机器人关节角

S1.5、按照公式

τ_m＝J^TF_m

计算在操纵力F_m下手控器的操作力矩τ_m.

S1.6、将手控器的运动简化为质量-阻尼-弹簧系统，手控器动力学模型表示为公式

$M_m{\stackrel{\&CenterDot;\&CenterDot;}{q}}_m+B_m{\stackrel{\&CenterDot;}{q}}_m+K_m{q}_m=k_1{\mathrm{\&tau;}}_m,$

式中，F_m为手控器操纵力，J为手控器的雅可比矩阵，M_m、B_m和K_m为手控器的惯性系数矩阵，阻尼系数矩阵和弹簧系数矩阵，k₁为操作者手动控制的权值系数。

3.根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在于，所述步骤S2包括以下步骤：

S2.1、根据公式

$q_{st}^t=t\&CenterDot;k_{ms}{q}_m+q_s,(q_m\&NotEqual;0)$

预测t秒后机器人关节角位置式中，q_m、q_s为手控器及机器人当前时刻关节角；

S2.2、通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境；

S2.3、结合环境相机测得的背景环境或障碍物的高度信息，及预测得到的机器人末端执行器位置，求解机器人末端沿速度方向与背景环境的距离d；

S2.4、依据公式

$F_t=\left\{\begin{array}{cc}{k}_t(d_0-d)\stackrel{\&RightArrow;}{v}& d<d_0\\ 0& d\&GreaterEqual;d_0\end{array}\right.$

计算机器人所受的碰撞预警力F_t，式中，k_t为比例因子，d₀为机器人末端与障碍物间的安全距离，为机器人末端运动速度的单位向量；

S2.5、依据公式

$\left[\begin{array}{c}{\mathrm{\&tau;}}_t\\ {\mathrm{\&tau;}}_r\end{array}\right]=J^T\left[\begin{array}{c}\left[\begin{array}{c}{\mathrm{RF}}_t\\ {\mathrm{RF}}_r\end{array}\right]\\ \left[\begin{array}{c}{\mathrm{RF}}_e\\ {\mathrm{RM}}_e\end{array}\right]\end{array}\right]$

计算机器人与环境间的接触反馈力矩τ_r和预警反馈力矩τ_t，式中，F_e为机器人末端与环境间的作用力，M_t、M_e为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋转矩阵。

4.根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在于，所述步骤S3包括以下步骤：

S3.1、按照公式

$M_m{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{md}+B_m{\stackrel{\&CenterDot;}{q}}_{md}+K_m{q}_{md}=k_1{\mathrm{\&tau;}}_m-k_2({\mathrm{\&tau;}}_t+{\mathrm{\&tau;}}_r)$

设计手控器的位置预测控制器，式中，k₂为预警反馈力矩的权值系数，τ_m为手控器在操纵杆的操作力矩、τ_r为机器人与环境的接触力矩，τ_t为预警反馈力矩，q_md为手控器的运动趋势；

S3.2、按照公式

$u_m=k_{md}({\stackrel{\&CenterDot;}{q}}_{md}-{\stackrel{\&CenterDot;}{q}}_m)+k_{mp}(q_{md}-q_m)$

设计手控器的控制量u_m，使手控器跟随反馈力矩输出的运动趋势，式中，k_mp、k_md为手控器的比例增益和微分增益；

S3.3、依据公式

$M_d({\stackrel{\&CenterDot;\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;\&CenterDot;}{q}}_s)+B_d({\stackrel{\&CenterDot;}{q}}_d-{\stackrel{\&CenterDot;}{q}}_s)+K_d(q_d-q_s)=F_e$

设计机器人阻抗模型，式中，M_d、B_d和K_d分别为期望阻抗模型的惯性矩阵、阻尼矩阵和刚度矩阵，q_d和q_s分别为机器人参考关节角和实际关节角；

S3.4、机器人与环境间的作用力矩满足公式

$M_d\mathrm{\&Delta;}\stackrel{\&CenterDot;\&CenterDot;}{q}+B_d\mathrm{\&Delta;}\stackrel{\&CenterDot;}{q}+K_d\mathrm{\&Delta;}q=F_e;$

S3.5、按公式

$\mathrm{\&Delta;}Q\left(s\right)=\frac{F_e}{M_d\stackrel{\&CenterDot;\&CenterDot;}{s}+B_d\stackrel{\&CenterDot;}{s}+K_{d}s}$

计算作用力F_e经过阻抗模型后，机器人关节控制的轨迹修正量

S3.6、依据公式

Q_c＝Q_d-ΔQ

计算经过修正的机器人关节控制指令式中，

S3.7、根据公式

$u_s={\stackrel{\&CenterDot;\&CenterDot;}{q}}_c+k_{sd}({\stackrel{\&CenterDot;}{q}}_c-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_c-q_s)$

设计经阻抗修正的机器人控制量u_s，实现机器人对手控器的跟随；

S3.8、机器人与环境(或障碍物)无接触情况时，依据公式

$u_s={\stackrel{\&CenterDot;}{q}}_m+k_{sd}(q_m-{\stackrel{\&CenterDot;}{q}}_s)+k_{sp}(q_{sd}-q_s)$

设计机器人的控制量u_s，实现机器人对手控器的跟随；式中，k_sp、k_sd为机器人控制器的比例增益和微分增益。