独立关节PD控制及其matlab simulink仿真

2024-06-25 11:58| 来源: 网络整理| 查看: 265

原理

机器人控制常采用PD控制和PID控制，其优点是控制律简单，易于实现，无需建模。

当忽略重力和外力干扰时，采用独立的PD控制，能满足机器人定点控制的要求。

根据欧拉-拉格朗日方程建立n连杆机器人的动力学模型： D ( q ) q ¨ + C ( q , q ˙ ) q ˙ + g ( q ) = τ D(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau D(q)q¨+C(q,q˙)q˙+g(q)=τ 其中D代表惯性项，C代表向心力项/哥氏力项，g代表重力项，q代表关节坐标， τ \tau τ代表了系统广义力。

PID控制基于误差，设固定的期望位置为 q d q_d qd，跟踪误差为 e = q d − q e=q_d-q e=qd−q，由于 q d q_d qd为常数， q ˙ d = q ¨ d = 0 \dot q_d=\ddot q_d =0 q˙d=q¨d=0。

PD控制律为： τ = K d e ˙ + K p e \tau = K_d \dot e + K_p e τ=Kde˙+Kpe 忽略重力，机器人方程表示为： D ( q ) ( 0 − q ¨ ) + C ( q , q ˙ ) ( 0 − q ˙ ) = − τ D(q)(0 - \ddot q)+C(q,\dot q)(0 -\dot q)=- \tau D(q)(0−q¨)+C(q,q˙)(0−q˙)=−τ 进一步化为： D ( q ) ( q ¨ d − q ¨ ) + C ( q , q ˙ ) ( q ˙ d − q ˙ ) + K d e ˙ = − K p e D(q)(\ddot q_d - \ddot q)+C(q,\dot q)(\dot q_d -\dot q)+ K_d \dot e =-K_p e D(q)(q¨d−q¨)+C(q,q˙)(q˙d−q˙)+Kde˙=−Kpe

考虑如下二连杆机器人系统：

在这里插入图片描述

其中：

实例化，假设 p = [ 2.90 0.76 0.87 ] T , q 0 = [ 0 0 ] T , q ˙ 0 = [ 0 0 ] T p=[2.90{\,} 0.76 {\,}0.87]^T, q_0 =[0 {\,}0]^T, \dot q_0 = [0{\,} 0 ]^T p=[2.900.760.87]T,q0=[00]T,q˙0=[00]T,目标位置 q d ( 0 ) = [ 1.0 1.0 ] T q_d(0)=[1.0 {\,}1.0]^T qd(0)=[1.01.0]T，取 K p = [ 100 0 0 100 ] , K d = [ 100 0 0 100 ] K_p=\begin{bmatrix}100 &0\\0&100 \end{bmatrix}, K_d=\begin{bmatrix}100 &0\\0&100 \end{bmatrix} Kp=[10000100],Kd=[10000100]。

得到： D ( q ) = [ 3.66 + 1.74 cos ⁡ q 2 0.76 + 0.87 cos ⁡ q 2 0.76 + 0.87 cos ⁡ q 2 0.76 ] , C ( q , q ˙ ) = [ − 0.87 q ˙ 2 sin ⁡ q 2 − 0.87 ( q ˙ 1 + q ˙ 2 ) sin ⁡ q 2 0.87 q ˙ 1 sin ⁡ q 2 0 ] D(q)=\begin{bmatrix}3.66+1.74\cos q_2 &0.76+0.87\cos q_2\\0.76+0.87\cos q_2&0.76 \end{bmatrix}, C(q,\dot q)=\begin{bmatrix}-0.87 \dot q_2\sin q_2 &-0.87(\dot q_1 + \dot q_2)\sin q_2\\0.87 \dot q_1\sin q_2&0 \end{bmatrix} D(q)=[3.66+1.74cosq20.76+0.87cosq20.76+0.87cosq20.76],C(q,q˙)=[−0.87q˙2sinq20.87q˙1sinq2−0.87(q˙1+q˙2)sinq20] 代入刚刚得到的机器人方程，并选取系统状态变量 [ θ 1 , θ ˙ 1 , θ 2 , θ ˙ 2 ] = [ q 1 , q ˙ 1 , q 2 , q ˙ 2 ] [θ_1,\dot θ_1 ,θ_2,\dot θ_2] = [q_1,\dot q_1, q_2,\dot q_2] [θ1,θ˙1,θ2,θ˙2]=[q1,q˙1,q2,q˙2]： { θ ˙ 1 = θ ˙ 1 θ ¨ 1 = τ 1 − ( 0.76 + 0.87 c o s θ 2 ) θ ¨ 2 + 0.87 s i n ( θ 2 ) θ ˙ 1 θ ˙ 2 + 0.87 ( θ ˙ 1 + θ ˙ 2 ) s i n θ 2 θ ˙ 2 ( 3.66 + 1.74 c o s θ 2 ) θ ˙ 2 = θ ˙ 2 θ ¨ 2 = τ 2 − ( 0.76 + 0.87 c o s θ 2 ) θ ¨ 1 − 0.87 s i n θ 2 ( θ ˙ 1 ) 2 0.76 τ 1 = K p ( q d 1 − q 1 ) − K d q ˙ 1 τ 2 = K p ( q d 2 − q 2 ) − K d q ˙ 2 \begin{cases} \dot θ_1 = \dot θ_1 \\ \ddot \theta_1 = {τ_1-(0.76+0.87cosθ_2 ) \ddot θ_2 + 0.87sin(θ_2 )\dot θ_1 \dot θ_2 +0.87(\dot θ_1 +\dot θ_2)sinθ_2 \dot θ_2 \over (3.66+1.74cosθ_2 )} \\ \dot θ_2 =\dot θ_2 \\ \ddot \theta_2 = {τ_2-(0.76+0.87cosθ_2 ) \ddot θ_1 -0.87sinθ_2 (\dot θ_1 )^2 \over 0.76}\\ \tau_1 = K_p(q_{d1}-q_1)-K_d \dot q_1 \\ \tau_2 = K_p(q_{d2}-q_2)-K_d \dot q_2 \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧θ˙1=θ˙1θ¨1=(3.66+1.74cosθ2)τ1−(0.76+0.87cosθ2)θ¨2+0.87sin(θ2)θ˙1θ˙2+0.87(θ˙1+θ˙2)sinθ2θ˙2θ˙2=θ˙2θ¨2=0.76τ2−(0.76+0.87cosθ2)θ¨1−0.87sinθ2(θ˙1)2τ1=Kp(qd1−q1)−Kdq˙1τ2=Kp(qd2−q2)−Kdq˙2

Matlab/simulink 仿真建模

依据上面得到的式子建立matlab simulink仿真模型：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-r7H7RAFz-1610609990391)(image-20210107132308822.png)]

