QT与Coin3D实现机器人的仿真

喜欢ヅ旅行 2022-10-15 04:46 506阅读 0赞

# 前言 #

> Coin3D编程环境配置好后，开始下一步的机器人仿真任务。本文根据在研5轴机器人，完成三维模型生成与机器人的仿真。主要参考博主冯Jungle的博客，在此表示感谢。  
> ——2021.05.23

# 环境 #

Win10：VS2019、QT5.15.2、Coin3D、SolidWorks2016

# 机器人三维模型生成 #

> 目前广泛应用于因特网的VRML ( Virtual Reality Modeling Language，虚拟现实建模语言)为用户提供一种可参与的、并能对场景中对象主动做出反应的虚拟现实环境，现已成为一种比较有效的程序设计语言。  
> 创建VRML文件一般有三种方法。第一种方法是直接使用文本编辑器书写VRML文件。这样做的不足之处是只能创建比较简单的物体和场景，因为几乎不可能使用文本编辑器直接编写出复杂的场景。场景之间的相互关系和位置确定起来太过复杂，对复杂物体的建模显得无能为力，且对数学能力的要求也较高。第二种方法是使用可视化编辑器建模。这就避免了直接使用文本建模时的一些问题，如不能生成复杂场景等缺陷。这些工具的一个突出的优点就是使用方便，不需要手工输入大量命令而只需拖动鼠标即可创建复杂的场景。常见的可视化编辑器有Cosmo World 2.0、Virtual Home Space Builder、Community Place、Pioneer 以及World View等。第三种方法是使用常见的三维建模软件进行场景文件的转换。  
> **本文使用第三种建模方式，利用SolidWorks2016直接将三维模型导出为VRML格式。**

### 将三维模型装配成相互独立的部件（我这里是5轴机器人，所以有六个部件） ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70]

根据后面Coin3D中机器人三维模型连接的两种方式，这里生成三维模型也可以有两种方式。

### 方式一：将坐标系建立在旋转轴原点 ###

以部件3为例，只需建立在旋转轴的原点，方向任意  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 1]

### 方式二：将坐标系与根据DH法建立的坐标系对应 ###

**原点对应，方向也对应**

我这里DH法建模坐标系如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 2]  
所以部件3与部件4的坐标系建立如下图所示：

同一高度  
![在这里插入图片描述][20210523171848223.png]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 3]

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 4]

### 导出VRML格式 ###

另存为->保存类型：VRML  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 5]  
点击选项  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 6]  
输出为版本：VRML97，单位：毫米  
选择在单一文件中保存装配体  
输出坐标系选择**自己建立好的坐标系**  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 7]  
至此已经生成了用于仿真的三维模型文件

# UI界面编写 #

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 8]

# Coin3D中机器人仿真 #

Coin3D对3D数据使用的是右手坐标系，从屏幕内指向外，表示z轴的正方向。所有的角度单位都是**弧度**。对象都是在自己的**局部坐标系空间**下进行描述的，既众所周知的“对象坐标系空间”（object coordinate space）。当场景中的所有物体都已经进行完坐标变换后，那么它们就都在“世界坐标系空间”下描述了（world coordinate space）。照相机和灯光节点处于世界坐标系空间下。

本文的五轴机器人仿真模型节点图如图所示  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70_pic_center]  
具体流程为：

### 1、读入三维模型 ###

SoVRMLGroup * Robot::openModel(QString fileName)
    {
        
    	SoInput * myInput = new SoInput;
    	if (myInput->openFile(fileName.toStdString().data()))
    	{
        
    		SoVRMLGroup *model = SoDB::readAllVRML(myInput);
    		myInput->closeFile();
    		delete myInput;
    		return model;
    	}
    	myInput->closeFile();
    	delete myInput;
    	return NULL;
    }

### 2、进行坐标变换 ###

**方式一**：  
根据前面三维模型生成中**方式一**的三维模型

核心思想是，将模型的坐标系相对当前局部坐标系变换到目标坐标系，变换完成后部件就在世界坐标系下描述了。

核心代码如下（**有注释**）：

#include <Inventor/nodes/SoTransform.h>
    #include <Inventor/nodes/SoRotationXYZ.h>
    
    SoTransform* angleAxisTransform = new SoTransform;//模型变换节点
    SoRotationXYZ* angleAxisRotation = new SoRotationXYZ;
    angleAxisTransform->translation.setValue(0, 0, 500);//将模型沿Z轴平移500mm
    angleAxisTransform->rotation.setValue(SbVec3f(1, 0, 0), 1.5707963f);//将模型绕X轴旋转90度
    angleAxisRotation->axis.setValue("Z");//设置旋转轴为Z（变换后的坐标系Z轴）

**方式二：**

根据前面三维模型生成中**方式二**的三维模型

核心思想是根据节点SoMatrixTransform，将坐标变换描述成DH法中`T0*T1*T2*T3*T4*T5`的形式，优点是可以进行正逆运动学解算等操作。

这里需要理解一下几种不同的节点，变换节点是累乘的，所以angleAxis会向下传递，这样就有了前面轴转动后面部件跟着一起转动的效果。参考[Coin3D相关模块学习][Coin3D]

核心代码如下：

SoDHTransform.h

