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

权利要求书

1.一种机器人先顺时针再逆时针运动控制方法，其特征在于，该方法包括以下步骤：

步骤S1：设定并输入多个机器人运动参数，所述运动参数至少包括：机器人运动初始点S坐标（x_s，y_s），机器人在初始点S处的初始方向单位向量Ps（p_xs，p_ys），机器人运动目标点G坐标（x_g，y_g），机器人在目标点G处的目标方向单位向量Pg（p_xg，p_yg），以及机器人容许的转弯半径R；

步骤S2：基于初始点S、初始方向单位向量Ps和转弯半径R计算得到机器人从初始点S沿初始方向单位向量Ps以转弯半径R做顺时针绕第一圆心Os运动的第一圆心Os的坐标(x_os,y_os)；

步骤S3：基于目标点G、目标方向单位向量Pg和转弯半径R计算得到机器人以转弯半径R沿逆时针绕第二圆心Og运动以到达目标点G和目标方向单位向量Pg的第二圆心Og的坐标(x_og,y_og)；

步骤S4：基于第一圆心Os和第二圆心Og的坐标计算得到由第一圆心Os指向第二圆心Og的第一单位向量Q(q_x,q_y)；

步骤S5：基于第一圆心Os、第二圆心Og和转弯半径R计算得到第一单位向量Q与第二单位向量W之间的夹角γ；

步骤S6：基于所述第一圆心Os、转弯半径R和第一单位向量Q计算得到机器人从绕第一圆心Os顺时针运动转为沿第二单位向量W方向作直线运动的第一转换点Ws的坐标(x_ws,y_ws)；

步骤S7：基于所述第二圆心Og、转弯半径R和第一单位向量Q计算得到机器人从沿第二单位向量W方向直线运动转为绕第二圆心Og逆时针运动的第二转换点Wg的坐标(x_wg,y_wg)；

步骤S8：计算得到由第一转换点Ws指向第二转换点Wg的第二单位向量W(w_x,w_y)；

步骤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。

2.根据权利要求1所述的方法，其特征在于，所述步骤S2具体为：先将初始方向单位向量Ps顺时针旋转90度，然后与转弯半径R相乘，再将相乘结果与初始点S坐标相加即得第一圆心Os的坐标。

3.根据权利要求1所述的方法，其特征在于，所述步骤S3具体为：先将目标方向单位向量Pg逆时针旋转90度，然后与转弯半径R相乘，再将相乘结果与目标点G的坐标相加即得第二圆心Og的坐标。

4.根据权利要求1所述的方法，其特征在于，所述步骤S4具体为：用第二圆心Og的坐标减去第一圆心Os的坐标，再将相减结果除以第一圆心Os和第二圆心Og两点间的长度值即得第一单位向量Q。

5.根据权利要求1所述的方法，其特征在于，所述步骤S5具体为：用2乘以R除以第一圆心Os和第二圆心Og两点间的长度值，再将计算结果进行反正弦即得夹角γ。

6.根据权利要求1所述的方法，其特征在于，所述步骤S6具体为：先将第一单位向量Q逆时针旋转(90-γ)度，然后与转弯半径R相乘，再将相乘结果与第一圆心Os的坐标相加即得第一转换点的Ws坐标。