[发明专利]一种冗余机器人轨迹规划方法

权利要求书

1.一种冗余机器人轨迹规划方法，其特征在于，其步骤为：

步骤1：建立冗余机器人的运动学和动力学模型，获得关节角速度与末端执行器位姿间的数学关系；

步骤2：采用闭环伪逆法求解关节角，进行轨迹规划：

已知机器人末端执行器的参考位姿与实际位姿，在一次迭代中，通过雅克比矩阵的伪逆求解关节角速度误差，对关节角速度误差积分获得补偿关节角；重复迭代，直到关节角速度误差趋近于0，实现机器人运动学逆解，求得描述运动轨迹的关节角，完成轨迹规划；

在对关节角速度误差积分获得补偿关节角步骤中，构造约束冗余机器人轨迹规划的目标函数，通过萤火虫算法求的目标函数的最小值来修正在积分时产生的累积误差即积分误差；

改进萤火虫算法包括个体初始化、决策域更新、位置更新及荧光素更新四个步骤，其中位置更新步骤采用广义反向学习机制来进行位置更新。

2.根据权利要求1所述的冗余机器人轨迹规划方法，其特征在于，步骤1具体为：

步骤1.1)：建立冗余机器人的正向运动学方程：

x＝f(q) (1)

式中，n为冗余机器人的自由度数量，q表示冗余机器人自由度在关节空间的关节角度，q＝[q₁,q₂,…,q_n]^T；x表示冗余机器人末端在操作空间的关节位姿，x＝[x₁,x₂,…,x_m]^T，其中，m为操作空间变量的数目，m<n；

步骤1.2)：将正向运动学方程微分，生成对应的微分运动学方程，表示为：

式中，为冗余机器人末端在操作空间的速度，即操作速度，为冗余机器人自由度在关节空间的角速度，即关节角速度，J(q)为冗余机器人的雅克比矩阵，可以用给定的路径点x(t)来计算出关节角向量q(t)；

步骤1.3)：根据微分运动学方程，求出关节角速度得到关节角速度随时间变化的轨迹；

其中，为雅克比矩阵的广义逆，其解可使关节角速度的模取得局部最小值；

步骤1.4)：对于n自由度冗余机器人，其动力学方程可描述为：

其中，T∈R^n×1为力矩向量，M(q)∈R^n×n为惯性矩阵，为含科氏力与离心力的矩阵，G(q)∈R^n×1为重力矩阵；q表示关节角的角度，表示关节角的角速度，表示关节角的角加速度，对求导可得角加速度

3.根据权利要求1所述的冗余机器人轨迹规划方法，其特征在于，所述步骤2，在闭环伪逆法中，关节角可通过下式积分计算获得：

其中，x_ref为操作空间中末端执行器的参考位姿，x为操作空间中末端执行器的实际位姿。