【VREP】四舵轮（or n舵轮）自旋与平移融合运动解算

最新推荐文章于 2025-03-14 12:52:07 发布

LANLANLAN_hust

最新推荐文章于 2025-03-14 12:52:07 发布

阅读量1.2w

点赞数 37

分类专栏：一些记录文章标签：仿真器算法

本文链接：https://blog.csdn.net/LanLanLan1927/article/details/120395301

版权

一些记录专栏收录该内容

1 篇文章

订阅专栏

修改日志:
2021-10-16 增加了VS2019联和Vrep仿真代码

实现效果如图：

1.四舵轮模型

先介绍舵轮几个概念

v1 v2 v3 v4四个舵轮轮速

O1 O2 O3 O4四个舵轮舵角，规定舵角为与车体正方向为x正半轴建立坐标系中，与x正半轴的夹角，大小为-180到180

V Vw V是用户输入车体的平移方向，V是是矢量（假如输入Vx与Vy那么可以自己算V) 与自旋速度值。我们要做的就是分解三个速度为八个电机的值

平移

平移应该很容易相通就是四个舵角朝向一致，轮速为V

自旋

自旋也很容易，逆时针为正向的话，此时就是轮子摆成与车体正中心垂直的角度，轮速方向逆时针转一圈，速度可正可负，则为了让电机转角最小，如下

融合，矢量叠加

如上图最后一个所示

那么实际也是如此吗？看下图例子

大家思考一下对不对，最好自己画一下

哈哈哈是对的！因为这是画的图，但也不对，因为没有具体写代码来分解运动，代码过程中要考虑自旋矢量的方向是时时刻刻改变的，并且叠加完的速度再传递给电机时候需要从xy坐标系转换到电机的转角坐标系，电机的转角的多少是相对于车头方向的！

我的当时笔记如下，虽然字有些丑咳咳

那么解决方法就是引入角 $\Theta$ ，表示车体朝向

下面是个例子，如何计算某一时刻的自旋矢量 $\overrightarrow{M}$

至于为什么速度是负数，不换成角度-180°后，速度为正？

因为自旋过程中我们要减少电机变向力度，你可以从车头为0°一点一点画一圈，你会发现1号和4号轮子负速度是最节省的。

代码具体自己谢谢看咯，哪里不清楚可以评论

补充：

使用vs2019配合官方给的remoteAPI写的一个运动结算函数，截取关键步骤，建议读懂后自己写一下，有些细节处理可能只看代码一下子是看不懂的，需要自己谢谢根据实际情况调试才能懂。

main部分

#define	UP							VK_UP	
#define	DOWN						VK_DOWN	
#define	LEFT						VK_LEFT	
#define	RIGHT						VK_RIGHT
#define W_control						0x4A    //  J 设置自选速度W 转转转
#define	K_Gimbal						0x4B	//	K 模拟云台选择10°，相对于自选时候的基准方向旋转了10°
#define switch_							0x4C	//	L 切换运动模式，直线底盘跟随云台，或者自选运动模式

void main()
{


	if (clientID != -1)
	{
		cout << "V-rep connected.";
		simxStartSimulation(clientID,simx_opmode_oneshot);
	}
	else
	{
		cout << "V-rep can't be connected.";
	}

	Chassis Robot1(clientID);
	while (1) {
		Key_control();
		Robot1.Control(x, y, z, Gimbal);
		Robot1.Chassis_run();


		Sleep(10);
	}
	//cin.get();
	simxStopSimulation(clientID, simx_opmode_oneshot);
	simxFinish(clientID);
	return;
}


Key_contol_type key1 = {0,0,0,0};
//键盘控制
void Key_control() {

	x = 0;
	y = 0;
	z = 0;
	if (GetAsyncKeyState(UP) & 0x8000) //下
	{
		x = 40;
	}
	if (GetAsyncKeyState(DOWN) & 0x8000) //下
	{
		x = -40;
	}
	if (GetAsyncKeyState(LEFT) & 0x8000) //下
	{
		y = 40;
	}
	if (GetAsyncKeyState(RIGHT) & 0x8000) //下
	{
		y = -40;
	}
	if (GetAsyncKeyState(W_control) & 0x8000) //下
	{
		/*W_Flag++;*/
		z = 50;
	}
	if (GetAsyncKeyState(K_Gimbal) & 0x8000) {
		Gimbal += 10;
		if (Gimbal == 180) {
			Gimbal = -180;
		}
	}
	
	if (GetAsyncKeyState(switch_) & 0x8000) {
		move_flag ++;
	}

}

