一种基于操作器的机器人控制方法及其系统与流程

本发明涉及机器人控制领域，更具体地，涉及一种基于操作器的机器人控制方法及其系统。

背景技术：

1、随着自动化技术的发展，各种自动化辅助设备如机器人等被应用至各行各业。如在医疗行业中，有各种手术机器人等辅助设备，例如腹腔手术机器人、骨科手术机器人等。对于主从控制的机器人，需要跟踪操作者手部的运动，并将其复刻到机器人的运动上。目前现有的机器人控制器利用了操作杆控制器的各关节上的编码器和其他各类传感器进行位置和姿态信息计算，从而对机器人下达指令，完成主从运动控制。基于上述设计的操作器结构较为庞大，需要专用的控制台安置，具有不够轻便化和可移动性差的问题。同时机械式设计的操作器对于操作者来说与习惯中的握持器械进行手术操作的动作有较大差别，能够熟练掌握的学习曲线较长。

2、为了将操作器轻便化，现有设计一种手持式的操作器，操作其通过惯性测量单元或者红外定位其进行定位和获得运动信息，然后将运动信息转化为机器人的运动。但是使用惯性测量单元会导致较大的位移累计误差，激光定位器又受限于环境产生的激光偏移或反射，容易产生误差波动，单独使用两者的信号都会导致非正常的位移和抖动，令操作器的空间定位不够精准稳定，最终导致机器人的运动准确性下降。

技术实现思路

1、本发明为克服上述现有技术操作器空间定位不够精准稳定问题，提供一种基于操作器的机器人控制方法及其系统，同时使用惯性测量单元和激光定位器来实现操作器的准确稳定的空间定位。

2、为解决上述技术问题，本发明采用的技术方案是：一种基于操作器的机器人控制方法，包括如下步骤：

3、步骤一：在操作器上设置惯性测量单元和激光定位器；

4、步骤二：分别获取惯性测量单元的加速度数据和转角数据以及激光定位器的转角数据，并进行重力加速误差和测量误差修正，得到操作器加速度信号u和操作器旋转角度qm；

5、步骤三：基于加速度信号u计算操作器速度u和基于激光定位器获取操作器位置数据p；

6、步骤四：分别计算前后两个时刻的操作器速度u和操作器位置数据p，根据前后两个时刻的操作器速度u和操作器位置数据p得到操作器位移信号δp；

7、步骤五：将操作器位移信号δp转化为机器人的位移指令；将操作器旋转角度qm转化为机器人的姿态指令。

8、在上述的技术方案中，首先使用激光定位器进行指令信号的计算，但考虑到：1)激光易于在玻璃或金属材质上发生折射或反射，从而导致单方面使用激光定位器进行亚毫米级别精细的机器人控制存在一定误差；2)在各种应用场景中，例如手术机器人控制，机器人的控制需要过滤人手的抖动等噪声信号，以实现安全平滑的控制。因此技术方案融合了惯性测量单元辅助激光定位器解决上述的误差及噪声问题。惯性测量单元可以通过同步捕获计算操作器速度来过滤激光反射/折射造成的瞬时定位偏移以及人手低速的抖动偏移。但惯性测量单元的计算底层为加速度信号，本身受磁场等因素干扰，存在一定误差。计算惯性测量单元的运动速度需要对加速度进行积分操作，从而进一步放大了误差。因此方案通过分别获取惯性测量单元和激光定位器的数据对操作器的加速度信号进行修正，其中通过激光定位器来修正惯性测量单元的重力加速度误差和测量误差，获得操作器更加准确的加速度信号，通过更加准确的加速度信号获得更加精准的操作器速度u，然后结合操作器速度u和操作器位置数据p来过滤掉人手抖动噪声和激光器定位受折射/反射产生的偏移误差，并计算操作器在前后两个时刻的位移信号δp。以操作器的位移信号δp为基础，将其转换为机器人的位移指令。以操作器的旋转角度也就是操作器的姿态变换为基础，将其转换为机器人的姿态指令。机器人执行位移指令和姿态指令。

9、优选的，在所述步骤二中，加速度信号u的公式具体如下：

10、

11、式中，rimu表示为基于惯性测量单元的旋转矩阵；上标-1表示矩阵求逆；上标t表示矩阵求转置；uimu表示为惯性测量单元的加速度数据；|| ||表示求矩阵的二范数；为惯性测量单元的测量误差矩阵在惯性测量单元的加速度输出矩阵uimu的映射分量；rimug表示为重力加速度在惯性测量单元的加速度输出矩阵uimu的映射分量；rimu表示为基于激光定位器的旋转矩阵；gimu为利用激光定位器姿态及惯性测量单元加速度数据求解出的重力加速度在惯性测量单元中的测量值；e表示为惯性测量单元测量误差矩阵。

12、优选的，惯性测量单元的旋转矩阵根据惯性测量单元的转角数据构造，具体为：

13、

