Webots仿真 一阶倒立摆建模及LQR轨迹跟踪（上）

 

Webots建模

我们首先要过一遍官方指导，了解node的相关概念:下图为一个solid node

一个有物理效果的实体（重力，碰撞）就必须有boundingObject ,shape,physics节点组成

多个solid可以在children目录通过自身相对上级节点的坐标变换串接起来

一个实体主要要由boundingObject ,shape,physics 即物理属性 碰撞绑定形状，形状组成

箭头从上到下依次是：

坐标变换：参考坐标系为上一级坐标系（如果是再树中建立，就是以世界坐标系为参考，如果再children目录中的solid及其派生节点，则是以children的坐标原点变换(点击children目录就可以看到其对应的坐标系)）

rotation：四个参数，前三个定义一根旋转轴，第四个为角度

children:子节点用来连接其他东西，

contactMaterial：接触材质，在树最开始的worldInfo中设置

参数相关含义即英文含义，这里主要定义接触材质名称，和摩擦系数

boundingObject：绑定碰撞形状，参考官方例程。

physics：物理参数，参考官方例程：如果其为空（NULL),其所在节点将不会移动，参考示例中的场地墙的案例

机器人建模需要的Robot节点和solid节点基本相同这里就不再说明

开始修改模型

我们直接修改官方示例：

需要更改的项目将用其他颜色标出

我们将其另存为新后就可以开始更改

蓝绿色的字表示值是非默认值 

contactProperties：定义接触面的相关参数

material：接触面名称

coulombFriction：摩擦系数

 

我们可以看到官方例程里的碰撞绑定为图中红色箭头指示的红色框；我们要将它和下面的物理属性删掉；

因为不知道什么原因，这里设置的质量会影响LQR控制器的控制;比如本来小车只有1KG的质量，但是如果你要在计算状态空间方程时小车质量 设为1KG 控制器控制的小车就会飞掉。

修改小车材质，使其透明 方便后续

在其中选择 show joint axis 显示关节轴，可以看到其中会有两条条黑线：

其他显示参数自行调整

蓝色箭头方向的为小车导轨，红色箭头方向的就是摆杆的转轴。白色线框为小车碰撞边界

 

SliderJoint :线性驱动器，参数名称就是英文本意 ，从上到下为

关节参数：关于初始位置，方向 ，限位，弹簧参数，阻尼系数

驱动器:线性马达，位置传感器，要记住驱动器名称 后期的控制器需要它来调用API

 

现在可以看到：

箭头所指的有一个颜色较暗的小坐标系，这个是一个solid 节点质心的标志，它的位置明显不对劲

我们可以看到线性驱动器的endPoint的物理属性,形状和碰撞边界表现为小车，centerofmass为默认，没有偏移，所以这个节点的坐标系原点应该是摆杆的质心，其质量为1KG

 

我们将转动关节HIP 的关节参数的position(初始位置)改为0，方便读取后续坐标值，

anchor：转轴原点，改不改对本例没有影响。

可以看到 图中大号坐标系为转动关节endpoint 的坐标系，其参考坐标系为上级，（即滑动关节endpoint），有兴趣的可以自己改下参数观察。

我们看下面物理参数部分，centerOFMass有值，y=-0.27 就是以endpoint为参考，我们将其删掉，或回复默认值

这样质心就会和endpoint solid的坐标系重合，质心离转轴等于摆杆的坐标系Y轴数值

如果要修改摆杆长度，需要先修改endpoint的坐标变换，在修改其children中的shape相关。

倒立摆控制率及属性模型参考https://zhuanlan.zhihu.com/p/54071212

这里仅给出状态空间方程：

       

其中q:

控制器输入U=-[K1 K2 K3 K4] X

X=[X,X_DOT,TH,TH_DOT],

K矩阵由matlab的LQR函数算出 代码参照上述链接

控制器主要通过传感器获取位置和角度信息，计算状态变量，传给控制率来控制小车实现平衡。

这是控制效果，位置设置为1