[发明专利]一种机器人控制方法与装置

摘要

本发明公开了一种机器人控制方法与装置，针对具有全状态约束和系统不确定性的全驱动机器人，提出了一种鲁棒定时轨迹跟踪控制方法，包括采样点与机器人状态期望值设定，机器人位置、方向、前进速度、侧向速度与偏航速度的数据采集，计算转移矩阵，计算位置和方向跟踪误差与速度跟踪误差，施加控制量前向、侧向、偏航速度的控制量。并基于该方法提出了一种机器人控制装置，包括：机器人位置检测单元，机器人方向检测单元，机器人前进与测向速度检测单元，机器人偏航速度检测单元，以及数据处理单元。本发明的优点在于：在不依赖于系统初始条件的情况下，控制器可以在有限时间内实现系统的稳定。

主权项

1.一种机器人控制方法，其特征在于，具体包括以下步骤：S101：设定采样点t为0，设定机器人期望的位置和方向η_d＝[x_d，y_d，ψ_d]^T，设定机器人期望的速度θ_d＝[u_d，v_d，r_d]^T。η＝[x,y，ψ]^T表示位置和方向组合的向量，(x，y)表示位置，x表示东向坐标，y表示北向坐标，ψ表示方向，θ＝[u,v,r]^T表示速度组合的向量，u、v、r分别表示前进速度、侧向速度与偏航速度；下标d表示该下标对应的变量的期望，上标T表示转置；S102：采样点t自增1，采集机器人位置和方向的数据η_m＝[x_m，y_m，ψ_m]^T，η_m中的三个元素分别对应东向坐标、北向坐标与方向的检测值；采集机器人速度的数据θ_m＝[u_m,v_m,r_m]^T，θ_m中的三个元素分别表示前进速度、侧向速度与偏航速度的检测值；下标m表示该下标对应的变量的检测值；S103：计算转移矩阵R(ψ_m)，其中R(ψ)定义如下其中，R(ψ)具有如下性质：R^T(ψ)R(ψ)＝I，I为单位矩阵；S(r)的定义如下：S104：计算位置和方向跟踪误差η_e＝η_m‑η_d与速度跟踪误差θ_e＝θ_m‑θ_d，下标e表示该下标对应的变量的跟踪误差值；S105：施加控制量Υ中元素分别表示前向、侧向、偏航速度的控制量，具体如下：其中，M为对称的惯性矩阵，x₁＝η_e，k₁，k₂皆为3×1控制参数向量，其每个元素都大于0，l₁(x₂)为3×1向量，其中的元素k_1,i,x_2，i分别为k₁，x₂中的元素；l₂(x₂)为3×1向量，其中的元素ρ∈(0，1)，某变量上方一点表示该变量的微分，例如为η_e的微分；「k₂」表示该向量扩展成的对角矩阵，即diag{k_2，1，k_2，2，k_2，3}；f(x₁，x₂)具体如下：其中，C(θ_m)是科里奥利向心矩阵，D(θ_m)为阻尼矩阵；S106：重复步骤S102至S105，实现每个采样点的控制量输出。