CN103592945B - 一种机器人顺时针运动控制方法 - Google Patents

Abstract

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

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：基于所述单位向量Q、转弯半径R和第一圆心Os计算得到机器人从绕第一圆心Os顺时针运动转为沿单位向量Q方向作直线运动的第一转换点Ws的坐标(x_ws，y_ws)；

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

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

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

步骤S9：基于所述步骤S2-S8计算得到的运动路径参数，对于机器人的顺时针运动进行控制，其中，所述机器人的顺时针运动路径具体为：由初始点S以转弯半径R绕第一圆心Os顺时针运动转过第一转角α_s到达第一转换点Ws，由第一转换点Ws沿单位向量Q方向直线运动到第二转换点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：基于所述单位向量Q、转弯半径R和第一圆心Os计算得到机器人从绕第一圆心Os顺时针运动转为沿单位向量Q方向作直线运动的第一转换点Ws的坐标(x_ws，y_ws)；

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

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

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

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

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

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

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

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

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

所述步骤S74具体为：

当pys<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=α。

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

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

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

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

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

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

所述步骤S84具体为：

当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π-β。

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

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

Claims (8)

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

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

步骤S7：基于第一圆心Os、初始点S、第一转换点Ws计算得到机器人从初始点S顺时针绕第一圆心Os运动到第一转换点Ws转过的第一转角α_s，所述步骤S7进一步包括以下步骤：

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

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

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

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

步骤S8：基于第二圆心Og、目标点G、第二转换点Wg计算得到机器人从第二转换点Wg沿顺时针绕第二圆心Og运动到目标点G转过的第二转角β_g，所述步骤S8进一步包括以下步骤：

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

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

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

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

步骤S9：基于所述步骤S2-S8计算得到的运动路径参数，对于机器人的顺时针运动进行控制，其中，所述机器人的顺时针运动路径具体为：由初始点S以转弯半径R绕第一圆心Os顺时针运动转过第一转角α_s到达第一转换点Ws，由第一转换点Ws沿单位向量Q方向直线运动到第二转换点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具体为：先将单位向量Q逆时针旋转90度，然后与转弯半径R相乘，再将相乘结果与第一圆心Os坐标相加即得第一转换点Ws的坐标。

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

7.根据权利要求1所述的方法，其特征在于，所述步骤S74具体为：

当p_ys＜0且时，α_s＝2π-α；当p_ys＜0且时，α_s＝α；

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

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

8.根据权利要求1所述的方法，其特征在于，所述步骤S84具体为：

当p_yg＜0且时，β_g＝β；当p_yg＜0且时，β_g＝2π-β；

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

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