[发明专利]机器人圆弧运动控制方法和系统

权利要求书

1.一种机器人圆弧运动控制方法，其特征在于，包括：

接收控制端传送的圆弧运动指令；所述圆弧运动指令包括圆弧运动的目标中间点位置、终点位置以及运动需要的时间；

根据所述圆弧运动指令异步触发圆弧运动执行函数，根据所述圆弧运动执行函数并通过第一接口调用圆弧运动规划函数；所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口；

执行所述圆弧运动规划函数，读取机械臂末端的当前状态参数；根据所述当前状态参数以及所述目标中间点位置、终点位置计算圆弧运动的圆心、圆弧半径、圆心角；根据目标中间点位置、终点位置计算圆弧所在平面的法矢量；根据圆心、圆弧半径、圆心角及所述当前状态参数得到圆弧的轨迹方程，根据所述法矢量和法矢量确定圆弧运动轨迹；其中，所述当前状态参数包括当前位置、当前速度和当前加速度；

根据所述运动需要的时间计算机械臂末端在所述圆弧运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度；通过第二接口将所述目标角度、目标角速度和目标角加速度发送给设备通信软件，由设备通信软件转发至控制主站；所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口。

2.根据权利要求1所述的机器人圆弧运动控制方法，其特征在于，所述计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括：

构建所述直线运动轨迹的方程；

根据方程计算在所述圆弧运动轨迹运动后设定时刻t时的轨迹位置s_t，轨迹速度轨迹加速度

计算机械臂末端在所述设定时刻t时的目标位置、目标速度和目标加速度；

通过逆运动学将所述机械臂末端的目标位置、目标速度和目标加速度转换为机械臂每个关节的目标角度、目标角速度和目标角加速度。

3.根据权利要求2所述的机器人圆弧运动控制方法，其特征在于，所述机械臂末端当前位置为P₀＝[x₀,y₀,z₀]^T，当前速度为和加速度以及目标中间点位置为P₁＝[x₁,y₁,z₁]^T，终点位置为P₂＝[x₂,y₂,z₂]^T；

所述圆心计算公式为：

其中，

A₁＝y₀z₁-y₀z₂-z₀y₁+z₀y₂+y₁z₂-y₂z₁

B₁＝-x₀z₁+x₀z₂+z₀x₁-z₀x₂-x₁z₂+x₂z₁

C₁＝x₀y₁-x₀y₂-y₀x₁+y₀x₂+x₁y₂-x₂y₁

D₁＝-x₀y₁z₂+x₀y₂z₁+x₁y₀z₂-x₂y₀z₁-x₁y₂z₀+x₂y₁z₀

A₂＝2(x₁-x₀)

B₂＝2(y₁-y₀)

C₂＝2(z₁-z₀)

A₃＝2(x₂-x₀)

B₃＝2(y₂-y₀)

C₃＝2(z₂-z₀)

圆弧半径计算公式为：

圆弧所在平面的法矢量n计算公式为：

其中，

圆弧P₀P₁P₂的圆心角θ_c计算公式为：

当0＜θ_c＜π时，

当2π＞θ_c＞π时，

圆弧轨迹计算公式为：

s₁＝θ_cR,

式中，s₀为当前轨迹位置，为当前轨迹速度，为当前轨迹加速度；s₁为目标轨迹位置，为目标轨迹速度，为目标轨迹加速度，θ_c为圆心角，R为圆弧半径。