[发明专利]一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法

摘要

本发明公开了一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法，属于机器人技术领域。本发明通过参数的动态调节，将机器人的避障路径由一阶梯度下降方向朝二阶高斯方向旋转，从而提高机器人避障路径平滑度的方法。利用保守力场的二阶梯度信息来调整机器人的移动方向，同时通过对参数的动态调整来进一步改善原有算法的性能。与原有人工物理方法相比，本发明所提方法得到的机器人避障路径更加平滑、通过障碍区域的速度更快。该方法是解决狭窄空间下机器人移动路径导引与障碍规避的有效技术途径，也可应用于空中飞行器航路规划、无人驾驶车辆导引等技术领域。

主权项

1.一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法，其特征在于：该方法利用人工物理力的二阶梯度信息计算机器人的避障方向，以改善机器人避障路径平滑度，具体实施过程如下：确定机器人的初始坐标probot(0)＝(x0,y0)和目标点坐标pgoal＝(xgoal,ygoal)，机器人最大移动速度vmax，目标点引力常数Ggoal，障碍排斥力作用范围Rrep，障碍排斥系数Gobs，虚拟摩擦系数f，以及各个障碍物的位置，(x0,y0)为初始坐标，(xgoal,ygoal)为目标点坐标；步骤一：根据机器人当前位置probot与目标点位置pgoal之间的距离及相对方位，计算机器人受到的虚拟吸引力Fatt作用，Fatt的计算如下，步骤二：计算机器人在目标引力场中的高斯矩阵G(p,Fatt)，高斯矩阵G(p,Fatt)计算方式如下，式中，Fatt为虚拟吸引力Fatt的矢量表示，Fatt，x和Fatt，y分别为虚拟吸引力矢量Fatt在x方向和y方向上的分量；步骤三：根据G(p,Fatt)是否为正定矩阵，对参数ν进行调节，并计算逆矩阵如下B(p,Fatt)＝(G(p,Fatt)+ν·I)‑1  (8)式中的I为单位矩阵；步骤四：计算机器人到周围障碍物之间的距离，如果机器人与第i个障碍物的最小距离||probot‑pobs,i||小于Rrep，pobs,i表示第i个障碍物的位置，则机器人受到第i个障碍物的排斥力，第i个障碍物的Frep,i排斥力计算方式如下式中，||p_robot‑p_obs,i||为机器人到第i个障碍物的距离，用表示从第i个障碍物指向机器人的单位向量；步骤五：计算机器人在第i个障碍物排斥力场中的高斯矩阵G(p,Frep,i)，计算方式如下式中，Frep,i,x和Frep,i,y分别为虚拟吸引力矢量Frep,i在x方向和y方向上的分量；步骤六：根据G(p,Frep,i)是否为正定矩阵，对参数ν进行调节，并计算逆矩阵如下B(p,Frep,i)＝(G(p,Frep,i)+ν·I)‑1  (11)式中的I为单位矩阵；步骤七：计算机器人受到的虚拟摩擦力作用f＝‑f·v；步骤八：综合上述各个力的作用，计算机器人的控制输入如下u＝B(p,Fatt)·Fatt+∑B(p,Frep,i)·Frep,i+f  (12)步骤九：将式(12)中的机器人控制量代入到机器人运动方程中，更新机器人的速度及位置；步骤十：判断机器人是否到达目标点，若已到达，则机器人停止运动；若未到达，则返回到步骤一。