chassis.h

定义了一些数据结构，重要的有Velocity_vector_type 是个速度向量，因为上面的速度分解都是根据向量来算的，思路比较方便，到时候向量的角度赋给舵向转角即可

#define V_speed_limit    15//m/s
#define W_speed_limit    0.9//rad/s
#define CAR_LENGTH    0.5//m  电机0到2之间长度
#define CAR_WIDTH     0.4//m  电机0到1之间长度

typedef struct {
	float v;//m/s
	float yaw;//rad
}Velocity_vector_type;


typedef struct {
	simxInt motor_pitch, motor_yaw;//电机控制handle
	Velocity_vector_type Velocity_vector;//速度向量
	Velocity_vector_type Velocity_vector_last;//上一次速度；
}Wheel_control_type;


Velocity_vector_type Subtraction(Velocity_vector_type *a, Velocity_vector_type *b);//实现向量a-b
Velocity_vector_type add(Velocity_vector_type *a, Velocity_vector_type *b);//实现向量a+b


class Chassis {
public:
	Chassis(int clientID);
private:
	Wheel_control_type Wheel[4];//用于控制四个轮子运动
	Velocity_vector_type Wheel_Info[4];
	float Circle_rad;//正方形就是45°的弧度制

	float yaw_diff;
	float Chassis_yaw;
	float Gimbal_;
	simxInt Car_Handle;
private:
	int clientID;
public:
	void Chassis_run();
	void Chassis_GetOrientation(void);
	void Control(float x_speed, float y_speed, float w_speed,float Gimbal);//外部接口输出速度分解,gimbal是云台偏离正方向的角度
	float Control_direct(float x_speed, float y_speed, float w_speed);//直接输入三项速度，返回得到云台与底盘夹角作为setpoint
	void Circle_Control(float w_speed);
};


#endif // CHASSIS_H

chassis.c

主要解算函数，现在Contol函数内计算完八个电机的参数，Run函数内给电机传参数

计算的函数理论就是基于上面分析,有几个函数是Vrep的remoteAPI函数，用来控制仿真的车，作用看函数名即可，都是用来获取yaw数据，转速等等，实际车上对应就是陀螺仪与编码器反馈，重要的是理解，不懂仿真也没事，可以看的懂

#include "Chassis.h"

//获取底盘位姿
float AngleFloats[3];
float PointFloats[3];
char StrTemp[20] = { 0 };
extern int move_flag;
Velocity_vector_type Wheel_Init[4] = { 0 };
Pid_Typedef move_Gimbal_speed = { 0 };

//初始化
Chassis::Chassis(int ID){
	clientID = ID;
	Gimbal_ = 0;
	Chassis_yaw = 0;
	yaw_diff = 0;
	for (int i = 0; i < 4; i++) Wheel[i].Velocity_vector.v = 0, Wheel[i].Velocity_vector.yaw = 0;
	simxGetObjectHandle(clientID, "motor_yaw_0", &Wheel[0].motor_yaw, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_pitch_0", &Wheel[0].motor_pitch, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_yaw_1", &Wheel[1].motor_yaw, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_pitch_1", &Wheel[1].motor_pitch, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_yaw_2", &Wheel[2].motor_yaw, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_pitch_2", &Wheel[2].motor_pitch, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_yaw_3", &Wheel[3].motor_yaw, simx_opmode_blocking);
	simxGetObjectHandle(clientID, "motor_pitch_3", &Wheel[3].motor_pitch, simx_opmode_blocking);

	simxGetObjectHandle(clientID, "HEAD", &Car_Handle , simx_opmode_blocking);

	//pid init
	move_Gimbal_speed.P = 5;
	move_Gimbal_speed.I = 0;
	move_Gimbal_speed.D = 0;
	move_Gimbal_speed.ErrorMax = 1000.0f;
	move_Gimbal_speed.IMax = 200;
	move_Gimbal_speed.SetPoint = 0.0f;
	move_Gimbal_speed.OutMax = 30;	//最大占空比

	//逆时针为正方向
	Circle_rad = atan(CAR_LENGTH / CAR_WIDTH);
	printf("atan:%.4f\t", Circle_rad);
	Wheel_Init[0].v = 0; Wheel_Init[0].yaw = - Circle_rad;
	Wheel_Init[1].v = 0; Wheel_Init[1].yaw = Circle_rad;
	Wheel_Init[2].v = 0; Wheel_Init[2].yaw = Circle_rad;
	Wheel_Init[3].v = 0; Wheel_Init[3].yaw = - Circle_rad;

}


