一种机器人腿部支撑算法的制作方法

1.本发明涉及机器人人腿部支撑控制算法，具体涉及一种机器人腿部支撑算法。


背景技术：

2.六足机器人在结构上具有较多冗余自由度，能够适应较为复杂地形，在路况复杂的野外行走，完成履带式或轮式所不能完成的非结构性环境中的运输作业，因此在很多方面具有广阔的应用前景。但是，以往六足机器人关节控制存在很大弊端，各腿在前进过程中绕各自跟关节摆动的轨迹为一条弧线，这样两侧腿在行进过程中形成互斥力，当机体较重时，对关节损伤比较严重；此外，各腿足端下落过程中，若与地面不垂直，容易造成侧滑，影响机体稳定性。
3.在中国申请号为201811396712 .4，公布日为2019.3.26的专利文献公开了一种六足机器人支撑控制算法，包括：机器人坐标系建立方法，机器人机体坐标系的建立，单步行腿坐标系建立，正逆支撑学解，支撑控制方法五个步骤；从各腿跟关节处看，该算法使机器人处于支撑态行走过程中足端轨迹为一条直线。
4.六足机器人的腿部多有由三个自由度的关节组成；其分别为第一关节、第二关节和第三关节；第二关节位于第一关节和第三关节之间；机械腿各关节铰接处为机械腿的支撑点；若左机械腿的支撑点向右摆动较大幅度，则不能对机器人进行支撑；若右机械腿的支撑点向左摆动较大幅度，则不能对机器人进行支撑；但是该六足机器人未考虑机械腿移动到目标点时；左机械腿或右机械腿支撑点的位置。


技术实现要素：

