一种基于PSO‑PD神经网络的四足机器人运动轨迹控制方法与流程

技术特征：

1.一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，包括：

(1)根据四足机器人运动轨迹与预设步长，求取四足机器人重心在该运动轨迹上的目标落点,即四足机器人重心在该运动轨迹上的坐标；

(2)将步骤(1)求取的四足机器人重心在该运动轨迹上的目标落点、实时反馈的躯干重心位置(y_x,y_y)输入PD神经网络模型的输入层；线性变换后进入PD神经网络模型的第一隐含层，在第一隐含层进行比例运算与微分运算；再次线性变换后进入PD神经网络模型的第二隐含层，在第二隐含层得到x方向与y方向上的指导位移，所处坐标系为四足机器人运动所在的水平平面的坐标系，原点为四足机器人初始重心位置；进入PD神经网络模型的第三隐含层，在第三隐含层求得指导位移距离dr、指导转向角度dθ，指导位移距离dr是指四足机器人从当前这一步到下一步迈出的距离，指导转向角度是指四足机器人从当前这一步到下一步的躯干转向角度；进入PD神经网络模型的第五层，在第五层实现对四足机器人的足端轨迹控制、转向控制；进入PD神经网络模型的第六层，当四足机器人足端发生滑动时，调整四足机器人姿态，保证四足机器人运行稳定；在第六层加入扰动进行模拟，并将加入扰动后的步长作为实际步长反馈给第五层；进入PD神经网络模型的第七层，求取四足机器人的躯干朝向y_θ、躯干重心位置(y_x,y_y)，躯干朝向y_θ是指当前迈步结束后，躯干的实际朝向，即躯干与X轴的夹角；躯干重心位置(y_x,y_y)是指四足机器人重心在所述坐标系中的坐标；四足机器人的躯干朝向y_θ反馈给PD神经网络模型的第五层，躯干重心位置(y_x,y_y)反馈给PD神经网络模型的输入层。

2.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，采用改进的粒子群优化算法调整PD神经网络模型中多层神经元之间的连接权值，实现面向机器人运动轨迹控制的PD神经网络的自适应学习，包括：

a、将多层神经元之间的连接权值的选取问题转化为最优化问题，最优化问题的目标函数即输出向量与导师信号向量的L2范数，如式(Ⅰ)所示，导师信号即目标落点坐标，输出向量即四足机器人实际重心位置；

$\min Error={\mathrm{\Σ}}_{k=1}^{200}\sqrt{{\left(xd\right(k)-y_x(k\left)\right)}^2+{\left(yd\right(k)-y_y(k\left)\right)}^2}---\left(I\right)$

式(Ⅰ)中，Error为最优化问题的目标函数，xd(k)、yd(k)分别为四足机器人第k步的重心目标落点的横坐标、纵坐标，y_x(k)、y_y(k)为四足机器人第k步的重心目标落点的实际横坐标、纵坐标；

b、利用先前经验确定每个连接权值的取值范围，即确定寻优范围；

c、在寻优范围内随机初始化一群粒子，即粒子群，包括初始化粒子的初始位置与初始速度，用位置、速度和适应度这三个指标表示粒子特征，位置表示PD神经网络模型中所有的连接权值取值，速度表示每个粒子演化的方向，适应度值由适应度函数求得，即每个粒子对应的目标函数；

粒子的速度依据粒子的当前位置、当前速度、粒子的历史最佳位置Pbest与粒子群中最优粒子的位置Gbest更新，粒子的速度的更新公式如式(Ⅱ)所示：

$V_{id}^{i+1}=\mathrm{\ω}\left(i\right)V_{id}^i+c_1{r}_1(P_{id}^i-X_{id}^i)+c_2{r}_2(P_{gd}^i-X_{id}^i)---\left(II\right)$

式(Ⅱ)中，id为粒子群中粒子的编号，为第i代粒子的速度，为第i代粒子在第i代之前的历史最佳位置，为第i代粒子群中最优粒子的位置；ω(i)为第i代粒子的惯性权重，其大小决定速度在多大程度上继承上一代粒子的运动速度；％₁、％₂为加速度因子，取值为非负常数；r₁、r₂为0到1之间的随机数；是第i代粒子的位置；初始化时ω的取值ω_start为0.9，迭代结束时ω的取值ω_34d为0.01，在迭代过程中惯性权重ω加速衰减，初期优先寻优速度，后期着重寻优精度，惯性权重ω的更新公式如式(Ⅲ)所示：

$\mathrm{\ω}\left(i\right)={\mathrm{\ω}}_{start}-({\mathrm{\ω}}_{start}-{\mathrm{\ω}}_{end})*{\left(\frac{i}{\max gen}\right)}^2---\left(III\right)$

式(Ⅲ)中，maxgen为最大迭代次数；

