CN103576688B - 一种机器人先逆时针再顺时针运动控制方法 - Google Patents

Abstract

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

Description

一种机器人先逆时针再顺时针运动控制方法

技术领域

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

背景技术

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

具体实施方式

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

图1是本发明机器人先逆时针再顺时针运动控制方法，如图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)；

该步骤具体为：先将初始方向单位向量Ps逆时针旋转90度，然后与转弯半径R相乘，再将相乘结果与初始点S坐标相加即得第一圆心Os的坐标。

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

该步骤具体为：先将目标方向单位向量Pg顺时针旋转90度，然后与转弯半径R相乘，再将相乘结果与目标点G坐标相加即得第二圆心Og的坐标。

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

该步骤具体为：用第二圆心Og的坐标减去第一圆心Os的坐标，再将相减结果除以第一圆心Os和第二圆心Og两点间的长度值即得第一单位向量Q。

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

该步骤具体为：用2乘以R除以第一圆心Os和第二圆心Og两点间的长度值，再将计算结果进行反正弦即得夹角γ。

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

该步骤具体为：先将第一单位向量Q顺时针旋转(90-γ)度，然后与转弯半径R相乘，再将相乘结果与第一圆心Os的坐标相加即得第一转换点Ws的坐标。

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

该步骤具体为：先将第一单位向量Q逆时针旋转(90+γ)度，然后与转弯半径R相乘，再将相乘结果与第二圆心Og坐标相加即得第二转换点Wg的坐标。

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

该步骤具体为：用第二转换点Wg的坐标减去第一转换点Ws的坐标，再将相减结果除以第一转换点Ws和第二转换点Wg两点间的长度值即得到第二单位向量W。

步骤S9：基于第一圆心Os、初始点S、第一转换点Ws计算得到机器人从初始点S逆时针绕第一圆心Os运动到第一转换点Ws转过的第一转角α_s；

所述步骤S9进一步包括以下步骤：

步骤S91：计算第一圆心Os指向初始点S的第一向量MS：由初始点S的坐标减第一圆心Os的坐标可得第一向量MS；

步骤S92：计算第一圆心Os指向第一转换点Ws的第二向量NS：由第一转换点Ws的坐标减第一圆心Os的坐标可得第二向量NS；

步骤S93：用反余弦求取第一向量MS和第二向量NS之间的夹角α；

步骤S94：判断从初始点S逆时针绕第一圆心Os到达第一转换点Ws转过的第一转角α_s是否大于180度，如果α_s小于等于180度，则令α_s等于α，否则令α_s等于2π-α；

所述步骤S94具体为：

当p_ys＜0且时，α_s＝2π-α；当p_ys＜0且 $y_{\mathrm{ws}}<y_s-\frac{p_{\mathrm{xs}}}{p_{\mathrm{ys}}}(x_{\mathrm{ws}}-x_s)$ 时，α_s＝α；

当p_ys＞0且时，α_s＝2π-α；当p_ys＞0且 $y_{\mathrm{ws}}>y_s-\frac{p_{\mathrm{xs}}}{p_{\mathrm{ys}}}(x_{\mathrm{ws}}-x_s)$ 时，α_s＝α；

当p_ys＝0且x_ws≤0时，α_s＝2π-α；当p_ys＝0且x_ws＞0时，α_s＝α。

步骤S10：基于第二圆心Og、目标点G、第二转换点Wg计算得到机器人从第二转换点Wg沿顺时针绕第二圆心Og运动到目标点G转过的第二转角β_g；

所述步骤S10进一步包括以下步骤：

步骤S101：计算第二圆心Og指向目标点G的第三向量MG：由目标点G的坐标减第二圆心Og的坐标即可得到第三向量MG；

步骤S102：计算第二圆心Og指向第二转换点Wg的第四向量NG：由第二转换点Wg的坐标减第二圆心Og的坐标即可得到第四向量NG；

步骤S103：用反余弦求取第三向量MG和第四向量NG之间的夹角β；

步骤S104：判断从第二转换点Wg顺时针绕第二圆心Og运动到目标点G转过的角度β_g是否大于180度，如果β_g小于等于180度，则令β_g等于β，否则令β_g等于2π-β。

所述步骤S104具体为：

当p_yg＜0且时，β_g＝β；当p_yg＜0且 $y_{\mathrm{wg}}<y_g-\frac{p_{\mathrm{xg}}}{p_{\mathrm{yg}}}(x_{\mathrm{wg}}-x_g)$ 时，β_g＝2π-β；

当p_yg＞0且时，β_g＝β；当p_yg＞0且 $y_{\mathrm{wg}}>y_g-\frac{p_{\mathrm{xg}}}{p_{\mathrm{yg}}}(x_{\mathrm{wg}}-x_g)$ 时，β_g＝2π-β；

当p_yg＝0且x_wg≤0时，β_g＝β；当p_yg＝0且x_wg＞0时，β_g＝2π-β。

步骤S11：基于所述步骤S2-S10计算得到的运动路径参数，对于机器人的先逆时针后顺时针运动进行控制，其中，所述先逆时针后顺时针运动路径参数包括：第一圆心Os坐标(x_os,y_os)，第一转换点Ws坐标(x_ws,y_ws)，从初始点S逆时针绕第一圆心Os运动到第一转换点Ws转过的第一转角α_s，第二单位向量W(w_x，w_y)，第二圆心Og坐标(x_og,y_og)，第二转换点Wg坐标(x_wg,y_wg)，从第二转换点Wg沿顺时针绕第二圆心Og运动到目标点G转过的第二转角β_g。所述机器人先逆时针后顺时针的运动路径具体为：由初始点S以转弯半径R绕第一圆心Os逆时针运动转过第一转角α_s到达第一转换点Ws，由第一转换点Ws沿第二单位向量W方向直线运动到第二转换点Wg，由第二转换点Wg以转弯半径R绕第二圆心Og顺时针运动转过第二转角β_g即到达目标点G。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

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之间的夹角γ，其中，用2乘以R除以第一圆心Os和第二圆心Og两点间的长度值，再将计算结果进行反正弦即得夹角γ；

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

其中，所述步骤S9进一步包括以下步骤：

步骤S91：计算第一圆心Os指向初始点S的第一向量MS；

步骤S92：计算第一圆心Os指向第一转换点Ws的第二向量NS；

步骤S93：用反余弦求取第一向量MS和第二向量NS之间的夹角α；

步骤S94：判断从初始点S逆时针绕第一圆心Os到达第一转换点Ws转过的第一转角α_s是否大于180度，如果α_s小于等于180度，则令α_s等于α，否则令α_s等于2π-α；

所述步骤S10进一步包括以下步骤：

步骤S101：计算第二圆心Og指向目标点G的第三向量MG；

步骤S102：计算第二圆心Og指向第二转换点Wg的第四向量NG；

步骤S103：用反余弦求取第三向量MG和第四向量NG之间的夹角β；

步骤S104：判断从第二转换点Wg顺时针绕第二圆心Og运动到目标点G转过的角度β_g是否大于180度，如果β_g小于等于180度，则令β_g等于β，否则令β_g等于2π-β。

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所述的方法，其特征在于，所述步骤S6具体为：先将第一单位向量Q顺时针旋转(90-γ)度，然后与转弯半径R相乘，再将相乘结果与第一圆心Os的坐标相加即得第一转换点Ws的坐标。

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

7.根据权利要求1所述的方法，其特征在于，所述步骤S8具体为：用第二转换点Wg的坐标减去第一转换点Ws的坐标，再将相减结果除以第一转换点Ws和第二转换点Wg两点间的长度值即得到第二单位向量W。