一种有脊椎仿生足式机器人运动规划方法

本发明涉及机器人领域，具体涉及一种有脊椎仿生足式机器人运动轨迹方法。

背景技术：

1、随着足式机器人的发展，越来越多的仿生足式机器人出现在人们的视野中。现阶段大部分成熟的仿生足式机器人均没有脊椎关节，与没有脊椎关节的足式机器人相较而言，有脊椎关节可通过协调脊椎和四肢之间的运动，提高足式机器人的运动效率。但随着有脊椎仿生足式机器人脊椎关节的增加，机器人整体的自由度也增多，自由度数的增多也增加了足式机器人的控制难度。脊椎的摆动也会影响机器人的运动，现有的足式机器人运动规划方法常应用于无脊椎足式机器人上，无法直接应用到有脊椎足式机器人中。

技术实现思路

1、本发明的目的在于实现有脊椎仿生足式机器人的四肢和脊椎之间的协调运动，提出了一种有脊椎仿生足式机器人运动规划方法。

2、实现本发明目的的技术解决方案为：一种有脊椎仿生足式机器人运动规划方法，采用d-h法建立包含腿关节及其与脊椎相连部分的运动学模型，这样机器在运动时，不光依靠腿部关节，脊椎关节也参与运动，使机器人的整体运动更加协调。采样真实有脊椎动物的足端运动轨迹作为基础，并进行拟合，可以快速规划出机器人的足端运动轨迹，具体包括以下步骤：

3、步骤1、使用d-h法对机器人腿部关节及其与脊椎相连部分建立机器人运动学模型，包括正运动学模型与逆运动学模型，具体如下：

4、s1-1、所述机器人腿关节及其与脊椎相连部分包括6个旋转关节，自第一个脊椎关节到腿部最后一个关节依次命名为第一关节、第二关节……第六关节，为每个关节添加坐标系并指定方向。

5、s1-2、根据d-h法建立d-h参数表，qi表示机器人第i个关节的关节角度，di表示机器人第i个关节到第i+1个关节的连杆偏移量，ai表示第i个关节到第i+1个关节的连杆之间的长度，αi表示第i个关节和第i+1个关节之间的连杆扭转角，c表示角度的余弦值，s表示角度的正弦值；其中第三关节、第四关节和第五关节的旋转轴线交于一点，将d-h参数带入下式：

6、

7、得到第i个关节处的齐次变换矩阵i-1ti。

8、s1-3、机器人足端坐标系o6-x6y6z6相对于基坐标系o0-x0y0z0的齐次变换矩阵如下：

9、0t6＝0t11t22t33t44t55t6

10、转入步骤2和步骤3。

11、步骤2、给定每个关节的运动范围，在上述运动学模型基础上求出机器人正运动学解，得到机器人足端的可达运动空间，具体如下：

12、s2-1、采用蒙特卡洛法计算机器人足端的可达运动空间，每个关节的关节角度qi由下式生成：

13、

14、其中，表示第i个关节的最小关节角度，表示第i个关节的最大关节角度，rand是产生0～1之间随机数的函数。

15、s2-2、通过多次采样得到qi的多组采样值，通过机器人的正运动学模型，得到每组采样值对应的足端位置。

16、s2-3、根据每组采样值对应的足端位置，计算所有足端位置的包络面，即为足端的可达运动空间。

17、转入步骤4。

18、步骤3、给定机器人足端位姿信息，根据步骤1的机器人运动学模型，求解机器人腿部关节及其与脊椎相连部分的逆运动学，具体如下：

19、s3-1、用单位正交矢量n、o、a表示相对于基座参考坐标系的末端运动关节中心点的运动坐标系的三个轴；a＝[ax ay az]表示接近轴，其方向为机器人末端运动关节沿着末端z轴的方向，ax为接近轴在基座参考坐标系x轴上的投影，ay为接近轴在基座参考坐标系y轴上的投影，az为接近轴在基座参考坐标系z轴上的投影；o＝[ox oy oz]表示方向轴，其方向为末端运动关节接近物体的方向，ox为方向轴在基座参考坐标系x轴上的投影，oy为方向轴在基座参考坐标系y轴上的投影，oz为方向轴在基座参考坐标系z轴上的投影；n＝[nx nynz]代表的是垂直轴，其方向分别垂直于a轴和o轴，nz为垂直轴在基座参考坐标系x轴上的投影，ny为垂直轴在基座参考坐标系y轴上的投影，nz为垂直轴在基座参考坐标系z轴上的投影；则机器人足端坐标系o6-x6y6z6相对于基坐标系o0-x0y0z0的齐次变换矩阵表示为：

