[发明专利]一种机器人先顺时针再逆时针运动控制方法

摘要

本发明公开了一种机器人先顺时针再逆时针运动控制方法，该方法包括以下步骤：设定并输入多个机器人运动参数；计算机器人运动的第一圆心的坐标；计算机器人运动的第二圆心的坐标；计算第一圆心指向第二圆心的第一单位向量；计算第一单位向量与第二单位向量之间的夹角；计算机器人运动的第一转换点的坐标；计算机器人运动的第二转换点的坐标；计算由第一转换点指向第二转换点的第二单位向量；计算机器人运动的第一转角；计算机器人运动的第二转角；基于上述运动路径参数，对于机器人的先顺时针后逆时针运动进行控制。本发明结合机器人学知识，利用坐标旋转变换方法实现了对于机器人先逆时针再顺时针运动的控制，本发明简单而且有效。

主权项

一种机器人先顺时针再逆时针运动控制方法，其特征在于，该方法包括以下步骤：步骤S1：设定并输入多个机器人运动参数，所述运动参数至少包括：机器人运动初始点S坐标（xs，ys），机器人在初始点S处的初始方向单位向量Ps（pxs，pys），机器人运动目标点G坐标（xg，yg），机器人在目标点G处的目标方向单位向量Pg（pxg，pyg），以及机器人容许的转弯半径R；步骤S2：基于初始点S、初始方向单位向量Ps和转弯半径R计算得到机器人从初始点S沿初始方向单位向量Ps以转弯半径R做顺时针绕第一圆心Os运动的第一圆心Os的坐标(xos,yos)；步骤S3：基于目标点G、目标方向单位向量Pg和转弯半径R计算得到机器人以转弯半径R沿逆时针绕第二圆心Og运动以到达目标点G和目标方向单位向量Pg的第二圆心Og的坐标(xog,yog)；步骤S4：基于第一圆心Os和第二圆心Og的坐标计算得到由第一圆心Os指向第二圆心Og的第一单位向量Q(qx,qy)；步骤S5：基于第一圆心Os、第二圆心Og和转弯半径R计算得到第一单位向量Q与第二单位向量W之间的夹角γ；步骤S6：基于所述第一圆心Os、转弯半径R和第一单位向量Q计算得到机器人从绕第一圆心Os顺时针运动转为沿第二单位向量W方向作直线运动的第一转换点Ws的坐标(xws,yws)；步骤S7：基于所述第二圆心Og、转弯半径R和第一单位向量Q计算得到机器人从沿第二单位向量W方向直线运动转为绕第二圆心Og逆时针运动的第二转换点Wg的坐标(xwg,ywg)；步骤S8：计算得到由第一转换点Ws指向第二转换点Wg的第二单位向量W(wx,wy)；步骤S9：基于第一圆心Os、初始点S、第一转换点Ws计算得到机器人从初始点S顺时针绕第一圆心Os运动到第一转换点Ws转过的第一转角αs；步骤S10：基于第二圆心Og、目标点G、第二转换点Wg计算得到机器人从第二转换点Wg沿逆时针绕第二圆心Og运动到目标点G转过的第二转角βg；步骤S11：基于所述步骤S2‑S10计算得到的运动路径参数，对于机器人的先顺时针后逆时针运动进行控制，其中，所述机器人先顺时针后逆时针的运动路径为：由初始点S以半径R绕第一圆心Os顺时针运动转过第一转角αs到达第一转换点Ws，由第一转换点Ws沿第二单位向量W方向直线运动到第二转换点Wg，由第二转换点Wg以转弯半径R绕第二圆心Og逆时针运动转过第二转角βg即到达目标点G。