一种头戴式数据采集机器人的姿态协同控制方法与流程

摘要: 本发明涉及机器人运动控制,具体为一种头戴式数据采集机器人的姿态协同控制方法。背景技术:1、头戴式数据采集机器人多用于跟随佩戴者头部运动调整末端位姿,其控制依赖头...
一种头戴式数据采集机器人的姿态协同控制方法与流程

本发明涉及机器人运动控制,具体为一种头戴式数据采集机器人的姿态协同控制方法。

背景技术:

1、头戴式数据采集机器人多用于跟随佩戴者头部运动调整末端位姿,其控制依赖头部惯性测量单元获取头部姿态或角速度,并通过固定映射关系生成机器人末端目标位姿。佩戴者静止或基座保持固定时,该方式可以基本反映头部运动意图。佩戴者行走、俯身、转向或承载基座姿态变化时,头部惯性数据中包含头部相对大地的绝对姿态变化,而机器人末端需要跟随头部相对于基座的运动,二者在动态条件下并不一致。现有控制方式未根据关节编码器数据获得基座姿态四元数,也未将头部姿态四元数与基座姿态四元数解算为相对姿态矩阵,导致末端目标位姿容易混入基座运动分量,造成数据采集位置偏差。头戴式数据采集机器人通常具有多个关节,存在冗余调节空间。现有跟随控制将头部运动直接映射为任务空间位姿,关节速度合成多采用伪逆或固定映射方式,未根据当前雅可比矩阵提取零空间向量,无法在不改变末端目标位姿增量的前提下对关节构型进行调节,接近关节极限或奇异位形时会出现指令突变和运动不协调。

2、惯性数据积分误差、地磁数据波动以及机器人末端与佩戴者头部之间的安装间隙会引起位姿漂移。现有控制回路大多未引入机器人末端与佩戴者头部之间的测距数据对关节速度前馈指令进行修正,缺少实际距离变化反馈,长时间工作时难以保持末端与头部之间的相对位置一致性。

技术实现思路

1、针对现有控制方式中头部姿态与基座姿态耦合、冗余关节协调能力不足以及缺少距离闭环修正的问题,本发明的目的在于提供一种头戴式数据采集机器人的姿态协同控制方法,利用相对姿态矩阵、零空间向量和末端测距修正生成关节速度协同指令,减小末端跟随偏差,改善非平稳佩戴条件下的关节运动协调性。

2、为了实现上述目的,本发明提供如下技术方案:一种头戴式数据采集机器人的姿态协同控制方法,包括:

3、实时采集佩戴者头部的惯性数据和地磁数据以及头戴式数据采集机器人的关节编码器数据,对惯性数据和地磁数据进行姿态更新,得到头部姿态四元数和头部角速度;

4、根据关节编码器数据得到基座姿态四元数,将头部姿态四元数和基座姿态四元数解算为相对姿态矩阵;

5、将相对姿态矩阵映射至头戴式数据采集机器人操作空间,得到目标位姿增量;

6、根据关节编码器数据解算当前雅可比矩阵并提取零空间向量;

7、将目标位姿增量与零空间向量合成关节速度前馈指令;

8、根据头戴式数据采集机器人末端与佩戴者头部之间的测距数据修正关节速度前馈指令,得到关节速度协同指令并驱动头戴式数据采集机器人关节运动。

9、优选的,所述对惯性数据和地磁数据进行姿态更新,得到头部姿态四元数和头部角速度,包括:

10、将惯性数据中的角速度按采样间隔进行四元数积分,得到第一姿态四元数;

11、根据地磁数据与重力方向构建观测矢量,将观测矢量与第一姿态四元数对应的参考矢量进行叉积,得到姿态校正矢量;

12、根据姿态校正矢量生成角速度补偿量,将角速度补偿量叠加至惯性数据中的角速度,得到补偿角速度;

13、根据补偿角速度更新第一姿态四元数,得到头部姿态四元数;

14、将补偿角速度中去除零偏后的角速度作为头部角速度。

15、优选的,所述将头部姿态四元数和基座姿态四元数解算为相对姿态矩阵,包括:

16、对基座姿态四元数求逆,得到基座逆四元数;

17、将头部姿态四元数与基座逆四元数按四元数乘法合成,得到相对姿态四元数;

18、根据相对姿态四元数构建旋转矩阵;

19、将旋转矩阵的水平分量投影至头戴式数据采集机器人的基座平面,得到水平投影矩阵;

20、将旋转矩阵的竖直分量与水平投影矩阵组合,得到相对姿态矩阵。

21、优选的,所述将相对姿态矩阵映射至头戴式数据采集机器人操作空间,得到目标位姿增量,包括:

22、从相对姿态矩阵提取航向角、俯仰角和横滚角;

23、根据航向角生成头戴式数据采集机器人末端绕竖直轴的姿态增量;

24、根据俯仰角生成头戴式数据采集机器人末端沿前后方向的位置增量;

25、根据横滚角生成头戴式数据采集机器人末端沿左右方向的位置增量;

26、根据航向角、俯仰角和横滚角生成头戴式数据采集机器人末端绕水平轴的姿态增量;