得到更新后的粒子的速度后，更新该粒子的位置，粒子的位置的更新公式如式(Ⅳ)所示：

$X_{id}^{i+1}=X_{id}^i+V_{id}^{i+1}---\left(IV\right)$

若式(Ⅳ)求取的对应的目标函数小于对应的适应度函数，则反之，同时，更新种群中最优粒子的位置，得到

如此，进行多次迭代即得到近似最优解，即使PD神经网络模型控制误差最小的近似最优权值。

3.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，所述步骤(1)，根据四足机器人运动轨迹，求取四足机器人在该运动轨迹上的目标落点，设定四足机器人的运动轨迹为正弦曲线，设置预设步长L的取值范围为0.38-0.42m，求取四足机器人在该运动轨迹上的目标落点的公式如式(Ⅴ)所示：

$\left\{\begin{array}{c}y=4*(1-cos(0.25*x))\\ {(x-x\_1)}^2+{(y-y\_1)}^2=L^2\\ x-x\_1\≥0\end{array}\right.---\left(V\right)$

式(Ⅴ)中，x_1、y_1分别为前一步四足机器人重心的横坐标与纵坐标，x、y分别为当前一步四足机器人重心的横坐标与纵坐标；

x_1、y_1的初始值均为0，求取x、y后，再赋值给x_1、y_1，反复迭代后，得到四足机器人在该运动轨迹上的目标落点。

4.根据权利要求3所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，L的取值为0.4m。

5.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，在第三隐含层求得指导位移距离dr、指导躯干朝向dθ，包括：

通过式(Ⅵ)求取指导位移距离dr，式(Ⅵ)如下所示：

$dr=\sqrt{{\left(d\mathrm{\Δ}x\right)}^2+{\left(d\mathrm{\Δ}y\right)}^2}---\left(VI\right)$

式(Ⅵ)中，dΔx,dΔy为第二隐含层所求得的指导x增量与指导y增量，即x方向与y方向上的指导位移；

通过式(Ⅶ)求取指导躯干朝向dθ，式(Ⅶ)如下所示：

$d\mathrm{\θ}=arctan\frac{d\mathrm{\Δ}x}{d\mathrm{\Δ}y}---\left(VII\right).$

6.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，实现对四足机器人的足端轨迹控制，四足机器人包括躯干以及与所述躯干连接的四条腿，在四足机器人的一个迈步周期中，在同一对角线上的两条腿以相同运动方式摆动，支撑躯干并推动躯干前进的两条腿称为支撑相，而按照事先设定轨迹进行摆动的另一对角线上的两条腿称为摆动相，包括步骤如下：

A、通过分析四足机器人腿部结构，构建四足机器人腿部运动数学模型，包括四足机器人足端位置模型：

每条腿包括大腿、小腿；四足机器人的一个迈步周期包括一个支撑相、一个摆动相，躯干坐标系中，x轴表示足端在水平方向上的位移，y轴代表足端高度，原点o为腿髋关节在地面上的投影点，躯干坐标系下足端位置模型如式(Ⅷ)所示：

$\left\{\begin{array}{c}{p}_x({\mathrm{\θ}}_1,{\mathrm{\θ}}_2)=L_1*{\mathrm{sin\θ}}_1-L_2*sin({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ p_z({\mathrm{\θ}}_1,{\mathrm{\θ}}_2)=H-L_1*cos+L_2*\cos ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}\right.---\left(VIII\right)$

式(Ⅷ)中，p_x(θ₁,θ₂)为足端水平位移与θ₁、θ₂的函数，p_z(θ₁,θ₂)为足端高度与θ₁、θ₂的函数，L₁为机器人大腿的长度，L₂为机器人小腿的长度，θ₁为机器人髋关节纵向开合角度，取值范围为0°-180°；θ₂为膝关节开合角度，取值范围为0°-180°；H为机器人躯干底端到地面的距离；

B、采用一种新型正弦对角步态，求取支撑相足端在躯干坐标系下的运动轨迹、摆动相足端在躯干坐标系下的运动轨迹，所述新型正弦对角步态是指：四足机器人的四条腿以对角分为两组，前左腿和右后腿为一组，前右腿和后左腿为一组，两组交替作为支撑相与摆动相；支撑相推动躯干前行，摆动相向前迈步跨越障碍；

支撑相足端在躯干坐标系下的运动轨迹如式(Ⅸ)所示：

$\left\{\begin{array}{c}p{1}_x\left(t\right)=0.5*S\_1-0.5*(S+S\_1)*\frac{t}{T}\\ p{1}_z\left(t\right)=0\end{array}\right.---\left(IX\right)$

式(Ⅸ)中，p1_x(t)为支撑相足端水平位移关于时间t的函数，p1_z(t)支撑相足端高度关于时间t的函数，S为当前迈步周期步长，S_1为上一迈步周期实际步长，t为迈步周期内的时刻，T为迈步周期；

摆动相足端在躯干坐标系下的运动轨迹如式(Ⅹ)所示：

$\left\{\begin{array}{cc}p{2}_x\left(t\right)=-0.5*S\_1+0.5+(S\_1+S)*sin(0.5*\mathrm{\π}*\frac{t}{T}),& 0\≤t\≤T\\ p{2}_z\left(t\right)=h*sin(\mathrm{\π}*\frac{t}{T}),& 0\≤t\≤T\end{array}\right.---\left(X\right)$

式(Ⅹ)中，p2_x(t)为摆动相足端水平位移关于时间t的函数，p2_z(t)摆动相足端高度关于时间t的函数，h为步高，为摆动相上升过程中达到的最高点；

C、控制率的求解

设定步长的取值范围是0.38m～0.42m，精度为1mm，则摆动相足端在躯干坐标系下的运动轨迹有41*41＝1681种可能性；

将1681种可能性对应的S_1、S带入公式(Ⅹ)，求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)；

将求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)代入公式(Ⅷ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制摆动相运动的控制率；

将1681种可能性对应的S_1、S带入公式(Ⅹ)，求得支撑相足端不同时刻的位置p1_x(t)、p1_z(t)；

将求得的支撑相足端不同时刻的位置p1_x(t)、p1_z(t)代入公式(Ⅷ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制支撑相运动的控制率；

D、根据求取的摆动相运动的控制率、支撑相运动的控制率，建立数据库

数据库采用二维地址指针，第一维为S_1，第二维为S，每个地址存有2个2*200的控制矩阵，分别为支撑相关节控制率与摆动相关节控制率；

E、通过快速查表法，实现对四足机器人的足端轨迹控制

a、求取S与S_1；

b、根据S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

c、将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动。

7.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，实现对四足机器人的转向控制，包括步骤如下：

F、获取四足机器人前一迈步周期结束时刻的躯干朝向y_θ_1、理想情况下迈步周期结束时躯干的朝向θ；

G、通过式(Ⅺ)求得躯干的转向角度Δθ，式(Ⅺ)如下所示：

y_θ＝y_θ_1+Δθ (Ⅺ)

H、通过式(Ⅻ)求得支撑相髋关节横向开合角的控制率θ1₄：

$\mathrm{\θ}{1}_4=-\mathrm{\Δ}\mathrm{\θ}*\frac{t}{T}---\left(XII\right)$

I、摆动相的胯关节横向开合角要回正，准备状态时，y_θ_1＝y_θ_2＝0，对式(XIII)进行迭代运算求得摆动相的回正角度为Δθ_1，大小为当前躯干朝向y_θ_1与前一周期初始时刻躯干朝向y_θ_2的差：

Δθ_1＝y_θ_1-y_θ_2(XIII)

J、通过式(XIV)求得摆动相髋关节横向开合角的控制率θ2₄：

$\mathrm{\θ}{2}_4=\mathrm{\Δ}\mathrm{\θ}\_1*\frac{t}{T}---\left(XIV\right)$

K、通过控制率分配，将信号传递给相应的腿组，令其依照控制率变化，实现四足机器人的转向控制。

8.根据权利要求1所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，当四足机器人发生扰动时，调整四足机器人姿态，保证四足机器人运行稳定，包括步骤如下：

在迈步周期结束后T/10内，足端轨迹控制单元与转向控制单元协同工作调整机器人姿态，假设随机发生的扰动大小为(Δp_x,Δp_y)，(Δp_x,Δp_y)是指相较于目标落点的偏差量，包括：

作为摆动相的腿贴着地面向前挪动，在调整姿态的同时，把能耗降到最低，步长为Δr，通过式(XV)求取：

$\mathrm{\Δ}r=\sqrt{{\mathrm{\Δp}}_x^2+{\mathrm{\Δp}}_y^2}---\left(XV\right)$

与此同时，支撑相将机器人躯干向前推进同时髋关节横向开合角改变Δθ₂，通过式(XVI)求取：

${\mathrm{\Δ\θ}}_2=arctan\frac{p_y}{p_x}-arctan\frac{p_y+{\mathrm{\Δp}}_y}{p_x+{\mathrm{\Δp}}_x}---\left(XVI\right).$

9.根据权利要求6所述的一种基于PSO-PD神经网络的四足机器人运动轨迹控制方法，其特征在于，求取四足机器人的躯干朝向y_θ、躯干重心位置(y_x,y_y)，求取公式如式(XVII)所示：

$\left\{\begin{array}{c}{y}_{\mathrm{\θ}}=y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}}\\ y_x=0.5*cos(y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}})*U_r\\ y_y=0.5*sin(y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}})*U_r\end{array}\right.---\left(XVII\right);$

式(XVII)中，U_θ是指实际躯干转向角度，U_r是指实际四足机器人重心位移距离。