基于余弦曲线的机器人移动路径自主规划

李峰，金永喆，朱文慧，李自建

（1.国立群山大学 机械工程学院，韩国 群山 54150；2.郑州经贸学院 智慧制造学院，郑州 451191）

0 引言

余弦曲线是一种三角函数曲线，曲线如湖面水波，连续平滑过渡，且可无限延伸。该曲线应用比较广泛，可用来做信号的模拟、模仿物体的简谐运动等[1]。目前机器人技术发展日新月异，机器人在社会上的应用也越来越广泛。机器人路径自主规划是机器人研究的一个重点，它是保证机器人从所在位置到达目标位置并完成任务所必须解决的问题。

虽然现在的路径规划算法很多，如蚁群算法、神经网络算法、人工势场法、A*算法等，都能够较好地完成机器人的路径规划，但这些算法内部参数繁多，且对不同的问题参数设置较多，改变较大，大多不能作为独立的可重复的子路径单元重复使用。它们的智能程度虽高，但作为可重复执行的子路径单元，就会使计算量增大，计算繁琐且规律性欠佳，不易掌握。而子路径单元如果仅为直线移动，则机器人在规避危险、穿越障碍等特殊情况下困难重重。为此，中国海洋大学的宋大雷等[2]提出了椭圆曲线路径规划算法；华北电力大学计算机科学与技术学院的刘春阳等[3]提出了Bezier曲线模型；李峰等[4]提出了可以用弹道曲线来作为机器人航道曲线；许哲、朱海洋、王庆诚等[5]提出了S型速度曲线路径平滑过渡算法。另外，陈丹[6]、张玲[7]、范程华等[8]、李亮玉等[9]、姚学峰等[10]也分别用B样条曲线、三次样条曲线、NURBS曲线等设计了机器人的路径轨迹算法。以上算法虽可以完成机器人点对点的路径规划，但由于曲线本身的特点，大多不可在保持形状的前提下无限延伸，导致两点距离过大时机器人点对点规划路径被过度放大，机器人从一点到另一点无效路径过长，路径规划效果较差，为此需要寻找一种既可以曲线平滑点对点的子路径规划方法。机器人通过路径规划，既可以到达目标位置，又可以应对突发情况，规避危险，而且路径相对变化较小。

正常情况下，机器人路径单元规划大多为直线，即从A点到B点路径为一条直线段。但在特殊情况下，需要规避风险，穿越障碍物时就不能完成任务了。任意两点之间余弦曲线连接的难点不仅在于余弦曲线的周期性波动和峰值变化，还要保证曲线不同方向的延伸，本文利用MATLAB软件进行参数设定、方程求解、路径规划、曲线绘制，将机器人移动路径设计为余弦曲线，机器人沿着余弦曲线一步一步地移动，从所在位置沿着余弦曲线轨迹到达目标位置，完成预定任务。余弦曲线的连续、平滑且无穷延伸的特性不仅适合于机器人的行走，而且可保证机器人能够从初始位置到达目标位置，完成设定任务。

1 余弦曲线模型

1.1 余弦曲线变换模型

如图1 所示，机器人余弦曲线变换模型中，curve1为初始位置余弦曲线，curve2为余弦曲线平移后的曲线，curve3为curve2 旋转后的曲线。

图1 余弦曲线变换模型

平面内任意两点之间确定余弦曲线的过程较为繁琐，可通过将curve1平移为curve2，然后curve2绕初始点旋转某一角度的方法得到curve3，让curve3通过初始、终点坐标位置，整个过程如图1所示。从图1中可以看出，curve1、curve2、curve3均为余弦曲线，且连续光滑，因只取曲线的一部分，因此未做无限延伸处理。如图2所示，在机器人的余弦曲线路径规划出后，机器人可以从初始位置到达目标位置，一步一步地按照规划路径行进。行进过程中按照规划路径位姿，即可到达目标位置。余弦方程的简洁特点既保证了计算量的降低，又保证了路径的连续、平滑、可行。

图2 余弦曲线运动模型

1.2 余弦曲线路径规划算法设计

在确定好余弦曲线规划方法后，对余弦曲线进行详细分析，研究曲线参数，假定初始坐标为（x1，y1），目标坐标为（x2，y2），求解其他相关核心参数，以便确定曲线，求得最终规划路径余弦曲线。

余弦曲线的基本方程为

为方便计算机分析计算，将方程参数化，可得：

即

曲线可以先平移、后旋转，直至通过已知点（x1，y1）、（x2，y2），而曲线平移、旋转后的参数方程矩阵为

即：

但由于方程中x、y、θ、t、m、n未知数为5个，而已知条件只有两点坐标，所以未知参数在现有条件下不可解，需要创造新条件。

为此，令过（x1，y1）、（x2，y2）两点直线与X轴夹角为基本余弦曲线旋转角度，则有

为了使得曲线既通过（x1，y1）、（x2，y2）两点，又能够圆滑连接，令（x1，y1）为基本余弦曲线旋转、平移后初始位置，而基本余弦曲线旋转平移后可从（x1，y1）开始无限延伸，（x1，y1）为余弦曲线第一个（-π/2，π/2）区间，则易得：

设定参数t区间为（-0.5π，zπ-0.5π），其中：

经过前面分析计算，分别求出θ、t、m、n，利用式（5）可得经过任意（x1，y1）、（x2，y2）两点余弦曲线。

1.3 机器人余弦曲线路径规划

要将余弦曲线应用于机器人路径规划，还要针对机器人的运动特点进行分析。针对机器人运动特点，在机器人行进过程中还要实时调整机器人的姿态ρi：