5.本发明提供一种机器人腿部支撑算法，基于待移动坐标，计算机械腿各关节需要摆动的角度；同时以第一关节和第二关节的铰接处为原点建立第一坐标系；沿机器人前进方向区分左机械腿和右机械腿；分别判断左机械腿和右机械腿铰接处所在象限；避免机械腿端部移动到待移动坐标时，机械腿各关节摆动幅度过大，导致不能支撑机器人的情况出现。
6.为达到上述目的，本发明的技术方案是：一种机器人腿部支撑算法，通过机器人实现，机器人包括底盘和两条以上的机械腿；机械腿对称设置在底盘两侧；所述机械腿包括第一关节、第二关节和第三关节；第一关节与底盘连接，第二关节与第一关节连接，第三关节与第二关节连接，第一关节带动第二关节和第三关节同步活动，第二关节带动第三关节同步活动，第三关节相对第二关节独立活动，包括以下步骤：步骤（1）.以第一关节和第二关节的铰接处为原点建立第一坐标系；预设第一关节和第二关节铰接处与第二关节和第三关节铰接处之间的距离l1；预设第二关节和第三关节铰接处与第三关节末端之间的距离l2。
7.步骤（2）.输入第一待移动坐标；第一待移动坐标为第三关节末端的下一移动目标点。
8.步骤（3）.以原点（x1，y1）为圆心，l1为半径，得到第一方程(x-x1)2 (x-y1)2=l
12
；第一方程为第一关节的活动范围；以第一待移动坐标（x2，y2）为圆心，l2为半径，得到第二方程(x-x2)2 (y-y2)2=l
22
；第二方程为第三关节活动范围。
9.步骤（4）.判断第一关节的活动范围与第三关节的活动范围是否存在交点，若否，则判断第三关节末端移动到第一待移动坐标时，机械腿无法支撑机器人；若是，则判断第三关节末端移动到第一待移动坐标时，机械腿能够支撑机器人。
10.以上方法，以第一关节和第二关节的铰接处为圆心，第一关节和第二关节铰接处与第二关节和第三关节铰接处之间的距离为半径、以第三关节末端为圆心，第二关节和第三关节铰接处与第三关节末端之间的距离为半径分别建立方程；通过判断两个方程之间是否有交点从而判断第三关节能否对第二关节进行支撑；若两个方程之间没有交点，则表示第三关节末端移动到第一待移动坐标时，第三关节不能对第二关节进行支撑，进而机械腿不能对机器人进行支撑；若两个方程之间具有一个以上的交点，则表示第三关节末端移动到第一待移动坐标时，第三关节能对第二关节进行支撑，进而机械腿能对机器人进行支撑。
11.进一步的，步骤（4）中，若第一关节的活动范围与第三关节的活动范围是否存在交点；则进行步骤（5）。
12.步骤（5）.将交点确定为第二待移动坐标；第二待移动坐标为第三关节末端移动到第一待移动坐标时，第二关节和第三关节铰接处所在坐标。
13.步骤（5.1）.沿机器人前进方向判断当前机械腿属于左侧机械腿还是右侧机械腿；若为左侧机械腿，则进行步骤（5.2）；若为右侧机械腿，则进行步骤（5.3）。
14.步骤（5.2）.判断交点所在象限；若为第一象限或第四象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿无法支撑机器人；若交点在第二象限或第三象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿能够支撑机器人。
15.步骤（5.3）.判断交点所在象限；若为第二象限或第三象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿无法支撑机器人；若为第一象限或第四象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿能够支撑机器人。
16.以上方法，以第一关节和第二关节的铰接处为原点建立第一坐标系；沿机器人前进方向区分左侧机械腿和右侧机械腿；用第二象限和第三象限限制左侧机械腿的第二待移动坐标位置；用第一象限和第四象限限制右侧机械腿的第二待移动坐标位置；这样避免左侧机械腿向右摆动较大幅度导致不能对机器人进行支撑；也避免右侧机械腿向左摆动较大幅度导致不能对机器人进行支撑；通过左侧机械腿的第二待移动坐标、右侧机械腿的第二待移动坐标判断移动到第一待移动坐标时能否对机器人进行支撑。
17.进一步的，步骤（5）还包括：通过第二待移动坐标（xm，ym）和公式θ1=arc tan(|y
m-y1|，|x
m-x1|)计算θ1的角度；以第二关节与第一关节的平行方向为始边；第二关节上摆动为正方向；θ1为第三关节末端移动到第一待移动坐标时，第二关节与第一关节之间的角度。
18.进一步的，若存在两个交点，步骤（5）中分别计算不同交点对应θ1的角度数值；将角度数值小的θ1对应的交点作为第二待移动坐标。
19.以上方法，通过计算第二关节与第一关节之间的角度θ1；通过θ1的大小可以判断出第二关节与水平面之间的距离；θ1角度小时，机械腿能移动的距离更远；这样能增大机械腿的移动距离。
附图说明
20.图1为本发明的流程图。
21.图2为实现本发明的机器人的示意图。
22.图3为实现本发明的机器人中左侧机械腿、右侧机械腿与底盘连接的示意图。
23.图4为本发明中左侧机械腿移动到第一待移动坐标的示意图。
24.图5为本发明中右侧机械腿移动到第一待移动坐标的示意图。
具体实施方式
25.下面结合附图和具体实施方式对本发明做进一步详细说明。
26.如图1-5所示；一种机器人腿部支撑算法，通过机器人实现，机器人为六足机器人；机器人包括底盘1和两条以上的机械腿2；在本实施中机械腿2设有六条；其中三条机械腿2设置在底盘1的一侧；另外三条机械2腿设置在底盘1的另一侧；位于底盘1两侧的机械腿2对称设置。
27.所述机械腿2包括第一关节21、第二关节22和第三关节23；第一关节21与底盘1连接，第二关节22连接有第二驱动装置（图中未示出），第二关节22通过第二驱动装置与第一关节21连接；所述第三关节23连接有第三驱动装置（图中未示出），第三关节23通过第三驱动装置与第二关节22连接。第一关节21带动第二关节22和第三关节23同步活动，第二关节22带动第三关节23同步活动，第三关节23相对第二关节22独立活动。所述第二驱动装置和第三驱动装置为舵机。
28.机器人腿部支撑算法，包括以下步骤：步骤（1）.预存第二关节摆动角度与第二驱动装置的编码器数据的对应信息；预存第三关节摆动角度与第三驱动装置的编码器数据的对应信息；以第一关节和第二关节的铰接处为原点建立第一坐标系；预设第一关节和第二关节铰接处与第二关节和第三关节铰接处之间的距离l1；预设第二关节和第三关节铰接处与第三关节末端之间的距离l2。
29.步骤（2）.输入第一待移动坐标；第一待移动坐标为第三关节末端的下一移动目标点。
30.步骤（3）.以原点（x1，y1）为圆心，l1为半径，得到第一方程(x-x1)2 (x-y1)2=l
12
；第一方程为第一关节的活动范围；以第一待移动坐标（x2，y2）为圆心，l2为半径，得到第二方程(x-x2)2 (y-y2)2=l
22
；第二方程为第三关节活动范围。
31.步骤（4）.判断第一关节的活动范围与第三关节的活动范围是否存在交点，若否，则判断第三关节末端移动到第一待移动坐标时，机械腿无法支撑机器人；然后进行步骤步骤（2）重新确定第一待移动坐标；若是，则判断第三关节末端移动到第一待移动坐标时，机械腿能够支撑机器人，然后进行步骤（5）。
32.步骤（5）.将交点确定为第二待移动坐标；第二待移动坐标为第三关节末端移动到第一待移动坐标时，第二关节和第三关节铰接处所在坐标；通过第二待移动坐标（xm，ym）和公式θ1=arc tan(|y
m-y1|，|x
m-x1|)计算θ1的角度；以第二关节与第一关节的平行方向为始边；第二关节上摆动为正方向；θ1为第三关节末端移动到第一待移动坐标时，第二关节与第一关节之间的角度。
33.步骤（5.1）.沿机器人前进方向判断当前机械腿属于左侧机械腿还是右侧机械腿；
若为左侧机械腿，则进行步骤（5.2）；若为右侧机械腿，则进行步骤（5.3）。
34.步骤（5.2）.判断交点所在象限；若为第一象限或第四象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿无法支撑机器人；然后进行步骤（2）重新确定第一待移动坐标；若交点在第二象限或第三象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿能够支撑机器人。
35.步骤（5.3）.判断交点所在象限；若为第二象限或第三象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿无法支撑机器人，然后进行步骤（2）重新确定第一待移动坐标；若为第一象限或第四象限，则第二关节和第三关节铰接处移动到第二待移动坐标时，机械腿能够支撑机器人。
36.在本实施中，还包括；若步骤（5.2）中，机械腿能够支撑机器人，则进行步骤（6）；若步骤（5.3）中，机械腿能够支撑机器人，则进行步骤（6）。（6）.判断交点所在象限，若交点在第一象限或第二象限，则进行步骤（7）；若交点在第三象限或第四象限，则进行步骤（8）。
37.步骤（7）.通过公式θ2=180-arc tan(|y
m-y1|，|x
m-x1|) θ1计算θ2的角度；在本实施中，以第三关节与第二关节的平行方向为始边；第三关节向下摆动为正方向；θ2为第三关节末端移动到第一待移动坐标时，第三关节与第二关节之间的角度；然后进行步骤（9）。
38.步骤（8）.通过公式θ2=180-arc tan(|y
m-y1|，|x
m-x1|)-θ1计算θ2的角度；θ2为第三关节末端移动到第一待移动坐标时，第三关节与第二关节之间的角度；然后进行步骤（9）。
39.步骤（9）. 计算第二关节与第一关节之间的当前角度与θ1之间的差值；第二驱动装置驱动第二关节摆动，使第二关节与第一关节之间的角度为θ1；计算第三关节与第二关节之间的当前角度与θ2之间的差值；第三驱动装置驱动第三关节摆动；使第三关节与第二关节之间的角度为θ2；第三关节末端移动到第一待移动坐标。
40.以上方法，以第一关节和第二关节的铰接处为圆心，第一关节和第二关节铰接处与第二关节和第三关节铰接处之间的距离为半径、以第三关节末端为圆心，第二关节和第三关节铰接处与第三关节末端之间的距离为半径分别建立方程；通过判断两个方程之间是否有交点从而判断第三关节能否对第二关节进行支撑；若两个方程之间没有交点，则表示第三关节末端移动到第一待移动坐标时，第三关节不能对第二关节进行支撑，进而机械腿不能对机器人进行支撑；若两个方程之间具有一个以上的交点，则表示第三关节末端移动到第一待移动坐标时，第三关节能对第二关节进行支撑，进而机械腿能对机器人进行支撑。
41.同时以第一关节和第二关节的铰接处为原点建立第一坐标系；沿机器人前进方向区分左侧机械腿和右侧机械腿；用第二象限和第三象限限制左侧机械腿的第二待移动坐标位置；用第一象限和第四象限限制右侧机械腿的第二待移动坐标位置；这样避免左侧机械腿向右摆动较大幅度导致不能对机器人进行支撑；也避免右侧机械腿向左摆动较大幅度导致不能对机器人进行支撑；通过左侧机械腿的第二待移动坐标、右侧机械腿的第二待移动坐标判断移动到第一待移动坐标时能否对机器人进行支撑。
42.通过计算出第二关节与第一关节之间的角度θ1、第三关节与第二关节之间的角度θ2；然后控制第二关节和第三关节摆动。通过预存编码器数据的对应信息；这样通过获取第二关节与第一关节之间的当前角度、第三关节与第二关节之间的当前角度；然后第二驱动电机驱动第二关节转动对应角度；第三驱动电机驱动第三关节转动对应角度；便于控制。这样实现机器人的横向移动。
43.上述方法中，若存在两个交点，步骤（5）-步骤（9）中分别计算不同交点对应θ1的角度数值；将角度数值小的θ1对应的交点作为第二待移动坐标。这样移动后的机械腿对机器人的支撑稳定性好。