#ifndef SODHTRANSFORM_H
    #define SODHTRANSFORM_H
    
    #include <QString>
    #include <QDebug>
    
    #include <Inventor/nodes/SoMatrixTransform.h>
    #include <qmath.h>
    
    class SoDHTransform : public SoMatrixTransform
    {
        
    public:
    	enum DHTransformType
    	{
        
    		FIXED, 
    		ROTATION, 
    		MOVEMENT
    	};
    
    	SoDHTransform();
    	~SoDHTransform();
    	void init(double itheta, double id, double ialpha, double ia, DHTransformType type = ROTATION);
    	void transform(double value);
    	SoMatrixTransform * inverseMatrix();
    	double getValue();
    	QString getTpye();
    
    	QString name;
    	QString description;
    	double max_ang;
    	double min_ang;
    
    private:
    	DHTransformType type;
    	double theta;
    	double d;
    	double a;
    	double alpha;
    	void calculate();
    };
    
    #endif // SODHTRANSFORM_H

SoDHTransform.cpp

#include "sodhtransform.h"
    
    SoDHTransform::SoDHTransform()
    	:theta(0.0), d(0.0), a(0.0), alpha(0.0), type(ROTATION), name(""), description(""), max_ang(180), min_ang(-180)
    {
        
    
    }
    
    SoDHTransform::~SoDHTransform()
    {
        
    
    }
    
    void SoDHTransform::init(double itheta, double id, double ialpha, double ia, DHTransformType type)
    {
        
    	this->type = type;
    	this->theta = itheta / 180 * 3.141592653;
    	this->d = id;
    	this->a = ia;
    	this->alpha = ialpha / 180 * 3.141592653;
    	calculate();
    }
    
    void SoDHTransform::transform(double value)
    {
        
    	if (this->type == ROTATION)
    	{
        
    		this->theta = value / 180 * 3.141592653;
    	}
    	else if(this->type == MOVEMENT)
    	{
        
    		this->d = value;
    	}
    	else //this->type = FIXED
    	{
        
    		qDebug() << "Warning: this->type = FIXED";
    	}
    	calculate();
    }
    
    void SoDHTransform::calculate()
    {
        
    	this->matrix.setValue(
    		 qCos(theta), qSin(theta)*qCos(alpha),  qSin(theta)*qSin(alpha),  0.0,
    		-qSin(theta), qCos(theta)*qCos(alpha),  qCos(theta)*qSin(alpha),  0.0,
    		         0.0,            -qSin(alpha),              qCos(alpha),  0.0,
    		           a,        -d * qSin(alpha),            d*qCos(alpha),  1.0);
    }
    
    SoMatrixTransform * SoDHTransform::inverseMatrix()
    {
        
    	SoMatrixTransform *inMatrix = new SoMatrixTransform;
    	inMatrix->matrix.setValue(this->matrix.getValue().inverse());
    	return inMatrix;
    }
    
    double SoDHTransform::getValue()
    {
        
    	if (type == FIXED)
    	{
        
    		return 0.0;
    	}
    	else if (type == MOVEMENT)
    	{
        
    		return d;
    	}
    	else if (type == ROTATION)
    	{
        
    		return theta * 180.0 / 3.141592653;
    	}
    	else
    		return -1;
    }
    
    QString SoDHTransform::getTpye()
    {
        
    	if (type == FIXED)
    	{
        
    		return "FIXED";
    	}
    	else if (type == MOVEMENT)
    	{
        
    		return "MOVEMENT";
    	}
    	else if (type == ROTATION)
    	{
        
    		return "ROTATION";
    	}
    	else
    		return "";
    }

根据DH法结果，设置DH参数

void Robot::setDHParameters()
    {
        
    	a2 = 400.00;
    	a3 = 185.00;  
    	a4 = 384.50;
    	d2 = 677.15;
    
    	angleAxis1->init( 0.0,   0.0,   0.0,  0.0, SoDHTransform::MOVEMENT);
    	angleAxis2->init( 0.0,   d2,  0.0,   0.0);
    	angleAxis3->init( 0.0,  0.0,   0.0,   a2);
    	angleAxis4->init( 0.0,   a3,  -90.0,   0);	
    	angleAxis5->init( 0.0,  0, -90.0,  0);
    }

# 效果 #

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 9]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 10]

# 参考资料： #

[Qt设计机器人仿真控制器][Qt]

[Coin3D相关模块学习][Coin3D]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70]: /images/20221014/a15d6901cd3440f48538e881aa0888cb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 1]: /images/20221014/fc6690995f9840e5a52f529f9a384c68.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 2]: /images/20221014/ef842121b8554046ac32d1bcc13c6cd2.png
[20210523171848223.png]: /images/20221014/53320536602645cdb269d26a0199112c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 3]: /images/20221014/4d64e0bf15a34d0790162e22733644ea.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 4]: /images/20221014/58d788b3b033426ab458940afbf11312.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 5]: /images/20221014/5ef2af67822946cd9883c2ea52cc7e72.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 6]: /images/20221014/4c9c7a0dce634ee99a681fba120cc6e8.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 7]: /images/20221014/02d02213bf9648bc9783c43e48b54683.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 8]: /images/20221014/82913f17fea749eeab078115e197a185.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70_pic_center]: /images/20221014/9e3ef47c46ba42d5a9102876bd83540e.png
[Coin3D]: https://blog.csdn.net/jiaolu295/article/details/117023774
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 9]: /images/20221014/04994141e9c045c28a00a01d27790b30.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ppYW9sdTI5NQ_size_16_color_FFFFFF_t_70 10]: /images/20221014/f7a34f3436cc4ffc95a7aa076c54db52.png
[Qt]: https://blog.csdn.net/sinat_21107433/article/details/100054427