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

说明书

技术领域

本发明涉及机器人技术领域，具体是一种机器人先顺时针再逆时针运动控制方法。

背景技术

近年来，勘察、目标获取、搜索与援救、监督、环境监测等方面广泛的应用需求使移动机器人技术得到快速的发展。其中，导航技术是移动机器人研究的核心技术之一，而路径规划是导航的基本环节之一。机器人路径规划的基本思想是依据某种标准（如时间最短、能量最少、路径最短等）寻找一条从起始点到目标点的无碰撞的最优或近似最优的路径。路径规划可分为全局路径规划与局部路径规划，全局路径规划的主要算法有可视图法、栅格解耦法、概率图法、拓扑法和神经网络法；局部路径规划的主要算法有人工势场法、快速随机搜索树（RRT）和模糊逻辑算法等。

发明内容

本发明的目的是提出一种机器人先顺时针再逆时针运动控制方法，以进行机器人CSC路径规划，使得机器人按照规划的路线进行先顺时针再逆时针运动。

本发明提出的一种机器人先顺时针再逆时针运动控制方法包括以下步骤：

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

本发明结合机器人学，提出了一种先顺时针再逆时针运动控制方法，其具体通过坐标旋转变换的方法来实现，简单而且有效。

附图说明

图1是本发明机器人先顺时针再逆时针运动控制方法的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。