//外部接口输出速度分解
float car_V;//车体总体朝向与速度
float car_R;
float car_yaw;
void Chassis::Control(float x_speed, float y_speed, float w_speed, float Gimbal) {
	for (int i = 0; i < 4; i++) {//记录上次速度
		Wheel[i].Velocity_vector_last = Wheel[i].Velocity_vector;
	}
	Gimbal_ = DEG2R(Gimbal);
	Chassis_GetOrientation();//获取底盘与云台的夹角

	car_V = sqrt(x_speed * x_speed + y_speed * y_speed);
	if (car_V > V_speed_limit) car_V = V_speed_limit;


	car_yaw = R2DEG(atan2(y_speed, x_speed));
	//角度处理
	if (ABS(car_yaw > 90)) car_yaw = car_yaw - 180 , car_V = -car_V;
	if (ABS(car_yaw < -90)) car_yaw = car_yaw + 180 , car_V = -car_V;
	car_yaw = DEG2R(car_yaw);

	Wheel[0].Velocity_vector.yaw = car_yaw + DEG2R(8); Wheel[0].Velocity_vector.v = car_V;
	Wheel[1].Velocity_vector.yaw = car_yaw + DEG2R(13); Wheel[1].Velocity_vector.v = car_V;
	Wheel[2].Velocity_vector.yaw = car_yaw + DEG2R(13); Wheel[2].Velocity_vector.v = car_V;
	Wheel[3].Velocity_vector.yaw = car_yaw + DEG2R(13); Wheel[3].Velocity_vector.v = car_V;

	switch (move_flag%2) {
	case 0:
		//在世界坐标系中云台跟随，当前云台朝向设置为setpoint，当前值为车头
		move_Gimbal_speed.SetPoint = Gimbal_;
		w_speed = PID_Calc(&move_Gimbal_speed, Chassis_yaw);
		if (ABS(R2DEG(Gimbal_ - Chassis_yaw)) < 3) {
			w_speed = 0;
		}
		if (w_speed != 0) {
			/**************************circle self part************************************/
			//速度赋值Gimbal_
			Wheel_Init[0].v = w_speed;
			Wheel_Init[1].v = -w_speed;
			Wheel_Init[2].v = w_speed;
			Wheel_Init[3].v = -w_speed;

			//计算轮子垂直向量坐标的角度朝向，在需要控制的云台坐标系中,use for circle self
			Wheel_Init[0].yaw = Gimbal_ - Circle_rad;
			Wheel_Init[1].yaw = Gimbal_ + Circle_rad;
			Wheel_Init[2].yaw = Gimbal_ + Circle_rad;
			Wheel_Init[3].yaw = Gimbal_ - Circle_rad;
			/*********************************end**************************************/

			//this time ,car's move position is gimbal's positon
			for (int i = 0; i < 4; i++) {
				Wheel[i].Velocity_vector.yaw += Gimbal_;
			}

			//分别在云台坐标系中直线与垂直向量相加得到和矢量，然后换算回电机坐标系
			for (int i = 0; i < 4; i++) {
				Wheel[i].Velocity_vector = add(&(Wheel[i].Velocity_vector), &(Wheel_Init[i]));
				Wheel[i].Velocity_vector.yaw = Wheel[i].Velocity_vector.yaw - Gimbal_;//切换回电机yaw的坐标系
			}
			//printf("结果:%.8f, 叠加后的速度:%.5f\n\n", R2DEG(Wheel[3].Velocity_vector.yaw), Wheel[3].Velocity_vector.v);
		}
		printf("zhixian:%.5f\n\n", R2DEG(Gimbal_));
		break;
	case 1:
		if (w_speed != 0) {
			//速度赋值
			Wheel_Init[0].v = w_speed;
			Wheel_Init[1].v = -w_speed;
			Wheel_Init[2].v = w_speed;
			Wheel_Init[3].v = -w_speed;

			//计算轮子垂直向量坐标的角度朝向，在云台坐标系中
			Wheel_Init[0].yaw = yaw_diff - Circle_rad;
			Wheel_Init[1].yaw = yaw_diff + Circle_rad;
			Wheel_Init[2].yaw = yaw_diff + Circle_rad;
			Wheel_Init[3].yaw = yaw_diff - Circle_rad;

			//分别在云台坐标系中直线与垂直向量相加得到和矢量，然后换算回电机坐标系
			for (int i = 0; i < 4; i++) {
				Wheel[i].Velocity_vector = add(&(Wheel[i].Velocity_vector), &(Wheel_Init[i]));
				Wheel[i].Velocity_vector.yaw = Wheel[i].Velocity_vector.yaw - yaw_diff;//切换回电机yaw的坐标系
			}
			//printf("结果:%.8f, 叠加后的速度:%.5f\n\n", R2DEG(Wheel[3].Velocity_vector.yaw), Wheel[3].Velocity_vector.v);
		}
		break;
	}
	
		

	//printf("正方向:%.8f\n", car_yaw);
	//printf("V_x:%.5f   V_y:%.5f   V:%.5f  \tYAW:%.2f\n", x_speed, y_speed, car_V, Wheel[0].yaw);
}


