动态环境下六自由度机械臂在线运动规划算法(2)

算法的优化目标是使每一时刻的运动轨迹点和参考轨迹点的误差最小,使机器人关节空间的角度、角速度、角加速度对参考轨迹进行跟随,同时对障碍物的距离增加一个惩罚项。将uk作为第k时刻优化问题的决策变量,基于上述参考轨迹定义目标函数为:

式中:p为角度误差的权重;v为角速度误差的权重;a为角加速度误差的权重;o为避障惩罚项的权重;分别为第k+1时刻的关节角度、角速度;xk+1为末端执行器第k+1时刻的空间位置。

xk+1为:

式中:xk为末端执行器第k时刻的空间位置。

式(4)中等号右边前三项依次表示对参考轨迹位置、速度、加速度的跟踪,最后一项表示对末端执行器和障碍物之间的距离的惩罚。

综合式(1)～式(7),定义优化问题为:

式中:qlow、qhigh分别为关节角度的下界、上界;分别为关节角速度的下界、上界。

可以看出,该优化问题是有约束二次规划问题,可以直接使用二次规划求解器求解。笔者在仿真中使用MATLAB软件的优化工具箱。

3.2 自适应权重参数方法

对于目标函数中的四个权重p、v、a、o,很多学者通过试验给出了最优值。然而使用已确定的最优值得到的轨迹往往存在振动过大和偏离问题。例如,在没有障碍物的时候,如果参考加速度的跟踪权重过大,那么会使末端执行器的轨迹偏离参考轨迹;在避障过程中,如果仅仅对位置项和速度项进行跟踪,那么会降低轨迹的抗干扰能力,由于传感器引入的误差,轨迹往往会产生较大的振动。对此,笔者提出自适应权重参数方法,使权重与障碍物距离成函数映射关系,用于拟合不同障碍物距离下的权重参数。

考虑到良好的函数性质,笔者使用Sigmoid函数变体来拟合权重参数变化。以加速度误差权重为例,有:

式中:μ、β、γ、λ为控制曲线形状的参数,μ、β分别控制纵轴方向上的拉伸比例和位置,γ、λ分别控制横轴方向上的拉伸比例和位置。

笔者使用的角度误差权重、角速度误差权重、角加速度误差权重及避障惩罚项权重与距离的拟合曲线如图2所示。

▲图2 权重与距离拟合曲线

4 算法实现

应用笔者提出的在线运动规划算法,在每一个时刻使用二次规划求解器求解优化问题,得到当前时刻的输出,并将控制参数传至底层控制器。在线运动规划算法流程如图3所示。这一算法相比离线规划算法,优势是计算量小,收敛速度快,实时性强。

▲图3 在线运动规划算法流程

5 算法仿真

笔者进行了两方面仿真。第一,使用笔者提出的关节空间在线运动规划算法与文献[10]任务空间规划算法进行比较。第二,对自适应权重参数方法和固定参数法的仿真结果进行比较。

计算机仿真环境如图4所示,障碍物碰撞模型和末端执行器碰撞模型均为凸包模型,参考轨迹为预先定义的B样条曲线,实时规划轨迹为使用在线规划算法得到的实际运动轨迹。

▲图4 计算机仿真环境

为了模拟实际的传感器噪声,笔者在仿真过程中对障碍物模型的位置变量加入高斯噪声,使Gilbert-Johnson-Keerthi算法测量得到的距离符合高斯分布,从而更加接近真实环境。并且,直接对Gilbert-Johnson-Keerthi算法的输出距离加以均值为0、方差为0.003的高斯噪声。

为了评估不同规划算法产生的轨迹的平滑程度差异,除给出直观的轨迹图像外,还使用滑动窗标准差来衡量轨迹的平滑程度。滑动窗标准差原理如图5所示。使用一个固定大小的窗口在轨迹图像上滑动,计算窗口内数据的标准差。这样,对于两条整体轨迹相似但局部振动幅度相差较大的轨迹,可以通过这一方法来量化平滑程度的差异。如果直接计算整条轨迹的标准差,那么无法衡量局部轨迹的平滑程度。同样,窗口宽度太大也无法达到要求,因此必须选择适当的窗口长度和滑动步幅。笔者仿真的滑动标准差窗口长度和滑动步幅分别取1 s、0.5 s。

▲图5 滑动窗标准差原理

6 关节空间规划与任务空间规划比较

文献[10]提出的任务空间实时规划算法,在任务空间中对参考轨迹点的位置、速度进行跟踪,轨迹的抗噪声能力较差。

笔者在关节空间中对参考轨迹点的位置、速度、加速度进行跟踪,在一定程度上提升了抗噪声能力,可以有效抑制避障过程中轨迹的抖动。

对比两种不同规划方法在有障碍物的情况下得到的轨迹规划结果,并使用滑动窗标准差来评估两条轨迹的平滑程度。两种规划算法的仿真参数见表1。

文章来源：《运动精品》 网址: http://www.ydjpzz.cn/qikandaodu/2021/0524/1801.html