机械狗实习项目

项⽬

⼀

:

利⽤ Realsense 的 ⼯ 化 实现 对 机械 狗 的 闭环 控制

。

内容

,

设计 ⼀个 函数

go-to-positioncintx.int ,

yny-i.cxo.gs

i

o

> x

x

。

以 机械 狗 当前 的 位置 为 原点

,

调⽤ 此 函数 后

,

机 城 狗 到达 指定 点

。

要求

:

① 使⽤ c.tt

② 利⽤ IMU 采⽤ 闭环 控制

③ 误差 尽可能 ⼩

提示

:

可选 的 算法

#include<iostream>
#include<algorithm>
#include<cmath>
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
// Declare RealSense pipeline, encapsulating the actual device and sensors
rs2::pipeline pipe;
// Create a configuration for configuring the pipeline with a non default profile
rs2::config cfg;
// Add pose stream
cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
// Start pipeline with chosen configuration
pipe.start(cfg);
// Declare RealSense pipeline, encapsulating the actual device and sensors
rs2::pipeline pipe;
// Create a configuration for configuring the pipeline with a non default profile
rs2::config cfg;
// Add pose stream
cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
// Start pipeline with chosen configuration
pipe.start(cfg);

// Main loop
while (true) {
	// Wait for the next set of frames from the camera
	auto frames = pipe.wait_for_frames();
	// Get a frame from the pose stream
	auto f = frames.first_or_default(RS2_STREAM_POSE);
	```

	We cast the frame that arrives to `pose_frame` in order to access its `pose_data`.
	```cpp
// Cast the frame to pose_frame and get its data
	auto pose_data = f.as<rs2::pose_frame>().get_pose_data();
// Main loop
	while (true) {
		// Wait for the next set of frames from the camera
		auto frames = pipe.wait_for_frames();
		// Get a frame from the pose stream
		auto f = frames.first_or_default(RS2_STREAM_POSE);
		```

		We cast the frame that arrives to `pose_frame` in order to access its `pose_data`.
		```cpp
// Cast the frame to pose_frame and get its data
		auto pose_data = f.as<rs2::pose_frame>().get_pose_data();

//平移（以米为单位，相对于初始位置）
//速度（以米/秒为单位）
//旋转（以四元数旋转表示，相对于初始位置）
//角速度（以弧度/秒为单位）
//角加速度（以弧度/秒^2为单位）
//跟踪器置信度（姿势数据置信度0x0 - 失败，0x1 - 低，0x2 - 中，0x3 - 高）
//映射器置信度（姿势数据置信度0x0 - 失败，0x1 - 低，0x2 - 中，0x3 - 高）

// Print the x, y, z values of the translation, relative to initial position
		std::cout << "\r" << "Device Position: " << std::setprecision(3) << std::fixed << pose_data.translation.x << " " <<
		          pose_data.translation.y << " " << pose_data.translation.z << " (meters)";
// Print the x, y, z values of the translation, relative to initial position
		std::cout << "\r" << "Device Position: " << std::setprecision(3) << std::fixed << pose_data.translation.x << " " <<
		          pose_data.translation.y << " " << pose_data.translation.z << " (meters)";

//定义值
		struct PID {
			double XSetSpeed,YSetSpeed;             //定义设定值
			double XActualSpeed,YActualSpeed;       //定义实际值
			double Xerr,Yerr;                       //定义偏差值
			double Xerr_last,Yerr_last;             //定义上一个偏差值
			double Kp,Ki,Kd;                        //定义比例、积分、微分系数
			double voltage;                         //定义电压值（控制执行器的变量）
			double XincrementSpeed,YincrementSpeed; //定义积分值
		} pid;

		cin >> pid.Kp >>pid.Ki >>pid.Kd;

//输入目的地坐标值    传输当前坐标值  若不在该坐标值上，则以直线：y=kx为路径前往目的地
		double x,y,x0,y0;  //xo,yo为初始坐标值;x,y为目标坐标值
		cin >>x0 >>y0 >>x >>y;

//机械狗每走一步IMU判断一次机械狗位置
//利用PID算法将偏差值缩小至无限接近于0 ，修正机械狗行走的路线
		double frequency,stride,P;         //机械狗行走的 频率 ,步幅
		int n,P=0;                         //定义 一个机械狗行走的时间函数 ,P随之 增加
		while(P%frequency==0&&fabs(x-x0)>0.01&&fabs(y-y0)>0.01) { /*0.01米*/
			//Xspeed ，Yspeed 由IMU传入
			n=p%frequency;
			double PID_realize(double Xspeed,double Yspeed) {
				// y=kx
				pid.XSetSpeed=x*n/stride;             //这四个值是实时改变的
				pid.YSetSpeed=k*x*n/stride;

				pid.XActualSpeed=Xspeed;
				pid.YActualSpeed=Yspeed;

				pid.Xerr=pid.XSetSpeed-pid.XActualSpeed;
				pid.Yerr=pid.YSetSpeed-pid.YActualSpeed;

				//位置型控制算法  微积分
				XincrementSpeed=pid.Kp*(pid.Xerr-pid.Xerr_next)+pid.Ki*pid.Xerr+pid.Kd*(pid.Xerr-2*pid.Xerr_next+pid.Xerr_last);
				YincrementSpeed=pid.Kp*(pid.Yerr-pid.Yerr_next)+pid.Ki*pid.Yerr+pid.Kd*(pid.Yerr-2*pid.Yerr_next+pid.Yerr_last);

				pid.XActualSpeed+=XincrementSpeed;
				pid.YActualSpeed+=YincrementSpeed;

				pid.Xerr_last=pid.Xerr_next;
				pid.Yerr_last=pid.Yerr_next;
				pid.Xerr_next=pid.Xerr;
				pid.Yerr_next=pid.Yerr;

				return pid.XActualSpeed,pid.YActualSpeed;      //返回经修正后需实际到达的坐标点
			}
		}

//使最终机械狗离目的地误差不超过0.01米