Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: SIM_Rover: add simulation for omni3 mecanum rover 

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
Rhys Mainwaring 2024-07-17 16:13:40 +01:00 committed by Peter Barker
parent 0272f59d0c
commit 4354072d34
2 changed files with 146 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
libraries/SITL/SIM_Rover.cpp
View File

@ -21,6 +21,8 @@
#include <string.h>
#include <stdio.h>
#include <AP_Math/AP_Math.h>
namespace SITL {
SimRover::SimRover(const char *frame_str) :
@ -41,11 +43,15 @@ SimRover::SimRover(const char *frame_str) :
if (vectored_thrust) {
printf("Vectored Thrust Rover Simulation Started\n");
}
omni3 = strstr(frame_str, "omni3mecanum") != nullptr;
if (omni3) {
printf("Omni3 Mecanum Rover Simulation Started\n");
}
lock_step_scheduled = true;
}
/*
return turning circle (diameter) in meters for steering angle proportion in degrees
*/
@ -93,55 +99,17 @@ float SimRover::calc_lat_accel(float steering_angle, float speed)
*/
void SimRover::update(const struct sitl_input &input)
{
float steering, throttle;
// if in skid steering mode the steering and throttle values are used for motor1 and motor2
if (skid_steering) {
float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
steering = motor1 - motor2;
throttle = 0.5*(motor1 + motor2);
} else {
steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
// vectored thrust conversion
if (vectored_thrust) {
const float steering_angle_rad = radians(steering * vectored_angle_max);
steering = sinf(steering_angle_rad) * throttle;
throttle *= cosf(steering_angle_rad);
}
}
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
// speed in m/s in body frame
Vector3f velocity_body = dcm.transposed() * velocity_ef;
// update gyro and accel_body according to frame type
if (omni3) {
update_omni3(input, delta_time);
} else {
update_ackermann_or_skid(input, delta_time);
}
// speed along x axis, +ve is forward
float speed = velocity_body.x;
// yaw rate in degrees/s
float yaw_rate = calc_yaw_rate(steering, speed);
// target speed with current throttle
float target_speed = throttle * max_speed;
// linear acceleration in m/s/s - very crude model
float accel = max_accel * (target_speed - speed) / max_speed;
gyro = Vector3f(0,0,radians(yaw_rate));
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// accel in body frame due to motor
accel_body = Vector3f(accel, 0, 0);
// add in accel due to direction change
accel_body.y += radians(yaw_rate) * speed;
// common to all rovers
// now in earth frame
Vector3f accel_earth = dcm * accel_body;
@ -170,4 +138,126 @@ void SimRover::update(const struct sitl_input &input)
update_mag_field_bf();
}
/*
update the ackermann or skid rover simulation by one time step
*/
void SimRover::update_ackermann_or_skid(const struct sitl_input &input, float delta_time)
{
float steering, throttle;
// if in skid steering mode the steering and throttle values are used for motor1 and motor2
if (skid_steering) {
float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
steering = motor1 - motor2;
throttle = 0.5*(motor1 + motor2);
} else {
steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
// vectored thrust conversion
if (vectored_thrust) {
const float steering_angle_rad = radians(steering * vectored_angle_max);
steering = sinf(steering_angle_rad) * throttle;
throttle *= cosf(steering_angle_rad);
}
}
// speed in m/s in body frame
Vector3f velocity_body = dcm.transposed() * velocity_ef;
// speed along x axis, +ve is forward
float speed = velocity_body.x;
// yaw rate in degrees/s
float yaw_rate = calc_yaw_rate(steering, speed);
// target speed with current throttle
float target_speed = throttle * max_speed;
// linear acceleration in m/s/s - very crude model
float accel = max_accel * (target_speed - speed) / max_speed;
gyro = Vector3f(0,0,radians(yaw_rate));
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// accel in body frame due to motor (excluding gravity)
accel_body = Vector3f(accel, 0, 0);
// add in accel due to direction change
accel_body.y += radians(yaw_rate) * speed;
}
/*
update the omni3 rover simulation by one time step
*/
void SimRover::update_omni3(const struct sitl_input &input, float delta_time)
{
// in omni3 mode the first three servos are motor speeds
float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
float motor2 = 2*((input.servos[1]-1000)/1000.0f - 0.5f);
float motor3 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
// use forward kinematics to calculate body frame velocity
Vector3f wheel_ang_vel(
motor1 * omni3_wheel_max_ang_vel,
motor2 * omni3_wheel_max_ang_vel,
motor3 * omni3_wheel_max_ang_vel
);
// derivation of forward kinematics for an Omni3Mecanum rover
// A. Gfrerrer. "Geometry and kinematics of the Mecanum wheel",
// Computer Aided Geometric Design 25 (2008) 784–791.
// Retrieved from https://www.geometrie.tugraz.at/gfrerrer/publications/MecanumWheel.pdf.
//
// the frame is equilateral triangle
//
// d[i] = 0.18 m is distance from frame centre to each wheel
// r_w = 0.04725 m is the wheel radius.
// delta = radians(-45) is angle of the roller to the direction of forward rotation
// alpha[i] is the angle the wheel axis is rotated about the body z-axis
// c[i] = cos(alpha[i] + delta)
// s[i] = sin(alpha[i] + delta)
//
// wheel d[i] alpha[i] a_x[i] a_y[i] c[i] s[i]
// 1 0.18 1.04719 0.09 0.15588 0.965925 0.258819
// 2 0.18 3.14159 -0.18 0.0 -0.707106 0.707106
// 3 0.18 5.23598 0.09 -0.15588 -0.258819 -0.965925
//
// k = 1/(r_w * sin(delta)) = -29.930445 is a scale factor
//
// inverse kinematic matrix
// M[i, 0] = k * c[i]
// M[i, 1] = k * s[i]
// M[i, 2] = k * (a_x[i] s[i] - a_y[i] c[i])
//
// forward kinematics matrix: Minv = M^-1
constexpr Matrix3f Minv(
-0.0215149, 0.01575, 0.0057649,
-0.0057649, -0.01575, 0.0215149,
0.0875, 0.0875, 0.0875);
// twist - this is the target linear and angular velocity
Vector3f twist = Minv * wheel_ang_vel;
// speed in m/s in body frame
Vector3f velocity_body = dcm.transposed() * velocity_ef;
// linear acceleration in m/s/s - very crude model
float accel_x = omni3_max_accel * (twist.x - velocity_body.x) / omni3_max_speed;
float accel_y = omni3_max_accel * (twist.y - velocity_body.y) / omni3_max_speed;
gyro = Vector3f(0, 0, twist.z);
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// accel in body frame due to motors (excluding gravity)
accel_body = Vector3f(accel_x, accel_y, 0);
}
} // namespace SITL

9
libraries/SITL/SIM_Rover.h
View File

@ -51,6 +51,15 @@ private:
float vectored_angle_max = 90.0f; // maximum angle (in degrees) to which thrust can be turned
float vectored_turn_rate_max = 90.0f; // maximum turn rate (in deg/sec) with full throttle angled at 90deg
// omni3 Mecanum related members
bool omni3; // true if vehicle is omni-directional with 3 Mecanum wheels
float omni3_max_speed = 2.3625f; // omni vehicle's maximum forward speed in m/s
float omni3_max_accel = 1.0f; // omni vehicle's maximum forward acceleration in m/s/s
float omni3_wheel_max_ang_vel = 50.0f; // omni vehicle's wheel maximum angular velocity in rad/s
void update_ackermann_or_skid(const struct sitl_input &input, float delta_time);
void update_omni3(const struct sitl_input &input, float delta_time);
float turn_circle(float steering) const;
float calc_yaw_rate(float steering, float speed);
float calc_lat_accel(float steering_angle, float speed);