[发明专利]一种基于导航路径约束的柔性机器人路径规划方法

权利要求书

1.一种基于导航路径约束的柔性机器人路径规划方法，其特征在于，包括以下步骤：

步骤一、对机器人的参数进行初始化设定，包括：关节段数、关节长度、弯曲单元弯曲角、旋转角、起始点和目标点；

步骤二、将机器人的起始点与目标点之间的理想路径曲线均匀离散，并控制机器人从初始零位开始步进；

步骤三、根据理想路径曲线上的各离散点以及离散点的主法线、副法线和切线构建机器人实际步进点在基坐标系的位姿矩阵；

步骤四、基于距离函数求解机器人末端弯曲单元的关节参数；

步骤五、基于逆向递推法求解机器人其他弯曲单元的关节参数；

步骤六、根据步骤三-五求解的机器人各弯曲单元的关节参数，控制机器人前进一步长；

步骤七、判断机器人是否达到目标位置，未到达目标点时，重新求解再前进一步长时各弯曲单元的两个关节变量值，若到达目标点，结束机器人的路径规划。

2.根据权利要求1所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，步骤三具体包括：

设理想路径曲线上距离机器人最近的离散点设为M，s为理想路径的起点到M点的弧长，则曲线在点M处的切向量为：

其中，t为切向量，曲线上点M处的径矢为

如果则M点处的切向量和位置向量唯一确定一个密切平面，密切平面方程为：

其中，为副法线方程；副法线标准方程表示为：

在已知切向量和副法线向量的情况下，M点处主法线通过右手定则求解为：

其中，为主法线向量；

机器人沿着理想路径曲线前进弧长s₀时，机器人实际步进点相对于基坐标系的齐次变换矩阵为：

其中，M_m为M点在基坐标系下的位置坐标；

以每个点的切线、主法线、副法线为点的坐标系的x、y、z轴，求解出点相对基坐标系的齐次变换矩阵，进而获得机器人在所有理想路径点的位姿矩阵。

3.根据权利要求1所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，步骤四具体包括：利用距离函数、末端弯曲单元的齐次变换矩阵和机器人实际步进点位置坐标计算得到弯曲角和旋转角。

4.根据权利要求3所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，距离函数的公式为：

d＝min{(x_s-x_l)²+(y_s-y_l)²+(z_s-z_l)²} (7)

式中，(x_s,y_s,z_s)为机器人实际步进点位置，通过正运动学模型获得，(x_l,y_l,z_l)为路径上理想路径点的位置。

5.根据权利要求3所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，末端弯曲单元的齐次变换矩阵的公式为：

其中，为第i段弯曲单元相对第1段的初始坐标系的齐次变换矩阵。

6.根据权利要求3所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，弯曲角和旋转角的公式为：

其中，(x_i,y_i,z_i)为机器人实际步进点位置坐标。

7.根据权利要求1所述的基于导航路径约束的柔性机器人路径规划方法，其特征在于，步骤五具体包括：

将机器人未介入弯曲单元与推送单元等效为机器人的平移关节，未介入弯曲单元的关节参数始终为0；采用逆向递推法，将机器人实际步进点矩阵乘以第n段弯曲单元相对前n-1段弯曲单元的齐次变换矩阵的逆矩阵获得n-1段其他弯曲单元变换矩阵及实际步进点位置。