智能小车项目（七）通过PID实现给定和实际速度值计算PWM输出

我们先看大脑（上位机nano）
在这里插入图片描述
keybord_ctrl节点发布’cmd_vel’消息消息类型为Twist队列大小为1

pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

if not stop: pub.publish(twist)

driver_node订阅这个消息当有消息时cmd_vel_callback回掉函数处理消息调用set_car_motion处理消息

 rospy.init_node("driver_node", anonymous=False)

    def cmd_vel_callback(self, msg):# 小车运动控制，订阅者回调函数# Car motion control, subscriber callback functionif not isinstance(msg, Twist): return# 下发线速度和角速度# Issue linear vel and angular velvx = msg.linear.xvy = msg.linear.yangular = msg.angular.z# 小车运动控制,vel: ±1, angular: ±5# Trolley motion control,vel=[-1, 1], angular=[-5, 5]# rospy.loginfo("cmd_velx: {}, cmd_vely: {}, cmd_ang: {}".format(vx, vy, angular))self.car.set_car_motion(vx, vy, angular)

    def set_car_motion(self, v_x, v_y, v_z):try:vx_parms = bytearray(struct.pack('h', int(v_x*1000)))vy_parms = bytearray(struct.pack('h', int(v_y*1000)))vz_parms = bytearray(struct.pack('h', int(v_z*1000)))cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_MOTION, self.__CAR_TYPE, \vx_parms[0], vx_parms[1], vy_parms[0], vy_parms[1], vz_parms[0], vz_parms[1]]cmd[2] = len(cmd) - 1checksum = sum(cmd, self.__COMPLEMENT) & 0xffcmd.append(checksum)self.ser.write(cmd)if self.__debug:print("motion:", cmd)time.sleep(self.__delay_time)except:print('---set_car_motion error!---')pass

再看小脑（下位机32）
下位机接到串口消息进行速度处理Motion_Ctrl

	case FUNC_MOTION:{uint8_t parm = (uint8_t) *(data_buf + 4);int16_t Vx_recv = *(data_buf + 6) << 8 | *(data_buf + 5);int16_t Vy_recv = *(data_buf + 8) << 8 | *(data_buf + 7);int16_t Vz_recv = *(data_buf + 10) << 8 | *(data_buf + 9);uint8_t adjust = parm & 0x80;DEBUG("motion: 0x%02X, %d, %d, %d\n", parm, Vx_recv, Vy_recv, Vz_recv);if (Vx_recv == 0 && Vy_recv == 0 && Vz_recv == 0){Motion_Stop(STOP_BRAKE);}else{Motion_Ctrl(Vx_recv, Vy_recv, Vz_recv, (adjust==0?0:1));}break;}

通过公式计算每个轮子的速度调用Mecanum_Ctrl函数处理速度

