二连杆机械臂的空间运动信息确定方法、装置、设备及存储介质与流程

本技术涉及状态估计，尤其涉及二连杆机械臂的空间运动信息确定方法、装置、设备及存储介质。

背景技术：

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、由于采用了基于状态估计值和协方差矩阵生成采样点、并将采样点通过状态转移方程进行非线性传播后执行滤波校正的技术手段，关节角速度滤波估计值由滤波校正直接输出，无需对关节角度测量值进行数值差分，从而解决了仅依靠关节角度传感器无法直接提供关节角速度信息的问题。与现有技术中通过数值差分获取角速度或通过线性化近似估计角速度的方式相比，本技术避免了关节角速度估计过程中的数值差分噪声和线性化截断误差。