注意计算 q ¨ 1 , q ¨ 2 \ddot q_1, \ddot q_2 q¨1,q¨2使用函数模块时候不能选择这个：

在这里插入图片描述

否则就会报错：

在这里插入图片描述

要选择这个：

在这里插入图片描述

示波器输出波形为：

在这里插入图片描述

修改Kp，Kd，使得 K p = [ 1000 0 0 1000 ] , K d = [ 100 0 0 100 ] K_p=\begin{bmatrix}1000&0\\0&1000 \end{bmatrix}, K_d=\begin{bmatrix}100 &0\\0&100 \end{bmatrix} Kp=[1000001000],Kd=[10000100]，得到：

在这里插入图片描述

调节时间缩短，响应变快。

修改Kp，Kd，使得 K p = [ 100 0 0 100 ] , K d = [ 10 0 0 10 ] K_p=\begin{bmatrix}100&0\\0&100 \end{bmatrix}, K_d=\begin{bmatrix}10 &0\\0&10 \end{bmatrix} Kp=[10000100],Kd=[100010]，得到：

在这里插入图片描述

出现过多超调，响应时间变长。

加入重力补偿

事实上完全不受外力，没有干扰的机械手系统是不存在的，独立的PD控制只是基础，可以作为参考来对系统进行分析。如果加入重力补偿，PD控制律变为： τ = K d e ˙ + K p e + G ^ ( q ) \tau = K_d \dot e + K_p e + \hat G(q) τ=Kde˙+Kpe+G^(q) 其中 G ^ ( q ) \hat G(q) G^(q)表示对重力矩的估计值。

机器人方程为： D ( q ) ( q ¨ d − q ¨ ) + C ( q , q ˙ ) ( q ˙ d − q ˙ ) + K d e ˙ + K p e + G ^ ( q ) − G ( q ) = 0 D(q)(\ddot q_d - \ddot q)+C(q,\dot q)(\dot q_d -\dot q)+ K_d \dot e + K_p e+\hat G(q)-G(q)=0 D(q)(q¨d−q¨)+C(q,q˙)(q˙d−q˙)+Kde˙+Kpe+G^(q)−G(q)=0 对重力矩的估计方法有如下两类：

当对重力矩估计准确时候， G ^ ( q ) , G ( q ) \hat G(q), G(q) G^(q),G(q)抵消，方程退化为上面的独立PD控制。当对重力矩的估计值不准确时候，需要设计重力补偿算法。目前代表性的重力补偿PD控制算法有在线估计的和固定的重力补偿PD控制。其中在线补偿更为准确，但是会消耗更多的计算资源，固定的补偿特性与之相反。

【本文地址】

公司简介

联系我们