[发明专利]一种机器人避障控制方法、装置、终端设备及存储介质

权利要求书

1.一种机器人避障控制方法，其特征在于，包括：

根据第k时刻的末端位置和关节角度及第k时刻的关节角速度，获取第k+1时刻至第k+n时刻的末端位置和关节角度；

基于碰撞检测库，根据第k时刻至第k+n-1时刻的关节角速度，构建避障函数；

根据第k时刻至第k+n时刻的末端位置和关节角度、第k时刻至第k+n时刻的末端位置和关节角度的期望值及第k时刻至第k+n-1时刻的关节角速度和所述避障函数，构建目标函数；

基于非线性模型预测控制方法，对所述目标函数进行极小值求解，获取满足约束条件的第k+1时刻的关节角速度；

根据所述第k+1时刻的关节角速度，对第k+1时刻机器人的末端的速度进行控制；

其中，所述约束条件基于所述关节角速度的取值范围构建，k和n为正整数。

2.如权利要求1所述的机器人避障控制方法，其特征在于，所述根据第k时刻的末端位置和关节角度及第k时刻的关节角速度，获取第k+1时刻至第k+n时刻的末端位置和关节角度，包括：

根据第k+i时刻的关节角度和第k+i时刻的关节角速度，获取第k+i时刻机器人的末端的速度；

根据所述第k+i时刻机器人的末端的速度、第k+i时刻的末端位置和关节角度及第k+i+1时刻与第k+i时刻之间的间隔时间，获取第k+i+1时刻的末端位置和关节角度；

其中，i＝0,1,2,…,n-1。

3.如权利要求2所述的机器人避障控制方法，其特征在于，所述第k+i时刻机器人的末端的速度的计算公式为：

所述第k+i+1时刻的末端位置和关节角度的计算公式为：

其中，表示所述第k+i时刻机器人的末端的速度，θ(k+i)表示所述第k+i时刻的关节角度，J()表示雅可比矩阵，表示所述第k+i时刻的关节角速度，X(k+i+1)表示所述第k+i+1时刻的末端位置和关节角度，X(k+i)表示所述第k+i时刻的末端位置和关节角度，T_k+i+1表示所述第k+i+1时刻对应的时间点，T_k+i表示所述第k+i时刻对应的时间点。

4.如权利要求1所述的机器人避障控制方法，其特征在于，所述基于碰撞检测库，根据第k时刻至第k+n-1时刻的关节角速度，构建避障函数，包括：

基于碰撞检测库，根据第k时刻至第k+n-1时刻的障碍物距离权重、所述机器人的初始关节角度、第k时刻至第k+n-1时刻的关节角速度及第k时刻至第k+n时刻中每组相邻的两个时刻之间的时间间隔，构建避障函数。

5.如权利要求4所述的机器人避障控制方法，其特征在于，所述避障函数的表达式为：

其中，f_obj表示所述避障函数，δ_k+i表示第k+i时刻的障碍物距离权重，q_init表示所述初始关节角度，g()表示最短距离检测函数，u(k+i)表示第k+i时刻的关节角速度，T_k+i表示第k+i时刻对应的时间点，T_k+i+1表示第k+i+1时刻对应的时间点。

6.如权利要求1至5任一项所述的机器人避障控制方法，其特征在于，所述目标函数的表达式为：

其中，min()表示求最小值函数，∑()表示求和函数，|| ||₂表示二范数，y_d(k+i)表示所述第k+i时刻的末端位置和关节角度的期望值，X(k+i)表示所述第k+i时刻的末端位置和关节角度，u(k+i)表示所述第k+i时刻的关节角速度，f_obj表示所述避障函数，α表示轨迹跟踪权重，β表示输入平滑权重，χ表示避障权重。