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

权利要求书

1.一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法，其特征在于：该方法利用人工物理力的二阶梯度信息计算机器人的避障方向，以改善机器人避障路径平滑度，具体实施过程如下：

确定机器人的初始坐标p_robot(0)＝(x₀,y₀)和目标点坐标p_goal＝(x_goal,y_goal)，机器人最大移动速度v_max，目标点引力常数G_goal，障碍排斥力作用范围R_rep，障碍排斥系数G_obs，虚拟摩擦系数f，以及各个障碍物的位置，(x₀,y₀)为初始坐标，(x_goal,y_goal)为目标点坐标；

步骤一：根据机器人当前位置p_robot与目标点位置p_goal之间的距离及相对方位，计算机器人受到的虚拟吸引力F_att作用，F_att的计算如下，

步骤二：计算机器人在目标引力场中的高斯矩阵G(p,F_att)，高斯矩阵G(p,F_att)计算方式如下，

式中，F_att为虚拟吸引力F_att的矢量表示，F_att，_x和F_att，_y分别为虚拟吸引力矢量F_att在x方向和y方向上的分量；

步骤三：根据G(p,F_att)是否为正定矩阵，对参数ν进行调节，并计算逆矩阵如下

B(p,F_att)＝(G(p,F_att)+ν·I)^-1 (8)

式中的I为单位矩阵；

步骤四：计算机器人到周围障碍物之间的距离，如果机器人与第i个障碍物的最小距离||p_robot-p_obs,i||小于R_rep，p_obs,i表示第i个障碍物的位置，则机器人受到第i个障碍物的排斥力，第i个障碍物的F_rep,i排斥力计算方式如下

式中，||p_robot-p_obs,i||为机器人到第i个障碍物的距离，用表示从第i个障碍物指向机器人的单位向量；

步骤五：计算机器人在第i个障碍物排斥力场中的高斯矩阵G(p,F_rep,i)，计算方式如下

式中，F_rep,i,x和F_rep,i,y分别为虚拟吸引力矢量F_rep,i在x方向和y方向上的分量；

步骤六：根据G(p,F_rep,i)是否为正定矩阵，对参数ν进行调节，并计算逆矩阵如下

B(p,F_rep,i)＝(G(p,F_rep,i)+ν·I)^-1 (11)

式中的I为单位矩阵；

步骤七：计算机器人受到的虚拟摩擦力作用f＝-f·v；

步骤八：综合上述各个力的作用，计算机器人的控制输入如下

u＝B(p,F_att)·F_att+∑B(p,F_rep,i)·F_rep,i+f (12)

步骤九：将式(12)中的机器人控制量代入到机器人运动方程中，更新机器人的速度及位置；

步骤十：判断机器人是否到达目标点，若已到达，则机器人停止运动；若未到达，则返回到步骤一。

2.根据权利要求1所述的一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法，其特征在于：参数ν均需要按照如下S1-S4得到：

S1：如果矩阵G+ν·I是正定的，那么令ν＝ν/2，转到S3；否则，令ν＝4·ν，转到S2；

S2：如果矩阵G+ν·I是正定的，那么转到S4；否则，返回到S1；

S3：如果矩阵G+ν·I是正定的，那么返回到S1；否则，令ν＝2·ν，转到S4；

S4：参数调整结束，得到参数ν的值。