20、

21、用(px,py,pz)表示足端坐标系o6-x6y6z6的位置，px、py和pz分别为足端坐标系位置的x分量，y分量和z分量，同样是基于基坐标系o0-x0y0z0。

22、对公式(1)两边同时右乘矩阵得到以下公式：

23、

24、转入s3-2。

25、s3-2、求解第四关节的关节角度q4：

26、将0t5和0t6(5t6)-1两个矩阵的第3行第4列元素建立等式，求得第四个关节的关节角度q4如下：

27、

28、其中，a6表示第6个关节到第7个关节的连杆之间的长度。

29、转入s3-3。

30、s3-3、求解第六关节的关节角度q6：

31、将0t5和0t6(5t6)-1两个矩阵的第3行第2列元素建立等式，求得第六个关节的关节角度q6如下：

32、

33、其中，第i个关节的角度余弦ci＝cos(qi)；

34、转入s3-4。

35、s3-4、求解第五关节的关节角度q5：

36、将0t5和0t6(5t6)-1个矩阵的第3行第1列、第3列元素分别建立等式，当s4≠0，即q4≠0且q4≠±π，求得q5如下：

37、

38、其中，第i个关节的角度正弦si＝sin(qi)；

39、转入s3-5。

40、s3-5、求解第一关节的关节角度q1、第二关节的关节角度q2和第三关节的关节角度q3：

41、将0t5和0t6(5t6)-1两个矩阵的第1行第2列、第2行第2列元素分别建立等式，当s4≠0时，即q4≠0且q4≠±π，求得第一、第二、第三关节的关节角度之和q123如下：

42、

43、定义中间变量a＝px-a6nx-d5s4c123，中间变量b＝py-a6ny-d5s4s123，将0t5和0t6(5t6)-1两个矩阵的第1行第4列和第2行第4列元素分别建立等式，求得第一关节的关节角度q1和第一、第二关节角度之和q12如下：

44、

45、

46、其中余弦三角函数c123＝cos(q1+q2+q3)，正弦三角函数s123＝sin(q1+q2+q3)。

47、由公式(2)、公式(3)、公式(4)求得第二关节的关节角度q2和第三关节的关节角度q3：

48、

49、

50、通过以上步骤求得各个关节角度的解析解，共8组解，通过对每个关节的运动范围及轨迹优化过程来选取一组最优解。

51、转入步骤5。

52、步骤4、在机器人正运动学解的基础上，结合机器人足端的可达运动空间，规划机器人足端运动轨迹，具体如下：

53、s4-1、采样并优化真实有脊椎动物爬行时的足端摆动相运动轨迹，转入s4-2。

54、s4-2、通过高斯函数对优化的足端摆动相运动轨迹进行拟合，转入s4-3。

55、s4-3、在机器人足端运动空间的基础上，缩放拟合的足端摆动相运动轨迹，使得足端摆动相运动轨迹在机器人的足端可达运动空间范围内，转入s4-4。

56、s4-4、在缩放的足端摆动相运动轨迹的基础上，规划足端支撑相运动轨迹。

57、转入步骤5。

58、步骤5、使用机器人腿部关节及其与脊椎相连部分的逆运动学对机器人足端运动轨迹对应的各关节转动角度进行求解，实现对有脊椎仿生足式机器人运动轨迹规划。

59、本发明与现有技术相比，其显著优点在于：

60、(1)采用本发明的运动规划方法，可应用到有脊椎足式机器人上。通过对脊椎和四肢连接部分建立运动学模型，通过正逆运动学求解，实现对机器人脊椎和四肢的协调控制，提高足式机器人运动效率。

61、(2)通过采集真实脊椎动物足端摆动相运动轨迹并应用到机器人运动规划中，使得机器人腿足结构在运动过程中更加平滑。