14、式中，αimu,βimu,γimu分别表示围绕惯性测量单元的x轴、y轴及z轴旋转的角度；

15、gimu＝δr-1uimut

16、δr＝rimu-1-rir-1

17、

18、式中，rir为激光定位器的旋转矩阵，αir,βir,γir分别表示围绕红外激光定位器的x轴、y轴及z轴旋转的角度；

19、e＝gimu-g

20、式中，g＝[0,0,g]t，其中g为重力加速度。

21、优选的，在所述步骤三中，对加速度信号u进行滤波处理后得到u′，对u′进行累加处理得到u′，再对u′进行二次滤波得到操作器速度u。

22、优选的，u′的滤波方式为设置滤波阈值，设置滤波阈值与加速度信号u的不同大小对比关系与u′值的映射关系，根据输入的加速度信号u和映射关系得到u′。通过设置滤波阈值，过滤人手生理震颤的效果。

23、优选的，二次滤波的方式为：设置可调节阈值，设置可调节阈值与u′的大小对比关系与操作器速度u的映射关系，根据输入的u′和映射关系得到操作器速度u。通过合理地设置可调节阈值，可以有效抑制非正常的快速运动，如意外碰撞操作器、操作人员手被误碰、操作器滑落等情况。

24、优选的，在所述步骤四中，具体的流程为:

25、s4.1：在t0时刻执行步骤一至三，获得t0时刻的操作器速度u和操作器位置数据p，分别记为ut0及pt0；

26、s4.2：在t1时刻重新执行步骤一至步骤三，获得t1时刻的操作器速度u和操作器位置数据p，分别记为ut1和pt1；

27、s4.3：处理并输出t0和t1两个时刻之间的操作器位移信号δp，具体为：

28、

29、式中，abs表示为取绝对值运算；k2表示为预设的阈值。

30、t0和t1两个时刻中其中一个为当前时刻，另一个可以是当前时刻的前一个时刻或后一个时刻。通过上述的公式对激光定位器位移信号的噪声过滤，避免了非正常的噪声运动信号，最终实现稳定的操作器控制效果。

31、优选的，在所述步骤五中，还获取机器人旋转矩阵rr和基于操作器旋转角度qm构造操作器旋转矩阵rm，利用上述旋转矩阵，将操作器的空间位置映射到机器人的坐标系中。具体地，可以实现人手以任意方式及任意姿态握持操作器，并处于激光照射的任意位置，机器人响应操作器的运动方向，始终与人手握持操作器的运动方向完全一致。从而实现了利用操作定位控制机器人的同时，避免了当握持方向与机器人运动方向不一致时，出现人机不协调的情况；

32、根据操作器位移信号δp及机器人旋转矩阵rr，计算操作器映射到机器人坐标系下的位移指令δpr：

33、δpr＝rr-1δp

34、根据操作器旋转矩阵rm及机器人旋转矩阵rr，计算操作器映射到机器人坐标系下的姿态指令α、β、及γ：

35、rtmp＝rr-1rm

36、

37、

38、γ＝atan(-rtmp(1,2)cos(β)rtmp(3,3)/rtmp(1,1)cos(α)rtmp(2,2))

39、式中，rtmp表示rtmp矩阵；rtmp(1,3)表示为rtmp矩阵的第1行第3列数值；rtmp(1,2)表示为rtmp矩阵的第1行第2列数值；rtmp(2,3)表示为rtmp矩阵的第2行第3列数值；α、β、γ分别表示操作器围绕机器人坐标系的x轴、y轴及z轴旋转的角度。

40、优选的，机器人旋转矩阵rr具体为：

41、

42、式中，αr,βr,γr分别表示机器人基座围绕激光器基站的x轴、y轴及z轴旋转的角度；c表示为取cos；s表示为取sin。

43、操作器旋转矩阵rm具体为：

44、

45、式中，αm、βm及γm分别表示了操作器围绕激光器基站的x轴、y轴及z轴旋转的角度。

46、一种基于操作器的机器人控制系统，用于实现上述的基于操作器的机器人控制方法，包括操作器、机器人和安装于室内的激光器基站；所述操作器包括激光定位器和惯性测量单元；所述机器人也安装有激光定位器。

47、与现有技术相比，本发明的有益效果是：本发明同时使用惯性测量单元和激光定位器来获得操作器的加速度信号，修正其中的误差值，然后基于加速度信号得到操作器的位移信号，最后通过位移信号转换为机器人的位移，通过两个传感器互相补偿，从而避免了激光器因折射/反射导致的定位误差并同时过滤因人手的抖动等产生的噪声信号。

48、通过操作器与机器人的坐标系转换，实现了利用操作定位控制机器人的同时，避免了当握持方向与机器人运动方向不一致时出现人机不协调的情况，保证了机器人响应操作器的运动方向，始终与人手握持操作器的运动方向完全一致。通过上述增益效果，最终实现了空间中稳定、精准及完全人机一致的定位导航效果。