基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质与流程

本发明涉及多自由度并联机器人，具体而言涉及一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质。

背景技术：

1、三自由度曲柄连杆并联运动平台是一种轻量化的多自由度运动平台，与传统的运动平台类似的是，将多个运动部件设置在上平台和下平台(底座、地面等)之间，基于运动部件的摆动运动(曲柄连杆摆动角度变化)实现对上平台的位置姿态的控制。在三自由度曲柄连杆并联运动平台的设计中，运动部件是基于电机驱动曲柄连杆的摆动而构造的，通过电机驱动曲柄连杆的摆动角度变化实现对上平台的位置、姿态控制。

2、与传统的电动缸式并联运动平台相比，采用曲柄连杆实现的三自由度并联运动平台具有体积小、结构简单、成本低的优点，但其位置反解比电动缸并联运动平台复杂。目前典型的基于曲柄连杆并联运动平台位置反解是通过列出曲柄末端点轨迹方程、上铰接点距离曲柄末端点距离为固定常数的原理，建立非线性方程组，求解曲柄末端点位置，进而求解位置反解，其解算获得的近似解，对平台位置和姿态的控制精度较低。

技术实现思路

1、根据本发明目的的第一方面，提出一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，利用余弦定理和三角形摆动误差修正，获得三自由度曲柄连杆并联运动平台的位置反解，实现对曲柄连杆运动机构的曲柄旋转角度的精确控制。

2、作为可选的实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，包括以下步骤：

3、定义三自由度并联运动平台的曲柄连杆机构，及其与上平台、下平台之间的运动关节，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节；

4、定义上铰接点、下铰接点的位置；

5、对于任意一曲柄连杆运动机构i，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度l_i，i＝1，2，3；

6、根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i；

7、根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标；

8、根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度l'_i；

9、根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i；

10、获取运动后上铰接点距离运动前曲柄末端点的距离l”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i；

11、判断上平台的位置和姿态是否超过曲柄连杆机构的行程，如果任意一组曲柄连杆机构的上铰接点与下铰接点连线的初始长度l'_i大于sum，或小于sub，则上平台的位置和姿态超过曲柄连杆机构的行程，判定曲柄连杆机构反解无解并结束流程，否则进入下一步反解过程；其中sum表示曲柄与连杆长度之和，sub表示曲柄与连杆长度之差的绝对值；

12、基于上铰接点与下铰接点连线运动后的倾斜角度θ'_i与上铰接点与下铰接点连线的初始倾斜角度θ_i，计算倾斜角度差值△θ_i；

13、基于上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i与上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，计算曲柄夹角差值△ω_i；以及

14、基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度angle_i，获得曲柄连杆机构的曲柄摆动角度反解。

15、作为可选的实施方式，所述定义上铰接点、下铰接点的位置，包括：

16、上铰接点：位于连杆的的顶端与上平台的球铰球心位置；

17、曲柄末端点：位于曲柄与连杆的单旋转关节处，具体位于连杆轴线与该单旋转关节轴线交点位置；

18、下铰接点：位于连杆的运动平面与电机旋转轴延长线的交点位置；

19、并且，定义上铰接点、曲柄末端点以及下铰接点的坐标如下：

20、xb_i：下铰接点初始x坐标值；

21、yb_i：下铰接点初始y坐标值；

22、zb_i：下铰接点初始z坐标值；

23、xp_i：上铰接点初始x坐标值；

24、yp_i：上铰接点初始y坐标值；

25、zp_i：上铰接点初始z坐标值；

26、xm_i：曲柄末端点初始x坐标值；

27、ym_i：曲柄末端点初始y坐标值；

28、zm_i：曲柄末端点初始z坐标值；

29、xp'_i：上铰接点运动后的x坐标值；

30、yp'_i：上铰接点运动后的y坐标值；

31、zp'_i：上铰接点运动后的z坐标值。

32、根据本发明目的的第二方面，还提出一种计算机系统，包括：

33、一个或多个处理器；以及

34、存储器，存储可被操作的指令；

35、其中，所述指令在通过一个或多个处理器执行时使得前述的一个或多个处理器执行操作，所述操作包括执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

36、根据本发明目的的第三方面，还提出一种计算机可读存储介质，用于存储一个或多个程序，所述一个或多个程序包括能够被一个或者多个处理器执行的指令或者指令集；

37、其中，所述指令或者指令集在由一个或多个处理器执行时，执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

38、应当理解，前述构思以及在下面更加详细地描述的额外构思的所有组合只要在这样的构思不相互矛盾的情况下都可以被视为本公开的发明主题的一部分。另外，所要求保护的主题的所有组合都被视为本公开的发明主题的一部分。

39、结合附图从下面的描述中可以更加全面地理解本发明教导的前述和其他方面、实施例和特征。本发明的其他附加方面例如示例性实施方式的特征和/或有益效果将在下面的描述中显见，或通过根据本发明教导的具体实施方式的实践中得知。

技术特征：

1.一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，包括：

2.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述定义上铰接点、下铰接点的位置，包括：

3.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度l_i，包括：

4.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，包括：

5.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标，包括：

6.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度l'_i，包括：

7.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i，包括：

8.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述获取运动后上铰接点距离运动前曲柄末端点的距离l”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i，包括：

9.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度angle_i，获得曲柄连杆机构的曲柄摆动角度反解，包括：

10.一种计算机系统，其特征在于，包括：

11.一种计算机可读存储介质，用于存储一个或多个程序，其特征在于，所述一个或多个程序包括能够被一个或者多个处理器执行的指令或者指令集；

技术总结
本发明提供一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质，该反解控制方法适用于以曲柄连杆机构为运动机构的三自由度并联运动平台，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节。定义上铰接点位于连杆的的顶端与上平台的球铰球心位置，铰接点位于连杆的运动平面与电机旋转轴延长线的交点位置。利用余弦定理和三角形摆动误差修正，获得三自由度曲柄连杆并联运动平台的位置反解，实现对曲柄连杆运动机构的曲柄旋转角度的精确控制，完成对曲柄摆动的反解。

技术研发人员：叶江,倪进宇,杨宗平,王昭
受保护的技术使用者：江苏普旭科技股份有限公司
技术研发日：
技术公布日：2024/2/29