// 控制小车运动
void Motion_Ctrl(int16_t V_x, int16_t V_y, int16_t V_z, uint8_t adjust)
{switch (g_car_type){case CAR_MECANUM:{Mecanum_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_MECANUM_MAX:{if (V_x > CAR_X3_PLUS_MAX_SPEED)  V_x = CAR_X3_PLUS_MAX_SPEED;if (V_x < -CAR_X3_PLUS_MAX_SPEED) V_x = -CAR_X3_PLUS_MAX_SPEED;if (V_y > CAR_X3_PLUS_MAX_SPEED)  V_y = CAR_X3_PLUS_MAX_SPEED;if (V_y < -CAR_X3_PLUS_MAX_SPEED) V_y = -CAR_X3_PLUS_MAX_SPEED;Mecanum_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_FOURWHEEL:{Fourwheel_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_ACKERMAN:{Ackerman_Ctrl(V_x, V_y, V_z, adjust);break;}default:break;}
}

调用Motion_Set_Speed设置给定速度到motor_data结构体中

typedef struct _motor_data_t
{float speed_mm_s[4];        // 输入值，编码器计算速度float speed_pwm[4];         // 输出值，PID计算出PWM值int16_t speed_set[4];       // 速度设置值
} motor_data_t;

void Motion_Set_Speed(int16_t speed_m1, int16_t speed_m2, int16_t speed_m3, int16_t speed_m4)
{g_start_ctrl = 1;motor_data.speed_set[0] = speed_m1;motor_data.speed_set[1] = speed_m2;motor_data.speed_set[2] = speed_m3;motor_data.speed_set[3] = speed_m4;for (uint8_t i = 0; i < MAX_MOTOR; i++){PID_Set_Motor_Target(i, motor_data.speed_set[i]*1.0);}
}

将目标速度给入pid_motor[i].target_val中

typedef struct _pid
{float target_val;               //目标值float output_val;               //输出值float pwm_output;        		//PWM输出值float Kp,Ki,Kd;          		//定义比例、积分、微分系数float err;             			//定义偏差值float err_last;          		//定义上一个偏差值float err_next;                 //定义下一个偏差值, 增量式float integral;          		//定义积分值，位置式
} pid_t;

void PID_Set_Motor_Target(uint8_t motor_id, float target)
{if (motor_id > MAX_MOTOR) return;if (motor_id == MAX_MOTOR){for (int i = 0; i < MAX_MOTOR; i++){pid_motor[i].target_val = target;}}else{pid_motor[motor_id].target_val = target;}
}

到这我们完成给定输入

然后我们看实际和给定之间的计算

void Motion_Handle(void)
{Motion_Get_Speed(&car_data);if (g_start_ctrl){Motion_Set_Pwm(motor_data.speed_pwm[0], motor_data.speed_pwm[1], motor_data.speed_pwm[2], motor_data.speed_pwm[3]);}
}

Motion_Get_Speed(&car_data);

首先得到编码器的当前值，编码器的值如何传过来的后面再说
通过该值计算当前各轮速度
通过各轮速度由公式计算当前小车xyz速度

 
// 从编码器读取当前各轮子速度，单位mm/s
void Motion_Get_Speed(car_data_t* car)
{int i = 0;float speed_mm[MAX_MOTOR] = {0};float circle_mm = Motion_Get_Circle_MM();float circle_pulse = Motion_Get_Circle_Pulse();float robot_APB = Motion_Get_APB();Motion_Get_Encoder();// 计算轮子速度，单位mm/s。for (i = 0; i < 4; i++){speed_mm[i] = (g_Encoder_All_Offset[i]) * 100 * circle_mm / circle_pulse;//printf("speed%d : %f",i,speed_mm[i]);}switch (g_car_type){case CAR_MECANUM:{car->Vx = (speed_mm[0] + speed_mm[1] + speed_mm[2] + speed_mm[3]) / 4;car->Vy = -(speed_mm[0] - speed_mm[1] - speed_mm[2] + speed_mm[3]) / 4;car->Vz = -(speed_mm[0] + speed_mm[1] - speed_mm[2] - speed_mm[3]) / 4.0f / robot_APB * 1000;break;}case CAR_MECANUM_MAX:{car->Vx = (speed_mm[0] + speed_mm[1] + speed_mm[2] + speed_mm[3]) / 4;car->Vy = -(speed_mm[0] - speed_mm[1] - speed_mm[2] + speed_mm[3]) / 4;car->Vz = -(speed_mm[0] + speed_mm[1] - speed_mm[2] - speed_mm[3]) / 4.0f / robot_APB * 1000;
//				printf("linear_x:%d\n",car->Vx);
//				printf("linear_y:%d\n",car->Vy);
//				printf("linear_z:%f\n",car->Vz);break;}case CAR_ACKERMAN:{car->Vx = (speed_mm[1] + speed_mm[3]) / 2;car->Vy = Ackerman_Get_Steer_Angle();car->Vz = -(speed_mm[1] - speed_mm[3]) * 1000 / robot_APB;break;}    default:break;}if (g_start_ctrl){for (i = 0; i < MAX_MOTOR; i++){motor_data.speed_mm_s[i] = speed_mm[i];}#if ENABLE_YAW_ADJUST//这个宏关的if (g_yaw_adjust){#if ENABLE_INV_MEMSMecanum_Yaw_Calc(ICM20948_Get_Yaw_Now());#elif ENABLE_MPU9250Mecanum_Yaw_Calc(MPU_Get_Yaw_Now());#endif}#endifPID_Calc_Motor(&motor_data);#if PID_ASSISTANT_EN//这个宏关的if (start_tool()){int32_t speed_send = car->Vx;// int32_t speed_send = (int32_t)speed_m1;set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_send, 1);}#endif}
}

调用这个函数计算值
PID_Calc_Motor(&motor_data);

void PID_Calc_Motor(motor_data_t* motor)
{int i;// float pid_out[4] = {0};// for (i = 0; i < MAX_MOTOR; i++)// {//     pid_out[i] = PID_Location_Calc(&pid_motor[i], 0);//     PID_Set_Motor_Target(i, pid_out[i]);// }for (i = 0; i < MAX_MOTOR; i++){motor->speed_pwm[i] = PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]);}
}

调用PID_Incre_Calc这个函数完成PID计算计算出的值给到 motor->speed_pwm中

typedef struct _pid
{float target_val;               //目标值float output_val;               //输出值float pwm_output;        		//PWM输出值float Kp,Ki,Kd;          		//定义比例、积分、微分系数float err;             			//定义偏差值float err_last;          		//定义上两个个偏差值float err_next;                 //定义上一个偏差值, 增量式float integral;          		//定义积分值，位置式
} pid_t;

u(k)=Kp * e(k-1)+Ki *e(k) +Kd *(e(k)-2e(k-1)+e(k-2))+u(k-1)；

float PID_Incre_Calc(pid_t *pid, float actual_val)
{/*计算目标值与实际值的误差*/pid->err = pid->target_val - actual_val;/*PID算法实现*/pid->pwm_output += pid->Kp * (pid->err - pid->err_next) + pid->Ki * pid->err + pid->Kd * (pid->err - 2 * pid->err_next + pid->err_last);/*传递误差*/pid->err_last = pid->err_next;pid->err_next = pid->err;/*返回PWM输出值 这里如果忽略死区将MOTOR_IGNORE_PULSE设置为0*/if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE))pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE);if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE))pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE);return pid->pwm_output;// // 计算偏差// pid->err = pid->target_val - actual_val;// //增量式PI控制器// pid->pwm_output += pid->Kp * (pid->err - pid->err_last) + pid->Ki * pid->err;// if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE))//     pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE);// if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE))//     pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE);// pid->err_last = pid->err;// return pid->pwm_output;
}