智能小车项目（七）通过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 function
        if not isinstance(msg, Twist): return
        # 下发线速度和角速度
        # Issue linear vel and angular vel
        vx = msg.linear.x
        vy = msg.linear.y
        angular = 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) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.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_MEMS
            Mecanum_Yaw_Calc(ICM20948_Get_Yaw_Now());
            #elif ENABLE_MPU9250
            Mecanum_Yaw_Calc(MPU_Get_Yaw_Now());
            #endif
        }
        #endif
        PID_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;
}