//底盘运行函数
void Chassis::Chassis_run(void) {

	//最小转角
	for (int i = 0; i < 4; i++) {
		if (ABS(Wheel[i].Velocity_vector.yaw - Wheel[i].Velocity_vector_last.yaw) > DEG2R(90) )//如果变换角度大于90° 
		{
			if (Wheel[i].Velocity_vector.yaw < Wheel[i].Velocity_vector_last.yaw) {
				Wheel[i].Velocity_vector.yaw += DEG2R(180);
			}
			else { 
				Wheel[i].Velocity_vector.yaw -= DEG2R(180);
			}
			Wheel[i].Velocity_vector.v = -Wheel[i].Velocity_vector.v;
		}
		simxSetJointTargetVelocity(clientID, Wheel[i].motor_pitch, Wheel[i].Velocity_vector.v, simx_opmode_oneshot);
		simxSetJointTargetPosition(clientID, Wheel[i].motor_yaw, Wheel[i].Velocity_vector.yaw, simx_opmode_oneshot);
	}

}

//获取底盘目前朝向相对于云台夹角，取-90到90
static short First_Flag = 0;
void Chassis::Chassis_GetOrientation(void) {

	simxGetObjectOrientation(clientID, Car_Handle, -1, &AngleFloats[0], simx_opmode_blocking);
	sprintf_s(StrTemp, 20, "%.8f", AngleFloats[2]);
	Chassis_yaw = (float)(atof(StrTemp));//车头朝向
	yaw_diff = (float)(atof(StrTemp))+DEG2R(68) - Gimbal_;//自旋参数，得到底盘与云台的差值,后面减的值是云台偏离正方向的角度，模拟云台
	printf("云台正方向:%.8f\n", R2DEG(Gimbal_));

}



Velocity_vector_type Subtraction(Velocity_vector_type *a, Velocity_vector_type *b) //实现向量a-b
{
	Velocity_vector_type temp = { 0 };
	return temp;
}

Velocity_vector_type add(Velocity_vector_type *a, Velocity_vector_type *b)//实现向量a+b
{

	Velocity_vector_type temp;
	float x = a->v * cos(a->yaw) + b->v * cos(b->yaw);
	float y = a->v * sin(a->yaw) + b->v * sin(b->yaw);
	temp.v = sqrt(x * x + y * y);
	temp.yaw = atan2(y, x);
	//printf("x:%.5f,y:%.5f,yaw = :%.5f\n", x,y,R2DEG(temp.yaw));
	return temp;
}