27、将位置增量与姿态增量组成目标位姿增量。

28、优选的,所述根据关节编码器数据解算当前雅可比矩阵并提取零空间向量,包括:

29、根据关节编码器数据得到各关节角度;

30、根据各关节角度和连杆参数构建各关节的运动旋量;

31、将各运动旋量按关节顺序排列,得到当前雅可比矩阵;

32、对当前雅可比矩阵进行增广,得到增广矩阵;

33、根据增广矩阵的转置和逆矩阵得到零空间投影矩阵;

34、将零空间投影矩阵与预设的关节速度向量相乘,得到零空间向量。

35、优选的,所述将目标位姿增量与零空间向量合成关节速度前馈指令,包括:

36、将目标位姿增量按采集周期转换为任务空间速度;

37、根据当前雅可比矩阵的转置与任务空间速度得到第一关节速度;

38、将零空间向量与第一关节速度进行加权叠加,得到合成关节速度;

39、根据各关节的允许速度范围对合成关节速度进行限幅,得到限幅关节速度;

40、根据限幅关节速度与关节加速度限制生成关节速度前馈指令。

41、优选的,所述根据头戴式数据采集机器人末端与佩戴者头部之间的测距数据修正关节速度前馈指令,得到关节速度协同指令,包括:

42、接收测距数据中的距离变化方向和距离变化速率;

43、根据距离变化方向确定沿头戴式数据采集机器人末端至佩戴者头部方向的速度修正方向;

44、根据距离变化速率确定速度修正幅值;

45、将速度修正方向与速度修正幅值构成速度修正量;

46、将速度修正量经当前雅可比矩阵的转置映射至关节空间,得到关节修正速度;

47、将关节修正速度与关节速度前馈指令叠加,得到关节速度协同指令。

48、优选的,所述驱动头戴式数据采集机器人关节运动,包括:

49、将关节速度协同指令发送至各关节伺服驱动器;

50、采集各关节伺服驱动器的电流反馈和位置反馈;

51、根据位置反馈和关节速度协同指令生成关节速度校正量;

52、根据电流反馈生成关节负载力矩;

53、将关节速度校正量与关节负载力矩输入关节阻抗控制器,得到关节力矩指令;

54、根据关节力矩指令驱动头戴式数据采集机器人的关节运动。

55、优选的,所述根据关节编码器数据得到基座姿态四元数,包括:

56、从关节编码器数据中提取底座关节的编码器读数;

57、根据底座关节的编码器读数确定头戴式数据采集机器人基座相对于行走机构的安装姿态;

58、采集头戴式数据采集机器人基座上的惯性单元输出的基座角速度和基座加速度;

59、根据基座角速度和基座加速度更新基座初始四元数;

60、根据安装姿态对基座初始四元数进行旋转补偿,得到基座姿态四元数。

61、优选的,所述实时采集佩戴者头部的惯性数据和地磁数据以及头戴式数据采集机器人的关节编码器数据,包括:

62、按照不同采样周期分别触发头部惯性采集单元、地磁采集单元和关节编码器;

63、将头部惯性采集单元输出的三轴角速度和三轴加速度与地磁采集单元输出的三轴磁场强度按时间戳对齐;

64、根据时间戳插值补齐缺失的头部采集数据;

65、将关节编码器输出的绝对位置和增量位置按同一时基转换为关节角度;

66、将补齐后的头部采集数据和关节角度打包为同步数据帧。

67、在上述技术方案中,本发明提供的技术效果和优点:

68、将头部姿态四元数与基座姿态四元数解算为相对姿态矩阵,并将相对姿态矩阵映射至头戴式数据采集机器人操作空间,得到目标位姿增量。相对姿态矩阵通过基座逆四元数和四元数乘法获得,使头部运动在基座坐标系下描述,目标位姿增量中不再混入基座姿态变化带来的干扰分量。佩戴者行走、俯身或转向时,基座姿态变化被同步扣除,末端运动指令仅响应头部相对基座的运动,由此减小头部姿态与基座姿态耦合造成的操作空间偏差。目标位姿增量与零空间向量合成关节速度前馈指令后,冗余关节在完成末端位姿跟随之外获得自运动调节能力,关节速度可在零空间内重新分配,使关节趋向远离极限位置和奇异构型的方向运动。该方式相对于单一映射控制更平稳,关节速度指令的连续性得到改善,降低了佩戴者非平稳运动时的关节速度突变风险。机器人末端与佩戴者头部之间的测距数据用于修正关节速度前馈指令。根据距离变化方向和距离变化速率得到的速度修正量,利用当前雅可比矩阵转置映射至关节空间,并与关节速度前馈指令叠加形成关节速度协同指令。测距回路提供末端与头部之间实际相对距离的变化信息,当惯性数据存在积分漂移、安装姿态误差或头部产生未被惯性测量充分反映的微小位移时,距离变化能够触发关节速度修正,使末端与头部之间的相对距离变化得到抑制。相比仅依靠头部惯性姿态前馈的常规方式,该闭环修正对误差源具有直接响应能力,降低长时间运行中的累积漂移和末端贴合误差,增强关节速度协同指令对外部扰动和佩戴者头部运动不确定性的适应性。