式中：ρi为机器人在（xi，yi）点的方向矢量，ui、vi分别为x、y方向的单位方向矢量，li为各段距离。

如图3所示，根据式（5），机器人可从A点一步一步地沿坐标位置逐步调整姿态，依次以直线段方式经过C、D等各点，到达目标位置B。即将从A到B的余弦曲线首先离散化设计，分为若干段（如CD段），然后完成各段，各段综合逼近余弦曲线路径即可完成路径规划。

图3 机器人余弦线路径位姿规划模型

2 试验参数

为了验证路径规划方法的正确性、合理性和有效性，利用MATLAB进行试验分析。MATLAB是一款进行数据分析与图像处理的软件，可以进行计算机视觉、信号处理及机器人研究、控制分析，是美国研发的一款具有强大功能的数学工具[11]。试验参数如表1所示。

表1 余弦曲线路径规划初始参数 m

根据表1中试验参数，输入机器人余弦曲线路径初始位置和目标位置坐标，确定差分离散区间后，根据式（1）～式（8），依次生成曲线，并产生中间差分点坐标。

3 仿真试验

图4～图9分别为表1中曲线1～曲线6，6条曲线过渡平滑，曲线1和曲线4、曲线6由于两点距离较近，为小于1个周期的余弦曲线，曲线3、曲线5稍远，为1个周期左右的余弦曲线，曲线2因初始坐标位置和目标坐标位置较远，为多个周期余弦曲线，虽然较远，但从图5明显可以看出，其路径始终围绕初始坐标位置到目标坐标位置的直线移动，达到预期目标。

图4 曲线1

图1～图6中根据离散化设计，从初始坐标到目标坐标已离散成了若干份，可根据机器人单信号行走距离确定该离散区间。从图4～图9可以看出，本文设计的余弦曲线路径规划可以实现任意两点的机器人移动路径规划，根据本文设计方法，在输入首尾坐标及差分区间后，机器人余弦曲线路径将自动规划生成。

图6 曲线3

图7 曲线4

图8 曲线5

图9 曲线6

从表2参数可以看出，曲线2两点距离最长，单位置相对误差最小，曲线4两点距离最小，单位置相对误差并非最大，但在离散区间同为0.5的曲线内为相对误差最大。同样地，曲线5比曲线6距离远，但相对误差小。分析原因为：使用该路径规划方法时，在离散区间相同的情况下，由于余弦曲线的连续重复循环性，曲线整体变化规律不因曲线的长短而变化，因此曲线越长，相对误差越小，而这也是在机器人控制过程中期望的。

表2 余弦曲线路径规划试验结果m

4 机器人余弦曲线移动轨迹生成试验

4.1 试验平台

试验所用机器人为九自由度人形机器人（如图10）。九自由度人形机器人是一种比较特殊的机器人，它可以多种方式模仿人类运行。人形机器人凭借9个自由度可以向各个方向移动，比传统轮式机器人设计更能够自然地在环境中移动。试验平台为课题研发的人形机器人北斗导航试验系统，该系统主要由机器人余弦曲线运动轨迹规划模块、人形机器人基本运动轨迹生成模块及无线控制模块组成[12]。试验场景如图11所示。

图10 试验用机器人

图11 试验现场

4.2 试验参数

为了对机器人余弦曲线路径运动规划方法进行合理性验证，本课题组做了机器人余弦曲线路径运动控制试验。试验主要参数如表3所示。从表3中可以看出，系统主要参数包括初始、目标坐标和九自由度人形机器人基本试验运动参数。

表3 机器人余弦曲线路径运动控制主要试验参数

4.3 试验结果及分析

图12中两条路径分别为机器人余弦曲线运动控制试验规划路径及试验路径。从图12中可以看出，人形机器人余弦曲线规划和试验路径接近，这说明人形机器人余弦曲线规划算法可以达到对人形机器人进行余弦曲线控制的目的。从试验路径看出，虽然人形机器人在初期位移变化较大，但由于余弦曲线路径规划算法的设计，人形机器人逐渐逼近规划路径，实现了算法的预期目的。在整个人形机器人运动过程中，在机器人姿态变化较大处，位移较大，这是因为人形机器人本身每控制信号旋转角度和每控制信号移动位移有一定的调整度，另外，由于试验地面光滑程度的不同，导致与控制规划路径有一定的精度误差。从整体上看，余弦曲线的机器人移动路径自主规划算法实现了预期目的，达到了对人形机器人进行余弦曲线运动控制的目的。

图12 机器人余弦曲线运动控制试验规划路径及试验路径

5 结论

人形机器人运动控制是今后机器人技术发展的主要研究方向。为提高机器人路径规划自主程度，针对机器人远距离运动时避障和规避危险问题，提出利用余弦曲线来进行机器人运动路径规划的方法。通过仿真与试验研究，可以得出以下结论：1）通过分析，建立了余弦曲线变换模型和余弦曲线运动模型，进行了余弦曲线路径规划算法设计和机器人余弦曲线路径规划；2）通过不同坐标位置、参数的Matlab仿真试验表明，本文建立的数学模型是正确的，提出的机器人余弦曲线移动路径规划方法是合理的；3）在提出的余弦曲线移动路径自主规划方法时，在差分区间相同的情况下，其设计的运动精度将随着两点距离的增大而提高；4）利用该余弦曲线移动路径规划方法可节省规划时间，提高规划效率，为后续机器人技术的发展打下良好基础。