[发明专利]并联双足机器人关节实时控制方法和装置

权利要求书

1.一种并联双足机器人关节实时控制方法，其特征在于，包括以下步骤：

步骤一：根据双足机器人并联关节的几何关系，列出并联关节角度与电机角度的运动方程；

所述双足机器人并联关节包括联合带动所述双足机器人的足部的横滚角和俯仰角转动的第一运动链M₁N₁P₁O和第二运动链M₂N₂P₂O，所述第一运动链M₁N₁P₁O包括第一水平杆和第一竖直杆，所述第二运动链M₂N₂P₂O包括第二水平杆和第二竖直杆；其中点N₁为第一水平杆和第一竖直杆的铰接点，点N₂为第二水平杆和第二竖直杆的铰接点，点P₁为第一竖直杆与脚板的铰接点，点P₂为第二竖直杆与脚板的铰接点，O点为脚板与十字铰接轴的中心点，以O点为圆心建立X、Y、Z坐标系，并给出并联关节角度与电机角度的运动方程：

，

；

其中和为电机角度，即电机带动第一水平杆和第二水平杆的转动角度，当第一水平杆和第二水平杆水平时和的角度分别为0，当第一水平杆和第二水平杆向上转动时和的角度分别为正值，反之则为负值；和是并联关节角度，分别为所述双足机器人的足部的俯仰角和横滚角，当脚板水平时和都为90°，当脚板绕x轴正方向转动时的角度变小，反之则变大，当脚板绕y轴正方向转动时的角度变小，反之则变大；，，，，，，P点为线P₁P₂的中点，R点为点M₁在Z轴上的投影，Q点为点M₂在Z轴上的投影，OM₁M₂组成的平面始终在XOZ平面内，点Z’为过通过P点且平行于Z轴的辅助线正半轴的任意一点；

步骤二：根据并联关节角度与电机角度的运动方程，获得电机角度与并联关节角度的数值联系；

步骤三：读取初始并联关节角度，由电机角度与并联关节角度的数值联系计算得到初始并联关节角度相对应的初始电机角度，将并联关节角度与电机角度的运动方程转化为牛顿-拉夫逊迭代的标准形式，获得牛顿-拉夫迭代的计算式；

步骤四：以初始并联关节角度作为牛顿-拉夫逊迭代的计算式的初值进行求解；当迭代计算过程达到结束条件，则迭代计算结束，将求得的迭代求解值作为并联双足机器人相对于初始并联关节角度的下一并联关节角度；当迭代计算过程中出现异常处理机制，则终止迭代计算过程，将初始并联关节角度作为并联双足机器人的下一并联关节角度；

步骤五：以步骤四得到的下一并联关节角度作为牛顿-拉夫逊迭代的计算式的初值，重复步骤四，从而实现机器人的闭环控制。

2.根据权利要求1所述的一种并联双足机器人关节实时控制方法，其特征在于，所述步骤二具体为：根据并联关节角度与电机角度的运动方程获得电机角度与并联关节角度的数值联系，由并联关节角度计算电机角度，得到电机角度和的解析式：

，

其中，A，B，D，E，F，G，H，I的中间变量如下：

。

3.根据权利要求1所述的一种并联双足机器人关节实时控制方法，其特征在于，所述牛顿-拉夫逊迭代的标准形式为：

；

其中，

。

4.根据权利要求3所述的一种并联双足机器人关节实时控制方法，其特征在于，所述牛顿-拉夫逊迭代的计算式为：

，

其中和为牛顿-拉夫逊迭代的计算式的初值，和为迭代求解。

5.根据权利要求4所述的一种并联双足机器人关节实时控制方法，其特征在于，所述结束条件为：

。

6.根据权利要求5所述的一种并联双足机器人关节实时控制方法，其特征在于，所述异常处理机制包括迭代计算时间超过设定的最大值或迭代计算过程中出现奇异值。